fix: [WIP] IT "WORKS"
This commit is contained in:
parent
28295eb8d6
commit
f2bb638c0b
1 changed files with 298 additions and 316 deletions
614
source/main.c
614
source/main.c
|
|
@ -10,20 +10,20 @@
|
|||
|
||||
#define REG_DISPCNT *((volatile uint16_t *)0x04000000)
|
||||
|
||||
#define REG_KEYINPUT *((volatile uint16_t *)0x04000130)
|
||||
#define DEFAULT_REG_KEYINPUT *((volatile uint16_t *)0x04000130)
|
||||
#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 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
|
||||
|
|
@ -52,7 +52,7 @@ static uint8_t *buffer = (uint8_t *)FRONT_BUFFER;
|
|||
static inline uint16_t
|
||||
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
|
||||
|
|
@ -68,17 +68,12 @@ flipBuffers()
|
|||
}
|
||||
|
||||
void
|
||||
putPx(uint8_t x, uint8_t y, uint16_t c)
|
||||
putPx(uint16_t x, uint16_t y, uint8_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;
|
||||
}
|
||||
uint16_t pos = (GBA_SCREEN_W * y + x);
|
||||
buffer[pos] = c;
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////////
|
||||
|
||||
#include <math.h>
|
||||
|
|
@ -93,13 +88,13 @@ putPx(uint8_t x, uint8_t y, uint16_t c)
|
|||
#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,
|
||||
FACE_FRONT = 0,
|
||||
FACE_LEFT,
|
||||
FACE_RIGHT,
|
||||
FACE_BOTTOM,
|
||||
FACE_TOP,
|
||||
FACE_BACK,
|
||||
NUM_FACES,
|
||||
};
|
||||
|
||||
#define STEP 20
|
||||
|
|
@ -108,7 +103,7 @@ enum faces {
|
|||
#define ACTION_STEP 0.1
|
||||
#define ACTION_STEP_FP ((1 << FP) * ACTION_STEP)
|
||||
|
||||
#define SCALE 10 // how much is our initial render scaled
|
||||
#define SCALE 3 // how much is our initial render scaled
|
||||
|
||||
volatile FIXED_POINT K1 = 20;
|
||||
volatile FIXED_POINT K2 = (2 * CUBE_WIDTH) + 10;
|
||||
|
|
@ -118,22 +113,20 @@ volatile FIXED_POINT K2 = (2 * CUBE_WIDTH) + 10;
|
|||
#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 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) ; \
|
||||
struct Quaternions q = {}; q.w = FLOAT2FIXED(cos(_a * .5)); \
|
||||
q.x = FLOAT2FIXED(sin(_a * .5)); q; })
|
||||
struct Quaternions q = {}; q.w = FLOAT2FIXED(cos(_a * .5)); \
|
||||
q.x = FLOAT2FIXED(sin(_a * .5)); q; })
|
||||
#define GET_ROTATE_Y_Q(a) ({ float _a = (a) ; \
|
||||
struct Quaternions q = {}; q.w = FLOAT2FIXED(cos(_a * .5)); \
|
||||
q.y = FLOAT2FIXED(sin(_a * .5)); q; })
|
||||
struct Quaternions q = {}; q.w = FLOAT2FIXED(cos(_a * .5)); \
|
||||
q.y = FLOAT2FIXED(sin(_a * .5)); q; })
|
||||
#define GET_ROTATE_Z_Q(a) ({ float _a = (a) ; \
|
||||
struct Quaternions q = {}; q.w = FLOAT2FIXED(cos(_a * .5)); \
|
||||
q.z = FLOAT2FIXED(sin(_a * .5)); q; })
|
||||
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)
|
||||
|
|
@ -152,29 +145,27 @@ init_colors()
|
|||
RGB15(31, 0, 31),
|
||||
};
|
||||
|
||||
for ( ; lastPaletteIndex <= NUM_FACES ; ++lastPaletteIndex) {
|
||||
for ( ; lastPaletteIndex <= NUM_FACES ; ++lastPaletteIndex)
|
||||
PALETTE[lastPaletteIndex] = color_order[lastPaletteIndex];
|
||||
}
|
||||
}
|
||||
|
||||
struct {
|
||||
uint8_t x;
|
||||
uint8_t y;
|
||||
uint8_t z;
|
||||
uint8_t x;
|
||||
uint8_t y;
|
||||
uint8_t z;
|
||||
} Idle;
|
||||
|
||||
|
||||
struct Quaternions {
|
||||
FIXED_POINT w;
|
||||
FIXED_POINT x;
|
||||
FIXED_POINT y;
|
||||
FIXED_POINT z;
|
||||
FIXED_POINT w;
|
||||
FIXED_POINT x;
|
||||
FIXED_POINT y;
|
||||
FIXED_POINT z;
|
||||
} Target, Current;
|
||||
|
||||
static FIXED_POINT interpolationStep = 0;
|
||||
static FIXED_POINT zBuffer[VHEIGHT * VWIDTH];
|
||||
static FIXED_POINT maxZbufByColor[NUM_FACES * 2];
|
||||
static uint8_t output[VHEIGHT * VWIDTH];
|
||||
static FIXED_POINT interpolationStep = 0;
|
||||
static volatile uint16_t vertices[NUM_FACES * 2 * 4];
|
||||
static volatile FIXED_POINT zBuffer[NUM_FACES * 2];
|
||||
|
||||
static volatile uint8_t shouldBreak = 1;
|
||||
static volatile uint8_t currentlyMoving = 0;
|
||||
|
|
@ -184,54 +175,54 @@ static volatile uint8_t 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 || n == 1)
|
||||
return;
|
||||
float n = sqrt(FIXED2FLOAT(SQ_FP(q->w) + SQ_FP(q->x) +
|
||||
SQ_FP(q->y) + SQ_FP(q->z)));
|
||||
if (n == 0 || n == 1)
|
||||
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);
|
||||
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;
|
||||
//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.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.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)));
|
||||
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;
|
||||
return res;
|
||||
}
|
||||
|
||||
// res in quat p
|
||||
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->z <= SHIFT_THRESHOLD_FP && p->z >= -SHIFT_THRESHOLD_FP) {
|
||||
p = q;
|
||||
return;
|
||||
}
|
||||
|
||||
if (q->x <= SHIFT_THRESHOLD_FP && q->x >= -SHIFT_THRESHOLD_FP
|
||||
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;
|
||||
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);
|
||||
|
|
@ -251,8 +242,8 @@ multQ(struct Quaternions *p, struct Quaternions *q)
|
|||
uint16_t
|
||||
chooseColor(uint8_t c)
|
||||
{
|
||||
if (c >= 1 && c <= NUM_FACES)
|
||||
return c;
|
||||
if (c >= 0 && c < NUM_FACES)
|
||||
return c + 1; // palette 0 is bg
|
||||
else
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -260,106 +251,108 @@ chooseColor(uint8_t c)
|
|||
uint8_t
|
||||
chooseMainFace()
|
||||
{
|
||||
int total = 0;
|
||||
int faces[NUM_FACES] = {0};
|
||||
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;
|
||||
}
|
||||
for (int k = 0; k < VWIDTH * VHEIGHT; ++k)
|
||||
if (vertices[k] >= 0 && vertices[k] < NUM_FACES) {
|
||||
faces[vertices[k]]++;
|
||||
++total;
|
||||
}
|
||||
|
||||
int max = 0, idx = 0;
|
||||
for (int k = 0; k < NUM_FACES; ++k)
|
||||
if (faces[k] > max) {
|
||||
max = faces[k];
|
||||
idx = k;
|
||||
}
|
||||
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;
|
||||
frontFacingFace = max > total * 0.9 ? idx : -1;
|
||||
return frontFacingFace;
|
||||
}
|
||||
|
||||
// ISSUE the 4 vertices rendered do not make a convex quad
|
||||
// we have to switch 2 vertices for that
|
||||
#pragma GCC
|
||||
uint8_t
|
||||
isInQuad(uint8_t curr[2], uint8_t points[8])
|
||||
isInQuad(const uint16_t x, const uint16_t y, uint8_t current_face)
|
||||
{
|
||||
uint8_t pos = 0, neg = 0;
|
||||
uint8_t x = curr[0];
|
||||
uint8_t y = curr[1];;
|
||||
int d;
|
||||
uint16_t *points = (uint16_t *)&vertices[current_face * 8];
|
||||
// little hack
|
||||
|
||||
for (uint8_t i = 0; i < 4; ++i) {
|
||||
if (points[2 * i] == curr[0] && points[2 * i + 1] == curr[1])
|
||||
return 1;
|
||||
uint16_t tmpX = points[6], tmpY = points[7];
|
||||
points[6] = points[4];
|
||||
points[7] = points[5];
|
||||
points[4] = tmpX;
|
||||
points[5] = tmpY;
|
||||
|
||||
//Form a segment between the i'th point
|
||||
uint8_t x1 = points[2 * i];
|
||||
uint8_t y1 = points[2 * i + 1];
|
||||
uint8_t pos = 0, neg = 0;
|
||||
int32_t d;
|
||||
|
||||
//And the i+1'th, or if i is the last, with the first point
|
||||
uint8_t i2 = (i + 1) % 4;
|
||||
for (uint8_t i = 0; i < 4; ++i) {
|
||||
if (points[2 * i] == x && points[2 * i + 1] == y)
|
||||
return 1;
|
||||
|
||||
uint8_t x2 = points[2 * i2];
|
||||
uint8_t y2 = points[2 * i2 + 1];
|
||||
//Form a segment between the i'th point
|
||||
int32_t x1 = points[2 * i];
|
||||
int32_t y1 = points[2 * i + 1];
|
||||
|
||||
//And the i+1'th, or if i is the last, with the first point
|
||||
uint8_t i2 = (i + 1) % 4;
|
||||
|
||||
int32_t x2 = points[2 * i2];
|
||||
int32_t y2 = points[2 * i2 + 1];
|
||||
|
||||
|
||||
//Compute the cross product
|
||||
d = (x - x1) * (y2 - y1) - (y - y1) * (x2 - x1);
|
||||
//Compute the cross product
|
||||
d = (x - x1) * (y2 - y1) - (y - y1) * (x2 - x1);
|
||||
|
||||
if (d > 0) ++pos;
|
||||
if (d < 0) ++neg;
|
||||
if (d > 0) ++pos;
|
||||
if (d < 0) ++neg;
|
||||
|
||||
//If the sign changes, then point is outside
|
||||
if (pos > 0 && neg > 0)
|
||||
return 0;
|
||||
}
|
||||
//If the sign changes, then point is outside
|
||||
if (pos > 0 && neg > 0)
|
||||
return 0;
|
||||
}
|
||||
|
||||
return 1;
|
||||
return 1;
|
||||
}
|
||||
|
||||
void
|
||||
fill_quads(uint8_t *points, uint8_t current_face)
|
||||
fillQuads(uint8_t current_face)
|
||||
{
|
||||
uint8_t top = 0, bot = 0, left = 0, right = 0;
|
||||
for (uint8_t x = 0 ; x < 8 ; ++x) {
|
||||
if (x % 2 == 0) {
|
||||
if (points[x] > right)
|
||||
right = points[x];
|
||||
if (points[x] < left)
|
||||
left = points[x];
|
||||
uint16_t top = UINT16_MAX, bot = 0, left = UINT16_MAX, right = 0;
|
||||
for (uint8_t k = 0 ; k < 8 ; ++k) {
|
||||
const uint16_t item = vertices[current_face * 8 + k];
|
||||
|
||||
if (k & 1) {
|
||||
if (item > bot)
|
||||
bot = item;
|
||||
if (item < top)
|
||||
top = item;
|
||||
} else {
|
||||
if (points[x] > bot)
|
||||
bot = points[x];
|
||||
if (points[x] < top)
|
||||
top = points[x];
|
||||
if (item > right)
|
||||
right = item;
|
||||
if (item < left)
|
||||
left = item;
|
||||
}
|
||||
}
|
||||
|
||||
for (uint16_t y = top ; y <= bot ; ++y) {
|
||||
for (uint16_t x = left ; x <= right ; ++x) {
|
||||
if (isInQuad(x, y, current_face))
|
||||
PLOT_COORD(x, y, current_face);
|
||||
}
|
||||
}
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t
|
||||
detect(uint8_t *points, uint8_t current_face) {
|
||||
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)
|
||||
detect(uint8_t current_face)
|
||||
{
|
||||
for (uint8_t k = current_face * 8; k < current_face * 8 + 8; ++k)
|
||||
if (vertices[k] == UINT16_MAX)
|
||||
return 0;
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
|
@ -372,222 +365,211 @@ comp(const void *p1, const void *p2) {
|
|||
}
|
||||
|
||||
void
|
||||
detect_and_fill_quads()
|
||||
detectAndFillQuads()
|
||||
{
|
||||
qsort(maxZbufByColor, NUM_FACES, 2 * sizeof(FIXED_POINT), comp);
|
||||
for (uint8_t idx = 0 ; idx < NUM_FACES ; ++idx) {
|
||||
uint8_t current_face = maxZbufByColor[2 * idx + 1];
|
||||
uint8_t points[8] = { 0 };
|
||||
if (detect(points, current_face))
|
||||
fill_quads(points, current_face);
|
||||
}
|
||||
qsort((FIXED_POINT *)zBuffer, NUM_FACES, 2 * sizeof(FIXED_POINT), comp);
|
||||
for (uint8_t idx = 0 ; idx < NUM_FACES ; ++idx) {
|
||||
char ch = zBuffer[2 * idx + 1];
|
||||
if (detect(ch))
|
||||
fillQuads(ch);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
printAscii()
|
||||
{
|
||||
detect_and_fill_quads();
|
||||
detectAndFillQuads();
|
||||
flipBuffers();
|
||||
|
||||
// DISPLAY POINTS
|
||||
//for (int i = 0; i < VHEIGHT; ++i) {
|
||||
// for (int j = 0; j < VWIDTH; ++j) {
|
||||
// uint8_t prevc = 0;
|
||||
// uint8_t c = output[i * VWIDTH + j];
|
||||
// MEM_VRAM_MODE4[i * GBA_SCREEN_W + j] = chooseColor(c);
|
||||
// }
|
||||
//}
|
||||
}
|
||||
|
||||
void
|
||||
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 y = q.y >> FP;
|
||||
uint32_t x = q.x >> FP;
|
||||
uint32_t y = q.y >> FP;
|
||||
|
||||
// not fixed point yet!!
|
||||
float invZ = (1 << FP) / (float)(q.z + K2 * (1 << 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
|
||||
int32_t screenX = CUBE_WIDTH * 2 + (int32_t)((x) * K1) * invZ;
|
||||
int32_t screenY = CUBE_WIDTH * 2 + (int32_t)((y) * K1) * invZ;
|
||||
//TODO luminescence
|
||||
|
||||
if (screenX > VWIDTH || screenX < 0) return;
|
||||
if (screenX > GBA_SCREEN_W || screenX < 0
|
||||
|| screenY > GBA_SCREEN_H || screenY < 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;
|
||||
if (invZ > maxZbufByColor[ch]) {
|
||||
maxZbufByColor[2 * ch] = invZ;
|
||||
maxZbufByColor[2 * ch + 1] = ch; //palette[0] is bg
|
||||
}
|
||||
}
|
||||
}
|
||||
FIXED_POINT invZFixed = FLOAT2FIXED(invZ);
|
||||
uint8_t firstEmptyVertex = ch * 8;
|
||||
for ( ; vertices[firstEmptyVertex] != UINT16_MAX
|
||||
&& firstEmptyVertex - ch * 8 < 8 ; firstEmptyVertex+=2);
|
||||
|
||||
vertices[firstEmptyVertex] = screenX;
|
||||
vertices[firstEmptyVertex + 1] = screenY;
|
||||
|
||||
if (zBuffer[2 * ch] < invZFixed) {
|
||||
zBuffer[2 * ch] = invZFixed;
|
||||
zBuffer[2 * ch + 1] = ch;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
interpolate(struct Quaternions *qa, struct Quaternions *qb)
|
||||
{
|
||||
frontFacingFace = -1;
|
||||
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) {
|
||||
goto exit;
|
||||
}
|
||||
if (cosHalfTheta < 0) {
|
||||
qb->w = -qb->w;
|
||||
qb->x = -qb->x;
|
||||
qb->y = -qb->y;
|
||||
qb->z = qb->z;
|
||||
cosHalfTheta = -cosHalfTheta;
|
||||
}
|
||||
frontFacingFace = -1;
|
||||
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) {
|
||||
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.01 && sinHalfTheta > -0.01) {
|
||||
qa->w = ((qa->w >> 1) + (qb->w >> 1));
|
||||
qa->x = ((qa->x >> 1) + (qb->x >> 1));
|
||||
qa->y = ((qa->y >> 1) + (qb->y >> 1));
|
||||
qa->z = ((qa->z >> 1) + (qb->z >> 1));
|
||||
goto exit;
|
||||
}
|
||||
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.01 && sinHalfTheta > -0.01) {
|
||||
qa->w = ((qa->w >> 1) + (qb->w >> 1));
|
||||
qa->x = ((qa->x >> 1) + (qb->x >> 1));
|
||||
qa->y = ((qa->y >> 1) + (qb->y >> 1));
|
||||
qa->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);
|
||||
FIXED_POINT ratioA = FLOAT2FIXED(sin((1 - FIXED2FLOAT(interpolationStep)) * halfTheta) / sinHalfTheta);
|
||||
FIXED_POINT ratioB = FLOAT2FIXED(sin(FIXED2FLOAT(interpolationStep) * halfTheta) / sinHalfTheta);
|
||||
|
||||
qa->w = (MULT_FP(qa->w, ratioA) + MULT_FP(qb->w, ratioB));
|
||||
qa->x = (MULT_FP(qa->x, ratioA) + MULT_FP(qb->x, ratioB));
|
||||
qa->y = (MULT_FP(qa->y, ratioA) + MULT_FP(qb->y, ratioB));
|
||||
qa->z = (MULT_FP(qa->z, ratioA) + MULT_FP(qb->z, ratioB));
|
||||
qa->w = (MULT_FP(qa->w, ratioA) + MULT_FP(qb->w, ratioB));
|
||||
qa->x = (MULT_FP(qa->x, ratioA) + MULT_FP(qb->x, ratioB));
|
||||
qa->y = (MULT_FP(qa->y, ratioA) + MULT_FP(qb->y, ratioB));
|
||||
qa->z = (MULT_FP(qa->z, ratioA) + MULT_FP(qb->z, ratioB));
|
||||
|
||||
exit:
|
||||
interpolationStep += ACTION_STEP_FP;
|
||||
interpolationStep += ACTION_STEP_FP;
|
||||
}
|
||||
|
||||
void
|
||||
handleAngle(uint8_t input)
|
||||
{
|
||||
// TODO
|
||||
if (currentlyMoving == 0) {
|
||||
currentlyMoving = input;
|
||||
// TODO
|
||||
if (currentlyMoving == 0) {
|
||||
currentlyMoving = input;
|
||||
struct Quaternions tmp;
|
||||
switch (input) {
|
||||
case 'w':
|
||||
case 'W':
|
||||
switch (input) {
|
||||
case 'w':
|
||||
case 'W':
|
||||
tmp = GET_ROTATE_X_Q(M_PI_2);
|
||||
break;
|
||||
case 'a':
|
||||
case 'A':
|
||||
tmp = GET_ROTATE_Y_Q(-M_PI_2);
|
||||
break;
|
||||
case 's':
|
||||
case 'S':
|
||||
tmp = GET_ROTATE_X_Q(-M_PI_2);
|
||||
break;
|
||||
case 'd':
|
||||
case 'D':
|
||||
tmp = GET_ROTATE_Y_Q(M_PI_2);
|
||||
break;
|
||||
case 'q':
|
||||
case 'Q':
|
||||
tmp = GET_ROTATE_Z_Q(-M_PI_2);
|
||||
break;
|
||||
case 'e':
|
||||
case 'E':
|
||||
tmp = GET_ROTATE_Z_Q(M_PI_2);
|
||||
break;
|
||||
default:
|
||||
currentlyMoving = 0;
|
||||
break;
|
||||
case 'a':
|
||||
case 'A':
|
||||
tmp = GET_ROTATE_Y_Q(-M_PI_2);
|
||||
break;
|
||||
case 's':
|
||||
case 'S':
|
||||
tmp = GET_ROTATE_X_Q(-M_PI_2);
|
||||
break;
|
||||
case 'd':
|
||||
case 'D':
|
||||
tmp = GET_ROTATE_Y_Q(M_PI_2);
|
||||
break;
|
||||
case 'q':
|
||||
case 'Q':
|
||||
tmp = GET_ROTATE_Z_Q(-M_PI_2);
|
||||
break;
|
||||
case 'e':
|
||||
case 'E':
|
||||
tmp = GET_ROTATE_Z_Q(M_PI_2);
|
||||
break;
|
||||
default:
|
||||
currentlyMoving = 0;
|
||||
return;
|
||||
//TODO idle movement
|
||||
}
|
||||
//TODO idle movement
|
||||
}
|
||||
multQ(&tmp, &Target);
|
||||
Target = tmp;
|
||||
normalize(&Target);
|
||||
} else {
|
||||
if (interpolationStep < (1 << FP) ) {
|
||||
interpolate(&Current, &Target);
|
||||
normalize(&Current);
|
||||
}
|
||||
else {
|
||||
Current = Target;
|
||||
interpolationStep = 0;
|
||||
currentlyMoving = 0;
|
||||
}
|
||||
}
|
||||
normalize(&Target);
|
||||
} else {
|
||||
if (interpolationStep < (1 << FP) ) {
|
||||
interpolate(&Current, &Target);
|
||||
normalize(&Current);
|
||||
}
|
||||
else {
|
||||
Current = Target;
|
||||
interpolationStep = 0;
|
||||
currentlyMoving = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t
|
||||
getInput()
|
||||
{
|
||||
// TODO
|
||||
uint8_t c = 'd';
|
||||
handleAngle(c);
|
||||
return c;
|
||||
// TODO
|
||||
uint8_t c = 'd';
|
||||
handleAngle(c);
|
||||
return c;
|
||||
}
|
||||
|
||||
int
|
||||
main()
|
||||
{
|
||||
REG_DISPCNT = DISPCNT_BG_MODE(4) | DISPCNT_BG2_ENABLE;
|
||||
|
||||
Current = GET_ROTATE_Z_Q(0);
|
||||
|
||||
REG_DISPCNT = DISPCNT_BG_MODE(4) | DISPCNT_BG2_ENABLE;
|
||||
Current = GET_ROTATE_Z_Q(0);
|
||||
init_colors();
|
||||
|
||||
while (1) {
|
||||
memset(MEM_VRAM_MODE4, 0, GBA_SCREEN_H * GBA_SCREEN_W);
|
||||
memset(output, NUM_FACES, VWIDTH * VHEIGHT);
|
||||
memset(maxZbufByColor, 0, 2 * sizeof(FIXED_POINT) * NUM_FACES);
|
||||
memset(zBuffer, 0xffffffff, VWIDTH * VHEIGHT * sizeof(FIXED_POINT));
|
||||
while (1) {
|
||||
memset(MEM_VRAM_MODE4, 0, GBA_SCREEN_H * GBA_SCREEN_W);
|
||||
memset((uint16_t *)vertices, UINT16_MAX, sizeof(uint16_t) * NUM_FACES * 4 * 2);
|
||||
//zBuff is not correct data struct, need a map or smth
|
||||
memset((uint16_t *)zBuffer, 0, NUM_FACES * 2 * sizeof(FIXED_POINT));
|
||||
|
||||
for (FIXED_POINT cubeX = -CUBE_WIDTH_FP + 2 * (1 << FP);
|
||||
cubeX <= CUBE_WIDTH_FP - 2 * (1 << FP); cubeX += STEP_FP - 4 * (1 << FP)) {
|
||||
for (FIXED_POINT cubeY = -CUBE_WIDTH_FP + 2 * (1 << FP);
|
||||
cubeY <= CUBE_WIDTH_FP - 2 * (1 << FP); cubeY += STEP_FP - 4 * (1 << 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
for (FIXED_POINT cubeX = -CUBE_WIDTH_FP + 1 * (1 << FP);
|
||||
cubeX <= CUBE_WIDTH_FP - 1 * (1 << FP); cubeX += STEP_FP - 2 * (1 << FP)) {
|
||||
for (FIXED_POINT cubeY = -CUBE_WIDTH_FP + 1 * (1 << FP);
|
||||
cubeY <= CUBE_WIDTH_FP - 1 * (1 << FP); cubeY += STEP_FP - 2 * (1 << 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();
|
||||
}
|
||||
printAscii();
|
||||
getInput();
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue