summaryrefslogtreecommitdiff
path: root/src/math/quat.c
diff options
context:
space:
mode:
Diffstat (limited to 'src/math/quat.c')
-rw-r--r--src/math/quat.c314
1 files changed, 314 insertions, 0 deletions
diff --git a/src/math/quat.c b/src/math/quat.c
new file mode 100644
index 0000000..e68e254
--- /dev/null
+++ b/src/math/quat.c
@@ -0,0 +1,314 @@
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "math.h"
+#include "../util/assert.h"
+#include "../core/mem.h"
+
+static Vec3 sharedVec3;
+static Quat sharedQuat;
+static Quat sharedQuat2;
+static Euler sharedEuler;
+
+#define shrquat(q) \
+do{\
+sharedQuat = *q;\
+q = &sharedQuat;\
+}while(0)
+
+#define shrquat2(q) \
+do{\
+sharedQuat2 = *q;\
+q = &sharedQuat2;\
+}while(0)
+
+#define shrvec3(v) \
+do{\
+sharedVec3 = *v;\
+v = &sharedVec3;\
+}while(0)
+
+#define shreuler(v) \
+do{\
+sharedEuler = *v;\
+v = &sharedEuler;\
+}while(0)
+
+void quat_tostring(Quat* v, char buf[]) {
+ ssr_assert(v);
+ sprintf(buf, "%8.3f %8.3f %8.3f", v->x, v->y, v->z);
+}
+
+void quat_print(Quat* q) {
+ ssr_assert(q);
+ quat_tostring(q, printbuffer);
+ printf("\n%s\n", printbuffer);
+}
+
+void euler_tostring(Euler* v, char buf[]) {
+ sprintf(buf, "%8.3f %8.3f %8.3f", v->x, v->y, v->z);
+}
+
+void euler_print(Euler* v) {
+ euler_tostring(v, printbuffer);
+ printf("\n%s\n", printbuffer);
+}
+
+void euler_deg2rad(Euler* in, Euler* out) {
+ ssr_assert(in && out);
+ out->x = radians(in->x);
+ out->y = radians(in->y);
+ out->z = radians(in->z);
+}
+
+void euler_rad2deg(Euler* in, Euler* out) {
+ ssr_assert(in && out);
+ out->x = degree(in->x);
+ out->y = degree(in->y);
+ out->z = degree(in->z);
+}
+
+void euler_toquat(Euler* euler, Quat* out) {
+ ssr_assert(euler && out);
+ quat_fromeuler(euler, out);
+}
+
+bool quat_isidentity(Quat* q) {
+ return compare(quat_magnitude(q), 1.f);
+}
+
+void quat_fromaxisangle(Vec3* axis, float angle, Quat* out) {
+ ssr_assert(compare(vec3_magnitude2(axis), 1.f));
+ angle = radians(angle);
+ float half = angle * 0.5;
+ float s = sin(half);
+ out->w = cos(half);
+ out->x = s * axis->x;
+ out->y = s * axis->y;
+ out->z = s * axis->z;
+}
+
+void quat_fromeuler(Euler* el, Quat* out) {
+ ssr_assert(el && out);
+ Euler euler;
+ euler_deg2rad(el, &euler);
+ float cx = cos(euler.x * 0.5f);
+ float sx = sin(euler.x * 0.5f);
+ float cy = cos(euler.y * 0.5f);
+ float sy = sin(euler.y * 0.5f);
+ float cz = cos(euler.z * 0.5f);
+ float sz = sin(euler.z * 0.5f);
+ Quat qx = {sx, 0, 0,cx};
+ Quat qy = {0, sy, 0,cy};
+ Quat qz = {0, 0,sz,cz};
+ /*按ZXY顺序*/
+ quat_multiply(&qx, &qz, &qx);
+ quat_multiply(&qy, &qx, out);
+ ssr_assert(quat_isidentity(out));
+}
+
+/*
+** 四元数直接对向量进行旋转和先转成矩阵形式再旋转计算量一样
+*/
+void quat_applytovec3(Quat* q, Vec3* v, Vec3* out) {
+ ssr_assert(q && v && out);
+ if (v == out) {
+ shrvec3(v);
+ }
+ /* q[v,0]q^-1 */
+ /* https://gamedev.stackexchange.com/questions/28395/rotating-vector3-by-a-quaternion */
+ float x = q->x * 2.0F;
+ float y = q->y * 2.0F;
+ float z = q->z * 2.0F;
+ float xx = q->x * x;
+ float yy = q->y * y;
+ float zz = q->z * z;
+ float xy = q->x * y;
+ float xz = q->x * z;
+ float yz = q->y * z;
+ float wx = q->w * x;
+ float wy = q->w * y;
+ float wz = q->w * z;
+
+ /*从这里能看到矩阵形式*/
+ out->x = (1.0f - (yy + zz)) * v->x + (xy - wz) * v->y + (xz + wy) * v->z;
+ out->y = (xy + wz) * v->x + (1.0f - (xx + zz)) * v->y + (yz - wx) * v->z;
+ out->z = (xz - wy) * v->x + (yz + wx) * v->y + (1.0f - (xx + yy)) * v->z;
+}
+
+void quat_tomat4(Quat* q, Mat4* out) {
+ ssr_assert(q && out);
+ ssr_assert(quat_isidentity(q));
+ mat4_setidentity(out);
+
+ float x = q->x * 2.0F; /*从quat_applytovec3能得到矩阵形式*/
+ float y = q->y * 2.0F;
+ float z = q->z * 2.0F;
+ float xx = q->x * x;
+ float yy = q->y * y;
+ float zz = q->z * z;
+ float xy = q->x * y;
+ float xz = q->x * z;
+ float yz = q->y * z;
+ float wx = q->w * x;
+ float wy = q->w * y;
+ float wz = q->w * z;
+
+ /*和mat4_setaxisangle实际上结果一样*/
+ out->l[0] = 1.0f - (yy + zz);
+ out->l[1] = xy + wz;
+ out->l[2] = xz - wy;
+
+ out->l[4] = xy - wz;
+ out->l[5] = 1.0f - (xx + zz);
+ out->l[6] = yz + wx;
+
+ out->l[8] = xz + wy;
+ out->l[9] = yz - wx;
+ out->l[10] = 1.0f - (xx + yy);
+}
+
+void quat_toeuler(Quat* q, Euler* out) {
+ ssr_assert(q && out);
+ Mat4 mat;
+ quat_tomat4(q, &mat); /*计算变换矩阵*/
+ mat4_toeuler(&mat, out); /*mat是按照RyRxRz顺序(z->y->x)的旋转矩阵*/
+}
+
+void quat_normalize(Quat* q, Quat* out) {
+ ssr_assert(q && out);
+ float mag = quat_magnitude(q);
+ quat_scale(q, 1.f/mag, out);
+}
+
+void quat_scale(Quat* q, float scale, Quat* out) {
+ ssr_assert(q && out);
+ out->x = q->x * scale;
+ out->y = q->y * scale;
+ out->z = q->z * scale;
+ out->w = q->w * scale;
+}
+
+void quat_negtive(Quat* in, Quat* out) {
+ ssr_assert(in && out);
+ out->w = -in->w;
+ out->x = -in->x;
+ out->y = -in->y;
+ out->z = -in->z;
+}
+
+void quat_devide(Quat* q, float k, Quat* out) {
+ ssr_assert(q && out);
+ k = 1 / k;
+ out->w = q->w * k;
+ out->x = q->x * k;
+ out->y = q->y * k;
+ out->z = q->z * k;
+}
+
+void quat_invert(Quat* q, Quat* out) {
+ ssr_assert(q && out);
+ float mag2 = quat_magnitude2(q);
+ quat_conjugate(q, out);
+ quat_devide(out, mag2, out);
+ /*实际上如果是单位四元数，共轭就是逆*/
+}
+
+bool quat_setlookrotation(Vec3* view, Vec3* up, Quat* out) {
+ ssr_assert(view && up && out);
+ Mat4 m;
+ mat4_setlookrotation(view, up, &m); /*先以view为准构建正交矩阵*/
+ mat4_toquat(&m, out);
+ return 1;
+}
+
+/*q1 * q2^-1*/
+void quat_minus(Quat* q1, Quat* q2, Quat* out) {
+ ssr_assert(q1 && q2 && out);
+ Quat q2i; quat_invert(q2, &q2i);
+ quat_multiply(q1, &q2i, out);
+}
+
+void quat_multiply(Quat* q1, Quat* q2, Quat* out) {
+ ssr_assert(q1 && q2 && out);
+
+ float w1 = q1->w, x1 = q1->x, y1 = q1->y, z1 = q1->z,
+ w2 = q2->w, x2 = q2->x, y2 = q2->y, z2 = q2->z;
+ out->x = x1 * w2 + x2 * w1 + y1 * z2 - z1 * y2;
+ out->y = w1 * y2 + w2 * y1 + z1 * x2 - z2 * x1;
+ out->z = w1 * z2 + w2 * z1 + x1 * y2 - x2 * y1;
+ out->w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2;
+}
+
+/*
+** 共轭的几何意义是颠倒旋转轴方向，代表了和q相反的角位移
+*/
+void quat_conjugate(Quat* in, Quat* out) {
+ ssr_assert(in && out);
+ out->w = in->w;
+ out->x = -in->x;
+ out->y = -in->y;
+ out->z = -in->z;
+}
+
+float quat_dot(Quat* q1, Quat* q2) {
+ ssr_assert(q1 && q2);
+ return (q1->x*q2->x + q1->y*q2->y + q1->z*q2->z + q1->w*q2->w);
+}
+
+float quat_magnitude(Quat* q) {
+ ssr_assert(q);
+ return sqrt(quat_dot(q, q));
+}
+
+float quat_magnitude2(Quat* q) {
+ ssr_assert(q);
+ return quat_dot(q, q);
+}
+
+void quat_slerp(Quat* q1, Quat* q2, float t, Quat* out) {
+ ssr_assert(q1 && q2 && out);
+ ssr_assert(quat_isidentity(q1) && quat_isidentity(q2)); /*适用于单位四元数*/
+ float dot = quat_dot(q1, q2);
+ Quat temp;
+ if (dot < 0.0f) {
+ dot = -dot;
+ temp.x = -q2->x;
+ temp.y = -q2->y;
+ temp.z = -q2->z;
+ temp.w = -q2->w;
+ } else {
+ temp = *q2;
+ }
+ if (dot < 0.95f) {
+ float angle = acos(dot);
+ float sinadiv, sinat, sinaomt;
+ sinadiv = 1.0f / sin(angle);
+ sinat = sin(angle*t);
+ sinaomt = sin(angle*(1.0f - t));
+ out->x = (q1->x*sinaomt + temp.x*sinat)*sinadiv;
+ out->y = (q1->y*sinaomt + temp.y*sinat)*sinadiv;
+ out->z = (q1->z*sinaomt + temp.z*sinat)*sinadiv;
+ out->w = (q1->w*sinaomt + temp.w*sinat)*sinadiv;
+ } else { /*小范围时使用lerp*/
+ quat_lerp(q1, &temp, t, out);
+ }
+}
+
+void quat_lerp(Quat* q1, Quat* q2, float t, Quat* out) {
+ ssr_assert(q1 && q2 && out);
+ float t2 = 1 - t;
+ if (quat_dot(q1, q2) < 0.f) { /*点乘不能是负的，翻转一个四元数的符号，使得落回360°*/
+ out->x = q1->x * t2 - t * q2->x;
+ out->y = q1->y * t2 - t * q2->y;
+ out->z = q1->z * t2 - t * q2->z;
+ out->w = q1->w * t2 - t * q2->w;
+ } else {
+ out->x = q1->x * t2 + t * q2->x;
+ out->y = q1->y * t2 + t * q2->y;
+ out->z = q1->z * t2 + t * q2->z;
+ out->w = q1->w * t2 + t * q2->w;
+ }
+ quat_normalize(out, out);
+}
generated by cgit v1.1-26-g67d0 at 2025-05-18 17:53:25 +0000