mirror of
https://github.com/encounter/aurora.git
synced 2026-07-09 18:19:33 -07:00
Jeffrey Crowell
109 lines
3.7 KiB
C
109 lines
3.7 KiB
C
#include <assert.h>
|
|
#include <stdint.h>
|
|
#include <dolphin/mtx.h>
|
|
|
|
void MTXInitStack(MTXStack* sPtr, u32 numMtx) {
|
|
assert(sPtr && "MTXInitStack(): NULL MtxStackPtr 'sPtr' ");
|
|
assert(sPtr->stackBase && "MTXInitStack(): 'sPtr' contains a NULL ptr to stack memory ");
|
|
assert(numMtx && "MTXInitStack(): 'numMtx' is 0 ");
|
|
|
|
sPtr->numMtx = numMtx;
|
|
sPtr->stackPtr = 0;
|
|
}
|
|
|
|
MtxPtr MTXPush(MTXStack* sPtr, const Mtx m) {
|
|
assert(sPtr && "MTXPush(): NULL MtxStackPtr 'sPtr' ");
|
|
assert(sPtr->stackBase && "MTXPush(): 'sPtr' contains a NULL ptr to stack memory ");
|
|
assert(m && "MTXPush(): NULL MtxPtr 'm' ");
|
|
|
|
if (sPtr->stackPtr == NULL) {
|
|
sPtr->stackPtr = sPtr->stackBase;
|
|
MTXCopy(m, sPtr->stackPtr);
|
|
} else {
|
|
assert(((((uintptr_t)sPtr->stackPtr - (uintptr_t)sPtr->stackBase) / 16) / 3) < (sPtr->numMtx - 1) && "MTXPush(): stack overflow ");
|
|
MTXCopy(m, sPtr->stackPtr + 3);
|
|
sPtr->stackPtr += 3;
|
|
}
|
|
|
|
return sPtr->stackPtr;
|
|
}
|
|
|
|
MtxPtr MTXPushFwd(MTXStack* sPtr, const Mtx m) {
|
|
assert(sPtr && "MTXPushFwd(): NULL MtxStackPtr 'sPtr' ");
|
|
assert(sPtr->stackBase && "MTXPushFwd(): 'sPtr' contains a NULL ptr to stack memory ");
|
|
assert(m && "MTXPushFwd(): NULL MtxPtr 'm' ");
|
|
|
|
if (sPtr->stackPtr == NULL) {
|
|
sPtr->stackPtr = sPtr->stackBase;
|
|
MTXCopy(m, sPtr->stackPtr);
|
|
} else {
|
|
assert(((((uintptr_t)sPtr->stackPtr - (uintptr_t)sPtr->stackBase) / 16) / 3) < (sPtr->numMtx - 1) && "MTXPushFwd(): stack overflow");
|
|
MTXConcat(sPtr->stackPtr, m, sPtr->stackPtr + 3);
|
|
sPtr->stackPtr += 3;
|
|
}
|
|
|
|
return sPtr->stackPtr;
|
|
}
|
|
|
|
MtxPtr MTXPushInv(MTXStack* sPtr, const Mtx m) {
|
|
Mtx mInv;
|
|
assert(sPtr && "MTXPushInv(): NULL MtxStackPtr 'sPtr' ");
|
|
assert(sPtr->stackBase && "MTXPushInv(): 'sPtr' contains a NULL ptr to stack memory ");
|
|
assert(m && "MTXPushInv(): NULL MtxPtr 'm' ");
|
|
|
|
MTXInverse(m, mInv);
|
|
if (sPtr->stackPtr == NULL) {
|
|
sPtr->stackPtr = sPtr->stackBase;
|
|
MTXCopy(mInv, sPtr->stackPtr);
|
|
} else {
|
|
assert(((((uintptr_t)sPtr->stackPtr - (uintptr_t)sPtr->stackBase) / 16) / 3) < (sPtr->numMtx - 1) && "MTXPushInv(): stack overflow");
|
|
MTXConcat(mInv, sPtr->stackPtr, sPtr->stackPtr + 3);
|
|
sPtr->stackPtr += 3;
|
|
}
|
|
|
|
return sPtr->stackPtr;
|
|
}
|
|
|
|
MtxPtr MTXPushInvXpose(MTXStack* sPtr, const Mtx m) {
|
|
Mtx mIT;
|
|
assert(sPtr && "MTXPushInvXpose(): NULL MtxStackPtr 'sPtr' ");
|
|
assert(sPtr->stackBase && "MTXPushInvXpose(): 'sPtr' contains a NULL ptr to stack memory ");
|
|
assert(m && "MTXPushInvXpose(): NULL MtxPtr 'm' ");
|
|
|
|
MTXInverse(m, mIT);
|
|
MTXTranspose(mIT, mIT);
|
|
if (sPtr->stackPtr == NULL) {
|
|
sPtr->stackPtr = sPtr->stackBase;
|
|
MTXCopy(mIT, sPtr->stackPtr);
|
|
} else {
|
|
assert(((((uintptr_t)sPtr->stackPtr - (uintptr_t)sPtr->stackBase) / 16) / 3) < (sPtr->numMtx - 1) && "MTXPushInvXpose(): stack overflow ");
|
|
MTXConcat(sPtr->stackPtr, mIT, sPtr->stackPtr + 3);
|
|
sPtr->stackPtr += 3;
|
|
}
|
|
|
|
return sPtr->stackPtr;
|
|
}
|
|
|
|
MtxPtr MTXPop(MTXStack* sPtr) {
|
|
assert(sPtr && "MTXPop(): NULL MtxStackPtr 'sPtr' ");
|
|
assert(sPtr->stackBase && "MTXPop(): 'sPtr' contains a NULL ptr to stack memory ");
|
|
|
|
if (sPtr->stackPtr == NULL) {
|
|
return NULL;
|
|
}
|
|
|
|
if (sPtr->stackBase == sPtr->stackPtr) {
|
|
sPtr->stackPtr = NULL;
|
|
return NULL;
|
|
}
|
|
|
|
sPtr->stackPtr -= 3;
|
|
return sPtr->stackPtr;
|
|
}
|
|
|
|
MtxPtr MTXGetStackPtr(const MTXStack* sPtr) {
|
|
assert(sPtr && "MTXGetStackPtr(): NULL MtxStackPtr 'sPtr' ");
|
|
assert(sPtr->stackBase && "MTXGetStackPtr(): 'sPtr' contains a NULL ptr to stack memory ");
|
|
return sPtr->stackPtr;
|
|
}
|