idk wip

cube is turning, but has some scalling issue vertically while turning allong z
also need to implement quad detection along Y axis

source/main.c

@@ -0,0 +1,543 @@
+// 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) (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();
+	}
+}