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/Makefile.module | 2 +- digital/avr/modules/math/geometry/intersection.c | 87 ++++++++++++++++++++++ digital/avr/modules/math/geometry/intersection.h | 39 ++++++++++ .../avr/modules/math/geometry/test/test_geometry.c | 21 ++++++ 4 files changed, 148 insertions(+), 1 deletion(-) create mode 100644 digital/avr/modules/math/geometry/intersection.c create mode 100644 digital/avr/modules/math/geometry/intersection.h (limited to 'digital/avr') diff --git a/digital/avr/modules/math/geometry/Makefile.module b/digital/avr/modules/math/geometry/Makefile.module index ff28f01b..e801b844 100644 --- a/digital/avr/modules/math/geometry/Makefile.module +++ b/digital/avr/modules/math/geometry/Makefile.module @@ -1 +1 @@ -math_geometry_SOURCES = vect.c distance.c +math_geometry_SOURCES = vect.c distance.c 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); + } + } +} diff --git a/digital/avr/modules/math/geometry/intersection.h b/digital/avr/modules/math/geometry/intersection.h new file mode 100644 index 00000000..9f9cf161 --- /dev/null +++ b/digital/avr/modules/math/geometry/intersection.h @@ -0,0 +1,39 @@ +#ifndef intersection_h +#define intersection_h +/* intersection.h */ +/* 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 "modules/math/geometry/vect.h" + +/** Test intersection between two line segments. Return non zero if they + * intersect. + * - a, b: first line segment vertices. + * - c, d: second line segment vertices. + * + * If AB and CD are parallel, consider there is no intersection. */ +uint8_t +intersection_segment_segment (const vect_t *a, const vect_t *b, + const vect_t *c, const vect_t *d); + +#endif /* intersection_h */ diff --git a/digital/avr/modules/math/geometry/test/test_geometry.c b/digital/avr/modules/math/geometry/test/test_geometry.c index 17afb787..7b3c5e47 100644 --- a/digital/avr/modules/math/geometry/test/test_geometry.c +++ b/digital/avr/modules/math/geometry/test/test_geometry.c @@ -26,6 +26,7 @@ #include "modules/math/geometry/geometry.h" #include "modules/math/geometry/vect.h" #include "modules/math/geometry/distance.h" +#include "modules/math/geometry/intersection.h" #include "modules/utils/utils.h" #include "modules/uart/uart.h" @@ -236,6 +237,25 @@ test_distance (void) test (d == 670); } +void +test_intersection (void) +{ + vect_t v00 = { 0, 0 }; + vect_t v02 = { 0, 20 }; + vect_t v20 = { 20, 0 }; + vect_t v22 = { 20, 20 }; + vect_t v44 = { 40, 40 }; + test (intersection_segment_segment (&v00, &v22, &v20, &v02) == 1); + test (intersection_segment_segment (&v00, &v22, &v02, &v20) == 1); + test (intersection_segment_segment (&v22, &v00, &v20, &v02) == 1); + test (intersection_segment_segment (&v22, &v00, &v02, &v20) == 1); + test (intersection_segment_segment (&v00, &v22, &v00, &v22) == 0); + test (intersection_segment_segment (&v00, &v22, &v00, &v44) == 0); + test (intersection_segment_segment (&v22, &v44, &v02, &v20) == 0); + test (intersection_segment_segment (&v00, &v44, &v02, &v20) == 1); + test (intersection_segment_segment (&v44, &v00, &v02, &v20) == 1); +} + int main (void) { @@ -243,6 +263,7 @@ main (void) test_geometry (); test_vect (); test_distance (); + test_intersection (); uart0_putc ('o'); uart0_putc ('k'); uart0_putc ('\n'); -- cgit v1.2.3