+init

7 files changed, 1562 insertions, 0 deletions

diff --git a/src/math/math.c b/src/math/math.c
new file mode 100644
index 0000000..4cca29e
--- /dev/null
+++ b/src/math/math.c
@@ -0,0 +1,21 @@
+#include "math.h"
+
+char printbuffer[2048] = { 0 };
+
+float rsqrt(float number) {
+	long i;
+	float x2, y;
+	const float threehalfs = 1.5F;
+
+	x2 = number * 0.5F;
+	y = number;
+	i = *(long *)&y;
+	i = 0x5f3759df - (i >> 1);
+	y = *(float *)&i;
+	y = y * (threehalfs - (x2 * y * y));
+	return y;
+}
+
+float lerp(float a, float b, float t) {
+	return a * (1 - t) + b * t;
+}
\ No newline at end of file
diff --git a/src/math/math.h b/src/math/math.h
new file mode 100644
index 0000000..4498920
--- /dev/null
+++ b/src/math/math.h
@@ -0,0 +1,306 @@
+#ifndef _SOFTSHADEROOM_MATH_H_
+#define _SOFTSHADEROOM_MATH_H_
+
+#include <stdio.h>
+#include <string.h>
+#include <math.h>
+#include <limits.h>
+
+#include "../util/type.h"
+
+#define PI 3.141592653f
+#define RAD2DEG 57.295779523f /*180.f/PI*/
+#define DEG2RAG 0.0174532925f /*PI/180.f*/
+#define EPSILON 0.000001f
+
+/* 用来打印的公共buffer */
+extern char printbuffer[2048];
+
+/*
+** 数学函数
+*/
+#define min(a, b) ((a) < (b) ? (a) : (b))
+#define max(a, b) ((a) > (b) ? (a) : (b))
+#define clamp(v, l, h) ((v) > (l) ? ((v) < (h) ? (v) : (h)) : (l))
+#define absf(v) ((v )> 0 ? (v ): -(v))
+#define radians(angle) (angle * DEG2RAG)
+#define degree(rad) (rad * RAD2DEG)
+#define compare(v1, v2) (absf((v1) - (v2)) < EPSILON)
+#define swapi(a, b) {int temp = a; a = b; b = temp;}
+float rsqrt(float n);
+float lerp(float from, float to, float t);
+
+/*
+** 二维向量，用来做屏幕上的一些计算
+*/
+typedef struct Vec2 {
+	float x, y;
+} Vec2;
+
+/*
+** 三维向量，用来做三维空间的计算
+*/
+typedef union Vec3 {
+	struct {
+		float x, y, z;
+	};
+	struct {
+		float A, B, C; /*重心坐标*/
+	};
+	Vec2 xy;
+} Vec3;
+
+/*
+** 齐次坐标，列主项，平移变换和透视投影需要
+*/
+typedef union Vec4 {
+	struct {
+		float x, y, z, w;
+	};
+	struct {
+		float r, g, b, a;
+	};
+	Vec3 xyz;
+} Vec4;
+
+/*
+** 用来可视化四元数，欧拉角默认使用角度存储，用euler_deg2rad()转弧度
+*/
+typedef union Euler {
+	struct {
+		float x, y, z;
+	};
+	struct {
+		float pitch, yaw, roll;
+	};
+} Euler;
+
+/*
+** 四元数，用来做旋转变换。在进行变换复合以及插值的时候用，但最终还是需要通过quat_mat4转换成矩阵和其他变换矩阵
+** 一起对向量进行变换
+*/
+typedef struct Quat {
+	float x, y, z, w;
+} Quat;
+
+/*
+** 4x4矩阵，列主项，用来做平移和缩放变换。之所以用列主序存储，是为了快速读取矩阵的基向量
+*/
+typedef union Mat4 {
+	float l[16];
+	float m[4][4];
+	struct {
+		float
+			e00, e10, e20, e30, /*colum 0*/
+			e01, e11, e21, e31,
+			e02, e12, e22, e32,
+			e03, e13, e23, e33;
+	};
+	struct {
+		Vec4 x;/*colum 0*/
+		Vec4 y;
+		Vec4 z;
+		Vec4 w;
+	} axis; /*轴*/
+	struct {
+		Vec4 x;/*colum 0*/
+		Vec4 y;
+		Vec4 z;
+		Vec4 w;
+	} basis; /*基向量*/
+	struct {
+		Vec4 axisx;
+		Vec4 axisy;
+		Vec4 axisz;
+		Vec4 pos;
+	};
+	Vec4 colums[4];
+} Mat4;
+
+typedef union Mat3 {
+	struct {
+		float
+			e00, e10, e20, /*colum 0*/
+			e01, e11, e21,
+			e02, e12, e22;
+	};
+} Mat3;
+
+typedef union Mat23 {
+	struct {
+		float
+			e00, e10, /*colum 0*/
+			e01, e11, 
+			e02, e12;
+	};
+} Mat23;
+
+typedef union Mat43 {
+	struct {
+		float
+			e00, e10, e20, e30, /*colum 0*/
+			e01, e11, e21, e31,
+			e02, e12, e22, e32;
+	};
+	struct { /*三个齐次裁剪坐标*/
+		Vec4 p1;
+		Vec4 p2;
+		Vec4 p3;
+	};
+} Mat43;
+
+//#define MAT(m, r, c) (m->l[r + (c<<2)])
+#define MAT(M, r, c) (M->m[c][r])
+/************************************************************************/
+/* Vec                                                                  */
+/************************************************************************/
+
+void vec2_scale(Vec2* v, float k, Vec2* out);
+void vec2_plus(Vec2* v1, Vec2* v2, Vec2* out);
+void vec2_offset(Vec2* v, float offset, Vec2* out);
+void vec2_rotate(Vec2* v, float angle, Vec2* out);
+
+float vec2_dot(Vec2* v1, Vec2* v2);
+
+void vec2_tostring(Vec2* v, char buf[]);
+void vec2_print(Vec2* v);
+
+#define vec3_xy(v) (v->xy)
+
+extern Vec3 vec3forward; /*(0,0,1)*/
+extern Vec3 vec3up; /*(0,1,0)*/
+extern Vec3 vec3left;/*(1,0,0)*/
+
+void vec3_tostring(Vec3* v, char buf[]);
+void vec3_print(Vec3* v);
+
+float vec3_intersection(Vec3* v1, Vec3* v2); /*夹角*/
+void vec3_projection(Vec3* v1, Vec3* v2, Vec3* out);/*v1在v2上的投影*/
+void vec3_scale(Vec3* v, float k, Vec3* out);
+void vec3_plus(Vec3* v1, Vec3* v2, Vec3* out);
+void vec3_offset(Vec3* v, float offset, Vec3* out);
+void vec3_normalize(Vec3* v, Vec3* out);
+void vec3_vec4(float w, Vec4* out);
+
+void vec3_minus(Vec3* v1, Vec3* v2, Vec3* out);
+float vec3_dot(Vec3* v1, Vec3* v2);
+void  vec3_cross(Vec3* v1, Vec3* v2, Vec3* out);
+void vec3_multiply(Vec3* v1, Vec3* v2, Quat* out);// 向量的乘法，st=sxt-s*t，结果是一个四元数
+
+float vec3_magnitude(Vec3* v1);
+float vec3_magnitude2(Vec3* v1);
+
+void vec3_lerp(Vec3* v1, Vec3* v2, float t, Vec3* out);
+void vec3_slerp(Vec3* v1, Vec3* v2, float t, Vec3* out);
+
+void vec4_dividew(Vec4* v, Vec3* out);
+
+void vec4_tostring(Vec4* v, char buf[]);
+void vec4_print(Vec4* v);
+
+/************************************************************************/
+/* Matrix                                                               */
+/************************************************************************/
+
+extern Mat4 mat4identity;
+
+void mat4_tostring(Mat4* m, char str[]);
+void mat4_print(Mat4* m);
+
+void mat4_zero(Mat4* out);
+void mat4_setidentity(Mat4* out);
+void mat4_setfrustum(float l, float r, float b, float t, float n, float f, Mat4* out);
+void mat4_setperspective(float fov, float aspect, float near, float far, Mat4* out);
+void mat4_setscale(float kx, float ky, float kz, Mat4* out);
+void mat4_setposition(float x, float y, float z, Mat4* out);
+void mat4_setrotatez(float angle, Mat4* out);
+void mat4_setrotatex(float angle, Mat4* out);
+void mat4_setrotatey(float angle, Mat4* out);
+void mat4_setrotate(float angleX, float angleY, float angleZ, Mat4* out);/*RyRxRz*/
+void mat4_setaxisangle(Vec3* axis, float angle, Mat4* out);
+
+bool mat4_setlookrotation(Vec3* view, Vec3* up, Mat4* out);
+void mat4_setorthonormalbias(Vec3* x, Vec3* y, Vec3* z, Mat4* out); /*正交的三个轴*/
+
+void mat4_orthogonalize(Mat4* in, Mat4* out); /*解决矩阵蠕变，对左上角3x3矩阵进行正交化，结果是右手系的正交矩阵*/
+bool mat4_isorthogonal(Mat4* m); /*判断是不是正交矩阵*/
+bool mat4_isidentity(Mat4* m);
+
+void mat4_settr(Vec3* pos, Quat* rot, Mat4* out); /*用旋转和平移初始化mat4*/
+void mat4_settrs(Vec3* pos, Quat* rot, Vec3* scale, Mat4* out);
+void mat4_settrinverse(Vec3* pos, Quat* rot, Mat4* out);
+
+void mat4_multiply(Mat4* m1, Mat4* m2, Mat4* out); /* m1的行乘m2的列，意义是用m1变换m2 */
+
+void mat4_transpose(Mat4* m, Mat4* out);
+
+void mat4_scale(Mat4* m, Vec3* scale, Mat4* out);/* 后乘post-multiply scale */
+void mat4_translate(Mat4* m, Vec3* pos, Mat4* out); /* 后乘post-multiply translate */
+void mat4_rotate(Mat4*m, float angle, Vec3* rot, Mat4* out);/*后乘绕任意轴向量旋转矩阵*/
+
+bool mat4_invertfull(Mat4* in, Mat4* out); /* 并不是所有矩阵都能求逆 */
+bool mat4_invertgeneral3d(Mat4* in, Mat4* out); /* 对scale rotate translate求逆 */
+void mat4_invertscale(Mat4* scale, Mat4* out); /* 对缩放矩阵求逆 */
+void mat4_invertrot(Mat4* rot, Mat4* out); /* 对旋转矩阵求逆 */
+void mat4_invertpos(Mat4* pos, Mat4* out); /* 对平移矩阵求逆 */
+
+void mat4_decomposetrs(Mat4* src, Vec3* pos, Quat* quat, Vec3* scale); /*分解trs矩阵*/
+
+void mat4_applytovec4(Mat4* m, Vec4* v, Vec4* out);
+
+bool mat4_toeuler(Mat4* in, Euler* out); /* 计算YXZ旋转矩阵的欧拉角 */
+void mat4_toquat(Mat4* in, Quat* out); /*in是正交矩阵*/
+
+#define ROWMAT(A, ...)\
+Mat4 A={__VA_ARGS__};mat4_transpose(&A, &A);
+
+void mat3_applytovec3(Mat3* m, Vec3* v, Vec3* out);
+void mat23_applytovec3(Mat23* m, Vec3* v, Vec2* out);
+void mat43_applytovec3(Mat43* m, Vec3* v, Vec4* out);
+
+/************************************************************************/
+/* Quat                                                                 */
+/************************************************************************/
+
+void quat_tostring(Quat* q, char str[]);
+void quat_print(Quat* q);
+
+void euler_toquat(Euler* e, Quat* out);
+void euler_deg2rad(Euler* in, Euler* out);
+void euler_rad2deg(Euler* in, Euler* out);
+
+void euler_tostring(Euler* v, char buf[]);
+void euler_print(Euler* v);
+
+void quat_fromaxisangle(Vec3* axis, float angle, Quat* out); /*轴角转四元数*/
+void quat_fromeuler(Euler* euler, Quat* out); /*按照zxy顺序*/
+
+void quat_tomat4(Quat* q, Mat4* out);
+void quat_toeuler(Quat*q, Euler* out);
+
+void quat_normalize(Quat* q, Quat* out); /*解决蠕变，保持四元数合法*/
+
+void quat_scale(Quat* q, float scale, Quat* out);
+void quat_rotate();
+
+void quat_minus(Quat* q1, Quat* q2, Quat* out);
+void quat_slerp(Quat* start, Quat* end, float t, Quat* out);
+void quat_lerp(Quat* start, Quat* end, float t, Quat* out);
+void quat_translate(Quat* q, Vec4* v, Vec4* out);
+void quat_invert(Quat* q, Quat* out);
+float quat_dot(Quat* q1, Quat* q2);
+void quat_multiply(Quat* q1, Quat* q2, Quat* out);
+void quat_devide(Quat* q, float k, Quat* out);
+void quat_negtive(Quat* in, Quat* out);
+bool quat_isidentity(Quat* q);
+
+void quat_applytovec3(Quat* q, Vec3* v, Vec3* out); /*用四元数直接旋转向量*/
+
+void quat_conjugate(Quat* in, Quat* out);
+
+bool quat_setlookrotation(Vec3* view, Vec3* up, Quat* out);
+
+float quat_magnitude(Quat* q);
+float quat_magnitude2(Quat* q);
+
+#endif
\ No newline at end of file
diff --git a/src/math/matrix.c b/src/math/matrix.c
new file mode 100644
index 0000000..458f432
--- /dev/null
+++ b/src/math/matrix.c
@@ -0,0 +1,741 @@
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "math.h"
+#include "../util/assert.h"
+#include "../core/mem.h"
+
+
+static Mat4 sharedMat;
+static Mat4 sharedMat2;
+static Vec4 sharedVec4;
+
+Mat4 mat4identity = {
+	1,0,0,0,
+	0,1,0,0,
+	0,0,1,0,
+	0,0,0,1
+};
+
+#define shrmat(p) \
+do{\
+sharedMat = *p;\
+p = &sharedMat;\
+}while(0)
+
+#define shrmat2(p) \
+do{\
+sharedMat2 = *p;\
+p = &sharedMat2;\
+}while(0)
+
+void mat4_tostring(Mat4* m, char str[]) {
+	ssrM_zero(str, sizeof(str));
+	for (int r = 0; r < 4; ++r) {
+		for (int c = 0; c < 4; ++c) {
+			sprintf(str, "%8.3f ", MAT(m, r, c) == -0 ? +0 : MAT(m, r, c));
+			str += strlen(str);
+		}
+		if(r != 3) sprintf(str, "\n");
+		str += strlen(str);
+	}
+}
+
+void mat4_print(Mat4* m) {
+	mat4_tostring(m, printbuffer);
+	printf("\n%s\n", printbuffer);
+}
+
+void mat4_zero(Mat4* out) {
+	ssr_assert(out);
+	ssrM_zero(out, sizeof(Mat4));
+}
+
+void mat4_setidentity(Mat4* out) {
+	ssr_assert(out);
+	mat4_zero(out);
+	out->e00 = 1; 
+	out->e11 = 1; 
+	out->e22 = 1; 
+	out->e33 = 1;
+}
+
+void mat4_setfrustum(float l, float r, float b, float t, float n, float f, Mat4* out) {
+	ssr_assert(out);
+	mat4_zero(out);
+	out->e00 = (2.f * n) / (r - l);
+	out->e02 = (r + l) / (r - l);
+	out->e11 = 2.f * n / (t - b);
+	out->e12 = (t + b) / (t - b);
+	out->e22 = -(f + n) / (f - n);
+	out->e23 = -2.f * f * n / (f - n);
+	out->e32 = -1;
+}
+
+void mat4_setperspective(float _fov, float aspect, float near, float far, Mat4* out) {
+	float fov = _fov * PI / 180.f;
+	float tanf = tan(fov * 0.5);
+	mat4_setfrustum(
+	  -near*tanf*aspect,
+		near*tanf*aspect,
+		-near*tanf,
+		near*tanf, 
+		near, 
+		far,
+		out
+	);
+}
+
+static float _mul(float* r, float* c) {
+	return c[0] * r[0] + c[1] * r[4] + c[2] * r[8] + c[3] * r[12];
+}
+
+#define mul(r, c) _mul(&MAT(m1,r,0), &MAT(m2,0,c))
+
+void mat4_multiply(Mat4* m1, Mat4* m2, Mat4* out) {
+	ssr_assert(m1 && m2 && out);
+	if (mat4_isidentity(m1)) { if(m2 != out) *out = *m2; return; }
+	if (mat4_isidentity(m2)) { if(m1 != out) *out = *m1; return; }
+	if (m1 == out) shrmat(m1);
+	if (m2 == out) shrmat2(m2);
+
+	out->e00 = mul(0, 0); out->e01 = mul(0, 1); out->e02 = mul(0, 2); out->e03 = mul(0, 3);
+	out->e10 = mul(1, 0); out->e11 = mul(1, 1); out->e12 = mul(1, 2); out->e13 = mul(1, 3);
+	out->e20 = mul(2, 0); out->e21 = mul(2, 1); out->e22 = mul(2, 2); out->e23 = mul(2, 3);
+	out->e30 = mul(3, 0); out->e31 = mul(3, 1); out->e32 = mul(3, 2); out->e33 = mul(3, 3);
+}
+
+void mat4_setscale(float kx, float ky, float kz, Mat4* out) {
+	ssr_assert(out);
+	mat4_zero(out);
+	out->e00 = kx;
+	out->e11 = ky;
+	out->e22 = kz;
+	out->e33 = 1;
+}
+
+void mat4_setposition(float x, float y, float z, Mat4* out) {
+	ssr_assert(out);
+	mat4_setidentity(out);
+	out->e03 = x;
+	out->e13 = y;
+	out->e23 = z;
+}
+
+void mat4_setrotatez(float angle, Mat4* out) {
+	ssr_assert(out);
+	mat4_setidentity(out);
+	angle = radians(angle);
+	float s = sin(angle), c = cos(angle);
+	out->e00 = c; out->e01 = -s;
+	out->e10 = s; out->e11 = c;
+}
+
+void mat4_setrotatex(float angle, Mat4* out) {
+	ssr_assert(out);
+	mat4_setidentity(out);
+	angle = radians(angle);
+	float s = sin(angle), c = cos(angle);
+	out->e11 = c; out->e12 = -s; 
+	out->e21 = s; out->e22 = c;
+}
+
+void mat4_setrotatey(float angle, Mat4* out) {
+	ssr_assert(out);
+	mat4_setidentity(out);
+	angle = radians(angle);
+	float s = sin(angle), c = cos(angle);
+	out->e00 = c; out->e02 = s;
+	out->e20 = -s; out->e22 = c;
+}
+
+/*https://www.geometrictools.com/Documentation/EulerAngles.pdf*/
+void mat4_setrotate(float angleX, float angleY, float angleZ, Mat4* out) {
+	ssr_assert(out);
+	mat4_setidentity(out);
+	angleX = radians(angleX); angleY = radians(angleY); angleZ = radians(angleZ);
+	float sx = sin(angleX), cx = cos(angleX);
+	float sy = sin(angleY), cy = cos(angleY);
+	float sz = sin(angleZ), cz = cos(angleZ);
+	out->e00 = cy * cz + sx * sy * sz;  out->e01 = cz * sx*sy - cy * sz; out->e02 = cx * sy;
+	out->e10 = cx * sz;                 out->e11 = cx * cz;              out->e12 = -sx;
+	out->e20 = -cz * sy + cy * sx * sz; out->e21 = cy * cz*sx + sy * sz; out->e22 = cx * cy;
+}
+
+void mat4_setaxisangle(Vec3* ax, float angle, Mat4* out) {
+	ssr_assert(ax && out);
+
+	float a = radians(angle);
+	float c = cos(a);
+	float s = sin(a);
+
+	Vec3 axis = *ax;
+	Vec3 temp;
+	vec3_normalize(&axis, &axis);
+	vec3_scale(&axis, 1 - c, &temp);
+
+	/*
+	rotation matrix 推导过程 https://zhuanlan.zhihu.com/p/56587491
+	X^2(1-c)+c, XY(1-c)-Zs, XZ(1-c)+Ys, 0
+	XY(1-c)+Zs, Y^2(1-c)+c, YZ(1-c)-Xs, 0
+	XZ(1-c)-Ys, YZ(1-c)+Xs, Z^2(1-c)+c, 0
+	0,          0,          0, 1
+	*/
+
+	mat4_setidentity(out);
+	out->m[0][0] = c + temp.x * axis.x;
+	out->m[0][1] = 0 + temp.x * axis.y + s * axis.z;
+	out->m[0][2] = 0 + temp.x * axis.z - s * axis.y;
+
+	out->m[1][0] = 0 + temp.y * axis.x - s * axis.z;
+	out->m[1][1] = c + temp.y * axis.y;
+	out->m[1][2] = 0 + temp.y * axis.z + s * axis.x;
+
+	out->m[2][0] = 0 + temp.z * axis.x + s * axis.y;
+	out->m[2][1] = 0 + temp.z * axis.y - s * axis.x;
+	out->m[2][2] = c + temp.z * axis.z;
+
+}
+
+void mat4_setorthonormalbias(Vec3* x, Vec3* y, Vec3* z, Mat4* out) {
+	ssr_assert(x && y && z);
+	mat4_setidentity(out);
+	Vec4 asix = { x->x, x->y, x->z, 0 };
+	Vec4 asiy = { y->x, y->y, y->z, 0 };
+	Vec4 asiz = { z->x, z->y, z->z, 0 };
+	out->colums[0] = asix;
+	out->colums[1] = asiy;
+	out->colums[2] = asiz;
+}
+
+bool mat4_isidentity(Mat4* m) {
+	ssr_assert(m);
+	//return memcmp(m, &mat4identity, sizeof(Mat4)) == 0;
+	return
+		compare(m->axisx.x, 1) && compare(m->axisx.y, 0) && compare(m->axisx.z,0) && compare(m->axisx.w, 0) &&
+		compare(m->axisy.x, 0) && compare(m->axisy.y, 1) && compare(m->axisy.z,0) && compare(m->axisy.w, 0) &&
+		compare(m->axisz.x, 0) && compare(m->axisz.y, 0) && compare(m->axisz.z,1) && compare(m->axisz.w, 0) &&
+		compare(m->pos.x, 0  ) && compare(m->pos.y, 0  ) && compare(m->pos.z, 0  ) &&compare( m->pos.w, 1);
+}
+
+bool mat4_isorthogonal(Mat4* m) {
+	ssr_assert(m);
+	Mat4 trans = {0}, res = { 0 };
+	mat4_transpose(m, &trans);
+	mat4_multiply(m, &trans, &res);
+	return mat4_isidentity(&res);
+}
+
+/*
+** 以z轴为准进行正交化，分为施密特正交化和叉乘正交化，施密特过程更加普遍，叉乘适用于三维空间，两种方法实际上等价
+** 如果用叉乘的方法，只需要关注yz，x通过叉乘得到
+*/
+void mat4_orthogonalize(Mat4* in, Mat4* out) {
+	ssr_assert(in && out);
+	if (in == out) {
+		shrmat(in);
+	}
+	
+	mat4_setidentity(out);
+	Vec4 z = in->basis.z;
+	vec3_normalize(&z, &z);
+	Vec4 y = in->basis.y;
+	Vec4 x = {0};
+	vec3_cross(&y, &z, &x);
+	vec3_normalize(&x, &x);
+	vec3_cross(&z, &x, &y);
+	out->basis.x = x;
+	out->basis.y = y;
+	out->basis.z = z;
+	
+	/*
+	mat4_setidentity(out);
+
+	Vec4 x = in->basis.x;
+	Vec4 y = in->basis.y;
+	Vec4 z = in->basis.z;
+	Vec3 temp, temp2;
+
+	vec3_normalize(&z, &z);
+	out->basis.z = z;
+
+	float dot = vec3_dot(&y, &z);
+	vec3_scale(&z, dot, &temp);
+	vec3_minus(&y, &temp, &y);
+	vec3_normalize(&y, &y);
+	out->basis.y = y;
+
+	vec3_cross(&y, &z, &out->basis.x);
+	*/
+	/*针对右手系调整basis.x的方向*/ 
+	/*https://math.stackexchange.com/questions/1847465/why-to-use-gram-schmidt-process-to-orthonormalise-a-basis-instead-of-cross-produ*/
+	/*由于需要针对右手系，这里不这样计算，因为可能要对结果进行翻转
+	dot = vec3_dot(&x, &z);
+	vec3_scale(&z, dot, &temp);
+	vec3_minus(&x, &temp, &temp2);
+	dot = vec3_dot(&x, &y);
+	vec3_scale(&y, dot, &temp);
+	vec3_minus(&temp2, &temp, &x);
+	vec3_normalize(&x, &x);
+	out->basis.x = x;
+	*/
+}
+
+bool mat4_setlookrotation(Vec3* view, Vec3* up, Mat4* out) {
+	ssr_assert(view && up && out);
+
+	/*正交化*/
+	float mag = vec3_magnitude(view);
+	if (mag < EPSILON) return 0;
+	Vec3 z;
+	vec3_scale(view, 1.f / mag, &z);
+
+	Vec3 x;
+	vec3_cross(up, &z, &x);
+	mag = vec3_magnitude(&x);
+	if (mag < EPSILON) return 0;
+	vec3_scale(&x, 1.f / mag, &x);
+
+	Vec3 y;
+	vec3_cross(&z, &x, &y);
+	mag = vec3_magnitude(&y);
+	if (!compare(mag, 1)) return 0;
+
+	mat4_setorthonormalbias(&x, &y, &z, out); /*xyz正交*/
+
+	return 1;
+}
+
+void mat4_applytovec4(Mat4* mat, Vec4* v, Vec4* out) {
+	ssr_assert(mat && v && out);
+	if (v == out) {
+		sharedVec4 = *v;
+		v = &sharedVec4;
+	}
+	out->x = mat->e00 * v->x + mat->e01 * v->y + mat->e02 * v->z + mat->e03 * v->w;
+	out->y = mat->e10 * v->x + mat->e11 * v->y + mat->e12 * v->z + mat->e13 * v->w;
+	out->z = mat->e20 * v->x + mat->e21 * v->y + mat->e22 * v->z + mat->e23 * v->w;
+	out->w = mat->e30 * v->x + mat->e31 * v->y + mat->e32 * v->z + mat->e33 * v->w;
+}
+
+#define trans(r, c) out->e##r##c = m->e##c##r
+
+void mat4_transpose(Mat4* m, Mat4* out) {
+	ssr_assert(m && out);
+	if (m == out) shrmat(m);
+
+	trans(0, 0); trans(0, 1); trans(0, 2); trans(0, 3);
+	trans(1, 0); trans(1, 1); trans(1, 2); trans(1, 3);
+	trans(2, 0); trans(2, 1); trans(2, 2); trans(2, 3);
+	trans(3, 0); trans(3, 1); trans(3, 2); trans(3, 3);
+}
+
+/*
+** 使用高斯消元法计算任意矩阵的逆矩阵。针对不含投影的3D变换矩阵，应该使用
+** mat4_invertgeneral3d() 
+** 更快一些
+*/
+bool mat4_invertfull(Mat4* m, Mat4* out) {
+	ssr_assert(m && out);
+
+#define _m(r, c) MAT(m, r, c)
+	float wtmp[4][8] = {
+		{ /*M*/ _m(0,0), _m(0, 1), _m(0, 2), _m(0, 3), /*I*/ 1, 0, 0, 0 },
+		{ /*M*/ _m(1,0), _m(1, 1), _m(1, 2), _m(1, 3), /*I*/ 0, 1, 0, 0 },
+		{ /*M*/ _m(2,0), _m(2, 1), _m(2, 2), _m(2, 3), /*I*/ 0, 0, 1, 0 },
+		{ /*M*/ _m(3,0), _m(3, 1), _m(3, 2), _m(3, 3), /*I*/ 0, 0, 0, 1 },
+	};
+#undef _m
+	float m0, m1, m2, m3, s;
+	float *r0, *r1, *r2, *r3;
+	r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3];
+#define SWAP_ROWS(a, b) { float *_tmp = a; (a)=(b); (b)=_tmp; }
+
+	//#define optimize(block) if(s!=0.f){block}
+#define optimize(block) block
+
+	/* choose pivot - or die */
+	if (absf(r3[0]) > absf(r2[0])) SWAP_ROWS(r3, r2);
+	if (absf(r2[0]) > absf(r1[0])) SWAP_ROWS(r2, r1);
+	if (absf(r1[0]) > absf(r0[0])) SWAP_ROWS(r1, r0);
+	if (0.0f == r0[0]) return 0;
+
+	/* eliminate first variable */
+	m1 = r1[0] / r0[0]; m2 = r2[0] / r0[0]; m3 = r3[0] / r0[0];
+	s = r0[1]; optimize(r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s; )
+	s = r0[2]; optimize(r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s; )
+	s = r0[3]; optimize(r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s; )
+	s = r0[4]; optimize(r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s; )
+	s = r0[5]; optimize(r1[5] -= m1 * s; r2[5] -= m2 * s; r3[5] -= m3 * s; )
+	s = r0[6]; optimize(r1[6] -= m1 * s; r2[6] -= m2 * s; r3[6] -= m3 * s; )
+	s = r0[7]; optimize(r1[7] -= m1 * s; r2[7] -= m2 * s; r3[7] -= m3 * s; )
+
+	/* choose pivot - or die */
+	if (absf(r3[1]) > absf(r2[1])) SWAP_ROWS(r3, r2);
+	if (absf(r2[1]) > absf(r1[1])) SWAP_ROWS(r2, r1);
+	if (0.0F == r1[1]) return 0;
+
+	/* eliminate second variable */
+	m2 = r2[1] / r1[1]; m3 = r3[1] / r1[1];
+	s = r1[2]; optimize(r2[2] -= m2 * s; r3[2] -= m3 * s; )
+	s = r1[3]; optimize(r2[3] -= m2 * s; r3[3] -= m3 * s; )
+	s = r1[4]; optimize(r2[4] -= m2 * s; r3[4] -= m3 * s; )
+	s = r1[5]; optimize(r2[5] -= m2 * s; r3[5] -= m3 * s; )
+	s = r1[6]; optimize(r2[6] -= m2 * s; r3[6] -= m3 * s; )
+	s = r1[7]; optimize(r2[7] -= m2 * s; r3[7] -= m3 * s; )
+
+	/* choose pivot - or die */
+	if (absf(r3[2])>absf(r2[2])) SWAP_ROWS(r3, r2);
+	if (0.0F == r2[2]) return 0;
+
+	/* eliminate third variable */
+	m3 = r3[2] / r2[2];
+	s = r2[3]; optimize(r3[3] -= m3 * s; )
+	s = r2[4]; optimize(r3[4] -= m3 * s; )
+	s = r2[5]; optimize(r3[5] -= m3 * s; )
+	s = r2[6]; optimize(r3[6] -= m3 * s; )
+	s = r2[7]; optimize(r3[7] -= m3 * s; )
+
+#undef optimize
+
+	/* last check */
+	if (0.0F == r3[3]) return 0;
+
+	s = 1.0F / r3[3];             /* now back substitute row 3 */
+	r3[4] *= s; r3[5] *= s; r3[6] *= s; r3[7] *= s;
+
+	m2 = r2[3];                 /* now back substitute row 2 */
+	s = 1.0F / r2[2];
+	r2[4] = s * (r2[4] - r3[4] * m2), r2[5] = s * (r2[5] - r3[5] * m2),
+	r2[6] = s * (r2[6] - r3[6] * m2), r2[7] = s * (r2[7] - r3[7] * m2);
+	m1 = r1[3];
+	r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1,
+	r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1;
+	m0 = r0[3];
+	r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0,
+	r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0;
+
+	m1 = r1[2];                 /* now back substitute row 1 */
+	s = 1.0F / r1[1];
+	r1[4] = s * (r1[4] - r2[4] * m1), r1[5] = s * (r1[5] - r2[5] * m1),
+	r1[6] = s * (r1[6] - r2[6] * m1), r1[7] = s * (r1[7] - r2[7] * m1);
+	m0 = r0[2];
+	r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0,
+	r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0;
+
+	m0 = r0[1];                 /* now back substitute row 0 */
+	s = 1.0F / r0[0];
+	r0[4] = s * (r0[4] - r1[4] * m0), r0[5] = s * (r0[5] - r1[5] * m0),
+	r0[6] = s * (r0[6] - r1[6] * m0), r0[7] = s * (r0[7] - r1[7] * m0);
+
+	out->e00 = r0[4]; out->e01 = r0[5]; out->e02 = r0[6]; out->e03 = r0[7];
+	out->e10 = r1[4]; out->e11 = r1[5]; out->e12 = r1[6]; out->e13 = r1[7];
+	out->e20 = r2[4]; out->e21 = r2[5]; out->e22 = r2[6]; out->e23 = r2[7];
+	out->e30 = r3[4]; out->e31 = r3[5]; out->e32 = r3[6]; out->e33 = r3[7];
+
+#undef SWAP_ROWS
+
+	return 1;
+}
+
+/*
+** 对只包含基本3D变换的矩阵进行变换，先计算左上角3x3的RS矩阵的逆（通过伴随矩阵），然后
+** 乘上平移矩阵的逆矩阵，即
+** M^-1 = (T(RS))^-1 = (RS)^-1 * T^-1
+*/
+bool mat4_invertgeneral3d(Mat4* in, Mat4* out) {
+	ssr_assert(in && out);
+	if (in == out) shrmat(in);
+
+	mat4_setidentity(out);
+
+	/*计算左上角3x3矩阵的行列式*/
+	float pos = 0, neg = 0, t;
+	float det;
+
+	t = in->e00 * in->e11 * in->e22;
+	if (t >= 0) pos += t; else neg += t;
+	t = in->e10 * in->e21 * in->e02;
+	if (t >= 0) pos += t; else neg += t;
+	t = in->e20 * in->e01 * in->e12;
+	if (t >= 0) pos += t; else neg += t;
+
+	t = -in->e20 * in->e11 * in->e02;
+	if (t >= 0) pos += t; else neg += t;
+	t = -in->e10 * in->e01 * in->e22;
+	if (t >= 0) pos += t; else neg += t;
+	t = -in->e00 * in->e21 * in->e12;
+	if (t >= 0) pos += t; else neg += t;
+
+	det = pos + neg;
+
+	if (det * det < 1e-25)
+		return 0; /*行列式为0*/
+
+	det = 1.f / det; 
+	MAT(out, 0, 0) = ((MAT(in, 1, 1)*MAT(in, 2, 2) - MAT(in, 2, 1)*MAT(in, 1, 2))*det);
+	MAT(out, 0, 1) = (-(MAT(in, 0, 1)*MAT(in, 2, 2) - MAT(in, 2, 1)*MAT(in, 0, 2))*det);
+	MAT(out, 0, 2) = ((MAT(in, 0, 1)*MAT(in, 1, 2) - MAT(in, 1, 1)*MAT(in, 0, 2))*det);
+	MAT(out, 1, 0) = (-(MAT(in, 1, 0)*MAT(in, 2, 2) - MAT(in, 2, 0)*MAT(in, 1, 2))*det);
+	MAT(out, 1, 1) = ((MAT(in, 0, 0)*MAT(in, 2, 2) - MAT(in, 2, 0)*MAT(in, 0, 2))*det);
+	MAT(out, 1, 2) = (-(MAT(in, 0, 0)*MAT(in, 1, 2) - MAT(in, 1, 0)*MAT(in, 0, 2))*det);
+	MAT(out, 2, 0) = ((MAT(in, 1, 0)*MAT(in, 2, 1) - MAT(in, 2, 0)*MAT(in, 1, 1))*det);
+	MAT(out, 2, 1) = (-(MAT(in, 0, 0)*MAT(in, 2, 1) - MAT(in, 2, 0)*MAT(in, 0, 1))*det);
+	MAT(out, 2, 2) = ((MAT(in, 0, 0)*MAT(in, 1, 1) - MAT(in, 1, 0)*MAT(in, 0, 1))*det);
+
+	// 乘T^-1
+	MAT(out, 0, 3) = -(MAT(in, 0, 3) * MAT(out, 0, 0) +
+		MAT(in, 1, 3) * MAT(out, 0, 1) +
+		MAT(in, 2, 3) * MAT(out, 0, 2));
+	MAT(out, 1, 3) = -(MAT(in, 0, 3) * MAT(out, 1, 0) +
+		MAT(in, 1, 3) * MAT(out, 1, 1) +
+		MAT(in, 2, 3) * MAT(out, 1, 2));
+	MAT(out, 2, 3) = -(MAT(in, 0, 3) * MAT(out, 2, 0) +
+		MAT(in, 1, 3) * MAT(out, 2, 1) +
+		MAT(in, 2, 3) * MAT(out, 2, 2));
+
+	return 1;
+}
+
+void mat4_invertpos(Mat4* in, Mat4* out) {
+
+}
+
+void mat4_invertscale(Mat4* in, Mat4* out) {
+
+}
+
+void mat4_invertrot(Mat4* in, Mat4* out) {
+	ssr_assert(in && out);
+	mat4_transpose(in, out);
+}
+
+void mat4_settr(Vec3* pos, Quat* rot, Mat4* out) {
+	ssr_assert(pos && rot && out);
+	mat4_zero(out);
+	quat_tomat4(rot, out);
+	out->e03 = pos->x;
+	out->e13 = pos->y;
+	out->e23 = pos->z;
+}
+
+void mat4_settrs(Vec3* pos, Quat* rot, Vec3* scale, Mat4* out) {
+	ssr_assert(pos && rot && scale && out);
+	mat4_zero(out);
+	quat_tomat4(rot, out); /*pos*rot*scale的顺序*/
+	out->e00 *= scale->x; out->e01 *= scale->y; out->e02 *= scale->z;
+	out->e10 *= scale->x; out->e11 *= scale->y; out->e12 *= scale->z;
+	out->e20 *= scale->x; out->e21 *= scale->y; out->e22 *= scale->z;
+	out->e03 = pos->x;
+	out->e13 = pos->y;
+	out->e23 = pos->z;
+}
+
+void mat4_settrinverse(Vec3* pos, Quat* rot, Mat4* out) {
+	ssr_assert(pos && rot && out);
+	mat4_zero(out);
+	quat_invert(rot, rot);
+	quat_tomat4(rot, out);
+	Vec3 reverse = { -pos->x, -pos->y, -pos->z};
+	mat4_translate(out, &reverse, out); /* (TR)^-1 = R^-1*T^-1所以这里是右乘*/
+}
+
+void mat4_scale(Mat4* m, Vec3* scale, Mat4* out) {
+	ssr_assert(m && scale && out);
+	if (out != m) {
+		*out = *m;
+	}
+	/*
+	scale matrix 
+	x, 0, 0, 0,
+	0, y, 0, 0,
+	0, 0, z, 0, 
+	0, 0, 0, 1
+	*/
+	out->e00 *= scale->x;
+	out->e10 *= scale->x;
+	out->e20 *= scale->x;
+	out->e30 *= scale->x;
+
+	out->e01 *= scale->y;
+	out->e11 *= scale->y;
+	out->e21 *= scale->y;
+	out->e31 *= scale->y;
+	
+	out->e02 *= scale->z;
+	out->e12 *= scale->z;
+	out->e22 *= scale->z;
+	out->e32 *= scale->z;
+}
+
+void mat4_translate(Mat4* m, Vec3* pos, Mat4* out) {
+	ssr_assert(m && pos && out);
+	if (out != m) {
+		*out = *m;
+	}
+	/*
+	translate matrix 
+	1, 0, 0, x,
+	0, 1, 0, y,
+	0, 0, 1, z,
+	0, 0, 0, 1,
+	*/
+	out->e03 = out->e00 * pos->x + out->e01 * pos->y + out->e02 * pos->z + out->e03;
+	out->e13 = out->e10 * pos->x + out->e11 * pos->y + out->e12 * pos->z + out->e13;
+	out->e23 = out->e20 * pos->x + out->e21 * pos->y + out->e22 * pos->z + out->e23;
+	out->e33 = out->e30 * pos->x + out->e31 * pos->y + out->e32 * pos->z + out->e33;
+}
+
+void mat4_rotate(Mat4* m, float angle, Vec3* ax, Mat4* out) {
+	ssr_assert(m && ax && out);
+	Mat4 rot; 
+	mat4_setaxisangle(ax, angle, &rot);
+	mat4_multiply(m, &rot, out);
+}
+
+void mat4_decomposetrs(Mat4* src, Vec3* pos, Quat* quat, Vec3* scale) {
+	ssr_assert(src && pos && quat && scale); 
+
+	Vec3* x = &src->colums[0];
+	Vec3* y = &src->colums[1];
+	Vec3* z = &src->colums[2];
+	Vec3* w = &src->colums[3];
+
+	*pos = *w;
+
+	quat_setlookrotation(z, y, quat);
+
+	scale->x = vec3_magnitude(x);
+	scale->y = vec3_magnitude(y);
+	scale->z = vec3_magnitude(z);
+}
+
+static void MakePositive(Euler* euler) {/*弧度制欧拉角*/
+	const float negativeFlip = -0.0001F;
+	const float positiveFlip = (PI * 2.0F) - 0.0001F;
+
+	if (euler->x < negativeFlip)
+		euler->x += 2.0 * PI;
+	else if (euler->x > positiveFlip)
+		euler->x -= 2.0 * PI;
+
+	if (euler->y < negativeFlip)
+		euler->y += 2.0 * PI;
+	else if (euler->y > positiveFlip)
+		euler->y -= 2.0 * PI;
+
+	if (euler->z < negativeFlip)
+		euler->z += 2.0 * PI;
+	else if (euler->z > positiveFlip)
+		euler->z -= 2.0 * PI;
+}
+
+static void SanitizeEuler(Euler* e) {/*弧度制欧拉角*/
+	MakePositive(e);
+}
+
+/*from unity src*/
+bool mat4_toeuler(Mat4* in, Euler* out) {
+	ssr_assert(in && out);
+	// from http://www.geometrictools.com/Documentation/EulerAngles.pdf
+	// YXZ order
+	if (MAT(in, 1, 2) < 0.999F) // some fudge for imprecision
+	{
+		if (MAT(in, 1, 2) > -0.999F) // some fudge for imprecision
+		{
+			out->x = asin(-MAT(in, 1, 2));
+			out->y = atan2(MAT(in, 0, 2), MAT(in, 2, 2));
+			out->z = atan2(MAT(in, 1, 0), MAT(in, 1, 1));
+			//euler_rad2deg(out, out);
+			SanitizeEuler(out);
+			euler_rad2deg(out, out);
+			return 1;
+		}
+		else
+		{
+			// WARNING.  Not unique.  YA - ZA = atan2(r01,r00)
+			out->x = PI * 0.5F;
+			out->y = atan2(MAT(in, 0, 1), MAT(in, 0, 0));
+			out->z = 0.0F;
+			//euler_rad2deg(out, out);
+			SanitizeEuler(out);
+			euler_rad2deg(out, out);
+			return 0;
+		}
+	}
+	else
+	{
+		// WARNING.  Not unique.  YA + ZA = atan2(-r01,r00)
+		out->x = -PI * 0.5F;
+		out->y = atan2(-MAT(in, 0, 1), MAT(in, 0, 0));
+		out->z = 0.0F;
+		//euler_rad2deg(out, out);
+		SanitizeEuler(out);
+		euler_rad2deg(out, out);
+		return 0;
+	}
+}
+
+/*from unity src*/
+/*https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/*/
+void mat4_toquat(Mat4* in, Quat* out) {
+	// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
+	// article "Quaternionf Calculus and Fast Animation".
+	float fTrace = MAT(in, 0, 0) + MAT(in, 1, 1) + MAT(in, 2, 2);
+	float fRoot;
+
+	if (fTrace > 0.0f)
+	{
+		// |w| > 1/2, may as well choose w > 1/2
+		fRoot = sqrt(fTrace + 1.0f);  // 2w
+		out->w = 0.5f*fRoot;
+		fRoot = 0.5f / fRoot;  // 1/(4w)
+		out->x = (MAT(in, 2, 1) - MAT(in, 1, 2))*fRoot;
+		out->y = (MAT(in, 0, 2) - MAT(in, 2, 0))*fRoot;
+		out->z = (MAT(in, 1, 0) - MAT(in, 0, 1))*fRoot;
+	}
+	else
+	{
+		// |w| <= 1/2
+		int s_iNext[3] = { 1, 2, 0 };
+		int i = 0;
+		if (MAT(in, 1, 1) > MAT(in, 0, 0))
+			i = 1;
+		if (MAT(in, 2, 2) > MAT(in, i, i))
+			i = 2;
+		int j = s_iNext[i];
+		int k = s_iNext[j];
+
+		fRoot = sqrt(MAT(in, i, i) - MAT(in, j, j) - MAT(in, k, k) + 1.0f);
+		float* apkQuat[3] = { &out->x, &out->y, &out->z };
+		ssr_assert(fRoot >= EPSILON);
+		*apkQuat[i] = 0.5f*fRoot;
+		fRoot = 0.5f / fRoot;
+		out->w = (MAT(in, k, j) - MAT(in, j, k)) * fRoot;
+		*apkQuat[j] = (MAT(in, j, i) + MAT(in, i, j))*fRoot;
+		*apkQuat[k] = (MAT(in, k, i) + MAT(in, i, k))*fRoot;
+	}
+	quat_normalize(out, out);
+}
+
+void mat3_applytovec3(Mat3* m, Vec3* v, Vec3* out) {
+	ssr_assert(m && v && out);
+	out->x = m->e00 * v->x + m->e01 * v->y + m->e02 * v->z;
+	out->y = m->e10 * v->x + m->e11 * v->y + m->e12 * v->z;
+	out->z = m->e20 * v->x + m->e21 * v->y + m->e22 * v->z;
+}
+
+void mat23_applytovec3(Mat23* m, Vec3* v, Vec2* out) {
+	ssr_assert(m && v && out);
+	out->x = m->e00 * v->x + m->e01 * v->y + m->e02 * v->z;
+	out->y = m->e10 * v->x + m->e11 * v->y + m->e12 * v->z;
+}
+
+void mat43_applytovec3(Mat43* m, Vec3* v, Vec4* out) {
+	ssr_assert(m && v && out);
+	out->x = m->e00 * v->x + m->e01 * v->y + m->e02 * v->z;
+	out->y = m->e10 * v->x + m->e11 * v->y + m->e12 * v->z;
+	out->z = m->e20 * v->x + m->e21 * v->y + m->e22 * v->z;
+	out->w = m->e30 * v->x + m->e31 * v->y + m->e32 * v->z;
+}
\ No newline at end of file
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);
+}
diff --git a/src/math/vec2.c b/src/math/vec2.c
new file mode 100644
index 0000000..86192dc
--- /dev/null
+++ b/src/math/vec2.c
@@ -0,0 +1,36 @@
+#include "math.h"
+#include "../util/assert.h"
+#include "../core/mem.h"
+
+void vec2_scale(Vec2* v, float k, Vec2* out) {
+	ssr_assert(v && out);
+	out->x = v->x * k;
+	out->y = v->y * k;
+}
+
+void vec2_plus(Vec2* v1, Vec2* v2, Vec2* out) {
+	ssr_assert(v1 && v2 && out);
+	out->x = v1->x + v2->x;
+	out->y = v1->y + v2->y;
+}
+
+void vec2_offset(Vec2* v, float offset, Vec2* out) {
+	ssr_assert(v && out);
+	out->x = v->x + offset; 
+	out->y = v->y + offset;
+}
+
+float vec2_dot(Vec2* v1, Vec2* v2) {
+	ssr_assert(v1 && v2);
+	float d = v1->x * v2->x + v1->y * v2->y;
+	return d; 
+}
+
+void vec2_tostring(Vec2* v, char buf[]) {
+	sprintf(buf, "%8.3f %8.3f", v->x, v->y);
+}
+
+void vec2_print(Vec2* v) {
+	vec2_tostring(v, printbuffer);
+	printf("%s", printbuffer);
+}
\ No newline at end of file
diff --git a/src/math/vec3.c b/src/math/vec3.c
new file mode 100644
index 0000000..4d77b96
--- /dev/null
+++ b/src/math/vec3.c
@@ -0,0 +1,120 @@
+#include <math.h>
+
+#include "math.h"
+#include "../util/assert.h"
+#include "../core/mem.h"
+
+Vec3 vec3forward = {0,0,1};
+Vec3 vec3up = {0, 1, 0};
+Vec3 vec3left = {1, 0, 0};
+
+void vec3_cross(Vec3* v1, Vec3* v2, Vec3* out) {
+	ssr_assert(v1 && v2 && out);
+	out->x = v1->y*v2->z - v1->z*v2->y;
+	out->y = v1->z*v2->x - v1->x*v2->z;
+	out->z = v1->x*v2->y - v1->y*v2->x;
+}
+
+void vec3_scale(Vec3* v, float k, Vec3* out) {
+	ssr_assert(v && out); 
+	out->x = v->x * k; 
+	out->y = v->y * k; 
+	out->z = v->z * k;
+}
+
+void vec3_plus(Vec3* v1, Vec3* v2, Vec3* out) {
+	ssr_assert(v1 && v2 && out);
+	out->x = v1->x + v2->x; 
+	out->y = v1->y + v2->y; 
+	out->z = v1->z + v2->z;
+}
+
+void vec3_offset(Vec3* v, float offset, Vec3* out) {
+	ssr_assert(v && out);
+	out->x = v->x + offset;
+	out->y = v->y + offset;
+	out->z = v->z + offset;
+}
+
+float vec3_magnitude(Vec3* v) {
+	ssr_assert(v);
+	float l2 = vec3_magnitude2(v);
+	return sqrt(l2);
+}
+
+float vec3_magnitude2(Vec3* v) {
+	ssr_assert(v);
+	return v->x*v->x + v->y*v->y + v->z*v->z;
+}
+
+void vec3_lerp(Vec3* v1, Vec3* v2, float t, Vec3* out) {
+	ssr_assert(v1 && v2 && out);
+	float t2 = 1 - t;
+	out->x = v1->x * t2 + v2->x * t;
+	out->y = v1->y * t2 + v2->y * t;
+	out->z = v1->z * t2 + v2->z * t;
+}
+
+void vec3_slerp(Vec3* v1, Vec3* v2, float t, Vec3* out) {
+	ssr_assert(v1 && v2 && out);
+	float mag1 = vec3_magnitude(v1);
+	float mag2 = vec3_magnitude(v2);
+	if (mag1 < EPSILON || mag2 < EPSILON) {
+		vec3_lerp(v1, v2, t, out);
+		return;
+	}
+	float lerplen = lerp(mag1, mag2, t);
+	/*分为长度上的lerp和角度上的slerp*/
+	float dot = vec3_dot(v1, v2) / (mag1 * mag2);
+	if (dot > 1.f - EPSILON) { /*几乎是同向*/
+		vec3_lerp(v1, v2, t, out);
+		return;
+	} else if (dot < -1 + EPSILON) { /*几乎是反向*/
+
+	} else {
+		Vec3 axis; vec3_cross(v1, v2, &axis); vec3_normalize(&axis, &axis);
+		Vec3 v1n; vec3_normalize(v1, &v1n);
+		float angle = acos(dot) * t;
+		//Mat4 m; mat4_setaxisangle(&axis, angle, &m);
+		//mat4_applytovec4(&m, &v1n, &v1n);
+		Quat q; quat_fromaxisangle(&axis, angle, &q);
+		quat_applytovec3(&q, &v1n, &v1n);
+		vec3_scale(&v1n, lerplen, out);
+	}
+}
+
+float vec3_dot(Vec3* v1, Vec3* v2) {
+	ssr_assert(v1 && v2);
+	return v1->x * v2->x + v1->y * v2->y + v1->z * v2->z;
+}
+
+void vec3_multiply(Vec3* v1, Vec3* v2, Quat* out) {
+	ssr_assert(v1 && v2 && out);
+	vec3_cross(v1, v2, out);
+	out->w = -vec3_dot(v1, v2);
+}
+
+void vec3_normalize(Vec3* v, Vec3* out) {
+	ssr_assert(v && out);
+	//float mag = rsqrt(v->x * v->x + v->y * v->y + v->z * v->z);
+	float mag = 1.f / vec3_magnitude(v);
+	out->x = v->x * mag;
+	out->y = v->y * mag;
+	out->z = v->z * mag;
+}
+
+void vec3_tostring(Vec3* v, char buf[]) {
+	sprintf(buf, "%8.3f %8.3f %8.3f", v->x, v->y, v->z);
+}
+
+void vec3_print(Vec3* v) {
+	vec3_tostring(v, printbuffer);
+	printf("\n%s\n", printbuffer);
+}
+
+void vec3_minus(Vec3* v1, Vec3* v2, Vec3* out) {
+	ssr_assert(v1 && v2 && out);
+	out->x = v1->x - v2->x;
+	out->y = v1->y - v2->y;
+	out->z = v1->z - v2->z;
+}
\ No newline at end of file
diff --git a/src/math/vec4.c b/src/math/vec4.c
new file mode 100644
index 0000000..4216a08
--- /dev/null
+++ b/src/math/vec4.c
@@ -0,0 +1,24 @@
+#include <math.h>
+
+#include "math.h"
+#include "../util/assert.h"
+#include "../core/mem.h"
+
+
+
+void vec4_dividew(Vec4* v, Vec3* out) {
+	ssr_assert(out && v);
+	float w = 1.f / v->w;
+	out->x = v->x * w ;
+	out->y = v->y * w;
+	out->z = v->z * w;
+}
+
+void vec4_tostring(Vec4* v, char buf[]) {
+	sprintf(buf, "%8.3f %8.3f %8.3f %8.3f", v->x, v->y, v->z, v->w);
+}
+
+void vec4_print(Vec4* v) {
+	vec4_tostring(v, printbuffer);
+	printf("\n%s\n", printbuffer);
+}
\ No newline at end of file