From 3d4c3356183686b97e8fa750b73ce727a59ff89f Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 27 Apr 2010 00:17:22 +0200 Subject: digital/avr/modules/math/geometry: add distance_point_point --- digital/avr/modules/math/geometry/distance.c | 9 +++++++++ digital/avr/modules/math/geometry/distance.h | 5 +++++ digital/avr/modules/math/geometry/test/test_geometry.c | 13 +++++++++++++ 3 files changed, 27 insertions(+) diff --git a/digital/avr/modules/math/geometry/distance.c b/digital/avr/modules/math/geometry/distance.c index b5912aaa..348087fd 100644 --- a/digital/avr/modules/math/geometry/distance.c +++ b/digital/avr/modules/math/geometry/distance.c @@ -26,6 +26,15 @@ #include "distance.h" #include "modules/utils/utils.h" +#include "modules/math/fixed/fixed.h" + +int16_t +distance_point_point (vect_t *a, vect_t *b) +{ + int16_t dx = b->x - a->x; + int16_t dy = b->y - a->y; + return fixed_sqrt_ui32 (dx * dx + dy * dy); +} int16_t distance_segment_point (vect_t *a, vect_t *b, vect_t *p) diff --git a/digital/avr/modules/math/geometry/distance.h b/digital/avr/modules/math/geometry/distance.h index 77bc1e8c..c48c2226 100644 --- a/digital/avr/modules/math/geometry/distance.h +++ b/digital/avr/modules/math/geometry/distance.h @@ -26,6 +26,11 @@ * }}} */ #include "modules/math/geometry/vect.h" +/** Compute distance between two points. + * - a, b: points. */ +int16_t +distance_point_point (vect_t *a, vect_t *b); + /** Compute distance between a segment and a point. * - a: segment first vertice. * - b: segment second vertice. diff --git a/digital/avr/modules/math/geometry/test/test_geometry.c b/digital/avr/modules/math/geometry/test/test_geometry.c index 90c063bc..17afb787 100644 --- a/digital/avr/modules/math/geometry/test/test_geometry.c +++ b/digital/avr/modules/math/geometry/test/test_geometry.c @@ -197,6 +197,19 @@ test_distance (void) { vect_t a, b, p; int16_t d; + /* distance_point_point. */ + a.x = 100; a.y = 100; + b.x = 200; b.y = 200; + d = distance_point_point (&a, &b); + test (d == 141); + a.x = -100; a.y = -100; + b.x = -200; b.y = -200; + d = distance_point_point (&a, &b); + test (d == 141); + a.x = 0; a.y = -100; + b.x = 0; b.y = 200; + d = distance_point_point (&a, &b); + test (d == 300); /* distance_segment_point. */ a.x = 300; a.y = 200; b.x = 900; b.y = 500; -- cgit v1.2.3