int Matrix_Inv(GLfloat *r,GLfloat *m,int n)
{
int i,j,k,l;
GLfloat max,tmp,t;
/* identitée dans r */
for(i=0;i<n*n;i++) r[i]=int2sll(0);
for(i=0;i<n;i++) r[i*n+i]=int2sll(1);
for(j=0;j<n;j++) {
/* recherche du nombre de plus grand module sur la colonne j */
max=m[j*n+j];
k=j;
for(i=j+1;i<n;i++)
if (sllvalue(sll_abs(m[i*n+j]))>sllvalue(sll_abs(max))) {
k=i;
max=m[i*n+j];
}
/* non intersible matrix */
if (sllvalue(max)==sllvalue(int2sll(0))) return 1;
/* permutation des lignes j et k */
if (k!=j) {
for(i=0;i<n;i++) {
tmp=m[j*n+i];
m[j*n+i]=m[k*n+i];
m[k*n+i]=tmp;
tmp=r[j*n+i];
r[j*n+i]=r[k*n+i];
r[k*n+i]=tmp;
}
}
/* multiplication de la ligne j par 1/max */
max=slldiv(int2sll(1), max);
for(i=0;i<n;i++) {
tmp=m[j*n+i];
m[j*n+i]=sllmul(tmp, max);
tmp=r[j*n+i];
r[j*n+i]=sllmul(tmp, max);
}
for(l=0;l<n;l++) if (l!=j) {
t=m[l*n+j];
for(i=0;i<n;i++) {
tmp=m[l*n+i];
m[l*n+i]=sllsub(tmp, sllmul(m[j*n+i],t));
tmp=r[l*n+i];
r[l*n+i]=sllsub(tmp, sllmul(r[j*n+i],t));
}
}
}
return 0;
}
void glColor4fv(GLfloat *v)
{
GLParam p[8];
p[0].op=OP_Color;
p[1].f=v[0];
p[2].f=v[1];
p[3].f=v[2];
p[4].f=v[3];
/* direct convertion to integer to go faster if no shading */
p[5].ui = (unsigned int) (sll2int(
slladd(
sllmul(v[0], int2sll(ZB_POINT_RED_MAX - ZB_POINT_RED_MIN)),
int2sll(ZB_POINT_RED_MIN)
)
)
);
p[6].ui = (unsigned int) (sll2int(
slladd(
sllmul(v[1], int2sll(ZB_POINT_GREEN_MAX - ZB_POINT_GREEN_MIN)),
int2sll(ZB_POINT_GREEN_MIN)
)
)
);
p[7].ui = (unsigned int) (sll2int(
slladd(
sllmul(v[2], int2sll(ZB_POINT_BLUE_MAX - ZB_POINT_BLUE_MIN)),
int2sll(ZB_POINT_BLUE_MIN)
)
)
);
gl_add_op(p);
}
// http://math.stackexchange.com/questions/1098487/atan2-faster-approximation
// atan2(y,x)
// a := min (|x|, |y|) / max (|x|, |y|)
// s := a * a
// r := ((-0.0464964749 * s + 0.15931422) * s - 0.327622764) * s * a + a
// if |y| > |x| then r := 1.57079637 - r
// if x < 0 then r := 3.14159274 - r
// if y < 0 then r := -r
sll sllatan2(sll y, sll x) {
sll abs_x = sllabs(x);
sll abs_y = sllabs(y);
sll maxyx = MAX(abs_x, abs_y);
sll minyx = MIN(abs_x, abs_y);
sll a = slldiv(minyx, maxyx);
sll s = sllmul(a, a);
sll r_1 = sllmul(dbl2sll((-0.0464964749)), s);
sll r_2 = slladd(r_1, dbl2sll(0.15931422));
sll r_3 = sllsub(sllmul(r_2, s), dbl2sll(0.327622764));
sll r = slladd(sllmul(sllmul(r_3, s), a), a);
if(sllabs(y) > sllabs(x)) {
r = sllsub(CONST_PI_2, r);
}
if(x < CONST_0) {
r = sllsub(CONST_PI, r);
}
if(y < CONST_0) {
r = sllneg(r);
}
return r;
}
void glopFrustum(GLContext *c,GLParam *p)
{
GLfloat *r;
M4 m;
GLfloat left=p[1].f;
GLfloat right=p[2].f;
GLfloat bottom=p[3].f;
GLfloat top=p[4].f;
GLfloat near=p[5].f;
GLfloat farp=p[6].f;
GLfloat x,y,A,B,C,D,tmp2=int2sll(2);
x = slldiv(sllmul(tmp2,near), sllsub(right,left));
y = slldiv(sllmul(tmp2,near), sllsub(top,bottom));
A = slldiv(slladd(right,left), sllsub(right,left));
B = slldiv(slladd(top,bottom), sllsub(top,bottom));
C = slldiv(sllneg(slladd(farp,near)), sllsub(farp,near));
D = slldiv(sllneg(sllmul(sllmul(tmp2,farp),near)), sllsub(farp,near));
r=&m.m[0][0];
r[0]= x; r[1]=int2sll(0); r[2]=A; r[3]=int2sll(0);
r[4]= int2sll(0); r[5]=y; r[6]=B; r[7]=int2sll(0);
r[8]= int2sll(0); r[9]=int2sll(0); r[10]=C; r[11]=D;
r[12]=int2sll(0); r[13]=int2sll(0); r[14]=int2sll(-1); r[15]=int2sll(0);
gl_M4_MulLeft(c->matrix_stack_ptr[c->matrix_mode],&m);
gl_matrix_update(c);
}
// Calculates the distance between two GPS lat/lon coordinates
// returns units: KM
// From: http://www.movable-type.co.uk/scripts/latlong.html
sll DistanceBetweenSLL(sll lat1, sll lon1, sll lat2, sll lon2) {
sll R = int2sll(6371); // kilometres
sll lat1_rad = slldeg2rad(lat1);
sll lon1_rad = slldeg2rad(lon1);
sll lat2_rad = slldeg2rad(lat2);
sll lon2_rad = slldeg2rad(lon2);
sll dist_lat = sllsub(lat2_rad, lat1_rad);
sll dist_lon = sllsub(lon2_rad, lon1_rad);
dist_lat = slldiv2(dist_lat);
dist_lon = slldiv2(dist_lon);
dist_lat = sllsin(dist_lat);
dist_lon = sllsin(dist_lon);
sll a = sllmul(dist_lat, dist_lat);
sll b = sllmul(dist_lon, dist_lon);
sll c = sllmul(sllcos(lat1_rad), sllcos(lat2_rad));
sll d = slladd(a, sllmul(b,c));
sll d_sqrt = sllsqrt(d);
sll d_sqrt_1 = sllsqrt(sllsub(CONST_1,d));
sll result = sllmul2(sllatan2(d_sqrt, d_sqrt_1));
return sllmul(R,result);
}
void glColor4f(GLfloat r,GLfloat g,GLfloat b,GLfloat a)
{
GLParam p[8];
p[0].op=OP_Color;
p[1].f=r;
p[2].f=g;
p[3].f=b;
p[4].f=a;
/* direct convertion to integer to go faster if no shading */
p[5].ui = (unsigned int) (sll2int(
slladd(
sllmul(r,int2sll(ZB_POINT_RED_MAX - ZB_POINT_RED_MIN)),
int2sll(ZB_POINT_RED_MIN)
)
)
);
p[6].ui = (unsigned int) (sll2int(
slladd(
sllmul(g, int2sll(ZB_POINT_GREEN_MAX - ZB_POINT_GREEN_MIN)),
int2sll(ZB_POINT_GREEN_MIN)
)
)
);
p[7].ui = (unsigned int) (sll2int(
slladd(
sllmul(b, int2sll(ZB_POINT_BLUE_MAX - ZB_POINT_BLUE_MIN)),
int2sll(ZB_POINT_BLUE_MIN)
)
)
);
gl_add_op(p);
}
int gl_V3_Norm(V3 *a)
{
GLfloat n;
n=sllsqrt(slladd(slladd(sllmul(a->X,a->X), sllmul(a->Y,a->Y)), sllmul(a->Z,a->Z)));
if (sllvalue(n)==sllvalue(int2sll(0))) return 1;
a->X=slldiv(a->X, n);
a->Y=slldiv(a->Y, n);
a->Z=slldiv(a->Z, n);
return 0;
}
void gl_M4_Mul(M4 *c,M4 *a,M4 *b)
{
int i,j,k;
GLfloat s;
for(i=0;i<4;i++)
for(j=0;j<4;j++) {
s=int2sll(0);
for(k=0;k<4;k++) s=slladd(s,sllmul(a->m[i][k],b->m[k][j]));
c->m[i][j]=s;
}
}
void Firework_Update(GBitmap *bitmap) {
static int darken_count = 0;
if (darken_count++ >= 4) {
darken_count = 0;
graphics_darken(bitmap);
}
// Draw fireworks
for (int i = 0; i < FIREWORKS; i++) {
FireworkStruct* firework = &prv_fireworks[i];
GColor color;
if (firework->hasExploded) {
color = GColorFromRGB(
sll2int(sllmul(int2sll(firework->red), firework->alpha)),
sll2int(sllmul(int2sll(firework->green), firework->alpha)),
sll2int(sllmul(int2sll(firework->blue), firework->alpha)));
for (int p = 0; p < FIREWORK_PARTICLES; p++) {
int xpos = sll2int(firework->x[p]);
int ypos = sll2int(firework->y[p]);
graphics_draw_pixel_color(bitmap, (GPoint){xpos, ypos}, color);
graphics_draw_pixel_color(bitmap, (GPoint){xpos + 1, ypos}, color);
graphics_draw_pixel_color(bitmap, (GPoint){xpos + 1, ypos + 1}, color);
graphics_draw_pixel_color(bitmap, (GPoint){xpos, ypos + 1}, color);
}
Firework_Explode(&prv_fireworks[i]);
} else {
color = GColorFromRGB(255, 255, 0);
int xpos = sll2int(firework->x[0]);
int ypos = sll2int(firework->y[0]);
graphics_draw_pixel_color(bitmap, (GPoint){xpos, ypos}, color);
graphics_draw_pixel_color(bitmap, (GPoint){xpos + 1, ypos}, color);
graphics_draw_pixel_color(bitmap, (GPoint){xpos + 1, ypos + 1}, color);
graphics_draw_pixel_color(bitmap, (GPoint){xpos, ypos + 1}, color);
Firework_Move(&prv_fireworks[i]);
}
}
}
/* inversion of an orthogonal matrix of type Y=M.X+P */
void gl_M4_InvOrtho(M4 *a,M4 b)
{
int i,j;
GLfloat s;
for(i=0;i<3;i++)
for(j=0;j<3;j++) a->m[i][j]=b.m[j][i];
a->m[3][0]=int2sll(0); a->m[3][1]=int2sll(0); a->m[3][2]=int2sll(0); a->m[3][3]=int2sll(1);
for(i=0;i<3;i++) {
s=int2sll(0);
for(j=0;j<3;j++) s=sllsub(s, sllmul(b.m[j][i],b.m[j][3]));
a->m[i][3]=s;
}
}
/* c=c*a */
void gl_M4_MulLeft(M4 *c,M4 *b)
{
int i,j,k;
GLfloat s;
M4 a;
/*memcpy(&a, c, 16*sizeof(GLfloat));
*/
a=*c;
for(i=0;i<4;i++)
for(j=0;j<4;j++) {
s=int2sll(0);
for(k=0;k<4;k++) s=slladd(s,sllmul(a.m[i][k],b->m[k][j]));
c->m[i][j]=s;
}
}
void Firework_Explode(FireworkStruct* firework) {
for (int loop = 0; loop < FIREWORK_PARTICLES; loop++) {
// Dampen the horizontal speed by 1% per frame
firework->xSpeed[loop] = sllmul(firework->xSpeed[loop], Firework_xDampen);
// Move the particle
firework->x[loop] = slladd(firework->x[loop], firework->xSpeed[loop]);
firework->y[loop] = slladd(firework->y[loop], firework->ySpeed[loop]);
// Apply gravity to the particle's speed
firework->ySpeed[loop] = slladd(firework->ySpeed[loop], Firework_GRAVITY);
}
// Fade out the particles (alpha is stored per firework, not per particle)
if (firework->alpha > CONST_0) {
firework->alpha = sllsub(firework->alpha, Firework_aDampen);
if (firework->alpha < CONST_0) {
firework->alpha = CONST_0;
}
} else { // Once the alpha hits zero reset the firework
Firework_Start(firework);
}
}
sll slldeg2rad(sll deg) {
return (slldiv(sllmul(deg,CONST_PI), int2sll(180)));
}
void gl_MulM3V3(V3 *a,M4 *b,V3 *c)
{
a->X=slladd(slladd(sllmul(b->m[0][0],c->X), sllmul(b->m[0][1],c->Y)), sllmul(b->m[0][2],c->Z));
a->Y=slladd(slladd(sllmul(b->m[1][0],c->X), sllmul(b->m[1][1],c->Y)), sllmul(b->m[1][2],c->Z));
a->Z=slladd(slladd(sllmul(b->m[2][0],c->X), sllmul(b->m[2][1],c->Y)), sllmul(b->m[2][2],c->Z));
}
void glopTranslate(GLContext *c,GLParam *p)
{
GLfloat *m;
GLfloat x=p[1].f,y=p[2].f,z=p[3].f;
m=&c->matrix_stack_ptr[c->matrix_mode]->m[0][0];
m[3] = slladd(slladd(slladd(sllmul(m[0], x), sllmul(m[1], y)), sllmul(m[2], z)), m[3]);
m[7] = slladd(slladd(slladd(sllmul(m[4], x), sllmul(m[5], y)), sllmul(m[6], z)), m[7]);
m[11] = slladd(slladd(slladd(sllmul(m[8], x), sllmul(m[9], y)), sllmul(m[10], z)), m[11]);
m[15] = slladd(slladd(slladd(sllmul(m[12], x), sllmul(m[13], y)), sllmul(m[14], z)), m[15]);
gl_matrix_update(c);
}
/* inverse of a 3x3 matrix */
void gl_M3_Inv(M3 *a,M3 *m)
{
GLfloat det;
det =
sllsub(
slladd(
slladd(
sllsub(
sllsub(sllmul(sllmul(m->m[0][0], m->m[1][1]), m->m[2][2]),
sllmul(sllmul(m->m[0][0], m->m[1][2]), m->m[2][1])),
sllmul(sllmul(m->m[1][0], m->m[0][1]), m->m[2][2])),
sllmul(sllmul(m->m[1][0], m->m[0][2]), m->m[2][1])),
sllmul(sllmul(m->m[2][0], m->m[0][1]), m->m[1][2])),
sllmul(sllmul(m->m[2][0], m->m[0][2]), m->m[1][1]));
a->m[0][0] = slldiv(sllsub(sllmul(m->m[1][1],m->m[2][2]), sllmul(m->m[1][2],m->m[2][1])), det);
a->m[0][1] = sllneg(slldiv(sllsub(sllmul(m->m[0][1],m->m[2][2]), sllmul(m->m[0][2],m->m[2][1])), det));
a->m[0][2] = sllneg(slldiv(slladd(sllmul(sllneg(m->m[0][1]),m->m[1][2]), sllmul(m->m[0][2], m->m[1][1])), det));
a->m[1][0] = sllneg(slldiv(sllsub(sllmul(m->m[1][0],m->m[2][2]), sllmul(m->m[1][2],m->m[2][0])), det));
a->m[1][1] = slldiv(sllsub(sllmul(m->m[0][0],m->m[2][2]), sllmul(m->m[0][2],m->m[2][0])), det);
a->m[1][2] = sllneg(slldiv(sllsub(sllmul(m->m[0][0],m->m[1][2]), sllmul(m->m[0][2],m->m[1][0])), det));
a->m[2][0] = slldiv(sllsub(sllmul(m->m[1][0],m->m[2][1]), sllmul(m->m[1][1],m->m[2][0])), det);
a->m[2][1] = sllneg(slldiv(sllsub(sllmul(m->m[0][0],m->m[2][1]), sllmul(m->m[0][1],m->m[2][0])), det));
a->m[2][2] = slldiv(sllsub(sllmul(m->m[0][0],m->m[1][1]), sllmul(m->m[0][1],m->m[1][0])), det);
}
void glopScale(GLContext *c,GLParam *p)
{
GLfloat *m;
GLfloat x=p[1].f,y=p[2].f,z=p[3].f;
m=&c->matrix_stack_ptr[c->matrix_mode]->m[0][0];
m[0] = sllmul(m[0],x); m[1] = sllmul(m[1],y); m[2] = sllmul(m[2],z);
m[4] = sllmul(m[4],x); m[5] = sllmul(m[5],y); m[6] = sllmul(m[6],z);
m[8] = sllmul(m[8],x); m[9] = sllmul(m[9],y); m[10] = sllmul(m[10],z);
m[12] = sllmul(m[12],x); m[13] = sllmul(m[13],y); m[14] = sllmul(m[14],z);
gl_matrix_update(c);
}
void gl_M4_MulV4(V4 *a,M4 *b,V4 *c)
{
a->X=slladd(slladd(slladd(sllmul(b->m[0][0],c->X), sllmul(b->m[0][1],c->Y)), sllmul(b->m[0][2],c->Z)), sllmul(b->m[0][3],c->W));
a->Y=slladd(slladd(slladd(sllmul(b->m[1][0],c->X), sllmul(b->m[1][1],c->Y)), sllmul(b->m[1][2],c->Z)), sllmul(b->m[1][3],c->W));
a->Z=slladd(slladd(slladd(sllmul(b->m[2][0],c->X), sllmul(b->m[2][1],c->Y)), sllmul(b->m[2][2],c->Z)), sllmul(b->m[2][3],c->W));
a->W=slladd(slladd(slladd(sllmul(b->m[3][0],c->X), sllmul(b->m[3][1],c->Y)), sllmul(b->m[3][2],c->Z)), sllmul(b->m[3][3],c->W));
}
void phase2_process(void)
{
sll inc=dbl2sll(0.0022);
phase2_ship_x=128; phase2_ship_y=147;
phase2_shoot.x=0; phase2_shoot.y=0; phase2_shoot.state=100;
phase2_bad.x=0; phase2_bad.y=300; phase2_bad.state=105;
phase2_global_x=0; phase2_global_y=0;
music_play_fade(MUSIC_LEVEL);
init_frame_time();
theend=0;
while (!theend)
{
getKey();
if (hit1_pressed)
if (phase2_shoot.state>=25)
{
sll angle=sllsub(mode7_angle,sllmul(inc,int2sll((128-phase2_ship_x)<<1)));
sll multi=int2sll((256-phase2_ship_y)/24);
phase2_shoot.dx=sllmul(sllrotl(sllcos(angle),3),SLL_CONST_PI);
phase2_shoot.dy=sllmul(sllrotl(sllsin(angle),3),SLL_CONST_PI);
phase2_shoot.x=slladd(mode7_x,sllmul(phase2_shoot.dx,multi));
phase2_shoot.y=slladd(mode7_y,sllmul(phase2_shoot.dy,multi));
angle=sllsub(mode7_angle,sllmul(inc,int2sll((128-phase2_ship_x)<<2)));
phase2_shoot.dx=sllmul(sllrotl(sllcos(angle),3),SLL_CONST_PI);
phase2_shoot.dy=sllmul(sllrotl(sllsin(angle),3),SLL_CONST_PI);
phase2_shoot.alt=(phase2_ship_y-128)>>2;
phase2_shoot.state=24;
play_sound(SND_SHIP_LASER);
}
if (left_pressed)
phase2_ship_x-=8;
else if (right_pressed)
phase2_ship_x+=8;
else if (phase2_ship_x<128)
phase2_ship_x+=4;
else if (phase2_ship_x>128)
phase2_ship_x-=4;
if (phase2_ship_x>=192)
phase2_ship_x-=8;
else if (phase2_ship_x<=64)
phase2_ship_x+=8;
if (up_pressed)
{
if (mode7_z<512)
{
mode7_z += 32;
phase2_ship_y-=8;
}
}
else if (down_pressed)
{
if (mode7_z>256)
{
mode7_z -= 32;
phase2_ship_y+=8;
}
}
else
if (mode7_z>400)
{
mode7_z-=16;
phase2_ship_y+=4;
}
else
if (mode7_z<368)
{
mode7_z+=16;
phase2_ship_y-=4;
}
mode7_process(sllsub(mode7_angle,sllmul(inc,int2sll((128-phase2_ship_x)>>1))));
phase2_global_x= sll2int(mode7_x);
phase2_global_y= sll2int(mode7_y);
phase2_draw();
delay_frame_time();
}
sll rand_sll(sll max_val) {
sll max_scale = int2sll(1000);
int max_val_int = sll2int(sllmul(max_val, max_scale));
return slldiv(int2sll(rand() % (max_val_int + 1)), max_scale);
}
void glopRotate(GLContext *c,GLParam *p)
{
#define SLL_M_PI dbl2sll(M_PI)
M4 m;
GLfloat u[3];
GLfloat angle;
int dir_code;
angle = slldiv(sllmul(p[1].f, SLL_M_PI), int2sll(180));
u[0]=p[2].f;
u[1]=p[3].f;
u[2]=p[4].f;
/* simple case detection */
dir_code = ((sllvalue(u[0]) != sllvalue(int2sll(0)))<<2) | ((sllvalue(u[1]) != sllvalue(int2sll(0)))<<1) | (sllvalue(u[2]) != sllvalue(int2sll(0)));
switch(dir_code) {
case 0:
gl_M4_Id(&m);
break;
case 4:
if (sllvalue(u[0]) < sllvalue(int2sll(0))) angle=sllneg(angle);
gl_M4_Rotate(&m,angle,0);
break;
case 2:
if (sllvalue(u[1]) < sllvalue(int2sll(0))) angle=sllneg(angle);
gl_M4_Rotate(&m,angle,1);
break;
case 1:
if (sllvalue(u[2]) < sllvalue(int2sll(0))) angle=sllneg(angle);
gl_M4_Rotate(&m,angle,2);
break;
default:
{
GLfloat cost, sint;
/* normalize vector */
GLfloat len = slladd(
slladd(
sllmul(u[0],u[0]),
sllmul(u[1],u[1])
),
sllmul(u[2],u[2]));
if (sllvalue(len) == sllvalue(int2sll(0))) return;
len = slldiv(int2sll(1), sllsqrt(len));
u[0] = sllmul(u[0], len);
u[1] = sllmul(u[1], len);
u[2] = sllmul(u[2], len);
/* store cos and sin values */
cost=sllcos(angle);
sint=sllsin(angle);
/* fill in the values */
m.m[3][0]=m.m[3][1]=m.m[3][2]=
m.m[0][3]=m.m[1][3]=m.m[2][3]=int2sll(0);
m.m[3][3]=int2sll(1);
/* do the math */
m.m[0][0]=slladd(sllmul(u[0],u[0]), sllmul(cost, sllsub(int2sll(1), sllmul(u[0],u[0]))));
m.m[1][0]=sllsub(sllmul(sllmul(u[0],u[1]), sllsub(int2sll(1), cost)), sllmul(u[2],sint));
m.m[2][0]=slladd(sllmul(sllmul(u[2],u[0]), sllsub(int2sll(1), cost)), sllmul(u[1],sint));
m.m[0][1]=slladd(sllmul(sllmul(u[0],u[1]), sllsub(int2sll(1), cost)), sllmul(u[2],sint));
m.m[1][1]=slladd(sllmul(u[1],u[1]), sllmul(cost, sllsub(int2sll(1), sllmul(u[1],u[1]))));
m.m[2][1]=sllsub(sllmul(sllmul(u[1],u[2]), sllsub(int2sll(1), cost)), sllmul(u[0],sint));
m.m[0][2]=sllsub(sllmul(sllmul(u[2],u[0]), sllsub(int2sll(1), cost)), sllmul(u[1],sint));
m.m[1][2]=slladd(sllmul(sllmul(u[1],u[2]), sllsub(int2sll(1), cost)), sllmul(u[0],sint));
m.m[2][2]=slladd(sllmul(u[2],u[2]), sllmul(cost, sllsub(int2sll(1), sllmul(u[2],u[2]))));
}
}
gl_M4_MulLeft(c->matrix_stack_ptr[c->matrix_mode],&m);
gl_matrix_update(c);
}
