commit e0bc8ec773b3216517fd54ddbb9b364766cf44b0
parent 6abe368d089426535925565cba4f08b86703267f
Author: vaplv <vaplv@free.fr>
Date: Sat, 14 Feb 2015 14:41:20 +0100
Add functions on quaternions
Diffstat:
3 files changed, 174 insertions(+), 0 deletions(-)
diff --git a/cmake/CMakeLists.txt b/cmake/CMakeLists.txt
@@ -52,6 +52,7 @@ set(RSYS_FILES_SRC
mem_allocator.c
pthread/pthread_condition.c
pthread/pthread_mutex.c
+ quaternion.c
str.c)
set(RSYS_FILES_INC
binary_heap.h
@@ -82,6 +83,7 @@ set(RSYS_FILES_INC
math.h
mem_allocator.h
mutex.h
+ quaternion.h
ref_count.h
rsys.h
signal.h
@@ -100,6 +102,7 @@ set_target_properties(rsys PROPERTIES
rcmake_setup_devel(rsys RSys ${VERSION} rsys/rsys_version.h)
+target_link_libraries(rsys m)
if(NOT MINGW)
target_link_libraries(rsys dl)
endif(NOT MINGW)
diff --git a/src/quaternion.c b/src/quaternion.c
@@ -0,0 +1,77 @@
+/* Copyright (C) 2013-2015 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 "quaternion.h"
+
+float*
+quat_slerp(float dst[4], const float from[4], const float to[4], const float t)
+{
+ float cos_omega;
+ float scale0, scale1;
+ float tmp0[4], tmp1[4];
+ ASSERT(dst && from && to && t >= 0.f && t <= 1.f);
+
+ if(eq_eps(t, 0.f, 1.e-6f))
+ return f4_set(dst, from);
+ if(eq_eps(t, 1.f, 1.e-6f))
+ return f4_set(dst, to);
+
+ cos_omega = f4_dot(from, to);
+ if(cos_omega < 0.f) {
+ f4_minus(tmp0, to);
+ cos_omega = -cos_omega;
+ } else {
+ f4_set(tmp0, to);
+ }
+ if((1.f - cos_omega) > 1.e-6f) {
+ const double omega = acos((double)cos_omega);
+ const double sin_omega = sin(omega);
+ scale0 = (float)(sin(omega * t) / sin_omega);
+ scale1 = (float)(sin((1.0 - t) * omega) / sin_omega);
+ } else {
+ scale0 = t;
+ scale1 = 1 - t;
+
+ }
+ f4_mulf(tmp0, tmp0, scale0);
+ f4_mulf(tmp1, from, scale1);
+ return f4_add(dst, tmp0, tmp1);
+}
+
+float*
+quat_to_f33(float mat33[9], const float quat[4])
+{
+ float i2j2k2[3];
+ float ijka[4];
+ ASSERT(mat33 && quat);
+
+ f3_mul(i2j2k2, quat, quat);
+ f4_set(ijka, quat);
+
+ mat33[0] = 1.f - 2.f * (i2j2k2[1] + i2j2k2[2]);
+ mat33[1] = 2.f * (ijka[0]*ijka[1] + ijka[2]*ijka[3]);
+ mat33[2] = 2.f * (ijka[0]*ijka[2] - ijka[1]*ijka[3]);
+
+ mat33[3] = 2.f * (ijka[0]*ijka[1] - ijka[2]*ijka[3]);
+ mat33[4] = 1.f - 2.f * (i2j2k2[0] + i2j2k2[2]);
+ mat33[5] = 2.f * (ijka[1]*ijka[2] + ijka[0]*ijka[3]);
+
+ mat33[6] = 2.f * (ijka[0]*ijka[2] + ijka[1]*ijka[3]);
+ mat33[7] = 2.f * (ijka[1]*ijka[2] - ijka[0]*ijka[3]);
+ mat33[8] = 1.f - 2.f * (i2j2k2[0] + i2j2k2[1]);
+
+ return mat33;
+}
+
diff --git a/src/quaternion.h b/src/quaternion.h
@@ -0,0 +1,94 @@
+/* Copyright (C) 2013-2015 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 QUATERNION_H
+#define QUATERNION_H
+
+#include "float3.h"
+#include "float4.h"
+
+/*
+ * Quaternion encoded in a float4 as { i, j, k, a }
+ */
+
+static FINLINE float*
+quat_identity(float quat[4])
+{
+ return f4(quat, 0.f, 0.f, 0.f, 1.f);
+}
+
+static FINLINE float*
+quat_set_axis_angle(float quat[4], const float axis[3], const float angle)
+{
+ const float hangle = angle * 0.5f;
+ const float s = (float)sin((double)hangle);
+ const float c = (float)cos((double)hangle);
+ ASSERT(quat && axis);
+
+ f3_set(quat, axis);
+ f3_mulf(quat, quat, s);
+ quat[3] = c;
+ return quat;
+}
+
+static FINLINE float*
+quat_conj(float dst[4], const float quat[4]) /* { -ix, -jy, -kz, a } */
+{
+ ASSERT(dst && quat);
+ return f4(dst, -quat[0], -quat[1], -quat[2], quat[3]);
+}
+
+static FINLINE float*
+quat_mul(float dst[4], const float q0[4], const float q1[4])
+{
+ float res[4];
+ ASSERT(dst && q0 && q1);
+ res[0] = q0[3]*q1[0] + q0[0]*q1[3] + q0[1]*q1[2] - q0[2]*q1[1];
+ res[1] = q0[3]*q1[1] - q0[0]*q1[2] + q0[1]*q1[3] + q0[2]*q1[0];
+ res[2] = q0[3]*q1[2] + q0[0]*q1[1] - q0[1]*q1[0] + q0[2]*q1[3];
+ res[3] = q0[3]*q1[3] - q0[0]*q1[0] - q0[1]*q1[1] - q0[2]*q1[2];
+ return f4_set__(dst, res);
+}
+
+static FINLINE float
+quat_calca(const float ijk[3])
+{
+ float ijk_sqr;
+ ASSERT(ijk);
+ ijk_sqr = f3_dot(ijk, ijk);
+ return (float)sqrt((double)absf(1.f - ijk_sqr));
+}
+
+static FINLINE float* /* Normalized Linear interpolation */
+quat_nlerp(float dst[4], const float from[4], const float to[4], const float t)
+{
+ f4_normalize(dst, f4_lerp(dst, from, to, t));
+ return dst;
+}
+
+RSYS_API float*
+quat_slerp /* Spherical linear interplation */
+ (float dst[4],
+ const float from[4],
+ const float to[4],
+ const float t);
+
+RSYS_API float*
+quat_to_f33
+ (float mat33[9] /* column major matrix */,
+ const float quat[4]);
+
+#endif /* QUATERNION_H */
+
