From 1bc3170755794ad4c0338f54a310c8ff46112646 Mon Sep 17 00:00:00 2001 From: Vicki Pfau Date: Mon, 27 Feb 2017 19:02:24 -0800 Subject: [PATCH] DS GX: Add additional matrix operations --- include/mgba/internal/ds/matrix.h | 5 + src/ds/gx.c | 232 +++++++++++++++++++++++++++--- src/ds/matrix.c | 64 ++++++++- 3 files changed, 284 insertions(+), 17 deletions(-) diff --git a/include/mgba/internal/ds/matrix.h b/include/mgba/internal/ds/matrix.h index fdf4d357d..30b55cb08 100644 --- a/include/mgba/internal/ds/matrix.h +++ b/include/mgba/internal/ds/matrix.h @@ -10,11 +10,16 @@ CXX_GUARD_START +#define MTX_ONE 0x00001000 + struct DSGXMatrix { int32_t m[16]; // 20.12 }; void DSGXMtxIdentity(struct DSGXMatrix*); +void DSGXMtxMultiply(struct DSGXMatrix*, const struct DSGXMatrix*); +void DSGXMtxScale(struct DSGXMatrix*, const int32_t* m); +void DSGXMtxTranslate(struct DSGXMatrix*, const int32_t* m); CXX_GUARD_END diff --git a/src/ds/gx.c b/src/ds/gx.c index 1ba690e30..14351da61 100644 --- a/src/ds/gx.c +++ b/src/ds/gx.c @@ -194,22 +194,6 @@ static void _fifoRun(struct mTiming* timing, void* context, uint32_t cyclesLate) mLOG(DS_GX, GAME_ERROR, "Invalid GX MTX_MODE %02X", entry.params[0]); } break; - case DS_GX_CMD_MTX_IDENTITY: - switch (gx->mtxMode) { - case 0: - DSGXMtxIdentity(&gx->projMatrix); - break; - case 2: - DSGXMtxIdentity(&gx->vecMatrix); - // Fall through - case 1: - DSGXMtxIdentity(&gx->posMatrix); - break; - case 3: - DSGXMtxIdentity(&gx->texMatrix); - break; - } - break; case DS_GX_CMD_MTX_PUSH: switch (gx->mtxMode) { case 0: @@ -252,6 +236,222 @@ static void _fifoRun(struct mTiming* timing, void* context, uint32_t cyclesLate) } break; } + case DS_GX_CMD_MTX_IDENTITY: + switch (gx->mtxMode) { + case 0: + DSGXMtxIdentity(&gx->projMatrix); + break; + case 2: + DSGXMtxIdentity(&gx->vecMatrix); + // Fall through + case 1: + DSGXMtxIdentity(&gx->posMatrix); + break; + case 3: + DSGXMtxIdentity(&gx->texMatrix); + break; + } + break; + case DS_GX_CMD_MTX_LOAD_4x4: { + struct DSGXMatrix m; + int i; + for (i = 0; i < 16; ++i) { + m.m[i] = gx->activeEntries[i].params[0]; + m.m[i] |= gx->activeEntries[i].params[1] << 8; + m.m[i] |= gx->activeEntries[i].params[2] << 16; + m.m[i] |= gx->activeEntries[i].params[3] << 24; + } + switch (gx->mtxMode) { + case 0: + memcpy(&gx->projMatrix, &m, sizeof(gx->projMatrix)); + break; + case 2: + memcpy(&gx->vecMatrix, &m, sizeof(gx->vecMatrix)); + // Fall through + case 1: + memcpy(&gx->posMatrix, &m, sizeof(gx->posMatrix)); + break; + case 3: + memcpy(&gx->texMatrix, &m, sizeof(gx->texMatrix)); + break; + } + break; + } + case DS_GX_CMD_MTX_LOAD_4x3: { + struct DSGXMatrix m; + int i, j; + for (j = 0; j < 4; ++j) { + for (i = 0; i < 3; ++i) { + m.m[i + j * 4] = gx->activeEntries[i + j * 3].params[0]; + m.m[i + j * 4] |= gx->activeEntries[i + j * 3].params[1] << 8; + m.m[i + j * 4] |= gx->activeEntries[i + j * 3].params[2] << 16; + m.m[i + j * 4] |= gx->activeEntries[i + j * 3].params[3] << 24; + } + m.m[j * 4 + 3] = 0; + } + m.m[15] = MTX_ONE; + switch (gx->mtxMode) { + case 0: + memcpy(&gx->projMatrix, &m, sizeof(gx->projMatrix)); + break; + case 2: + memcpy(&gx->vecMatrix, &m, sizeof(gx->vecMatrix)); + // Fall through + case 1: + memcpy(&gx->posMatrix, &m, sizeof(gx->posMatrix)); + break; + case 3: + memcpy(&gx->texMatrix, &m, sizeof(gx->texMatrix)); + break; + } + break; + } + case DS_GX_CMD_MTX_MULT_4x4: { + struct DSGXMatrix m; + int i; + for (i = 0; i < 16; ++i) { + m.m[i] = gx->activeEntries[i].params[0]; + m.m[i] |= gx->activeEntries[i].params[1] << 8; + m.m[i] |= gx->activeEntries[i].params[2] << 16; + m.m[i] |= gx->activeEntries[i].params[3] << 24; + } + switch (gx->mtxMode) { + case 0: + DSGXMtxMultiply(&gx->projMatrix, &m); + break; + case 2: + DSGXMtxMultiply(&gx->vecMatrix, &m); + // Fall through + case 1: + DSGXMtxMultiply(&gx->posMatrix, &m); + break; + case 3: + DSGXMtxMultiply(&gx->texMatrix, &m); + break; + } + break; + } + case DS_GX_CMD_MTX_MULT_4x3: { + struct DSGXMatrix m; + int i, j; + for (j = 0; j < 4; ++j) { + for (i = 0; i < 3; ++i) { + m.m[i + j * 4] = gx->activeEntries[i + j * 3].params[0]; + m.m[i + j * 4] |= gx->activeEntries[i + j * 3].params[1] << 8; + m.m[i + j * 4] |= gx->activeEntries[i + j * 3].params[2] << 16; + m.m[i + j * 4] |= gx->activeEntries[i + j * 3].params[3] << 24; + } + m.m[j * 4 + 3] = 0; + } + m.m[15] = MTX_ONE; + switch (gx->mtxMode) { + case 0: + DSGXMtxMultiply(&gx->projMatrix, &m); + break; + case 2: + DSGXMtxMultiply(&gx->vecMatrix, &m); + // Fall through + case 1: + DSGXMtxMultiply(&gx->posMatrix, &m); + break; + case 3: + DSGXMtxMultiply(&gx->texMatrix, &m); + break; + } + break; + } + case DS_GX_CMD_MTX_MULT_3x3: { + struct DSGXMatrix m; + int i, j; + for (j = 0; j < 3; ++j) { + for (i = 0; i < 3; ++i) { + m.m[i + j * 4] = gx->activeEntries[i + j * 3].params[0]; + m.m[i + j * 4] |= gx->activeEntries[i + j * 3].params[1] << 8; + m.m[i + j * 4] |= gx->activeEntries[i + j * 3].params[2] << 16; + m.m[i + j * 4] |= gx->activeEntries[i + j * 3].params[3] << 24; + } + m.m[j * 4 + 3] = 0; + } + m.m[12] = 0; + m.m[13] = 0; + m.m[14] = 0; + m.m[15] = MTX_ONE; + switch (gx->mtxMode) { + case 0: + memcpy(&gx->projMatrix, &m, sizeof(gx->projMatrix)); + break; + case 2: + memcpy(&gx->vecMatrix, &m, sizeof(gx->vecMatrix)); + // Fall through + case 1: + memcpy(&gx->posMatrix, &m, sizeof(gx->posMatrix)); + break; + case 3: + memcpy(&gx->texMatrix, &m, sizeof(gx->projMatrix)); + break; + } + break; + } + case DS_GX_CMD_MTX_TRANS: { + int32_t m[3]; + m[0] = gx->activeEntries[0].params[0]; + m[0] |= gx->activeEntries[0].params[1] << 8; + m[0] |= gx->activeEntries[0].params[2] << 16; + m[0] |= gx->activeEntries[0].params[3] << 24; + m[1] = gx->activeEntries[1].params[0]; + m[1] |= gx->activeEntries[1].params[1] << 8; + m[1] |= gx->activeEntries[1].params[2] << 16; + m[1] |= gx->activeEntries[1].params[3] << 24; + m[2] = gx->activeEntries[2].params[0]; + m[2] |= gx->activeEntries[2].params[1] << 8; + m[2] |= gx->activeEntries[2].params[2] << 16; + m[2] |= gx->activeEntries[2].params[3] << 24; + switch (gx->mtxMode) { + case 0: + DSGXMtxTranslate(&gx->projMatrix, m); + break; + case 2: + DSGXMtxTranslate(&gx->vecMatrix, m); + // Fall through + case 1: + DSGXMtxTranslate(&gx->posMatrix, m); + break; + case 3: + DSGXMtxTranslate(&gx->texMatrix, m); + break; + } + break; + } + case DS_GX_CMD_MTX_SCALE: { + int32_t m[3]; + m[0] = gx->activeEntries[0].params[0]; + m[0] |= gx->activeEntries[0].params[1] << 8; + m[0] |= gx->activeEntries[0].params[2] << 16; + m[0] |= gx->activeEntries[0].params[3] << 24; + m[1] = gx->activeEntries[1].params[0]; + m[1] |= gx->activeEntries[1].params[1] << 8; + m[1] |= gx->activeEntries[1].params[2] << 16; + m[1] |= gx->activeEntries[1].params[3] << 24; + m[2] = gx->activeEntries[2].params[0]; + m[2] |= gx->activeEntries[2].params[1] << 8; + m[2] |= gx->activeEntries[2].params[2] << 16; + m[2] |= gx->activeEntries[2].params[3] << 24; + switch (gx->mtxMode) { + case 0: + DSGXMtxScale(&gx->projMatrix, m); + break; + case 2: + DSGXMtxScale(&gx->vecMatrix, m); + // Fall through + case 1: + DSGXMtxScale(&gx->posMatrix, m); + break; + case 3: + DSGXMtxScale(&gx->texMatrix, m); + break; + } + break; + } case DS_GX_CMD_SWAP_BUFFERS: gx->swapBuffers = true; break; diff --git a/src/ds/matrix.c b/src/ds/matrix.c index 1b7aaba3e..f52556e2e 100644 --- a/src/ds/matrix.c +++ b/src/ds/matrix.c @@ -5,7 +5,24 @@ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */ #include -#define MTX_ONE 0x00001000 +static int32_t _dot(const int32_t* col, const int32_t* row) { + int64_t a; + int64_t b; + int64_t sum; + a = col[0]; + b = row[0]; + sum = a * b; + a = col[4]; + b = row[1]; + sum += a * b; + a = col[8]; + b = row[2]; + sum += a * b; + a = col[12]; + b = row[3]; + sum += a * b; + return sum >> 12LL; +} void DSGXMtxIdentity(struct DSGXMatrix* mtx) { memset(mtx, 0, sizeof(*mtx)); @@ -14,3 +31,48 @@ void DSGXMtxIdentity(struct DSGXMatrix* mtx) { mtx->m[10] = MTX_ONE; mtx->m[15] = MTX_ONE; } + +void DSGXMtxMultiply(struct DSGXMatrix* mtx, const struct DSGXMatrix* m) { + struct DSGXMatrix out; + out.m[0] = _dot(&mtx->m[0], &m->m[0]); + out.m[1] = _dot(&mtx->m[1], &m->m[0]); + out.m[2] = _dot(&mtx->m[2], &m->m[0]); + out.m[3] = _dot(&mtx->m[3], &m->m[0]); + out.m[4] = _dot(&mtx->m[0], &m->m[4]); + out.m[5] = _dot(&mtx->m[1], &m->m[4]); + out.m[6] = _dot(&mtx->m[2], &m->m[4]); + out.m[7] = _dot(&mtx->m[3], &m->m[4]); + out.m[8] = _dot(&mtx->m[0], &m->m[8]); + out.m[9] = _dot(&mtx->m[1], &m->m[8]); + out.m[10] = _dot(&mtx->m[2], &m->m[8]); + out.m[11] = _dot(&mtx->m[3], &m->m[8]); + out.m[12] = _dot(&mtx->m[0], &m->m[12]); + out.m[13] = _dot(&mtx->m[1], &m->m[12]); + out.m[14] = _dot(&mtx->m[2], &m->m[12]); + out.m[15] = _dot(&mtx->m[3], &m->m[12]); + *mtx = out; +} + +void DSGXMtxScale(struct DSGXMatrix* mtx, const int32_t* m) { + struct DSGXMatrix s = { + .m = { + m[0], 0, 0, 0, + 0, m[1], 0, 0, + 0, 0, m[2], 0, + 0, 0, 0, MTX_ONE + } + }; + DSGXMtxMultiply(mtx, &s); +} + +void DSGXMtxTranslate(struct DSGXMatrix* mtx, const int32_t* m) { + struct DSGXMatrix t = { + .m = { + MTX_ONE, 0, 0, 0, + 0, MTX_ONE, 0, 0, + 0, 0, MTX_ONE, 0, + m[0], m[1], m[2], MTX_ONE + } + }; + DSGXMtxMultiply(mtx, &t); +}