With the need to calculate the facing angles for pathfinding usage, I need to work out the initial facing direction of the model loaded from the modeller.

D3DXMatrixDecompose doesn't perform it well as it outputs a quaternion as a result

Thanks in advance

Jack

I once extended that so it can handle various orders. I did not check if it works with orders like XYX, but XYZ is ok.

Note that it's for OpenGL matrices, so you need to swap matrix indices.

static v3f ToEulerAngles (matrix4f &m, int order = 0x012) // 0x012 = xyz, 0x201 = zxy and so on...

{
int a0 = (order>>8)&3;
int a1 = (order>>4)&3;
int a2 = (order>>0)&3;

v3f euler;
// Assuming the angles are in radians.
if (m.m[a0][a2] > 0.9999)
{ // singularity at north pole
euler[a0] = -atan2(m.m[a2][a1], m.m[a1][a1]);
euler[a1] = -PI/2;
euler[a2] = 0;
return euler;
}
if (m.m[a0][a2] < -0.9999)
{ // singularity at south pole
euler[a0] = -atan2(m.m[a2][a1], m.m[a1][a1]);
euler[a1] = PI/2;
euler[a2] = 0;
return euler;
}
euler[a0] = -atan2(-m.m[a1][a2], m.m[a2][a2]);
euler[a1] = -asin ( m.m[a0][a2]);
euler[a2] = -atan2(-m.m[a0][a1], m.m[a0][a0]);
return euler;

}

Here's the function from my matrix class.

In my coordinate system, Positive Y is up, positive X is right and positive Z is into the screen which makes it left-handed. Matrices are row-major. The ordering of the euler operations to reconstruct the matrix would be heading, then pitch, then roll. i.e. eulerY then eulerX then eulerZ.

It won't work for matrices with scale, it'd probably be a simple enough modification to make it work with uniform scales, but not non-uniform scaling. If that's a problem, you could always use D3DXMatrixDecompose first, then do something like this on the result, although I'm sure there's a more direct route if it's a performance critical part of your code.

void ExtractHPR(float &fHeading, float &fPitch, float &fRoll) const
{
    // This function is only suitable for rotation matrices.
    My_Assert(IsOrthogonal() && !HasScale(), "This doesn't seem to be a rotation matrix");
    fHeading = atan2f(m_fElements[0][2], m_fElements[2][2]);
    fPitch = asinf(-m_fElements[1][2]);
    fRoll = atan2f(m_fElements[1][0], m_fElements[1][1]);
}
 

This will not work for any rotational matrix. It all depends on the matrix rotation Order. There is a very good article and PDF on www.geometrictools.com about exactly that.

The way i handle it in my engine is like so. But it only works if your Rotation order is YXZ instead of XYZ or any order that you may have used. If your rotation order is YXZ where Y is Yaw,X is Pitch and Z is Roll then my code will work for you. Also my code handles matrix with scale in them with no problem by removing it first from the matrix

before computing the euler angles.

//**************************************************
//Compute the Euler Angles from the matrix.
//The rotation order is:YXZ
//**************************************************
Vector3 Matrix4x4::EulerAngles(const Matrix4x4& matrix)
{
Vector3 dotx = Vector3(matrix.m11,matrix.m12,matrix.m13);
Vector3 doty = Vector3(matrix.m21,matrix.m22,matrix.m23);
Vector3 dotz = Vector3(matrix.m31,matrix.m32,matrix.m33);
float x = Vector3::Dot(dotx,dotx);
if(Mathf::Approximately(x,1.0f)==false)
{
float invx = 1.0f/Mathf::Sqrt(x);
dotx = dotx*invx;
}
float y = Vector3::Dot(doty,doty);
if(Mathf::Approximately(y,1.0f)==false)
{
float invy = 1.0f/Mathf::Sqrt(y);
doty = doty*invy;
}
float z = Vector3::Dot(dotz,dotz);
if(Mathf::Approximately(z,1.0f)==false)
{
float invz = 1.0f/Mathf::Sqrt(z);
dotz = dotz*invz;
}
float thetaX=0;
float thetaY=0;
float thetaZ=0;
if(dotz.y<1)
{
if(dotz.y>-1)
{
thetaX = Mathf::Asin(-dotz.y);
thetaY = Mathf::Atan2(dotz.x,dotz.z);
thetaZ = Mathf::Atan2(dotx.y,doty.y);
}
else
{
thetaX = Mathf::PI*0.5f;
thetaY = -Mathf::Atan2(-doty.x,dotx.x);
thetaZ = 0;
}
}
else
{
thetaX = -Mathf::PI*0.5f;
thetaY = Mathf::Atan2(-doty.x,dotx.x);
thetaZ =0;
}
return Vector3 (Mathf::Repeat(thetaX * Mathf::Rad2Deg,360),
Mathf::Repeat(thetaY * Mathf::Rad2Deg,360),
Mathf::Repeat(thetaZ * Mathf::Rad2Deg,360));
}

