From cdc38652ea2b310cf8f501924f3dec5d3e2818ea Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 13 Apr 2010 00:22:01 +0200 Subject: digital/avr/modules/math/geometry: add distance computations --- digital/avr/modules/math/geometry/Makefile.module | 2 +- digital/avr/modules/math/geometry/distance.c | 71 ++++++++++++++++++++++ digital/avr/modules/math/geometry/distance.h | 36 +++++++++++ .../avr/modules/math/geometry/test/test_geometry.c | 33 ++++++++++ 4 files changed, 141 insertions(+), 1 deletion(-) create mode 100644 digital/avr/modules/math/geometry/distance.c create mode 100644 digital/avr/modules/math/geometry/distance.h (limited to 'digital/avr/modules/math/geometry') 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'); -- cgit v1.2.3