work on trajectory, update cobboard and ballboard too

[aversive.git] / modules / base / math / geometry / lines.c

diff --git a/modules/base/math/geometry/lines.c b/modules/base/math/geometry/lines.c
index 09ad1a1..935d012 100755 (executable)
--- a/modules/base/math/geometry/lines.c
+++ b/modules/base/math/geometry/lines.c
@@ -46,24 +46,24 @@
  *  p argument is the crossing point coordinates (dummy for 0 or 2
  *  result)
  */
-uint8_t 
+uint8_t
 intersect_line(const line_t *l1, const line_t *l2, point_t *p)
-{      
+{
        double tmp1, tmp2;
 
-       debug_printf("l1:%2.2f,%2.2f,%2.2f l2:%2.2f,%2.2f,%2.2f\n", 
+       debug_printf("l1:%2.2f,%2.2f,%2.2f l2:%2.2f,%2.2f,%2.2f\n",
                     l1->a, l1->b, l1->c, l2->a, l2->b, l2->c);
        /* if dummy lines */
        if ((l1->a == 0 && l1->b == 0) || (l2->a == 0 && l2->b == 0))
                return 0;
-       
+
        if (l1->a == 0) {
                if (l2->a == 0) {
                        if (l1->b*l2->c == l2->b*l1->c)
                                return 2;
                        return 0;
                }
-               
+
                /*       by  + c  = 0
                 * a'x + b'y + c' = 0 */
                /*
@@ -74,7 +74,7 @@ intersect_line(const line_t *l1, const line_t *l2, point_t *p)
                p->x = -(l2->b*(-l1->c) + l2->c*l1->b)/(l2->a*l1->b);
                return 1;
        }
-       
+
        if (l1->b == 0) {
                if (l2->b == 0) {
                        if (l1->a*l2->c == l2->a*l1->c)
@@ -118,12 +118,12 @@ void pts2line(const point_t *p1, const point_t *p2, line_t *l)
        p1y = p1->y;
        p2x = p2->x;
        p2y = p2->y;
-       
+
 
        l->a = -(p2y - p1y);
        l->b =  (p2x - p1x);
        l->c = -(l->a * p1x + l->b * p1y);
-  
+
        debug_printf("%s: %2.2f, %2.2f, %2.2f\r\n",
                     __FUNCTION__, l->a, l->b, l->c);
 }
@@ -145,16 +145,16 @@ void proj_pt_line(const point_t * p, const line_t * l, point_t * p_out)
 
 /* return values:
  *  0 dont cross
- *  1 cross 
+ *  1 cross
  *  2 cross on point
  *  3 parallel and one point in
  *
  *  p argument is the crossing point coordinates (dummy for 0 1 or 3
  *  result)
  */
-uint8_t 
-intersect_segment(const point_t *s1, const point_t *s2, 
-                 const point_t *t1, const point_t *t2, 
+uint8_t
+intersect_segment(const point_t *s1, const point_t *s2,
+                 const point_t *t1, const point_t *t2,
                  point_t *p)
 {
        line_t l1, l2;
@@ -210,7 +210,7 @@ intersect_segment(const point_t *s1, const point_t *s2,
                *p = *s2;
                return 2;
        }
-       
+
        debug_printf("px=%" PRIi32 " py=%" PRIi32 "\n", p->x, p->y);
 
        /* Consider as parallel if intersection is too far */
@@ -242,3 +242,8 @@ intersect_segment(const point_t *s1, const point_t *s2,
        return 1;
 
 }
+
+void line_translate(line_t *l, vect_t *v)
+{
+       l->c -= (l->a * v->x + l->b * v->y);
+}