[WIP] Fixed Timestep Interpolation (3D) - version four

core/engine.cpp

@@ -75,6 +75,10 @@ uint32_t Engine::get_frame_delay() const {
 	return _frame_delay;
 }
 
+void Engine::set_physics_interpolation_enabled(bool p_enabled) {
+	_physics_interpolation_enabled = is_editor_hint() ? false : p_enabled;
+}
+
 void Engine::set_time_scale(float p_scale) {
 	_time_scale = p_scale;
 }

core/engine.h

@@ -60,6 +60,7 @@ class Engine {
 	bool _gpu_pixel_snap;
 	uint64_t _physics_frames;
 	float _physics_interpolation_fraction;
+	bool _physics_interpolation_enabled;
 	bool _portals_active;
 	bool _occlusion_culling_active;
 
@@ -96,6 +97,9 @@ class Engine {
 	float get_idle_frame_step() const { return _frame_step; }
 	float get_physics_interpolation_fraction() const { return _physics_interpolation_fraction; }
 
+	void set_physics_interpolation_enabled(bool p_enabled);
+	bool is_physics_interpolation_enabled() const { return _physics_interpolation_enabled; }
+
 	void set_time_scale(float p_scale);
 	float get_time_scale() const;
 

core/local_vector.h

@@ -101,6 +101,22 @@ class LocalVector {
 		}
 	}
 
+	U erase_multiple_unordered(const T &p_val) {
+		U from = 0;
+		U count = 0;
+		while (true) {
+			int64_t idx = find(p_val, from);
+
+			if (idx == -1) {
+				break;
+			}
+			remove_unordered(idx);
+			from = idx;
+			count++;
+		}
+		return count;
+	}
+
 	void invert() {
 		for (U i = 0; i < count / 2; i++) {
 			SWAP(data[i], data[count - i - 1]);

core/math/interpolator.cpp

@@ -0,0 +1,52 @@
+/*************************************************************************/
+/*  interpolator.cpp                                                     */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "interpolator.h"
+
+#include "core/math/transform.h"
+
+void Interpolator::interpolate_transform_linear(const Transform &p_prev, const Transform &p_curr, Transform &r_result, real_t p_fraction) {
+	// interpolate translate
+	r_result.origin = p_prev.origin + ((p_curr.origin - p_prev.origin) * p_fraction);
+
+	// interpolate basis
+	for (int n = 0; n < 3; n++) {
+		r_result.basis.elements[n] = p_prev.basis.elements[n].linear_interpolate(p_curr.basis.elements[n], p_fraction);
+	}
+}
+
+real_t Interpolator::checksum_transform(const Transform &p_transform) {
+	// just a really basic checksum, this can probably be improved
+	real_t sum = vec3_sum(p_transform.origin);
+	sum -= vec3_sum(p_transform.basis.elements[0]);
+	sum += vec3_sum(p_transform.basis.elements[1]);
+	sum -= vec3_sum(p_transform.basis.elements[2]);
+	return sum;
+}

core/math/interpolator.h

@@ -0,0 +1,49 @@
+/*************************************************************************/
+/*  interpolator.h                                                       */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef INTERPOLATOR_H
+#define INTERPOLATOR_H
+
+#include "core/math/math_defs.h"
+#include "core/math/vector3.h"
+
+// Keep all the functions for fixed timestep interpolation together
+
+class Transform;
+
+class Interpolator {
+	static real_t vec3_sum(const Vector3 &p_pt) { return p_pt.x + p_pt.y + p_pt.z; }
+
+public:
+	static void interpolate_transform_linear(const Transform &p_prev, const Transform &p_curr, Transform &r_result, real_t p_fraction);
+	static real_t checksum_transform(const Transform &p_transform);
+};
+
+#endif // INTERPOLATOR_H

doc/classes/Node.xml

@@ -786,6 +786,8 @@
 		<constant name="NOTIFICATION_POST_ENTER_TREE" value="27">
 			Notification received when the node is ready, just before [constant NOTIFICATION_READY] is received. Unlike the latter, it's sent every time the node enters tree, instead of only once.
 		</constant>
+		<constant name="NOTIFICATION_TELEPORT" value="28">
+		</constant>
 		<constant name="NOTIFICATION_WM_MOUSE_ENTER" value="1002">
 			Notification received from the OS when the mouse enters the game window.
 			Implemented on desktop and web platforms.

doc/classes/ProjectSettings.xml

@@ -1107,6 +1107,8 @@
 			The number of fixed iterations per second. This controls how often physics simulation and [method Node._physics_process] methods are run.
 			[b]Note:[/b] This property is only read when the project starts. To change the physics FPS at runtime, set [member Engine.iterations_per_second] instead.
 		</member>
+		<member name="physics/common/physics_interpolation" type="bool" setter="" getter="" default="true">
+		</member>
 		<member name="physics/common/physics_jitter_fix" type="float" setter="" getter="" default="0.5">
 			Controls how much physics ticks are synchronized with real time. For 0 or less, the ticks are synchronized. Such values are recommended for network games, where clock synchronization matters. Higher values cause higher deviation of in-game clock and real clock, but allows smoothing out framerate jitters. The default value of 0.5 should be fine for most; values above 2 could cause the game to react to dropped frames with a noticeable delay and are not recommended.
 			[b]Note:[/b] For best results, when using a custom physics interpolation solution, the physics jitter fix should be disabled by setting [member physics/common/physics_jitter_fix] to [code]0[/code].

main/main.cpp

@@ -1186,6 +1186,7 @@ Error Main::setup(const char *execpath, int argc, char *argv[], bool p_second_ph
 
 	Engine::get_singleton()->set_iterations_per_second(GLOBAL_DEF("physics/common/physics_fps", 60));
 	ProjectSettings::get_singleton()->set_custom_property_info("physics/common/physics_fps", PropertyInfo(Variant::INT, "physics/common/physics_fps", PROPERTY_HINT_RANGE, "1,1000,1"));
+	Engine::get_singleton()->set_physics_interpolation_enabled(GLOBAL_DEF("physics/common/physics_interpolation", true));
 	Engine::get_singleton()->set_physics_jitter_fix(GLOBAL_DEF("physics/common/physics_jitter_fix", 0.5));
 	Engine::get_singleton()->set_target_fps(GLOBAL_DEF("debug/settings/fps/force_fps", 0));
 	ProjectSettings::get_singleton()->set_custom_property_info("debug/settings/fps/force_fps", PropertyInfo(Variant::INT, "debug/settings/fps/force_fps", PROPERTY_HINT_RANGE, "0,1000,1"));
@@ -2140,6 +2141,8 @@ bool Main::iteration() {
 	bool exit = false;
 
 	for (int iters = 0; iters < advance.physics_steps; ++iters) {
+		VisualServer::get_singleton()->tick();
+
 		if (InputDefault::get_singleton()->is_using_input_buffering() && agile_input_event_flushing) {
 			InputDefault::get_singleton()->flush_buffered_events();
 		}
@@ -2199,6 +2202,8 @@ bool Main::iteration() {
 			Engine::get_singleton()->frames_drawn++;
 			force_redraw_requested = false;
 		}
+	} else {
+		VisualServer::get_singleton()->no_draw();
 	}
 
 #ifndef TOOLS_ENABLED

main/main_timer_sync.cpp

@@ -291,6 +291,17 @@ int64_t MainTimerSync::DeltaSmoother::smooth_delta(int64_t p_delta) {
 // before advance_core considers changing the physics_steps return from
 // the typical values as defined by typical_physics_steps
 float MainTimerSync::get_physics_jitter_fix() {
+	// Turn off jitter fix when using fixed timestep interpolation
+	// Note this shouldn't be on UNTIL 2d interpolation is implemented,
+	// otherwise we will get people making 2d games with the physics_interpolation
+	// set to on getting jitter fix disabled unexpectedly.
+#if 0
+	if (Engine::get_singleton()->is_physics_interpolation_enabled()) {
+		// would be better to write a simple bypass for jitter fix but this will do to get started
+		return 0.0;
+	}
+#endif
+
 	return Engine::get_singleton()->get_physics_jitter_fix();
 }
 

scene/3d/camera.cpp

@@ -98,6 +98,10 @@ void Camera::_update_camera() {
 	}
 }
 
+void Camera::_physics_interpolated_changed() {
+	VisualServer::get_singleton()->camera_set_interpolated(camera, is_physics_interpolated());
+}
+
 void Camera::_notification(int p_what) {
 	switch (p_what) {
 		case NOTIFICATION_ENTER_WORLD: {
@@ -119,6 +123,11 @@ void Camera::_notification(int p_what) {
 				velocity_tracker->update_position(get_global_transform().origin);
 			}
 		} break;
+		case NOTIFICATION_TELEPORT: {
+			if (is_physics_interpolated()) {
+				VisualServer::get_singleton()->camera_teleport(camera);
+			}
+		} break;
 		case NOTIFICATION_EXIT_WORLD: {
 			if (!get_tree()->is_node_being_edited(this)) {
 				if (is_current()) {
@@ -666,6 +675,12 @@ Camera::Camera() {
 	doppler_tracking = DOPPLER_TRACKING_DISABLED;
 	set_notify_transform(true);
 	set_disable_scale(true);
+	_set_branch_physics_interpolated(true);
+
+	// most nodes will teleport when calling set_transform,
+	// but cameras are different.
+	// (note: this logic is way overcomplicated imo).
+	_set_physics_teleport_on_transform(false);
 }
 
 Camera::~Camera() {

scene/3d/camera.h

@@ -94,6 +94,8 @@ class Camera : public Spatial {
 	virtual void _request_camera_update();
 	void _update_camera_mode();
 
+	virtual void _physics_interpolated_changed();
+
 	void _notification(int p_what);
 	virtual void _validate_property(PropertyInfo &p_property) const;
 

scene/3d/physics_body.cpp

@@ -357,7 +357,7 @@ void RigidBody::_direct_state_changed(Object *p_state) {
 	ERR_FAIL_COND_MSG(!state, "Method '_direct_state_changed' must receive a valid PhysicsDirectBodyState object as argument");
 
 	set_ignore_transform_notification(true);
-	set_global_transform(state->get_transform());
+	_physics_set_global_transform(state->get_transform());
 	linear_velocity = state->get_linear_velocity();
 	angular_velocity = state->get_angular_velocity();
 	inverse_inertia_tensor = state->get_inverse_inertia_tensor();
@@ -1042,7 +1042,7 @@ bool KinematicBody::move_and_collide(const Vector3 &p_motion, bool p_infinite_in
 
 	if (!p_test_only) {
 		gt.origin += result.motion;
-		set_global_transform(gt);
+		_physics_set_global_transform(gt);
 	}
 
 	return colliding;
@@ -1134,7 +1134,7 @@ Vector3 KinematicBody::_move_and_slide_internal(const Vector3 &p_linear_velocity
 						} else {
 							gt.origin -= collision.travel;
 						}
-						set_global_transform(gt);
+						_physics_set_global_transform(gt);
 						return Vector3();
 					}
 				}
@@ -1189,7 +1189,7 @@ Vector3 KinematicBody::_move_and_slide_internal(const Vector3 &p_linear_velocity
 			}
 			if (apply) {
 				gt.origin += col.travel;
-				set_global_transform(gt);
+				_physics_set_global_transform(gt);
 			}
 		}
 	}
@@ -1275,7 +1275,7 @@ bool KinematicBody::separate_raycast_shapes(bool p_infinite_inertia, Collision &
 	}
 
 	gt.origin += recover;
-	set_global_transform(gt);
+	_physics_set_global_transform(gt);
 
 	if (deepest != -1) {
 		r_collision.collider = sep_res[deepest].collider_id;
@@ -1383,7 +1383,7 @@ void KinematicBody::_direct_state_changed(Object *p_state) {
 
 	last_valid_transform = state->get_transform();
 	set_notify_local_transform(false);
-	set_global_transform(last_valid_transform);
+	_physics_set_global_transform(last_valid_transform);
 	set_notify_local_transform(true);
 	_on_transform_changed();
 }
@@ -1407,7 +1407,7 @@ void KinematicBody::_notification(int p_what) {
 		PhysicsServer::get_singleton()->body_set_state(get_rid(), PhysicsServer::BODY_STATE_TRANSFORM, new_transform);
 		//but then revert changes
 		set_notify_local_transform(false);
-		set_global_transform(last_valid_transform);
+		_physics_set_global_transform(last_valid_transform);
 		set_notify_local_transform(true);
 		_on_transform_changed();
 	}

scene/3d/spatial.cpp

@@ -242,7 +242,27 @@ void Spatial::_notification(int p_what) {
 	}
 }
 
+void Spatial::_physics_set_transform(const Transform &p_transform) {
+	_set_branch_physics_interpolated(true);
+	_set_transform(p_transform);
+}
+
+void Spatial::_physics_set_global_transform(const Transform &p_transform) {
+	_set_branch_physics_interpolated(true);
+	_set_global_transform(p_transform);
+}
+
 void Spatial::set_transform(const Transform &p_transform) {
+	_set_transform(p_transform);
+	_teleport();
+}
+
+void Spatial::set_global_transform(const Transform &p_transform) {
+	_set_global_transform(p_transform);
+	_teleport();
+}
+
+void Spatial::_set_transform(const Transform &p_transform) {
 	data.local_transform = p_transform;
 	data.dirty |= DIRTY_VECTORS;
 	_change_notify("translation");
@@ -255,10 +275,9 @@ void Spatial::set_transform(const Transform &p_transform) {
 	}
 }
 
-void Spatial::set_global_transform(const Transform &p_transform) {
+void Spatial::_set_global_transform(const Transform &p_transform) {
 	Transform xform = (data.parent && !data.toplevel_active) ? data.parent->get_global_transform().affine_inverse() * p_transform : p_transform;
-
-	set_transform(xform);
+	_set_transform(xform);
 }
 
 Transform Spatial::get_transform() const {

scene/3d/spatial.h

@@ -128,6 +128,11 @@ class Spatial : public Node {
 		}
 	}
 
+	void _physics_set_transform(const Transform &p_transform);
+	void _physics_set_global_transform(const Transform &p_transform);
+	void _set_transform(const Transform &p_transform);
+	void _set_global_transform(const Transform &p_transform);
+
 	void _notification(int p_what);
 	static void _bind_methods();
 

scene/3d/vehicle_body.cpp

@@ -833,7 +833,7 @@ void VehicleBody::_direct_state_changed(Object *p_state) {
 
 	for (int i = 0; i < wheels.size(); i++) {
 		_ray_cast(i, state);
-		wheels[i]->set_transform(state->get_transform().inverse() * wheels[i]->m_worldTransform);
+		wheels[i]->_physics_set_transform(state->get_transform().inverse() * wheels[i]->m_worldTransform);
 	}
 
 	_update_suspension(state);