From 7f24f37772871ad4a420f59f93b97ab1ad9b0e24 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 13 Apr 2010 00:21:52 +0200 Subject: digital/avr/modules/math/geometry: add normal and dot product --- .../avr/modules/math/geometry/test/test_geometry.c | 32 ++++++++++++++++++++++ digital/avr/modules/math/geometry/vect.c | 15 ++++++++++ digital/avr/modules/math/geometry/vect.h | 11 ++++++++ 3 files changed, 58 insertions(+) diff --git a/digital/avr/modules/math/geometry/test/test_geometry.c b/digital/avr/modules/math/geometry/test/test_geometry.c index 59d1f854..55c5cb1f 100644 --- a/digital/avr/modules/math/geometry/test/test_geometry.c +++ b/digital/avr/modules/math/geometry/test/test_geometry.c @@ -80,6 +80,7 @@ test_vect (void) { int i; int16_t n; + int32_t p; vect_t a, b; /* vect_*. */ a.x = 50; a.y = 0; @@ -120,6 +121,37 @@ test_vect (void) test (a.x == 0 && a.y == 141); vect_from_polar_uf016 (&a, 141, G_ANGLE_UF016_DEG (180)); test (a.x == -141 && a.y == 0); + /* vect_normal. */ + a.x = 10; a.y = 20; + vect_normal (&a); + test (a.x == -20 && a.y == 10); + vect_normal (&a); + test (a.x == -10 && a.y == -20); + vect_normal (&a); + test (a.x == 20 && a.y == -10); + vect_normal (&a); + test (a.x == 10 && a.y == 20); + /* vect_dot_product. */ + a.x = 100; a.y = 0; + b.x = 100; b.y = 0; + p = vect_dot_product (&a, &b); + test (p == 10000); + a.x = 100; a.y = 0; + b.x = 0; b.y = 100; + p = vect_dot_product (&a, &b); + test (p == 0); + a.x = -100; a.y = 0; + b.x = 100; b.y = 0; + p = vect_dot_product (&a, &b); + test (p == -10000); + vect_from_polar_uf016 (&a, 100, G_ANGLE_UF016_DEG (0)); + vect_from_polar_uf016 (&b, 100, G_ANGLE_UF016_DEG (60)); + p = vect_dot_product (&a, &b); + test (p == 5000); + a.x = 30000; a.y = 0; + b.x = 30000; b.y = 0; + p = vect_dot_product (&a, &b); + test (p == 900000000l); /* vect_array_*. */ vect_t c[4] = { diff --git a/digital/avr/modules/math/geometry/vect.c b/digital/avr/modules/math/geometry/vect.c index 57b5a4d4..88cc125f 100644 --- a/digital/avr/modules/math/geometry/vect.c +++ b/digital/avr/modules/math/geometry/vect.c @@ -83,6 +83,21 @@ vect_from_polar_uf016 (vect_t *v, int16_t l, uint16_t a_uf016) v->y = fixed_mul_f824 (l, fixed_sin_f824 (a_uf824)); } +void +vect_normal (vect_t *v) +{ + assert (v); + int16_t tx = v->x, ty = v->y; + v->x = -ty; + v->y = tx; +} + +int32_t +vect_dot_product (const vect_t *a, const vect_t *b) +{ + return (int32_t) a->x * b->x + (int32_t) b->y * a->y; +} + void vect_array_scale_f824 (vect_t *va, uint8_t vn, int32_t s_f824) { diff --git a/digital/avr/modules/math/geometry/vect.h b/digital/avr/modules/math/geometry/vect.h index 6a1cc570..17fda6cb 100644 --- a/digital/avr/modules/math/geometry/vect.h +++ b/digital/avr/modules/math/geometry/vect.h @@ -83,6 +83,17 @@ vect_norm (const vect_t *v); void vect_from_polar_uf016 (vect_t *v, int16_t l, uint16_t a_uf016); +/** Compute a vector normal (rotate the vector counter-clockwise of pi/2). + * - v: input/result vector. */ +void +vect_normal (vect_t *v); + +/** Compute the dot product (scalar product) of two vectors. + * - a, b: vectors to make product of. + * - returns: dot product. */ +int32_t +vect_dot_product (const vect_t *a, const vect_t *b); + /** Scale (multiply) vectors by a fixed point value. * - va: input/result vectors array. * - vn: number of vectors. -- cgit v1.2.3