From a2b340a548325ca6dc378bc4282aaaf9b2447f9f Mon Sep 17 00:00:00 2001 From: lawnjelly Date: Tue, 29 Apr 2025 18:17:19 +0100 Subject: [PATCH] FTI - Add custom interpolation for wheels --- scene/3d/physics/vehicle_body_3d.cpp | 123 ++++++++++++++++++++++++++- scene/3d/physics/vehicle_body_3d.h | 46 ++++++++++ 2 files changed, 166 insertions(+), 3 deletions(-) diff --git a/scene/3d/physics/vehicle_body_3d.cpp b/scene/3d/physics/vehicle_body_3d.cpp index f02dc489f2d..5efa207f248 100644 --- a/scene/3d/physics/vehicle_body_3d.cpp +++ b/scene/3d/physics/vehicle_body_3d.cpp @@ -30,6 +30,8 @@ #include "vehicle_body_3d.h" +#include "core/config/engine.h" + #define ROLLING_INFLUENCE_FIX class btVehicleJacobianEntry { @@ -78,6 +80,40 @@ public: } }; +void VehicleWheel3D::FTIData::update_world_xform(Transform3D &r_xform, real_t p_interpolation_fraction) { + // Note that when unset (during the first few frames before a physics tick) + // the xform will be whatever it was loaded as. + if (!unset) { + real_t f = p_interpolation_fraction; + + WheelXform i; + i.up = prev.up.lerp(curr.up, f); + i.up.normalize(); + i.right = prev.right.lerp(curr.right, f); + i.right.normalize(); + + Vector3 fwd = i.up.cross(i.right); + fwd.normalize(); + + i.origin = prev.origin.lerp(curr.origin, f); + i.steering = Math::lerp(prev.steering, curr.steering, f); + + real_t rotation = Math::lerp(curr_rotation - curr_rotation_delta, curr_rotation, f); + + Basis steeringMat(i.up, i.steering); + + Basis rotatingMat(i.right, rotation); + + Basis basis2( + i.right[0], i.up[0], fwd[0], + i.right[1], i.up[1], fwd[1], + i.right[2], i.up[2], fwd[2]); + + r_xform.set_basis(steeringMat * rotatingMat * basis2); + r_xform.set_origin(i.origin); + } +} + void VehicleWheel3D::_notification(int p_what) { switch (p_what) { case NOTIFICATION_ENTER_TREE: { @@ -381,6 +417,28 @@ void VehicleBody3D::_update_wheel(int p_idx, PhysicsDirectBodyState3D *s) { Vector3 fwd = up.cross(right); fwd = fwd.normalized(); + VehicleWheel3D::FTIData &id = wheel.fti_data; + + Vector3 origin = wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength; + + if (is_physics_interpolated_and_enabled()) { + id.curr.up = up; + id.curr.right = right; + id.curr.steering = wheel.m_steering; + id.curr.origin = origin; + + // Pump the xform the first update to the wheel, + // otherwise the world xform prev will be NULL. + if (id.unset || id.reset_queued) { + id.unset = false; + id.reset_queued = false; + id.pump(); + } + + // The physics etc relies on the rest of this being correct, even with FTI, + // so we can't pre-maturely return here. + } + Basis steeringMat(up, wheel.m_steering); Basis rotatingMat(right, wheel.m_rotation); @@ -392,8 +450,7 @@ void VehicleBody3D::_update_wheel(int p_idx, PhysicsDirectBodyState3D *s) { wheel.m_worldTransform.set_basis(steeringMat * rotatingMat * basis2); //wheel.m_worldTransform.set_basis(basis2 * (steeringMat * rotatingMat)); - wheel.m_worldTransform.set_origin( - wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength); + wheel.m_worldTransform.set_origin(origin); } real_t VehicleBody3D::_ray_cast(int p_idx, PhysicsDirectBodyState3D *s) { @@ -817,6 +874,58 @@ void VehicleBody3D::_update_friction(PhysicsDirectBodyState3D *s) { } } +void VehicleBody3D::_physics_interpolated_changed() { + _update_process_mode(); + RigidBody3D::_physics_interpolated_changed(); +} + +void VehicleBody3D::fti_pump_xform() { + for (int i = 0; i < wheels.size(); i++) { + VehicleWheel3D &w = *wheels[i]; + w.fti_data.pump(); + } + + RigidBody3D::fti_pump_xform(); +} + +void VehicleBody3D::_update_process_mode() { + set_process_internal(is_physics_interpolated_and_enabled()); +} + +void VehicleBody3D::_notification(int p_what) { + switch (p_what) { + case NOTIFICATION_ENTER_TREE: { + _update_process_mode(); + } break; + case NOTIFICATION_INTERNAL_PROCESS: { +#ifdef DEV_ENABLED + if (!is_physics_interpolated_and_enabled()) { + WARN_PRINT_ONCE("VehicleBody NOTIFICATION_INTERNAL_PROCESS with physics interpolation OFF. (benign)"); + } +#endif + real_t f = Engine::get_singleton()->get_physics_interpolation_fraction(); + + Transform3D xform; + Transform3D inv_vehicle_xform = get_global_transform_interpolated().affine_inverse(); + + for (int i = 0; i < wheels.size(); i++) { + VehicleWheel3D &w = *wheels[i]; + w.fti_data.update_world_xform(xform, f); + w.set_transform(inv_vehicle_xform * xform); + } + } break; + case NOTIFICATION_RESET_PHYSICS_INTERPOLATION: { + _update_process_mode(); + for (int i = 0; i < wheels.size(); i++) { + VehicleWheel3D &w = *wheels[i]; + w.fti_data.reset_queued = true; + } + } break; + default: + break; + } +} + void VehicleBody3D::_body_state_changed(PhysicsDirectBodyState3D *p_state) { RigidBody3D::_body_state_changed(p_state); @@ -826,9 +935,14 @@ void VehicleBody3D::_body_state_changed(PhysicsDirectBodyState3D *p_state) { _update_wheel(i, p_state); } + bool use_fti = is_physics_interpolated_and_enabled(); + for (int i = 0; i < wheels.size(); i++) { _ray_cast(i, p_state); - wheels[i]->set_transform(p_state->get_transform().inverse() * wheels[i]->m_worldTransform); + if (!use_fti) { + // TODO: can this path also just use world space directly instead of inverse parent space? + wheels[i]->set_transform(p_state->get_transform().inverse() * wheels[i]->m_worldTransform); + } } _update_suspension(p_state); @@ -880,6 +994,9 @@ void VehicleBody3D::_body_state_changed(PhysicsDirectBodyState3D *p_state) { } wheel.m_rotation += wheel.m_deltaRotation; + if (use_fti) { + wheel.fti_data.rotate(wheel.m_deltaRotation); + } wheel.m_rpm = ((wheel.m_deltaRotation / step) * 60) / Math::TAU; wheel.m_deltaRotation *= real_t(0.99); //damping of rotation when not in contact diff --git a/scene/3d/physics/vehicle_body_3d.h b/scene/3d/physics/vehicle_body_3d.h index 0e36691248f..21614309a1c 100644 --- a/scene/3d/physics/vehicle_body_3d.h +++ b/scene/3d/physics/vehicle_body_3d.h @@ -40,6 +40,44 @@ class VehicleWheel3D : public Node3D { friend class VehicleBody3D; + struct WheelXform { + Vector3 up; + Vector3 right; + Vector3 origin; + real_t steering = 0; + }; + + class FTIData { + real_t curr_rotation = 0; + real_t curr_rotation_delta = 0; + + public: + WheelXform curr; + WheelXform prev; + + // If a wheel is added on a frame, the xform will not be set until it has been physics updated at least once. + bool unset = true; + bool reset_queued = false; + + void rotate(real_t p_delta) { + curr_rotation += p_delta; + curr_rotation_delta = p_delta; + + // Wrap rotation to prevent float error. + double wrapped = Math::fmod(curr_rotation + Math::PI, Math::TAU); + if (wrapped < 0) { + wrapped += Math::TAU; + } + curr_rotation = wrapped - Math::PI; + } + + void pump() { + prev = curr; + curr_rotation_delta = 0; + } + void update_world_xform(Transform3D &r_xform, real_t p_interpolation_fraction); + } fti_data; + Transform3D m_worldTransform; Transform3D local_xform; bool engine_traction = false; @@ -193,6 +231,8 @@ class VehicleBody3D : public RigidBody3D { void _update_wheel_transform(VehicleWheel3D &wheel, PhysicsDirectBodyState3D *s); void _update_wheel(int p_idx, PhysicsDirectBodyState3D *s); + void _update_process_mode(); + friend class VehicleWheel3D; Vector wheels; @@ -201,6 +241,12 @@ class VehicleBody3D : public RigidBody3D { static void _body_state_changed_callback(void *p_instance, PhysicsDirectBodyState3D *p_state); virtual void _body_state_changed(PhysicsDirectBodyState3D *p_state) override; +protected: + void _notification(int p_what); + + virtual void _physics_interpolated_changed() override; + virtual void fti_pump_xform() override; + public: void set_engine_force(real_t p_engine_force); real_t get_engine_force() const;