Add aurora::mtx lib

This commit is contained in:
Luke Street 2025-04-14 17:12:27 -06:00
parent 609c4bfb72
commit 3316ad9a7f
6 changed files with 697 additions and 1 deletions

View File

@ -9,8 +9,9 @@ add_subdirectory(extern)
include(cmake/aurora_core.cmake)
include(cmake/aurora_gx.cmake)
include(cmake/aurora_vi.cmake)
include(cmake/aurora_main.cmake)
include(cmake/aurora_mtx.cmake)
include(cmake/aurora_vi.cmake)
if (CMAKE_SOURCE_DIR STREQUAL CMAKE_CURRENT_SOURCE_DIR)
add_subdirectory(examples)

6
cmake/aurora_mtx.cmake Normal file
View File

@ -0,0 +1,6 @@
add_library(aurora_mtx STATIC
lib/dolphin/mtx.c
)
add_library(aurora::mtx ALIAS aurora_mtx)
target_include_directories(aurora_mtx PUBLIC include)

320
include/dolphin/mtx.h Normal file
View File

@ -0,0 +1,320 @@
#ifndef _DOLPHIN_MTX
#define _DOLPHIN_MTX
#include <dolphin/mtx/GeoTypes.h>
#include <dolphin/types.h>
#ifdef __cplusplus
extern "C" {
#endif
#ifndef GEKKO
#define MTX_USE_C
#undef MTX_USE_PS
#endif
#if !defined(MTX_USE_PS) && !defined(MTX_USE_C) && defined(GEKKO)
#ifndef _DEBUG
#define MTX_USE_PS
#endif
#endif
#define MTX_PTR_OFFSET 3
#define MTX44_PTR_OFFSET 4
typedef struct {
u32 numMtx;
MtxPtr stackBase;
MtxPtr stackPtr;
} MtxStack, *MtxStackPtr;
#define MTXDegToRad(a) ((a)*0.01745329252f)
#define MTXRadToDeg(a) ((a)*57.29577951f)
#define MTXRowCol(m, r, c) ((m)[(r)][(c)])
void C_MTXIdentity(Mtx m);
void C_MTXCopy(const Mtx src, Mtx dst);
void C_MTXConcat(const Mtx a, const Mtx b, Mtx ab);
void C_MTXConcatArray(const Mtx a, const Mtx* srcBase, Mtx* dstBase, u32 count);
void C_MTXTranspose(const Mtx src, Mtx xPose);
u32 C_MTXInverse(const Mtx src, Mtx inv);
u32 C_MTXInvXpose(const Mtx src, Mtx invX);
#ifdef GEKKO
void PSMTXIdentity(Mtx m);
void PSMTXCopy(const Mtx src, Mtx dst);
void PSMTXConcat(const Mtx a, const Mtx b, Mtx ab);
void PSMTXConcatArray(const Mtx a, const Mtx* srcBase, Mtx* dstBase, u32 count);
void PSMTXTranspose(const Mtx src, Mtx xPose);
u32 PSMTXInverse(const Mtx src, Mtx inv);
u32 PSMTXInvXpose(const Mtx src, Mtx invX);
#endif
#ifdef MTX_USE_PS
#define MTXIdentity PSMTXIdentity
#define MTXCopy PSMTXCopy
#define MTXConcat PSMTXConcat
#define MTXConcatArray PSMTXConcatArray
#define MTXTranspose PSMTXTranspose
#define MTXInverse PSMTXInverse
#define MTXInvXpose PSMTXInvXpose
#else
#define MTXIdentity C_MTXIdentity
#define MTXCopy C_MTXCopy
#define MTXConcat C_MTXConcat
#define MTXConcatArray C_MTXConcatArray
#define MTXTranspose C_MTXTranspose
#define MTXInverse C_MTXInverse
#define MTXInvXpose C_MTXInvXpose
#endif
void C_MTXMultVec(const Mtx m, const Vec* src, Vec* dst);
void C_MTXMultVecArray(const Mtx m, const Vec* srcBase, Vec* dstBase, u32 count);
void C_MTXMultVecSR(const Mtx m, const Vec* src, Vec* dst);
void C_MTXMultVecArraySR(const Mtx m, const Vec* srcBase, Vec* dstBase, u32 count);
#ifdef GEKKO
void PSMTXMultVec(const Mtx m, const Vec* src, Vec* dst);
void PSMTXMultVecArray(const Mtx m, const Vec* srcBase, Vec* dstBase, u32 count);
void PSMTXMultVecSR(const Mtx m, const Vec* src, Vec* dst);
void PSMTXMultVecArraySR(const Mtx m, const Vec* srcBase, Vec* dstBase, u32 count);
#endif
#ifdef MTX_USE_PS
#define MTXMultVec PSMTXMultVec
#define MTXMultVecArray PSMTXMultVecArray
#define MTXMultVecSR PSMTXMultVecSR
#define MTXMultVecArraySR PSMTXMultVecArraySR
#else // MTX_USE_C
#define MTXMultVec C_MTXMultVec
#define MTXMultVecArray C_MTXMultVecArray
#define MTXMultVecSR C_MTXMultVecSR
#define MTXMultVecArraySR C_MTXMultVecArraySR
#endif
void C_MTXQuat(Mtx m, const Quaternion* q);
void C_MTXReflect(Mtx m, const Vec* p, const Vec* n);
void C_MTXTrans(Mtx m, f32 xT, f32 yT, f32 zT);
void C_MTXTransApply(const Mtx src, Mtx dst, f32 xT, f32 yT, f32 zT);
void C_MTXScale(Mtx m, f32 xS, f32 yS, f32 zS);
void C_MTXScaleApply(const Mtx src, Mtx dst, f32 xS, f32 yS, f32 zS);
void C_MTXRotRad(Mtx m, char axis, f32 rad);
void C_MTXRotTrig(Mtx m, char axis, f32 sinA, f32 cosA);
void C_MTXRotAxisRad(Mtx m, const Vec* axis, f32 rad);
#ifdef GEKKO
void PSMTXQuat(Mtx m, const Quaternion* q);
void PSMTXReflect(Mtx m, const Vec* p, const Vec* n);
void PSMTXTrans(Mtx m, f32 xT, f32 yT, f32 zT);
void PSMTXTransApply(const Mtx src, Mtx dst, f32 xT, f32 yT, f32 zT);
void PSMTXScale(Mtx m, f32 xS, f32 yS, f32 zS);
void PSMTXScaleApply(const Mtx src, Mtx dst, f32 xS, f32 yS, f32 zS);
void PSMTXRotRad(Mtx m, char axis, f32 rad);
void PSMTXRotTrig(Mtx m, char axis, f32 sinA, f32 cosA);
void PSMTXRotAxisRad(Mtx m, const Vec* axis, f32 rad);
#endif
#ifdef MTX_USE_PS
#define MTXQuat PSMTXQuat
#define MTXReflect PSMTXReflect
#define MTXTrans PSMTXTrans
#define MTXTransApply PSMTXTransApply
#define MTXScale PSMTXScale
#define MTXScaleApply PSMTXScaleApply
#define MTXRotRad PSMTXRotRad
#define MTXRotTrig PSMTXRotTrig
#define MTXRotAxisRad PSMTXRotAxisRad
#define MTXRotDeg(m, axis, deg) PSMTXRotRad(m, axis, MTXDegToRad(deg))
#define MTXRotAxisDeg(m, axis, deg) PSMTXRotAxisRad(m, axis, MTXDegToRad(deg))
#else // MTX_USE_C
#define MTXQuat C_MTXQuat
#define MTXReflect C_MTXReflect
#define MTXTrans C_MTXTrans
#define MTXTransApply C_MTXTransApply
#define MTXScale C_MTXScale
#define MTXScaleApply C_MTXScaleApply
#define MTXRotRad C_MTXRotRad
#define MTXRotTrig C_MTXRotTrig
#define MTXRotAxisRad C_MTXRotAxisRad
#define MTXRotDeg(m, axis, deg) C_MTXRotRad(m, axis, MTXDegToRad(deg))
#define MTXRotAxisDeg(m, axis, deg) C_MTXRotAxisRad(m, axis, MTXDegToRad(deg))
#endif
void C_MTXLookAt(Mtx m, const Point3d* camPos, const Vec* camUp, const Point3d* target);
#define MTXLookAt C_MTXLookAt
void C_MTXFrustum(Mtx44 m, f32 t, f32 b, f32 l, f32 r, f32 n, f32 f);
void C_MTXPerspective(Mtx44 m, f32 fovY, f32 aspect, f32 n, f32 f);
void C_MTXOrtho(Mtx44 m, f32 t, f32 b, f32 l, f32 r, f32 n, f32 f);
#define MTXFrustum C_MTXFrustum
#define MTXPerspective C_MTXPerspective
#define MTXOrtho C_MTXOrtho
void C_MTXLightFrustum(Mtx m, f32 t, f32 b, f32 l, f32 r, f32 n, f32 scaleS, f32 scaleT, f32 transS,
f32 transT);
void C_MTXLightPerspective(Mtx m, f32 fovY, f32 aspect, f32 scaleS, f32 scaleT, f32 transS,
f32 transT);
void C_MTXLightOrtho(Mtx m, f32 t, f32 b, f32 l, f32 r, f32 scaleS, f32 scaleT, f32 transS,
f32 transT);
#define MTXLightFrustum C_MTXLightFrustum
#define MTXLightPerspective C_MTXLightPerspective
#define MTXLightOrtho C_MTXLightOrtho
void C_VECAdd(const Vec* a, const Vec* b, Vec* ab);
void C_VECSubtract(const Vec* a, const Vec* b, Vec* a_b);
void C_VECScale(const Vec* src, Vec* dst, f32 scale);
void C_VECNormalize(const Vec* src, Vec* unit);
f32 C_VECSquareMag(const Vec* v);
f32 C_VECMag(const Vec* v);
f32 C_VECDotProduct(const Vec* a, const Vec* b);
void C_VECCrossProduct(const Vec* a, const Vec* b, Vec* axb);
f32 C_VECSquareDistance(const Vec* a, const Vec* b);
f32 C_VECDistance(const Vec* a, const Vec* b);
void C_VECReflect(const Vec* src, const Vec* normal, Vec* dst);
void C_VECHalfAngle(const Vec* a, const Vec* b, Vec* half);
#ifdef GEKKO
void PSVECAdd(const Vec* a, const Vec* b, Vec* ab);
void PSVECSubtract(const Vec* a, const Vec* b, Vec* a_b);
void PSVECScale(const Vec* src, Vec* dst, f32 scale);
void PSVECNormalize(const Vec* src, Vec* unit);
f32 PSVECSquareMag(const Vec* v);
f32 PSVECMag(const Vec* v);
f32 PSVECDotProduct(const Vec* a, const Vec* b);
void PSVECCrossProduct(const Vec* a, const Vec* b, Vec* axb);
f32 PSVECSquareDistance(const Vec* a, const Vec* b);
f32 PSVECDistance(const Vec* a, const Vec* b);
#endif
// TODO
#if defined( MTX_USE_PS) && 0
#define VECAdd PSVECAdd
#define VECSubtract PSVECSubtract
#define VECScale PSVECScale
#define VECNormalize PSVECNormalize
#define VECSquareMag PSVECSquareMag
#define VECMag PSVECMag
#define VECDotProduct PSVECDotProduct
#define VECCrossProduct PSVECCrossProduct
#define VECSquareDistance PSVECSquareDistance
#define VECDistance PSVECDistance
#else // MTX_USE_C
#define VECAdd C_VECAdd
#define VECSubtract C_VECSubtract
#define VECScale C_VECScale
#define VECNormalize C_VECNormalize
#define VECSquareMag C_VECSquareMag
#define VECMag C_VECMag
#define VECDotProduct C_VECDotProduct
#define VECCrossProduct C_VECCrossProduct
#define VECSquareDistance C_VECSquareDistance
#define VECDistance C_VECDistance
#endif
#define VECReflect C_VECReflect
#define VECHalfAngle C_VECHalfAngle
void C_QUATAdd(const Quaternion* p, const Quaternion* q, Quaternion* r);
void C_QUATSubtract(const Quaternion* p, const Quaternion* q, Quaternion* r);
void C_QUATMultiply(const Quaternion* p, const Quaternion* q, Quaternion* pq);
void C_QUATDivide(const Quaternion* p, const Quaternion* q, Quaternion* r);
void C_QUATScale(const Quaternion* q, Quaternion* r, f32 scale);
f32 C_QUATDotProduct(const Quaternion* p, const Quaternion* q);
void C_QUATNormalize(const Quaternion* src, Quaternion* unit);
void C_QUATInverse(const Quaternion* src, Quaternion* inv);
void C_QUATExp(const Quaternion* q, Quaternion* r);
void C_QUATLogN(const Quaternion* q, Quaternion* r);
void C_QUATMakeClosest(const Quaternion* q, const Quaternion* qto, Quaternion* r);
void C_QUATRotAxisRad(Quaternion* r, const Vec* axis, f32 rad);
void C_QUATMtx(Quaternion* r, const Mtx m);
void C_QUATLerp(const Quaternion* p, const Quaternion* q, Quaternion* r, f32 t);
void C_QUATSlerp(const Quaternion* p, const Quaternion* q, Quaternion* r, f32 t);
void C_QUATSquad(const Quaternion* p, const Quaternion* a, const Quaternion* b, const Quaternion* q,
Quaternion* r, f32 t);
void C_QUATCompA(const Quaternion* qprev, const Quaternion* q, const Quaternion* qnext,
Quaternion* a);
#ifdef GEKKO
void PSQUATAdd(const Quaternion* p, const Quaternion* q, Quaternion* r);
void PSQUATSubtract(const Quaternion* p, const Quaternion* q, Quaternion* r);
void PSQUATMultiply(const Quaternion* p, const Quaternion* q, Quaternion* pq);
void PSQUATDivide(const Quaternion* p, const Quaternion* q, Quaternion* r);
void PSQUATScale(const Quaternion* q, Quaternion* r, f32 scale);
f32 PSQUATDotProduct(const Quaternion* p, const Quaternion* q);
void PSQUATNormalize(const Quaternion* src, Quaternion* unit);
void PSQUATInverse(const Quaternion* src, Quaternion* inv);
#endif
#ifdef MTX_USE_PS
#define QUATAdd PSQUATAdd
#define QUATSubtract PSQUATSubtract
#define QUATMultiply PSQUATMultiply
#define QUATDivide PSQUATDivide
#define QUATScale PSQUATScale
#define QUATDotProduct PSQUATDotProduct
#define QUATNormalize PSQUATNormalize
#define QUATInverse PSQUATInverse
#else // MTX_USE_C
#define QUATAdd C_QUATAdd
#define QUATSubtract C_QUATSubtract
#define QUATMultiply C_QUATMultiply
#define QUATDivide C_QUATDivide
#define QUATScale C_QUATScale
#define QUATDotProduct C_QUATDotProduct
#define QUATNormalize C_QUATNormalize
#define QUATInverse C_QUATInverse
#endif
#define QUATExp C_QUATExp
#define QUATLogN C_QUATLogN
#define QUATMakeClosest C_QUATMakeClosest
#define QUATRotAxisRad C_QUATRotAxisRad
#define QUATMtx C_QUATMtx
#define QUATLerp C_QUATLerp
#define QUATSlerp C_QUATSlerp
#define QUATSquad C_QUATSquad
#define QUATCompA C_QUATCompA
#ifdef GEKKO
void PSMTXReorder(const Mtx src, ROMtx dest);
void PSMTXROMultVecArray(const ROMtx m, const Vec* srcBase, Vec* dstBase, u32 count);
void PSMTXROSkin2VecArray(const ROMtx m0, const ROMtx m1, const f32* wtBase, const Vec* srcBase,
Vec* dstBase, u32 count);
void PSMTXMultS16VecArray(const Mtx m, const S16Vec* srcBase, Vec* dstBase, u32 count);
void PSMTXROMultS16VecArray(const ROMtx m, const S16Vec* srcBase, Vec* dstBase, u32 count);
#endif
void MTXInitStack(MtxStack* sPtr, u32 numMtx);
MtxPtr MTXPush(MtxStack* sPtr, const Mtx m);
MtxPtr MTXPushFwd(MtxStack* sPtr, const Mtx m);
MtxPtr MTXPushInv(MtxStack* sPtr, const Mtx m);
MtxPtr MTXPushInvXpose(MtxStack* sPtr, const Mtx m);
MtxPtr MTXPop(MtxStack* sPtr);
MtxPtr MTXGetStackPtr(const MtxStack* sPtr);
#define MTXAllocStack(sPtr, numMtx) \
(((MtxStackPtr)(sPtr))->stackBase = (MtxPtr)OSAlloc(((numMtx) * sizeof(Mtx))))
#define MTXFreeStack(sPtr) (OSFree((void*)(((MtxStackPtr)(sPtr))->stackBase)))
#ifdef __cplusplus
}
#endif
#endif // _DOLPHIN_MTX

