mirror/ss
1
0
Fork 0
mirror of https://github.com/zeldaret/ss synced 2026-05-23 06:54:20 -04:00
Code Issues Packages Projects Releases Wiki Activity
Files
f7b4c029fd5176e817e3de7209727480c445a3d2
ss/src/egg/math/eggMatrix.cpp
T
robojumper d8744e1f34 MTX/VEC cleanup
2026-05-07 19:53:30 +02:00

368 lines
9.6 KiB
C++
Raw Blame History

#include "egg/math/eggMatrix.h"
#include "rvl/GX.h" // IWYU pragma: export
#include "rvl/MTX.h" // IWYU pragma: export
namespace EGG {
void Matrix34f::inverseTo(Matrix34f &to) const {
MTXInverse(m, to.m);
}
void Matrix34f::inverseTransposeTo(Matrix34f &to) const {
MTXInvXpose(m, to.m);
}
void Matrix34f::makeIdentity() {
makeZero();
m[2][2] = 1.0f;
m[1][1] = 1.0f;
m[0][0] = 1.0f;
} // namespace EGG
Matrix34f::Matrix34f(f32 xx, f32 xy, f32 xz, f32 xw, f32 yx, f32 yy, f32 yz, f32 yw, f32 zx, f32 zy, f32 zz, f32 zw) {
m[0][0] = xx;
m[0][1] = xy;
m[0][2] = xz;
m[0][3] = xw;
m[1][0] = yx;
m[1][1] = yy;
m[1][2] = yz;
m[1][3] = yw;
m[2][0] = zx;
m[2][1] = zy;
m[2][2] = zz;
m[2][3] = zw;
}
void Matrix34f::makeSRT(const Vector3f &s, const Vector3f &r, const Vector3f &t) {
const f32 sin[3] = {Math<f32>::sin(r.x), Math<f32>::sin(r.y), Math<f32>::sin(r.z)};
const f32 cos[3] = {Math<f32>::cos(r.x), Math<f32>::cos(r.y), Math<f32>::cos(r.z)};
const f32 c0_c2 = cos[0] * cos[2];
const f32 s0_s1 = sin[0] * sin[1];
const f32 c0_s2 = cos[0] * sin[2];
m[0][0] = s.x * (cos[1] * cos[2]);
m[1][0] = s.x * (cos[1] * sin[2]);
m[2][0] = s.x * (-sin[1]);
m[0][1] = s.y * ((s0_s1 * cos[2]) - c0_s2);
m[1][1] = s.y * ((s0_s1 * sin[2]) + c0_c2);
m[2][1] = s.y * (sin[0] * cos[1]);
m[0][2] = s.z * ((c0_c2 * sin[1]) + (sin[0] * sin[2]));
m[1][2] = s.z * ((c0_s2 * sin[1]) - (sin[0] * cos[2]));
m[2][2] = s.z * (cos[0] * cos[1]);
m[0][3] = t.x;
m[1][3] = t.y;
m[2][3] = t.z;
}
void Matrix34f::makeRT(const Vector3f &r, const Vector3f &t) {
const f32 sin[3] = {Math<f32>::sin(r.x), Math<f32>::sin(r.y), Math<f32>::sin(r.z)};
const f32 cos[3] = {Math<f32>::cos(r.x), Math<f32>::cos(r.y), Math<f32>::cos(r.z)};
const f32 c0_c2 = cos[0] * cos[2];
const f32 s0_s1 = sin[0] * sin[1];
const f32 c0_s2 = cos[0] * sin[2];
m[0][0] = (cos[1] * cos[2]);
m[1][0] = (cos[1] * sin[2]);
m[2][0] = (-sin[1]);
m[0][1] = (s0_s1 * cos[2]) - c0_s2;
m[1][1] = (s0_s1 * sin[2]) + c0_c2;
m[2][1] = (sin[0] * cos[1]);
m[0][2] = (c0_c2 * sin[1]) + (sin[0] * sin[2]);
m[1][2] = (c0_s2 * sin[1]) - (sin[0] * cos[2]);
m[2][2] = (cos[0] * cos[1]);
m[0][3] = t.x;
m[1][3] = t.y;
m[2][3] = t.z;
}
void Matrix34f::makeR(const Vector3f &r) {
const f32 sin[3] = {Math<f32>::sin(r.x), Math<f32>::sin(r.y), Math<f32>::sin(r.z)};
const f32 cos[3] = {Math<f32>::cos(r.x), Math<f32>::cos(r.y), Math<f32>::cos(r.z)};
const f32 c0_c2 = cos[0] * cos[2];
const f32 s0_s1 = sin[0] * sin[1];
const f32 c0_s2 = cos[0] * sin[2];
m[0][0] = (cos[1] * cos[2]);
m[1][0] = (cos[1] * sin[2]);
m[2][0] = (-sin[1]);
m[0][1] = (s0_s1 * cos[2]) - c0_s2;
m[1][1] = (s0_s1 * sin[2]) + c0_c2;
m[2][1] = (sin[0] * cos[1]);
m[0][2] = (c0_c2 * sin[1]) + (sin[0] * sin[2]);
m[1][2] = (c0_s2 * sin[1]) - (sin[0] * cos[2]);
m[2][2] = (cos[0] * cos[1]);
m[0][3] = 0.0f;
m[1][3] = 0.0f;
m[2][3] = 0.0f;
}
void Matrix34f::makeST(const Vector3f &s, const Vector3f &t) {
m[0][0] = s.x;
m[1][0] = 0.0f;
m[2][0] = 0.0f;
m[0][1] = 0.0f;
m[1][1] = s.y;
m[2][1] = 0.0f;
m[0][2] = 0.0f;
m[1][2] = 0.0f;
m[2][2] = s.z;
m[0][3] = t.x;
m[1][3] = t.y;
m[2][3] = t.z;
}
void Matrix34f::makeSQT(const Vector3f &s, const Quatf &q, const Vector3f &t) {
f32 yy = 2.0f * q.v.y * q.v.y;
f32 zz = 2.0f * q.v.z * q.v.z;
f32 xx = 2.0f * q.v.x * q.v.x;
f32 xy = 2.0f * q.v.x * q.v.y;
f32 xz = 2.0f * q.v.x * q.v.z;
f32 yz = 2.0f * q.v.y * q.v.z;
f32 wz = 2.0f * q.w * q.v.z;
f32 wx = 2.0f * q.w * q.v.x;
f32 wy = 2.0f * q.w * q.v.y;
m[0][0] = s.x * (1.0f - yy - zz);
m[0][1] = s.y * (xy - wz);
m[0][2] = s.z * (xz + wy);
m[1][0] = s.x * (xy + wz);
m[1][1] = s.y * (1.0f - xx - zz);
m[1][2] = s.z * (yz - wx);
m[2][0] = s.x * (xz - wy);
m[2][1] = s.y * (yz + wx);
m[2][2] = s.z * (1.0f - xx - yy);
m[0][3] = t.x;
m[1][3] = t.y;
m[2][3] = t.z;
}
void Matrix34f::makeQT(const Quatf &q, const Vector3f &t) {
f32 yy = 2.0f * q.v.y * q.v.y;
f32 zz = 2.0f * q.v.z * q.v.z;
f32 xx = 2.0f * q.v.x * q.v.x;
f32 xy = 2.0f * q.v.x * q.v.y;
f32 xz = 2.0f * q.v.x * q.v.z;
f32 yz = 2.0f * q.v.y * q.v.z;
f32 wz = 2.0f * q.w * q.v.z;
f32 wx = 2.0f * q.w * q.v.x;
f32 wy = 2.0f * q.w * q.v.y;
m[0][0] = 1.0f - yy - zz;
m[0][1] = xy - wz;
m[0][2] = xz + wy;
m[1][0] = xy + wz;
m[1][1] = 1.0f - xx - zz;
m[1][2] = yz - wx;
m[2][0] = xz - wy;
m[2][1] = yz + wx;
m[2][2] = 1 - xx - yy;
m[0][3] = t.x;
m[1][3] = t.y;
m[2][3] = t.z;
}
void Matrix34f::makeQ(const Quatf &q) {
f32 yy = 2.0f * q.v.y * q.v.y;
f32 zz = 2.0f * q.v.z * q.v.z;
f32 xx = 2.0f * q.v.x * q.v.x;
f32 xy = 2.0f * q.v.x * q.v.y;
f32 xz = 2.0f * q.v.x * q.v.z;
f32 yz = 2.0f * q.v.y * q.v.z;
f32 wz = 2.0f * q.w * q.v.z;
f32 wx = 2.0f * q.w * q.v.x;
f32 wy = 2.0f * q.w * q.v.y;
m[0][0] = 1.0f - yy - zz;
m[0][1] = xy - wz;
m[0][2] = xz + wy;
m[1][0] = xy + wz;
m[1][1] = 1.0f - xx - zz;
m[1][2] = yz - wx;
m[2][0] = xz - wy;
m[2][1] = yz + wx;
m[2][2] = 1.0f - xx - yy;
m[0][3] = 0.0f;
m[1][3] = 0.0f;
m[2][3] = 0.0f;
}
void Matrix34f::makeS(const Vector3f &s) {
m[0][0] = s.x;
m[0][1] = 0.0f;
m[0][2] = 0.0f;
m[1][0] = 0.0f;
m[1][1] = s.y;
m[1][2] = 0.0f;
m[2][0] = 0.0f;
m[2][1] = 0.0f;
m[2][2] = s.z;
m[0][3] = 0.0f;
m[1][3] = 0.0f;
m[2][3] = 0.0f;
}
void Matrix34f::makeT(const Vector3f &t) {
m[0][0] = 1.0f;
m[0][1] = 0.0f;
m[0][2] = 0.0f;
m[1][0] = 0.0f;
m[1][1] = 1.0f;
m[1][2] = 0.0f;
m[2][0] = 0.0f;
m[2][1] = 0.0f;
m[2][2] = 1.0f;
m[0][3] = t.x;
m[1][3] = t.y;
m[2][3] = t.z;
}
void Matrix34f::fromQuat(const Quatf &q) {
m[0][0] = 1.0f - (2 * q.v.y * q.v.y) - (2.0f * q.v.z * q.v.z);
m[0][1] = (2.0f * q.v.x * q.v.y) - (2.0f * q.w * q.v.z);
m[0][2] = (2.0f * q.v.x * q.v.z) + (2.0f * q.w * q.v.y);
m[1][0] = (2.0f * q.v.x * q.v.y) + (2.0f * q.w * q.v.z);
m[1][1] = 1.0f - (2.0f * q.v.x * q.v.x) - (2.0f * q.v.z * q.v.z);
m[1][2] = (2.0f * q.v.y * q.v.z) - (2.0f * q.w * q.v.x);
m[2][0] = (2.0f * q.v.x * q.v.z) - (2.0f * q.w * q.v.y);
m[2][1] = (2.0f * q.v.y * q.v.z) + (2.0f * q.w * q.v.x);
m[2][2] = 1.0f - (2.0f * q.v.x * q.v.x) - (2.0f * q.v.y * q.v.y);
m[2][3] = 0.0f;
m[1][3] = 0.0f;
m[0][3] = 0.0f;
}
void Matrix34f::toQuat(Quatf &q) const {
const f32 temp0 = (m[0][0] + m[1][1] + m[2][2] + 1.0f) * 0.25f;
const f32 temp1 = temp0 - (m[1][1] + m[2][2]) * 0.5f;
const f32 temp2 = temp0 - (m[2][2] + m[0][0]) * 0.5f;
const f32 temp3 = temp0 - (m[0][0] + m[1][1]) * 0.5f;
// The fun method
// int tempMax = temp0 > temp1 ?
// (temp0 > temp2 ? (temp0 > temp3 ? 0 : 3) : (temp2 > temp3 ? 2 : 3)) :
// (temp1 > temp2 ? temp1 > temp3 ? 1 : 3 : (temp2 > temp3 ? 2 : 3));
int tempMax;
if (temp0 > temp1) {
if (temp0 > temp2) {
if (temp0 > temp3) {
tempMax = 0;
} else {
tempMax = 3;
}
} else {
if (temp2 > temp3) {
tempMax = 2;
} else {
tempMax = 3;
}
}
} else if (temp1 > temp2) {
if (temp1 > temp3) {
tempMax = 1;
} else {
tempMax = 3;
}
} else if (temp2 > temp3) {
tempMax = 2;
} else {
tempMax = 3;
}
switch (tempMax) {
case 0:
q.w = Math<f32>::sqrt(temp0);
q.v.x = (0.25f / q.w) * (m[2][1] - m[1][2]);
q.v.y = (0.25f / q.w) * (m[0][2] - m[2][0]);
q.v.z = (0.25f / q.w) * (m[1][0] - m[0][1]);
break;
case 1:
q.v.x = Math<f32>::sqrt(temp1);
q.w = (0.25f / q.v.x) * (m[2][1] - m[1][2]);
q.v.y = (0.25f / q.v.x) * (m[0][1] + m[1][0]);
q.v.z = (0.25f / q.v.x) * (m[0][2] + m[2][0]);
break;
case 2:
q.v.y = Math<f32>::sqrt(temp2);
q.w = (0.25f / q.v.y) * (m[0][2] - m[2][0]);
q.v.z = (0.25f / q.v.y) * (m[1][2] + m[2][1]);
q.v.x = (0.25f / q.v.y) * (m[1][0] + m[0][1]);
break;
case 3:
q.v.z = Math<f32>::sqrt(temp3);
q.w = (0.25f / q.v.z) * (m[1][0] - m[0][1]);
q.v.x = (0.25f / q.v.z) * (m[2][0] + m[0][2]);
q.v.y = (0.25f / q.v.z) * (m[2][1] + m[1][2]);
break;
default: break;
}
if (q.w < 0.0f) {
q.w = -q.w;
q.v.x = -q.v.x;
q.v.y = -q.v.y;
q.v.z = -q.v.z;
}
q.multScalar(Math<f32>::inv(Math<f32>::sqrt(q.w * q.w + q.v.dot(q.v))));
}
void Matrix34f::slerpTo(const Matrix34f &m2, Matrix34f &out, f32 t) const {
Quatf q1, q2, q3;
m2.toQuat(q1);
toQuat(q2);
q2.slerpTo(q1, t, q3);
out.makeQ(q3);
}
void Matrix34f::setAxisRotation(const Vector3f &axis, f32 rot) {
Quatf q;
q.setAxisRotation(axis, rot);
makeQ(q);
}
Vector3f Matrix34f::multVector(const Vector3f &vec) const {
Vector3f ret;
multVectorTo(vec, ret);
return ret;
}
void Matrix34f::loadPosMtx(u32 posMtxId) {
GXLoadPosMtxImm(m, posMtxId);
}
void Matrix34f::multiplyTo(const Matrix34f &m2, Matrix34f &to) const {
MTXConcat(m, m2.m, to.m);
}
void Matrix34f::dump() {}
Matrix34f Matrix34f::ident(1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f);
} // namespace EGG
Reference in New Issue View Git Blame Copy Permalink