summaryrefslogtreecommitdiffhomepage
path: root/digital/avr
diff options
context:
space:
mode:
authorNicolas Schodet2010-04-13 00:21:52 +0200
committerNicolas Schodet2010-04-13 00:21:52 +0200
commit7f24f37772871ad4a420f59f93b97ab1ad9b0e24 (patch)
treedba5e34822f7937399abf6be56cd66f52297ccd2 /digital/avr
parentd7211fc08b077030cf7f065d3834e201d17c18a5 (diff)
digital/avr/modules/math/geometry: add normal and dot product
Diffstat (limited to 'digital/avr')
-rw-r--r--digital/avr/modules/math/geometry/test/test_geometry.c32
-rw-r--r--digital/avr/modules/math/geometry/vect.c15
-rw-r--r--digital/avr/modules/math/geometry/vect.h11
3 files changed, 58 insertions, 0 deletions
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
@@ -84,6 +84,21 @@ vect_from_polar_uf016 (vect_t *v, int16_t l, uint16_t a_uf016)
}
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)
{
for (; vn; va++, vn--)
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.