mirror/dusklight
1
0
Fork 0
mirror of https://github.com/TwilitRealm/dusklight synced 2026-08-01 15:59:45 -04:00
Code Issues Packages Projects Releases Wiki Activity
Files
4bcecb3926bda9b752cfc69d1d2b38c0138ca7c0
dusklight/libs/dolphin/src/mtx/mtx44.c
T
Luke Street 4df8ccc871 Reorganize library code into libs/ (#3119) 
* Reorganize files into libs/{dolphin,JSystem,PowerPC_EABI_Support,revolution,TRK_MINNOW_DOLPHIN}

* Update configure.py and project.py for new libs structure

* Refactor `#include <dolphin/x.h>` -> `<x.h>`

* Remove `__REVOLUTION_SDK__` forwards from dolphin

* Fix dolphin/ references in revolution

* Wrap `#include <dolphin.h>` in `!__REVOLUTION_SDK__`

* Always build TRK against dolphin headers

* Resolve revolution SDK header resolution issues
2026-03-01 14:35:36 -08:00

889 lines
23 KiB
C
Raw Blame History

#include <dolphin/dolphin.h>
#include <dolphin/mtx.h>
#include <sdk_math.h>
static f32 mtxUnit[] = {0.0f, 1.0f, 0.5f, 3.0f};
void C_MTXFrustum(Mtx44 m, f32 t, f32 b, f32 l, f32 r, f32 n, f32 f) {
f32 tmp;
ASSERTMSGLINE(105, m, "MTXFrustum(): NULL Mtx44Ptr 'm' ");
ASSERTMSGLINE(106, t != b, "MTXFrustum(): 't' and 'b' clipping planes are equal ");
ASSERTMSGLINE(107, l != r, "MTXFrustum(): 'l' and 'r' clipping planes are equal ");
ASSERTMSGLINE(108, n != f, "MTXFrustum(): 'n' and 'f' clipping planes are equal ");
tmp = 1 / (r - l);
m[0][0] = (2 * n * tmp);
m[0][1] = 0;
m[0][2] = (tmp * (r + l));
m[0][3] = 0;
tmp = 1 / (t - b);
m[1][0] = 0;
m[1][1] = (2 * n * tmp);
m[1][2] = (tmp * (t + b));
m[1][3] = 0;
m[2][0] = 0;
m[2][1] = 0;
tmp = 1 / (f - n);
m[2][2] = (-n * tmp);
m[2][3] = (tmp * -(f * n));
m[3][0] = 0;
m[3][1] = 0;
m[3][2] = -1;
m[3][3] = 0;
}
void C_MTXPerspective(Mtx44 m, f32 fovY, f32 aspect, f32 n, f32 f) {
f32 angle;
f32 cot;
f32 tmp;
ASSERTMSGLINE(179, m, "MTXPerspective(): NULL Mtx44Ptr 'm' ");
ASSERTMSGLINE(180, (fovY > 0.0) && (fovY < 180.0), "MTXPerspective(): 'fovY' out of range ");
ASSERTMSGLINE(181, 0.0f != aspect, "MTXPerspective(): 'aspect' is 0 ");
angle = (0.5f * fovY);
angle = MTXDegToRad(angle);
cot = 1 / tanf(angle);
m[0][0] = (cot / aspect);
m[0][1] = 0;
m[0][2] = 0;
m[0][3] = 0;
m[1][0] = 0;
m[1][1] = (cot);
m[1][2] = 0;
m[1][3] = 0;
m[2][0] = 0;
m[2][1] = 0;
tmp = 1 / (f - n);
m[2][2] = (-n * tmp);
m[2][3] = (tmp * -(f * n));
m[3][0] = 0;
m[3][1] = 0;
m[3][2] = -1;
m[3][3] = 0;
}
void C_MTXOrtho(Mtx44 m, f32 t, f32 b, f32 l, f32 r, f32 n, f32 f) {
f32 tmp;
ASSERTMSGLINE(254, m, "MTXOrtho(): NULL Mtx44Ptr 'm' ");
ASSERTMSGLINE(255, t != b, "MTXOrtho(): 't' and 'b' clipping planes are equal ");
ASSERTMSGLINE(256, l != r, "MTXOrtho(): 'l' and 'r' clipping planes are equal ");
ASSERTMSGLINE(257, n != f, "MTXOrtho(): 'n' and 'f' clipping planes are equal ");
tmp = 1 / (r - l);
m[0][0] = 2 * tmp;
m[0][1] = 0;
m[0][2] = 0;
m[0][3] = (tmp * -(r + l));
tmp = 1 / (t - b);
m[1][0] = 0;
m[1][1] = 2 * tmp;
m[1][2] = 0;
m[1][3] = (tmp * -(t + b));
m[2][0] = 0;
m[2][1] = 0;
tmp = 1 / (f - n);
m[2][2] = (-1 * tmp);
m[2][3] = (-f * tmp);
m[3][0] = 0;
m[3][1] = 0;
m[3][2] = 0;
m[3][3] = 1;
}
void C_MTX44Identity(Mtx44 m) {
ASSERTMSGLINE(324, m != 0, "MTX44Identity(): NULL Mtx44 'm' ");
m[0][0] = 1.0f;
m[0][1] = 0.0f;
m[0][2] = 0.0f;
m[0][3] = 0.0f;
m[1][0] = 0.0f;
m[1][1] = 1.0f;
m[1][2] = 0.0f;
m[1][3] = 0.0f;
m[2][0] = 0.0f;
m[2][1] = 0.0f;
m[2][2] = 1.0f;
m[2][3] = 0.0f;
m[3][0] = 0.0f;
m[3][1] = 0.0f;
m[3][2] = 0.0f;
m[3][3] = 1.0f;
}
void PSMTX44Identity(__REGISTER Mtx44 m) {
__REGISTER f32 c1 = 1.0f;
__REGISTER f32 c0 = 0.0f;
asm {
stfs c1, 0x0(m)
psq_st c0, 0x4(m), 0, 0
psq_st c0, 0xc(m), 0, 0
stfs c1, 0x14(m)
psq_st c0, 0x18(m), 0, 0
psq_st c0, 0x20(m), 0, 0
stfs c1, 0x28(m)
psq_st c0, 0x2c(m), 0, 0
psq_st c0, 0x34(m), 0, 0
stfs c1, 0x3c(m)
}
}
void C_MTX44Copy(const Mtx44 src, Mtx44 dst) {
ASSERTMSGLINE(382, src != 0, "MTX44Copy(): NULL Mtx44Ptr 'src' ");
ASSERTMSGLINE(383, dst != 0, "MTX44Copy(): NULL Mtx44Ptr 'dst' ");
if (src != dst) {
dst[0][0] = src[0][0];
dst[0][1] = src[0][1];
dst[0][2] = src[0][2];
dst[0][3] = src[0][3];
dst[1][0] = src[1][0];
dst[1][1] = src[1][1];
dst[1][2] = src[1][2];
dst[1][3] = src[1][3];
dst[2][0] = src[2][0];
dst[2][1] = src[2][1];
dst[2][2] = src[2][2];
dst[2][3] = src[2][3];
dst[3][0] = src[3][0];
dst[3][1] = src[3][1];
dst[3][2] = src[3][2];
dst[3][3] = src[3][3];
}
}
asm void PSMTX44Copy(const __REGISTER Mtx44 src, __REGISTER Mtx44 dst) {
nofralloc
psq_l f1, 0x0(src), 0, 0
psq_st f1, 0x0(dst), 0, 0
psq_l f1, 0x8(src), 0, 0
psq_st f1, 0x8(dst), 0, 0
psq_l f1, 0x10(src), 0, 0
psq_st f1, 0x10(dst), 0, 0
psq_l f1, 0x18(src), 0, 0
psq_st f1, 0x18(dst), 0, 0
psq_l f1, 0x20(src), 0, 0
psq_st f1, 0x20(dst), 0, 0
psq_l f1, 0x28(src), 0, 0
psq_st f1, 0x28(dst), 0, 0
psq_l f1, 0x30(src), 0, 0
psq_st f1, 0x30(dst), 0, 0
psq_l f1, 0x38(src), 0, 0
psq_st f1, 0x38(dst), 0, 0
blr
}
void C_MTX44Concat(const Mtx44 a, const Mtx44 b, Mtx44 ab) {
Mtx44 mTmp;
Mtx44Ptr m;
ASSERTMSGLINE(454, a, "MTX44Concat(): NULL Mtx44Ptr 'a' ");
ASSERTMSGLINE(455, b, "MTX44Concat(): NULL Mtx44Ptr 'b' ");
ASSERTMSGLINE(456, ab, "MTX44Concat(): NULL Mtx44Ptr 'ab' ");
if (ab == a || ab == b) {
m = mTmp;
} else {
m = ab;
}
m[0][0] = (a[0][0] * b[0][0]) + (a[0][1] * b[1][0]) + (a[0][2] * b[2][0]) + (a[0][3] * b[3][0]);
m[0][1] = (a[0][0] * b[0][1]) + (a[0][1] * b[1][1]) + (a[0][2] * b[2][1]) + (a[0][3] * b[3][1]);
m[0][2] = (a[0][0] * b[0][2]) + (a[0][1] * b[1][2]) + (a[0][2] * b[2][2]) + (a[0][3] * b[3][2]);
m[0][3] = (a[0][0] * b[0][3]) + (a[0][1] * b[1][3]) + (a[0][2] * b[2][3]) + (a[0][3] * b[3][3]);
m[1][0] = (a[1][0] * b[0][0]) + (a[1][1] * b[1][0]) + (a[1][2] * b[2][0]) + (a[1][3] * b[3][0]);
m[1][1] = (a[1][0] * b[0][1]) + (a[1][1] * b[1][1]) + (a[1][2] * b[2][1]) + (a[1][3] * b[3][1]);
m[1][2] = (a[1][0] * b[0][2]) + (a[1][1] * b[1][2]) + (a[1][2] * b[2][2]) + (a[1][3] * b[3][2]);
m[1][3] = (a[1][0] * b[0][3]) + (a[1][1] * b[1][3]) + (a[1][2] * b[2][3]) + (a[1][3] * b[3][3]);
m[2][0] = (a[2][0] * b[0][0]) + (a[2][1] * b[1][0]) + (a[2][2] * b[2][0]) + (a[2][3] * b[3][0]);
m[2][1] = (a[2][0] * b[0][1]) + (a[2][1] * b[1][1]) + (a[2][2] * b[2][1]) + (a[2][3] * b[3][1]);
m[2][2] = (a[2][0] * b[0][2]) + (a[2][1] * b[1][2]) + (a[2][2] * b[2][2]) + (a[2][3] * b[3][2]);
m[2][3] = (a[2][0] * b[0][3]) + (a[2][1] * b[1][3]) + (a[2][2] * b[2][3]) + (a[2][3] * b[3][3]);
m[3][0] = (a[3][0] * b[0][0]) + (a[3][1] * b[1][0]) + (a[3][2] * b[2][0]) + (a[3][3] * b[3][0]);
m[3][1] = (a[3][0] * b[0][1]) + (a[3][1] * b[1][1]) + (a[3][2] * b[2][1]) + (a[3][3] * b[3][1]);
m[3][2] = (a[3][0] * b[0][2]) + (a[3][1] * b[1][2]) + (a[3][2] * b[2][2]) + (a[3][3] * b[3][2]);
m[3][3] = (a[3][0] * b[0][3]) + (a[3][1] * b[1][3]) + (a[3][2] * b[2][3]) + (a[3][3] * b[3][3]);
if (m == mTmp) {
C_MTX44Copy(mTmp, ab);
}
}
asm void PSMTX44Concat(const __REGISTER Mtx44 a, const __REGISTER Mtx44 b, __REGISTER Mtx44 ab) {
nofralloc
psq_l f0, 0x0(a), 0, 0
psq_l f2, 0x0(b), 0, 0
ps_muls0 f6, f2, f0
psq_l f3, 0x10(b), 0, 0
psq_l f4, 0x20(b), 0, 0
ps_madds1 f6, f3, f0, f6
psq_l f1, 0x8(a), 0, 0
psq_l f5, 0x30(b), 0, 0
ps_madds0 f6, f4, f1, f6
psq_l f0, 0x10(a), 0, 0
ps_madds1 f6, f5, f1, f6
psq_l f1, 0x18(a), 0, 0
ps_muls0 f8, f2, f0
ps_madds1 f8, f3, f0, f8
psq_l f0, 0x20(a), 0, 0
ps_madds0 f8, f4, f1, f8
ps_madds1 f8, f5, f1, f8
psq_l f1, 0x28(a), 0, 0
ps_muls0 f10, f2, f0
ps_madds1 f10, f3, f0, f10
psq_l f0, 0x30(a), 0, 0
ps_madds0 f10, f4, f1, f10
ps_madds1 f10, f5, f1, f10
psq_l f1, 0x38(a), 0, 0
ps_muls0 f12, f2, f0
psq_l f2, 0x8(b), 0, 0
ps_madds1 f12, f3, f0, f12
psq_l f0, 0x0(a), 0, 0
ps_madds0 f12, f4, f1, f12
psq_l f3, 0x18(b), 0, 0
ps_madds1 f12, f5, f1, f12
psq_l f1, 0x8(a), 0, 0
ps_muls0 f7, f2, f0
psq_l f4, 0x28(b), 0, 0
ps_madds1 f7, f3, f0, f7
psq_l f5, 0x38(b), 0, 0
ps_madds0 f7, f4, f1, f7
psq_l f0, 0x10(a), 0, 0
ps_madds1 f7, f5, f1, f7
psq_l f1, 0x18(a), 0, 0
ps_muls0 f9, f2, f0
psq_st f6, 0x0(ab), 0, 0
ps_madds1 f9, f3, f0, f9
psq_l f0, 0x20(a), 0, 0
ps_madds0 f9, f4, f1, f9
psq_st f8, 0x10(ab), 0, 0
ps_madds1 f9, f5, f1, f9
psq_l f1, 0x28(a), 0, 0
ps_muls0 f11, f2, f0
psq_st f10, 0x20(ab), 0, 0
ps_madds1 f11, f3, f0, f11
psq_l f0, 0x30(a), 0, 0
ps_madds0 f11, f4, f1, f11
psq_st f12, 0x30(ab), 0, 0
ps_madds1 f11, f5, f1, f11
psq_l f1, 0x38(a), 0, 0
ps_muls0 f13, f2, f0
psq_st f7, 0x8(ab), 0, 0
ps_madds1 f13, f3, f0, f13
psq_st f9, 0x18(ab), 0, 0
ps_madds0 f13, f4, f1, f13
psq_st f11, 0x28(ab), 0, 0
ps_madds1 f13, f5, f1, f13
psq_st f13, 0x38(ab), 0, 0
blr
}
void C_MTX44Transpose(const Mtx44 src, Mtx44 xPose) {
Mtx44 mTmp;
Mtx44Ptr m;
ASSERTMSGLINE(637, src, "MTX44Transpose(): NULL Mtx44Ptr 'src' ");
ASSERTMSGLINE(638, xPose, "MTX44Transpose(): NULL Mtx44Ptr 'xPose' ");
if (src == xPose) {
m = mTmp;
} else {
m = xPose;
}
m[0][0] = src[0][0];
m[0][1] = src[1][0];
m[0][2] = src[2][0];
m[0][3] = src[3][0];
m[1][0] = src[0][1];
m[1][1] = src[1][1];
m[1][2] = src[2][1];
m[1][3] = src[3][1];
m[2][0] = src[0][2];
m[2][1] = src[1][2];
m[2][2] = src[2][2];
m[2][3] = src[3][2];
m[3][0] = src[0][3];
m[3][1] = src[1][3];
m[3][2] = src[2][3];
m[3][3] = src[3][3];
if (m == mTmp) {
MTX44Copy(mTmp, xPose);
}
}
asm void PSMTX44Transpose(const __REGISTER Mtx44 src, __REGISTER Mtx44 xPose) {
nofralloc
psq_l f0, 0x0(src), 0, 0
psq_l f1, 0x10(src), 0, 0
ps_merge00 f4, f0, f1
psq_l f2, 0x8(src), 0, 0
psq_st f4, 0x0(xPose), 0, 0
ps_merge11 f5, f0, f1
psq_l f3, 0x18(src), 0, 0
psq_st f5, 0x10(xPose), 0, 0
ps_merge00 f4, f2, f3
psq_l f0, 0x20(src), 0, 0
psq_st f4, 0x20(xPose), 0, 0
ps_merge11 f5, f2, f3
psq_l f1, 0x30(src), 0, 0
psq_st f5, 0x30(xPose), 0, 0
ps_merge00 f4, f0, f1
psq_l f2, 0x28(src), 0, 0
psq_st f4, 0x8(xPose), 0, 0
ps_merge11 f5, f0, f1
psq_l f3, 0x38(src), 0, 0
psq_st f5, 0x18(xPose), 0, 0
ps_merge00 f4, f2, f3
psq_st f4, 0x28(xPose), 0, 0
ps_merge11 f5, f2, f3
psq_st f5, 0x38(xPose), 0, 0
blr
}
#define SWAP(a, b) \
{ \
f32 tmp; \
tmp = a; \
a = b; \
b = tmp; \
}
u32 C_MTX44Inverse(const Mtx44 src, Mtx44 inv) {
Mtx44 gjm;
s32 i;
s32 j;
s32 k;
f32 w;
f32 max;
s32 swp;
f32 ftmp;
ASSERTMSGLINE(734, src, "MTX44Inverse(): NULL Mtx44Ptr 'src' ");
ASSERTMSGLINE(735, inv, "MTX44Inverse(): NULL Mtx44Ptr 'inv' ");
MTX44Copy(src, gjm);
MTX44Identity(inv);
for (i = 0; i < 4; i++) {
max = 0.0f;
swp = i;
for (k = i; k < 4; k++) {
ftmp = fabsf(gjm[k][i]);
if (ftmp > max) {
max = ftmp;
swp = k;
}
}
if (max == 0.0f) {
return 0;
}
if (swp != i) {
for (k = 0; k < 4; k++) {
SWAP(gjm[i][k], gjm[swp][k]);
SWAP(inv[i][k], inv[swp][k]);
}
}
w = 1.0f / gjm[i][i];
for (j = 0; j < 4; j++) {
gjm[i][j] *= w;
inv[i][j] *= w;
}
for (k = 0; k < 4; k++) {
if (k != i) {
w = gjm[k][i];
for (j = 0; j < 4; j++) {
gjm[k][j] -= gjm[i][j] * w;
inv[k][j] -= inv[i][j] * w;
}
}
}
}
return 1;
}
void C_MTX44Trans(Mtx44 m, f32 xT, f32 yT, f32 zT) {
ASSERTMSGLINE(835, m, "MTX44Trans(): NULL Mtx44Ptr 'm' ");
m[0][0] = 1.0f;
m[0][1] = 0.0f;
m[0][2] = 0.0f;
m[0][3] = xT;
m[1][0] = 0.0f;
m[1][1] = 1.0f;
m[1][2] = 0.0f;
m[1][3] = yT;
m[2][0] = 0.0f;
m[2][1] = 0.0f;
m[2][2] = 1.0f;
m[2][3] = zT;
m[3][0] = 0.0f;
m[3][1] = 0.0f;
m[3][2] = 0.0f;
m[3][3] = 1.0f;
}
void PSMTX44Trans(__REGISTER Mtx44 m, __REGISTER f32 xT, __REGISTER f32 yT, __REGISTER f32 zT) {
__REGISTER f32 c_zero = 0.0f;
__REGISTER f32 c_one = 1.0f;
__REGISTER f32 c_01;
asm {
stfs xT, 0xc(m)
stfs yT, 0x1c(m)
ps_merge00 c_01, c_zero, c_one
stfs zT, 0x2c(m)
psq_st c_one, 0x0(m), 1, 0
psq_st c_zero, 0x4(m), 0, 0
psq_st c_01, 0x10(m), 0, 0
psq_st c_zero, 0x18(m), 1, 0
psq_st c_zero, 0x20(m), 0, 0
psq_st c_one, 0x28(m), 1, 0
psq_st c_zero, 0x30(m), 0, 0
psq_st c_01, 0x38(m), 0, 0
}
}
void C_MTX44TransApply(const Mtx44 src, Mtx44 dst, f32 xT, f32 yT, f32 zT) {
ASSERTMSGLINE(899, src, "MTX44TransApply(): NULL Mtx44Ptr 'src' ");
ASSERTMSGLINE(900, dst, "MTX44TransApply(): NULL Mtx44Ptr 'src' "); //! wrong assert string
if (src != dst) {
dst[0][0] = src[0][0];
dst[0][1] = src[0][1];
dst[0][2] = src[0][2];
dst[1][0] = src[1][0];
dst[1][1] = src[1][1];
dst[1][2] = src[1][2];
dst[2][0] = src[2][0];
dst[2][1] = src[2][1];
dst[2][2] = src[2][2];
dst[3][0] = src[3][0];
dst[3][1] = src[3][1];
dst[3][2] = src[3][2];
dst[3][3] = src[3][3];
}
dst[0][3] = (src[0][3] + xT);
dst[1][3] = (src[1][3] + yT);
dst[2][3] = (src[2][3] + zT);
}
asm void PSMTX44TransApply(const __REGISTER Mtx44 src, __REGISTER Mtx44 dst, __REGISTER f32 xT, __REGISTER f32 yT, __REGISTER f32 zT) {
nofralloc
psq_l f4, 0x0(src), 0, 0
frsp xT, xT
psq_l f5, 0x8(src), 0, 0
frsp yT, yT
psq_l f6, 0x10(src), 0, 0
frsp zT, zT
psq_l f7, 0x18(src), 0, 0
psq_st f4, 0x0(dst), 0, 0
ps_sum1 f5, xT, f5, f5
psq_l f4, 0x28(src), 0, 0
psq_st f6, 0x10(dst), 0, 0
ps_sum1 f7, yT, f7, f7
psq_l f8, 0x20(src), 0, 0
psq_st f5, 0x8(dst), 0, 0
ps_sum1 f4, zT, f4, f4
psq_st f7, 0x18(dst), 0, 0
psq_st f8, 0x20(dst), 0, 0
psq_l f5, 0x30(src), 0, 0
psq_l f6, 0x38(src), 0, 0
psq_st f4, 0x28(dst), 0, 0
psq_st f5, 0x30(dst), 0, 0
psq_st f6, 0x38(dst), 0, 0
blr
}
void C_MTX44Scale(Mtx44 m, f32 xS, f32 yS, f32 zS) {
ASSERTMSGLINE(976, m, "MTX44Scale(): NULL Mtx44Ptr 'm' ");
m[0][0] = xS;
m[0][1] = 0.0f;
m[0][2] = 0.0f;
m[0][3] = 0.0f;
m[1][0] = 0.0f;
m[1][1] = yS;
m[1][2] = 0.0f;
m[1][3] = 0.0f;
m[2][0] = 0.0f;
m[2][1] = 0.0f;
m[2][2] = zS;
m[2][3] = 0.0f;
m[3][0] = 0.0f;
m[3][1] = 0.0f;
m[3][2] = 0.0f;
m[3][3] = 1.0f;
}
void PSMTX44Scale(__REGISTER Mtx44 m, __REGISTER f32 xS, __REGISTER f32 yS, __REGISTER f32 zS) {
__REGISTER f32 c_zero = 0.0f;
__REGISTER f32 c_one = 1.0f;
asm {
stfs xS, 0x0(m)
psq_st c_zero, 0x4(m), 0, 0
psq_st c_zero, 0xc(m), 0, 0
stfs yS, 0x14(m)
psq_st c_zero, 0x18(m), 0, 0
psq_st c_zero, 0x20(m), 0, 0
stfs zS, 0x28(m)
psq_st c_zero, 0x2c(m), 0, 0
psq_st c_zero, 0x34(m), 0, 0
stfs c_one, 0x3c(m)
}
}
void C_MTX44ScaleApply(const Mtx44 src, Mtx44 dst, f32 xS, f32 yS, f32 zS) {
ASSERTMSGLINE(1036, src, "MTX44ScaleApply(): NULL Mtx44Ptr 'src' ");
ASSERTMSGLINE(1037, dst, "MTX44ScaleApply(): NULL Mtx44Ptr 'dst' ");
dst[0][0] = (src[0][0] * xS);
dst[0][1] = (src[0][1] * xS);
dst[0][2] = (src[0][2] * xS);
dst[0][3] = (src[0][3] * xS);
dst[1][0] = (src[1][0] * yS);
dst[1][1] = (src[1][1] * yS);
dst[1][2] = (src[1][2] * yS);
dst[1][3] = (src[1][3] * yS);
dst[2][0] = (src[2][0] * zS);
dst[2][1] = (src[2][1] * zS);
dst[2][2] = (src[2][2] * zS);
dst[2][3] = (src[2][3] * zS);
dst[3][0] = src[3][0];
dst[3][1] = src[3][1];
dst[3][2] = src[3][2];
dst[3][3] = src[3][3];
}
asm void PSMTX44ScaleApply(const __REGISTER Mtx44 src, __REGISTER Mtx44 dst, __REGISTER f32 xS, __REGISTER f32 yS, __REGISTER f32 zS) {
nofralloc
psq_l f4, 0x0(src), 0, 0
frsp xS, xS
psq_l f5, 0x8(src), 0, 0
frsp yS, yS
psq_l f6, 0x10(src), 0, 0
ps_muls0 f4, f4, xS
psq_l f7, 0x18(src), 0, 0
ps_muls0 f5, f5, xS
psq_l f8, 0x20(src), 0, 0
frsp zS, zS
psq_st f4, 0x0(dst), 0, 0
ps_muls0 f6, f6, yS
psq_l f9, 0x28(src), 0, 0
psq_st f5, 0x8(dst), 0, 0
ps_muls0 f7, f7, yS
psq_l f10, 0x30(src), 0, 0
psq_st f6, 0x10(dst), 0, 0
ps_muls0 f8, f8, zS
psq_l f11, 0x38(src), 0, 0
psq_st f7, 0x18(dst), 0, 0
ps_muls0 f9, f9, zS
psq_st f8, 0x20(dst), 0, 0
psq_st f9, 0x28(dst), 0, 0
psq_st f10, 0x30(dst), 0, 0
psq_st f11, 0x38(dst), 0, 0
blr
}
void C_MTX44RotRad(Mtx44 m, char axis, f32 rad) {
f32 sinA;
f32 cosA;
ASSERTMSGLINE(1118, m, "MTX44RotRad(): NULL Mtx44Ptr 'm' ");
sinA = sinf(rad);
cosA = cosf(rad);
C_MTX44RotTrig(m, axis, sinA, cosA);
}
void PSMTX44RotRad(Mtx44 m, char axis, f32 rad) {
f32 sinA;
f32 cosA;
sinA = sinf(rad);
cosA = cosf(rad);
PSMTX44RotTrig(m, axis, sinA, cosA);
}
void C_MTX44RotTrig(Mtx44 m, char axis, f32 sinA, f32 cosA) {
ASSERTMSGLINE(1163, m, "MTX44RotTrig(): NULL Mtx44Ptr 'm' ");
axis |= 0x20;
switch(axis) {
case 'x':
m[0][0] = 1.0f;
m[0][1] = 0.0f;
m[0][2] = 0.0f;
m[0][3] = 0.0f;
m[1][0] = 0.0f;
m[1][1] = cosA;
m[1][2] = -sinA;
m[1][3] = 0.0f;
m[2][0] = 0.0f;
m[2][1] = sinA;
m[2][2] = cosA;
m[2][3] = 0.0f;
m[3][0] = 0.0f;
m[3][1] = 0.0f;
m[3][2] = 0.0f;
m[3][3] = 1.0f;
break;
case 'y':
m[0][0] = cosA;
m[0][1] = 0.0f;
m[0][2] = sinA;
m[0][3] = 0.0f;
m[1][0] = 0.0f;
m[1][1] = 1.0f;
m[1][2] = 0.0f;
m[1][3] = 0.0f;
m[2][0] = -sinA;
m[2][1] = 0.0f;
m[2][2] = cosA;
m[2][3] = 0.0f;
m[3][0] = 0.0f;
m[3][1] = 0.0f;
m[3][2] = 0.0f;
m[3][3] = 1.0f;
break;
case 'z':
m[0][0] = cosA;
m[0][1] = -sinA;
m[0][2] = 0.0f;
m[0][3] = 0.0f;
m[1][0] = sinA;
m[1][1] = cosA;
m[1][2] = 0.0f;
m[1][3] = 0.0f;
m[2][0] = 0.0f;
m[2][1] = 0.0f;
m[2][2] = 1.0f;
m[2][3] = 0.0f;
m[3][0] = 0.0f;
m[3][1] = 0.0f;
m[3][2] = 0.0f;
m[3][3] = 1.0f;
break;
default:
ASSERTMSGLINE(1191, FALSE, "MTX44RotTrig(): invalid 'axis' value ");
break;
}
}
void PSMTX44RotTrig(__REGISTER Mtx44 m, __REGISTER char axis, __REGISTER f32 sinA, __REGISTER f32 cosA) {
__REGISTER f32 ftmp0;
__REGISTER f32 ftmp1;
__REGISTER f32 ftmp2;
__REGISTER f32 ftmp3;
__REGISTER f32 ftmp4;
__REGISTER f32 c_zero = 0.0f;
__REGISTER f32 c_one = 1.0f;
asm {
frsp sinA, sinA
ori axis, axis, 0x20
frsp cosA, cosA
cmplwi axis, 'x'
beq L_00001AB4
cmplwi axis, 'y'
beq L_00001AE8
cmplwi axis, 'z'
beq L_00001B20
b L_00001B54
L_00001AB4:
psq_st c_one, 0x0(m), 1, 0
psq_st c_zero, 0x4(m), 0, 0
ps_neg ftmp0, sinA
psq_st c_zero, 0xc(m), 0, 0
ps_merge00 ftmp1, sinA, cosA
psq_st c_zero, 0x1c(m), 0, 0
ps_merge00 ftmp0, cosA, ftmp0
psq_st c_zero, 0x2c(m), 0, 0
psq_st c_zero, 0x34(m), 0, 0
psq_st ftmp1, 0x24(m), 0, 0
psq_st ftmp0, 0x14(m), 0, 0
psq_st c_one, 0x3c(m), 1, 0
b L_00001B54
L_00001AE8:
ps_merge00 ftmp1, cosA, c_zero
psq_st c_zero, 0x30(m), 0, 0
ps_neg ftmp0, sinA
psq_st c_zero, 0x18(m), 0, 0
ps_merge00 ftmp3, c_zero, c_one
psq_st ftmp1, 0x0(m), 0, 0
ps_merge00 ftmp4, ftmp0, c_zero
ps_merge00 ftmp2, sinA, c_zero
psq_st ftmp3, 0x10(m), 0, 0
psq_st ftmp2, 0x8(m), 0, 0
psq_st ftmp4, 0x20(m), 0, 0
psq_st ftmp1, 0x28(m), 0, 0
psq_st ftmp3, 0x38(m), 0, 0
b L_00001B54
L_00001B20:
psq_st c_zero, 0x8(m), 0, 0
ps_neg ftmp0, sinA
psq_st c_zero, 0x18(m), 0, 0
ps_merge00 ftmp1, sinA, cosA
psq_st c_zero, 0x20(m), 0, 0
ps_merge00 ftmp2, c_one, c_zero
psq_st c_zero, 0x30(m), 0, 0
ps_merge00 ftmp3, c_zero, c_one
psq_st ftmp1, 0x10(m), 0, 0
ps_merge00 ftmp4, cosA, ftmp0
psq_st ftmp2, 0x28(m), 0, 0
psq_st ftmp3, 0x38(m), 0, 0
psq_st ftmp4, 0x0(m), 0, 0
L_00001B54:
}
}
void C_MTX44RotAxisRad(Mtx44 m, const Vec* axis, f32 rad) {
Vec vN;
f32 s;
f32 c;
f32 t;
f32 x;
f32 y;
f32 z;
f32 xSq;
f32 ySq;
f32 zSq;
ASSERTMSGLINE(1300, m, "MTX44RotAxisRad(): NULL Mtx44Ptr 'm' ");
ASSERTMSGLINE(1301, axis, "MTX44RotAxisRad(): NULL VecPtr 'axis' ");
s = sinf(rad);
c = cosf(rad);
t = 1 - c;
C_VECNormalize(axis, &vN);
x = vN.x;
y = vN.y;
z = vN.z;
xSq = (x * x);
ySq = (y * y);
zSq = (z * z);
m[0][0] = (c + (t * xSq));
m[0][1] = (y * (t * x)) - (s * z);
m[0][2] = (z * (t * x)) + (s * y);
m[0][3] = 0.0f;
m[1][0] = ((y * (t * x)) + (s * z));
m[1][1] = (c + (t * ySq));
m[1][2] = ((z * (t * y)) - (s * x));
m[1][3] = 0.0f;
m[2][0] = ((z * (t * x)) - (s * y));
m[2][1] = ((z * (t * y)) + (s * x));
m[2][2] = (c + (t * zSq));
m[2][3] = 0.0f;
m[3][0] = 0.0f;
m[3][1] = 0.0f;
m[3][2] = 0.0f;
m[3][3] = 1.0f;
}
static void __PSMTX44RotAxisRadInternal(__REGISTER Mtx44 m, const __REGISTER Vec* axis, __REGISTER f32 sT, __REGISTER f32 cT) {
__REGISTER f32 tT;
__REGISTER f32 fc0;
__REGISTER f32 tmp0;
__REGISTER f32 tmp1;
__REGISTER f32 tmp2;
__REGISTER f32 tmp3;
__REGISTER f32 tmp4;
__REGISTER f32 tmp5;
__REGISTER f32 tmp6;
__REGISTER f32 tmp7;
__REGISTER f32 tmp8;
__REGISTER f32 tmp9;
tmp9 = 0.5f;
tmp8 = 3.0f;
asm {
frsp cT, cT
psq_l tmp0, 0x0(axis), 0, 0
frsp sT, sT
lfs tmp1, 0x8(axis)
ps_mul tmp2, tmp0, tmp0
fadds tmp7, tmp9, tmp9
ps_madd tmp3, tmp1, tmp1, tmp2
fsubs fc0, tmp9, tmp9
ps_sum0 tmp4, tmp3, tmp1, tmp2
fsubs tT, tmp7, cT
frsqrte tmp5, tmp4
ps_merge00 tmp7, fc0, tmp7
fmuls tmp2, tmp5, tmp5
fmuls tmp3, tmp5, tmp9
psq_st fc0, 0x30(m), 0, 0
fnmsubs tmp2, tmp2, tmp4, tmp8
fmuls tmp5, tmp2, tmp3
psq_st tmp7, 0x38(m), 0, 0
ps_merge00 cT, cT, cT
ps_muls0 tmp0, tmp0, tmp5
ps_muls0 tmp1, tmp1, tmp5
ps_muls0 tmp4, tmp0, tT
ps_muls0 tmp9, tmp0, sT
ps_muls0 tmp5, tmp1, tT
ps_muls1 tmp3, tmp4, tmp0
ps_muls0 tmp2, tmp4, tmp0
ps_muls0 tmp4, tmp4, tmp1
fnmsubs tmp6, tmp1, sT, tmp3
fmadds tmp7, tmp1, sT, tmp3
ps_neg tmp0, tmp9
ps_sum0 tmp8, tmp4, fc0, tmp9
ps_sum0 tmp2, tmp2, tmp6, cT
ps_sum1 tmp3, cT, tmp7, tmp3
ps_sum0 tmp6, tmp0, fc0, tmp4
psq_st tmp8, 0x8(m), 0, 0
ps_sum0 tmp0, tmp4, tmp4, tmp0
psq_st tmp2, 0x0(m), 0, 0
ps_muls0 tmp5, tmp5, tmp1
psq_st tmp3, 0x10(m), 0, 0
ps_sum1 tmp4, tmp9, tmp0, tmp4
psq_st tmp6, 0x18(m), 0, 0
ps_sum0 tmp5, tmp5, fc0, cT
psq_st tmp4, 0x20(m), 0, 0
psq_st tmp5, 0x28(m), 0, 0
}
}
void PSMTX44RotAxisRad(Mtx44 m, const Vec* axis, f32 rad) {
f32 sinT, cosT;
sinT = sinf(rad);
cosT = cosf(rad);
__PSMTX44RotAxisRadInternal(m, axis, sinT, cosT);
}
Reference in New Issue View Git Blame Copy Permalink