I have got a rotation matrix set up and it seems to work well, but I am having a problem getting the correct end result when I rotate a local point around a rotation point. My code:
glm::mat4 clsBone::Rotate( float a_Pitch, float a_Yaw, float a_Roll )
{
glm::mat4 l_M = m_MatrixHandler->GetRotationMatrix( );
OutputDebugStringA( ( "Old Rotation Matrix: \n" +
std::to_string( l_M[ 0 ][ 0 ] ) + ", " + std::to_string( l_M[ 0 ][ 1 ] ) + ", " + std::to_string( l_M[ 0 ][ 2 ] ) + ", " + std::to_string( l_M[ 0 ][ 3 ] ) + "\n" +
std::to_string( l_M[ 1 ][ 0 ] ) + ", " + std::to_string( l_M[ 1 ][ 1 ] ) + ", " + std::to_string( l_M[ 1 ][ 2 ] ) + ", " + std::to_string( l_M[ 1 ][ 3 ] ) + "\n" +
std::to_string( l_M[ 2 ][ 0 ] ) + ", " + std::to_string( l_M[ 2 ][ 1 ] ) + ", " + std::to_string( l_M[ 2 ][ 2 ] ) + ", " + std::to_string( l_M[ 2 ][ 3 ] ) + "\n" +
std::to_string( l_M[ 3 ][ 0 ] ) + ", " + std::to_string( l_M[ 3 ][ 1 ] ) + ", " + std::to_string( l_M[ 3 ][ 2 ] ) + ", " + std::to_string( l_M[ 3 ][ 3 ] ) + "\n" ).c_str( ) );
glm::mat4 l_RotMatrix = m_MatrixHandler->Rotate( a_Pitch, a_Yaw, a_Roll );
l_M = l_RotMatrix;
OutputDebugStringA( ( "New Rotation Matrix: \n" +
std::to_string( l_M[ 0 ][ 0 ] ) + ", " + std::to_string( l_M[ 0 ][ 1 ] ) + ", " + std::to_string( l_M[ 0 ][ 2 ] ) + ", " + std::to_string( l_M[ 0 ][ 3 ] ) + "\n" +
std::to_string( l_M[ 1 ][ 0 ] ) + ", " + std::to_string( l_M[ 1 ][ 1 ] ) + ", " + std::to_string( l_M[ 1 ][ 2 ] ) + ", " + std::to_string( l_M[ 1 ][ 3 ] ) + "\n" +
std::to_string( l_M[ 2 ][ 0 ] ) + ", " + std::to_string( l_M[ 2 ][ 1 ] ) + ", " + std::to_string( l_M[ 2 ][ 2 ] ) + ", " + std::to_string( l_M[ 2 ][ 3 ] ) + "\n" +
std::to_string( l_M[ 3 ][ 0 ] ) + ", " + std::to_string( l_M[ 3 ][ 1 ] ) + ", " + std::to_string( l_M[ 3 ][ 2 ] ) + ", " + std::to_string( l_M[ 3 ][ 3 ] ) + "\n" ).c_str( ) );
glm::vec4 l_LocalPos = glm::vec4( m_EndJoint->m_Position - m_StartJoint->m_Position, 1 );
OutputDebugStringA( ( "Old Local Pos: " + std::to_string( l_LocalPos.x ) + ", " + std::to_string( l_LocalPos.y ) + ", " + std::to_string( l_LocalPos.z ) + "\n" ).c_str( ) );
glm::vec4 l_NewLocalPos = l_LocalPos * l_RotMatrix;
OutputDebugStringA( ( "New Local Pos: " + std::to_string( l_NewLocalPos.x ) + ", " + std::to_string( l_NewLocalPos.y ) + ", " + std::to_string( l_NewLocalPos.z ) + "\n" ).c_str( ) );
return l_RotMatrix;
}
glm::mat4 clsMatrixHandler::Rotate( float a_Pitch, float a_Yaw, float a_Roll )
{
glm::mat4 l_Rotx;
glm::mat4 l_Roty;
glm::mat4 l_Rotz;
PitchYawRollToXYZMatrices( a_Pitch, a_Yaw, a_Roll, l_Rotx, l_Roty, l_Rotz );
m_PitchYawRolls.push_back( glm::vec3( a_Pitch, a_Yaw, a_Roll ) );
glm::mat4 l_RotationMatrix = l_Rotx * l_Roty * l_Rotz;
m_RotationMatrix *= l_RotationMatrix;
m_TransformMatrix = m_RotationMatrix * m_TranslationMatrix;
return m_RotationMatrix;
}
Result:
Old Rotation Matrix:
1.000000, 0.000000, 0.000000, 0.000000
0.000000, -0.000000, 1.000000, 0.000000
0.000000, -1.000000, -0.000000, 0.000000
0.000000, 0.000000, 0.000000, 1.000000
New Rotation Matrix:
0.707107, 0.707107, 0.000000, 0.000000
0.000000, -0.000000, 1.000000, 0.000000
0.707107, -0.707107, -0.000000, 0.000000
0.000000, 0.000000, 0.000000, 1.000000
Old Local Pos: 0.000000, -40.000000, -0.000002
New Local Pos: -28.284271, 0.000000, 28.284271
How is it possible that the new local position rotated 90 degrees into a diagonal direction while the rotation matrix rotated 45 degrees in its local y axis (world z axis)?
So it seems I made a mistake here. I am multiplying the local position with the rotation matrix, but I should have multiplied the other way around:
glm::vec4 l_NewLocalPos = l_RotMatrix * l_LocalPos;
This will output:
Old Rotation Matrix:
1.000000, 0.000000, 0.000000, 0.000000
0.000000, -0.000000, 1.000000, 0.000000
0.000000, -1.000000, -0.000000, 0.000000
0.000000, 0.000000, 0.000000, 1.000000
New Rotation Matrix:
0.707107, 0.707107, 0.000000, 0.000000
0.000000, -0.000000, 1.000000, 0.000000
0.707107, -0.707107, -0.000000, 0.000000
0.000000, 0.000000, 0.000000, 1.000000
Old Local Pos: 0.000000, -40.000000, -0.000002
New Local Pos: 0.000000, 0.000000, -40.000000
Which makes sense: From the identity rotation matrix, I am first pitching down, which moves the local point from (0, -40, 0) to (0, 0, -40), then I yaw 45 degrees, but since the local point is on the yaw rotation axis (local y axis, world z axis), the point will not move.