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 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);
}