Here's the function from my matrix class.

In my coordinate system, Positive Y is up, positive X is right and positive Z is into the screen which makes it left-handed. Matrices are row-major. The ordering of the euler operations to reconstruct the matrix would be heading, then pitch, then roll. i.e. eulerY then eulerX then eulerZ.

It won't work for matrices with scale, it'd probably be a simple enough modification to make it work with uniform scales, but not non-uniform scaling. If that's a problem, you could always use D3DXMatrixDecompose first, then do something like this on the result, although I'm sure there's a more direct route if it's a performance critical part of your code.

void ExtractHPR(float &fHeading, float &fPitch, float &fRoll) const
{
    // This function is only suitable for rotation matrices.
    My_Assert(IsOrthogonal() && !HasScale(), "This doesn't seem to be a rotation matrix");
    fHeading = atan2f(m_fElements[0][2], m_fElements[2][2]);
    fPitch = asinf(-m_fElements[1][2]);
    fRoll = atan2f(m_fElements[1][0], m_fElements[1][1]);
}
 

This will not work for any rotational matrix. It all depends on the matrix rotation Order. There is a very good article and PDF on www.geometrictools.com about exactly that.

The way i handle it in my engine is like so. But it only works if your Rotation order is YXZ instead of XYZ or any order that you may have used. If your rotation order is YXZ where Y is Yaw,X is Pitch and Z is Roll then my code will work for you. Also my code handles matrix with scale in them with no problem by removing it first from the matrix

before computing the euler angles.

//**************************************************
//Compute the Euler Angles from the matrix.
//The rotation order is:YXZ
//**************************************************
Vector3 Matrix4x4::EulerAngles(const Matrix4x4& matrix)
{
Vector3 dotx = Vector3(matrix.m11,matrix.m12,matrix.m13);
Vector3 doty = Vector3(matrix.m21,matrix.m22,matrix.m23);
Vector3 dotz = Vector3(matrix.m31,matrix.m32,matrix.m33);
float x = Vector3::Dot(dotx,dotx);
if(Mathf::Approximately(x,1.0f)==false)
{
float invx = 1.0f/Mathf::Sqrt(x);
dotx = dotx*invx;
}
float y = Vector3::Dot(doty,doty);
if(Mathf::Approximately(y,1.0f)==false)
{
float invy = 1.0f/Mathf::Sqrt(y);
doty = doty*invy;
}
float z = Vector3::Dot(dotz,dotz);
if(Mathf::Approximately(z,1.0f)==false)
{
float invz = 1.0f/Mathf::Sqrt(z);
dotz = dotz*invz;
}
float thetaX=0;
float thetaY=0;
float thetaZ=0;
if(dotz.y<1)
{
if(dotz.y>-1)
{
thetaX = Mathf::Asin(-dotz.y);
thetaY = Mathf::Atan2(dotz.x,dotz.z);
thetaZ = Mathf::Atan2(dotx.y,doty.y);
}
else
{
thetaX = Mathf::PI*0.5f;
thetaY = -Mathf::Atan2(-doty.x,dotx.x);
thetaZ = 0;
}
}
else
{
thetaX = -Mathf::PI*0.5f;
thetaY = Mathf::Atan2(-doty.x,dotx.x);
thetaZ =0;
}
return Vector3 (Mathf::Repeat(thetaX * Mathf::Rad2Deg,360),
Mathf::Repeat(thetaY * Mathf::Rad2Deg,360),
Mathf::Repeat(thetaZ * Mathf::Rad2Deg,360));
}

As I am using the math.h library from the C "standard" library, what is the equivalence of Mathf::Repeat in the C standard library?

I am only using math repeat in order to keep the rotation range in the positive values and keeping it withing the range of 0 to 360. You can just remove the Math repeat and replace it with return Vector3 (thetaX * Mathf::Rad2Deg,thetaY * Mathf::Rad2Deg,thetaZ * Mathf::Rad2Deg,360); The result will be the same it is just that instead of 315 degrees for example you would get -45 degress which is the same thing.

Is Mathf a "off-shelf", "open-source" or "self-written" package that you obtain from?

If open-source, can I have the link of it?

Thanks

Jack

Mathf is my own math class. Here is the repeat fuction

float Mathf::Repeat(float value,float length)
{
return value-Mathf::Floor(value/length)*length;
}