From e0c9bedbaa25f8ab21294c9c210910479f6c52f6 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 27 Apr 2010 00:16:57 +0200 Subject: digital/avr/modules/math/geometry: add vect_normal_dot_product --- .../avr/modules/math/geometry/test/test_geometry.c | 21 +++++++++++++++++++++ digital/avr/modules/math/geometry/vect.c | 6 ++++++ digital/avr/modules/math/geometry/vect.h | 10 ++++++++++ 3 files changed, 37 insertions(+) (limited to 'digital/avr/modules') diff --git a/digital/avr/modules/math/geometry/test/test_geometry.c b/digital/avr/modules/math/geometry/test/test_geometry.c index f18aadd9..90c063bc 100644 --- a/digital/avr/modules/math/geometry/test/test_geometry.c +++ b/digital/avr/modules/math/geometry/test/test_geometry.c @@ -153,6 +153,27 @@ test_vect (void) b.x = 30000; b.y = 0; p = vect_dot_product (&a, &b); test (p == 900000000l); + /* vect_normal_dot_product. */ + a.x = 0; a.y = -100; + b.x = 100; b.y = 0; + p = vect_normal_dot_product (&a, &b); + test (p == 10000); + a.x = 0; a.y = -100; + b.x = 0; b.y = 100; + p = vect_normal_dot_product (&a, &b); + test (p == 0); + a.x = 0; a.y = 100; + b.x = 100; b.y = 0; + p = vect_normal_dot_product (&a, &b); + test (p == -10000); + vect_from_polar_uf016 (&a, 100, G_ANGLE_UF016_DEG (-90)); + vect_from_polar_uf016 (&b, 100, G_ANGLE_UF016_DEG (60)); + p = vect_normal_dot_product (&a, &b); + test (p == 5000); + a.x = 0; a.y = -30000; + b.x = 30000; b.y = 0; + p = vect_normal_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 88cc125f..821f6844 100644 --- a/digital/avr/modules/math/geometry/vect.c +++ b/digital/avr/modules/math/geometry/vect.c @@ -98,6 +98,12 @@ vect_dot_product (const vect_t *a, const vect_t *b) return (int32_t) a->x * b->x + (int32_t) b->y * a->y; } +int32_t +vect_normal_dot_product (const vect_t *a, const vect_t *b) +{ + return (int32_t) a->x * b->y - (int32_t) b->x * 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 17fda6cb..0042db7a 100644 --- a/digital/avr/modules/math/geometry/vect.h +++ b/digital/avr/modules/math/geometry/vect.h @@ -94,6 +94,16 @@ vect_normal (vect_t *v); int32_t vect_dot_product (const vect_t *a, const vect_t *b); +/** Compute the dot product (scalar product) of a normal of the first vector + * with the second vector. + * - a: vector to take normal of. + * - b: vector to make product with. + * - returns: dot product. + * The first vector is rotated of pi/2. The result is the same as the z part + * of the vector product. */ +int32_t +vect_normal_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