diff --git a/source/main.c b/source/main.c new file mode 100644 index 0000000..5d58d7a --- /dev/null +++ b/source/main.c @@ -0,0 +1,543 @@ +// SPDX-License-Identifier: CC0-1.0 +// +// SPDX-FileContributor: Antonio Niño Díaz, 2022 + +#include + +#define GBA_SCREEN_W 240 +#define GBA_SCREEN_H 160 + +#define REG_DISPCNT *((volatile uint16_t *)0x04000000) + +#define REG_KEYINPUT *((volatile uint16_t *)0x04000130) +#define DEFAULT_REG_KEYINPUT *((volatile uint16_t *)0x04000130) +#define KEY_DOWN_NOW(key) (~(REG_KEYINPUT) & key) + +#define KEY_A 1 +#define KEY_B 2 +#define KEY_SELECT 3 +#define KEY_START 4 +#define KEY_DPAD_RIGHT 5 +#define KEY_DPAD_LEFT 6 +#define KEY_DPAD_UP 7 +#define KEY_DPAD_DOWN 8 +#define KEY_TRIGGER_LEFT 9 +#define KEY_TRIGGER_RIGHT 10 + +#define DISPCNT_BG_MODE_MASK (0x7) +#define DISPCNT_BG_MODE(n) ((n) & DISPCNT_BG_MODE_MASK) // 0 to 5 + +#define DISPCNT_BG2_ENABLE (1 << 10) + +#define MEM_VRAM_MODE3_FB ((uint16_t *)0x06000000) + +#define FIXED_POINT int32_t +#define fp 12 +#define SHIFT_THRESHOLD 0.05 +#define SHIFT_THRESHOLD_FP ((1 << fp) * SHIFT_THRESHOLD) + +#define FLOAT2FIXED(value) (int)((value) * (1 << fp)) +#define FIXED2FLOAT(value) ((value) / (float)(1 << fp)) + +static inline uint16_t +RGB15(uint16_t r, uint16_t g, uint16_t b) +{ + return (r & 0x1F) | ((g & 0x1F) << 5) | ((b & 0x1F) << 10); +} + +/////////////////////////////////////////////////////////// + +#include +#include +#include +#include +#include +#include + +#define VWIDTH 50 +#define VHEIGHT 50 +#define CUBE_WIDTH 10 +#define CUBE_WIDTH_FP ((1 << fp) * CUBE_WIDTH) + +enum faces { + FACE_FRONT = 0, + FACE_LEFT, + FACE_RIGHT, + FACE_BOTTOM, + FACE_TOP, + FACE_BACK, + NUM_FACES, +}; + +#define STEP 5 +#define STEP_FP ((1 << fp) * STEP) + +#define ACTION_STEP 0.1 +#define ACTION_STEP_FP ((1 << fp) * ACTION_STEP) + +#define PITCH_STEP 0.05 +#define ROLL_STEP 0.05 +#define YAW_STEP 0.05 + +volatile FIXED_POINT K1 = 60; +volatile FIXED_POINT K2 = (2 * CUBE_WIDTH) + 20; + + +#define MULT_FP(a,b) ((a * b) >> fp) + +#define SQ(n) (n * n) +#define SQ_FP(n) (MULT_FP(n, n)) + +#define COORD2INDEX(x, y) (y * VWIDTH + x) +#define COUPLE2INDEX(x) (COORD2INDEX(x[0], x[1])) + +#define GET_ROTATE_X_Q(a) ({ float _a = (FIXED2FLOAT(a)) ; \ + struct Quaternions q = {}; q.w = FLOAT2FIXED(cos(_a * .5)); \ + q.x = FLOAT2FIXED(sin(_a * .5)); q; }) +#define GET_ROTATE_Y_Q(a) ({ float _a = (FIXED2FLOAT(a)) ; \ + struct Quaternions q = {}; q.w = FLOAT2FIXED(cos(_a * .5)); \ + q.y = FLOAT2FIXED(sin(_a * .5)); q; }) +#define GET_ROTATE_Z_Q(a) ({ float _a = (FIXED2FLOAT(a)) ; \ + struct Quaternions q = {}; q.w = FLOAT2FIXED(cos(_a * .5)); \ + q.z = FLOAT2FIXED(sin(_a * .5)); q; }) + +//TODO Idle animations +#define IS_IDLE (Idle.x || Idle.y || Idle.z) +#define RESET_IDLE {Idle.x = 0; Idle.y = 0; Idle.z = 0;} + +struct { + char x; + char y; + char z; +} Idle; + + +struct Quaternions { + FIXED_POINT w; + FIXED_POINT x; + FIXED_POINT y; + FIXED_POINT z; +} Target, Current; + +FIXED_POINT interpolationStep = 0; +FIXED_POINT zBuffer[VHEIGHT * VWIDTH]; +char output[VHEIGHT * VWIDTH]; + +static volatile char shouldBreak = 1; +static volatile char currentlyMoving = 0; +static volatile char currentCountR = 0; +static volatile char frontFacingFace = FACE_FRONT; + +void +normalize(struct Quaternions *q) +{ + float n = sqrt(FIXED2FLOAT(SQ_FP(q->w) + SQ_FP(q->x) + + SQ_FP(q->y) + SQ_FP(q->z))); + if (n == 0) + return; + q->w = FLOAT2FIXED(FIXED2FLOAT(q->w) / n); + q->x = FLOAT2FIXED(FIXED2FLOAT(q->x) / n); + q->y = FLOAT2FIXED(FIXED2FLOAT(q->y) / n); + q->z = FLOAT2FIXED(FIXED2FLOAT(q->z) / n); +} + +struct Quaternions +mult(struct Quaternions q, FIXED_POINT x, FIXED_POINT y, FIXED_POINT z) +{ + //p = q * p * qbar + struct Quaternions res; + + res.w = 0; + res.x = MULT_FP(x, (SQ_FP(q.w) + SQ_FP(q.x) - SQ_FP(q.y) - SQ_FP(q.z))) + + (MULT_FP(y, (MULT_FP(q.x, q.y) - MULT_FP(q.w, q.z))) * 2) + + (MULT_FP(z, (MULT_FP(q.x, q.z) + MULT_FP(q.w, q.y))) * 2); + + res.y = (MULT_FP(x, (MULT_FP(q.x, q.y) + MULT_FP(q.w,q.z))) * 2) + + (MULT_FP(y, (SQ_FP(q.w) - SQ_FP(q.x) + SQ_FP(q.y) - SQ_FP(q.z)))) + + (MULT_FP(z, (MULT_FP(q.y, q.z) - MULT_FP(q.w, q.x))) << 2); + + res.z = (MULT_FP(x, (MULT_FP(q.x, q.z) - MULT_FP(q.w, q.y)))* 2) + + (MULT_FP(y, (MULT_FP(q.y, q.z) + MULT_FP(q.w, q.x))) * 2) + + MULT_FP(z, (SQ_FP(q.w) - SQ_FP(q.x) - SQ_FP(q.y) + SQ_FP(q.z))); + + return res; +} + +struct Quaternions +multQ(struct Quaternions p, struct Quaternions q) +{ + if (p.x <= SHIFT_THRESHOLD_FP && p.x >= -SHIFT_THRESHOLD_FP + && p.y <= SHIFT_THRESHOLD_FP && p.y >= -SHIFT_THRESHOLD_FP + && p.z <= SHIFT_THRESHOLD_FP && p.z >= -SHIFT_THRESHOLD_FP) + return q; + + if (q.x <= SHIFT_THRESHOLD_FP && q.x >= -SHIFT_THRESHOLD_FP + && q.y <= SHIFT_THRESHOLD_FP && q.y >= -SHIFT_THRESHOLD_FP + && q.z <= SHIFT_THRESHOLD_FP && q.z >= -SHIFT_THRESHOLD_FP) + return p; + + struct Quaternions res = { + .w = MULT_FP(p.w, q.w) - MULT_FP(p.x, q.x) - + MULT_FP(p.y, q.y) - MULT_FP(p.z, q.z), + .x = MULT_FP(p.w, q.x) + MULT_FP(p.x, q.w) + + MULT_FP(p.y, q.z) - MULT_FP(p.z, q.y), + .y = MULT_FP(p.w, q.y) - MULT_FP(p.x, q.z) + + MULT_FP(p.y, q.w) + MULT_FP(p.z, q.x), + .z = MULT_FP(p.w, q.z) + MULT_FP(p.x, q.y) - + MULT_FP(p.y, q.x) + MULT_FP(p.z, q.w), + }; + + return res; +} + +uint16_t +chooseColor(char c) +{ + switch (c) { + case FACE_FRONT: + return RGB15(31, 0, 0); + case FACE_BACK: + return RGB15(31, 15, 31); + case FACE_BOTTOM: + return RGB15(31, 0, 31); + case FACE_LEFT: + return RGB15(0, 0, 31); + case FACE_RIGHT: + return RGB15(0, 31, 31); + case FACE_TOP: + return RGB15(0, 31, 0); + default: + // BG + return RGB15(31, 31, 31); + } +} + +char +chooseMainFace() +{ + int total = 0; + int faces[NUM_FACES] = {0}; + + for (int k = 0; k < VWIDTH * VHEIGHT; ++k) + if (output[k] >= 0 && output[k] < NUM_FACES) { + faces[output[k]]++; + ++total; + } + + int max = 0, idx = 0; + for (int k = 0; k < NUM_FACES; ++k) + if (faces[k] > max) { + max = faces[k]; + idx = k; + } + + frontFacingFace = max > total * 0.9 ? idx : -1; + return frontFacingFace; +} + +char +isInQuad(char curr[2], char top[2], char left[2], + char right[2], char bot[2]) +{ + char *points[4] = {top, left, bot, right}; + + char pos = 0, neg = 0; + char x = curr[0]; + char y = curr[1];; + int d; + + for (char i = 0; i < 4; ++i) { + if (points[i][0] == curr[0] && points[i][1] == curr[1]) + return 1; + + //Form a segment between the i'th point + char x1 = points[i][0]; + char y1 = points[i][1]; + + //And the i+1'th, or if i is the last, with the first point + char i2 = (i + 1) % 4; + + char x2 = points[i2][0]; + char y2 = points[i2][1]; + + + //Compute the cross product + d = (x - x1) * (y2 - y1) - (y - y1) * (x2 - x1); + + if (d > 0) ++pos; + if (d < 0) ++neg; + + //If the sign changes, then point is outside + if (pos > 0 && neg > 0) + return 0; + } + + return 1; +} + +void +fill_quads(char current_face, char top[2], char left[2], + char right[2], char bot[2]) +{ + if (current_face != 0) return; + output[COUPLE2INDEX(top)] = RGB15(0, 0, 15); + output[COUPLE2INDEX(left)] = RGB15(0, 0, 15); + output[COUPLE2INDEX(right)] = RGB15(0, 0, 15); + + for (int y = top[1] ; y < bot[1] ; ++y) { + for (int x = left[0] ; x < right[0] ; ++x) { + char curr[2] = {x, y}; + if (isInQuad(curr, top, left, right, bot)) + //zbuffer issue + {} + //output[COORD2INDEX(x, y)] = current_face; + } + } +} + +void +detect_and_fill_quads() +{ + for (int current_face = 0 ; current_face < NUM_FACES ; ++current_face) { + char last_top [2] = {VWIDTH, VHEIGHT}; + char last_left[2] = {VWIDTH, 0}; + char last_right [2] = {0, 0}; + char last_bot[2] = {0, 0}; + char top [2] = {VWIDTH, VHEIGHT}; + char left[2] = {VWIDTH, 0}; + char right [2] = {0, 0}; + char bot[2] = {0, 0}; + for (char y = 0; y < VHEIGHT; ++y) { + for (char x = 0; x < VWIDTH; ++x) { + if (output[COORD2INDEX(x, y)] != current_face) + continue; + if (x <= left[0]) { + left[0] = x; + left[1] = y; + } + if (y <= top[1]) { + top[0] = x; + top[1] = y; + } + if (x >= right[0]) { + right[0] = x; + right[1] = y; + } + if (y >= bot[1]) { + bot[0] = x; + bot[1] = y; + } + } + } + fill_quads(current_face, top, left, right, bot); + } + +} + +void +printAscii() +{ + // TODO scale up + MEM_VRAM_MODE3_FB[120 + 80 * GBA_SCREEN_W] = RGB15(currentCountR, 31 - currentCountR, 0); + MEM_VRAM_MODE3_FB[136 + 80 * GBA_SCREEN_W] = RGB15(currentCountR, 31 - currentCountR, 0); + MEM_VRAM_MODE3_FB[120 + 96 * GBA_SCREEN_W] = RGB15(currentCountR, 31 - currentCountR, 0); + currentCountR = currentCountR == 31 ? 0 : 31; + + detect_and_fill_quads(); + + for (int i = 0; i < VHEIGHT; ++i) { + for (int j = 0; j < VWIDTH; ++j) { + char prevc = 0; + char *c = output + (i * VWIDTH + j); + MEM_VRAM_MODE3_FB[(i + 50) * GBA_SCREEN_W + j + 50] = chooseColor(*c); + } + } +} + +void +rotateCube(FIXED_POINT cubeX, FIXED_POINT cubeY, FIXED_POINT cubeZ, char ch) +{ + struct Quaternions q = mult(Current, cubeX, cubeY, cubeZ); + + int x = q.x >> fp; + int y = q.y >> fp; + + // not fixed point yet!! + float invZ = (1 << fp) / (float)(q.z + K2 * (1 << fp)); + + int screenX = (int)(VWIDTH * 0.5) + (int)((x) * K1) * invZ; + int screenY = (int)(VHEIGHT * 0.5) + (int)((y) * K1) * invZ; + //TODO luminescence + + if (screenX > VWIDTH || screenX < 0) return; + + int idx = screenY * VWIDTH + screenX; + if (idx >= 0 && idx < VWIDTH * VHEIGHT) { + invZ = FLOAT2FIXED(invZ); + if (zBuffer[idx] < invZ) { + zBuffer[idx] = invZ; + output[idx] = ch; + } + } +} + +struct Quaternions +interpolate(struct Quaternions qa, struct Quaternions qb) +{ + frontFacingFace = -1; + struct Quaternions res; + float cosHalfTheta = + FIXED2FLOAT(MULT_FP(qa.w, qb.w) + + MULT_FP(qa.x, qb.x) + + MULT_FP(qa.y, qb.y) + + MULT_FP(qa.z, qb.z)); + //if qa = qb or qa = -qb then theta = 0 and we can return qa + if (cosHalfTheta >= 1.0 || cosHalfTheta <= -1.0) { + res.w = qa.w; + res.x = qa.x; + res.y = qa.y; + res.z = qa.z; + goto exit; + } + if (cosHalfTheta < 0) { + qb.w = -qb.w; + qb.x = -qb.x; + qb.y = -qb.y; + qb.z = qb.z; + cosHalfTheta = -cosHalfTheta; + } + + float halfTheta = acos(cosHalfTheta); + float sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta); + //if theta = 180 degrees then result is not fully defined + // we could rotate around any axis normal to qa or qb + if (sinHalfTheta < 0.001 && sinHalfTheta > -0.001) { + res.w = ((qa.w >> 1) + (qb.w >> 1)); + res.x = ((qa.x >> 1) + (qb.x >> 1)); + res.y = ((qa.y >> 1) + (qb.y >> 1)); + res.z = ((qa.z >> 1) + (qb.z >> 1)); + goto exit; + } + + + FIXED_POINT ratioA = FLOAT2FIXED(sin((1 - FIXED2FLOAT(interpolationStep)) * halfTheta) / sinHalfTheta); + FIXED_POINT ratioB = FLOAT2FIXED(sin(FIXED2FLOAT(interpolationStep) * halfTheta) / sinHalfTheta); + + res.w = (MULT_FP(qa.w, ratioA) + MULT_FP(qb.w, ratioB)); + res.x = (MULT_FP(qa.x, ratioA) + MULT_FP(qb.x, ratioB)); + res.y = (MULT_FP(qa.y, ratioA) + MULT_FP(qb.y, ratioB)); + res.z = (MULT_FP(qa.z, ratioA) + MULT_FP(qb.z, ratioB)); + +exit: + interpolationStep += ACTION_STEP_FP; + return res; +} + +void +handleAngle(char input) +{ + // TODO + if (currentlyMoving == 0) { + currentlyMoving = input; + switch (input) { + case 'w': + case 'W': + Target = multQ(GET_ROTATE_X_Q(FLOAT2FIXED(M_PI_2)), Current); + break; + case 'a': + case 'A': + Target = multQ(GET_ROTATE_Y_Q(-FLOAT2FIXED(M_PI_2)), Current); + break; + case 's': + case 'S': + Target = multQ(GET_ROTATE_X_Q(-FLOAT2FIXED(M_PI_2)), Current); + break; + case 'd': + case 'D': + Target = multQ(GET_ROTATE_Y_Q(FLOAT2FIXED(M_PI_2)), Current); + break; + case 'q': + case 'Q': + Target = multQ(GET_ROTATE_Z_Q(-FLOAT2FIXED(M_PI_2)), Current); + break; + case 'e': + case 'E': + Target = multQ(GET_ROTATE_Z_Q(FLOAT2FIXED(M_PI_2)), Current); + break; + default: + currentlyMoving = 0; + //TODO idle movement + } + normalize(&Target); + } else { + if (interpolationStep < (1 << fp) - ACTION_STEP_FP * 2) { + Current = interpolate(Current, Target); + normalize(&Current); + } + else { + Current = Target; + interpolationStep = 0; + currentlyMoving = 0; + } + } +} + +char +getInput() +{ + // TODO + char c = 'd'; + handleAngle(c); + return c; +} + + +int +main() +{ + REG_DISPCNT = DISPCNT_BG_MODE(3) | DISPCNT_BG2_ENABLE; + + Current = GET_ROTATE_Z_Q(0); + + while (1) { + memset(output, NUM_FACES, VWIDTH * VHEIGHT); + memset(zBuffer, 0xffffffff, VWIDTH * VHEIGHT * sizeof(FIXED_POINT)); + + for (FIXED_POINT cubeX = -CUBE_WIDTH_FP + STEP_FP ; + cubeX <= CUBE_WIDTH_FP - STEP_FP; cubeX += STEP_FP) { + for (FIXED_POINT cubeY = -CUBE_WIDTH_FP + STEP_FP; + cubeY <= CUBE_WIDTH_FP - STEP_FP; cubeY += STEP_FP) { + switch (FACE_FRONT) { + case FACE_FRONT: + rotateCube(cubeX, cubeY, -CUBE_WIDTH_FP, FACE_FRONT); + break; + case FACE_LEFT: + rotateCube(-CUBE_WIDTH_FP, cubeX, cubeY, FACE_LEFT); + break; + case FACE_RIGHT: + rotateCube(CUBE_WIDTH_FP, cubeX, cubeY, FACE_RIGHT); + break; + case FACE_BOTTOM: + rotateCube(cubeX, -CUBE_WIDTH_FP, cubeY, FACE_BOTTOM); + break; + case FACE_TOP: + rotateCube(cubeX, CUBE_WIDTH_FP, cubeY, FACE_TOP); + break; + case FACE_BACK: + rotateCube(cubeX, cubeY, CUBE_WIDTH_FP, FACE_BACK); + break; + default: // idk render all + rotateCube(cubeX, cubeY, -CUBE_WIDTH_FP, FACE_FRONT); + rotateCube(-CUBE_WIDTH_FP, cubeX, cubeY, FACE_LEFT); + rotateCube(CUBE_WIDTH_FP, cubeX, cubeY, FACE_RIGHT); + rotateCube(cubeX, -CUBE_WIDTH_FP, cubeY, FACE_TOP); + rotateCube(cubeX, CUBE_WIDTH_FP, cubeY, FACE_BOTTOM); + rotateCube(cubeX, cubeY, CUBE_WIDTH_FP, FACE_BACK); + } + } + } + + printAscii(); + getInput(); + } +}