mirror/tww
1
0
Fork 0
mirror of https://github.com/zeldaret/tww.git synced 2026-08-04 09:08:39 -04:00
Code Issues Packages Projects Releases Wiki Activity

mtx from dolsdk2004 (#748)

Browse Source
This commit is contained in:
Jcw87
2025-05-05 17:55:42 -07:00
committed by GitHub
parent c033afc1f0
commit 137ce36539
11 changed files with 2109 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files
configure.py
+7 -9
View File
@@ -243,9 +243,7 @@ cflags_runtime = [
# Dolphin library flags
cflags_dolphin = [
*cflags_base,
"-use_lmw_stmw on",
"-str reuse,pool,readonly",
"-inline auto",
"-fp_contract off",
]
# Framework flags
@@ -275,7 +273,7 @@ def DolphinLib(lib_name: str, objects: List[Object]) -> Dict[str, Any]:
return {
"lib": lib_name,
"mw_version": "GC/1.2.5n",
"cflags": cflags_base,
"cflags": cflags_dolphin,
"progress_category": "sdk",
"host": False,
"objects": objects,
@@ -1075,11 +1073,11 @@ config.libs = [
DolphinLib(
"mtx",
[
Object(NonMatching, "dolphin/mtx/mtx.c"),
Object(NonMatching, "dolphin/mtx/mtxvec.c"),
Object(NonMatching, "dolphin/mtx/mtx44.c"),
Object(NonMatching, "dolphin/mtx/vec.c"),
Object(NonMatching, "dolphin/mtx/quat.c"),
Object(Matching, "dolphin/mtx/mtx.c"),
Object(Matching, "dolphin/mtx/mtxvec.c"),
Object(Matching, "dolphin/mtx/mtx44.c"),
Object(Matching, "dolphin/mtx/vec.c"),
Object(Matching, "dolphin/mtx/quat.c"),
],
),
DolphinLib(
include/dolphin/mtx/mtx.h
+16 -22
View File
@@ -16,18 +16,30 @@ typedef f32 (*MtxP)[4];
typedef f32 (*Mtx3P)[3];
typedef const f32 (*CMtxP)[4]; // Change name later?
void C_MTXIdentity(Mtx m);
void PSMTXIdentity(Mtx m);
void C_MTXCopy(const Mtx src, Mtx dst);
void PSMTXCopy(const Mtx src, Mtx dst);
void C_MTXConcat(const Mtx a, const Mtx b, Mtx ab);
void PSMTXConcat(const Mtx a, const Mtx b, Mtx ab);
u32 C_MTXInverse(const Mtx src, Mtx inv);
u32 PSMTXInverse(const Mtx src, Mtx inv);
void C_MTXRotRad(Mtx m, u8 axis, f32 rad);
void PSMTXRotRad(Mtx m, u8 axis, f32 rad);
void C_MTXRotTrig(Mtx m, u8 axis, f32 sin, f32 cos);
void PSMTXRotTrig(Mtx m, u8 axis, f32 sin, f32 cos);
f64 __PSMTXRotAxisRadInternal(f64 param_1, f64 param_2, int param_3, int param_4);
void C_MTXRotAxisRad(Mtx m, const Vec* axis, f32 rad);
void __PSMTXRotAxisRadInternal(Mtx m, const Vec* axis, f32 sT, f32 cT);
void PSMTXRotAxisRad(Mtx m, const Vec* axis, f32 rad);
void C_MTXTrans(Mtx m, f32 x, f32 y, f32 z);
void PSMTXTrans(Mtx m, f32 x, f32 y, f32 z);
void C_MTXTransApply(const Mtx src, Mtx dst, f32 x, f32 y, f32 z);
void PSMTXTransApply(const Mtx src, Mtx dst, f32 x, f32 y, f32 z);
void C_MTXScale(Mtx m, f32 x, f32 y, f32 z);
void PSMTXScale(Mtx m, f32 x, f32 y, f32 z);
void C_MTXScaleApply(const Mtx src, Mtx dst, f32 x, f32 y, f32 z);
void PSMTXScaleApply(const Mtx src, Mtx dst, f32 x, f32 y, f32 z);
void C_MTXQuat(Mtx m, const Quaternion* q);
void PSMTXQuat(Mtx m, const Quaternion* q);
void C_MTXLookAt(Mtx m, const Vec* camPos, const Vec* camUp, const Vec* target);
@@ -36,15 +48,12 @@ void C_MTXLightPerspective(Mtx m, f32 fovY, f32 aspect, f32 scale_s, f32 scale_t
void C_MTXLightOrtho(Mtx m, f32 top, f32 bottom, f32 left, f32 right, f32 scale_s, f32 scale_t,
f32 trans_s, f32 trans_t);
void C_MTXRotAxisRad(Mtx m, const Vec* axis, f32 rad);
#define MTXDegToRad(deg) ((deg) * 0.01745329252f)
#define MTXRadToDeg(rad) ((rad) * 57.29577951f)
/* When compiling in debug mode, use C implementations */
#ifdef DEBUG
// TODO: Add debug rom C implementations
/* #define MTXIdentity C_MTXIdentity
#if defined(DEBUG) || !defined(__MWERKS__)
#define MTXIdentity C_MTXIdentity
#define MTXCopy C_MTXCopy
#define MTXConcat C_MTXConcat
#define MTXInverse C_MTXInverse
@@ -55,22 +64,7 @@ void C_MTXRotAxisRad(Mtx m, const Vec* axis, f32 rad);
#define MTXTransApply C_MTXTransApply
#define MTXScale C_MTXScale
#define MTXScaleApply C_MTXScaleApply
#define MTXQuat C_MTXQuat */
// Temporary until the C implementations are done
#define MTXIdentity PSMTXIdentity
#define MTXCopy PSMTXCopy
#define MTXConcat PSMTXConcat
#define MTXInverse PSMTXInverse
#define MTXRotRad PSMTXRotRad
#define MTXRotTrig PSMTXRotTrig
#define MTXRotAxisRad PSMTXRotAxisRad
#define MTXTrans PSMTXTrans
#define MTXTransApply PSMTXTransApply
#define MTXScale PSMTXScale
#define MTXScaleApply PSMTXScaleApply
#define MTXQuat PSMTXQuat
#define MTXQuat C_MTXQuat
#else
#define MTXIdentity PSMTXIdentity
#define MTXCopy PSMTXCopy
include/dolphin/mtx/mtxvec.h
+8 -12
View File
@@ -7,25 +7,21 @@
extern "C" {
#endif
void C_MTXMultVec(const Mtx m, const Vec* src, Vec* dst);
void PSMTXMultVec(const Mtx m, const Vec* src, Vec* dst);
void PSMTXMultVecSR(const Mtx m, const Vec* src, Vec* dst);
void C_MTXMultVecArray(const Mtx m, const Vec* srcBase, Vec* dstBase, u32 count);
void PSMTXMultVecArray(const Mtx m, const Vec* srcBase, Vec* dstBase, u32 count);
void C_MTXMultVecSR(const Mtx m, const Vec* src, Vec* dst);
void PSMTXMultVecSR(const Mtx m, const Vec* src, Vec* dst);
void C_MTXMultVecArraySR(const Mtx m, const Vec* srcBase, Vec* dstBase, u32 count);
void PSMTXMultVecArraySR(const Mtx m, const Vec* srcBase, Vec* dstBase, u32 count);
/* When compiling in debug mode, use C implementations */
#ifdef DEBUG
// TODO: Add debug rom C implementations
/* #define MTXMultVec C_MTXMultVec
#if defined(DEBUG) || !defined(__MWERKS__)
#define MTXMultVec C_MTXMultVec
#define MTXMultVecSR C_MTXMultVecSR
#define MTXMultVecArray C_MTXMultVecArray
#define MTXMultVecArraySR C_MTXMultVecArraySR */
// Temporary until the C implementations are done
#define MTXMultVec PSMTXMultVec
#define MTXMultVecSR PSMTXMultVecSR
#define MTXMultVecArray PSMTXMultVecArray
#define MTXMultVecArraySR PSMTXMultVecArraySR
#define MTXMultVecArraySR C_MTXMultVecArraySR
#else
#define MTXMultVec PSMTXMultVec
#define MTXMultVecSR PSMTXMultVecSR
include/dolphin/mtx/quat.h
+3 -7
View File
@@ -11,18 +11,14 @@ typedef struct Quaternion {
f32 x, y, z, w;
} Quaternion;
void C_QUATMultiply(const Quaternion* a, const Quaternion* b, Quaternion* ab);
void PSQUATMultiply(const Quaternion* a, const Quaternion* b, Quaternion* ab);
void C_QUATRotAxisRad(Quaternion* q, const Vec* axis, f32 rad);
void C_QUATSlerp(const Quaternion* p, const Quaternion* q, Quaternion* r, f32 t);
/* When compiling in debug mode, use C implementations */
#ifdef DEBUG
// TODO: Add debug rom C implementations
/* #define QUATMultiply C_QUATMultiply */
// Temporary until the C implementations are done
#define QUATMultiply PSQUATMultiply
#if defined(DEBUG) || !defined(__MWERKS__)
#define QUATMultiply C_QUATMultiply
#else
#define QUATMultiply PSQUATMultiply
#endif
include/dolphin/mtx/vec.h
+11 -17
View File
@@ -19,36 +19,30 @@ typedef struct SVec {
s16 x, y, z;
} SVec;
void C_VECAdd(const Vec* a, const Vec* b, Vec* ab);
void PSVECAdd(const Vec* a, const Vec* b, Vec* ab);
void C_VECSubtract(const Vec* a, const Vec* b, Vec* a_b);
void PSVECSubtract(const Vec* a, const Vec* b, Vec* a_b);
void C_VECScale(const Vec* src, Vec* dst, f32 scale);
void PSVECScale(const Vec* src, Vec* dst, f32 scale);
void C_VECNormalize(const Vec* src, Vec* unit);
void PSVECNormalize(const Vec* src, Vec* unit);
f32 C_VECSquareMag(const Vec* v);
f32 PSVECSquareMag(const Vec* v);
f32 C_VECMag(const Vec* v);
f32 PSVECMag(const Vec* v);
f32 C_VECDotProduct(const Vec* a, const Vec* b);
f32 PSVECDotProduct(const Vec* a, const Vec* b);
void C_VECCrossProduct(const Vec* a, const Vec* b, Vec* axb);
void PSVECCrossProduct(const Vec* a, const Vec* b, Vec* axb);
f32 C_VECSquareDistance(const Vec* a, const Vec* b);
f32 PSVECSquareDistance(const Vec* a, const Vec* b);
f32 C_VECDistance(const Vec* a, const Vec* b);
f32 PSVECDistance(const Vec* a, const Vec* b);
void C_VECHalfAngle(const Vec* a, const Vec* b, Vec* half);
void C_VECReflect(const Vec* src, const Vec* normal, Vec* dst);
inline void C_VECAdd(register const Vec* a, register const Vec* b, register Vec* ab) {
ab->x = a->x + b->x;
ab->y = a->y + b->y;
ab->z = a->z + b->z;
}
inline void C_VECSubtract(register const Vec* a, register const Vec* b, register Vec* ab) {
ab->x = a->x - b->x;
ab->y = a->y - b->y;
ab->z = a->z - b->z;
}
inline f32 C_VECSquareMag(const Vec* v) {
return v->x * v->x + v->y * v->y + v->z * v->z;
}
inline BOOL checkScaleOne(Vec v) {
if (v.x == 1.0f && v.y == 1.0f && v.z == 1.0f) {
return TRUE;
@@ -58,7 +52,7 @@ inline BOOL checkScaleOne(Vec v) {
}
/* When compiling in debug mode, use C implementations */
#if 0
#if defined(DEBUG) || !defined(__MWERKS__)
#define VECAdd C_VECAdd
#define VECSubtract C_VECSubtract
#define VECScale C_VECScale
src/PowerPC_EABI_Support/MSL/MSL_C/MSL_Common/Include/math.h
+6
View File
@@ -79,7 +79,12 @@ inline float atan2f(float y, float x) { return (float)atan2(y, x); }
inline float sinf(float x) { return sin(x); }
inline float cosf(float x) { return cos(x); }
inline float tanf(float x) { return tan(x); }
}; // namespace std
#endif
#ifdef __cplusplus
namespace std {
#endif
extern inline float sqrtf(float x) {
const double _half = .5;
const double _three = 3.0;
@@ -94,6 +99,7 @@ extern inline float sqrtf(float x) {
}
return x;
}
#ifdef __cplusplus
}; // namespace std
#endif
src/dolphin/mtx/mtx.c
+1165
View File
File diff suppressed because it is too large Load Diff
src/dolphin/mtx/mtx44.c
+92
View File
@@ -0,0 +1,92 @@
#include <dolphin/mtx/mtx.h>
#include <dolphin/mtx/mtx44.h>
#include <dolphin/os/OS.h>
#include <math.h>
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;
}
src/dolphin/mtx/mtxvec.c
+212
View File
@@ -0,0 +1,212 @@
#include <dolphin/mtx/mtxvec.h>
#include <dolphin/os/OS.h>
void C_MTXMultVec(const Mtx m, const Vec* src, Vec* dst) {
Vec vTmp;
ASSERTMSGLINE(66, m, "MTXMultVec(): NULL MtxPtr 'm' ");
ASSERTMSGLINE(67, src, "MTXMultVec(): NULL VecPtr 'src' ");
ASSERTMSGLINE(68, dst, "MTXMultVec(): NULL VecPtr 'dst' ");
vTmp.x = m[0][3] + ((m[0][2] * src->z) + ((m[0][0] * src->x) + (m[0][1] * src->y)));
vTmp.y = m[1][3] + ((m[1][2] * src->z) + ((m[1][0] * src->x) + (m[1][1] * src->y)));
vTmp.z = m[2][3] + ((m[2][2] * src->z) + ((m[2][0] * src->x) + (m[2][1] * src->y)));
dst->x = vTmp.x;
dst->y = vTmp.y;
dst->z = vTmp.z;
}
#ifdef __MWERKS__
asm void PSMTXMultVec(const register Mtx m, const register Vec* src, register Vec* dst) {
nofralloc
psq_l f0, Vec.x(src), 0, 0
psq_l f2, 0(m), 0, 0
psq_l f1, Vec.z(src), 1, 0
ps_mul f4, f2, f0
psq_l f3, 8(m), 0, 0
ps_madd f5, f3, f1, f4
psq_l f8, 16(m), 0, 0
ps_sum0 f6, f5, f6, f5
psq_l f9, 24(m), 0, 0
ps_mul f10, f8, f0
psq_st f6, Vec.x(dst), 1, 0
ps_madd f11, f9, f1, f10
psq_l f2, 32(m), 0, 0
ps_sum0 f12, f11, f12, f11
psq_l f3, 40(m), 0, 0
ps_mul f4, f2, f0
psq_st f12, Vec.y(dst), 1, 0
ps_madd f5, f3, f1, f4
ps_sum0 f6, f5, f6, f5
psq_st f6, Vec.z(dst), 1, 0
blr
}
#endif
void C_MTXMultVecArray(const Mtx m, const Vec* srcBase, Vec* dstBase, u32 count) {
u32 i;
Vec vTmp;
ASSERTMSGLINE(168, m, "MTXMultVecArray(): NULL MtxPtr 'm' ");
ASSERTMSGLINE(169, srcBase, "MTXMultVecArray(): NULL VecPtr 'srcBase' ");
ASSERTMSGLINE(170, dstBase, "MTXMultVecArray(): NULL VecPtr 'dstBase' ");
ASSERTMSGLINE(171, count > 1, "MTXMultVecArray(): count must be greater than 1.");
for(i = 0; i < count; i++) {
vTmp.x = m[0][3] + ((m[0][2] * srcBase->z) + ((m[0][0] * srcBase->x) + (m[0][1] * srcBase->y)));
vTmp.y = m[1][3] + ((m[1][2] * srcBase->z) + ((m[1][0] * srcBase->x) + (m[1][1] * srcBase->y)));
vTmp.z = m[2][3] + ((m[2][2] * srcBase->z) + ((m[2][0] * srcBase->x) + (m[2][1] * srcBase->y)));
dstBase->x = vTmp.x;
dstBase->y = vTmp.y;
dstBase->z = vTmp.z;
srcBase++;
dstBase++;
}
}
#ifdef __MWERKS__
asm void PSMTXMultVecArray(const register Mtx m, const register Vec* srcBase, register Vec* dstBase, register u32 count) {
nofralloc
psq_l f13, 0x0(m), 0, 0
psq_l f12, 0x10(m), 0, 0
subi count, count, 0x1
psq_l f11, 0x8(m), 0, 0
ps_merge00 f0, f13, f12
subi dstBase, dstBase, 0x4
psq_l f10, 0x18(m), 0, 0
ps_merge11 f1, f13, f12
mtctr count
psq_l f4, 0x20(m), 0, 0
ps_merge00 f2, f11, f10
psq_l f5, 0x28(m), 0, 0
ps_merge11 f3, f11, f10
psq_l f6, 0x0(srcBase), 0, 0
psq_lu f7, 0x8(srcBase), 1, 0
ps_madds0 f8, f0, f6, f3
ps_mul f9, f4, f6
ps_madds1 f8, f1, f6, f8
ps_madd f10, f5, f7, f9
L_000003C4:
psq_lu f6, 0x4(srcBase), 0, 0
ps_madds0 f12, f2, f7, f8
psq_lu f7, 0x8(srcBase), 1, 0
ps_sum0 f13, f10, f9, f10
ps_madds0 f8, f0, f6, f3
ps_mul f9, f4, f6
psq_stu f12, 0x4(dstBase), 0, 0
ps_madds1 f8, f1, f6, f8
psq_stu f13, 0x8(dstBase), 1, 0
ps_madd f10, f5, f7, f9
bdnz L_000003C4
ps_madds0 f12, f2, f7, f8
ps_sum0 f13, f10, f9, f10
psq_stu f12, 0x4(dstBase), 0, 0
psq_stu f13, 0x8(dstBase), 1, 0
blr
}
#endif
void C_MTXMultVecSR(const Mtx m, const Vec* src, Vec* dst) {
Vec vTmp;
ASSERTMSGLINE(313, m, "MTXMultVecSR(): NULL MtxPtr 'm' ");
ASSERTMSGLINE(314, src, "MTXMultVecSR(): NULL VecPtr 'src' ");
ASSERTMSGLINE(315, dst, "MTXMultVecSR(): NULL VecPtr 'dst' ");
vTmp.x = (m[0][2] * src->z) + ((m[0][0] * src->x) + (m[0][1] * src->y));
vTmp.y = (m[1][2] * src->z) + ((m[1][0] * src->x) + (m[1][1] * src->y));
vTmp.z = (m[2][2] * src->z) + ((m[2][0] * src->x) + (m[2][1] * src->y));
dst->x = vTmp.x;
dst->y = vTmp.y;
dst->z = vTmp.z;
}
#ifdef __MWERKS__
asm void PSMTXMultVecSR(const register Mtx m, const register Vec* src, register Vec* dst) {
nofralloc
psq_l f0, 0x0(m), 0, 0
psq_l f6, 0x0(src), 0, 0
psq_l f2, 0x10(m), 0, 0
ps_mul f8, f0, f6
psq_l f4, 0x20(m), 0, 0
ps_mul f10, f2, f6
psq_l f7, 0x8(src), 1, 0
ps_mul f12, f4, f6
psq_l f3, 0x18(m), 0, 0
ps_sum0 f8, f8, f8, f8
psq_l f5, 0x28(m), 0, 0
ps_sum0 f10, f10, f10, f10
psq_l f1, 0x8(m), 0, 0
ps_sum0 f12, f12, f12, f12
ps_madd f9, f1, f7, f8
psq_st f9, 0x0(dst), 1, 0
ps_madd f11, f3, f7, f10
psq_st f11, 0x4(dst), 1, 0
ps_madd f13, f5, f7, f12
psq_st f13, 0x8(dst), 1, 0
blr
}
#endif
void C_MTXMultVecArraySR(const Mtx m, const Vec* srcBase, Vec* dstBase, u32 count) {
u32 i;
Vec vTmp;
ASSERTMSGLINE(410, m, "MTXMultVecArraySR(): NULL MtxPtr 'm' ");
ASSERTMSGLINE(411, srcBase, "MTXMultVecArraySR(): NULL VecPtr 'srcBase' ");
ASSERTMSGLINE(412, dstBase, "MTXMultVecArraySR(): NULL VecPtr 'dstBase' ");
ASSERTMSGLINE(413, count > 1, "MTXMultVecArraySR(): count must be greater than 1.");
for(i = 0; i < count; i++) {
vTmp.x = (m[0][2] * srcBase->z) + ((m[0][0] * srcBase->x) + (m[0][1] * srcBase->y));
vTmp.y = (m[1][2] * srcBase->z) + ((m[1][0] * srcBase->x) + (m[1][1] * srcBase->y));
vTmp.z = (m[2][2] * srcBase->z) + ((m[2][0] * srcBase->x) + (m[2][1] * srcBase->y));
dstBase->x = vTmp.x;
dstBase->y = vTmp.y;
dstBase->z = vTmp.z;
srcBase++;
dstBase++;
}
}
#ifdef __MWERKS__
asm void PSMTXMultVecArraySR(const register Mtx m, const register Vec* srcBase, register Vec* dstBase, register u32 count) {
nofralloc
psq_l f13, 0x0(m), 0, 0
psq_l f12, 0x10(m), 0, 0
subi count, count, 0x1
psq_l f11, 0x8(m), 1, 0
ps_merge00 f0, f13, f12
subi dstBase, dstBase, 0x4
psq_l f10, 0x18(m), 1, 0
ps_merge11 f1, f13, f12
mtctr count
psq_l f3, 0x20(m), 0, 0
ps_merge00 f2, f11, f10
psq_l f4, 0x28(m), 1, 0
psq_l f6, 0x0(srcBase), 0, 0
psq_lu f7, 0x8(srcBase), 1, 0
ps_muls0 f8, f0, f6
ps_mul f9, f3, f6
ps_madds1 f8, f1, f6, f8
ps_madd f10, f4, f7, f9
L_000007D0:
psq_lu f6, 0x4(srcBase), 0, 0
ps_madds0 f12, f2, f7, f8
psq_lu f7, 0x8(srcBase), 1, 0
ps_sum0 f13, f10, f9, f9
ps_muls0 f8, f0, f6
ps_mul f9, f3, f6
psq_stu f12, 0x4(dstBase), 0, 0
ps_madds1 f8, f1, f6, f8
psq_stu f13, 0x8(dstBase), 1, 0
ps_madd f10, f4, f7, f9
bdnz L_000007D0
ps_madds0 f12, f2, f7, f8
ps_sum0 f13, f10, f9, f9
psq_stu f12, 0x4(dstBase), 0, 0
psq_stu f13, 0x8(dstBase), 1, 0
blr
}
#endif
src/dolphin/mtx/quat.c
+224
View File
@@ -0,0 +1,224 @@
#include <dolphin/mtx/quat.h>
#include <dolphin/os/OS.h>
#include <math.h>
void C_QUATMultiply(const Quaternion* p, const Quaternion* q, Quaternion* pq) {
Quaternion* r;
Quaternion pqTmp;
ASSERTMSGLINE(193, p, "QUATMultiply(): NULL QuaternionPtr 'p' ");
ASSERTMSGLINE(194, q, "QUATMultiply(): NULL QuaternionPtr 'q' ");
ASSERTMSGLINE(195, pq, "QUATMultiply(): NULL QuaternionPtr 'pq' ");
if (p == pq || q == pq){
r = &pqTmp;
} else {
r = pq;
}
r->w = (p->w * q->w) - (p->x * q->x) - (p->y * q->y) - (p->z * q->z);
r->x = (p->w * q->x) + (p->x * q->w) + (p->y * q->z) - (p->z * q->y);
r->y = (p->w * q->y) + (p->y * q->w) + (p->z * q->x) - (p->x * q->z);
r->z = (p->w * q->z) + (p->z * q->w) + (p->x * q->y) - (p->y * q->x);
if (r == &pqTmp) {
*pq = pqTmp;
}
}
#ifdef __MWERKS__
void PSQUATMultiply(const register Quaternion* p, const register Quaternion* q, register Quaternion* pq) {
register f32 pxy, pzw;
register f32 qxy, qzw;
register f32 pnxy, pnzw, pnxny, pnznw;
register f32 rxy, rzw;
register f32 sxy, szw;
asm {
psq_l pxy, 0x0(p), 0, 0
psq_l pzw, 0x8(p), 0, 0
psq_l qxy, 0x0(q), 0, 0
ps_neg pnxny, pxy
psq_l qzw, 0x8(q), 0, 0
ps_neg pnznw, pzw
ps_merge01 pnxy, pnxny, pxy
ps_muls0 rxy, pzw, qxy
ps_muls0 rzw, pnxny, qxy
ps_merge01 pnzw, pnznw, pzw
ps_muls1 szw, pnxy, qxy
ps_madds0 rxy, pnxy, qzw, rxy
ps_muls1 sxy, pnzw, qxy
ps_madds0 rzw, pnzw, qzw, rzw
ps_madds1 szw, pnznw, qzw, szw
ps_merge10 rxy, rxy, rxy
ps_madds1 sxy, pxy, qzw, sxy
ps_merge10 rzw, rzw, rzw
ps_add rxy, rxy, sxy
psq_st rxy, 0x0(pq), 0, 0
ps_sub rzw, rzw, szw
psq_st rzw, 0x8(pq), 0, 0
}
}
#endif
void C_QUATNormalize(const Quaternion* src, Quaternion* unit) {
f32 mag;
ASSERTMSGLINE(407, src, "QUATNormalize(): NULL QuaternionPtr 'src' ");
ASSERTMSGLINE(408, unit, "QUATNormalize(): NULL QuaternionPtr 'unit' ");
mag = (src->x * src->x) + (src->y * src->y) + (src->z * src->z) + (src->w * src->w);
if (mag >= 0.00001f) {
mag = 1.0f / sqrtf(mag);
unit->x = src->x * mag;
unit->y = src->y * mag;
unit->z = src->z * mag;
unit->w = src->w * mag;
} else {
unit->x = unit->y = unit->z = unit->w = 0.0f;
}
}
#ifdef __MWERKS__
void PSQUATNormalize(const register Quaternion* src, register Quaternion* unit) {
register f32 sxy, szw;
register f32 mag, rsqmag;
register f32 diff;
register f32 c_zero;
register f32 nwork0, nwork1;
register f32 epsilon = 0.00001f;
register f32 c_half = 0.5f;
register f32 c_three = 3.0f;
asm {
psq_l sxy, 0x0(src), 0, 0
ps_mul mag, sxy, sxy
psq_l szw, 0x8(src), 0, 0
ps_sub c_zero, epsilon, epsilon
ps_madd mag, szw, szw, mag
ps_sum0 mag, mag, mag, mag
frsqrte rsqmag, mag
ps_sub diff, mag, epsilon
fmul nwork0, rsqmag, rsqmag
fmul nwork1, rsqmag, c_half
fnmsub nwork0, nwork0, mag, c_three
fmul rsqmag, nwork0, nwork1
ps_sel rsqmag, diff, rsqmag, c_zero
ps_muls0 sxy, sxy, rsqmag
ps_muls0 szw, szw, rsqmag
psq_st sxy, 0x0(unit), 0, 0
psq_st szw, 0x8(unit), 0, 0
}
}
#endif
void C_QUATInverse(const Quaternion* src, Quaternion* inv) {
f32 mag, norminv;
ASSERTMSGLINE(498, src, "QUATInverse(): NULL QuaternionPtr 'src' ");
ASSERTMSGLINE(499, inv, "QUATInverse(): NULL QuaternionPtr 'inv' ");
mag = (src->x * src->x) + (src->y * src->y) + (src->z * src->z) + (src->w * src->w);
if (mag == 0.0f) {
mag = 1.0f;
}
norminv = 1.0f / mag;
inv->x = -src->x * norminv;
inv->y = -src->y * norminv;
inv->z = -src->z * norminv;
inv->w = src->w * norminv;
}
#ifdef __MWERKS__
void PSQUATInverse(const register Quaternion* src, register Quaternion* inv) {
register f32 sxy, szw;
register f32 izz, iww;
register f32 mag, nmag;
register f32 norminv, nninv;
register f32 nwork0;
register f32 c_two;
register f32 c_zero;
register f32 c_one = 1.0f;
asm {
psq_l sxy, 0x0(src), 0, 0
ps_mul mag, sxy, sxy
ps_sub c_zero, c_one, c_one
psq_l szw, 0x8(src), 0, 0
ps_madd mag, szw, szw, mag
ps_add c_two, c_one, c_one
ps_sum0 mag, mag, mag, mag
fcmpu cr0, mag, c_zero
beq L_00000948
fres norminv, mag
ps_neg nmag, mag
ps_nmsub nwork0, mag, norminv, c_two
ps_mul norminv, norminv, nwork0
b L_0000094C
L_00000948:
fmr norminv, c_one
L_0000094C:
ps_neg nninv, norminv
ps_muls1 iww, norminv, szw
ps_muls0 sxy, sxy, nninv
psq_st iww, 0xc(inv), 1, 0
ps_muls0 izz, szw, nninv
psq_st sxy, 0x0(inv), 0, 0
psq_st izz, 0x8(inv), 1, 0
}
}
#endif
void C_QUATRotAxisRad(Quaternion* r, const Vec* axis, f32 rad) {
f32 half, sh, ch;
Vec nAxis;
ASSERTMSGLINE(758, r, "QUATRotAxisRad(): NULL QuaternionPtr 'r' ");
ASSERTMSGLINE(759, axis, "QUATRotAxisRad(): NULL VecPtr 'axis' ");
VECNormalize(axis, &nAxis);
half = rad * 0.5f;
sh = sinf(half);
ch = cosf(half);
r->x = sh * nAxis.x;
r->y = sh * nAxis.y;
r->z = sh * nAxis.z;
r->w = ch;
}
void C_QUATSlerp(const Quaternion* p, const Quaternion* q, Quaternion* r, f32 t) {
f32 theta, sin_th, cos_th;
f32 tp, tq;
ASSERTMSGLINE(869, p, "QUATSlerp(): NULL QuaternionPtr 'p' ");
ASSERTMSGLINE(870, q, "QUATSlerp(): NULL QuaternionPtr 'q' ");
ASSERTMSGLINE(871, r, "QUATSlerp(): NULL QuaternionPtr 'r' ");
cos_th = p->x * q->x + p->y * q->y + p->z * q->z + p->w * q->w;
tq = 1.0f;
if (cos_th < 0.0f) {
cos_th = -cos_th;
tq = -tq;
}
if (cos_th <= 0.99999f) {
theta = acosf(cos_th);
sin_th = sinf(theta);
tp = sinf((1.0f - t) * theta) / sin_th;
tq *= sinf(t * theta) / sin_th;
} else {
tp = 1.0f - t;
tq *= t;
}
r->x = (tp * p->x) + (tq * q->x);
r->y = (tp * p->y) + (tq * q->y);
r->z = (tp * p->z) + (tq * q->z);
r->w = (tp * p->w) + (tq * q->w);
}
src/dolphin/mtx/vec.c
+365
View File
@@ -0,0 +1,365 @@
#include <dolphin/mtx/vec.h>
#include <dolphin/os/OS.h>
#include <math.h>
void C_VECAdd(const Vec* a, const Vec* b, Vec* ab) {
ASSERTMSGLINE(108, a, "VECAdd(): NULL VecPtr 'a' ");
ASSERTMSGLINE(109, b, "VECAdd(): NULL VecPtr 'b' ");
ASSERTMSGLINE(110, ab, "VECAdd(): NULL VecPtr 'ab' ");
ab->x = a->x + b->x;
ab->y = a->y + b->y;
ab->z = a->z + b->z;
}
#ifdef __MWERKS__
asm void PSVECAdd(const register Vec* a, const register Vec* b, register Vec* ab) {
psq_l f2, Vec.x(a), 0, 0
psq_l f4, Vec.x(b), 0, 0
ps_add f6, f2, f4
psq_st f6, Vec.x(ab), 0, 0
psq_l f3, Vec.z(a), 1, 0
psq_l f5, Vec.z(b), 1, 0
ps_add f7, f3, f5
psq_st f7, Vec.z(ab), 1, 0
}
#endif
void C_VECSubtract(const Vec* a, const Vec* b, Vec* a_b) {
ASSERTMSGLINE(177, a, "VECSubtract(): NULL VecPtr 'a' ");
ASSERTMSGLINE(178, b, "VECSubtract(): NULL VecPtr 'b' ");
ASSERTMSGLINE(179, a_b, "VECSubtract(): NULL VecPtr 'a_b' ");
a_b->x = a->x - b->x;
a_b->y = a->y - b->y;
a_b->z = a->z - b->z;
}
#ifdef __MWERKS__
asm void PSVECSubtract(const register Vec* a, const register Vec* b, register Vec* a_b) {
psq_l f2, Vec.x(a), 0, 0
psq_l f4, Vec.x(b), 0, 0
ps_sub f6, f2, f4
psq_st f6, Vec.x(a_b), 0, 0
psq_l f3, Vec.z(a), 1, 0
psq_l f5, Vec.z(b), 1, 0
ps_sub f7, f3, f5
psq_st f7, Vec.z(a_b), 1, 0
}
#endif
void C_VECScale(const Vec* src, Vec* dst, f32 scale) {
ASSERTMSGLINE(247, src, "VECScale(): NULL VecPtr 'src' ");
ASSERTMSGLINE(248, dst, "VECScale(): NULL VecPtr 'dst' ");
dst->x = (src->x * scale);
dst->y = (src->y * scale);
dst->z = (src->z * scale);
}
#ifdef __MWERKS__
void PSVECScale(const register Vec* src, register Vec* dst, register f32 scale) {
register f32 vxy, vz, rxy, rz;
asm {
psq_l vxy, 0x0(src), 0, 0
psq_l vz, 0x8(src), 1, 0
ps_muls0 rxy, vxy, scale
psq_st rxy, 0x0(dst), 0, 0
ps_muls0 rz, vz, scale
psq_st rz, 0x8(dst), 1, 0
}
}
#endif
void C_VECNormalize(const Vec* src, Vec* unit) {
f32 mag;
ASSERTMSGLINE(315, src, "VECNormalize(): NULL VecPtr 'src' ");
ASSERTMSGLINE(316, unit, "VECNormalize(): NULL VecPtr 'unit' ");
mag = (src->z * src->z) + ((src->x * src->x) + (src->y * src->y));
ASSERTMSGLINE(321, 0.0f != mag, "VECNormalize(): zero magnitude vector ");
mag = 1.0f / sqrtf(mag);
unit->x = src->x * mag;
unit->y = src->y * mag;
unit->z = src->z * mag;
}
#ifdef __MWERKS__
void PSVECNormalize(const register Vec* src, register Vec* unit) {
register float c_half = 0.5f;
register float c_three = 3.0f;
register float v1_xy;
register float v1_z;
register float xx_zz;
register float xx_yy;
register float sqsum;
register float rsqrt;
register float nwork0;
register float nwork1;
asm {
psq_l v1_xy, 0x0(src), 0, 0
ps_mul xx_yy, v1_xy, v1_xy
psq_l v1_z, 0x8(src), 1, 0
ps_madd xx_zz, v1_z, v1_z, xx_yy
ps_sum0 sqsum, xx_zz, v1_z, xx_yy
frsqrte rsqrt, sqsum
fmuls nwork0, rsqrt, rsqrt
fmuls nwork1, rsqrt, c_half
fnmsubs nwork0, nwork0, sqsum, c_three
fmuls rsqrt, nwork0, nwork1
ps_muls0 v1_xy, v1_xy, rsqrt
psq_st v1_xy, 0x0(unit), 0, 0
ps_muls0 v1_z, v1_z, rsqrt
psq_st v1_z, 0x8(unit), 1, 0
}
}
#endif
f32 C_VECSquareMag(const Vec* v) {
f32 sqmag;
ASSERTMSGLINE(405, v, "VECMag(): NULL VecPtr 'v' ");
sqmag = v->z * v->z + ((v->x * v->x) + (v->y * v->y));
return sqmag;
}
#ifdef __MWERKS__
f32 PSVECSquareMag(const register Vec* v) {
register f32 vxy, vzz, sqmag;
asm {
psq_l vxy, 0x0(v), 0, 0
ps_mul vxy, vxy, vxy
lfs vzz, 0x8(v)
ps_madd sqmag, vzz, vzz, vxy
ps_sum0 sqmag, sqmag, vxy, vxy
}
return sqmag;
}
#endif
f32 C_VECMag(const Vec* v) {
return sqrtf(C_VECSquareMag(v));
}
#ifdef __MWERKS__
f32 PSVECMag(const register Vec* v) {
register f32 vxy, vzz;
register f32 sqmag, rmag;
register f32 nwork0, nwork1;
register f32 c_three, c_half, c_zero;
c_half = 0.5f;
asm {
psq_l vxy, 0x0(v), 0, 0
ps_mul vxy, vxy, vxy
lfs vzz, 0x8(v)
fsubs c_zero, c_half, c_half
ps_madd sqmag, vzz, vzz, vxy
ps_sum0 sqmag, sqmag, vxy, vxy
fcmpu cr0, sqmag, c_zero
beq L_000005F0
frsqrte rmag, sqmag
}
c_three = 3.0f;
asm {
fmuls nwork0, rmag, rmag
fmuls nwork1, rmag, c_half
fnmsubs nwork0, nwork0, sqmag, c_three
fmuls rmag, nwork0, nwork1
fmuls sqmag, sqmag, rmag
L_000005F0:
}
return sqmag;
}
#endif
f32 C_VECDotProduct(const Vec* a, const Vec* b) {
f32 dot;
ASSERTMSGLINE(540, a, "VECDotProduct(): NULL VecPtr 'a' ");
ASSERTMSGLINE(541, b, "VECDotProduct(): NULL VecPtr 'b' ");
dot = (a->z * b->z) + ((a->x * b->x) + (a->y * b->y));
return dot;
}
#ifdef __MWERKS__
asm f32 PSVECDotProduct(const register Vec* a, const register Vec* b) {
psq_l f2, Vec.y(a), 0, 0
psq_l f3, Vec.y(b), 0, 0
ps_mul f2, f2, f3
psq_l f5, Vec.x(a), 0, 0
psq_l f4, Vec.x(b), 0, 0
ps_madd f3, f5, f4, f2
ps_sum0 f1, f3, f2, f2
}
#endif
void C_VECCrossProduct(const Vec* a, const Vec* b, Vec* axb) {
Vec vTmp;
ASSERTMSGLINE(602, a, "VECCrossProduct(): NULL VecPtr 'a' ");
ASSERTMSGLINE(603, b, "VECCrossProduct(): NULL VecPtr 'b' ");
ASSERTMSGLINE(604, axb, "VECCrossProduct(): NULL VecPtr 'axb' ");
vTmp.x = (a->y * b->z) - (a->z * b->y);
vTmp.y = (a->z * b->x) - (a->x * b->z);
vTmp.z = (a->x * b->y) - (a->y * b->x);
axb->x = vTmp.x;
axb->y = vTmp.y;
axb->z = vTmp.z;
}
#ifdef __MWERKS__
asm void PSVECCrossProduct(const register Vec* a, const register Vec* b, register Vec* axb) {
psq_l f1, Vec.x(b), 0, 0
lfs f2, Vec.z(a)
psq_l f0, Vec.x(a), 0, 0
ps_merge10 f6, f1, f1
lfs f3, Vec.z(b)
ps_mul f4, f1, f2
ps_muls0 f7, f1, f0
ps_msub f5, f0, f3, f4
ps_msub f8, f0, f6, f7
ps_merge11 f9, f5, f5
ps_merge01 f10, f5, f8
psq_st f9, Vec.x(axb), 1, 0
ps_neg f10, f10
psq_st f10, Vec.y(axb), 0, 0
}
#endif
void C_VECHalfAngle(const Vec* a, const Vec* b, Vec* half) {
Vec aTmp;
Vec bTmp;
Vec hTmp;
ASSERTMSGLINE(707, a, "VECHalfAngle(): NULL VecPtr 'a' ");
ASSERTMSGLINE(708, b, "VECHalfAngle(): NULL VecPtr 'b' ");
ASSERTMSGLINE(709, half, "VECHalfAngle(): NULL VecPtr 'half' ");
aTmp.x = -a->x;
aTmp.y = -a->y;
aTmp.z = -a->z;
bTmp.x = -b->x;
bTmp.y = -b->y;
bTmp.z = -b->z;
VECNormalize(&aTmp, &aTmp);
VECNormalize(&bTmp, &bTmp);
VECAdd(&aTmp, &bTmp, &hTmp);
if (VECDotProduct(&hTmp, &hTmp) > 0.0f) {
VECNormalize(&hTmp, half);
return;
}
*half = hTmp;
}
void C_VECReflect(const Vec* src, const Vec* normal, Vec* dst) {
f32 cosA;
Vec uI;
Vec uN;
ASSERTMSGLINE(763, src, "VECReflect(): NULL VecPtr 'src' ");
ASSERTMSGLINE(764, normal, "VECReflect(): NULL VecPtr 'normal' ");
ASSERTMSGLINE(765, dst, "VECReflect(): NULL VecPtr 'dst' ");
uI.x = -src->x;
uI.y = -src->y;
uI.z = -src->z;
VECNormalize(&uI, &uI);
VECNormalize(normal, &uN);
cosA = VECDotProduct(&uI, &uN);
dst->x = (2.0f * uN.x * cosA) - uI.x;
dst->y = (2.0f * uN.y * cosA) - uI.y;
dst->z = (2.0f * uN.z * cosA) - uI.z;
VECNormalize(dst, dst);
}
f32 C_VECSquareDistance(const Vec* a, const Vec* b) {
Vec diff;
diff.x = a->x - b->x;
diff.y = a->y - b->y;
diff.z = a->z - b->z;
return (diff.z * diff.z) + ((diff.x * diff.x) + (diff.y * diff.y));
}
#ifdef __MWERKS__
f32 PSVECSquareDistance(const register Vec* a, const register Vec* b) {
register f32 v0yz, v1yz, v0xy, v1xy, dyz, dxy;
register f32 sqdist;
asm {
psq_l v0yz, 0x4(a), 0, 0
psq_l v1yz, 0x4(b), 0, 0
ps_sub dyz, v0yz, v1yz
psq_l v0xy, 0x0(a), 0, 0
psq_l v1xy, 0x0(b), 0, 0
ps_mul dyz, dyz, dyz
ps_sub dxy, v0xy, v1xy
ps_madd sqdist, dxy, dxy, dyz
ps_sum0 sqdist, sqdist, dyz, dyz
}
return sqdist;
}
#endif
f32 C_VECDistance(const Vec* a, const Vec* b) {
return sqrtf(C_VECSquareDistance(a, b));
}
#ifdef __MWERKS__
f32 PSVECDistance(const register Vec* a, const register Vec* b) {
register f32 v0yz, v1yz, v0xy, v1xy, dyz, dxy;
register f32 sqdist, rdist;
register f32 nwork0, nwork1;
register f32 c_half, c_three, c_zero;
asm {
psq_l v0yz, 0x4(a), 0, 0
psq_l v1yz, 0x4(b), 0, 0
ps_sub dyz, v0yz, v1yz
psq_l v0xy, 0x0(a), 0, 0
psq_l v1xy, 0x0(b), 0, 0
ps_mul dyz, dyz, dyz
ps_sub dxy, v0xy, v1xy
}
c_half = 0.5f;
asm {
ps_madd sqdist, dxy, dxy, dyz
fsubs c_zero, c_half, c_half
ps_sum0 sqdist, sqdist, dyz, dyz
fcmpu cr0, c_zero, sqdist
beq L_00000CBC
}
c_three = 3.0f;
asm {
frsqrte rdist, sqdist
fmuls nwork0, rdist, rdist
fmuls nwork1, rdist, c_half
fnmsubs nwork0, nwork0, sqdist, c_three
fmuls rdist, nwork0, nwork1
fmuls sqdist, sqdist, rdist
L_00000CBC:
}
return sqdist;
}
#endif