rsys

commit 6abe368d089426535925565cba4f08b86703267f
parent bbd561e4c6e50a09b0bb123051d1f313760e13ca
Author: vaplv <vaplv@free.fr>
Date:   Thu, 12 Feb 2015 16:35:37 +0100

Add and test the f33_rotation_axis_angle function

Diffstat:
M
cmake/CMakeLists.txt
 | 
2
+-
M
src/float33.h
 | 
25
+++++++++++++++++++++++++
M
src/test_float33.c
 | 
15
+++++++++------

3 files changed, 35 insertions(+), 7 deletions(-)
diff --git a/cmake/CMakeLists.txt b/cmake/CMakeLists.txt
@@ -123,7 +123,7 @@ if(NOT NO_TEST)
   new_test(test_float3 m)
   new_test(test_float4 m)
   new_test(test_float22)
-  new_test(test_float33)
+  new_test(test_float33 m)
   new_test(test_float44 rsys)
   new_test(test_free_list rsys)
   new_test(test_hash_table rsys)
diff --git a/src/float33.h b/src/float33.h
@@ -93,6 +93,31 @@ f33_rotation /* XYZ norm */
 }
 
 static INLINE float*
+f33_rotation_axis_angle
+  (float* dst,
+   const float axis[3], /* Should be normalized */
+   const float angle) /* In radian */
+{
+  const float c = (float)cos((double)angle);
+  const float s = (float)sin((double)angle);
+  const float C = 1 - c;
+  ASSERT(dst && axis && f3_is_normalized(axis));
+
+  dst[0] = axis[0] * axis[0] * C + c;
+  dst[1] = axis[0] * axis[1] * C + s * axis[2];
+  dst[2] = axis[0] * axis[2] * C - s * axis[1];
+
+  dst[3] = axis[1] * axis[0] * C - s * axis[2];
+  dst[4] = axis[1] * axis[1] * C + c;
+  dst[5] = axis[1] * axis[2] * C + s * axis[0];
+
+  dst[6] = axis[2] * axis[0] * C + s * axis[1];
+  dst[7] = axis[2] * axis[1] * C - s * axis[0];
+  dst[8] = axis[2] * axis[2] * C + c;
+  return dst;
+}
+
+static INLINE float*
 f33_rotation_pitch(float dst[9], const float pitch/* in radian */)
 {
   const float c = (float)cos((double)pitch);
diff --git a/src/test_float33.c b/src/test_float33.c
@@ -165,15 +165,18 @@ main(int argc, char** argv)
   CHECK(f33_det(a), -60.f);
   CHECK(f33_inverse(b, a), -60.f);
   f33_mulf33(dst, a, b);
-  CHECK
-    (f33_eq_eps
-      (dst,
-       f33(c, 1.0f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 1.f),
-       1.e-6f),
-      1);
+  CHECK(f33_eq_eps(dst, f33(c, 1.0f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 1.f),
+    1.e-6f), 1);
   CHECK(f33_invtrans(a, a), -60.f);
   CHECK_F33
     (a, f33(c, b[0], b[3], b[6], b[1], b[4], b[7], b[2], b[5], b[8]));
 
+  f3(c, 0.666667f, 0.333333f, 0.666667f);
+  f33_rotation_axis_angle(a, c, 0.349066f);
+  CHECK(f33_eq_eps(a, f33(c, 
+    0.966496f, 0.241415f, -0.0872034f,
+    -0.214612f, 0.946393f, 0.241415f,
+    0.14081f, -0.214612f, 0.966496f), 1.e-6f), 1);
+
   return 0;
 }