path: root/src/math/quat.c

#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);
}

Quat quat_make(float rx, float ry, float rz) {
	Quat rot; 
	Euler euler = {rx, ry, rz};
	quat_fromeuler(&euler, &rot);
	return rot;
}

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);
}

/*q1*q2*/
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);
}