modules/base/math/geometry/circles.c

   1 /*
   2  *  Copyright Droids Corporation (2009)
   3  *
   4  *  This program is free software; you can redistribute it and/or modify
   5  *  it under the terms of the GNU General Public License as published by
   6  *  the Free Software Foundation; either version 2 of the License, or
   7  *  (at your option) any later version.
   8  *
   9  *  This program is distributed in the hope that it will be useful,
  10  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  *  GNU General Public License for more details.
  13  *
  14  *  You should have received a copy of the GNU General Public License
  15  *  along with this program; if not, write to the Free Software
  16  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  17  *
  18  *  Revision : $Id: f16.h,v 1.6.4.3 2008-05-10 15:06:26 zer0 Exp $
  19  *
  20  */
  21
  22 #include <math.h>
  23
  24 #include <aversive.h>
  25
  26 #include <vect_base.h>
  27 #include <circles.h>
  28
  29 static inline float sq(float x)
  30 {
  31         return x*x;
  32 }
  33
  34 uint8_t pt_is_inside_circle(const point_t *p, circle_t *c)
  35 {
  36         vect_t v;
  37         v.x = p->x - c->x;
  38         v.y = p->y - c->y;
  39         if ((v.x * v.x + v.y * v.y) < (c->r * c->r))
  40                 return 1;
  41         return 0;
  42 }
  43
  44 /*
  45  * return values:
  46  *  0 dont cross
  47  *  1 one intersection point
  48  *  2 two intersection points
  49  *
  50  *  p1, p2 arguments are the crossing points coordinates. Both p1 and
  51  *  p2 are dummy for 0 result. When result is 1, p1 and p2 are set to
  52  *  the same value.
  53  */
  54 uint8_t circle_intersect(const circle_t *c1, const circle_t *c2,
  55                          point_t *p1, point_t *p2)
  56 {
  57         circle_t ca, cb;
  58         float a, b, c, d, e;
  59         uint8_t ret = 0;
  60
  61         /* create circles with same radius, but centered on 0,0 : it
  62          * will make process easier */
  63         ca.x = 0;
  64         ca.y = 0;
  65         ca.r = c1->r;
  66         cb.x = c2->x - c1->x;
  67         cb.y = c2->y - c1->y;
  68         cb.r = c2->r;
  69
  70         /* inspired from
  71            http://www.loria.fr/~roegel/notes/note0001.pdf */
  72         a = 2. * cb.x;
  73         b = 2. * cb.y;
  74         c = sq(cb.x) + sq(cb.y) - sq(cb.r) + sq(ca.r);
  75         d = sq(2. * a * c) -
  76                 (4. * (sq(a) + sq(b)) * (sq(c) - sq(b) * sq(ca.r)) );
  77
  78         /* no intersection */
  79         if (d < 0)
  80                 return 0;
  81
  82         if (b == 0) {
  83                 /* special case */
  84                 e = sq(cb.r) - sq((2. * c - sq(a)) / (2. * a));
  85
  86                 /* no intersection */
  87                 if (e < 0)
  88                         return 0;
  89
  90                 p1->x = (2. * a * c - sqrt(d)) / (2. * (sq(a) + sq(b)));
  91                 p1->y = sqrt(e);
  92                 p2->x = p1->x;
  93                 p2->y = p1->y;
  94                 ret = 1;
  95         }
  96         else {
  97                 /* usual case */
  98                 p1->x = (2. * a * c - sqrt(d)) / (2. * (sq(a) + sq(b)));
  99                 p1->y = (c - a * p1->x) / b;
 100                 p2->x = (2. * a * c + sqrt(d)) / (2. * (sq(a) + sq(b)));
 101                 p2->y = (c - a * p2->x) / b;
 102                 ret = 2;
 103         }
 104
 105         /* retranslate */
 106         p1->x += c1->x;
 107         p1->y += c1->y;
 108         p2->x += c1->x;
 109         p2->y += c1->y;
 110
 111         return ret;
 112 }