FTI - Add custom interpolation for wheels

Author: lawnjelly

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 @@ class btVehicleJacobianEntry {
 	}
 };
 
+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

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<VehicleWheel3D *> 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;