rsys

commit a2a9433d509fccfb1a4d49558f7c62bb124d151c
parent a04ed8aeefea3efcdc563f14424422efc15a3252
Author: vaplv <vaplv@free.fr>
Date:   Wed,  1 Oct 2014 22:07:28 +0200

Add the f33_rotation_<pitch|yaw|roll> functions

Diffstat:
M
src/float33.h
 | 
51
++++++++++++++++++++++++++++++++++++++++++++-------

1 file changed, 44 insertions(+), 7 deletions(-)
diff --git a/src/float33.h b/src/float33.h
@@ -28,13 +28,13 @@
 /* Specific float33 funcs */
 static FINLINE float*
 f33
-  (float* dst, 
-   const float a, const float b, const float c, 
+  (float* dst,
+   const float a, const float b, const float c,
    const float d, const float e, const float f,
    const float g, const float h, const float i)
 {
   ASSERT(dst);
-  dst[0] = a; dst[1] = b; dst[2] = c; 
+  dst[0] = a; dst[1] = b; dst[2] = c;
   dst[3] = d; dst[4] = e; dst[5] = f;
   dst[6] = g; dst[7] = h; dst[8] = i;
   return dst;
@@ -71,11 +71,12 @@ f33_inverse(float* dst, const float* src)
   return det;
 }
 
-static FINLINE float*
+static INLINE float*
 f33_rotation /* XYZ norm */
-  (float* dst, 
-   const float pitch, 
-   const float yaw, 
+  (float* dst,
+   /* In radian */
+   const float pitch,
+   const float yaw,
    const float roll)
 {
   const float c1 = (float)cos((double)pitch);
@@ -91,5 +92,41 @@ f33_rotation /* XYZ norm */
   return dst;
 }
 
+static INLINE float*
+f33_rotation_pitch(float dst[9], const float pitch/* in radian */)
+{
+  const float c = (float)cos((double)pitch);
+  const float s = (float)sin((double)pitch);
+  ASSERT(dst);
+  dst[0] = 1.f; dst[1] = 0.f; dst[2] = 0.f;
+  dst[3] = 0.f; dst[4] = c;   dst[5] = s;
+  dst[6] = 0.f; dst[7] =-s;   dst[8] = c;
+  return dst;
+}
+
+static INLINE float*
+f33_rotation_yaw(float dst[9], const float yaw/* in radian */)
+{
+  const float c = (float)cos((double)yaw);
+  const float s = (float)sin((double)yaw);
+  ASSERT(dst);
+  dst[0] = c;   dst[1] = 0.f; dst[2] =-s;
+  dst[3] = 0.f; dst[4] = 1.f; dst[5] = 0.f;
+  dst[6] = s;   dst[7] = 0.f; dst[8] = c;
+  return dst;
+}
+
+static INLINE float*
+f33_rotation_roll(float dst[9], const float roll/* in radian */)
+{
+  const float c = (float)cos((double)roll);
+  const float s = (float)sin((double)roll);
+  ASSERT(dst);
+  dst[0] = c;   dst[1] = s;   dst[2] = 0.f;
+  dst[3] =-s;   dst[4] = c;   dst[5] = 0.f;
+  dst[6] = 0.f; dst[7] = 0.f; dst[8] = 1.f;
+  return dst;
+}
+
 #endif /* FLOAT33_H */