Reimplemented XMMatrixRotationRollPitchYaw(FromVector) (#144)

Author: walbourn

Inc/DirectXMathMatrix.inl

@@ -1655,8 +1655,41 @@ inline XMMATRIX XM_CALLCONV XMMatrixRotationRollPitchYaw
     float Roll
 ) noexcept
 {
+#if defined(_XM_NO_INTRINSICS_)
+    float cp = cosf(Pitch);
+    float sp = sinf(Pitch);
+
+    float cy = cosf(Yaw);
+    float sy = sinf(Yaw);
+
+    float cr = cosf(Roll);
+    float sr = sinf(Roll);
+
+    XMMATRIX M;
+    M.m[0][0] = cr * cy + sr * sp * sy;
+    M.m[0][1] = sr * cp;
+    M.m[0][2] = sr * sp * cy - cr * sy;
+    M.m[0][3] = 0.0f;
+
+    M.m[1][0] = cr * sp * sy - sr * cy;
+    M.m[1][1] = cr * cp;
+    M.m[1][2] = sr * sy + cr * sp * cy;
+    M.m[1][3] = 0.0f;
+
+    M.m[2][0] = cp * sy;
+    M.m[2][1] = -sp;
+    M.m[2][2] = cp * cy;
+    M.m[2][3] = 0.0f;
+
+    M.m[3][0] = 0.0f;
+    M.m[3][1] = 0.0f;
+    M.m[3][2] = 0.0f;
+    M.m[3][3] = 1.0f;
+    return M;
+#else
     XMVECTOR Angles = XMVectorSet(Pitch, Yaw, Roll, 0.0f);
     return XMMatrixRotationRollPitchYawFromVector(Angles);
+#endif
 }
 
 //------------------------------------------------------------------------------
@@ -1666,8 +1699,69 @@ inline XMMATRIX XM_CALLCONV XMMatrixRotationRollPitchYawFromVector
     FXMVECTOR Angles // <Pitch, Yaw, Roll, undefined>
 ) noexcept
 {
-    XMVECTOR Q = XMQuaternionRotationRollPitchYawFromVector(Angles);
-    return XMMatrixRotationQuaternion(Q);
+#if defined(_XM_NO_INTRINSICS_)
+    float cp = cosf(Angles.vector4_f32[0]);
+    float sp = sinf(Angles.vector4_f32[0]);
+
+    float cy = cosf(Angles.vector4_f32[1]);
+    float sy = sinf(Angles.vector4_f32[1]);
+
+    float cr = cosf(Angles.vector4_f32[2]);
+    float sr = sinf(Angles.vector4_f32[2]);
+
+    XMMATRIX M;
+    M.m[0][0] = cr * cy + sr * sp * sy;
+    M.m[0][1] = sr * cp;
+    M.m[0][2] = sr * sp * cy - cr * sy;
+    M.m[0][3] = 0.0f;
+
+    M.m[1][0] = cr * sp * sy - sr * cy;
+    M.m[1][1] = cr * cp;
+    M.m[1][2] = sr * sy + cr * sp * cy;
+    M.m[1][3] = 0.0f;
+
+    M.m[2][0] = cp * sy;
+    M.m[2][1] = -sp;
+    M.m[2][2] = cp * cy;
+    M.m[2][3] = 0.0f;
+
+    M.m[3][0] = 0.0f;
+    M.m[3][1] = 0.0f;
+    M.m[3][2] = 0.0f;
+    M.m[3][3] = 1.0f;
+    return M;
+#else
+    static const XMVECTORF32  Sign = { { { 1.0f, -1.0f, -1.0f, 1.0f } } };
+
+    XMVECTOR SinAngles, CosAngles;
+    XMVectorSinCos(&SinAngles, &CosAngles, Angles);
+
+    XMVECTOR P0 = XMVectorPermute<XM_PERMUTE_1X, XM_PERMUTE_0Z, XM_PERMUTE_1Z, XM_PERMUTE_1X>(SinAngles, CosAngles);
+    XMVECTOR Y0 = XMVectorPermute<XM_PERMUTE_0Y, XM_PERMUTE_1X, XM_PERMUTE_1X, XM_PERMUTE_1Y>(SinAngles, CosAngles);
+    XMVECTOR P1 = XMVectorPermute<XM_PERMUTE_1Z, XM_PERMUTE_0Z, XM_PERMUTE_1Z, XM_PERMUTE_0Z>(SinAngles, CosAngles);
+    XMVECTOR Y1 = XMVectorPermute<XM_PERMUTE_1Y, XM_PERMUTE_1Y, XM_PERMUTE_0Y, XM_PERMUTE_0Y>(SinAngles, CosAngles);
+    XMVECTOR P2 = XMVectorPermute<XM_PERMUTE_0Z, XM_PERMUTE_1Z, XM_PERMUTE_0Z, XM_PERMUTE_1Z>(SinAngles, CosAngles);
+    XMVECTOR P3 = XMVectorPermute<XM_PERMUTE_0Y, XM_PERMUTE_0Y, XM_PERMUTE_1Y, XM_PERMUTE_1Y>(SinAngles, CosAngles);
+    XMVECTOR Y2 = XMVectorSplatX(SinAngles);
+    XMVECTOR NS = XMVectorNegate(SinAngles);
+
+    XMVECTOR Q0 = XMVectorMultiply(P0, Y0);
+    XMVECTOR Q1 = XMVectorMultiply(P1, Sign.v);
+    Q1 = XMVectorMultiply(Q1, Y1);
+    XMVECTOR Q2 = XMVectorMultiply(P2, Y2);
+    Q2 = XMVectorMultiplyAdd(Q2, P3, Q1);
+
+    XMVECTOR V0 = XMVectorPermute<XM_PERMUTE_1X, XM_PERMUTE_0Y, XM_PERMUTE_1Z, XM_PERMUTE_0W>(Q0, Q2);
+    XMVECTOR V1 = XMVectorPermute<XM_PERMUTE_1Y, XM_PERMUTE_0Z, XM_PERMUTE_1W, XM_PERMUTE_0W>(Q0, Q2);
+    XMVECTOR V2 = XMVectorPermute<XM_PERMUTE_0X, XM_PERMUTE_1X, XM_PERMUTE_0W, XM_PERMUTE_0W>(Q0, NS);
+
+    XMMATRIX M;
+    M.r[0] = XMVectorSelect(g_XMZero, V0, g_XMSelect1110.v);
+    M.r[1] = XMVectorSelect(g_XMZero, V1, g_XMSelect1110.v);
+    M.r[2] = XMVectorSelect(g_XMZero, V2, g_XMSelect1110.v);
+    M.r[3] = g_XMIdentityR3;
+    return M;
+#endif
 }
 
 //------------------------------------------------------------------------------