mirror of
https://github.com/ACreTeam/ac-decomp
synced 2026-05-23 14:41:38 -04:00
Cuyler36
1072 lines
28 KiB
C
1072 lines
28 KiB
C
#include "sys_matrix.h"
|
|
#include "TwoHeadArena.h"
|
|
#include "m_skin_matrix.h"
|
|
#include "graph.h"
|
|
#include "libc/math.h"
|
|
#include "MSL_C/w_math.h"
|
|
#include "libforest/gbi_extensions.h"
|
|
|
|
Mtx Mtx_clear = gdSPDefMtx(
|
|
1.0f, 0.0f, 0.0f, 0.0f,
|
|
0.0f, 1.0f, 0.0f, 0.0f,
|
|
0.0f, 0.0f, 1.0f, 0.0f,
|
|
0.0f, 0.0f, 0.0f, 1.0f
|
|
);
|
|
|
|
MtxF MtxF_clear = { {
|
|
{1.0f, 0.0f, 0.0f, 0.0f},
|
|
{0.0f, 1.0f, 0.0f, 0.0f},
|
|
{0.0f, 0.0f, 1.0f, 0.0f},
|
|
{0.0f, 0.0f, 0.0f, 1.0f},
|
|
} };
|
|
|
|
static MtxF* Matrix_now;
|
|
static MtxF* Matrix_stack;
|
|
|
|
BSS_ORDER_GROUP_START
|
|
BSS_ORDER_ITEM(Matrix_stack)
|
|
BSS_ORDER_ITEM(Matrix_now)
|
|
BSS_ORDER_GROUP_END
|
|
|
|
void new_Matrix(GAME* game){
|
|
Matrix_now = THA_alloc16(&game->tha, 0x500);
|
|
Matrix_stack = Matrix_now;
|
|
}
|
|
void Matrix_push(){
|
|
Matrix_copy_MtxF(Matrix_now + 1, Matrix_now);
|
|
Matrix_now++;
|
|
}
|
|
|
|
void Matrix_pull(){
|
|
Matrix_now--;
|
|
}
|
|
|
|
void Matrix_get(MtxF* dest){
|
|
Matrix_copy_MtxF(dest, Matrix_now);
|
|
}
|
|
|
|
void Matrix_put(MtxF* src){
|
|
Matrix_copy_MtxF(Matrix_now, src);
|
|
}
|
|
|
|
MtxF* get_Matrix_now(void){
|
|
return (Matrix_now);
|
|
}
|
|
|
|
void Matrix_mult(MtxF* m, u8 mode){
|
|
MtxF* curm = get_Matrix_now();
|
|
|
|
if(mode == 1){
|
|
Skin_Matrix_MulMatrix(curm, m, curm);
|
|
}
|
|
else{
|
|
Matrix_copy_MtxF(Matrix_now, m);
|
|
}
|
|
}
|
|
|
|
void Matrix_translate(f32 x, f32 y, f32 z, u8 mode){
|
|
MtxF* curm = Matrix_now;
|
|
f32 tx, ty;
|
|
|
|
if(mode == 1){
|
|
tx = curm->xx;
|
|
ty = curm->xy;
|
|
curm->xw += tx * x + ty * y + curm->xz * z;
|
|
tx = curm->yx;
|
|
ty = curm->yy;
|
|
curm->yw += tx * x + ty * y + curm->yz * z;
|
|
tx = curm->zx;
|
|
ty = curm->zy;
|
|
curm->zw += tx * x + ty * y + curm->zz * z;
|
|
tx = curm->wx;
|
|
ty = curm->wy;
|
|
curm->ww += tx * x + ty * y + curm->wz * z;
|
|
}
|
|
else{
|
|
Skin_Matrix_SetTranslate(curm, x, y , z);
|
|
}
|
|
}
|
|
|
|
void Matrix_scale(f32 x, f32 y, f32 z, u8 mode){
|
|
MtxF* curm = Matrix_now;
|
|
|
|
if(mode == 1){
|
|
curm->xx *= x;
|
|
curm->yx *= x;
|
|
curm->zx *= x;
|
|
curm->xy *= y;
|
|
curm->yy *= y;
|
|
curm->zy *= y;
|
|
curm->xz *= z;
|
|
curm->yz *= z;
|
|
curm->zz *= z;
|
|
curm->wx *= x;
|
|
curm->wy *= y;
|
|
curm->wz *= z;
|
|
}
|
|
else{
|
|
Skin_Matrix_SetScale(curm, x, y, z);
|
|
}
|
|
}
|
|
|
|
void Matrix_RotateX(s16 x, int mode){
|
|
MtxF* curm;
|
|
f32 sin;
|
|
f32 cos;
|
|
f32 fp, st2;
|
|
|
|
if(mode == 1){
|
|
if(x != 0){
|
|
curm = Matrix_now;
|
|
|
|
sin = sin_s(x);
|
|
cos = cos_s(x);
|
|
|
|
fp = curm->xy;
|
|
st2 = curm->xz;
|
|
curm->xy = (fp * cos)+ (st2 * sin);
|
|
curm->xz = -(fp * sin) + (st2 * cos);
|
|
|
|
fp = curm->yy;
|
|
st2 = curm->yz;
|
|
curm->yy = (fp * cos)+ (st2 * sin);
|
|
curm->yz = -(fp * sin) + (st2 * cos);
|
|
|
|
fp = curm->zy;
|
|
st2 = curm->zz;
|
|
curm->zy = (fp * cos)+ (st2 * sin);
|
|
curm->zz = -(fp * sin) + (st2 * cos);
|
|
|
|
fp = curm->wy;
|
|
st2 = curm->wz;
|
|
curm->wy = (fp * cos)+ (st2 * sin);
|
|
curm->wz = -(fp * sin) + (st2 * cos);
|
|
}
|
|
} else {
|
|
curm = Matrix_now;
|
|
|
|
if (x != 0) {
|
|
sin = sin_s(x);
|
|
cos = cos_s(x);
|
|
|
|
} else {
|
|
sin = 0.0f;
|
|
cos = 1.0f;
|
|
}
|
|
curm->yx = 0.0f;
|
|
curm->zx = 0.0f;
|
|
curm->wx = 0.0f;
|
|
curm->xy = 0.0f;
|
|
curm->wy = 0.0f;
|
|
curm->xz = 0.0f;
|
|
curm->wz = 0.0f;
|
|
curm->xw = 0.0f;
|
|
curm->yw = 0.0f;
|
|
curm->zw = 0.0f;
|
|
curm->xx = 1.0f;
|
|
curm->ww = 1.0f;
|
|
curm->yy = cos;
|
|
curm->zz = cos;
|
|
curm->zy = sin;
|
|
curm->yz = -sin;
|
|
}
|
|
}
|
|
|
|
void Matrix_RotateY(s16 y, int mode){
|
|
MtxF* curm;
|
|
f32 sin;
|
|
f32 cos;
|
|
f32 fp, st2;
|
|
|
|
if(mode == 1){
|
|
if (y != 0) {
|
|
curm = Matrix_now;
|
|
|
|
sin = sin_s(y);
|
|
cos = cos_s(y);
|
|
|
|
fp = curm->xx;
|
|
st2 = curm->xz;
|
|
curm->xx = fp * cos - st2 * sin;
|
|
curm->xz = fp * sin + st2 * cos;
|
|
|
|
fp = curm->yx;
|
|
st2 = curm->yz;
|
|
curm->yx = fp * cos - st2 * sin;
|
|
curm->yz = fp * sin + st2 * cos;
|
|
|
|
fp = curm->zx;
|
|
st2 = curm->zz;
|
|
curm->zx = fp * cos - st2 * sin;
|
|
curm->zz = fp * sin + st2 * cos;
|
|
|
|
fp = curm->wx;
|
|
st2 = curm->wz;
|
|
curm->wx = fp * cos - st2 * sin;
|
|
curm->wz = fp * sin + st2 * cos;
|
|
}
|
|
} else {
|
|
curm = Matrix_now;
|
|
|
|
if (y != 0) {
|
|
sin = sin_s(y);
|
|
cos = cos_s(y);
|
|
|
|
|
|
} else {
|
|
sin = 0.0f;
|
|
cos = 1.0f;
|
|
}
|
|
|
|
curm->yx = 0.0f;
|
|
curm->wx = 0.0f;
|
|
curm->xy = 0.0f;
|
|
curm->zy = 0.0f;
|
|
curm->wy = 0.0f;
|
|
curm->yz = 0.0f;
|
|
curm->wz = 0.0f;
|
|
curm->xw = 0.0f;
|
|
curm->yw = 0.0f;
|
|
curm->zw = 0.0f;
|
|
curm->yy = 1.0f;
|
|
curm->ww = 1.0f;
|
|
curm->xx = cos;
|
|
curm->zz = cos;
|
|
curm->zx = -sin;
|
|
curm->xz = sin;
|
|
}
|
|
}
|
|
|
|
void Matrix_RotateZ(s16 z, int mode) {
|
|
MtxF* curm;
|
|
f32 sin;
|
|
f32 cos;
|
|
f32 fp, st2;
|
|
|
|
if(mode == 1){
|
|
if (z != 0) {
|
|
curm = Matrix_now;
|
|
|
|
sin = sin_s(z);
|
|
cos = cos_s(z);
|
|
|
|
fp = curm->xx;
|
|
st2 = curm->xy;
|
|
curm->xx = (fp * cos)+ (st2 * sin);
|
|
curm->xy = -(fp * sin) + (st2 * cos);
|
|
|
|
fp = curm->yx;
|
|
st2 = curm->yy;
|
|
curm->yx = (fp * cos)+ (st2 * sin);
|
|
curm->yy = -(fp * sin) + (st2 * cos);
|
|
|
|
fp = curm->zx;
|
|
st2 = curm->zy;
|
|
curm->zx = (fp * cos)+ (st2 * sin);
|
|
curm->zy = -(fp * sin) + (st2 * cos);
|
|
|
|
fp = curm->wx;
|
|
st2 = curm->wy;
|
|
curm->wx = (fp * cos)+ (st2 * sin);
|
|
curm->wy = -(fp * sin) + (st2 * cos);
|
|
}
|
|
} else {
|
|
curm = Matrix_now;
|
|
|
|
if (z != 0) {
|
|
sin = sin_s(z);
|
|
cos = cos_s(z);
|
|
|
|
} else {
|
|
sin = 0.0f;
|
|
cos = 1.0f;
|
|
}
|
|
|
|
curm->zx = 0.0f;
|
|
curm->wx = 0.0f;
|
|
curm->zy = 0.0f;
|
|
curm->wy = 0.0f;
|
|
curm->xz = 0.0f;
|
|
curm->yz = 0.0f;
|
|
curm->wz = 0.0f;
|
|
curm->xw = 0.0f;
|
|
curm->yw = 0.0f;
|
|
curm->zw = 0.0f;
|
|
curm->zz = 1.0f;
|
|
curm->ww = 1.0f;
|
|
curm->xx = cos;
|
|
curm->yy = cos;
|
|
curm->yx = sin;
|
|
curm->xy = -sin;
|
|
}
|
|
}
|
|
|
|
void Matrix_rotateXYZ(s16 x, s16 y, s16 z, int mode){
|
|
MtxF* curm = Matrix_now;
|
|
f32 sin;
|
|
f32 cos;
|
|
f32 fp, st2;
|
|
|
|
if(mode == 1){
|
|
sin = sin_s(z);
|
|
cos = cos_s(z);
|
|
|
|
fp = curm->xx;
|
|
st2 = curm->xy;
|
|
curm->xx = (fp * cos)+ (st2 * sin);
|
|
curm->xy = -(fp * sin) + (st2 * cos);
|
|
|
|
fp = curm->yx;
|
|
st2 = curm->yy;
|
|
curm->yx = (fp * cos)+ (st2 * sin);
|
|
curm->yy = -(fp * sin) + (st2 * cos);
|
|
|
|
fp = curm->zx;
|
|
st2 = curm->zy;
|
|
curm->zx = (fp * cos)+ (st2 * sin);
|
|
curm->zy = -(fp * sin) + (st2 * cos);
|
|
|
|
fp = curm->wx;
|
|
st2 = curm->wy;
|
|
curm->wx = (fp * cos)+ (st2 * sin);
|
|
curm->wy = -(fp * sin) + (st2 * cos);
|
|
|
|
if (y != 0) {
|
|
sin = sin_s(y);
|
|
cos = cos_s(y);
|
|
|
|
fp = curm->xx;
|
|
st2 = curm->xz;
|
|
curm->xx = (fp * cos) - (st2 * sin);
|
|
curm->xz = (fp * sin) + (st2 * cos);
|
|
|
|
fp = curm->yx;
|
|
st2 = curm->yz;
|
|
curm->yx = (fp * cos)- (st2 * sin);
|
|
curm->yz = (fp * sin) + (st2 * cos);
|
|
|
|
fp = curm->zx;
|
|
st2 = curm->zz;
|
|
curm->zx = (fp * cos)- (st2 * sin);
|
|
curm->zz = (fp * sin) + (st2 * cos);
|
|
|
|
fp = curm->wx;
|
|
st2 = curm->wz;
|
|
curm->wx = (fp * cos)- (st2 * sin);
|
|
curm->wz = (fp * sin) + (st2 * cos);
|
|
}
|
|
|
|
if (x != 0) {
|
|
sin = sin_s(x);
|
|
cos = cos_s(x);
|
|
|
|
fp = curm->xy;
|
|
st2 = curm->xz;
|
|
curm->xy = (fp * cos)+ (st2 * sin);
|
|
curm->xz = -(fp * sin) + (st2 * cos);
|
|
|
|
fp = curm->yy;
|
|
st2 = curm->yz;
|
|
curm->yy = (fp * cos)+ (st2 * sin);
|
|
curm->yz = -(fp * sin) + (st2 * cos);
|
|
|
|
fp = curm->zy;
|
|
st2 = curm->zz;
|
|
curm->zy = (fp * cos)+ (st2 * sin);
|
|
curm->zz = -(fp * sin) + (st2 * cos);
|
|
|
|
fp = curm->wy;
|
|
st2 = curm->wz;
|
|
curm->wy = (fp * cos)+ (st2 * sin);
|
|
curm->wz = -(fp * sin) + (st2 * cos);
|
|
}
|
|
} else {
|
|
Skin_Matrix_SetRotateXyz_s(curm, x, y, z);
|
|
}
|
|
}
|
|
|
|
void Matrix_softcv3_mult(xyz_t* pos, s_xyz* rot) {
|
|
MtxF* curm = Matrix_now;
|
|
f32 sin, cos;
|
|
f32 fp, st2;
|
|
|
|
sin = sin_s(rot->z);
|
|
cos = cos_s(rot->z);
|
|
|
|
fp = curm->xx;
|
|
st2 = curm->xy;
|
|
|
|
curm->xw = curm->xw + (fp * pos->x + st2 * pos->y + curm->xz * pos->z);
|
|
curm->xx = (fp * cos) + (st2 * sin);
|
|
curm->xy = -(fp * sin) + (st2 * cos);
|
|
|
|
fp = curm->yx;
|
|
st2 = curm->yy;
|
|
|
|
curm->yw = curm->yw + (fp * pos->x + st2 * pos->y + curm->yz * pos->z);
|
|
curm->yx = (fp * cos) + (st2 * sin);
|
|
curm->yy = -(fp * sin) + (st2 * cos);
|
|
|
|
fp = curm->zx;
|
|
st2 = curm->zy;
|
|
|
|
curm->zw = curm->zw + (curm->zx * pos->x + curm->zy * pos->y + curm->zz * pos->z);
|
|
curm->zx = (fp * cos) + (st2 * sin);
|
|
curm->zy = -(fp * sin) + (st2 * cos);
|
|
|
|
fp = curm->wx;
|
|
st2 = curm->wy;
|
|
curm->ww = curm->ww + (curm->wx * pos->x + curm->wy * pos->y + curm->wz * pos->z);
|
|
curm->wx = (fp * cos) + (st2 * sin);
|
|
curm->wy = -(fp * sin) + (st2 * cos);
|
|
|
|
if(rot->y != 0){
|
|
|
|
sin = sin_s(rot->y);
|
|
cos = cos_s(rot->y);
|
|
|
|
fp = curm->xx;
|
|
st2 = curm->xz;
|
|
curm->xx = (fp * cos) - (st2 * sin);
|
|
curm->xz = (fp * sin) + (st2 * cos);
|
|
|
|
fp = curm->yx;
|
|
st2 = curm->yz;
|
|
curm->yx = (fp * cos) - (st2 * sin);
|
|
curm->yz = (fp * sin) + (st2 * cos);
|
|
|
|
fp = curm->zx;
|
|
st2 = curm->zz;
|
|
curm->zx = (fp * cos) - (st2 * sin);
|
|
curm->zz = (fp * sin) + (st2 * cos);
|
|
|
|
|
|
fp = curm->wx;
|
|
st2 = curm->wz;
|
|
curm->wx = (fp * cos) - (st2 * sin);
|
|
curm->wz = (fp * sin) + (st2 * cos);
|
|
}
|
|
|
|
|
|
if(rot->x != 0){
|
|
sin = sin_s(rot->x);
|
|
cos = cos_s(rot->x);
|
|
|
|
fp = curm->xy;
|
|
st2 = curm->xz;
|
|
curm->xy = (fp * cos) + (st2 * sin);
|
|
curm->xz = -(fp * sin) + (st2 * cos);
|
|
|
|
fp = curm->yy;
|
|
st2 = curm->yz;
|
|
curm->yy = (fp * cos) + (st2 * sin);
|
|
curm->yz = -(fp * sin) + (st2 * cos);
|
|
|
|
fp = curm->zy;
|
|
st2 = curm->zz;
|
|
curm->zy = (fp * cos) + (st2 * sin);
|
|
curm->zz = -(fp * sin) + (st2 * cos);
|
|
|
|
fp = curm->wy;
|
|
st2 = curm->wz;
|
|
curm->wy = (fp * cos) + (st2 * sin);
|
|
curm->wz = -(fp * sin) + (st2 * cos);
|
|
}
|
|
}
|
|
|
|
void Matrix_softcv3_load(s_xyz* src, f32 x, f32 y, f32 z){
|
|
MtxF* curm = Matrix_now;
|
|
f32 sin, cos;
|
|
f32 ss, sc;
|
|
|
|
|
|
sin = sin_s(src->y);
|
|
cos = cos_s(src->y);
|
|
|
|
curm->xx = cos;
|
|
curm->zx = -sin;
|
|
curm->xw = x;
|
|
curm->yw = y;
|
|
curm->zw = z;
|
|
curm->wz = 0.0f;
|
|
curm->wy = 0.0f;
|
|
curm->wx = 0.0f;
|
|
curm->ww = 1.0f;
|
|
|
|
if(src->x != 0){
|
|
ss = sin_s(src->x);
|
|
sc = cos_s(src->x);
|
|
|
|
|
|
curm->zz = cos * sc;
|
|
curm->zy = cos * ss;
|
|
|
|
curm->xz = sin * sc;
|
|
curm->xy = sin * ss;
|
|
|
|
curm->yz = -ss;
|
|
curm->yy = sc;
|
|
}
|
|
else{
|
|
curm->zz = cos;
|
|
curm->xz = sin;
|
|
curm->yz = 0.0f;
|
|
curm->zy = 0.0f;
|
|
curm->xy = 0.0f;
|
|
curm->yy = 1.0f;
|
|
}
|
|
if(src->z != 0){
|
|
sin = sin_s(src->z);
|
|
cos = cos_s(src->z);
|
|
|
|
ss = curm->xy;
|
|
sc = curm->xx;
|
|
|
|
curm->xx = (sc * cos) + (ss * sin);
|
|
curm->xy = -(sc * sin) + (ss * cos);
|
|
|
|
ss = curm->zy;
|
|
sc = curm->zx;
|
|
|
|
curm->zx = (sc * cos) + (ss * sin);
|
|
curm->zy = -(sc * sin) + (ss * cos);
|
|
|
|
ss = curm->yy;
|
|
curm->yx = curm->yy * sin;
|
|
curm->yy = ss * cos;
|
|
}
|
|
else{
|
|
curm->yx = 0.0f;
|
|
}
|
|
}
|
|
|
|
Mtx* _MtxF_to_Mtx(MtxF* src, Mtx* dest) {
|
|
int fp;
|
|
u16* m1 = (u16*)&dest->m[0][0];
|
|
u16* m2 = (u16*)&dest->m[2][0];
|
|
|
|
fp = src->xx * 0x10000;
|
|
m1[0] = (fp >> 0x10);
|
|
m1[16 + 0] = fp & 0xFFFF;
|
|
|
|
fp = src->yx * 0x10000;
|
|
m1[1] = (fp >> 0x10);
|
|
m1[16 + 1] = fp & 0xFFFF;
|
|
|
|
fp = src->zx * 0x10000;
|
|
m1[2] = (fp >> 0x10);
|
|
m1[16 + 2] = fp & 0xFFFF;
|
|
|
|
fp = src->wx * 0x10000;
|
|
m1[3] = (fp >> 0x10);
|
|
m1[16 + 3] = fp & 0xFFFF;
|
|
|
|
fp = src->xy * 0x10000;
|
|
m1[4] = (fp >> 0x10);
|
|
m1[16 + 4] = fp & 0xFFFF;
|
|
|
|
fp = src->yy * 0x10000;
|
|
m1[5] = (fp >> 0x10);
|
|
m1[16 + 5] = fp & 0xFFFF;
|
|
|
|
fp = src->zy * 0x10000;
|
|
m1[6] = (fp >> 0x10);
|
|
m1[16 + 6] = fp & 0xFFFF;
|
|
|
|
fp = src->wy * 0x10000;
|
|
m1[7] = (fp >> 0x10);
|
|
m1[16 + 7] = fp & 0xFFFF;
|
|
|
|
fp = src->xz * 0x10000;
|
|
m1[8] = (fp >> 0x10);
|
|
m1[16 + 8] = fp & 0xFFFF;
|
|
|
|
fp = src->yz * 0x10000;
|
|
m1[9] = (fp >> 0x10);
|
|
m2[9] = fp & 0xFFFF;
|
|
|
|
fp = src->zz * 0x10000;
|
|
m1[10] = (fp >> 0x10);
|
|
m2[10] = fp & 0xFFFF;
|
|
|
|
fp = src->wz * 0x10000;
|
|
m1[11] = (fp >> 0x10);
|
|
m2[11] = fp & 0xFFFF;
|
|
|
|
fp = src->xw * 0x10000;
|
|
m1[12] = (fp >> 0x10);
|
|
m2[12] = fp & 0xFFFF;
|
|
|
|
fp = src->yw * 0x10000;
|
|
m1[13] = (fp >> 0x10);
|
|
m2[13] = fp & 0xFFFF;
|
|
|
|
fp = src->zw * 0x10000;
|
|
m1[14] = (fp >> 0x10);
|
|
m2[14] = fp & 0xFFFF;
|
|
|
|
fp = src->ww * 0x10000;
|
|
m1[15] = (fp >> 0x10);
|
|
m2[15] = fp & 0xFFFF;
|
|
return dest;
|
|
}
|
|
|
|
Mtx* _Matrix_to_Mtx(Mtx* dest) {
|
|
return _MtxF_to_Mtx(Matrix_now, dest);
|
|
}
|
|
|
|
Mtx* _Matrix_to_Mtx_new(GRAPH* graph){
|
|
return _Matrix_to_Mtx(GRAPH_ALLOC(graph, sizeof(Mtx)));
|
|
|
|
}
|
|
|
|
void Matrix_Position(xyz_t* old_pos, xyz_t* new_pos){
|
|
MtxF* curm = Matrix_now;
|
|
|
|
new_pos->x = (curm->xx * old_pos->x) + (curm->xy * old_pos->y) + (curm->xz * old_pos->z) + curm->xw;
|
|
new_pos->y = (curm->yx * old_pos->x) + (curm->yy * old_pos->y) + (curm->yz * old_pos->z) + curm->yw;
|
|
new_pos->z = (curm->zx * old_pos->x) + (curm->zy * old_pos->y) + (curm->zz * old_pos->z) + curm->zw;
|
|
}
|
|
|
|
void Matrix_Position_Zero(xyz_t* v){
|
|
MtxF* curm = Matrix_now;
|
|
|
|
v->x = curm->xw;
|
|
v->y = curm->yw;
|
|
v->z = curm->zw;
|
|
}
|
|
|
|
void Matrix_Position_VecX(xyz_t* v, f32 x){
|
|
MtxF* curm = Matrix_now;
|
|
|
|
v->x = curm->xw + (curm->xx * x);
|
|
v->y = curm->yw + (curm->yx * x);
|
|
v->z = curm->zw + (curm->zx * x);
|
|
|
|
}
|
|
void Matrix_Position_VecZ(xyz_t* v, f32 z){
|
|
MtxF* curm = Matrix_now;
|
|
|
|
v->x = curm->xw + (curm->xz * z);
|
|
v->y = curm->yw + (curm->yz * z);
|
|
v->z = curm->zw + (curm->zz * z);
|
|
}
|
|
|
|
|
|
void Matrix_copy_MtxF(MtxF* dest, MtxF* src) {
|
|
dest->xx = src->xx;
|
|
dest->yx = src->yx;
|
|
dest->zx = src->zx;
|
|
dest->wx = src->wx;
|
|
dest->xy = src->xy;
|
|
dest->yy = src->yy;
|
|
dest->zy = src->zy;
|
|
dest->wy = src->wy;
|
|
dest->xz = src->xz;
|
|
dest->yz = src->yz;
|
|
dest->zz = src->zz;
|
|
dest->wz = src->wz;
|
|
dest->xw = src->xw;
|
|
dest->yw = src->yw;
|
|
dest->zw = src->zw;
|
|
dest->ww = src->ww;
|
|
}
|
|
|
|
void Matrix_MtxtoMtxF(Mtx* src, MtxF* dest) {
|
|
u16* m1 = (u16*)&src->m[0][0];
|
|
u16* m2 = (u16*)&src->m[2][0];
|
|
|
|
dest->xx = ((m1[0] << 0x10) | m2[0]) * (1 / (f64)0x10000);
|
|
dest->yx = ((m1[1] << 0x10) | m2[1]) * (1 / (f64)0x10000);
|
|
dest->zx = ((m1[2] << 0x10) | m2[2]) * (1 / (f64)0x10000);
|
|
dest->wx = ((m1[3] << 0x10) | m2[3]) * (1 / (f64)0x10000);
|
|
dest->xy = ((m1[4] << 0x10) | m2[4]) * (1 / (f64)0x10000);
|
|
dest->yy = ((m1[5] << 0x10) | m2[5]) * (1 / (f64)0x10000);
|
|
dest->zy = ((m1[6] << 0x10) | m2[6]) * (1 / (f64)0x10000);
|
|
dest->wy = ((m1[7] << 0x10) | m2[7]) * (1 / (f64)0x10000);
|
|
dest->xz = ((m1[8] << 0x10) | m2[8]) * (1 / (f64)0x10000);
|
|
dest->yz = ((m1[9] << 0x10) | m2[9]) * (1 / (f64)0x10000);
|
|
dest->zz = ((m1[10] << 0x10) | m2[10]) * (1 / (f64)0x10000);
|
|
dest->wz = ((m1[11] << 0x10) | m2[11]) * (1 / (f64)0x10000);
|
|
dest->xw = ((m1[12] << 0x10) | m2[12]) * (1 / (f64)0x10000);
|
|
dest->yw = ((m1[13] << 0x10) | m2[13]) * (1 / (f64)0x10000);
|
|
dest->zw = ((m1[14] << 0x10) | m2[14]) * (1 / (f64)0x10000);
|
|
dest->ww = ((m1[15] << 0x10) | m2[15]) * (1 / (f64)0x10000);
|
|
}
|
|
|
|
void Matrix_reverse(MtxF* curm) {
|
|
f32 fp;
|
|
|
|
fp = curm->yx;
|
|
curm->yx = curm->xy;
|
|
curm->xy = fp;
|
|
|
|
fp = curm->zx;
|
|
curm->zx = curm->xz;
|
|
curm->xz = fp;
|
|
|
|
fp = curm->zy;
|
|
curm->zy = curm->yz;
|
|
curm->yz = fp;
|
|
}
|
|
|
|
void Matrix_to_rotate_new(MtxF* curm, s_xyz* vec, int flag){
|
|
f32 temp;
|
|
f32 temp2;
|
|
f32 temp3;
|
|
f32 temp4;
|
|
|
|
temp = curm->xz;
|
|
temp *= temp;
|
|
temp += (curm->zz * curm->zz);
|
|
vec->x = (fatan2(-curm->yz, sqrtf(temp)) * 10430.3779297f);
|
|
|
|
if ((vec->x == 0x4000) || (vec->x == -0x4000)) {
|
|
vec->z = 0;
|
|
|
|
vec->y = (fatan2(-curm->zx, curm->xx) * 10430.3779297f);
|
|
} else {
|
|
vec->y = (fatan2(curm->xz, curm->zz) * 10430.3779297f);
|
|
|
|
if (!flag) {
|
|
vec->z = (fatan2(curm->yx, curm->yy) * 10430.3779297f);
|
|
} else {
|
|
temp = curm->xx;
|
|
|
|
temp2 = curm->yx;
|
|
temp *= temp;
|
|
temp += curm->zx * curm->zx;
|
|
temp += (temp2 * temp2);
|
|
/* temp = xx^2+zx^2+yx^2 == 1 for a rotation matrix */
|
|
temp = temp2 / sqrtf(temp);
|
|
|
|
temp2 = curm->xy;
|
|
temp3 = curm->yy;
|
|
temp2 *= temp2;
|
|
temp2 += curm->zy * curm->zy;
|
|
temp2 += (temp3 * temp3);
|
|
/* temp2 = xy^2+zy^2+yy^2 == 1 for a rotation matrix */
|
|
temp2 = temp3 / sqrtf(temp2);
|
|
|
|
/* for a rotation matrix, temp == yx and temp2 == yy
|
|
* which is the same as in the !flag branch */
|
|
vec->z = (fatan2(temp, temp2) * 10430.3779297f);
|
|
}
|
|
}
|
|
}
|
|
|
|
void Matrix_to_rotate2_new(MtxF* curm, s_xyz* v, int flag) {
|
|
f32 temp;
|
|
f32 temp2;
|
|
f32 temp3;
|
|
f32 temp4;
|
|
|
|
temp = curm->xx;
|
|
temp *= temp;
|
|
temp += (curm->yx * curm->yx);
|
|
v->y = (fatan2(-curm->zx, sqrtf(temp))) * 10430.3779297f;
|
|
|
|
if ((v->y == 0x4000) || (v->y == -0x4000)) {
|
|
v->x = 0;
|
|
v->z = (fatan2(-curm->xy, curm->yy) * 10430.3779297f);
|
|
} else {
|
|
v->z = (fatan2(curm->yx, curm->xx) * 10430.3779297f);
|
|
|
|
if (!flag) {
|
|
v->x = (fatan2(curm->zy, curm->zz) * 10430.3779297f);
|
|
} else {
|
|
temp = curm->xy;
|
|
|
|
temp2 = curm->zy;
|
|
temp *= temp;
|
|
temp += curm->yy * curm->yy;
|
|
temp += (temp2 * temp2);
|
|
/* temp = zx^2+yy^2+zy^2 == 1 for a rotation matrix */
|
|
temp = temp2 / sqrtf(temp);
|
|
|
|
temp2 = curm->xz;
|
|
temp3 = curm->zz;
|
|
temp2 *= temp2;
|
|
temp2 += curm->yz * curm->yz;
|
|
temp2 += (temp3 * temp3);
|
|
/* temp2 = xz^2+yz^2+zz^2 == 1 for a rotation matrix */
|
|
temp2 = temp3 / sqrtf(temp2);
|
|
|
|
v->x = (fatan2(temp, temp2) * 10430.3779297f);
|
|
}
|
|
}
|
|
}
|
|
|
|
void Matrix_RotateVector(s16 angle, xyz_t* axis, u8 mode) {
|
|
MtxF* curm;
|
|
f32 sin;
|
|
f32 cos;
|
|
f32 temp1; // component x
|
|
f32 temp2; // component y
|
|
f32 temp3; // component z
|
|
f32 temp4; // component q?
|
|
|
|
if (mode == 1) {
|
|
if (angle != 0) {
|
|
curm = Matrix_now;
|
|
|
|
sin = sin_s(angle);
|
|
cos = cos_s(angle);
|
|
|
|
temp1 = curm->xx;
|
|
temp2 = curm->xy;
|
|
temp3 = curm->xz;
|
|
temp4 = (axis->x * temp1 + axis->y * temp2 + axis->z * temp3) * (1.0f - cos);
|
|
curm->xx = temp1 * cos + axis->x * temp4 + sin * (temp2 * axis->z - temp3 * axis->y);
|
|
curm->xy = temp2 * cos + axis->y * temp4 + sin * (temp3 * axis->x - temp1 * axis->z);
|
|
curm->xz = temp3 * cos + axis->z * temp4 + sin * (temp1 * axis->y - temp2 * axis->x);
|
|
|
|
temp1 = curm->yx;
|
|
temp2 = curm->yy;
|
|
temp3 = curm->yz;
|
|
temp4 = (axis->x * temp1 + axis->y * temp2 + axis->z * temp3) * (1.0f - cos);
|
|
curm->yx = temp1 * cos + axis->x * temp4 + sin * (temp2 * axis->z - temp3 * axis->y);
|
|
curm->yy = temp2 * cos + axis->y * temp4 + sin * (temp3 * axis->x - temp1 * axis->z);
|
|
curm->yz = temp3 * cos + axis->z * temp4 + sin * (temp1 * axis->y - temp2 * axis->x);
|
|
|
|
temp1 = curm->zx;
|
|
temp2 = curm->zy;
|
|
temp3 = curm->zz;
|
|
temp4 = (axis->x * temp1 + axis->y * temp2 + axis->z * temp3) * (1.0f - cos);
|
|
curm->zx = temp1 * cos + axis->x * temp4 + sin * (temp2 * axis->z - temp3 * axis->y);
|
|
curm->zy = temp2 * cos + axis->y * temp4 + sin * (temp3 * axis->x - temp1 * axis->z);
|
|
curm->zz = temp3 * cos + axis->z * temp4 + sin * (temp1 * axis->y - temp2 * axis->x);
|
|
}
|
|
} else {
|
|
curm = Matrix_now;
|
|
|
|
if (angle != 0) {
|
|
sin = sin_s(angle);
|
|
cos = cos_s(angle);
|
|
|
|
curm->xx = axis->x * axis->x * (1.0f - cos) + cos;
|
|
curm->yy = axis->y * axis->y * (1.0f - cos) + cos;
|
|
curm->zz = axis->z * axis->z * (1.0f - cos) + cos;
|
|
|
|
temp2 = axis->y * ((1.0f - cos) * axis->x);
|
|
temp3 = axis->z * sin;
|
|
curm->yx = temp2 + temp3;
|
|
curm->xy = temp2 - temp3;
|
|
|
|
temp2 = axis->z * ((1.0f - cos) * axis->x);
|
|
temp3 = axis->y * sin;
|
|
curm->zx = temp2 - temp3;
|
|
curm->xz = temp2 + temp3;
|
|
|
|
temp2 = axis->z * ((1.0f - cos) * axis->y);
|
|
temp3 = axis->x * sin;
|
|
curm->zy = temp2 + temp3 ;
|
|
curm->yz = temp2 - temp3;
|
|
|
|
curm->wx = 0.0f;
|
|
curm->wy = 0.0f;
|
|
curm->wz = 0.0f;
|
|
curm->xw = 0.0f;
|
|
curm->yw = 0.0f;
|
|
curm->zw = 0.0f;
|
|
curm->ww = 1.0f;
|
|
} else {
|
|
curm->yx = 0.0f;
|
|
curm->zx = 0.0f;
|
|
curm->wx = 0.0f;
|
|
curm->xy = 0.0f;
|
|
curm->zy = 0.0f;
|
|
curm->wy = 0.0f;
|
|
curm->xz = 0.0f;
|
|
curm->yz = 0.0f;
|
|
curm->wz = 0.0f;
|
|
curm->xw = 0.0f;
|
|
curm->yw = 0.0f;
|
|
curm->zw = 0.0f;
|
|
curm->xx = 1.0f;
|
|
curm->yy = 1.0f;
|
|
curm->zz = 1.0f;
|
|
curm->ww = 1.0f;
|
|
}
|
|
}
|
|
}
|
|
|
|
void suMtxMakeTS(Mtx *mtx, f32 scaleX, f32 scaleY, f32 scaleZ, f32 translateX, f32 translateY, f32 translateZ) {
|
|
struct {
|
|
s16 intPart[4][4];
|
|
u16 fracPart[4][4];
|
|
}* mu = (void*)mtx;
|
|
int fp;
|
|
|
|
fp = scaleX * 0x10000;
|
|
mtx->m[0][0] = fp;
|
|
mu->intPart[0][1] = 0;
|
|
mtx->m[0][1] = 0;
|
|
mtx->m[2][0] = (u32)fp << 16;
|
|
|
|
fp = scaleY * 0x10000;
|
|
mtx->m[2][1] = 0;
|
|
mtx->m[0][2] = (u32)fp >> 16;
|
|
mtx->m[0][3] = 0;
|
|
mtx->m[2][2] = fp & 0xFFFF;
|
|
mtx->m[2][3] = 0;
|
|
|
|
fp = scaleZ * 0x10000;
|
|
mtx->m[1][0] = 0;
|
|
mtx->m[1][1] = fp;
|
|
mu->intPart[2][3] = 0;
|
|
mtx->m[3][0] = 0;
|
|
mtx->m[3][1] = (u32)fp << 16;
|
|
|
|
fp = translateX * 0x10000;
|
|
mu->intPart[3][0] = ((u32)fp >> 16) & 0xFFFF;
|
|
mu->fracPart[3][0] = fp & 0xFFFF;
|
|
|
|
fp = translateY * 0x10000;
|
|
mu->intPart[3][1] = ((u32)fp >> 16) & 0xFFFF;
|
|
mu->fracPart[3][1] = fp & 0xFFFF;
|
|
|
|
fp = translateZ * 0x10000;
|
|
mu->intPart[3][2] = ((u32)fp >> 16) & 0xFFFF;
|
|
mu->intPart[3][3] = 1;
|
|
mtx->m[3][3] = (u32)fp << 16;
|
|
|
|
}
|
|
|
|
// S(RxRyRz)T where S is a scale matrix, Rx/Ry/Rz are rotations about the x/y/z axes, and T is a translation
|
|
void suMtxMakeSRT(Mtx* mtx, f32 scaleX, f32 scaleY, f32 scaleZ, s16 rotX, s16 rotY, s16 rotZ, f32 translateX, f32 translateY, f32 translateZ) {
|
|
int fp;
|
|
struct {
|
|
s16 intPart[4][4];
|
|
u16 fracPart[4][4];
|
|
}* mu = (void*)mtx;
|
|
f32 sinX = sin_s(rotX);
|
|
f32 sinY = sin_s(rotY); // sp+38
|
|
f32 sinZ = sin_s(rotZ); // sp+34
|
|
f32 cosX = cos_s(rotX); // sp+30
|
|
f32 cosY = cos_s(rotY); // sp+2C
|
|
f32 cosZ = cos_s(rotZ);
|
|
|
|
fp = cosY * cosZ * scaleX * 0x10000;
|
|
mu->intPart[0][0] = ((u32) fp >> 0x10) & 0xFFFF;
|
|
mu->fracPart[0][0] = fp & 0xFFFF;
|
|
|
|
fp = cosY * sinZ * scaleX * 0x10000;
|
|
mu->intPart[0][1] = ((u32) fp >> 0x10) & 0xFFFF;
|
|
mu->fracPart[0][1] = fp & 0xFFFF;
|
|
|
|
fp = -sinY * scaleX * 0x10000;
|
|
mu->intPart[0][2] = ((u32) fp >> 0x10) & 0xFFFF;
|
|
mu->fracPart[0][2] = fp & 0xFFFF;
|
|
|
|
fp = ((sinX * sinY * cosZ) - (cosX * sinZ)) * scaleY * 0x10000;
|
|
mu->intPart[1][0] = ((u32) fp >> 0x10) & 0xFFFF;
|
|
mu->fracPart[1][0] = fp & 0xFFFF;
|
|
|
|
fp = ((sinX * sinY * sinZ) + (cosX * cosZ)) * scaleY * 0x10000;
|
|
mu->intPart[1][1] = ((u32) fp >> 0x10) & 0xFFFF;
|
|
mu->fracPart[1][1] = fp & 0xFFFF;
|
|
|
|
fp = sinX * cosY * scaleY * 0x10000;
|
|
mu->intPart[1][2] = ((u32) fp >> 0x10) & 0xFFFF;
|
|
mu->fracPart[1][2] = fp & 0xFFFF;
|
|
|
|
fp = ((cosX * sinY * cosZ) + (sinX * sinZ)) * scaleZ * 0x10000;
|
|
mu->intPart[2][0] = ((u32) fp >> 0x10) & 0xFFFF;
|
|
mu->fracPart[2][0] = fp & 0xFFFF;
|
|
|
|
fp = ((cosX * sinY * sinZ) - (sinX * cosZ)) * scaleZ * 0x10000;
|
|
mu->intPart[2][1] = ((u32) fp >> 0x10) & 0xFFFF;
|
|
mu->fracPart[2][1] = fp & 0xFFFF;
|
|
|
|
fp = cosX * cosY * scaleZ * 0x10000;
|
|
mu->intPart[2][2] = ((u32) fp >> 0x10) & 0xFFFF;
|
|
mu->fracPart[2][2] = fp & 0xFFFF;
|
|
|
|
fp = translateX * 0x10000;
|
|
mu->intPart[3][0] = ((u32) fp >> 0x10) & 0xFFFF;
|
|
mu->fracPart[3][0] = fp & 0xFFFF;
|
|
|
|
fp = translateY * 0x10000;
|
|
mu->intPart[3][1] = ((u32) fp >> 0x10) & 0xFFFF;
|
|
mu->fracPart[3][1] = fp & 0xFFFF;
|
|
|
|
fp = translateZ * 0x10000;
|
|
mu->intPart[3][2] = ((u32) fp >> 0x10) & 0xFFFF;
|
|
mu->fracPart[3][2] = fp & 0xFFFF;
|
|
|
|
mu->intPart[0][3] = mu->intPart[1][3] = mu->intPart[2][3] = 0;
|
|
mu->fracPart[0][3] = mu->fracPart[1][3] = mu->fracPart[2][3] = 0;
|
|
mu->intPart[3][3] = 1;
|
|
mu->fracPart[3][3] = 0;
|
|
}
|
|
|
|
// S(RzRxRy)T where S is a scale matrix, Rx/Ry/Rz are rotations, and T is a translation
|
|
void suMtxMakeSRT_ZXY(Mtx* mtx, f32 scaleX, f32 scaleY, f32 scaleZ, s16 rotX, s16 rotY, s16 rotZ, f32 translateX, f32 translateY, f32 translateZ) {
|
|
int fp;
|
|
struct {
|
|
s16 intPart[4][4];
|
|
u16 fracPart[4][4];
|
|
}* mu = (void*)mtx;
|
|
f32 sinX = sin_s(rotX); // sp+4C
|
|
f32 sinY = sin_s(rotY); // likely sp+48
|
|
f32 sinZ = sin_s(rotZ); // sp+44
|
|
f32 cosX = cos_s(rotX); // sp+40
|
|
f32 cosY = cos_s(rotY); // sp+3C
|
|
f32 cosZ = cos_s(rotZ); // likely sp+38
|
|
|
|
fp = ((cosY * cosZ) + (sinX * sinY * sinZ)) * scaleX * 0x10000;
|
|
mu->intPart[0][0] = ((u32) fp >> 0x10) & 0xFFFF;
|
|
mu->fracPart[0][0] = fp & 0xFFFF;
|
|
|
|
fp = cosX * sinZ * scaleX * 0x10000;
|
|
mu->intPart[0][1] = ((u32) fp >> 0x10) & 0xFFFF;
|
|
mu->fracPart[0][1] = fp & 0xFFFF;
|
|
|
|
fp = ( - (sinY * cosZ) + (sinX * cosY * sinZ) ) * scaleX * 0x10000;
|
|
mu->intPart[0][2] = ((u32) fp >> 0x10) & 0xFFFF;
|
|
mu->fracPart[0][2] = fp & 0xFFFF;
|
|
|
|
fp = (- (cosY * sinZ) + (sinX * sinY * cosZ) ) * scaleY * 0x10000;
|
|
mu->intPart[1][0] = ((u32) fp >> 0x10) & 0xFFFF;
|
|
mu->fracPart[1][0] = fp & 0xFFFF;
|
|
|
|
fp = cosX * cosZ * scaleY * 0x10000;
|
|
mu->intPart[1][1] = ((u32) fp >> 0x10) & 0xFFFF;
|
|
mu->fracPart[1][1] = fp & 0xFFFF;
|
|
|
|
fp = ((sinY * sinZ) + (sinX * cosY * cosZ)) * scaleY * 0x10000;
|
|
mu->intPart[1][2] = ((u32) fp >> 0x10) & 0xFFFF;
|
|
mu->fracPart[1][2] = fp & 0xFFFF;
|
|
|
|
fp = cosX * sinY * scaleZ * 0x10000;
|
|
mu->intPart[2][0] = ((u32) fp >> 0x10) & 0xFFFF;
|
|
mu->fracPart[2][0] = fp & 0xFFFF;
|
|
|
|
fp = -sinX * scaleZ * 0x10000;
|
|
mu->intPart[2][1] = ((u32) fp >> 0x10) & 0xFFFF;
|
|
mu->fracPart[2][1] = fp & 0xFFFF;
|
|
|
|
fp = cosX * cosY * scaleZ * 0x10000;
|
|
mu->intPart[2][2] = ((u32) fp >> 0x10) & 0xFFFF;
|
|
mu->fracPart[2][2] = fp & 0xFFFF;
|
|
|
|
fp = translateX * 0x10000;
|
|
mu->intPart[3][0] = ((u32) fp >> 0x10) & 0xFFFF;
|
|
mu->fracPart[3][0] = fp & 0xFFFF;
|
|
|
|
fp = translateY * 0x10000;
|
|
mu->intPart[3][1] = ((u32) fp >> 0x10) & 0xFFFF;
|
|
mu->fracPart[3][1] = fp & 0xFFFF;
|
|
|
|
fp = translateZ * 0x10000;
|
|
mu->intPart[3][2] = ((u32) fp >> 0x10) & 0xFFFF;
|
|
mu->fracPart[3][2] = fp & 0xFFFF;
|
|
|
|
mu->intPart[0][3] = mu->intPart[1][3] = mu->intPart[2][3] = 0;
|
|
mu->fracPart[0][3] = mu->fracPart[1][3] = mu->fracPart[2][3] = 0;
|
|
mu->intPart[3][3] = 1;
|
|
mu->fracPart[3][3] = 0;
|
|
}
|
|
|