vect3D Quaternion_toEuler(Quaternion q) {
//http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm
vect3D v;
int32 test = mulf32(q.x, q.y) + mulf32(q.z, q.w);
if(test > floatToFixed(0.499f, 12)) { // singularity at north pole
v.x = 2 * atan2Lerp(q.x, q.w);
v.y = floatToFixed(M_PI / 2.0f, 12);
v.z = 0;
return v;
}
if(test < floatToFixed(-0.499f, 12)) { // singularity at south pole
v.x = -2 * atan2Lerp(q.x, q.w);
v.y = floatToFixed(-M_PI / 2.0f, 12);
v.z = 0;
return v;
}
int32 sqx = mulf32(q.x, q.x);
int32 sqy = mulf32(q.y, q.y);
int32 sqz = mulf32(q.z, q.z);
v.x = atan2Lerp(2 * mulf32(q.y, q.w) - 2 * mulf32(q.x, q.z), (1 << 12) - 2 * sqy - 2 * sqz);
v.y = asinLerp(2 * test);
v.z = atan2Lerp(2 * mulf32(q.x, q.w) - 2 * mulf32(q.y, q.z), (1 << 12) - 2 * sqx - 2 * sqz);
return v;
}
void Quaternion_normalize(Quaternion *quaternion) {
int32 magnitude;
magnitude = sqrtf32(mulf32(quaternion->x, quaternion->x) +
mulf32(quaternion->y, quaternion->y) +
mulf32(quaternion->z, quaternion->z) +
mulf32(quaternion->w, quaternion->w));
quaternion->x = divf32(quaternion->x, magnitude);
quaternion->y = divf32(quaternion->y, magnitude);
quaternion->z = divf32(quaternion->z, magnitude);
quaternion->w = divf32(quaternion->w, magnitude);
}
void initPlane(plane_struct* pl, int32 A, int32 B, int32 C, int32 D)
{
if(!pl)return;
pl->A=A;pl->B=B;pl->C=C;pl->D=D;
int32 r=magnitude(vect(A,B,C));
pl->A=divv16(pl->A,r);
pl->B=divv16(pl->B,r);
pl->C=divv16(pl->C,r);
pl->D=divv16(pl->D,r);
pl->point.x=-mulf32(pl->D,pl->A);
pl->point.y=-mulf32(pl->D,pl->B);
pl->point.z=-mulf32(pl->D,pl->C);
}
Quaternion Quaternion_fromAxisAngle(vect3D axis, int angle) {
Quaternion quaternion;
int32 sinAngle;
angle /= 2;
axis = normalize(axis);
sinAngle = sinLerp(angle);
quaternion.x = mulf32(axis.x, sinAngle);
quaternion.y = mulf32(axis.y, sinAngle);
quaternion.z = mulf32(axis.z, sinAngle);
quaternion.w = cosLerp(angle);
return quaternion;
}
vect3D getClosestPointRectangle(vect3D p, vect3D s, vect3D o)
{
vect3D u1, u2;
int32 x,y,sx,sy;
// NOGBA("p: %d %d %d",p.x,p.y,p.z);
// NOGBA("o: %d %d %d",o.x,o.y,o.z);
if(s.x){sx=abs(s.x);u1=vect((s.x>0)?inttof32(1):(-inttof32(1)),0,0);}
else{sx=abs(s.y);u1=vect(0,(s.y>0)?inttof32(1):(-inttof32(1)),0);}
if(s.z){sy=abs(s.z);u2=vect(0,0,(s.z>0)?inttof32(1):(-inttof32(1)));}
else{sy=abs(s.y);u2=vect(0,(s.y>0)?inttof32(1):(-inttof32(1)),0);}
o=vectDifference(o,p);
x=dotProduct(o,u1);y=dotProduct(o,u2);
// NOGBA("x, y: %d %d",x,y);
// NOGBA("sx, sy: %d %d",sx,sy);
bool r=true;
r=r&&x<sx&&x>=0;
r=r&&y<sy&&y>=0;
if(r)return addVect(p,vect(mulf32(x,u1.x)+mulf32(y,u2.x), mulf32(x,u1.y)+mulf32(y,u2.y), mulf32(x,u1.z)+mulf32(y,u2.z)));
if(x<0)
{
x=0;
if(y<0)y=0;
else if(y>sy)y=sy;
}else if(x>sx)
{
x=sx;
if(y<0)y=0;
else if(y>sy)y=sy;
}else if(y<0)
{
y=0;
if(x<0)x=0;
else if(x>sx)y=sx;
}else if(y>sy)
{
y=sy;
if(x<0)x=0;
else if(x>sx)x=sx;
}
return addVect(p,vect(mulf32(x,u1.x)+mulf32(y,u2.x), mulf32(x,u1.y)+mulf32(y,u2.y), mulf32(x,u1.z)+mulf32(y,u2.z)));
}
//---------------------------------------------------------------------------------
// Draws a 3x3 side of the rubiks cube. as with DrawTile, assumes that all rotation
// has already been performed.
//---------------------------------------------------------------------------------
void RubiksCube::DrawLine(RubikLine line, int32 x, int32 y, int32 z, int32 size) {
//---------------------------------------------------------------------------------
int color; // generate an int to signify tile colour (see enum RC_Color)
glBegin(GL_QUADS); //start drawin squares
for(int i=0; i<3; i++)
{
color = line.tile[i].color; //get this tile's colour
DrawTile(color, x, y + mulf32(inttof32(i),size), z, size, false); // draw the tile at (i,j)
}
glEnd(); // stop drawin squares
}
//---------------------------------------------------------------------------------
// Draws a 3x3 side of the rubiks cube. as with DrawTile, assumes that all rotation
// has already been performed.
//---------------------------------------------------------------------------------
void RubiksCube::DrawSide(RubikSide side, int sideNum, int32 x, int32 y, int32 z, int32 size, bool picking) {
//---------------------------------------------------------------------------------
int color; // generate an int to signify tile colour (see enum RC_Color)
glBegin(GL_QUADS); //start drawin squares
for(int i=0; i<3; i++)
{
for(int j=0; j<3; j++)
{
color = side.tile[i][j].color; //get this tile's colour
if(picking) startCheck(); // if picking...
DrawTile( color,
x + mulf32(inttof32(i),divf32(size,inttof32(3))),
y + mulf32(inttof32(j),divf32(size,inttof32(3))),
z,
divf32(size,inttof32(3)),
picking); // draw the tile at (i,j)
if(picking) endCheck(sideNum, i, j); // if picking...
}
}
glEnd(); // stop drawin squares
}
bool pointInTurretSight(turret_struct* t, vect3D p, int32* angle, int32* d2)
{
if(!t)return false;
const int32* m=t->OBB->transformationMatrix;
const vect3D u=vectDifference(p,vectDivInt(t->OBB->position,4));
const int32 d=magnitude(u);
const int32 v=dotProduct(u,vect(m[2],m[5],m[8]));
if(angle)*angle=dotProduct(u,vect(m[0],m[3],m[6]));
if(d2)*d2=d;
return v>mulf32(cosLerp(TURRET_SIGHTANGLE),d);
}
vect3D projectPoint(camera_struct* c, vect3D p)
{
if(!c)c=&playerCamera;
vect3D v;
int32* m=c->viewMatrix;
v.x=mulf32(p.x,m[0])+mulf32(p.y,m[4])+mulf32(p.z,m[8])+m[12];
v.y=mulf32(p.x,m[1])+mulf32(p.y,m[5])+mulf32(p.z,m[9])+m[13];
// v.z=mulf32(p.x,m[2])+mulf32(p.y,m[6])+mulf32(p.z,m[10])+m[14];
int32 w=mulf32(p.x,m[3])+mulf32(p.y,m[7])+mulf32(p.z,m[11])+m[15];
// GBATEK
// screen_x = (xx+ww)*viewport_width / (2*ww) + viewport_x1
// screen_y = (yy+ww)*viewport_height / (2*ww) + viewport_y1
v.x=divf32(v.x+(w),(w)*2)*128/2048;
v.y=192-divf32(v.y+(w),(w)*2)*96/2048;
v.z=0;
// v.z=divf32(v.z+(w),(w)*2);
return v;
}
void Quaternion_multiply(Quaternion * quaternion1, Quaternion quaternion2) {
vect3D vector1, vector2, cross;
int32 angle;
vector1 = Quaternion_toVector(*quaternion1);
vector2 = Quaternion_toVector(quaternion2);
angle = mulf32(quaternion1->w, quaternion2.w) - dotProduct(vector1, vector2);
cross = vectProduct(vector1, vector2);
vector1.x = mulf32(vector1.x, quaternion2.w);
vector1.y = mulf32(vector1.y, quaternion2.w);
vector1.z = mulf32(vector1.z, quaternion2.w);
vector2.x = mulf32(vector2.x, quaternion1->w);
vector2.y = mulf32(vector2.y, quaternion1->w);
vector2.z = mulf32(vector2.z, quaternion1->w);
quaternion1->x = vector1.x + vector2.x + cross.x;
quaternion1->y = vector1.y + vector2.y + cross.y;
quaternion1->z = vector1.z + vector2.z + cross.z;
quaternion1->w = angle;
}
void RubiksCube::Grab(VECTOR touchVector)
{
tvf32[0]+=floattof32(touchVector.X);
tvf32[1]+=floattof32(touchVector.Y);
tvf32[2]=0;
if(f32toint(sqrtf32(mulf32(tvf32[0],tvf32[0])+mulf32(tvf32[1],tvf32[1])+mulf32(tvf32[2],tvf32[2])))>5)
{
VECTOR upv, rightv, rotduv, rotdrv;
int32 uvf32[3];//up vector as f32
int32 rvf32[3];//right vector
int32 magup, magright;
m4x4 grabMatrix;//container for the Position Matrix
vectorFromSide(upv, rightv, clicked[0]);
//printf ("Initial Vector:\n %f, ", tmpv.X);
//printf("%f, ", tmpv.Y);
//printf("%f\n", tmpv.Z);
glGetFixed(GL_GET_MATRIX_POSITION, (int32*)&grabMatrix);
glMatrixMode(GL_MODELVIEW);
//rotate the up vector thru the projection matrix
//and cast it to f32
RotateVector(grabMatrix, upv, rotduv);
uvf32[0]=floattof32(rotduv.X);
uvf32[1]=floattof32(rotduv.Y);
uvf32[2]=floattof32(rotduv.Z);
//rinse and repeat with the right vector
RotateVector(grabMatrix, rightv, rotdrv);
rvf32[0]=floattof32(rotdrv.X);
rvf32[1]=floattof32(rotdrv.Y);
rvf32[2]=floattof32(rotdrv.Z);
if(controlStyle)
{
int32 suvf32[3];
int32 srvf32[3];
suvf32[0]=0;
suvf32[1]=inttof32(1);
suvf32[2]=0;
srvf32[0]=inttof32(1);
srvf32[1]=0;
srvf32[2]=0;
magup=dotf32(uvf32, suvf32);
magright=dotf32(uvf32, srvf32);
if(abs(magup)>abs(magright))
{
for(int i=0; i<3; i++)
{
if(magup>0)
{
rvf32[i]=srvf32[i];
uvf32[i]=suvf32[i];
}
else
{
rvf32[i]=-srvf32[i];
uvf32[i]=-suvf32[i];
}
}
} else {
for(int i=0; i<3; i++)
{
if(magright>0)
{
uvf32[i]=srvf32[i];
rvf32[i]=-suvf32[i];
}
else
{
uvf32[i]=-srvf32[i];
rvf32[i]=suvf32[i];
}
}
}
}
magup=dotf32(uvf32, tvf32);
magright=dotf32(rvf32, tvf32);
if(magup || magright)
{
int32 tmp[2];
if(abs(magup)>abs(magright))
{
tmp[0]=uvf32[0];
tmp[1]=uvf32[1];
unitVector((int32*)tmp);
InitTwist(true, tmp);
}else{
tmp[0]=rvf32[0];
tmp[1]=rvf32[1];
unitVector((int32*)tmp);
InitTwist(false, tmp);
}
Twisting=true;
Grabbing=false;
tvf32[0]=0;
tvf32[1]=0;
}
}
}
void unitVector (int32* vector)
{
int32 magnitude = sqrtf32(mulf32(vector[0], vector[0]) + mulf32(vector[1], vector[1]));
vector[0]=divf32(vector[0], magnitude);
vector[1]=divf32(vector[1], magnitude);
}
//---------------------------------------------------------------------------------
void glRotatef32i(int angle, int32 x, int32 y, int32 z) {
//---------------------------------------------------------------------------------
int32 axis[3];
int32 sine = SIN[angle & LUT_MASK];
int32 cosine = COS[angle & LUT_MASK];
int32 one_minus_cosine = inttof32(1) - cosine;
axis[0]=x;
axis[1]=y;
axis[2]=z;
normalizef32(axis); // should require passed in normalized?
MATRIX_MULT3x3 = cosine + mulf32(one_minus_cosine, mulf32(axis[0], axis[0]));
MATRIX_MULT3x3 = mulf32(one_minus_cosine, mulf32(axis[0], axis[1])) - mulf32(axis[2], sine);
MATRIX_MULT3x3 = mulf32(mulf32(one_minus_cosine, axis[0]), axis[2]) + mulf32(axis[1], sine);
MATRIX_MULT3x3 = mulf32(mulf32(one_minus_cosine, axis[0]), axis[1]) + mulf32(axis[2], sine);
MATRIX_MULT3x3 = cosine + mulf32(mulf32(one_minus_cosine, axis[1]), axis[1]);
MATRIX_MULT3x3 = mulf32(mulf32(one_minus_cosine, axis[1]), axis[2]) - mulf32(axis[0], sine);
MATRIX_MULT3x3 = mulf32(mulf32(one_minus_cosine, axis[0]), axis[2]) - mulf32(axis[1], sine);
MATRIX_MULT3x3 = mulf32(mulf32(one_minus_cosine, axis[1]), axis[2]) + mulf32(axis[0], sine);
MATRIX_MULT3x3 = cosine + mulf32(mulf32(one_minus_cosine, axis[2]), axis[2]);
}
inline Quaternion Quaternion_fromEuler(int32 roll, int32 pitch, int32 yaw) {
//http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q60
/*Quaternion q = Quaternion_identity();
Quaternion qt = Quaternion_identity();
Quaternion qx = { cosLerp(pitch/2), sinLerp(pitch/2), 0, 0 };
Quaternion qy = { cosLerp(yaw/2), 0, sinLerp(yaw/2), 0 };
Quaternion qz = { cosLerp(roll/2), 0, 0, sinLerp(roll/2) };
qt = Quaternion_multiplied(qx, qy);
q = Quaternion_multiplied(qt, qz);
return q;*/
//http://content.gpwiki.org/index.php/OpenGL%3aTutorials%3aUsing_Quaternions_to_represent_rotation#Quaternion_from_Euler_angles
Quaternion this;
int32 p = pitch / 2;
int32 y = yaw / 2;
int32 r = roll / 2;
int32 sinp = sinLerp(p);
int32 siny = sinLerp(y);
int32 sinr = sinLerp(r);
int32 cosp = cosLerp(p);
int32 cosy = cosLerp(y);
int32 cosr = cosLerp(r);
this.x = mulf32(mulf32(sinr, cosp), cosy) - mulf32(mulf32(cosr, sinp), siny);
this.y = mulf32(mulf32(cosr, sinp), cosy) + mulf32(mulf32(sinr, cosp), siny);
this.z = mulf32(mulf32(cosr, cosp), siny) - mulf32(mulf32(sinr, sinp), cosy);
this.w = mulf32(mulf32(cosr, cosp), cosy) + mulf32(mulf32(sinr, sinp), siny);
return this;
}
m4x4 Quaternion_toMatrix(Quaternion quaternion) {
m4x4 matrix;
matrix.m[0] = ((1 << 12) - (2 * (mulf32(quaternion.y,quaternion.y) + mulf32(quaternion.z,quaternion.z))));
matrix.m[1] = (2 * (mulf32(quaternion.x,quaternion.y) + mulf32(quaternion.z,quaternion.w)));
matrix.m[2] = (2 * (mulf32(quaternion.x,quaternion.z) - mulf32(quaternion.y,quaternion.w)));
matrix.m[3] = 0;
matrix.m[4] = (2 * (mulf32(quaternion.x,quaternion.y) - mulf32(quaternion.z,quaternion.w)));
matrix.m[5] = ((1 << 12) - (2 * (mulf32(quaternion.x,quaternion.x) + mulf32(quaternion.z,quaternion.z))));
matrix.m[6] = (2 * (mulf32(quaternion.y,quaternion.z) + mulf32(quaternion.x,quaternion.w)));
matrix.m[7] = 0;
matrix.m[8] = (2 * (mulf32(quaternion.x,quaternion.z) + mulf32(quaternion.y,quaternion.w)));
matrix.m[9] = (2 * (mulf32(quaternion.y,quaternion.z) - mulf32(quaternion.x,quaternion.w)));
matrix.m[10] = ((1 << 12) - (2 * (mulf32(quaternion.x,quaternion.x) + mulf32(quaternion.y,quaternion.y))));
matrix.m[11] = 0;
matrix.m[12] = 0;
matrix.m[13] = 0;
matrix.m[14] = 0;
matrix.m[15] = 1 << 12;
return matrix;
}
void updateTurret(turret_struct* t)
{
if(!t || !t->used)return;
if(!t->OBB || !t->OBB->used){t->OBB=NULL;t->used=false;return;}
t->counter+=2;t->counter%=63; //TEMP
if(t->dead)t->counter=31;
editPalette((u16*)t->OBB->modelInstance.palette,0,RGB15(abs(31-t->counter),0,0)); //TEMP
int32* m=t->OBB->transformationMatrix;
room_struct* r=getPlayer()->currentRoom;
if(!r)return;
int32 angle, d;
bool b=pointInTurretSight(t, getPlayer()->object->position, &angle, &d);
if(b)
{
vect3D u=vectDifference(getPlayer()->object->position,t->laserOrigin);
int32 d=magnitude(u);
u=divideVect(u,d);
if(collideLineMap(&gameRoom, NULL, t->laserOrigin, u, d, NULL, NULL))b=false;
}
switch(t->state)
{
case TURRET_CLOSED:
changeAnimation(&t->OBB->modelInstance, 0, false);
t->drawShot[0]=t->drawShot[1]=0;
if(b && !t->dead){t->state=TURRET_OPENING; playSFX(turretDeploySFX);}
break;
case TURRET_OPENING:
if(t->OBB->modelInstance.currentAnim==2)
{
t->state=TURRET_OPEN;
}else if(t->OBB->modelInstance.currentAnim!=1)
{
changeAnimation(&t->OBB->modelInstance, 2, false);
changeAnimation(&t->OBB->modelInstance, 1, true);
}
t->drawShot[0]=t->drawShot[1]=0;
break;
case TURRET_OPEN:
{
if(angle>mulf32(sinLerp(TURRET_SIGHTANGLE/3),d))changeAnimation(&t->OBB->modelInstance, 4, false);
else if(angle<-mulf32(sinLerp(TURRET_SIGHTANGLE/3),d))changeAnimation(&t->OBB->modelInstance, 5, false);
else changeAnimation(&t->OBB->modelInstance, 2, false);
int i;
for(i=0;i<2;i++)
{
if(!t->drawShot[i] && !(rand()%3))
{
t->drawShot[i]=rand()%8;
t->shotAngle[i]=rand();
shootPlayer(NULL, normalize(vectDifference(t->laserDestination,t->laserOrigin)), 6);
playSFX(turretFireSFX);
}
if(t->drawShot[i])t->drawShot[i]--;
}
if(!b || t->dead){t->state=TURRET_CLOSING; playSFX(turretRetractSFX);}
}
break;
case TURRET_CLOSING:
if(t->OBB->modelInstance.currentAnim==0)
{
t->state=TURRET_CLOSED;
}else if(t->OBB->modelInstance.currentAnim!=3)
{
changeAnimation(&t->OBB->modelInstance, 0, false);
changeAnimation(&t->OBB->modelInstance, 3, true);
}
t->drawShot[0]=t->drawShot[1]=0;
break;
}
if(!t->dead)
{
t->laserOrigin=addVect(vectDivInt(t->OBB->position,4),evalVectMatrix33(m,laserOrigin));
t->laserDestination=addVect(t->laserOrigin,vect(m[2],m[5],m[8]));
if(b)t->laserDestination=getPlayer()->object->position;
vect3D dir=normalize(vectDifference(t->laserDestination,t->laserOrigin));
t->laserThroughPortal=false;
laserProgression(r, &t->laserOrigin, &t->laserDestination, dir);
int32 x, y, z;
vect3D v;
portal_struct* portal=NULL;
if(isPointInPortal(&portal1, t->laserDestination, &v, &x, &y, &z))portal=&portal1;
if(abs(z)>=32)portal=NULL;
if(!portal)
{
if(isPointInPortal(&portal2, t->laserDestination, &v, &x, &y, &z))portal=&portal2;
if(abs(z)>=32)portal=NULL;
}
if(portal)
{
t->laserDestination=addVect(t->laserDestination,vectMult(dir,TILESIZE));
t->laserThroughPortal=true;
dir=warpVector(portal,dir);
t->laserOrigin2=addVect(portal->targetPortal->position, warpVector(portal, vectDifference(t->laserDestination, portal->position)));
t->laserDestination2=addVect(t->laserOrigin2,dir);
laserProgression(r, &t->laserOrigin2, &t->laserDestination2, dir);
t->laserOrigin2=addVect(t->laserOrigin2,vectMult(dir,-TILESIZE));
}
if(m[4]<sinLerp(8192/2))t->dead=true;
}
updateAnimation(&t->OBB->modelInstance);
}
