mirror of
https://github.com/coop-deluxe/sm64coopdx.git
synced 2024-10-19 19:52:39 +00:00
850 lines
24 KiB
C
850 lines
24 KiB
C
|
#include <ultra64.h>
|
||
|
#include <macros.h>
|
||
|
#include "gd_types.h"
|
||
|
#include "gd_macros.h"
|
||
|
#include "gd_main.h"
|
||
|
#include "gd_math.h"
|
||
|
#include "debug_utils.h"
|
||
|
#include "renderer.h"
|
||
|
|
||
|
/* 242300 -> 242338 */
|
||
|
f32 gd_sqrt_f(f32 val) {
|
||
|
return (f32) gd_sqrt_d(val);
|
||
|
}
|
||
|
|
||
|
/* 242338 -> 24292C */
|
||
|
void func_80193B68(Mat4f *mtx, f32 a1, f32 a2, f32 a3, f32 sp78, f32 sp7C, f32 sp80, f32 sp84, f32 sp88,
|
||
|
f32 sp8C) {
|
||
|
f32 sp64;
|
||
|
struct GdVec3f sp58;
|
||
|
struct GdVec3f sp4C;
|
||
|
struct GdVec3f sp40;
|
||
|
|
||
|
set_identity_mat4(mtx);
|
||
|
sp58.z = sp78 - a1;
|
||
|
sp58.y = sp7C - a2;
|
||
|
sp58.x = sp80 - a3;
|
||
|
|
||
|
sp64 = ABS(sp58.z) + ABS(sp58.y) + ABS(sp58.x);
|
||
|
|
||
|
if (sp64 > 10000.0f || sp64 < 10.0f) {
|
||
|
sp40.x = sp58.z;
|
||
|
sp40.y = sp58.y;
|
||
|
sp40.z = sp58.x;
|
||
|
into_unit_vec3f(&sp40);
|
||
|
sp40.x *= 10000.0f;
|
||
|
sp40.y *= 10000.0f;
|
||
|
sp40.z *= 10000.0f;
|
||
|
|
||
|
sp58.z = sp40.x;
|
||
|
sp58.y = sp40.y;
|
||
|
sp58.x = sp40.z;
|
||
|
}
|
||
|
// L80193D44
|
||
|
sp64 = -1.0 / gd_sqrt_f(SQ(sp58.z) + SQ(sp58.y) + SQ(sp58.x)); //? -1.0f
|
||
|
sp58.z *= sp64;
|
||
|
sp58.y *= sp64;
|
||
|
sp58.x *= sp64;
|
||
|
// 80193DC8
|
||
|
sp4C.z = sp88 * sp58.x - sp8C * sp58.y;
|
||
|
sp4C.y = sp8C * sp58.z - sp84 * sp58.x;
|
||
|
sp4C.x = sp84 * sp58.y - sp88 * sp58.z;
|
||
|
sp64 = 1.0 / gd_sqrt_f(SQ(sp4C.z) + SQ(sp4C.y) + SQ(sp4C.x)); //? 1.0f
|
||
|
sp4C.z *= sp64;
|
||
|
sp4C.y *= sp64;
|
||
|
sp4C.x *= sp64;
|
||
|
|
||
|
sp84 = sp58.y * sp4C.x - sp58.x * sp4C.y;
|
||
|
sp88 = sp58.x * sp4C.z - sp58.z * sp4C.x;
|
||
|
sp8C = sp58.z * sp4C.y - sp58.y * sp4C.z;
|
||
|
sp64 = 1.0 / gd_sqrt_f(SQ(sp84) + SQ(sp88) + SQ(sp8C)); //? 1.0f
|
||
|
sp84 *= sp64;
|
||
|
sp88 *= sp64;
|
||
|
sp8C *= sp64;
|
||
|
// 80193FA8
|
||
|
(*mtx)[0][0] = sp4C.z;
|
||
|
(*mtx)[1][0] = sp4C.y;
|
||
|
(*mtx)[2][0] = sp4C.x;
|
||
|
(*mtx)[3][0] = -(a1 * sp4C.z + a2 * sp4C.y + a3 * sp4C.x);
|
||
|
|
||
|
(*mtx)[0][1] = sp84;
|
||
|
(*mtx)[1][1] = sp88;
|
||
|
(*mtx)[2][1] = sp8C;
|
||
|
(*mtx)[3][1] = -(a1 * sp84 + a2 * sp88 + a3 * sp8C);
|
||
|
|
||
|
(*mtx)[0][2] = sp58.z;
|
||
|
(*mtx)[1][2] = sp58.y;
|
||
|
(*mtx)[2][2] = sp58.x;
|
||
|
(*mtx)[3][2] = -(a1 * sp58.z + a2 * sp58.y + a3 * sp58.x);
|
||
|
|
||
|
(*mtx)[0][3] = 0.0f;
|
||
|
(*mtx)[1][3] = 0.0f;
|
||
|
(*mtx)[2][3] = 0.0f;
|
||
|
(*mtx)[3][3] = 1.0f;
|
||
|
}
|
||
|
|
||
|
/* 24292C -> 2429F0 */
|
||
|
void func_8019415C(Mat4f *mtx, struct GdVec3f *vec) {
|
||
|
(*mtx)[0][0] *= vec->x;
|
||
|
(*mtx)[0][1] *= vec->x;
|
||
|
(*mtx)[0][2] *= vec->x;
|
||
|
(*mtx)[1][0] *= vec->y;
|
||
|
(*mtx)[1][1] *= vec->y;
|
||
|
(*mtx)[1][2] *= vec->y;
|
||
|
(*mtx)[2][0] *= vec->z;
|
||
|
(*mtx)[2][1] *= vec->z;
|
||
|
(*mtx)[2][2] *= vec->z;
|
||
|
}
|
||
|
|
||
|
/* 2429F0 -> 242AB4 */
|
||
|
void func_80194220(Mat4f *mtx, struct GdVec3f *vec) {
|
||
|
if (vec->x != 0.0f) {
|
||
|
absrot_mat4(mtx, 0, vec->x);
|
||
|
}
|
||
|
if (vec->y != 0.0f) {
|
||
|
absrot_mat4(mtx, 1, vec->y);
|
||
|
}
|
||
|
if (vec->z != 0.0f) {
|
||
|
absrot_mat4(mtx, 2, vec->z);
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/* 242AB4 -> 242B28 */
|
||
|
void func_801942E4(Mat4f *mtx, struct GdVec3f *vec) {
|
||
|
UNUSED Mat4f temp;
|
||
|
f32 z, y, x;
|
||
|
|
||
|
x = vec->x;
|
||
|
y = vec->y;
|
||
|
z = vec->z;
|
||
|
|
||
|
(*mtx)[3][0] += x;
|
||
|
(*mtx)[3][1] += y;
|
||
|
(*mtx)[3][2] += z;
|
||
|
}
|
||
|
|
||
|
/* 242B28 -> 242EF8 */
|
||
|
void func_80194358(Mat4f *mtx, struct GdVec3f *vec, f32 a2) {
|
||
|
f32 sp34;
|
||
|
f32 sp30; // distance between unit vec.x and unit vec.y
|
||
|
f32 sp2C; // cos(a2)
|
||
|
f32 sp28; // sin(a2)
|
||
|
f32 sp24 = RAD_PER_DEG;
|
||
|
struct GdVec3f unit; // 18
|
||
|
|
||
|
unit.x = vec->x;
|
||
|
unit.y = vec->y;
|
||
|
unit.z = vec->z;
|
||
|
into_unit_vec3f(&unit);
|
||
|
sp30 = gd_sqrt_f(SQ(unit.x) + SQ(unit.z));
|
||
|
a2 *= sp24; // convert a2 from degrees to radians?
|
||
|
sp28 = gd_sin_d(a2);
|
||
|
sp2C = gd_cos_d(a2);
|
||
|
|
||
|
set_identity_mat4(mtx);
|
||
|
if (sp30 != 0.0f) {
|
||
|
sp34 = 1.0f / sp30;
|
||
|
(*mtx)[0][0] = ((-unit.z * sp2C) - (sp28 * unit.y * unit.x)) * sp34;
|
||
|
(*mtx)[1][0] = ((unit.z * sp28) - (sp2C * unit.y * unit.x)) * sp34;
|
||
|
(*mtx)[2][0] = -unit.x;
|
||
|
(*mtx)[3][0] = 0.0f;
|
||
|
(*mtx)[0][1] = sp28 * sp30;
|
||
|
(*mtx)[1][1] = sp2C * sp30;
|
||
|
(*mtx)[2][1] = -unit.y;
|
||
|
(*mtx)[3][1] = 0.0f;
|
||
|
(*mtx)[0][2] = ((sp2C * unit.x) - (sp28 * unit.y * unit.z)) * sp34;
|
||
|
(*mtx)[1][2] = ((-sp28 * unit.x) - (sp2C * unit.y * unit.z)) * sp34;
|
||
|
(*mtx)[2][2] = -unit.z;
|
||
|
(*mtx)[3][2] = 0.0f;
|
||
|
(*mtx)[0][3] = 0.0f;
|
||
|
(*mtx)[1][3] = 0.0f;
|
||
|
(*mtx)[2][3] = 0.0f;
|
||
|
(*mtx)[3][3] = 1.0f;
|
||
|
} else { // L80194600
|
||
|
(*mtx)[0][0] = 0.0f;
|
||
|
(*mtx)[1][0] = 1.0f;
|
||
|
(*mtx)[2][0] = 0.0f;
|
||
|
(*mtx)[3][0] = 0.0f;
|
||
|
(*mtx)[0][1] = 0.0f;
|
||
|
(*mtx)[1][1] = 0.0f;
|
||
|
(*mtx)[2][1] = 1.0f;
|
||
|
(*mtx)[3][1] = 0.0f;
|
||
|
(*mtx)[0][2] = 1.0f;
|
||
|
(*mtx)[1][2] = 0.0f;
|
||
|
(*mtx)[2][2] = 0.0f;
|
||
|
(*mtx)[3][2] = 0.0f;
|
||
|
(*mtx)[0][3] = 0.0f;
|
||
|
(*mtx)[1][3] = 0.0f;
|
||
|
(*mtx)[2][3] = 0.0f;
|
||
|
(*mtx)[3][3] = 1.0f;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/* 242EF8 -> 242F40 */
|
||
|
f32 func_80194728(f32 a, f32 b) {
|
||
|
if (b < a) {
|
||
|
a = b;
|
||
|
} else if (a < -b) {
|
||
|
a = -b;
|
||
|
}
|
||
|
|
||
|
return a;
|
||
|
}
|
||
|
|
||
|
/* 242F40 -> 243050; orig name: func_80194770 */
|
||
|
void limit_vec3f(struct GdVec3f *vec, f32 limit) {
|
||
|
if (vec->x > limit) {
|
||
|
vec->x = limit;
|
||
|
} else if (vec->x < -limit) {
|
||
|
vec->x = -limit;
|
||
|
}
|
||
|
|
||
|
if (vec->y > limit) {
|
||
|
vec->y = limit;
|
||
|
} else if (vec->y < -limit) {
|
||
|
vec->y = -limit;
|
||
|
}
|
||
|
|
||
|
if (vec->z > limit) {
|
||
|
vec->z = limit;
|
||
|
} else if (vec->z < -limit) {
|
||
|
vec->z = -limit;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/* 243050 -> 24315C */
|
||
|
void func_80194880(f32 a0, f32 *a1, f32 *a2) {
|
||
|
f32 sp34;
|
||
|
f32 sp30;
|
||
|
f32 sp2C;
|
||
|
|
||
|
sp2C = a0 / DEG_PER_RAD; // degree to radians
|
||
|
sp34 = (*a1 * gd_cos_d(sp2C)) - (*a2 * gd_sin_d(sp2C));
|
||
|
sp30 = (*a1 * gd_sin_d(sp2C)) + (*a2 * gd_cos_d(sp2C));
|
||
|
*a1 = sp34;
|
||
|
*a2 = sp30;
|
||
|
}
|
||
|
|
||
|
/* 24315C -> 243224 */
|
||
|
void Unknown8019498C(Mat4f *a0, s32 row, f32 a2) {
|
||
|
Mat4f sp28;
|
||
|
struct GdVec3f vec;
|
||
|
|
||
|
vec.x = (*a0)[row][0];
|
||
|
vec.y = (*a0)[row][1];
|
||
|
vec.z = (*a0)[row][2];
|
||
|
|
||
|
func_801961F4(&sp28, &vec, a2 / 2.0); //? 2.0f
|
||
|
multiply_mat4(a0, &sp28, a0);
|
||
|
}
|
||
|
|
||
|
// TODO: enumerate second argument (0-2)?
|
||
|
/* 243224 -> 243368; orig name: func_80194A54 */
|
||
|
void absrot_mat4(Mat4f *mtx, s32 axisnum, f32 a2) {
|
||
|
Mat4f sp30;
|
||
|
struct GdVec3f rot; // 24
|
||
|
|
||
|
switch (axisnum) {
|
||
|
case 0:
|
||
|
rot.x = 1.0f;
|
||
|
rot.y = 0.0f;
|
||
|
rot.z = 0.0f;
|
||
|
break;
|
||
|
case 1:
|
||
|
rot.x = 0.0f;
|
||
|
rot.y = 1.0f;
|
||
|
rot.z = 0.0f;
|
||
|
break;
|
||
|
case 2:
|
||
|
rot.x = 0.0f;
|
||
|
rot.y = 0.0f;
|
||
|
rot.z = 1.0f;
|
||
|
break;
|
||
|
default:
|
||
|
fatal_printf("absrot_matrix4(): Bad axis num");
|
||
|
}
|
||
|
|
||
|
func_801961F4(&sp30, &rot, a2 / 2.0); //? 2.0f
|
||
|
multiply_mat4(mtx, &sp30, mtx);
|
||
|
}
|
||
|
|
||
|
/* 243368 -> 2433C4 */
|
||
|
f32 magnitude_vec3f(struct GdVec3f *vec) {
|
||
|
return gd_sqrt_f(SQ(vec->x) + SQ(vec->y) + SQ(vec->z));
|
||
|
}
|
||
|
|
||
|
/* 2433C4 -> 2434E4; orig name: func_80194BF4 */
|
||
|
s32 into_unit_vec3f(struct GdVec3f *vec) {
|
||
|
f32 mag;
|
||
|
if ((mag = SQ(vec->x) + SQ(vec->y) + SQ(vec->z)) == 0.0f) {
|
||
|
return FALSE;
|
||
|
}
|
||
|
|
||
|
mag = gd_sqrt_f(mag);
|
||
|
if (mag == 0.0f) {
|
||
|
vec->x = 0.0f;
|
||
|
vec->y = 0.0f;
|
||
|
vec->z = 0.0f;
|
||
|
return FALSE;
|
||
|
}
|
||
|
|
||
|
vec->x /= mag;
|
||
|
vec->y /= mag;
|
||
|
vec->z /= mag;
|
||
|
|
||
|
return TRUE;
|
||
|
}
|
||
|
|
||
|
/* 2434E4 -> 243588; orig name: func_80194D14 */
|
||
|
void cross_product_vec3f(struct GdVec3f *a, struct GdVec3f *b, struct GdVec3f *dst) {
|
||
|
struct GdVec3f result;
|
||
|
|
||
|
result.x = (a->y * b->z) - (a->z * b->y);
|
||
|
result.y = (a->z * b->x) - (a->x * b->z);
|
||
|
result.z = (a->x * b->y) - (a->y * b->x);
|
||
|
|
||
|
dst->x = result.x;
|
||
|
dst->y = result.y;
|
||
|
dst->z = result.z;
|
||
|
}
|
||
|
|
||
|
/* 243588 -> 2435CC; orig name: func_80194DB8 */
|
||
|
f32 dot_product_vec3f(struct GdVec3f *a, struct GdVec3f *b) {
|
||
|
return (a->x * b->x) + (a->y * b->y) + (a->z * b->z);
|
||
|
}
|
||
|
|
||
|
/* 2435CC -> 24364C */
|
||
|
void Unknown80194DFC(Mat4f *src, Mat4f *dst) {
|
||
|
s32 i;
|
||
|
s32 j;
|
||
|
|
||
|
for (i = 0; i < 4; i++) {
|
||
|
for (j = 0; j < 4; j++) {
|
||
|
(*dst)[i][j] = 1.0f / (*src)[i][j];
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/* self */ void func_80194F90(Mat4f *, Mat4f *);
|
||
|
/* self */ f32 func_80195578(Mat4f *);
|
||
|
|
||
|
/* 24364C -> 243760; orig name: func_80194E7C */
|
||
|
void inverse_mat4(Mat4f *src, Mat4f *dst) {
|
||
|
s32 i;
|
||
|
s32 j;
|
||
|
f32 determinant;
|
||
|
|
||
|
func_80194F90(src, dst);
|
||
|
determinant = func_80195578(dst);
|
||
|
|
||
|
if (ABS(determinant) < 1e-5) //? 1e-5f
|
||
|
{
|
||
|
fatal_print("Non-singular matrix, no inverse!\n");
|
||
|
}
|
||
|
|
||
|
for (i = 0; i < 4; i++) {
|
||
|
for (j = 0; j < 4; j++) {
|
||
|
(*dst)[i][j] /= determinant;
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/* self */ f32 func_80195844(f32, f32, f32, f32, f32, f32, f32, f32, f32);
|
||
|
|
||
|
struct Row4 {
|
||
|
f32 c0, c1, c2, c3;
|
||
|
};
|
||
|
struct InvMat4 {
|
||
|
struct Row4 r0, r1, r2, r3;
|
||
|
};
|
||
|
|
||
|
/* 243760 -> 243D48 */
|
||
|
// TODO: adjunct mat4?
|
||
|
void func_80194F90(Mat4f *src, Mat4f *adj) {
|
||
|
struct InvMat4 inv; // 30
|
||
|
|
||
|
inv.r3.c3 = (*src)[0][0];
|
||
|
inv.r2.c3 = (*src)[0][1];
|
||
|
inv.r1.c3 = (*src)[0][2];
|
||
|
inv.r0.c3 = (*src)[0][3];
|
||
|
inv.r3.c2 = (*src)[1][0];
|
||
|
inv.r2.c2 = (*src)[1][1];
|
||
|
inv.r1.c2 = (*src)[1][2];
|
||
|
inv.r0.c2 = (*src)[1][3];
|
||
|
inv.r3.c1 = (*src)[2][0];
|
||
|
inv.r2.c1 = (*src)[2][1];
|
||
|
inv.r1.c1 = (*src)[2][2];
|
||
|
inv.r0.c1 = (*src)[2][3];
|
||
|
inv.r3.c0 = (*src)[3][0];
|
||
|
inv.r2.c0 = (*src)[3][1];
|
||
|
inv.r1.c0 = (*src)[3][2];
|
||
|
inv.r0.c0 = (*src)[3][3];
|
||
|
|
||
|
(*adj)[0][0] = func_80195844(inv.r2.c2, inv.r2.c1, inv.r2.c0, inv.r1.c2, inv.r1.c1, inv.r1.c0,
|
||
|
inv.r0.c2, inv.r0.c1, inv.r0.c0);
|
||
|
|
||
|
(*adj)[1][0] = -func_80195844(inv.r3.c2, inv.r3.c1, inv.r3.c0, inv.r1.c2, inv.r1.c1, inv.r1.c0,
|
||
|
inv.r0.c2, inv.r0.c1, inv.r0.c0);
|
||
|
// 80195170
|
||
|
(*adj)[2][0] = func_80195844(inv.r3.c2, inv.r3.c1, inv.r3.c0, inv.r2.c2, inv.r2.c1, inv.r2.c0,
|
||
|
inv.r0.c2, inv.r0.c1, inv.r0.c0);
|
||
|
// 801951B8
|
||
|
(*adj)[3][0] = -func_80195844(inv.r3.c2, inv.r3.c1, inv.r3.c0, inv.r2.c2, inv.r2.c1, inv.r2.c0,
|
||
|
inv.r1.c2, inv.r1.c1, inv.r1.c0);
|
||
|
// 80195200
|
||
|
(*adj)[0][1] = -func_80195844(inv.r2.c3, inv.r2.c1, inv.r2.c0, inv.r1.c3, inv.r1.c1, inv.r1.c0,
|
||
|
inv.r0.c3, inv.r0.c1, inv.r0.c0);
|
||
|
// 80195248
|
||
|
(*adj)[1][1] = func_80195844(inv.r3.c3, inv.r3.c1, inv.r3.c0, inv.r1.c3, inv.r1.c1, inv.r1.c0,
|
||
|
inv.r0.c3, inv.r0.c1, inv.r0.c0);
|
||
|
// 80195290
|
||
|
(*adj)[2][1] = -func_80195844(inv.r3.c3, inv.r3.c1, inv.r3.c0, inv.r2.c3, inv.r2.c1, inv.r2.c0,
|
||
|
inv.r0.c3, inv.r0.c1, inv.r0.c0);
|
||
|
// 801952D8
|
||
|
(*adj)[3][1] = func_80195844(inv.r3.c3, inv.r3.c1, inv.r3.c0, inv.r2.c3, inv.r2.c1, inv.r2.c0,
|
||
|
inv.r1.c3, inv.r1.c1, inv.r1.c0);
|
||
|
// 80195320
|
||
|
(*adj)[0][2] = func_80195844(inv.r2.c3, inv.r2.c2, inv.r2.c0, inv.r1.c3, inv.r1.c2, inv.r1.c0,
|
||
|
inv.r0.c3, inv.r0.c2, inv.r0.c0);
|
||
|
// 80195368
|
||
|
(*adj)[1][2] = -func_80195844(inv.r3.c3, inv.r3.c2, inv.r3.c0, inv.r1.c3, inv.r1.c2, inv.r1.c0,
|
||
|
inv.r0.c3, inv.r0.c2, inv.r0.c0);
|
||
|
// 801953B0
|
||
|
(*adj)[2][2] = func_80195844(inv.r3.c3, inv.r3.c2, inv.r3.c0, inv.r2.c3, inv.r2.c2, inv.r2.c0,
|
||
|
inv.r0.c3, inv.r0.c2, inv.r0.c0);
|
||
|
// 801953F8
|
||
|
(*adj)[3][2] = -func_80195844(inv.r3.c3, inv.r3.c2, inv.r3.c0, inv.r2.c3, inv.r2.c2, inv.r2.c0,
|
||
|
inv.r1.c3, inv.r1.c2, inv.r1.c0);
|
||
|
// 80195440
|
||
|
(*adj)[0][3] = -func_80195844(inv.r2.c3, inv.r2.c2, inv.r2.c1, inv.r1.c3, inv.r1.c2, inv.r1.c1,
|
||
|
inv.r0.c3, inv.r0.c2, inv.r0.c1);
|
||
|
// 80195488
|
||
|
(*adj)[1][3] = func_80195844(inv.r3.c3, inv.r3.c2, inv.r3.c1, inv.r1.c3, inv.r1.c2, inv.r1.c1,
|
||
|
inv.r0.c3, inv.r0.c2, inv.r0.c1);
|
||
|
// 801954D0
|
||
|
(*adj)[2][3] = -func_80195844(inv.r3.c3, inv.r3.c2, inv.r3.c1, inv.r2.c3, inv.r2.c2, inv.r2.c1,
|
||
|
inv.r0.c3, inv.r0.c2, inv.r0.c1);
|
||
|
// 80195518
|
||
|
(*adj)[3][3] = func_80195844(inv.r3.c3, inv.r3.c2, inv.r3.c1, inv.r2.c3, inv.r2.c2, inv.r2.c1,
|
||
|
inv.r1.c3, inv.r1.c2, inv.r1.c1);
|
||
|
}
|
||
|
|
||
|
/* 243D48 -> 244014 */
|
||
|
// TODO: determinant mat4
|
||
|
f32 func_80195578(Mat4f *mtx) {
|
||
|
f32 det;
|
||
|
struct InvMat4 inv; // 54 -> 94
|
||
|
|
||
|
inv.r3.c3 = (*mtx)[0][0];
|
||
|
inv.r2.c3 = (*mtx)[0][1];
|
||
|
inv.r1.c3 = (*mtx)[0][2];
|
||
|
inv.r0.c3 = (*mtx)[0][3];
|
||
|
inv.r3.c2 = (*mtx)[1][0];
|
||
|
inv.r2.c2 = (*mtx)[1][1];
|
||
|
inv.r1.c2 = (*mtx)[1][2];
|
||
|
inv.r0.c2 = (*mtx)[1][3];
|
||
|
inv.r3.c1 = (*mtx)[2][0];
|
||
|
inv.r2.c1 = (*mtx)[2][1];
|
||
|
inv.r1.c1 = (*mtx)[2][2];
|
||
|
inv.r0.c1 = (*mtx)[2][3];
|
||
|
inv.r3.c0 = (*mtx)[3][0];
|
||
|
inv.r2.c0 = (*mtx)[3][1];
|
||
|
inv.r1.c0 = (*mtx)[3][2];
|
||
|
inv.r0.c0 = (*mtx)[3][3];
|
||
|
|
||
|
det = (inv.r3.c3
|
||
|
* func_80195844(inv.r2.c2, inv.r2.c1, inv.r2.c0, inv.r1.c2, inv.r1.c1, inv.r1.c0,
|
||
|
inv.r0.c2, inv.r0.c1, inv.r0.c0)
|
||
|
- inv.r2.c3
|
||
|
* func_80195844(inv.r3.c2, inv.r3.c1, inv.r3.c0, inv.r1.c2, inv.r1.c1, inv.r1.c0,
|
||
|
inv.r0.c2, inv.r0.c1, inv.r0.c0))
|
||
|
+ inv.r1.c3
|
||
|
* func_80195844(inv.r3.c2, inv.r3.c1, inv.r3.c0, inv.r2.c2, inv.r2.c1, inv.r2.c0,
|
||
|
inv.r0.c2, inv.r0.c1, inv.r0.c0)
|
||
|
- inv.r0.c3
|
||
|
* func_80195844(inv.r3.c2, inv.r3.c1, inv.r3.c0, inv.r2.c2, inv.r2.c1, inv.r2.c0,
|
||
|
inv.r1.c2, inv.r1.c1, inv.r1.c0);
|
||
|
|
||
|
return det;
|
||
|
}
|
||
|
|
||
|
/* self */ f32 func_8019590C(f32, f32, f32, f32);
|
||
|
|
||
|
/* 244014 -> 2440DC */
|
||
|
// TODO: determinant_3x3
|
||
|
f32 func_80195844(f32 r0c0, f32 r0c1, f32 r0c2, f32 r1c0, f32 r1c1, f32 r1c2, f32 r2c0, f32 r2c1,
|
||
|
f32 r2c2) {
|
||
|
f32 det;
|
||
|
|
||
|
det = r0c0 * func_8019590C(r1c1, r1c2, r2c1, r2c2) - r1c0 * func_8019590C(r0c1, r0c2, r2c1, r2c2)
|
||
|
+ r2c0 * func_8019590C(r0c1, r0c2, r1c1, r1c2);
|
||
|
|
||
|
return det;
|
||
|
}
|
||
|
|
||
|
/* 2440DC -> 244120 */
|
||
|
// TODO: determinant_2x2
|
||
|
f32 func_8019590C(f32 a, f32 b, f32 c, f32 d) {
|
||
|
f32 det = a * d - b * c;
|
||
|
|
||
|
return det;
|
||
|
}
|
||
|
|
||
|
/* 244120 -> 2441B0 */
|
||
|
void Unknown80195950(Mat4f *mtx, struct GdVec3f *vec, f32 x, f32 y, f32 z) {
|
||
|
s32 i;
|
||
|
|
||
|
vec->x = -x;
|
||
|
vec->y = -y;
|
||
|
vec->z = -z;
|
||
|
|
||
|
(*mtx)[0][0] = 1.0f;
|
||
|
|
||
|
for (i = 1; i < 4; i++) {
|
||
|
(*mtx)[0][i] = 0.0f;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/* 2441B0 -> 2442D4 */
|
||
|
void Unknown801959E0(f32 *a0, struct GdVec3f *vec, f32 a2, s32 a3, s32 sp38) {
|
||
|
s32 sp24;
|
||
|
s32 sp20;
|
||
|
UNUSED f32 sp1C;
|
||
|
UNUSED f32 sp18;
|
||
|
UNUSED struct GdVec3f sp0C;
|
||
|
struct GdVec3f sp00;
|
||
|
|
||
|
sp00.x = vec->x;
|
||
|
sp00.y = vec->y;
|
||
|
sp00.z = vec->z;
|
||
|
|
||
|
if (sp38 < 0) {
|
||
|
goto end;
|
||
|
}
|
||
|
// L80195A24
|
||
|
sp24 = a3 + 1;
|
||
|
if (sp24 >= 4) {
|
||
|
sp24 = 1;
|
||
|
}
|
||
|
// L80195A48
|
||
|
if ((sp20 = sp24 + 1) >= 4) {
|
||
|
sp20 = 1;
|
||
|
}
|
||
|
// L80195A68
|
||
|
sp1C = a0[sp24];
|
||
|
sp18 = a0[sp20];
|
||
|
sp0C.x = a0[0];
|
||
|
sp0C.y = a0[a3];
|
||
|
sp0C.z = a2 + a2;
|
||
|
|
||
|
end: // L80195AD0
|
||
|
vec->x = sp00.x;
|
||
|
vec->y = sp00.y;
|
||
|
vec->z = sp00.z;
|
||
|
}
|
||
|
|
||
|
/* 2442D4 -> 244448 */
|
||
|
void Unknown80195B04(f32 *a0, UNUSED s32 a1, f32 a2, f32 a3, s32 sp28, s32 sp2C) {
|
||
|
s32 sp14;
|
||
|
s32 sp10;
|
||
|
f32 sp0C;
|
||
|
UNUSED u32 pad[2];
|
||
|
|
||
|
if ((sp14 = sp28 + 1) >= 4) {
|
||
|
sp14 = 1;
|
||
|
}
|
||
|
if ((sp10 = sp14 + 1) >= 4) {
|
||
|
sp10 = 1;
|
||
|
}
|
||
|
|
||
|
sp0C = a0[sp28];
|
||
|
a0[sp28] = ((f32) sp2C) * a3 * a0[0] + sp0C * a2;
|
||
|
a0[0] = a0[0] * a2 - ((f32) sp2C) * a3 * sp0C;
|
||
|
|
||
|
sp0C = a0[sp14];
|
||
|
a0[sp14] = a0[sp10] * a3 + sp0C * a2;
|
||
|
a0[sp10] = a0[sp10] * a2 - a3 * sp0C;
|
||
|
}
|
||
|
|
||
|
/* 244448 -> 244568 */
|
||
|
void func_80195C78(Mat4f *mtx) {
|
||
|
s32 i; // 14
|
||
|
s32 j; // 10
|
||
|
f32 temp[3]; // 04
|
||
|
|
||
|
for (i = 0; i < 3; i++) {
|
||
|
temp[i] = (*mtx)[0][i + 1];
|
||
|
}
|
||
|
for (i = 1; i < 4; i++) {
|
||
|
for (j = 1; j < 4; j++) {
|
||
|
(*mtx)[i - 1][j - 1] = (*mtx)[i][j];
|
||
|
}
|
||
|
}
|
||
|
|
||
|
(*mtx)[0][3] = 0.0f;
|
||
|
(*mtx)[1][3] = 0.0f;
|
||
|
(*mtx)[2][3] = 0.0f;
|
||
|
(*mtx)[3][3] = 1.0f;
|
||
|
|
||
|
for (i = 0; i < 3; i++) {
|
||
|
(*mtx)[3][i] = temp[i];
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/* 244568 -> 2447A4 */
|
||
|
void Unknown80195D98(f32 *a0, UNUSED s32 a1, Mat4f *mtx) {
|
||
|
f32 sp3C;
|
||
|
f32 sp38;
|
||
|
f32 temp[4]; // 28
|
||
|
s32 i; // 24
|
||
|
s32 sp20; // row?
|
||
|
s32 sp1C;
|
||
|
|
||
|
for (i = 0; i < 4; i++) {
|
||
|
temp[i] = SQ(a0[i]);
|
||
|
}
|
||
|
for (i = 1; i < 4; i++) {
|
||
|
if ((sp20 = i + 1) >= 4) {
|
||
|
sp20 = 1;
|
||
|
}
|
||
|
if ((sp1C = sp20 + 1) >= 4) {
|
||
|
sp1C = 1;
|
||
|
}
|
||
|
|
||
|
sp3C = 2.0 * a0[i] * a0[sp20]; //? 2.0f
|
||
|
sp38 = 2.0 * a0[sp1C] * a0[0]; //? 2.0f
|
||
|
|
||
|
(*mtx)[sp20][i] = sp3C - sp38;
|
||
|
(*mtx)[i][sp20] = sp3C + sp38;
|
||
|
(*mtx)[i][i] = temp[i] + temp[0] - temp[sp20] - temp[sp1C];
|
||
|
(*mtx)[i][0] = 0.0f;
|
||
|
}
|
||
|
(*mtx)[0][0] = 1.0f;
|
||
|
func_80195C78(mtx);
|
||
|
}
|
||
|
|
||
|
/* 2447A4 -> 2449C4 */
|
||
|
void func_80195FD4(Mat4f *mtx, struct GdVec3f *vec, f32 a, f32 b) {
|
||
|
f32 spC;
|
||
|
struct GdVec3f rev; // 0
|
||
|
|
||
|
rev.z = vec->x;
|
||
|
rev.y = vec->y;
|
||
|
rev.x = vec->z;
|
||
|
|
||
|
spC = 1.0 - b;
|
||
|
|
||
|
(*mtx)[0][0] = spC * rev.z * rev.z + b;
|
||
|
(*mtx)[0][1] = spC * rev.z * rev.y + a * rev.x;
|
||
|
(*mtx)[0][2] = spC * rev.z * rev.x - a * rev.y;
|
||
|
(*mtx)[0][3] = 0.0f;
|
||
|
(*mtx)[1][0] = spC * rev.z * rev.y - a * rev.x;
|
||
|
(*mtx)[1][1] = spC * rev.y * rev.y + b;
|
||
|
(*mtx)[1][2] = spC * rev.y * rev.x + a * rev.z;
|
||
|
(*mtx)[1][3] = 0.0f;
|
||
|
(*mtx)[2][0] = spC * rev.z * rev.x + a * rev.y;
|
||
|
(*mtx)[2][1] = spC * rev.y * rev.x - a * rev.z;
|
||
|
(*mtx)[2][2] = spC * rev.x * rev.x + b;
|
||
|
(*mtx)[2][3] = 0.0f;
|
||
|
(*mtx)[3][0] = 0.0f;
|
||
|
(*mtx)[3][1] = 0.0f;
|
||
|
(*mtx)[3][2] = 0.0f;
|
||
|
(*mtx)[3][3] = 1.0f;
|
||
|
}
|
||
|
|
||
|
/* 2449C4 -> 244A50 */
|
||
|
void func_801961F4(Mat4f *mtx, struct GdVec3f *vec, f32 ang) {
|
||
|
f32 ycmp; // 1c
|
||
|
f32 xcmp; // 18
|
||
|
|
||
|
// probably is "(angle * pi / 180) / 2"
|
||
|
ycmp = gd_sin_d(ang / (DEG_PER_RAD / 2.0));
|
||
|
xcmp = gd_cos_d(ang / (DEG_PER_RAD / 2.0));
|
||
|
|
||
|
func_80195FD4(mtx, vec, ycmp, xcmp);
|
||
|
}
|
||
|
|
||
|
/* 244A50 -> 244B30 */
|
||
|
void set_identity_mat4(Mat4f *mtx) {
|
||
|
(*mtx)[0][0] = 1.0f;
|
||
|
(*mtx)[0][1] = 0.0f;
|
||
|
(*mtx)[0][2] = 0.0f;
|
||
|
(*mtx)[0][3] = 0.0f;
|
||
|
(*mtx)[1][0] = 0.0f;
|
||
|
(*mtx)[1][1] = 1.0f;
|
||
|
(*mtx)[1][2] = 0.0f;
|
||
|
(*mtx)[1][3] = 0.0f;
|
||
|
(*mtx)[2][0] = 0.0f;
|
||
|
(*mtx)[2][1] = 0.0f;
|
||
|
(*mtx)[2][2] = 1.0f;
|
||
|
(*mtx)[2][3] = 0.0f;
|
||
|
(*mtx)[3][0] = 0.0f;
|
||
|
(*mtx)[3][1] = 0.0f;
|
||
|
(*mtx)[3][2] = 0.0f;
|
||
|
(*mtx)[3][3] = 1.0f;
|
||
|
}
|
||
|
|
||
|
/* 244B30 -> 244C00 */
|
||
|
void cpy_mat4(const Mat4f *src, Mat4f *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];
|
||
|
}
|
||
|
|
||
|
/* 244C00 -> 244D10 */
|
||
|
void func_80196430(struct GdVec3f *vec, const Mat4f *mtx) {
|
||
|
struct GdVec3f dot;
|
||
|
|
||
|
dot.x = (*mtx)[0][0] * vec->x + (*mtx)[1][0] * vec->y + (*mtx)[2][0] * vec->z;
|
||
|
dot.y = (*mtx)[0][1] * vec->x + (*mtx)[1][1] * vec->y + (*mtx)[2][1] * vec->z;
|
||
|
dot.z = (*mtx)[0][2] * vec->x + (*mtx)[1][2] * vec->y + (*mtx)[2][2] * vec->z;
|
||
|
dot.x += (*mtx)[3][0];
|
||
|
dot.y += (*mtx)[3][1];
|
||
|
dot.z += (*mtx)[3][2];
|
||
|
|
||
|
vec->x = dot.x;
|
||
|
vec->y = dot.y;
|
||
|
vec->z = dot.z;
|
||
|
}
|
||
|
|
||
|
/* 244D10 -> 244DE4 */
|
||
|
void func_80196540(struct GdVec3f *vec, const Mat4f *mtx) {
|
||
|
struct GdVec3f dot;
|
||
|
|
||
|
dot.x = (*mtx)[0][0] * vec->x + (*mtx)[1][0] * vec->y + (*mtx)[2][0] * vec->z;
|
||
|
dot.y = (*mtx)[0][1] * vec->x + (*mtx)[1][1] * vec->y + (*mtx)[2][1] * vec->z;
|
||
|
dot.z = (*mtx)[0][2] * vec->x + (*mtx)[1][2] * vec->y + (*mtx)[2][2] * vec->z;
|
||
|
|
||
|
vec->x = dot.x;
|
||
|
vec->y = dot.y;
|
||
|
vec->z = dot.z;
|
||
|
}
|
||
|
|
||
|
#define MAT4_DOT_PROD(A, B, R, row, col) \
|
||
|
{ \
|
||
|
(R)[(row)][(col)] = (A)[(row)][0] * (B)[0][(col)]; \
|
||
|
(R)[(row)][(col)] += (A)[(row)][1] * (B)[1][(col)]; \
|
||
|
(R)[(row)][(col)] += (A)[(row)][2] * (B)[2][(col)]; \
|
||
|
(R)[(row)][(col)] += (A)[(row)][3] * (B)[3][(col)]; \
|
||
|
}
|
||
|
|
||
|
#define MAT4_MULTIPLY(A, B, R) \
|
||
|
{ \
|
||
|
MAT4_DOT_PROD((A), (B), (R), 0, 0); \
|
||
|
MAT4_DOT_PROD((A), (B), (R), 0, 1); \
|
||
|
MAT4_DOT_PROD((A), (B), (R), 0, 2); \
|
||
|
MAT4_DOT_PROD((A), (B), (R), 0, 3); \
|
||
|
MAT4_DOT_PROD((A), (B), (R), 1, 0); \
|
||
|
MAT4_DOT_PROD((A), (B), (R), 1, 1); \
|
||
|
MAT4_DOT_PROD((A), (B), (R), 1, 2); \
|
||
|
MAT4_DOT_PROD((A), (B), (R), 1, 3); \
|
||
|
MAT4_DOT_PROD((A), (B), (R), 2, 0); \
|
||
|
MAT4_DOT_PROD((A), (B), (R), 2, 1); \
|
||
|
MAT4_DOT_PROD((A), (B), (R), 2, 2); \
|
||
|
MAT4_DOT_PROD((A), (B), (R), 2, 3); \
|
||
|
MAT4_DOT_PROD((A), (B), (R), 3, 0); \
|
||
|
MAT4_DOT_PROD((A), (B), (R), 3, 1); \
|
||
|
MAT4_DOT_PROD((A), (B), (R), 3, 2); \
|
||
|
MAT4_DOT_PROD((A), (B), (R), 3, 3); \
|
||
|
}
|
||
|
|
||
|
/* 244DE4 -> 24575C; orig name: func_80196614 */
|
||
|
void multiply_mat4(const Mat4f *mA, const Mat4f *mB, Mat4f *dst) {
|
||
|
Mat4f res; // 18
|
||
|
|
||
|
MAT4_MULTIPLY((*mA), (*mB), res);
|
||
|
cpy_mat4(&res, dst);
|
||
|
}
|
||
|
|
||
|
#undef MAT4_MULTIPLY
|
||
|
#undef MAT4_DOT_PROD
|
||
|
|
||
|
/* 24575C -> 245778 */
|
||
|
void gd_print_vec(UNUSED const char *prefix, UNUSED const struct GdVec3f *vec) {
|
||
|
UNUSED u8 pad[8];
|
||
|
|
||
|
printf("%f,%f,%f\n", vec->x, vec->y, vec->z);
|
||
|
printf("\n");
|
||
|
}
|
||
|
|
||
|
/* 245778 -> 245794 */
|
||
|
void gd_print_plane(UNUSED const char *prefix, UNUSED const struct GdPlaneF *p) {
|
||
|
UNUSED u8 pad[8];
|
||
|
|
||
|
printf("Min X = %f, Max X = %f \n", p->p0.x, p->p1.x);
|
||
|
printf("Min Y = %f, Max Y = %f \n", p->p0.y, p->p1.y);
|
||
|
printf("Min Z = %f, Max Z = %f \n", p->p0.z, p->p1.z);
|
||
|
printf("\n");
|
||
|
}
|
||
|
|
||
|
/* 245794 -> 245838 */
|
||
|
void gd_print_mtx(UNUSED const char *prefix, const Mat4f *mtx) {
|
||
|
s32 i; // 1c
|
||
|
s32 j; // 18
|
||
|
|
||
|
for (i = 0; i < 4; i++) {
|
||
|
for (j = 0; j < 4; j++) {
|
||
|
gd_printf("%f ", (*mtx)[i][j]);
|
||
|
}
|
||
|
gd_printf("\n");
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/* 245838 -> 2458C0 */
|
||
|
void Unknown80197068(const char *prefix, const f32 *f) {
|
||
|
s32 i; // 1c
|
||
|
|
||
|
gd_printf(prefix);
|
||
|
for (i = 0; i < 4; i++) {
|
||
|
gd_printf("%f ", f[i]);
|
||
|
}
|
||
|
gd_printf("\n");
|
||
|
}
|
||
|
|
||
|
/* 2458C0 -> 245A48 */
|
||
|
void Unknown801970F0(Mat4f *dst, f32 x, f32 y, f32 z, s32 copy) {
|
||
|
f32 sp74 = 100.0f;
|
||
|
Mat4f sp34;
|
||
|
f32 sp30;
|
||
|
f32 sp2C;
|
||
|
f32 mag; // 28
|
||
|
f32 sp24;
|
||
|
struct GdVec3f sp18;
|
||
|
|
||
|
mag = gd_sqrt_f(SQ(x) + SQ(y) + SQ(z));
|
||
|
|
||
|
if (mag == 0.0f) {
|
||
|
if (copy) {
|
||
|
set_identity_mat4(dst);
|
||
|
}
|
||
|
return;
|
||
|
}
|
||
|
|
||
|
sp24 = gd_sqrt_f(SQ(sp74) + SQ(mag));
|
||
|
sp30 = sp74 / sp24;
|
||
|
sp2C = mag / sp24;
|
||
|
sp18.x = -y / mag;
|
||
|
sp18.y = -x / mag;
|
||
|
sp18.z = -z / mag;
|
||
|
|
||
|
func_80195FD4(&sp34, &sp18, sp2C, sp30);
|
||
|
if (!copy) {
|
||
|
multiply_mat4(dst, &sp34, dst);
|
||
|
} else {
|
||
|
cpy_mat4(&sp34, dst);
|
||
|
}
|
||
|
}
|