modules/base/math/geometry/lines.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 <aversive.h>
  23
  24 #include <stdint.h>
  25 #include <inttypes.h>
  26 #include <stdlib.h>
  27 #include <stdio.h>
  28 #include <math.h>
  29
  30 #include <vect_base.h>
  31 #include <lines.h>
  32
  33 #define DEBUG 0
  34
  35 #if DEBUG == 1
  36 #define debug_printf(args...) printf(args)
  37 #else
  38 #define debug_printf(args...)
  39 #endif
  40
  41 /* return values :
  42  *  0 dont cross
  43  *  1 cross
  44  *  2 parallel crossing
  45  *
  46  *  p argument is the crossing point coordinates (dummy for 0 or 2
  47  *  result)
  48  */
  49 uint8_t
  50 intersect_line(const line_t *l1, const line_t *l2, point_t *p)
  51 {
  52         double tmp1, tmp2;
  53
  54         debug_printf("l1:%2.2f,%2.2f,%2.2f l2:%2.2f,%2.2f,%2.2f\n",
  55                      l1->a, l1->b, l1->c, l2->a, l2->b, l2->c);
  56         /* if dummy lines */
  57         if ((l1->a == 0 && l1->b == 0) || (l2->a == 0 && l2->b == 0))
  58                 return 0;
  59
  60         if (l1->a == 0) {
  61                 if (l2->a == 0) {
  62                         if (l1->b*l2->c == l2->b*l1->c)
  63                                 return 2;
  64                         return 0;
  65                 }
  66
  67                 /*       by  + c  = 0
  68                  * a'x + b'y + c' = 0 */
  69                 /*
  70                   p->y = -l1->c/l1->b;
  71                   p->x = -(l2->b*p->y + l2->c)/l2->a;
  72                 */
  73                 p->y = -l1->c/l1->b;
  74                 p->x = -(l2->b*(-l1->c) + l2->c*l1->b)/(l2->a*l1->b);
  75                 return 1;
  76         }
  77
  78         if (l1->b == 0) {
  79                 if (l2->b == 0) {
  80                         if (l1->a*l2->c == l2->a*l1->c)
  81                                 return 2;
  82                         return 0;
  83                 }
  84                 /* ax        + c  = 0
  85                  * a'x + b'y + c' = 0 */
  86
  87                 /*
  88                   p->x = -l1->c/l1->a;
  89                   p->y = -(l2->a*p->x + l2->c)/l2->b;
  90                 */
  91                 p->x = -l1->c/l1->a;
  92                 p->y = -(l2->a*(-l1->c) + l2->c*(l1->a))/(l2->b*l1->a);
  93                 return 1;
  94         }
  95
  96         /* parallel lines */
  97         if (l2->a*l1->b-l1->a*l2->b == 0) {
  98                 if (l1->a*l2->c == l2->a*l1->c)
  99                         return 2;
 100                 return 0;
 101         }
 102         /*
 103           p->y = (l1->a*l2->c - l1->c*l2->a)/(l2->a*l1->b - l1->a*l2->b);
 104           p->x = -(l1->b*p->y+l1->c)/l1->a;
 105         */
 106         tmp1 = (l1->a*l2->c - l1->c*l2->a);
 107         tmp2 = (l2->a*l1->b - l1->a*l2->b);
 108         p->y = tmp1 / tmp2;
 109         p->x = -(l1->b*tmp1 + l1->c*tmp2) / (l1->a*tmp2);
 110         return 1;
 111 }
 112
 113 void pts2line(const point_t *p1, const point_t *p2, line_t *l)
 114 {
 115         double p1x, p1y, p2x, p2y;
 116
 117         p1x = p1->x;
 118         p1y = p1->y;
 119         p2x = p2->x;
 120         p2y = p2->y;
 121
 122
 123         l->a = -(p2y - p1y);
 124         l->b =  (p2x - p1x);
 125         l->c = -(l->a * p1x + l->b * p1y);
 126
 127         debug_printf("%s: %2.2f, %2.2f, %2.2f\r\n",
 128                      __FUNCTION__, l->a, l->b, l->c);
 129 }
 130
 131 void proj_pt_line(const point_t * p, const line_t * l, point_t * p_out)
 132 {
 133         line_t l_tmp;
 134
 135         l_tmp.a = l->b;
 136         l_tmp.b = -l->a;
 137         l_tmp.c = -l_tmp.a*p->x - l_tmp.b*p->y;
 138
 139         p_out->y = (l_tmp.a*l->c - l->a*l_tmp.c) / (l->a*l_tmp.b - l_tmp.a*l->b);
 140         p_out->x = (l->b*l_tmp.c - l_tmp.b*l->c) / (l->a*l_tmp.b - l_tmp.a*l->b);
 141
 142 }
 143
 144
 145
 146 /* return values:
 147  *  0 dont cross
 148  *  1 cross
 149  *  2 cross on point
 150  *  3 parallel and one point in
 151  *
 152  *  p argument is the crossing point coordinates (dummy for 0 1 or 3
 153  *  result)
 154  */
 155 uint8_t
 156 intersect_segment(const point_t *s1, const point_t *s2,
 157                   const point_t *t1, const point_t *t2,
 158                   point_t *p)
 159 {
 160         line_t l1, l2;
 161         uint8_t ret;
 162         int8_t u1, u2;
 163         vect_t v, w;
 164
 165         debug_printf("s1:%"PRIi32",%"PRIi32" s2:%"PRIi32",%"PRIi32" "
 166                      "t1:%"PRIi32",%"PRIi32" t2:%"PRIi32",%"PRIi32"\r\n",
 167                      s1->x, s1->y, s2->x, s2->y,
 168                      t1->x, t1->y, t2->x, t2->y);
 169
 170         pts2line(s1, s2, &l1);
 171         pts2line(t1, t2, &l2);
 172
 173         ret = intersect_line(&l1, &l2, p);
 174         if (!ret)
 175                 return 0;
 176         if (ret == 2) {
 177                 v.x = t1->x - s1->x;
 178                 v.y = t1->y - s1->y;
 179                 w.x = t1->x - s2->x;
 180                 w.y = t1->y - s2->y;
 181                 *p = *t1;
 182                 if (vect_pscal_sign(&v, &w)<=0)
 183                         return 3;
 184
 185                 v.x = t2->x - s1->x;
 186                 v.y = t2->y - s1->y;
 187                 w.x = t2->x - s2->x;
 188                 w.y = t2->y - s2->y;
 189                 *p = *t2;
 190                 if (vect_pscal_sign(&v, &w)<=0)
 191                         return 3;
 192                 return 0;
 193         }
 194
 195
 196         /* if points equal */
 197         if (s1->x == t1->x && s1->y == t1->y) {
 198                 *p = *s1;
 199                 return 2;
 200         }
 201         if (s1->x == t2->x && s1->y == t2->y) {
 202                 *p = *s1;
 203                 return 2;
 204         }
 205         if (s2->x == t1->x && s2->y == t1->y) {
 206                 *p = *s2;
 207                 return 2;
 208         }
 209         if (s2->x == t2->x && s2->y == t2->y) {
 210                 *p = *s2;
 211                 return 2;
 212         }
 213
 214         debug_printf("px=%" PRIi32 " py=%" PRIi32 "\n", p->x, p->y);
 215
 216         /* Consider as parallel if intersection is too far */
 217         if (ABS(p->x) > (1L << 15) || ABS(p->y) > (1L << 15))
 218                 return 0;
 219
 220         /* if prod scal neg: cut in middle of segment */
 221         /* todo for both segment */
 222         v.x = p->x-s1->x;
 223         v.y = p->y-s1->y;
 224         w.x = p->x-s2->x;
 225         w.y = p->y-s2->y;
 226         u1 = vect_pscal_sign(&v, &w );
 227
 228         v.x = p->x-t1->x;
 229         v.y = p->y-t1->y;
 230         w.x = p->x-t2->x;
 231         w.y = p->y-t2->y;
 232         u2 = vect_pscal_sign(&v, &w);
 233
 234         debug_printf("u1=%d u2=%d\n", u1, u2);
 235
 236         if (u1>0 || u2>0)
 237                 return 0;
 238
 239         if (u1==0 || u2==0)
 240                 return 2;
 241
 242         return 1;
 243
 244 }