From 6f25aee04f1f0ae57bd220afced9cc45712405b3 Mon Sep 17 00:00:00 2001 From: drakeerv Date: Thu, 16 Jan 2025 22:57:37 -0500 Subject: [PATCH] Fix get_rpm() on wheel which has steering This fixes #101646. --- scene/3d/physics/vehicle_body_3d.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/scene/3d/physics/vehicle_body_3d.cpp b/scene/3d/physics/vehicle_body_3d.cpp index c5c9153059d..c508d4ff4ff 100644 --- a/scene/3d/physics/vehicle_body_3d.cpp +++ b/scene/3d/physics/vehicle_body_3d.cpp @@ -852,18 +852,26 @@ void VehicleBody3D::_body_state_changed(PhysicsDirectBodyState3D *p_state) { for (int i = 0; i < wheels.size(); i++) { VehicleWheel3D &wheel = *wheels[i]; Vector3 relpos = wheel.m_raycastInfo.m_hardPointWS - p_state->get_transform().origin; - Vector3 vel = p_state->get_linear_velocity() + (p_state->get_angular_velocity()).cross(relpos); // * mPos); + Vector3 vel = p_state->get_linear_velocity() + (p_state->get_angular_velocity()).cross(relpos); if (wheel.m_raycastInfo.m_isInContact) { const Transform3D &chassisWorldTransform = p_state->get_transform(); + // Get forward vector. Vector3 fwd( chassisWorldTransform.basis[0][Vector3::AXIS_Z], chassisWorldTransform.basis[1][Vector3::AXIS_Z], chassisWorldTransform.basis[2][Vector3::AXIS_Z]); + // Apply steering rotation to forward vector for steerable wheels. + if (wheel.steers) { + Basis steering_mat(Vector3(0, 1, 0), wheel.m_steering); + fwd = steering_mat.xform(fwd); + } + real_t proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS); fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj; + fwd.normalize(); real_t proj2 = fwd.dot(vel);