update vehicle animator (#894)

NcStudios · Feb 17, 2025 · d16eeb1 · d16eeb1
1 parent c047198
commit d16eeb1
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 8 deletions.
diff --git a/include/ncmath/MatrixUtilities.h b/include/ncmath/MatrixUtilities.h
@@ -47,10 +47,12 @@ inline auto DecomposeRotation(DirectX::FXMMATRIX in) noexcept -> DirectX::XMVECT
 inline auto DecomposeScale(DirectX::FXMMATRIX in) noexcept -> DirectX::XMVECTOR
 {
     using namespace DirectX;
-    auto out = XMVectorSplatX(XMVector3Length(in.r[0]));
-    out = XMVectorPermute<XM_PERMUTE_0X, XM_PERMUTE_1Y, XM_PERMUTE_0Z, XM_PERMUTE_0W>(out, XMVector3Length(in.r[1]));
-    out = XMVectorPermute<XM_PERMUTE_0X, XM_PERMUTE_0Y, XM_PERMUTE_1X, XM_PERMUTE_0W>(out, XMVector3Length(in.r[2]));
-    return out;
+    constexpr auto selectX1Y1Z2 = XMVECTORU32{XM_SELECT_0, XM_SELECT_0, XM_SELECT_1, XM_SELECT_0};
+    const auto& x = XMVector3LengthSq(in.r[0]);
+    const auto& y = XMVector3LengthSq(in.r[1]);
+    const auto& z = XMVector3LengthSq(in.r[2]);
+    const auto xyz = XMVectorSelect(XMVectorMergeXY(x, y), z, selectX1Y1Z2);
+    return XMVectorSqrt(xyz);
 }
 
 inline DirectX::XMVECTOR ToXMVector(const Vector3& v)

diff --git a/source/ncengine/physics/jolt/VehicleAnimator.cpp b/source/ncengine/physics/jolt/VehicleAnimator.cpp
@@ -12,11 +12,15 @@ using namespace DirectX;
 auto CalculateWheelRotation(const JPH::Wheel& wheel) -> XMVECTOR
 {
     const auto* settings = wheel.GetSettings();
-    const auto right = nc::physics::ToXMVector(settings->mWheelUp.Cross(settings->mWheelForward).Normalized());
+    const auto up = nc::physics::ToXMVector(settings->mWheelUp);
+    const auto forward = nc::physics::ToXMVector(settings->mWheelForward);
+    const auto right = XMVector3Normalize(XMVector3Cross(up, forward));
+    const auto baseRotation = XMQuaternionRotationMatrix(XMMATRIX{right, up, forward, g_XMIdentityR3});
     const auto steerAxis = nc::physics::ToXMVector(settings->mSteeringAxis);
-    const auto spinRotation = XMQuaternionRotationAxis(right, wheel.GetRotationAngle());
-    const auto steerRotation = XMQuaternionRotationAxis(steerAxis, wheel.GetSteerAngle());
-    return XMQuaternionMultiply(spinRotation, steerRotation);
+    const auto spinRotation = XMQuaternionRotationNormal(right, wheel.GetRotationAngle());
+    const auto steerRotation = XMQuaternionRotationNormal(steerAxis, wheel.GetSteerAngle());
+    const auto animatedRotation = XMQuaternionMultiply(spinRotation, steerRotation);
+    return XMQuaternionMultiply(baseRotation, animatedRotation);
 }
 
 auto CalculateWheelPosition(const JPH::Wheel& wheel) -> XMVECTOR