feature: WIP GBA

cube is turning, but has some scalling issue vertically while turning allong z
also need to implement quad detection along Y axis
This commit is contained in:
violette 2025-08-17 21:44:27 -04:00
parent 4f8fde9eb5
commit d8f573c678

516
source/main.c Normal file
View file

@ -0,0 +1,516 @@
// SPDX-License-Identifier: CC0-1.0
//
// SPDX-FileContributor: Antonio Niño Díaz, 2022
#include <stdint.h>
#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 <stdio.h>
#include <math.h>
#include <unistd.h>
#include <string.h>
#include <fcntl.h>
#include <float.h>
#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) (x * VWIDTH + y)
#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, Last;
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;
}
void
fill_quads(char current_face, char top_left[2], char top_right[2],
char bot_left[2], char bot_right[2])
{
// calc slope foreach side
float slope_top = 0;
float slope_bot = 0;
float slope_left = INFINITY;
float slope_right = -INFINITY;
if (top_left[0] != top_right[0])
slope_top = (float)(top_right[1] - top_left[1]) /
(float)(top_right[0] - top_left[0]);
if (bot_left[0] == bot_right[0])
slope_bot = (float)(bot_right[1] - bot_left[1]) /
(float)(bot_right[0] - bot_left[0]);
if (top_left[0] == bot_left[0])
slope_left = (float)(bot_left[1] - top_left[1]) /
(float)(bot_left[0] - top_left[0]);
if (top_right[0] == bot_right[0])
slope_right = (float)(bot_right[1] - top_right[1]) /
(float)(bot_right[0] - top_right[0]);
int top = top_right[1] > top_left[1] ? top_right[1]: top_left[1];
int bot = bot_right[1] > bot_left[1] ? bot_right[1]: bot_left[1];
int left = top_left[0] > bot_left[0] ? top_left[0]: bot_left[0];
int right = top_right[0] > bot_right[0] ? top_right[0]: bot_right[0];
for (int y = top ; y <= bot ; ++y) {
for (int x = left ; x <= right ; ++x) {
if (slope_top * x + top <= y)
// TODO side check
output[COORD2INDEX(x, y)] = current_face;
}
}
}
void
detect_and_fill_quads()
{
for (int current_face = 0 ; current_face < NUM_FACES ; ++current_face) {
char top_left [2] = {VWIDTH, VHEIGHT};
char top_right[2] = {0, VHEIGHT};
char bot_left [2] = {VWIDTH, 0};
char bot_right[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 <= top_left[0] && y <= top_left[1]){
top_left[0] = x;
top_left[1] = y;
}
if (x >= top_right[0] && y <= top_right[1]){
top_right[0] = x;
top_right[1] = y;
}
if (x <= bot_left[0] && y >= bot_left[1]){
bot_left[0] = x;
bot_left[1] = y;
}
if (x >= bot_right[0] && y >= bot_right[1]){
bot_right[0] = x;
bot_right[1] = y;
}
}
}
fill_quads(current_face, top_left, top_right, bot_left, bot_right);
}
}
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;
Last = Current;
switch (input) {
case 'w':
case 'W':
Target = multQ(GET_ROTATE_X_Q(FLOAT2FIXED(M_PI_2)), Target);
break;
case 'a':
case 'A':
Target = multQ(GET_ROTATE_Y_Q(-FLOAT2FIXED(M_PI_2)), Target);
break;
case 's':
case 'S':
Target = multQ(GET_ROTATE_X_Q(-FLOAT2FIXED(M_PI_2)), Target);
break;
case 'd':
case 'D':
Target = multQ(GET_ROTATE_Y_Q(FLOAT2FIXED(M_PI_2)), Target);
break;
case 'q':
case 'Q':
Target = multQ(GET_ROTATE_Z_Q(-FLOAT2FIXED(M_PI_2)), Target);
break;
case 'e':
case 'E':
Target = multQ(GET_ROTATE_Z_Q(FLOAT2FIXED(M_PI_2)), Target);
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 (frontFacingFace) {
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();
}
}