Posted: 29th Dec 2007 20:41
struct eulr_angls
{
float X, Y, Z;
};
struct quaternion
{
float w, x, y, z;
};
quaternion CrossProduct(quaternion q1, quaternion q2)
{
quaternion r;
r.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
r.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
r.y = q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z;
r.z = q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x;
return r;
}
quaternion CreateAxisRotationQuaternion( float fTheta, float fX, float fY, float fZ )
{
quaternion q;
q.w = dbCOS( fTheta / 2 );
q.x = dbSIN( fTheta / 2 ) * fX;
q.y = dbSIN( fTheta / 2 ) * fY;
q.z = dbSIN( fTheta / 2 ) * fZ;
return q;
}
quaternion ConvertEulerAnglesToQuaternion( float fX, float fY, float fZ )
{
// Create the quaternions for pitch, turn, roll
quaternion p, t, r;
p = CreateAxisRotationQuaternion( fX, 1, 0, 0 );
t = CreateAxisRotationQuaternion( fY, 0, 1, 0 );
r = CreateAxisRotationQuaternion( fZ, 0, 0, 1 );
quaternion q;
q = CrossProduct( p, t );
q = CrossProduct( q, r );
return q;
}
eulr_angls ConvertQuaternionToEulerAngles( quaternion q )
{
int PI = 3.14159265359;
float r00, r01, r02;
float r10, r11, r12;
float r20, r21, r22;
r00 = 1 - 2 * q.y * q.y - 2 * q.z * q.z;
r01 = 2 * q.x * q.y + 2 * q.w * q.z;
r02 = 2 * q.x * q.z - 2 * q.w * q.y;
r10 = 2 * q.x * q.y - 2 * q.w * q.z;
r11 = 1 - 2 * q.x * q.x - 2 * q.z * q.z;
r12 = 2 * q.y * q.z + 2 * q.w * q.x;
r20 = 2 * q.x * q.z + 2 * q.w * q.y;
r21 = 2 * q.y * q.z - 2 * q.w * q.x;
r22 = 1 - 2 * q.x * q.x - 2 * q.y * q.y;
float thetaX, thetaY, thetaZ;
if (r02 < +1)
{
if (r02 > -1)
{
thetaY = asin(r02);
thetaX = atan2(-r12,r22);
thetaZ = atan2(-r01,r00);
}
else // r02 = -1
{
// Not a unique solution: thetaZ - thetaX = atan2(r10,r11)
thetaY = -PI/2;
thetaX = -atan2(r10,r11);
thetaZ = 0;
}
}
else // r02 = +1
{
// Not a unique solution: thetaZ + thetaX = atan2(r10,r11)
thetaY = PI;
thetaX = atan2(r10,r11);
thetaZ = 0;
}
eulr_angls EulerAngles;
EulerAngles.X = 180 / PI * thetaX;
EulerAngles.Y = 180 / PI * thetaY;
EulerAngles.Z = 180 / PI * thetaZ;
return EulerAngles;
}
//convert euler angles to quaternion q1
//create X axis rotation quaternion q2
//cross product q1 and q2: r = q1 * q2
//convert quaternion r to euler angles
//rotate the object using the euler angles
void dbInertialXRotateObject( int iObject, float fX )
{
}
void dbInertialYRotateObject( int iObject, float fY )
{
quaternion q1, q2, r;
q1 = ConvertEulerAnglesToQuaternion( dbObjectAngleX(iObject), dbObjectAngleY(iObject),dbObjectAngleZ(iObject) );
q2 = CreateAxisRotationQuaternion( 1, 0, 1, 0 );
r = CrossProduct( q1, q2 );
eulr_angls EulerAngles;
EulerAngles = ConvertQuaternionToEulerAngles( r );
dbRotateObject( iObject, EulerAngles.X, EulerAngles.Y, EulerAngles.Z );
}
void dbInertialZRotateObject( int iObject, float fZ )
{
}