I am having trouble with my interpolate function in my camera class. The function takes in two matrices, converts them to quaternions and then interpolates between the two based on time. The problem is when it rebuilds it into a matrix the XVector of the matrix(_11,_12,_13) will change even if the the two matrices that are passed in are the same. I am not that good with quaternions so any help would be appreciated. Thank you!



D3DXMATRIX CCamera::Interpolate(const D3DXMATRIX &MatrixA, const D3DXMATRIX &MatrixB, float lamda)
{
//Starting matrix
D3DXMATRIX Mat = MatrixA;
//Final result matrix
D3DXMATRIX FinalMatrix = MatrixB;

// Inverse of MatrixA
D3DXMatrixInverse(&Mat, NULL, &Mat);

// Remove MatrixA's transformation from MatrixB
FinalMatrix *= Mat;

// Mat is now the intermediary transformation from MatrixA to MatrixB
// ie: Mat * MatrixA = MatrixB
Mat = FinalMatrix;

// The trace of our matrix
float trace = 1.0f + Mat._11 + Mat._22 + Mat._33;

D3DXQUATERNION quatResult;

// Calculate the quaternion of Mat
// If trace is greater than 0, but consider small values that
// might result in 0 when operated upon due to floating point error
if( trace > 0.0f)
{
float S = sqrt(trace + 1.0f)*2.0f;
quatResult.x = (Mat._32 - Mat._23) / S;
quatResult.y = (Mat._13 - Mat._31) / S;
quatResult.z = (Mat._21 - Mat._12) / S;
quatResult.w = 0.25f * S;
}
else
{
if( Mat._11 > Mat._22 && Mat._11 > Mat._33 )
{
float S = sqrt( 1.0f + Mat._11 - Mat._22 - Mat._22 ) * 2.0f;
quatResult.x = 0.25f * S;
quatResult.y = (Mat._21 + Mat._12) / S;
quatResult.z = (Mat._13 + Mat._31) / S;
quatResult.w = (Mat._32 - Mat._23) / S;
}
else if( Mat._22 > Mat._33)
{
float S = sqrt( 1.0f + Mat._22 - Mat._11 - Mat._33) * 2.0f;
quatResult.x = (Mat._12 + Mat._21) / S;
quatResult.y = 0.25f * S;
quatResult.z = (Mat._32 + Mat._23) / S;
quatResult.w = (Mat._13 - Mat._31) / S;
}
else
{
float S = sqrt( 1.0f + Mat._33 - Mat._11 - Mat._22 ) * 2.0f;
quatResult.x = (Mat._13 + Mat._31) / S;
quatResult.y = (Mat._32 + Mat._23) / S;
quatResult.z = 0.25f * S;
quatResult.w = (Mat._21 - Mat._12) / S;
}
}

// Get the magnitude of our quaternion
float quatMagnitude = sqrt( quatResult[0]*quatResult[0] + quatResult[1]*quatResult[1] + quatResult[2]*quatResult[2] + quatResult[3]*quatResult[3] );

// Normalize our quaternion
D3DXQUATERNION quatNormalized(quatResult[0]/quatMagnitude, quatResult[1]/quatMagnitude, quatResult[2]/quatMagnitude, quatResult[3]/quatMagnitude);

// Calculate the angles relevant to our quaternion
float cos_a = quatNormalized[3];
float angle = acos( cos_a ) * 2;
float sin_a = sqrt( 1.0f - cos_a * cos_a );

// If there was no rotation between matrices, calculation
// of the rotation matrix will end badly. So just do the linear
// interpolation of the translation component and return
if( angle == 0.0 )
{
FinalMatrix = MatrixA;

FinalMatrix._41 = MatrixA._41 + ((MatrixB._41 - MatrixA._41)*lamda);
FinalMatrix._42 = MatrixA._42 + ((MatrixB._42 - MatrixA._42)*lamda);
FinalMatrix._43 = MatrixA._43 + ((MatrixB._43 - MatrixA._43)*lamda);

float Rotation = ((OPlayer*)m_ObjectAttachedTo)->GetCurrentCameraRotation();

if( Rotation > MAX_CAMERA_ROT_X )
{
Rotation = MAX_CAMERA_ROT_X;
}
else if( Rotation < MIN_CAMERA_ROT_X )
{
Rotation = MIN_CAMERA_ROT_X;
}

((OPlayer*)m_ObjectAttachedTo)->SetCurrentCameraRotation(Rotation);
//FinalMatrix = RotateLocalX(FinalMatrix,Rotation);

return FinalMatrix;
}

D3DXVECTOR3 axis;

if( fabs( sin_a ) < 0.0005f )
sin_a = 1;

axis.x = quatNormalized[0] / sin_a;
axis.y = quatNormalized[1] / sin_a;
axis.z = quatNormalized[2] / sin_a;

angle *= lamda;

D3DXVec3Normalize(&axis, &axis);

sin_a = sin( angle / 2 );
cos_a = cos( angle / 2 );
quatNormalized.x = axis.x * sin_a;
quatNormalized.y = axis.y * sin_a;
quatNormalized.z = axis.z * sin_a;
quatNormalized.w = cos_a;

quatMagnitude = sqrt(quatNormalized.x*quatNormalized.x + quatNormalized.y*quatNormalized.y + quatNormalized.z*quatNormalized.z + quatNormalized.w*quatNormalized.w);
quatNormalized.x /= quatMagnitude;
quatNormalized.y /= quatMagnitude;
quatNormalized.z /= quatMagnitude;
quatNormalized.w /= quatMagnitude;



float xx = quatNormalized.x * quatNormalized.x;
float xy = quatNormalized.x * quatNormalized.y;
float xz = quatNormalized.x * quatNormalized.z;
float xw = quatNormalized.x * quatNormalized.w;
float yy = quatNormalized.y * quatNormalized.y;
float yz = quatNormalized.y * quatNormalized.z;
float yw = quatNormalized.y * quatNormalized.w;
float zz = quatNormalized.z * quatNormalized.z;
float zw = quatNormalized.z * quatNormalized.w;

FinalMatrix._11 = 1 - 2 * ( yy + zz );
FinalMatrix._12 = 2 * ( xy - zw );
FinalMatrix._13 = ( xz + yw );
FinalMatrix._14 = 0;
FinalMatrix._21 = 2 * ( xy + zw );
FinalMatrix._22 = 1 - 2 * ( xx + zz );
FinalMatrix._23 = 2 * ( yz - xw );
FinalMatrix._24 = 0;
FinalMatrix._31 = 2 * ( xz - yw );
FinalMatrix._32 = 2 * ( yz + xw );
FinalMatrix._33 = 1 - 2 * ( xx + yy );
FinalMatrix._34 = 0;
FinalMatrix._41 = 0;
FinalMatrix._42 = 0;
FinalMatrix._43 = 0;
FinalMatrix._44 = 1;


FinalMatrix *= MatrixA;

FinalMatrix._41 = MatrixA._41 + ((MatrixB._41-MatrixA._41)*lamda);
FinalMatrix._42 = MatrixA._42 + ((MatrixB._42-MatrixA._42)*lamda);
FinalMatrix._43 = MatrixA._43 + ((MatrixB._43-MatrixA._43)*lamda);

float Rotation = ((OPlayer*)m_ObjectAttachedTo)->GetCurrentCameraRotation();

if( Rotation > MAX_CAMERA_ROT_X )
{
Rotation = MAX_CAMERA_ROT_X;
}
else if( Rotation < MIN_CAMERA_ROT_X )
{
Rotation = MIN_CAMERA_ROT_X;
}
((OPlayer*)m_ObjectAttachedTo)->SetCurrentCameraRotation(Rotation);

// FalMatrix = RotateLocalX(FinalMatrix,Rotation);

return FinalMatrix;
}