summaryrefslogtreecommitdiff
path: root/digital/avr/modules
diff options
context:
space:
mode:
Diffstat (limited to 'digital/avr/modules')
-rw-r--r--digital/avr/modules/math/geometry/distance.c9
-rw-r--r--digital/avr/modules/math/geometry/distance.h5
-rw-r--r--digital/avr/modules/math/geometry/test/test_geometry.c13
3 files changed, 27 insertions, 0 deletions
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;