summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2010-04-13 00:22:01 +0200
committerNicolas Schodet2010-04-13 00:22:01 +0200
commitcdc38652ea2b310cf8f501924f3dec5d3e2818ea (patch)
tree875ff17368a862c03f165e8e7ce8a9f837b2c0d8 /digital
parent7f24f37772871ad4a420f59f93b97ab1ad9b0e24 (diff)
digital/avr/modules/math/geometry: add distance computations
Diffstat (limited to 'digital')
-rw-r--r--digital/avr/modules/math/geometry/Makefile.module2
-rw-r--r--digital/avr/modules/math/geometry/distance.c71
-rw-r--r--digital/avr/modules/math/geometry/distance.h36
-rw-r--r--digital/avr/modules/math/geometry/test/test_geometry.c33
4 files changed, 141 insertions, 1 deletions
diff --git a/digital/avr/modules/math/geometry/Makefile.module b/digital/avr/modules/math/geometry/Makefile.module
index 945e0937..ff28f01b 100644
--- a/digital/avr/modules/math/geometry/Makefile.module
+++ b/digital/avr/modules/math/geometry/Makefile.module
@@ -1 +1 @@
-math_geometry_SOURCES = vect.c
+math_geometry_SOURCES = vect.c distance.c
diff --git a/digital/avr/modules/math/geometry/distance.c b/digital/avr/modules/math/geometry/distance.c
new file mode 100644
index 00000000..d9228638
--- /dev/null
+++ b/digital/avr/modules/math/geometry/distance.c
@@ -0,0 +1,71 @@
+/* distance.c */
+/* avr.math.geometry - Geometry math module. {{{
+ *
+ * Copyright (C) 2010 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 "distance.h"
+
+#include "modules/utils/utils.h"
+
+int16_t
+distance_segment_point (vect_t *a, vect_t *b, vect_t *p)
+{
+ int16_t proj;
+ /* The space is split in three regions delimited by the two parallel lines
+ * going through A and B, perpendicular to the segment:
+ *
+ * | |
+ * | 2 |
+ * | |
+ * 1 A=======B 3
+ * | |
+ * | 2 |
+ * | |
+ *
+ * If P is in region 2, distance is the distance between P and the AB
+ * line.
+ *
+ * If P is in region 1, distance is |AP|. If P is in region 3, distance
+ * is |BP|.
+ *
+ * If vAP . vAB <= 0, P is in region 1. If vBP . vAB > 0, P is in region
+ * 3. Else, it is in region 2.
+ *
+ * Distance to line is determined using dot product between vn, a normal
+ * of vAB and vBP. */
+ vect_t vab, vap, vbp;
+ /* Region 1? */
+ vab = *b; vect_sub (&vab, a);
+ vap = *p; vect_sub (&vap, a);
+ if (vect_dot_product (&vab, &vap) <= 0)
+ return vect_norm (&vap);
+ /* Region 3? */
+ vbp = *p; vect_sub (&vbp, b);
+ if (vect_dot_product (&vab, &vbp) > 0)
+ return vect_norm (&vbp);
+ /* Else, region 2. */
+ vect_normal (&vab);
+ proj = vect_dot_product (&vab, &vbp) / vect_norm (&vab);
+ return UTILS_ABS (proj);
+}
+
diff --git a/digital/avr/modules/math/geometry/distance.h b/digital/avr/modules/math/geometry/distance.h
new file mode 100644
index 00000000..77bc1e8c
--- /dev/null
+++ b/digital/avr/modules/math/geometry/distance.h
@@ -0,0 +1,36 @@
+#ifndef distance_h
+#define distance_h
+/* distance.h */
+/* avr.math.geometry - Geometry math module. {{{
+ *
+ * Copyright (C) 2010 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"
+
+/** Compute distance between a segment and a point.
+ * - a: segment first vertice.
+ * - b: segment second vertice.
+ * - p: point. */
+int16_t
+distance_segment_point (vect_t *a, vect_t *b, vect_t *p);
+
+#endif /* distance_h */
diff --git a/digital/avr/modules/math/geometry/test/test_geometry.c b/digital/avr/modules/math/geometry/test/test_geometry.c
index 55c5cb1f..f18aadd9 100644
--- a/digital/avr/modules/math/geometry/test/test_geometry.c
+++ b/digital/avr/modules/math/geometry/test/test_geometry.c
@@ -25,6 +25,7 @@
#include "common.h"
#include "modules/math/geometry/geometry.h"
#include "modules/math/geometry/vect.h"
+#include "modules/math/geometry/distance.h"
#include "modules/utils/utils.h"
#include "modules/uart/uart.h"
@@ -170,12 +171,44 @@ test_vect (void)
test (c[3].x == 0 + 60 && c[3].y == -70 + 20);
}
+void
+test_distance (void)
+{
+ vect_t a, b, p;
+ int16_t d;
+ /* distance_segment_point. */
+ a.x = 300; a.y = 200;
+ b.x = 900; b.y = 500;
+ p.x = 400; p.y = 600;
+ d = distance_segment_point (&a, &b, &p);
+ test (d == 313);
+ p.x = 800; p.y = 100;
+ d = distance_segment_point (&a, &b, &p);
+ test (d == 313);
+ p.x = 700; p.y = 750;
+ d = distance_segment_point (&a, &b, &p);
+ test (d == 313);
+ p.x = 100; p.y = 450;
+ d = distance_segment_point (&a, &b, &p);
+ test (d == 320);
+ p.x = -200; p.y = 300;
+ d = distance_segment_point (&a, &b, &p);
+ test (d == 509);
+ p.x = 1000; p.y = 900;
+ d = distance_segment_point (&a, &b, &p);
+ test (d == 412);
+ p.x = 1500; p.y = 800;
+ d = distance_segment_point (&a, &b, &p);
+ test (d == 670);
+}
+
int
main (void)
{
uart0_init ();
test_geometry ();
test_vect ();
+ test_distance ();
uart0_putc ('o');
uart0_putc ('k');
uart0_putc ('\n');