diff options
Diffstat (limited to 'src/math/mat.c')
| -rw-r--r-- | src/math/mat.c | 755 |
1 files changed, 755 insertions, 0 deletions
diff --git a/src/math/mat.c b/src/math/mat.c new file mode 100644 index 0000000..84a5a41 --- /dev/null +++ b/src/math/mat.c @@ -0,0 +1,755 @@ +#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_setortho(float l, float r, float b, float t, float n, float f, Mat4* out) { + ssr_assert(out); + mat4_zero(out); + out->e00 = 2 / (r - l); + out->e03 = -(r + l) / (r - l); + out->e11 = 2 / (t - b); + out->e13 = -(t + b) / (t - b); + out->e22 = -2 / (f - n); + out->e23 = -(f + n) / (f - n); + 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); + ssr_assert(fov > 0 && aspect > 0); + ssr_assert(near > 0 && far > 0 && near != far); + 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 |