Tait Bryan angles, yaw pitch roll
This commit is contained in:
commit
ca9f2d4c03
1 changed files with 278 additions and 0 deletions
278
main.c
Normal file
278
main.c
Normal file
|
|
@ -0,0 +1,278 @@
|
|||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <unistd.h>
|
||||
#include <string.h>
|
||||
#include <termios.h>
|
||||
#include <signal.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#define SCREEN_WIDTH 100
|
||||
#define SCREEN_HEIGHT 50
|
||||
#define CUBE_WIDTH 10
|
||||
|
||||
#define FACE_FRONT '#'
|
||||
#define FACE_BACK '0'
|
||||
#define FACE_BOTTOM '!'
|
||||
#define FACE_LEFT '%'
|
||||
#define FACE_RIGHT '@'
|
||||
#define FACE_TOP '-'
|
||||
|
||||
#define STEP 0.2
|
||||
|
||||
#define ACTION_STEP 0.1
|
||||
|
||||
#define PITCH_STEP 0.08
|
||||
#define ROLL_STEP 0.00
|
||||
#define YAW_STEP 0.01
|
||||
|
||||
#define K1 100
|
||||
#define K2 50
|
||||
|
||||
#define _RESET_ANGLE(input) do {\
|
||||
Angle.currentTarget = 0;\
|
||||
input = 0; \
|
||||
} while (0);
|
||||
|
||||
#define RESET_A_ANGLE(input) do {\
|
||||
Angle.A = Angle.ATarget; \
|
||||
_RESET_ANGLE(input); \
|
||||
} while (0);
|
||||
#define RESET_B_ANGLE(input) do {\
|
||||
Angle.B = Angle.BTarget; \
|
||||
_RESET_ANGLE(input); \
|
||||
} while (0);
|
||||
#define RESET_C_ANGLE(input) do {\
|
||||
Angle.C = Angle.CTarget; \
|
||||
_RESET_ANGLE(input); \
|
||||
} while (0);
|
||||
|
||||
struct {
|
||||
double A, B, C;
|
||||
double ATarget, BTarget, CTarget;
|
||||
char currentTarget;
|
||||
double cosA, sinA;
|
||||
double cosB, sinB;
|
||||
double cosC, sinC;
|
||||
} Angle;
|
||||
|
||||
void initAngles() {
|
||||
Angle.A = 0, Angle.B = 0, Angle.C = 0;
|
||||
Angle.ATarget = 0, Angle.BTarget = 0, Angle.CTarget = 0;
|
||||
Angle.currentTarget = 0;
|
||||
}
|
||||
|
||||
double zBuffer[SCREEN_HEIGHT * SCREEN_WIDTH];
|
||||
char output[SCREEN_HEIGHT * SCREEN_WIDTH];
|
||||
|
||||
static volatile char shouldBreak = 1;
|
||||
static struct termios originalTerm = {};
|
||||
|
||||
double calcX(double x, double y, double z){
|
||||
return
|
||||
x * Angle.cosA * Angle.cosB +
|
||||
y * Angle.sinA * Angle.cosB
|
||||
-z * Angle.sinB;
|
||||
}
|
||||
|
||||
double calcY(double x, double y, double z){
|
||||
return
|
||||
x * (Angle.cosA * Angle.sinB * Angle.sinC - Angle.sinA * Angle.cosC) +
|
||||
y * (Angle.sinA * Angle.sinB * Angle.sinC + Angle.cosA * Angle.cosC) +
|
||||
z * Angle.cosB * Angle.sinC;
|
||||
}
|
||||
|
||||
double calcZ(double x, double y, double z){
|
||||
return
|
||||
x * (Angle.cosA * Angle.sinB * Angle.cosC + Angle.sinA * Angle.sinC) +
|
||||
y * (Angle.sinA * Angle.sinB * Angle.cosC - Angle.cosA * Angle.sinC) +
|
||||
z * Angle.cosB * Angle.cosC;
|
||||
}
|
||||
|
||||
void calcAngle(){
|
||||
Angle.cosA = cos(Angle.A);
|
||||
Angle.sinA = sin(Angle.A);
|
||||
Angle.cosB = cos(Angle.B);
|
||||
Angle.sinB = sin(Angle.B);
|
||||
Angle.cosC = cos(Angle.C);
|
||||
Angle.sinC = sin(Angle.C);
|
||||
}
|
||||
|
||||
char *chooseColor(char c){
|
||||
switch(c) {
|
||||
case '#':
|
||||
return "\033[31;1;4m#\033[0m";
|
||||
case '0':
|
||||
return "\033[32;1;4m0\033[0m";
|
||||
case '!':
|
||||
return "\033[33;1;4m!\033[0m";
|
||||
case '%':
|
||||
return "\033[34;1;4m%\033[0m";
|
||||
case '@':
|
||||
return "\033[35;1;4m@\033[0m";
|
||||
case '-':
|
||||
return "\033[36;1;4m-\033[0m";
|
||||
case ' ':
|
||||
return " ";
|
||||
default: // unused, but issues no warning
|
||||
return "";
|
||||
}
|
||||
}
|
||||
|
||||
void printAscii() {
|
||||
#ifdef __APPLE__
|
||||
fcntl(STDOUT_FILENO, F_SETFL, ~O_NONBLOCK);
|
||||
#endif
|
||||
for (int k = 0 ; k < SCREEN_WIDTH * SCREEN_HEIGHT ; ++k)
|
||||
printf("%s", k % SCREEN_WIDTH ? chooseColor(output[k]) : "\r\n");
|
||||
printf("\r\n");
|
||||
#ifdef __APPLE__
|
||||
fcntl(STDIN_FILENO, F_SETFL, O_NONBLOCK);
|
||||
#endif
|
||||
}
|
||||
|
||||
void rotateCube(double cubeX, double cubeY, double cubeZ, char ch){
|
||||
calcAngle();
|
||||
double x = calcX(cubeX, cubeY, cubeZ);
|
||||
double y = calcY(cubeX, cubeY, cubeZ);
|
||||
double invZ = 1 / (calcZ(cubeX, cubeY, cubeZ) + K2);
|
||||
|
||||
int screenX = (int)(SCREEN_WIDTH / 2) + floor((x) * K1) * invZ;
|
||||
int screenY = (int)(SCREEN_HEIGHT / 2) + floor(((y) * K1 / 2 ) * invZ);
|
||||
// TODO luminescence
|
||||
|
||||
if (screenX > SCREEN_WIDTH || screenX < 0) return;
|
||||
|
||||
int idx = screenY * SCREEN_WIDTH + screenX;
|
||||
if(idx >= 0 && idx < SCREEN_WIDTH * SCREEN_HEIGHT)
|
||||
if(zBuffer[idx] < invZ) {
|
||||
zBuffer[idx] = invZ;
|
||||
output[idx] = ch;
|
||||
}
|
||||
}
|
||||
|
||||
void intHandler(int unused) {
|
||||
shouldBreak = 0;
|
||||
tcsetattr(STDIN_FILENO, TCSANOW, &originalTerm);
|
||||
}
|
||||
|
||||
void printBanner(char keyboardInput) {
|
||||
printf("Last input character: %c", keyboardInput);
|
||||
}
|
||||
|
||||
void handleAngle(char *input) {
|
||||
if (!Angle.currentTarget) {
|
||||
Angle.currentTarget = *input;
|
||||
switch (*input) {
|
||||
case 'W':
|
||||
Angle.CTarget -= M_PI_2;
|
||||
// pitch angle +pi/2
|
||||
break;
|
||||
case 'A':
|
||||
// yaw angle -pi/2
|
||||
Angle.BTarget -= M_PI_2;
|
||||
break;
|
||||
case 'S':
|
||||
// pitch angle -pi/2
|
||||
Angle.CTarget += M_PI_2;
|
||||
break;
|
||||
case 'D':
|
||||
// yaw angle +pi/2
|
||||
Angle.BTarget += M_PI_2;
|
||||
break;
|
||||
default:
|
||||
Angle.currentTarget = 0;
|
||||
}
|
||||
|
||||
// idle animation
|
||||
//Angle.A += ROLL_STEP;
|
||||
//Angle.B += YAW_STEP;
|
||||
//Angle.C += PITCH_STEP;
|
||||
}
|
||||
|
||||
else
|
||||
switch(Angle.currentTarget) {
|
||||
case 'W':
|
||||
Angle.C -= ACTION_STEP;
|
||||
if (Angle.C <= Angle.CTarget) RESET_C_ANGLE(*input);
|
||||
break;
|
||||
case 'A':
|
||||
Angle.B -= ACTION_STEP;
|
||||
if (Angle.B <= Angle.BTarget) RESET_B_ANGLE(*input);
|
||||
break;
|
||||
case 'S':
|
||||
Angle.C += ACTION_STEP;
|
||||
if (Angle.C >= Angle.CTarget) RESET_C_ANGLE(*input);
|
||||
break;
|
||||
case 'D':
|
||||
Angle.B += ACTION_STEP;
|
||||
if (Angle.B >= Angle.BTarget) RESET_B_ANGLE(*input);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
char getInput(char *keyboardInput) {
|
||||
char c = getchar();
|
||||
if (c == '\033') { // if the first value is esc
|
||||
if ((c = getchar()) == '[')
|
||||
switch(c = getchar()) { // the real value
|
||||
case 'A':
|
||||
c = 'W';
|
||||
break;
|
||||
case 'B':
|
||||
c = 'S';
|
||||
break;
|
||||
case 'C':
|
||||
c = 'D';
|
||||
break;
|
||||
case 'D':
|
||||
c = 'A';
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (c != EOF)
|
||||
*keyboardInput = c;
|
||||
while((c = getchar()) != '\n' && c != EOF) {} // clean stdin
|
||||
|
||||
handleAngle(keyboardInput);
|
||||
return c;
|
||||
}
|
||||
|
||||
int main(){
|
||||
char keyboardInput = 0;
|
||||
signal(SIGINT, intHandler);
|
||||
tcgetattr(STDIN_FILENO, &originalTerm);
|
||||
#ifndef __APPLE__
|
||||
fcntl(STDIN_FILENO, F_SETFL, O_NONBLOCK);
|
||||
#endif
|
||||
|
||||
struct termios t = {};
|
||||
cfmakeraw(&t);
|
||||
t.c_lflag |= ISIG;
|
||||
t.c_cc[VINTR] = 3;
|
||||
tcsetattr(STDIN_FILENO, TCSANOW, &t);
|
||||
|
||||
initAngles();
|
||||
|
||||
while(shouldBreak) {
|
||||
memset(output, ' ', SCREEN_WIDTH * SCREEN_HEIGHT);
|
||||
memset(zBuffer, 0, SCREEN_WIDTH * SCREEN_HEIGHT * sizeof(double));
|
||||
|
||||
for(double cubeX = -CUBE_WIDTH ; cubeX < CUBE_WIDTH ; cubeX += STEP) {
|
||||
for(double cubeY = -CUBE_WIDTH ; cubeY < CUBE_WIDTH ; cubeY += STEP) {
|
||||
rotateCube(cubeX, cubeY, -CUBE_WIDTH, FACE_FRONT);//front
|
||||
rotateCube(cubeX, cubeY, CUBE_WIDTH, FACE_BACK);//back
|
||||
rotateCube(CUBE_WIDTH, -cubeX, cubeY, FACE_RIGHT);//right
|
||||
rotateCube(-CUBE_WIDTH, cubeX, cubeY, FACE_LEFT);//left
|
||||
rotateCube(cubeX, CUBE_WIDTH, cubeY, FACE_BOTTOM);//bottom
|
||||
rotateCube(cubeX, -CUBE_WIDTH, cubeY, FACE_TOP);//top
|
||||
}
|
||||
}
|
||||
|
||||
printf("\x1b[2J");
|
||||
printBanner(keyboardInput);
|
||||
printAscii();
|
||||
getInput(&keyboardInput);
|
||||
usleep(100000);
|
||||
}
|
||||
}
|
||||
Loading…
Add table
Reference in a new issue