15 #define debug_printf(args...) printf(args)
17 #define debug_printf(args...)
25 * p argument is the crossing point coordinates (dummy for 0 or 2
29 intersect_line(const line_t *l1, const line_t *l2, point_t *p)
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);
36 if ((l1->a == 0 && l1->b == 0) || (l2->a == 0 && l2->b == 0))
41 if (l1->b*l2->c == l2->b*l1->c)
47 * a'x + b'y + c' = 0 */
50 p->x = -(l2->b*p->y + l2->c)/l2->a;
53 p->x = -(l2->b*(-l1->c) + l2->c*l1->b)/(l2->a*l1->b);
59 if (l1->a*l2->c == l2->a*l1->c)
64 * a'x + b'y + c' = 0 */
68 p->y = -(l2->a*p->x + l2->c)/l2->b;
71 p->y = -(l2->a*(-l1->c) + l2->c*(l1->a))/(l2->b*l1->a);
76 if (l2->a*l1->b-l1->a*l2->b == 0) {
77 if (l1->a*l2->c == l2->a*l1->c)
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;
85 tmp1 = (l1->a*l2->c - l1->c*l2->a);
86 tmp2 = (l2->a*l1->b - l1->a*l2->b);
88 p->x = -(l1->b*tmp1 + l1->c*tmp2) / (l1->a*tmp2);
92 void pts2line(const point_t *p1, const point_t *p2, line_t *l)
94 double p1x, p1y, p2x, p2y;
104 l->c = -(l->a * p1x + l->b * p1y);
106 debug_printf("%s: %2.2f, %2.2f, %2.2f\r\n",
107 __FUNCTION__, l->a, l->b, l->c);
110 void proj_pt_line(const point_t * p, const line_t * l, point_t * p_out)
116 l_tmp.c = -l_tmp.a*p->x - l_tmp.b*p->y;
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);
129 * 3 parallel and one point in
131 * p argument is the crossing point coordinates (dummy for 0 1 or 3
135 intersect_segment(const point_t *s1, const point_t *s2,
136 const point_t *t1, const point_t *t2,
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);
149 pts2line(s1, s2, &l1);
150 pts2line(t1, t2, &l2);
152 ret = intersect_line(&l1, &l2, p);
161 if (vect_pscal_sign(&v, &w)<=0)
169 if (vect_pscal_sign(&v, &w)<=0)
175 /* if points equal */
176 if (s1->x == t1->x && s1->y == t1->y) {
180 if (s1->x == t2->x && s1->y == t2->y) {
184 if (s2->x == t1->x && s2->y == t1->y) {
188 if (s2->x == t2->x && s2->y == t2->y) {
193 debug_printf("px=%" PRIi32 " py=%" PRIi32 "\n", p->x, p->y);
195 /* Consider as parallel if intersection is too far */
196 if (ABS(p->x) > (1L << 15) || ABS(p->y) > (1L << 15))
199 /* if prod scal neg: cut in middle of segment */
200 /* todo for both segment */
205 u1 = vect_pscal_sign(&v, &w );
211 u2 = vect_pscal_sign(&v, &w);
213 debug_printf("u1=%d u2=%d\n", u1, u2);