summaryrefslogtreecommitdiffhomepage
path: root/digital/avr/modules/math/geometry/intersection.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/avr/modules/math/geometry/intersection.c')
-rw-r--r--digital/avr/modules/math/geometry/intersection.c87
1 files changed, 87 insertions, 0 deletions
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);
+ }
+ }
+}