2 * Copyright Droids Corporation (2009)
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.
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.
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
18 * Revision : $Id: f16.h,v 1.6.4.3 2008-05-10 15:06:26 zer0 Exp $
30 #include <vect_base.h>
36 #define debug_printf(args...) printf(args)
38 #define debug_printf(args...)
46 * p argument is the crossing point coordinates (dummy for 0 or 2
50 intersect_line(const line_t *l1, const line_t *l2, point_t *p)
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);
57 if ((l1->a == 0 && l1->b == 0) || (l2->a == 0 && l2->b == 0))
62 if (l1->b*l2->c == l2->b*l1->c)
68 * a'x + b'y + c' = 0 */
71 p->x = -(l2->b*p->y + l2->c)/l2->a;
74 p->x = -(l2->b*(-l1->c) + l2->c*l1->b)/(l2->a*l1->b);
80 if (l1->a*l2->c == l2->a*l1->c)
85 * a'x + b'y + c' = 0 */
89 p->y = -(l2->a*p->x + l2->c)/l2->b;
92 p->y = -(l2->a*(-l1->c) + l2->c*(l1->a))/(l2->b*l1->a);
97 if (l2->a*l1->b-l1->a*l2->b == 0) {
98 if (l1->a*l2->c == l2->a*l1->c)
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;
106 tmp1 = (l1->a*l2->c - l1->c*l2->a);
107 tmp2 = (l2->a*l1->b - l1->a*l2->b);
109 p->x = -(l1->b*tmp1 + l1->c*tmp2) / (l1->a*tmp2);
113 void pts2line(const point_t *p1, const point_t *p2, line_t *l)
115 double p1x, p1y, p2x, p2y;
125 l->c = -(l->a * p1x + l->b * p1y);
127 debug_printf("%s: %2.2f, %2.2f, %2.2f\r\n",
128 __FUNCTION__, l->a, l->b, l->c);
131 void proj_pt_line(const point_t * p, const line_t * l, point_t * p_out)
137 l_tmp.c = -l_tmp.a*p->x - l_tmp.b*p->y;
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);
150 * 3 parallel and one point in
152 * p argument is the crossing point coordinates (dummy for 0 1 or 3
156 intersect_segment(const point_t *s1, const point_t *s2,
157 const point_t *t1, const point_t *t2,
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);
170 pts2line(s1, s2, &l1);
171 pts2line(t1, t2, &l2);
173 ret = intersect_line(&l1, &l2, p);
182 if (vect_pscal_sign(&v, &w)<=0)
190 if (vect_pscal_sign(&v, &w)<=0)
196 /* if points equal */
197 if (s1->x == t1->x && s1->y == t1->y) {
201 if (s1->x == t2->x && s1->y == t2->y) {
205 if (s2->x == t1->x && s2->y == t1->y) {
209 if (s2->x == t2->x && s2->y == t2->y) {
214 debug_printf("px=%" PRIi32 " py=%" PRIi32 "\n", p->x, p->y);
216 /* Consider as parallel if intersection is too far */
217 if (ABS(p->x) > (1L << 15) || ABS(p->y) > (1L << 15))
220 /* if prod scal neg: cut in middle of segment */
221 /* todo for both segment */
226 u1 = vect_pscal_sign(&v, &w );
232 u2 = vect_pscal_sign(&v, &w);
234 debug_printf("u1=%d u2=%d\n", u1, u2);