modules/base/math/geometry/lines.c

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