From 3316ad9a7fd1767983925a1b7c5ef246d79f9253 Mon Sep 17 00:00:00 2001 From: Luke Street Date: Mon, 14 Apr 2025 17:12:27 -0600 Subject: [PATCH] Add aurora::mtx lib --- CMakeLists.txt | 3 +- cmake/aurora_mtx.cmake | 6 + include/dolphin/mtx.h | 320 +++++++++++++++++++++++++++++++++ include/dolphin/mtx/GeoTypes.h | 40 +++++ include/dolphin/mtx/mtx44ext.h | 106 +++++++++++ lib/dolphin/mtx.c | 223 +++++++++++++++++++++++ 6 files changed, 697 insertions(+), 1 deletion(-) create mode 100644 cmake/aurora_mtx.cmake create mode 100644 include/dolphin/mtx.h create mode 100644 include/dolphin/mtx/GeoTypes.h create mode 100644 include/dolphin/mtx/mtx44ext.h create mode 100644 lib/dolphin/mtx.c diff --git a/CMakeLists.txt b/CMakeLists.txt index 7e98b19..76b0fef 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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) diff --git a/cmake/aurora_mtx.cmake b/cmake/aurora_mtx.cmake new file mode 100644 index 0000000..d221f2f --- /dev/null +++ b/cmake/aurora_mtx.cmake @@ -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) diff --git a/include/dolphin/mtx.h b/include/dolphin/mtx.h new file mode 100644 index 0000000..c6ca42b --- /dev/null +++ b/include/dolphin/mtx.h @@ -0,0 +1,320 @@ +#ifndef _DOLPHIN_MTX +#define _DOLPHIN_MTX + +#include +#include + +#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 diff --git a/include/dolphin/mtx/GeoTypes.h b/include/dolphin/mtx/GeoTypes.h new file mode 100644 index 0000000..e2a4831 --- /dev/null +++ b/include/dolphin/mtx/GeoTypes.h @@ -0,0 +1,40 @@ +#ifndef _DOLPHIN_GEOTYPES +#define _DOLPHIN_GEOTYPES + +#include + +#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 diff --git a/include/dolphin/mtx/mtx44ext.h b/include/dolphin/mtx/mtx44ext.h new file mode 100644 index 0000000..391ea57 --- /dev/null +++ b/include/dolphin/mtx/mtx44ext.h @@ -0,0 +1,106 @@ +#include + +#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 diff --git a/lib/dolphin/mtx.c b/lib/dolphin/mtx.c new file mode 100644 index 0000000..cf46312 --- /dev/null +++ b/lib/dolphin/mtx.c @@ -0,0 +1,223 @@ +#include + +#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; +} \ No newline at end of file