summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2012-05-05 16:13:45 +0200
committerNicolas Schodet2012-05-05 16:13:45 +0200
commit9524de0dfededc83dba3944d0fc872e859bcf449 (patch)
treee5ceaca9fa600854b9730ad3e501f439281350a6 /digital
parent4c2c509aad3740528df96248c9d14289770da1be (diff)
digital/avr/modules/math/geometry: add intersection code
Diffstat (limited to 'digital')
-rw-r--r--digital/avr/modules/math/geometry/Makefile.module2
-rw-r--r--digital/avr/modules/math/geometry/intersection.c87
-rw-r--r--digital/avr/modules/math/geometry/intersection.h39
-rw-r--r--digital/avr/modules/math/geometry/test/test_geometry.c21
4 files changed, 148 insertions, 1 deletions
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');