From 9524de0dfededc83dba3944d0fc872e859bcf449 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 5 May 2012 16:13:45 +0200 Subject: digital/avr/modules/math/geometry: add intersection code --- digital/avr/modules/math/geometry/intersection.c | 87 ++++++++++++++++++++++++ 1 file changed, 87 insertions(+) create mode 100644 digital/avr/modules/math/geometry/intersection.c (limited to 'digital/avr/modules/math/geometry/intersection.c') diff --git a/digital/avr/modules/math/geometry/intersection.c b/digital/avr/modules/math/geometry/intersection.c new file mode 100644 index 00000000..8438813b --- /dev/null +++ b/digital/avr/modules/math/geometry/intersection.c @@ -0,0 +1,87 @@ +/* intersection.c */ +/* avr.math.geometry - Geometry math module. {{{ + * + * Copyright (C) 2012 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "intersection.h" + +/** Compare a and b to determine if a / b is in [0:1]. Return non zero if + * true. */ +static uint8_t +intersection_div_is_in_0_1 (int32_t a, int32_t b) +{ + /* Test sign, a / b < 0 if different. */ + if ((a >> 31) ^ (b >> 31)) + return 0; + else if (a > 0) + return a <= b; + else + return a >= b; +} + +uint8_t +intersection_segment_segment (const vect_t *a, const vect_t *b, + const vect_t *c, const vect_t *d) +{ + /* + * For each point P on the line segment [AB], there is a real u in [0, 1] + * for which P = A + u (B - A) + * + * An intersection point must be on both line segments: + * + * A + u (B - A) = C + v (D - C) + * + * a.x + u (b.x - a.x) = c.x + v (d.x - c.x) + * a.y + u (b.y - a.y) = c.y + v (d.y - c.y) + * + * (c.x - a.x) (d.y - c.y) - (d.x - c.x) (c.y - a.y) + * u = ------------------------------------------------- + * (b.x - a.x) (d.y - c.y) - (d.x - c.x) (b.y - a.y) + * + * (c.x - a.x) (b.y - a.y) - (b.x - a.x) (c.y - a.y) + * v = ------------------------------------------------- + * (b.x - a.x) (d.y - c.y) - (d.x - c.x) (b.y - a.y) + * + * u = (vac.normal . vcd) / (vab.normal . vcd) + * v = (vac.normal . vab) / (vab.normal . vcd) + * + * If vab.normal . vcd is 0, AB and CD are parallel. + */ + vect_t vab = *b; vect_sub (&vab, a); + vect_t vcd = *d; vect_sub (&vcd, c); + int32_t den = vect_normal_dot_product (&vab, &vcd); + if (den == 0) + return 0; + else + { + vect_t vac = *c; vect_sub (&vac, a); + int32_t unum = vect_normal_dot_product (&vac, &vcd); + if (!intersection_div_is_in_0_1 (unum, den)) + return 0; + else + { + int32_t vnum = vect_normal_dot_product (&vac, &vab); + return intersection_div_is_in_0_1 (vnum, den); + } + } +} -- cgit v1.2.3