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/distance.c | 71 ++++++++++++++++++++++++++++ 1 file changed, 71 insertions(+) create mode 100644 digital/avr/modules/math/geometry/distance.c (limited to 'digital/avr/modules/math/geometry/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); +} + -- cgit v1.2.3