rsys

commit 9703c81891081db3cbf808bbe8164d6281506efd
parent 01ac7c6e899db34aa4eaa34476a28325bd4a7431
Author: vaplv <vaplv@free.fr>
Date:   Thu,  9 May 2019 11:00:13 +0200

Add and test the f22_rotation and d22_rotation functions

Build a 2x2 rotation matrix in the XY plane.

Diffstat:
M
cmake/CMakeLists.txt
 | 
2
+-
M
src/real22.h
 | 
12
++++++++++++
M
src/test_real22.h
 | 
24
++++++++++++++++++++++++

3 files changed, 37 insertions(+), 1 deletion(-)
diff --git a/cmake/CMakeLists.txt b/cmake/CMakeLists.txt
@@ -211,7 +211,7 @@ if(NOT NO_TEST)
   new_test(test_float2 ${MATH_LIB})
   new_test(test_float3 ${MATH_LIB})
   new_test(test_float4 ${MATH_LIB})
-  new_test(test_float22)
+  new_test(test_float22 ${MATH_LIB})
   new_test(test_float33 ${MATH_LIB})
   new_test(test_float44)
   new_test(test_free_list rsys)
diff --git a/src/real22.h b/src/real22.h
@@ -71,5 +71,17 @@ REALXY_FUNC__(invtrans)(REAL_TYPE__* dst, const REAL_TYPE__* mat)
   return det;
 }
 
+static FINLINE REAL_TYPE__*
+REALXY_FUNC__(rotation)
+  (REAL_TYPE__ dst[4], const REAL_TYPE__ angle/*in radian*/)
+{
+  const REAL_TYPE__ c = (REAL_TYPE__)cos((double)angle);
+  const REAL_TYPE__ s = (REAL_TYPE__)sin((double)angle);
+  ASSERT(dst);
+  dst[0] = c; dst[1] = s;
+  dst[2] =-s; dst[3] = c;
+  return dst;
+}
+
 #include "realXY_end.h"
 
diff --git a/src/test_real22.h b/src/test_real22.h
@@ -161,6 +161,30 @@ main(int argc, char** argv)
   CHK(REALXY_FUNC__(invtrans)(a, a) ==  -2.0);
   CHECK_REAL22(a, REALXY_CTOR__(c, -2.0, 1.0, 1.5, -0.5));
 
+  CHK(REALXY_FUNC__(rotation)(c, (REAL_TYPE__)PI/4) == c);
+
+  REALX_CTOR__(a, 1, 0);
+  CHK(REALXY_REALX_FUNC__(mul)(a, c, a) == a);
+  REALX_FUNC__(splat)(c, (REAL_TYPE__)(sqrt(2.0)/2.0));
+  CHK(REALX_FUNC__(eq_eps)(a, c, (REAL_TYPE__)1.e-6));
+
+  REALX_CTOR__(b, 0, -1);
+  CHK(REALXY_REALX_FUNC__(mul)(b, c, b) == b);
+  REALX_CTOR__(c, (REAL_TYPE__)(sqrt(2.0)/2.0), -(REAL_TYPE__)(sqrt(2.0)/2.0));
+  CHK(REALX_FUNC__(eq_eps)(b, c, (REAL_TYPE__)1.e-6));
+
+  CHK(REALXY_FUNC__(rotation)(c, -(REAL_TYPE__)PI/2) == c);
+
+  REALX_CTOR__(a, 1, 0);
+  CHK(REALXY_REALX_FUNC__(mul)(a, c, a) == a);
+  REALX_CTOR__(c, 0, -1);
+  CHK(REALX_FUNC__(eq_eps)(a, c, (REAL_TYPE__)1.e-6));
+
+  REALX_CTOR__(b, 0, -1);
+  CHK(REALXY_REALX_FUNC__(mul)(b, c, b) == b);
+  REALX_CTOR__(c, -1, 0);
+  CHK(REALX_FUNC__(eq_eps)(b, c, (REAL_TYPE__)1.e-6));
+
   return 0;
 }