violette/cube
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

feat: Scaling + qol stuff + some opti

Browse source
This commit is contained in:
violette 2025-09-04 07:09:17 -04:00
parent 326f6a894c
commit 28295eb8d6
2 changed files with 251 additions and 196 deletions
Download patch file Download diff file Expand all files Collapse all files

2
gbafix/Makefile
View file

@ -4,7 +4,7 @@
NAME := gbafix NAME := gbafix
CC := gcc # CC := gcc
RM := rm -rf RM := rm -rf
# `make V=` builds the binary in verbose build mode # `make V=` builds the binary in verbose build mode

449
source/main.c
View file

@ -3,6 +3,7 @@
// SPDX-FileContributor: Antonio Niño Díaz, 2022 // SPDX-FileContributor: Antonio Niño Díaz, 2022
#include <stdint.h> #include <stdint.h>
#include <stdlib.h>
#define GBA_SCREEN_W 240 #define GBA_SCREEN_W 240
#define GBA_SCREEN_H 160 #define GBA_SCREEN_H 160
@ -29,15 +30,24 @@
#define DISPCNT_BG2_ENABLE (1 << 10) #define DISPCNT_BG2_ENABLE (1 << 10)
#define MEM_VRAM_MODE3_FB ((uint16_t *)0x06000000) #define MEM_VRAM_MODE4 ((uint8_t *)buffer)
#define SHOW_BACK 0x10;
#define FRONT_BUFFER (0x6000000)
#define BACK_BUFFER (0x600A000)
#define PALETTE ((uint16_t *)0x5000000)
static volatile uint8_t lastPaletteIndex = 0;
#define FIXED_POINT int32_t #define FIXED_POINT int32_t
#define fp 12 #define FP 12
#define SHIFT_THRESHOLD 0.05 #define SHIFT_THRESHOLD 0.05
#define SHIFT_THRESHOLD_FP ((1 << fp) * SHIFT_THRESHOLD) #define SHIFT_THRESHOLD_FP ((1 << FP) * SHIFT_THRESHOLD)
#define FLOAT2FIXED(value) (int)((value) * (1 << fp)) #define FLOAT2FIXED(value) (int)((value) * (1 << FP))
#define FIXED2FLOAT(value) ((value) / (float)(1 << fp)) #define FIXED2FLOAT(value) ((value) / (float)(1 << FP))
static uint8_t *buffer = (uint8_t *)FRONT_BUFFER;
static inline uint16_t static inline uint16_t
RGB15(uint16_t r, uint16_t g, uint16_t b) RGB15(uint16_t r, uint16_t g, uint16_t b)
@ -45,6 +55,30 @@ RGB15(uint16_t r, uint16_t g, uint16_t b)
return (r & 0x1F) | ((g & 0x1F) << 5) | ((b & 0x1F) << 10); return (r & 0x1F) | ((g & 0x1F) << 5) | ((b & 0x1F) << 10);
} }
void
flipBuffers()
{
if(buffer == (uint8_t *)FRONT_BUFFER) {
REG_DISPCNT &= ~SHOW_BACK;
buffer = (uint8_t *)BACK_BUFFER;
} else {
REG_DISPCNT |= SHOW_BACK;
buffer = (uint8_t *)FRONT_BUFFER;
}
}
void
putPx(uint8_t x, uint8_t y, uint16_t c)
{
uint16_t pos = (GBA_SCREEN_W * y + x) >> 1;
uint8_t px = buffer[pos];
if (y & 1) {
buffer[pos] = c << 8;
buffer[pos] = (c << 8) | (px & 0x00ff);
} else {
buffer[pos] = (px & 0xff00) | c;
}
}
/////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////
#include <math.h> #include <math.h>
@ -53,10 +87,10 @@ RGB15(uint16_t r, uint16_t g, uint16_t b)
#include <fcntl.h> #include <fcntl.h>
#include <float.h> #include <float.h>
#define VWIDTH 50 #define VWIDTH 25
#define VHEIGHT 50 #define VHEIGHT 25
#define CUBE_WIDTH 10 #define CUBE_WIDTH 10
#define CUBE_WIDTH_FP ((1 << fp) * CUBE_WIDTH) #define CUBE_WIDTH_FP ((1 << FP) * CUBE_WIDTH)
enum faces { enum faces {
FACE_FRONT = 0, FACE_FRONT = 0,
@ -68,27 +102,28 @@ enum faces {
NUM_FACES, NUM_FACES,
}; };
#define STEP 5 #define STEP 20
#define STEP_FP ((1 << fp) * STEP) #define STEP_FP ((1 << FP) * STEP)
#define ACTION_STEP 0.1 #define ACTION_STEP 0.1
#define ACTION_STEP_FP ((1 << fp) * ACTION_STEP) #define ACTION_STEP_FP ((1 << FP) * ACTION_STEP)
#define PITCH_STEP 0.05 #define SCALE 10 // how much is our initial render scaled
#define ROLL_STEP 0.05
#define YAW_STEP 0.05
volatile FIXED_POINT K1 = 60; volatile FIXED_POINT K1 = 20;
volatile FIXED_POINT K2 = (2 * CUBE_WIDTH) + 20; volatile FIXED_POINT K2 = (2 * CUBE_WIDTH) + 10;
#define MULT_FP(a,b) ((a * b) >> FP)
#define MULT_FP(a,b) ((a * b) >> fp)
#define SQ(n) (n * n) #define SQ(n) (n * n)
#define SQ_FP(n) (MULT_FP(n, n)) #define SQ_FP(n) (MULT_FP(n, n))
#define COORD2INDEX(x, y) (y * VWIDTH + x) #define COORD2INDEX(x, y) (y * VWIDTH + x)
#define COUPLE2INDEX(x) (COORD2INDEX(x[0], x[1])) #define COUPLE2INDEX(x) (COORD2INDEX(x[0], x[1]))
#define PLOT_COORD(x, y, c) \
for (uint8_t i = 0 ; i < SCALE ; ++i) \
for (uint8_t j = 0 ; j < SCALE ; ++j) \
putPx(x * SCALE + i, j + (SCALE * y), chooseColor(c));
#define GET_ROTATE_X_Q(a) ({ float _a = (a) ; \ #define GET_ROTATE_X_Q(a) ({ float _a = (a) ; \
struct Quaternions q = {}; q.w = FLOAT2FIXED(cos(_a * .5)); \ struct Quaternions q = {}; q.w = FLOAT2FIXED(cos(_a * .5)); \
@ -104,10 +139,28 @@ volatile FIXED_POINT K2 = (2 * CUBE_WIDTH) + 20;
#define IS_IDLE (Idle.x || Idle.y || Idle.z) #define IS_IDLE (Idle.x || Idle.y || Idle.z)
#define RESET_IDLE {Idle.x = 0; Idle.y = 0; Idle.z = 0;} #define RESET_IDLE {Idle.x = 0; Idle.y = 0; Idle.z = 0;}
void
init_colors()
{
const uint16_t color_order[NUM_FACES + 1] = {
RGB15(0, 0, 0),
RGB15(31, 0, 0),
RGB15(0, 31, 0),
RGB15(0, 0, 31),
RGB15(0, 31, 31),
RGB15(31, 31, 0),
RGB15(31, 0, 31),
};
for ( ; lastPaletteIndex <= NUM_FACES ; ++lastPaletteIndex) {
PALETTE[lastPaletteIndex] = color_order[lastPaletteIndex];
}
}
struct { struct {
char x; uint8_t x;
char y; uint8_t y;
char z; uint8_t z;
} Idle; } Idle;
@ -118,22 +171,24 @@ struct Quaternions {
FIXED_POINT z; FIXED_POINT z;
} Target, Current; } Target, Current;
FIXED_POINT interpolationStep = 0; static FIXED_POINT interpolationStep = 0;
FIXED_POINT zBuffer[VHEIGHT * VWIDTH]; static FIXED_POINT zBuffer[VHEIGHT * VWIDTH];
char output[VHEIGHT * VWIDTH]; static FIXED_POINT maxZbufByColor[NUM_FACES * 2];
static uint8_t output[VHEIGHT * VWIDTH];
static volatile char shouldBreak = 1; static volatile uint8_t shouldBreak = 1;
static volatile char currentlyMoving = 0; static volatile uint8_t currentlyMoving = 0;
static volatile char currentCountR = 0; static volatile uint8_t currentCountR = 0;
static volatile char frontFacingFace = FACE_FRONT; static volatile uint8_t frontFacingFace = FACE_FRONT;
void void
normalize(struct Quaternions *q) normalize(struct Quaternions *q)
{ {
float n = sqrt(FIXED2FLOAT(SQ_FP(q->w) + SQ_FP(q->x) + float n = sqrt(FIXED2FLOAT(SQ_FP(q->w) + SQ_FP(q->x) +
SQ_FP(q->y) + SQ_FP(q->z))); SQ_FP(q->y) + SQ_FP(q->z)));
if (n == 0) if (n == 0 || n == 1)
return; return;
q->w = FLOAT2FIXED(FIXED2FLOAT(q->w) / n); q->w = FLOAT2FIXED(FIXED2FLOAT(q->w) / n);
q->x = FLOAT2FIXED(FIXED2FLOAT(q->x) / n); q->x = FLOAT2FIXED(FIXED2FLOAT(q->x) / n);
q->y = FLOAT2FIXED(FIXED2FLOAT(q->y) / n); q->y = FLOAT2FIXED(FIXED2FLOAT(q->y) / n);
@ -141,77 +196,68 @@ normalize(struct Quaternions *q)
} }
struct Quaternions struct Quaternions
mult(struct Quaternions q, FIXED_POINT x, FIXED_POINT y, FIXED_POINT z) mult(struct Quaternions *q, FIXED_POINT x, FIXED_POINT y, FIXED_POINT z)
{ {
//p = q * p * qbar //p = q * p * qbar
struct Quaternions res; struct Quaternions res;
res.w = 0; res.w = 0;
res.x = MULT_FP(x, (SQ_FP(q.w) + SQ_FP(q.x) - SQ_FP(q.y) - SQ_FP(q.z))) 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(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); + (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) 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(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); + (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) 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(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))); + MULT_FP(z, (SQ_FP(q->w) - SQ_FP(q->x) - SQ_FP(q->y) + SQ_FP(q->z)));
return res; return res;
} }
struct Quaternions // res in quat p
multQ(struct Quaternions p, struct Quaternions q) void
multQ(struct Quaternions *p, struct Quaternions *q)
{ {
if (p.x <= SHIFT_THRESHOLD_FP && p.x >= -SHIFT_THRESHOLD_FP if (p->x <= SHIFT_THRESHOLD_FP && p->x >= -SHIFT_THRESHOLD_FP
&& p.y <= SHIFT_THRESHOLD_FP && p.y >= -SHIFT_THRESHOLD_FP && p->y <= SHIFT_THRESHOLD_FP && p->y >= -SHIFT_THRESHOLD_FP
&& p.z <= SHIFT_THRESHOLD_FP && p.z >= -SHIFT_THRESHOLD_FP) && p->z <= SHIFT_THRESHOLD_FP && p->z >= -SHIFT_THRESHOLD_FP) {
return q; p = q;
return;
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;
} }
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;
FIXED_POINT 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);
FIXED_POINT 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);
FIXED_POINT 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);
FIXED_POINT 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);
p->w = w;
p->x = x;
p->y = y;
p->z = z;
}
uint16_t uint16_t
chooseColor(char c) chooseColor(uint8_t c)
{ {
switch (c) { if (c >= 1 && c <= NUM_FACES)
case FACE_FRONT: return c;
return RGB15(31, 0, 0); else
case FACE_BACK: return 0;
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 uint8_t
chooseMainFace() chooseMainFace()
{ {
int total = 0; int total = 0;
@ -234,30 +280,27 @@ chooseMainFace()
return frontFacingFace; return frontFacingFace;
} }
char uint8_t
isInQuad(char curr[2], char top[2], char left[2], isInQuad(uint8_t curr[2], uint8_t points[8])
char right[2], char bot[2])
{ {
char *points[4] = {top, left, bot, right}; uint8_t pos = 0, neg = 0;
uint8_t x = curr[0];
char pos = 0, neg = 0; uint8_t y = curr[1];;
char x = curr[0];
char y = curr[1];;
int d; int d;
for (char i = 0; i < 4; ++i) { for (uint8_t i = 0; i < 4; ++i) {
if (points[i][0] == curr[0] && points[i][1] == curr[1]) if (points[2 * i] == curr[0] && points[2 * i + 1] == curr[1])
return 1; return 1;
//Form a segment between the i'th point //Form a segment between the i'th point
char x1 = points[i][0]; uint8_t x1 = points[2 * i];
char y1 = points[i][1]; uint8_t y1 = points[2 * i + 1];
//And the i+1'th, or if i is the last, with the first point //And the i+1'th, or if i is the last, with the first point
char i2 = (i + 1) % 4; uint8_t i2 = (i + 1) % 4;
char x2 = points[i2][0]; uint8_t x2 = points[2 * i2];
char y2 = points[i2][1]; uint8_t y2 = points[2 * i2 + 1];
//Compute the cross product //Compute the cross product
@ -275,90 +318,97 @@ isInQuad(char curr[2], char top[2], char left[2],
} }
void void
fill_quads(char current_face, char top[2], char left[2], fill_quads(uint8_t *points, uint8_t current_face)
char right[2], char bot[2])
{ {
for (int y = top[1] ; y < bot[1] ; ++y) { uint8_t top = 0, bot = 0, left = 0, right = 0;
for (int x = left[0] ; x < right[0] ; ++x) { for (uint8_t x = 0 ; x < 8 ; ++x) {
char curr[2] = {x, y}; if (x % 2 == 0) {
if (isInQuad(curr, top, left, right, bot)) if (points[x] > right)
//zbuffer issue right = points[x];
output[COORD2INDEX(x, y)] = current_face; if (points[x] < left)
left = points[x];
} else {
if (points[x] > bot)
bot = points[x];
if (points[x] < top)
top = points[x];
}
}
for (int y = top ; y < bot ; ++y) {
for (int x = left ; x < right ; ++x) {
uint8_t curr[2] = {x, y};
if (isInQuad(curr, points))
PLOT_COORD(x, y, current_face + 1);
} }
} }
} }
float uint8_t
dist(char a[2], char b[2]) { detect(uint8_t *points, uint8_t current_face) {
return (a[0] - b[0]) * (a[0] - b[0]) + (a[1] - b[1]) * (a[1] - b[1]); uint8_t i = 0;
for (uint8_t y = 0; y < VHEIGHT; ++y) {
for (uint8_t x = 0; x < VWIDTH; ++x) {
if (output[COORD2INDEX(x, y)] != current_face)
continue;
// only 4 points are ploted
points[i] = x;
points[i + 1] = y;
i += 2;
}
} }
for (uint8_t x = 0 ; x < 8 ; ++x)
if (points[x] == 0)
return 0;
return 1;
}
int
comp(const void *p1, const void *p2) {
FIXED_POINT left = *(const FIXED_POINT *)p1;
FIXED_POINT right = *(const FIXED_POINT *)p2;
return ((left > right) - (left < right));
}
void void
detect_and_fill_quads() detect_and_fill_quads()
{ {
for (int current_face = 0 ; current_face < NUM_FACES ; ++current_face) { qsort(maxZbufByColor, NUM_FACES, 2 * sizeof(FIXED_POINT), comp);
char top [2] = {VWIDTH, VHEIGHT}; for (uint8_t idx = 0 ; idx < NUM_FACES ; ++idx) {
char left[2] = {VWIDTH, 0}; uint8_t current_face = maxZbufByColor[2 * idx + 1];
char right [2] = {0, 0}; uint8_t points[8] = { 0 };
char bot[2] = {0, 0}; if (detect(points, current_face))
for (char y = 0; y < VHEIGHT; ++y) { fill_quads(points, current_face);
for (char x = 0; x < VWIDTH; ++x) {
if (output[COORD2INDEX(x, y)] != current_face)
continue;
if (y < top[1]) {
top[0] = x;
top[1] = y;
} }
if (x > right[0] && (x != top[0] || y != top [1])) {
right[0] = x;
right[1] = y;
}
if (x <= left[0] && (x != top[0] || y != top [1])) { // <= to force it bot
left[0] = x;
left[1] = y;
}
if (y >= bot[1] && (x != right[0] || y != right [1]) && (x != left[0] || y != left[1])) {
bot[0] = x;
bot[1] = y;
}
}
}
fill_quads(current_face, top, left, right, bot);
}
} }
void void
printAscii() 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(); detect_and_fill_quads();
flipBuffers();
for (int i = 0; i < VHEIGHT; ++i) { // DISPLAY POINTS
for (int j = 0; j < VWIDTH; ++j) { //for (int i = 0; i < VHEIGHT; ++i) {
char prevc = 0; // for (int j = 0; j < VWIDTH; ++j) {
char *c = output + (i * VWIDTH + j); // uint8_t prevc = 0;
MEM_VRAM_MODE3_FB[(i + 50) * GBA_SCREEN_W + j + 50] = chooseColor(*c); // uint8_t c = output[i * VWIDTH + j];
} // MEM_VRAM_MODE4[i * GBA_SCREEN_W + j] = chooseColor(c);
} // }
//}
} }
void void
rotateCube(FIXED_POINT cubeX, FIXED_POINT cubeY, FIXED_POINT cubeZ, char ch) rotateCube(FIXED_POINT cubeX, FIXED_POINT cubeY, FIXED_POINT cubeZ, uint8_t ch)
{ {
struct Quaternions q = mult(Current, cubeX, cubeY, cubeZ); struct Quaternions q = mult(&Current, cubeX, cubeY, cubeZ);
int x = q.x >> fp; int x = q.x >> FP;
int y = q.y >> fp; int y = q.y >> FP;
// not fixed point yet!! // not fixed point yet!!
float invZ = (1 << fp) / (float)(q.z + K2 * (1 << fp)); float invZ = (1 << FP) / (float)(q.z + K2 * (1 << FP));
int screenX = (int)(VWIDTH * 0.5) + (int)((x) * K1) * invZ; int screenX = (int)(VWIDTH * 0.5) + (int)((x) * K1) * invZ;
int screenY = (int)(VHEIGHT * 0.5) + (int)((y) * K1) * invZ; int screenY = (int)(VHEIGHT * 0.5) + (int)((y) * K1) * invZ;
@ -372,33 +422,32 @@ rotateCube(FIXED_POINT cubeX, FIXED_POINT cubeY, FIXED_POINT cubeZ, char ch)
if (zBuffer[idx] < invZ) { if (zBuffer[idx] < invZ) {
zBuffer[idx] = invZ; zBuffer[idx] = invZ;
output[idx] = ch; output[idx] = ch;
if (invZ > maxZbufByColor[ch]) {
maxZbufByColor[2 * ch] = invZ;
maxZbufByColor[2 * ch + 1] = ch; //palette[0] is bg
}
} }
} }
} }
struct Quaternions void
interpolate(struct Quaternions qa, struct Quaternions qb) interpolate(struct Quaternions *qa, struct Quaternions *qb)
{ {
frontFacingFace = -1; frontFacingFace = -1;
struct Quaternions res;
float cosHalfTheta = float cosHalfTheta =
FIXED2FLOAT(MULT_FP(qa.w, qb.w) + FIXED2FLOAT(MULT_FP(qa->w, qb->w) +
MULT_FP(qa.x, qb.x) + MULT_FP(qa->x, qb->x) +
MULT_FP(qa.y, qb.y) + MULT_FP(qa->y, qb->y) +
MULT_FP(qa.z, qb.z)); MULT_FP(qa->z, qb->z));
//if qa = qb or qa = -qb then theta = 0 and we can return qa //if qa = qb or qa = -qb then theta = 0 and we can return qa
if (cosHalfTheta >= 1.0 || cosHalfTheta <= -1.0) { 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; goto exit;
} }
if (cosHalfTheta < 0) { if (cosHalfTheta < 0) {
qb.w = -qb.w; qb->w = -qb->w;
qb.x = -qb.x; qb->x = -qb->x;
qb.y = -qb.y; qb->y = -qb->y;
qb.z = qb.z; qb->z = qb->z;
cosHalfTheta = -cosHalfTheta; cosHalfTheta = -cosHalfTheta;
} }
@ -406,11 +455,11 @@ interpolate(struct Quaternions qa, struct Quaternions qb)
float sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta); float sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta);
//if theta = 180 degrees then result is not fully defined //if theta = 180 degrees then result is not fully defined
// we could rotate around any axis normal to qa or qb // we could rotate around any axis normal to qa or qb
if (sinHalfTheta < 0.001 && sinHalfTheta > -0.001) { if (sinHalfTheta < 0.01 && sinHalfTheta > -0.01) {
res.w = ((qa.w >> 1) + (qb.w >> 1)); qa->w = ((qa->w >> 1) + (qb->w >> 1));
res.x = ((qa.x >> 1) + (qb.x >> 1)); qa->x = ((qa->x >> 1) + (qb->x >> 1));
res.y = ((qa.y >> 1) + (qb.y >> 1)); qa->y = ((qa->y >> 1) + (qb->y >> 1));
res.z = ((qa.z >> 1) + (qb.z >> 1)); qa->z = ((qa->z >> 1) + (qb->z >> 1));
goto exit; goto exit;
} }
@ -418,55 +467,58 @@ interpolate(struct Quaternions qa, struct Quaternions qb)
FIXED_POINT ratioA = FLOAT2FIXED(sin((1 - FIXED2FLOAT(interpolationStep)) * halfTheta) / sinHalfTheta); FIXED_POINT ratioA = FLOAT2FIXED(sin((1 - FIXED2FLOAT(interpolationStep)) * halfTheta) / sinHalfTheta);
FIXED_POINT ratioB = FLOAT2FIXED(sin(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)); qa->w = (MULT_FP(qa->w, ratioA) + MULT_FP(qb->w, ratioB));
res.x = (MULT_FP(qa.x, ratioA) + MULT_FP(qb.x, ratioB)); qa->x = (MULT_FP(qa->x, ratioA) + MULT_FP(qb->x, ratioB));
res.y = (MULT_FP(qa.y, ratioA) + MULT_FP(qb.y, ratioB)); qa->y = (MULT_FP(qa->y, ratioA) + MULT_FP(qb->y, ratioB));
res.z = (MULT_FP(qa.z, ratioA) + MULT_FP(qb.z, ratioB)); qa->z = (MULT_FP(qa->z, ratioA) + MULT_FP(qb->z, ratioB));
exit: exit:
interpolationStep += ACTION_STEP_FP; interpolationStep += ACTION_STEP_FP;
return res;
} }
void void
handleAngle(char input) handleAngle(uint8_t input)
{ {
// TODO // TODO
if (currentlyMoving == 0) { if (currentlyMoving == 0) {
currentlyMoving = input; currentlyMoving = input;
struct Quaternions tmp;
switch (input) { switch (input) {
case 'w': case 'w':
case 'W': case 'W':
Target = multQ(GET_ROTATE_X_Q(M_PI_2), Current); tmp = GET_ROTATE_X_Q(M_PI_2);
break; break;
case 'a': case 'a':
case 'A': case 'A':
Target = multQ(GET_ROTATE_Y_Q(-M_PI_2), Current); tmp = GET_ROTATE_Y_Q(-M_PI_2);
break; break;
case 's': case 's':
case 'S': case 'S':
Target = multQ(GET_ROTATE_X_Q(-M_PI_2), Current); tmp = GET_ROTATE_X_Q(-M_PI_2);
break; break;
case 'd': case 'd':
case 'D': case 'D':
Target = multQ(GET_ROTATE_Y_Q(M_PI_2), Current); tmp = GET_ROTATE_Y_Q(M_PI_2);
break; break;
case 'q': case 'q':
case 'Q': case 'Q':
Target = multQ(GET_ROTATE_Z_Q(-M_PI_2), Current); tmp = GET_ROTATE_Z_Q(-M_PI_2);
break; break;
case 'e': case 'e':
case 'E': case 'E':
Target = multQ(GET_ROTATE_Z_Q(M_PI_2), Current); tmp = GET_ROTATE_Z_Q(M_PI_2);
break; break;
default: default:
currentlyMoving = 0; currentlyMoving = 0;
return;
//TODO idle movement //TODO idle movement
} }
multQ(&tmp, &Target);
Target = tmp;
normalize(&Target); normalize(&Target);
} else { } else {
if (interpolationStep < (1 << fp) - ACTION_STEP_FP * 2) { if (interpolationStep < (1 << FP) ) {
Current = interpolate(Current, Target); interpolate(&Current, &Target);
normalize(&Current); normalize(&Current);
} }
else { else {
@ -477,31 +529,34 @@ handleAngle(char input)
} }
} }
char uint8_t
getInput() getInput()
{ {
// TODO // TODO
char c = 'd'; uint8_t c = 'd';
handleAngle(c); handleAngle(c);
return c; return c;
} }
int int
main() main()
{ {
REG_DISPCNT = DISPCNT_BG_MODE(3) | DISPCNT_BG2_ENABLE; REG_DISPCNT = DISPCNT_BG_MODE(4) | DISPCNT_BG2_ENABLE;
Current = GET_ROTATE_Z_Q(0); Current = GET_ROTATE_Z_Q(0);
init_colors();
while (1) { while (1) {
memset(MEM_VRAM_MODE4, 0, GBA_SCREEN_H * GBA_SCREEN_W);
memset(output, NUM_FACES, VWIDTH * VHEIGHT); memset(output, NUM_FACES, VWIDTH * VHEIGHT);
memset(maxZbufByColor, 0, 2 * sizeof(FIXED_POINT) * NUM_FACES);
memset(zBuffer, 0xffffffff, VWIDTH * VHEIGHT * sizeof(FIXED_POINT)); memset(zBuffer, 0xffffffff, VWIDTH * VHEIGHT * sizeof(FIXED_POINT));
for (FIXED_POINT cubeX = -CUBE_WIDTH_FP + STEP_FP ; for (FIXED_POINT cubeX = -CUBE_WIDTH_FP + 2 * (1 << FP);
cubeX <= CUBE_WIDTH_FP - STEP_FP; cubeX += STEP_FP) { cubeX <= CUBE_WIDTH_FP - 2 * (1 << FP); cubeX += STEP_FP - 4 * (1 << FP)) {
for (FIXED_POINT cubeY = -CUBE_WIDTH_FP + STEP_FP; for (FIXED_POINT cubeY = -CUBE_WIDTH_FP + 2 * (1 << FP);
cubeY <= CUBE_WIDTH_FP - STEP_FP; cubeY += STEP_FP) { cubeY <= CUBE_WIDTH_FP - 2 * (1 << FP); cubeY += STEP_FP - 4 * (1 << FP)) {
switch (frontFacingFace) { switch (frontFacingFace) {
case FACE_FRONT: case FACE_FRONT:
rotateCube(cubeX, cubeY, -CUBE_WIDTH_FP, FACE_FRONT); rotateCube(cubeX, cubeY, -CUBE_WIDTH_FP, FACE_FRONT);