View File

@ -0,0 +1,40 @@
#ifndef _DOLPHIN_GEOTYPES
#define _DOLPHIN_GEOTYPES
#include <dolphin/types.h>
#ifdef __cplusplus
extern "C" {
#endif
typedef struct {
f32 x, y, z;
} Vec, *VecPtr, Point3d, *Point3dPtr;
typedef struct {
s16 x;
s16 y;
s16 z;
} S16Vec, *S16VecPtr;
typedef struct {
f32 x, y, z, w;
} Quaternion, *QuaternionPtr, Qtrn, *QtrnPtr;
typedef f32 Mtx[3][4];
typedef f32 (*MtxPtr)[4];
typedef f32 ROMtx[4][3];
typedef f32 (*ROMtxPtr)[3];
typedef f32 Mtx44[4][4];
typedef f32 (*Mtx44Ptr)[4];
#ifdef __cplusplus
}
#endif
#endif // _DOLPHIN_GEOTYPES

View File

@ -0,0 +1,106 @@
#include <dolphin/mtx.h>
#ifndef _DOLPHIN_MTX44EXT
#define _DOLPHIN_MTX44EXT
#ifdef __cplusplus
extern "C" {
#endif
void C_MTX44Identity(Mtx44 m);
void C_MTX44Copy(const Mtx44 src, Mtx44 dst);
void C_MTX44Concat(const Mtx44 a, const Mtx44 b, Mtx44 ab);
void C_MTX44Transpose(const Mtx44 src, Mtx44 xPose);
u32 C_MTX44Inverse(const Mtx44 src, Mtx44 inv);
#ifdef GEKKO
void PSMTX44Identity(Mtx44 m);
void PSMTX44Copy(const Mtx44 src, Mtx44 dst);
void PSMTX44Concat(const Mtx44 a, const Mtx44 b, Mtx44 ab);
void PSMTX44Transpose(const Mtx44 src, Mtx44 xPose);
#endif
#ifdef MTX_USE_PS
#define MTX44Identity PSMTX44Identity
#define MTX44Copy PSMTX44Copy
#define MTX44Concat PSMTX44Concat
#define MTX44Transpose PSMTX44Transpose
#else
#define MTX44Identity C_MTX44Identity
#define MTX44Copy C_MTX44Copy
#define MTX44Concat C_MTX44Concat
#define MTX44Transpose C_MTX44Transpose
#endif
#define MTX44Inverse C_MTX44Inverse
void C_MTX44Trans(Mtx44 m, f32 xT, f32 yT, f32 zT);
void C_MTX44TransApply(const Mtx44 src, Mtx44 dst, f32 xT, f32 yT, f32 zT);
void C_MTX44Scale(Mtx44 m, f32 xS, f32 yS, f32 zS);
void C_MTX44ScaleApply(const Mtx44 src, Mtx44 dst, f32 xS, f32 yS, f32 zS);
void C_MTX44RotRad(Mtx44 m, char axis, f32 rad);
void C_MTX44RotTrig(Mtx44 m, char axis, f32 sinA, f32 cosA);
void C_MTX44RotAxisRad(Mtx44 m, const Vec* axis, f32 rad);
#ifdef GEKKO
void PSMTX44Trans(Mtx44 m, f32 xT, f32 yT, f32 zT);
void PSMTX44TransApply(const Mtx44 src, Mtx44 dst, f32 xT, f32 yT, f32 zT);
void PSMTX44Scale(Mtx44 m, f32 xS, f32 yS, f32 zS);
void PSMTX44ScaleApply(const Mtx44 src, Mtx44 dst, f32 xS, f32 yS, f32 zS);
void PSMTX44RotRad(Mtx44 m, char axis, f32 rad);
void PSMTX44RotTrig(Mtx44 m, char axis, f32 sinA, f32 cosA);
void PSMTX44RotAxisRad(Mtx44 m, const Vec* axis, f32 rad);
#endif
#ifdef MTX_USE_PS
#define MTX44Trans PSMTX44Trans
#define MTX44TransApply PSMTX44TransApply
#define MTX44Scale PSMTX44Scale
#define MTX44ScaleApply PSMTX44ScaleApply
#define MTX44RotRad PSMTX44RotRad
#define MTX44RotTrig PSMTX44RotTrig
#define MTX44RotAxisRad PSMTX44RotAxisRad
#else
#define MTX44Trans C_MTX44Trans
#define MTX44TransApply C_MTX44TransApply
#define MTX44Scale C_MTX44Scale
#define MTX44ScaleApply C_MTX44ScaleApply
#define MTX44RotRad C_MTX44RotRad
#define MTX44RotTrig C_MTX44RotTrig
#define MTX44RotAxisRad C_MTX44RotAxisRad
#endif
void C_MTX44MultVec(const Mtx44 m, const Vec* src, Vec* dst);
void C_MTX44MultVecArray(const Mtx44 m, const Vec* srcBase, Vec* dstBase, u32 count);
void C_MTX44MultVecSR(const Mtx44 m, const Vec* src, Vec* dst);
void C_MTX44MultVecArraySR(const Mtx44 m, const Vec* srcBase, Vec* dstBase, u32 count);
#ifdef GEKKO
void PSMTX44MultVec(const Mtx44 m, const Vec* src, Vec* dst);
void PSMTX44MultVecArray(const Mtx44 m, const Vec* srcBase, Vec* dstBase, u32 count);
void PSMTX44MultVecSR(const Mtx44 m, const Vec* src, Vec* dst);
void PSMTX44MultVecArraySR(const Mtx44 m, const Vec* srcBase, Vec* dstBase, u32 count);
#endif
#ifdef MTX_USE_PS
#define MTX44MultVec PSMTX44MultVec
#define MTX44MultVecArray PSMTX44MultVecArray
#define MTX44MultVecSR PSMTX44MultVecSR
#define MTX44MultVecArraySR PSMTX44MultVecArraySR
#else
#define MTX44MultVec C_MTX44MultVec
#define MTX44MultVecArray C_MTX44MultVecArray
#define MTX44MultVecSR C_MTX44MultVecSR
#define MTX44MultVecArraySR C_MTX44MultVecArraySR
#endif
#ifdef __cplusplus
}
#endif
#endif // _DOLPHIN_MTX44EXT

