commit 32de987f38ce71b0478738fd9c7632f0dc3f15a5
parent fad8dd101e4a5e72f36e6cd7e32a96ab25646660
Author: vaplv <vaplv@free.fr>
Date: Mon, 4 Jul 2016 17:00:11 +0200
Make the float<22|33|44> types generic to the data type
Diffstat:
15 files changed, 520 insertions(+), 408 deletions(-)
diff --git a/cmake/CMakeLists.txt b/cmake/CMakeLists.txt
@@ -42,7 +42,6 @@ set(VERSION ${VERSION_MAJOR}.${VERSION_MINOR}.${VERSION_PATCH})
set(RSYS_FILES_SRC
clock_time.c
cstr.c
- float44.c
image.c
library.c
logger.c
@@ -100,7 +99,12 @@ set(RSYS_FILES_INC_API
realX.h
realX_begin.h
realX_end.h
+ real22.h
+ real33.h
+ real44.h
realXY.h
+ realXY_begin.h
+ realXY_end.h
ref_count.h
rsys.h
signal.h
diff --git a/src/float22.h b/src/float22.h
@@ -18,54 +18,8 @@
#include "float2.h"
-/* Generate common floatXY funcs */
-#define REALX_DIMENSION__ 2
-#define REALY_DIMENSION__ 2
#define REAL_TYPE__ float
-#include "realXY.h"
-
-/* Specific float22 funcs */
-static FINLINE float*
-f22(float* dst, const float a, const float b, const float c, const float d)
-{
- ASSERT(dst);
- dst[0] = a; dst[1] = b; dst[2] = c; dst[3] = d;
- return dst;
-}
-
-static FINLINE float
-f22_det(const float* mat)
-{
- return mat[0] * mat[3] - mat[2] * mat[1];
-}
-
-static FINLINE float
-f22_inverse(float* dst, const float* mat)
-{
- float det, rcp_det, mat0;
- ASSERT(dst && mat);
- det = f22_det(mat);
- rcp_det = 1.f / det;
- mat0 = mat[0];
- dst[0] = mat[3] * rcp_det;
- dst[1] = -mat[1] * rcp_det;
- dst[2] = -mat[2] * rcp_det;
- dst[3] = mat0 * rcp_det;
- return det;
-}
-
-static FINLINE float
-f22_invtrans(float* dst, const float* mat)
-{
- float det, dst1, dst2;
- ASSERT(dst && mat);
- det = f22_inverse(dst, mat);
- dst1 = dst[1];
- dst2 = dst[2];
- dst[1] = dst2;
- dst[2] = dst1;
- return det;
-}
+#include "real22.h"
#endif /* FLOAT22_H */
diff --git a/src/float33.h b/src/float33.h
@@ -17,162 +17,9 @@
#define FLOAT33_H
#include "float3.h"
-#include <math.h>
-/* Generate common floatXY funcs */
-#define REALX_DIMENSION__ 3
-#define REALY_DIMENSION__ 3
#define REAL_TYPE__ float
-#include "realXY.h"
-
-/* Specific float33 funcs */
-static FINLINE float*
-f33
- (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[3] = d; dst[4] = e; dst[5] = f;
- dst[6] = g; dst[7] = h; dst[8] = i;
- return dst;
-}
-
-static FINLINE float
-f33_det(const float* mat)
-{
- float tmp[3];
- f3_cross(tmp, f33_col_cptr(mat, 0), f33_col_cptr(mat, 1));
- return f3_dot(f33_col_cptr(mat, 2), tmp);
-}
-
-static FINLINE float
-f33_invtrans(float* dst, const float* src)
-{
- float f33[9];
- float det;
- f3_cross(f33_col_ptr(f33, 0), f33_col_cptr(src, 1), f33_col_cptr(src, 2));
- f3_cross(f33_col_ptr(f33, 1), f33_col_cptr(src, 2), f33_col_cptr(src, 0));
- f3_cross(f33_col_ptr(f33, 2), f33_col_cptr(src, 0), f33_col_cptr(src, 1));
-
- det = f3_dot(f33_col_cptr(f33, 2), f33_col_cptr(src, 2));
- f33_mul(dst, f33, 1.f / det);
- return det;
-}
-
-static FINLINE float
-f33_inverse(float* dst, const float* src)
-{
- float f33[9];
- const float det = f33_invtrans(f33, src);
- f33_transpose(dst, f33);
- return det;
-}
-
-static INLINE float*
-f33_rotation /* XYZ norm */
- (float* dst,
- /* In radian */
- const float pitch,
- const float yaw,
- const float roll)
-{
- const float c1 = (float)cos((double)pitch);
- const float c2 = (float)cos((double)yaw);
- const float c3 = (float)cos((double)roll);
- const float s1 = (float)sin((double)pitch);
- const float s2 = (float)sin((double)yaw);
- const float s3 = (float)sin((double)roll);
- ASSERT(dst);
- dst[0] = c2*c3; dst[1] = c1*s3 + c3*s1*s2; dst[2] = s1*s3 - c1*c3*s2;
- dst[3] =-c2*s3; dst[4] = c1*c3 - s1*s2*s3; dst[5] = c1*s2*s3 + c3*s1;
- dst[6] = s2; dst[7] =-c2*s1; dst[8] = c1*c2;
- return dst;
-}
-
-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);
- 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;
-}
-
-static INLINE float*
-f33_basis(float dst[9], const float N[3])
-{
- float a[3], b[3], x[3], y[3], normal[3], len;
- ASSERT(N);
- f3_set(normal, N);
- f3_cross(a, f3(a, 1.f, 0.f, 0.f), normal);
- f3_cross(b, f3(b, 0.f, 1.f, 0.f), normal);
- len = f3_normalize(x, f3_dot(a, a) > f3_dot(b, b) ? a : b);
- if(len <= 0.f) {
- ASSERT(0 && "Degenerated normal");
- return f33_splat(dst, 0.f);
- }
- len = f3_normalize(y, f3_cross(y, normal, x));
- ASSERT(len > 0);
- f3_set(dst + 0, x);
- f3_set(dst + 3, y);
- f3_set(dst + 6, normal);
- return dst;
-}
+#include "real33.h"
#endif /* FLOAT33_H */
diff --git a/src/float44.c b/src/float44.c
@@ -1,84 +0,0 @@
-/* Copyright (C) 2013-2016 Vincent Forest (vaplv@free.fr)
- *
- * The RSys library is free software: you can redistribute it and/or modify
- * it under the terms of the GNU Lesser General Public License as published
- * by the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * The RSys library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public License
- * along with the RSys library. If not, see <http://www.gnu.org/licenses/>. */
-
-#include "float44.h"
-
-float
-f44_inverse(float* dst, const float* m)
-{
- float tmp[9];
- float det_012[4], det_023[4], det_123[4], det_013[4];
- float cofacts[4];
- float det, rcp_det, mpmp_rcp_det[4], pmpm_rcp_det[4];
- ASSERT( dst && m );
-
- /* Define the 3x3 sub matrices and compute their determinants */
- #define C3( Dst, Mat, ICol, X, Y, Z ) \
- (Dst)[0] = Mat[ICol*4+X], (Dst)[1] = Mat[ICol*4+Y], (Dst)[2] = Mat[ICol*4+Z]
- C3(tmp+0 ,m, 1, 0, 1, 2); C3(tmp+3, m, 2, 0, 1, 2); C3(tmp+6, m, 3, 0, 1, 2);
- det_012[0] = f33_det(tmp);
- C3(tmp+0 ,m, 0, 0, 1, 2); C3(tmp+3, m, 2, 0, 1, 2); C3(tmp+6, m, 3, 0, 1, 2);
- det_012[1] = f33_det(tmp);
- C3(tmp+0 ,m, 0, 0, 1, 2); C3(tmp+3, m, 1, 0, 1, 2); C3(tmp+6, m, 3, 0, 1, 2);
- det_012[2] = f33_det(tmp);
- C3(tmp+0 ,m, 0, 0, 1, 2); C3(tmp+3, m, 1, 0, 1, 2); C3(tmp+6, m, 2, 0, 1, 2);
- det_012[3] = f33_det(tmp);
-
- C3(tmp+0 ,m, 1, 0, 2, 3); C3(tmp+3, m, 2, 0, 2, 3); C3(tmp+6, m, 3, 0, 2, 3);
- det_023[0] = f33_det(tmp);
- C3(tmp+0 ,m, 0, 0, 2, 3); C3(tmp+3, m, 2, 0, 2, 3); C3(tmp+6, m, 3, 0, 2, 3);
- det_023[1] = f33_det(tmp);
- C3(tmp+0 ,m, 0, 0, 2, 3); C3(tmp+3, m, 1, 0, 2, 3); C3(tmp+6, m, 3, 0, 2, 3);
- det_023[2] = f33_det(tmp);
- C3(tmp+0 ,m, 0, 0, 2, 3); C3(tmp+3, m, 1, 0, 2, 3); C3(tmp+6, m, 2, 0, 2, 3);
- det_023[3] = f33_det(tmp);
-
- C3(tmp+0 ,m, 1, 1, 2, 3); C3(tmp+3, m, 2, 1, 2, 3); C3(tmp+6, m, 3, 1, 2, 3);
- det_123[0] = f33_det(tmp);
- C3(tmp+0 ,m, 0, 1, 2, 3); C3(tmp+3, m, 2, 1, 2, 3); C3(tmp+6, m, 3, 1, 2, 3);
- det_123[1] = f33_det(tmp);
- C3(tmp+0 ,m, 0, 1, 2, 3); C3(tmp+3, m, 1, 1, 2, 3); C3(tmp+6, m, 3, 1, 2, 3);
- det_123[2] = f33_det(tmp);
- C3(tmp+0 ,m, 0, 1, 2, 3); C3(tmp+3, m, 1, 1, 2, 3); C3(tmp+6, m, 2, 1, 2, 3);
- det_123[3] = f33_det(tmp);
-
- C3(tmp+0 ,m, 1, 0, 1, 3); C3(tmp+3, m, 2, 0, 1, 3); C3(tmp+6, m, 3, 0, 1, 3);
- det_013[0] = f33_det(tmp);
- C3(tmp+0 ,m, 0, 0, 1, 3); C3(tmp+3, m, 2, 0, 1, 3); C3(tmp+6, m, 3, 0, 1, 3);
- det_013[1] = f33_det(tmp);
- C3(tmp+0 ,m, 0, 0, 1, 3); C3(tmp+3, m, 1, 0, 1, 3); C3(tmp+6, m, 3, 0, 1, 3);
- det_013[2] = f33_det(tmp);
- C3(tmp+0 ,m, 0, 0, 1, 3); C3(tmp+3, m, 1, 0, 1, 3); C3(tmp+6, m, 2, 0, 1, 3);
- det_013[3] = f33_det(tmp);
- #undef C3
-
- f4_set(cofacts, f4(tmp, -det_012[0], det_012[1], -det_012[2], det_012[3]));
-
- /* Compute the determinant of src */
- det = f4_dot(cofacts, f4(tmp, m[3], m[7], m[11], m[15]));
-
- /* Invert the matrix */
- if( det == 0.f )
- return det;
-
- rcp_det = 1.f / det;
- f4_set(mpmp_rcp_det, f4(tmp,-rcp_det, rcp_det,-rcp_det, rcp_det));
- f4_set(pmpm_rcp_det, f4(tmp, rcp_det,-rcp_det, rcp_det,-rcp_det));
- f4_mul(f44_col_ptr(dst, 0), det_123, pmpm_rcp_det);
- f4_mul(f44_col_ptr(dst, 1), det_023, mpmp_rcp_det);
- f4_mul(f44_col_ptr(dst, 2), det_013, pmpm_rcp_det);
- f4_mul(f44_col_ptr(dst, 3), det_012, mpmp_rcp_det);
- return det;
-}
diff --git a/src/float44.h b/src/float44.h
@@ -16,68 +16,11 @@
#ifndef FLOAT44_H
#define FLOAT44_H
-#include "float33.h"
#include "float4.h"
+#include "float33.h"
-/* Generate common floatXY funcs */
-#define REALX_DIMENSION__ 4
-#define REALY_DIMENSION__ 4
#define REAL_TYPE__ float
-#include "realXY.h"
-
-/* Specific float44 funcs*/
-static FINLINE float*
-f44
- (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, const float j, const float k, const float l,
- const float m, const float n, const float o, const float p)
-{
- ASSERT(dst);
- 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; dst[9] = j; dst[10] = k; dst[11] = l;
- dst[12] = m; dst[13] = n; dst[14] = o; dst[15] = p;
- return dst;
-}
-
-static FINLINE float
-f44_det(const float* mat)
-{
- float f33[9];
- float row3[4], tmp[4];
- ASSERT( mat );
-
- #define C3( Dst, Mat, ICol ) \
- (Dst)[0] = Mat[ICol*4], (Dst)[1] = Mat[ICol*4+1], (Dst)[2] = Mat[ICol*4+2]
- C3(f33 + 0, mat, 1); C3(f33 + 3, mat, 2); C3(f33 + 6, mat, 3);
- tmp[0] = -f33_det(f33);
- C3(f33 + 0, mat, 0); C3(f33 + 3, mat, 2); C3(f33 + 6, mat, 3);
- tmp[1] = f33_det(f33);
- C3(f33 + 0, mat, 0); C3(f33 + 3, mat, 1); C3(f33 + 6, mat, 3);
- tmp[2] = -f33_det(f33);
- C3(f33 + 0, mat, 0); C3(f33 + 3, mat, 1); C3(f33 + 6, mat, 2);
- #undef C3
- tmp[3] = f33_det(f33);
- f44_row(row3, mat, 3);
- return f4_dot(tmp, row3);
-}
-
-BEGIN_DECLS
-
-RSYS_API float
-f44_inverse(float* dst, const float* mat);
-
-END_DECLS
-
-static FINLINE float
-f44_invtrans(float* dst, const float* mat)
-{
- const float det = f44_inverse(dst, mat);
- f44_transpose(dst, dst);
- return det;
-}
+#include "real44.h"
#endif /* FLOAT44_H */
diff --git a/src/math.h b/src/math.h
@@ -32,7 +32,6 @@
#define MDEG2RAD(Deg) ((Deg)*PI/180.0)
#define MRAD2DEG(Rad) ((Rad)*180.0/PI)
-
static FINLINE size_t
round_up_pow2(const size_t i)
{
diff --git a/src/real22.h b/src/real22.h
@@ -0,0 +1,75 @@
+/* Copyright (C) 2013-2016 Vincent Forest (vaplv@free.fr)
+ *
+ * The RSys library is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published
+ * by the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * The RSys library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with the RSys library. If not, see <http://www.gnu.org/licenses/>. */
+
+#ifndef REAL_TYPE__
+ #error Missing arguments
+#endif
+
+/* Generate common realXY funcs */
+#define REALX_DIMENSION__ 2
+#define REALY_DIMENSION__ 2
+#include "realXY_begin.h"
+#include "realXY.h"
+
+/* Specific real22 funcs */
+static FINLINE REAL_TYPE__*
+CONCAT(REAL_LETTER__, 22)
+ (REAL_TYPE__* dst,
+ const REAL_TYPE__ a,
+ const REAL_TYPE__ b,
+ const REAL_TYPE__ c,
+ const REAL_TYPE__ d)
+{
+ ASSERT(dst);
+ dst[0] = a; dst[1] = b; dst[2] = c; dst[3] = d;
+ return dst;
+}
+
+static FINLINE REAL_TYPE__
+REALXY_FUNC__(det)(const REAL_TYPE__* mat)
+{
+ return mat[0] * mat[3] - mat[2] * mat[1];
+}
+
+static FINLINE REAL_TYPE__
+REALXY_FUNC__(inverse)(REAL_TYPE__* dst, const REAL_TYPE__* mat)
+{
+ REAL_TYPE__ det, rcp_det, mat0;
+ ASSERT(dst && mat);
+ det = REALXY_FUNC__(det)(mat);
+ rcp_det = 1.f / det;
+ mat0 = mat[0];
+ dst[0] = mat[3] * rcp_det;
+ dst[1] = -mat[1] * rcp_det;
+ dst[2] = -mat[2] * rcp_det;
+ dst[3] = mat0 * rcp_det;
+ return det;
+}
+
+static FINLINE REAL_TYPE__
+REALXY_FUNC__(invtrans)(REAL_TYPE__* dst, const REAL_TYPE__* mat)
+{
+ REAL_TYPE__ det, dst1, dst2;
+ ASSERT(dst && mat);
+ det = REALXY_FUNC__(inverse)(dst, mat);
+ dst1 = dst[1];
+ dst2 = dst[2];
+ dst[1] = dst2;
+ dst[2] = dst1;
+ return det;
+}
+
+#include "realXY_end.h"
+
diff --git a/src/real33.h b/src/real33.h
@@ -0,0 +1,194 @@
+/* Copyright (C) 2013-2016 Vincent Forest (vaplv@free.fr)
+ *
+ * The RSys library is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published
+ * by the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * The RSys library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with the RSys library. If not, see <http://www.gnu.org/licenses/>. */
+
+#ifndef REAL_TYPE__
+ #error Missing arguments
+#endif
+
+/* Generate common realXY funcs */
+#define REALX_DIMENSION__ 3
+#define REALY_DIMENSION__ 3
+#include "realXY_begin.h"
+#include "realXY.h"
+
+/* Specific real33 funcs */
+static FINLINE REAL_TYPE__*
+CONCAT(REAL_LETTER__, 33)
+ (REAL_TYPE__* dst,
+ const REAL_TYPE__ a, const REAL_TYPE__ b, const REAL_TYPE__ c,
+ const REAL_TYPE__ d, const REAL_TYPE__ e, const REAL_TYPE__ f,
+ const REAL_TYPE__ g, const REAL_TYPE__ h, const REAL_TYPE__ i)
+{
+ ASSERT(dst);
+ 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;
+}
+
+static FINLINE REAL_TYPE__
+REALXY_FUNC__(det)(const REAL_TYPE__* mat)
+{
+ REAL_TYPE__ tmp[3];
+ REALX_FUNC__(cross)
+ (tmp, REALXY_FUNC__(col_cptr)(mat, 0), REALXY_FUNC__(col_cptr)(mat, 1));
+ return REALX_FUNC__(dot)(REALXY_FUNC__(col_cptr)(mat, 2), tmp);
+}
+
+static FINLINE REAL_TYPE__
+REALXY_FUNC__(invtrans)(REAL_TYPE__* dst, const REAL_TYPE__* src)
+{
+ REAL_TYPE__ f33[9];
+ REAL_TYPE__ det;
+ REALX_FUNC__(cross)
+ (REALXY_FUNC__(col_ptr)(f33, 0),
+ REALXY_FUNC__(col_cptr)(src, 1),
+ REALXY_FUNC__(col_cptr)(src, 2));
+ REALX_FUNC__(cross)
+ (REALXY_FUNC__(col_ptr)(f33, 1),
+ REALXY_FUNC__(col_cptr)(src, 2),
+ REALXY_FUNC__(col_cptr)(src, 0));
+ REALX_FUNC__(cross)
+ (REALXY_FUNC__(col_ptr)(f33, 2),
+ REALXY_FUNC__(col_cptr)(src, 0),
+ REALXY_FUNC__(col_cptr)(src, 1));
+
+ det = REALX_FUNC__(dot)
+ (REALXY_FUNC__(col_cptr)(f33, 2),
+ REALXY_FUNC__(col_cptr)(src, 2));
+ REALXY_FUNC__(mul)(dst, f33, 1.f / det);
+ return det;
+}
+
+static FINLINE REAL_TYPE__
+REALXY_FUNC__(inverse)(REAL_TYPE__* dst, const REAL_TYPE__* src)
+{
+ REAL_TYPE__ f33[9];
+ const REAL_TYPE__ det = REALXY_FUNC__(invtrans)(f33, src);
+ REALXY_FUNC__(transpose)(dst, f33);
+ return det;
+}
+
+static INLINE REAL_TYPE__*
+f33_rotation /* XYZ norm */
+ (REAL_TYPE__* dst,
+ /* In radian */
+ const REAL_TYPE__ pitch,
+ const REAL_TYPE__ yaw,
+ const REAL_TYPE__ roll)
+{
+ const REAL_TYPE__ c1 = (REAL_TYPE__)cos((double)pitch);
+ const REAL_TYPE__ c2 = (REAL_TYPE__)cos((double)yaw);
+ const REAL_TYPE__ c3 = (REAL_TYPE__)cos((double)roll);
+ const REAL_TYPE__ s1 = (REAL_TYPE__)sin((double)pitch);
+ const REAL_TYPE__ s2 = (REAL_TYPE__)sin((double)yaw);
+ const REAL_TYPE__ s3 = (REAL_TYPE__)sin((double)roll);
+ ASSERT(dst);
+ dst[0] = c2*c3; dst[1] = c1*s3 + c3*s1*s2; dst[2] = s1*s3 - c1*c3*s2;
+ dst[3] =-c2*s3; dst[4] = c1*c3 - s1*s2*s3; dst[5] = c1*s2*s3 + c3*s1;
+ dst[6] = s2; dst[7] =-c2*s1; dst[8] = c1*c2;
+ return dst;
+}
+
+static INLINE REAL_TYPE__*
+f33_rotation_axis_angle
+ (REAL_TYPE__* dst,
+ const REAL_TYPE__ axis[3], /* Should be normalized */
+ const REAL_TYPE__ angle) /* In radian */
+{
+ const REAL_TYPE__ c = (REAL_TYPE__)cos((double)angle);
+ const REAL_TYPE__ s = (REAL_TYPE__)sin((double)angle);
+ const REAL_TYPE__ C = 1 - c;
+ ASSERT(dst && axis && REALX_FUNC__(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 REAL_TYPE__*
+REALXY_FUNC__(rotation_pitch)
+ (REAL_TYPE__ dst[9], const REAL_TYPE__ pitch/* in radian */)
+{
+ const REAL_TYPE__ c = (REAL_TYPE__)cos((double)pitch);
+ const REAL_TYPE__ s = (REAL_TYPE__)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 REAL_TYPE__*
+REALXY_FUNC__(rotation_yaw)
+ (REAL_TYPE__ dst[9], const REAL_TYPE__ yaw/* in radian */)
+{
+ const REAL_TYPE__ c = (REAL_TYPE__)cos((double)yaw);
+ const REAL_TYPE__ s = (REAL_TYPE__)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 REAL_TYPE__*
+REALXY_FUNC__(rotation_roll)
+ (REAL_TYPE__ dst[9], const REAL_TYPE__ roll/* in radian */)
+{
+ const REAL_TYPE__ c = (REAL_TYPE__)cos((double)roll);
+ const REAL_TYPE__ s = (REAL_TYPE__)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;
+}
+
+static INLINE REAL_TYPE__*
+REALXY_FUNC__(basis)(REAL_TYPE__ dst[9], const REAL_TYPE__ N[3])
+{
+ REAL_TYPE__ a[3], b[3], x[3], y[3], normal[3], len;
+ ASSERT(N);
+ REALX_FUNC__(set)(normal, N);
+ a[0] = 1.f, a[1] = 0.f, a[2] = 0.f;
+ b[0] = 0.f, b[1] = 1.f, b[2] = 0.f;
+ REALX_FUNC__(cross)(a, a, normal);
+ REALX_FUNC__(cross)(b, b, normal);
+ len = REALX_FUNC__(normalize)
+ (x, REALX_FUNC__(dot)(a, a) > REALX_FUNC__(dot)(b, b) ? a : b);
+ if(len <= 0.f) {
+ ASSERT(0 && "Degenerated normal");
+ return REALXY_FUNC__(splat)(dst, 0.f);
+ }
+ len = REALX_FUNC__(normalize)(y, REALX_FUNC__(cross)(y, normal, x));
+ ASSERT(len > 0);
+ REALX_FUNC__(set)(dst + 0, x);
+ REALX_FUNC__(set)(dst + 3, y);
+ REALX_FUNC__(set)(dst + 6, normal);
+ return dst;
+}
+
+#include "realXY_end.h"
+
diff --git a/src/real44.h b/src/real44.h
@@ -0,0 +1,158 @@
+/* Copyright (C) 2013-2016 Vincent Forest (vaplv@free.fr)
+ *
+ * The RSys library is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published
+ * by the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * The RSys library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with the RSys library. If not, see <http://www.gnu.org/licenses/>. */
+
+#include "rsys.h"
+
+#ifndef REAL_TYPE__
+ #error Missing arguments
+#endif
+
+#ifdef REAL33_FUNC__
+ #error Unexpected macro definition
+#endif
+
+#define REAL33_FUNC__(Func) CONCAT(CONCAT(CONCAT(REAL_LETTER__, 33), _), Func)
+
+/* Generate common realXY funcs */
+#define REALX_DIMENSION__ 4
+#define REALY_DIMENSION__ 4
+#include "realXY_begin.h"
+#include "realXY.h"
+
+/* Specific REAL_TYPE__44 funcs*/
+static FINLINE REAL_TYPE__*
+CONCAT(REAL_LETTER__, 44)
+ (REAL_TYPE__* dst,
+ const REAL_TYPE__ a, const REAL_TYPE__ b, const REAL_TYPE__ c, const REAL_TYPE__ d,
+ const REAL_TYPE__ e, const REAL_TYPE__ f, const REAL_TYPE__ g, const REAL_TYPE__ h,
+ const REAL_TYPE__ i, const REAL_TYPE__ j, const REAL_TYPE__ k, const REAL_TYPE__ l,
+ const REAL_TYPE__ m, const REAL_TYPE__ n, const REAL_TYPE__ o, const REAL_TYPE__ p)
+{
+ ASSERT(dst);
+ 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; dst[9] = j; dst[10] = k; dst[11] = l;
+ dst[12] = m; dst[13] = n; dst[14] = o; dst[15] = p;
+ return dst;
+}
+
+static FINLINE REAL_TYPE__
+REALXY_FUNC__(det)(const REAL_TYPE__* mat)
+{
+ REAL_TYPE__ m33[9];
+ REAL_TYPE__ row3[4], tmp[4];
+ ASSERT(mat);
+
+ #define C3( Dst, Mat, ICol ) \
+ (Dst)[0] = Mat[ICol*4], (Dst)[1] = Mat[ICol*4+1], (Dst)[2] = Mat[ICol*4+2]
+ C3(m33 + 0, mat, 1); C3(m33 + 3, mat, 2); C3(m33 + 6, mat, 3);
+ tmp[0] = -REAL33_FUNC__(det)(m33);
+ C3(m33 + 0, mat, 0); C3(m33 + 3, mat, 2); C3(m33 + 6, mat, 3);
+ tmp[1] = REAL33_FUNC__(det)(m33);
+ C3(m33 + 0, mat, 0); C3(m33 + 3, mat, 1); C3(m33 + 6, mat, 3);
+ tmp[2] = -REAL33_FUNC__(det)(m33);
+ C3(m33 + 0, mat, 0); C3(m33 + 3, mat, 1); C3(m33 + 6, mat, 2);
+ #undef C3
+ tmp[3] = REAL33_FUNC__(det)(m33);
+ REALXY_FUNC__(row)(row3, mat, 3);
+ return REALX_FUNC__(dot)(tmp, row3);
+}
+
+static INLINE REAL_TYPE__
+REALXY_FUNC__(inverse)(REAL_TYPE__* dst, const REAL_TYPE__* m)
+{
+ REAL_TYPE__ tmp[9];
+ REAL_TYPE__ det_012[4], det_023[4], det_123[4], det_013[4];
+ REAL_TYPE__ cofacts[4];
+ REAL_TYPE__ det, rcp_det, mpmp_rcp_det[4], pmpm_rcp_det[4];
+ ASSERT( dst && m );
+
+ /* Define the 3x3 sub matrices and compute their determinants */
+ #define C3( Dst, Mat, ICol, X, Y, Z ) \
+ (Dst)[0] = Mat[ICol*4+X], (Dst)[1] = Mat[ICol*4+Y], (Dst)[2] = Mat[ICol*4+Z]
+ C3(tmp+0 ,m, 1, 0, 1, 2); C3(tmp+3, m, 2, 0, 1, 2); C3(tmp+6, m, 3, 0, 1, 2);
+ det_012[0] = REAL33_FUNC__(det)(tmp);
+ C3(tmp+0 ,m, 0, 0, 1, 2); C3(tmp+3, m, 2, 0, 1, 2); C3(tmp+6, m, 3, 0, 1, 2);
+ det_012[1] = REAL33_FUNC__(det)(tmp);
+ C3(tmp+0 ,m, 0, 0, 1, 2); C3(tmp+3, m, 1, 0, 1, 2); C3(tmp+6, m, 3, 0, 1, 2);
+ det_012[2] = REAL33_FUNC__(det)(tmp);
+ C3(tmp+0 ,m, 0, 0, 1, 2); C3(tmp+3, m, 1, 0, 1, 2); C3(tmp+6, m, 2, 0, 1, 2);
+ det_012[3] = REAL33_FUNC__(det)(tmp);
+
+ C3(tmp+0 ,m, 1, 0, 2, 3); C3(tmp+3, m, 2, 0, 2, 3); C3(tmp+6, m, 3, 0, 2, 3);
+ det_023[0] = REAL33_FUNC__(det)(tmp);
+ C3(tmp+0 ,m, 0, 0, 2, 3); C3(tmp+3, m, 2, 0, 2, 3); C3(tmp+6, m, 3, 0, 2, 3);
+ det_023[1] = REAL33_FUNC__(det)(tmp);
+ C3(tmp+0 ,m, 0, 0, 2, 3); C3(tmp+3, m, 1, 0, 2, 3); C3(tmp+6, m, 3, 0, 2, 3);
+ det_023[2] = REAL33_FUNC__(det)(tmp);
+ C3(tmp+0 ,m, 0, 0, 2, 3); C3(tmp+3, m, 1, 0, 2, 3); C3(tmp+6, m, 2, 0, 2, 3);
+ det_023[3] = REAL33_FUNC__(det)(tmp);
+
+ C3(tmp+0 ,m, 1, 1, 2, 3); C3(tmp+3, m, 2, 1, 2, 3); C3(tmp+6, m, 3, 1, 2, 3);
+ det_123[0] = REAL33_FUNC__(det)(tmp);
+ C3(tmp+0 ,m, 0, 1, 2, 3); C3(tmp+3, m, 2, 1, 2, 3); C3(tmp+6, m, 3, 1, 2, 3);
+ det_123[1] = REAL33_FUNC__(det)(tmp);
+ C3(tmp+0 ,m, 0, 1, 2, 3); C3(tmp+3, m, 1, 1, 2, 3); C3(tmp+6, m, 3, 1, 2, 3);
+ det_123[2] = REAL33_FUNC__(det)(tmp);
+ C3(tmp+0 ,m, 0, 1, 2, 3); C3(tmp+3, m, 1, 1, 2, 3); C3(tmp+6, m, 2, 1, 2, 3);
+ det_123[3] = REAL33_FUNC__(det)(tmp);
+
+ C3(tmp+0 ,m, 1, 0, 1, 3); C3(tmp+3, m, 2, 0, 1, 3); C3(tmp+6, m, 3, 0, 1, 3);
+ det_013[0] = REAL33_FUNC__(det)(tmp);
+ C3(tmp+0 ,m, 0, 0, 1, 3); C3(tmp+3, m, 2, 0, 1, 3); C3(tmp+6, m, 3, 0, 1, 3);
+ det_013[1] = REAL33_FUNC__(det)(tmp);
+ C3(tmp+0 ,m, 0, 0, 1, 3); C3(tmp+3, m, 1, 0, 1, 3); C3(tmp+6, m, 3, 0, 1, 3);
+ det_013[2] = REAL33_FUNC__(det)(tmp);
+ C3(tmp+0 ,m, 0, 0, 1, 3); C3(tmp+3, m, 1, 0, 1, 3); C3(tmp+6, m, 2, 0, 1, 3);
+ det_013[3] = REAL33_FUNC__(det)(tmp);
+ #undef C3
+
+ cofacts[0] = -det_012[0];
+ cofacts[1] = det_012[1];
+ cofacts[2] = -det_012[2];
+ cofacts[3] = det_012[3];
+
+ /* Compute the determinant of src */
+ tmp[0] = m[3], tmp[1] = m[7], tmp[2] = m[11], tmp[3] = m[15];
+ det = REALX_FUNC__(dot)(cofacts, tmp);
+
+ /* Invert the matrix */
+ if(det == 0.f)
+ return det;
+
+ rcp_det = 1.f / det;
+
+ mpmp_rcp_det[0] = mpmp_rcp_det[2] = -rcp_det;
+ mpmp_rcp_det[1] = mpmp_rcp_det[3] = rcp_det;
+ pmpm_rcp_det[0] = pmpm_rcp_det[2] = rcp_det;
+ pmpm_rcp_det[1] = pmpm_rcp_det[3] = -rcp_det;
+ REALX_FUNC__(mul)(REALXY_FUNC__(col_ptr)(dst, 0), det_123, pmpm_rcp_det);
+ REALX_FUNC__(mul)(REALXY_FUNC__(col_ptr)(dst, 1), det_023, mpmp_rcp_det);
+ REALX_FUNC__(mul)(REALXY_FUNC__(col_ptr)(dst, 2), det_013, pmpm_rcp_det);
+ REALX_FUNC__(mul)(REALXY_FUNC__(col_ptr)(dst, 3), det_012, mpmp_rcp_det);
+ return det;
+}
+static FINLINE REAL_TYPE__
+REALXY_FUNC__(invtrans)(REAL_TYPE__* dst, const REAL_TYPE__* mat)
+{
+ const REAL_TYPE__ det = REALXY_FUNC__(inverse)(dst, mat);
+ REALXY_FUNC__(transpose)(dst, dst);
+ return det;
+}
+
+#undef REAL33_FUNC__
+
+#include "realXY_end.h"
+
diff --git a/src/realX.h b/src/realX.h
@@ -19,10 +19,6 @@
#include "math.h"
#include <math.h>
-#ifndef REALX_BEGIN_H
- #error The realX_begin.h header must be included
-#endif
-
#ifdef COMPILER_GCC
#pragma GCC push_options
#pragma GCC optimize("unroll-loops")
@@ -349,4 +345,3 @@ REALX_FUNC__(min)
#ifdef COMPILER_GCC
#pragma GCC pop_options
#endif
-
diff --git a/src/realXY.h b/src/realXY.h
@@ -17,20 +17,6 @@
* Internal header used to generate funcs on column major REAL_TYPE__ matrix
* of X x Y dimensions
*/
-#if !defined(REALX_DIMENSION__) \
- || !defined(REALY_DIMENSION__) \
- || !defined(REAL_TYPE__)
- #error Missing arguments
-#endif
-
-#if defined(REALXY_FUNC__) \
- || defined(REALX_FUNC__) \
- || defined(REALY_FUNC__) \
- || defined(REAL_EPSILON__) \
- || defined(REAL_LETTER__) \
- || defined(SIZEOF_REALXY__)
- #error Unexpected macro defintion
-#endif
#include "math.h"
#include "rsys.h"
@@ -40,31 +26,6 @@
#pragma GCC optimize("unroll-loops")
#endif
-STATIC_ASSERT
- (REALX_DIMENSION__ > 1 && REALY_DIMENSION__ > 1, Unexpected_value);
-
-/* Define the epsilon for each real type */
-#define REAL_EPSILON_double 1.e-8
-#define REAL_EPSILON_float 1.e-6f
-#define REAL_EPSILON__ CONCAT(REAL_EPSILON_, REAL_TYPE__)
-
-/* Define the suffix/prefix letter for each real type */
-#define REAL_LETTER_float f
-#define REAL_LETTER_double d
-#define REAL_LETTER__ CONCAT(REAL_LETTER_, REAL_TYPE__)
-
-#define REALXY_FUNC__(Func) \
- CONCAT(CONCAT(CONCAT(CONCAT \
- (REAL_LETTER__, REALX_DIMENSION__), REALY_DIMENSION__), _), Func)
-
-#define REALX_FUNC__(Func) \
- CONCAT(CONCAT(CONCAT(f, REALX_DIMENSION__), _), Func)
-#define REALY_FUNC__(Func) \
- CONCAT(CONCAT(CONCAT(f, REALY_DIMENSION__), _), Func)
-
-/* Helper macro */
-#define SIZEOF_REALXY__ sizeof(REAL_TYPE__[REALX_DIMENSION__*REALY_DIMENSION__])
-
static FINLINE REAL_TYPE__*
REALXY_FUNC__(splat)(REAL_TYPE__* dst, const REAL_TYPE__ val)
{
@@ -331,19 +292,6 @@ REALXY_FUNC__(CONCAT(CONCAT(mulf, REALX_DIMENSION__), REALY_DIMENSION__))
}
#endif /* REALX_DIMENSION__ == REALY_DIMENSION__ */
-#undef REAL_EPSILON__
-#undef REAL_EPSILON_float
-#undef REAL_EPSILON_double
-#undef REAL_LETTER__
-#undef REAL_LETTER_float
-#undef REAL_LETTER_double
-#undef REALX_FUNC__
-#undef REALY_FUNC__
-#undef REALXY_FUNC__
-#undef REALX_DIMENSION__
-#undef REALY_DIMENSION__
-#undef SIZEOF_REALXY__
-
#ifdef COMPILER_GCC
#pragma GCC pop_options
#endif
diff --git a/src/realXY_begin.h b/src/realXY_begin.h
@@ -0,0 +1,51 @@
+/* Copyright (C) 2013-2016 Vincent Forest (vaplv@free.fr)
+ *
+ * The RSys library is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published
+ * by the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * The RSys library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with the RSys library. If not, see <http://www.gnu.org/licenses/>. */
+
+/* This file can be included once */
+#if defined(REALXY_BEGIN_H)
+ #error The realXY_begin.h header is already included
+#endif
+#define REALXY_BEGIN_H
+
+
+/* Check that the expected arguments are defined */
+#if !defined(REALX_DIMENSION__) \
+ || !defined(REALY_DIMENSION__) \
+ || !defined(REAL_TYPE__)
+ #error Missing arguments
+#endif
+
+#if defined(REALXY_FUNC__) \
+ || defined(REALY_FUNC__) \
+ || defined(SIZEOF_REALXY__)
+ #error Unexpected macro definition
+#endif
+
+/* Define the function name generators */
+#define REALXY_FUNC__(Func) \
+ CONCAT(CONCAT(CONCAT(CONCAT \
+ (REAL_LETTER__, REALX_DIMENSION__), REALY_DIMENSION__), _), Func)
+#define REALY_FUNC__(Func) \
+ CONCAT(CONCAT(CONCAT(f, REALY_DIMENSION__), _), Func)
+
+#define SIZEOF_REALXY__ sizeof(REAL_TYPE__[REALX_DIMENSION__*REALY_DIMENSION__])
+
+/* Check the validity of the dimensions */
+#include "rsys.h"
+STATIC_ASSERT
+ (REALX_DIMENSION__ > 1 && REALY_DIMENSION__ > 1, Unexpected_value);
+
+#include "realX_begin.h"
+
diff --git a/src/realXY_end.h b/src/realXY_end.h
@@ -0,0 +1,29 @@
+/* Copyright (C) 2013-2016 Vincent Forest (vaplv@free.fr)
+ *
+ * The RSys library is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published
+ * by the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * The RSys library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with the RSys library. If not, see <http://www.gnu.org/licenses/>. */
+
+#ifndef REALXY_BEGIN_H
+ #error The realXY_begin.h header must be included
+#endif
+
+#undef REALY_DIMENSION__
+
+#undef REALY_FUNC__
+#undef REALXY_FUNC__
+#undef SIZEOF_REALXY__
+
+#undef REALXY_BEGIN_H
+
+#include "realX_end.h"
+
diff --git a/src/realX_begin.h b/src/realX_begin.h
@@ -13,15 +13,13 @@
* You should have received a copy of the GNU Lesser General Public License
* along with the RSys library. If not, see <http://www.gnu.org/licenses/>. */
-/* Check that the expected arguments are defined */
-
/* This file can be included once */
#if defined(REALX_BEGIN_H)
- #error The realX_begin.h header is already defined
+ #error The realX_begin.h header is already included
#endif
#define REALX_BEGIN_H
-/* Check the the file arguments are defined */
+/* Check that the expected arguments are defined */
#if !defined(REALX_DIMENSION__) || !defined(REAL_TYPE__)
#error Missing arguments
#endif
diff --git a/src/realX_end.h b/src/realX_end.h
@@ -18,13 +18,14 @@
#endif
#undef REAL_TYPE__
+#undef REALX_DIMENSION__
+
#undef REAL_LETTER__
#undef REAL_LETTER_float
#undef REAL_LETTER_double
#undef REAL_EPSILON__
#undef REAL_EPSILON_float
#undef REAL_EPSILON_double
-#undef REALX_DIMENSION__
#undef REALX_FUNC__
#undef SIZEOF_REALX__