223
lib/dolphin/mtx.c Normal file
View File

@ -0,0 +1,223 @@
#include <dolphin/mtx.h>
#define ASSERTLINE(line, cond) (void)0
#define ASSERTMSGLINE(line, cond, msg) (void)0
#define ASSERTMSG1LINE(line, cond, msg, arg1) (void)0
#define ASSERTMSG2LINE(line, cond, msg, arg1, arg2) (void)0
#define ASSERTMSGLINEV(line, cond, ...) (void)0
void C_MTXIdentity(Mtx m) {
ASSERTMSGLINE(189, m, "MtxIdentity(): NULL Mtx 'm' ");
m[0][0] = 1;
m[0][1] = 0;
m[0][2] = 0;
m[0][3] = 0;
m[1][0] = 0;
m[1][1] = 1;
m[1][2] = 0;
m[1][3] = 0;
m[2][0] = 0;
m[2][1] = 0;
m[2][2] = 1;
m[2][3] = 0;
}
void C_MTXCopy(const Mtx src, Mtx dst) {
ASSERTMSGLINE(250, src, "MTXCopy(): NULL MtxPtr 'src' ");
ASSERTMSGLINE(251, dst, "MTXCopy(): NULL MtxPtr '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];
}
}
void C_MTXConcat(const Mtx a, const Mtx b, Mtx ab) {
Mtx mTmp;
MtxPtr m;
ASSERTMSGLINE(324, a, "MTXConcat(): NULL MtxPtr 'a' ");
ASSERTMSGLINE(325, b, "MTXConcat(): NULL MtxPtr 'b' ");
ASSERTMSGLINE(326, ab, "MTXConcat(): NULL MtxPtr 'ab' ");
if (ab == a || ab == b) {
m = mTmp;
} else {
m = ab;
}
m[0][0] = 0 + a[0][2] * b[2][0] + ((a[0][0] * b[0][0]) + (a[0][1] * b[1][0]));
m[0][1] = 0 + a[0][2] * b[2][1] + ((a[0][0] * b[0][1]) + (a[0][1] * b[1][1]));
m[0][2] = 0 + a[0][2] * b[2][2] + ((a[0][0] * b[0][2]) + (a[0][1] * b[1][2]));
m[0][3] = a[0][3] + (a[0][2] * b[2][3] + (a[0][0] * b[0][3] + (a[0][1] * b[1][3])));
m[1][0] = 0 + a[1][2] * b[2][0] + ((a[1][0] * b[0][0]) + (a[1][1] * b[1][0]));
m[1][1] = 0 + a[1][2] * b[2][1] + ((a[1][0] * b[0][1]) + (a[1][1] * b[1][1]));
m[1][2] = 0 + a[1][2] * b[2][2] + ((a[1][0] * b[0][2]) + (a[1][1] * b[1][2]));
m[1][3] = a[1][3] + (a[1][2] * b[2][3] + (a[1][0] * b[0][3] + (a[1][1] * b[1][3])));
m[2][0] = 0 + a[2][2] * b[2][0] + ((a[2][0] * b[0][0]) + (a[2][1] * b[1][0]));
m[2][1] = 0 + a[2][2] * b[2][1] + ((a[2][0] * b[0][1]) + (a[2][1] * b[1][1]));
m[2][2] = 0 + a[2][2] * b[2][2] + ((a[2][0] * b[0][2]) + (a[2][1] * b[1][2]));
m[2][3] = a[2][3] + (a[2][2] * b[2][3] + (a[2][0] * b[0][3] + (a[2][1] * b[1][3])));
if (m == mTmp) {
C_MTXCopy(mTmp, ab);
}
}
u32 C_MTXInvXpose(const Mtx src, Mtx invX) {
Mtx mTmp;
MtxPtr m;
f32 det;
ASSERTMSGLINE(1185, src, "MTXInvXpose(): NULL MtxPtr 'src' ");
ASSERTMSGLINE(1186, invX, "MTXInvXpose(): NULL MtxPtr 'invX' ");
if (src == invX) {
m = mTmp;
} else {
m = invX;
}
det = ((((src[2][1] * (src[0][2] * src[1][0]))
+ ((src[2][2] * (src[0][0] * src[1][1]))
+ (src[2][0] * (src[0][1] * src[1][2]))))
- (src[0][2] * (src[2][0] * src[1][1])))
- (src[2][2] * (src[1][0] * src[0][1])))
- (src[1][2] * (src[0][0] * src[2][1]));
if (0 == det) {
return 0;
}
det = 1 / det;
m[0][0] = (det * +((src[1][1] * src[2][2]) - (src[2][1] * src[1][2])));
m[0][1] = (det * -((src[1][0] * src[2][2]) - (src[2][0] * src[1][2])));
m[0][2] = (det * +((src[1][0] * src[2][1]) - (src[2][0] * src[1][1])));
m[1][0] = (det * -((src[0][1] * src[2][2]) - (src[2][1] * src[0][2])));
m[1][1] = (det * +((src[0][0] * src[2][2]) - (src[2][0] * src[0][2])));
m[1][2] = (det * -((src[0][0] * src[2][1]) - (src[2][0] * src[0][1])));
m[2][0] = (det * +((src[0][1] * src[1][2]) - (src[1][1] * src[0][2])));
m[2][1] = (det * -((src[0][0] * src[1][2]) - (src[1][0] * src[0][2])));
m[2][2] = (det * +((src[0][0] * src[1][1]) - (src[1][0] * src[0][1])));
m[0][3] = 0;
m[1][3] = 0;
m[2][3] = 0;
if (m == mTmp) {
C_MTXCopy(mTmp, invX);
}
return 1;
}
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;
}
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;
}
void C_MTXTrans(Mtx m, f32 xT, f32 yT, f32 zT) {
ASSERTMSGLINE(1866, m, "MTXTrans(): NULL MtxPtr 'm' ");
m[0][0] = 1;
m[0][1] = 0;
m[0][2] = 0;
m[0][3] = xT;
m[1][0] = 0;
m[1][1] = 1;
m[1][2] = 0;
m[1][3] = yT;
m[2][0] = 0;
m[2][1] = 0;
m[2][2] = 1;
m[2][3] = zT;
}
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_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;
}