3D physics interpolation for Godot with GDExtension C++

physics_interpolation_3d.cpp

#include "physics_interpolation_3d.h"
#include <godot_cpp/core/class_db.hpp>
#include <godot_cpp/variant/utility_functions.hpp>

using namespace godot;


void PhysicsInterpolation3D::_bind_methods() {
	ClassDB::bind_method(D_METHOD("_update_xform_cache"), &PhysicsInterpolation3D::_update_xform_cache);
	ClassDB::bind_method(D_METHOD("set_interp_mode", "mode"), &PhysicsInterpolation3D::set_interp_mode);
	ClassDB::bind_method(D_METHOD("get_interp_mode"), &PhysicsInterpolation3D::get_interp_mode);
	ClassDB::bind_method(D_METHOD("set_target", "path"), &PhysicsInterpolation3D::set_target);
	ClassDB::bind_method(D_METHOD("get_target"), &PhysicsInterpolation3D::get_target);

	ADD_PROPERTY(PropertyInfo(Variant::INT, "interp_mode", PROPERTY_HINT_ENUM, "Transform,Camera", PROPERTY_USAGE_DEFAULT | PROPERTY_USAGE_UPDATE_ALL_IF_MODIFIED), "set_interp_mode", "get_interp_mode");
	ADD_PROPERTY(PropertyInfo(Variant::NODE_PATH, "path", PROPERTY_HINT_NODE_PATH_VALID_TYPES, "Node3D"), "set_target", "get_target");
	
	BIND_ENUM_CONSTANT(TRANSFORM);
	BIND_ENUM_CONSTANT(CAMERA);
}

// ================================
// Initialization / Destruction
// ================================

PhysicsInterpolation3D::PhysicsInterpolation3D() {
	set_process_priority(127);
	prev = get_transform();
	curr = get_transform();
}
PhysicsInterpolation3D::~PhysicsInterpolation3D() {}


// ================================
// Engine Callbacks
// ================================


void PhysicsInterpolation3D::_process(double delta) {
	if (Engine::get_singleton()->is_editor_hint() || !is_inside_tree() || cache.is_null()) {
		return;
	}
	_interpolate();
}

void PhysicsInterpolation3D::_physics_process(double delta) {
	if (Engine::get_singleton()->is_editor_hint() || !is_inside_tree() || cache.is_null()) {
		return;
	}
	_update_xform_cache();
	return;
}


void PhysicsInterpolation3D::_notification(int p_what) {
	switch (p_what) {
		case NOTIFICATION_ENTER_TREE: {
			_update_cache();
			_update_xform_cache();
		} break;
	}
}


// ================================
// Internal
// ================================

void PhysicsInterpolation3D::_interpolate() {
	Engine *e = Engine::get_singleton();

	if (e->is_editor_hint()) { 
		return;
	}

	// Teleporting desired - do not interpolate for this frame alone
	if (skip) {
		skip = false;
		set_global_transform(curr);
		return;

	// Otherwise if render framerate exceedes physics framerate, interpolate
	} else if (e->get_frames_per_second() >= e->get_physics_ticks_per_second()) {
		real_t f = CLAMP(e->get_physics_interpolation_fraction(), 0.0, 1.0);

		// Player cameras are special and only interpolate position.
		if (mode == CAMERA) {
			Node3D *n = Object::cast_to<Node3D>(ObjectDB::get_instance(cache));

			// I really hope this doesn't happen lol
			if (!n || !n->is_inside_tree()) {
				UtilityFunctions::print("Something got completely fucked up when interepolating, fix it fix it fix it fix it");
				return;
			}
			set_global_position(prev.origin.lerp(curr.origin, f));
			set_global_rotation(n->get_global_rotation());
			return;
		}

		// Otherwise just interpolate the whole transform.
		set_global_transform(prev.interpolate_with(curr, f));
		return;
		}

	// Just use the physics transform otherwise
	set_global_transform(curr);
	return;
	}


void PhysicsInterpolation3D::_update_xform_cache() {
	Node3D *n = Object::cast_to<Node3D>(ObjectDB::get_instance(cache));
	if (!n || !n->is_inside_tree()) {
		return;
		}

	prev = curr;
	curr = n->get_global_transform();
}


void PhysicsInterpolation3D::_update_cache() {
	cache = ObjectID();
	set_as_top_level(false);

	if (has_node(target)) {
		Node3D *node = get_node<Node3D>(target);
		if (!node || this == node) { 
			return;
		}

		cache = node->get_instance_id();
		UtilityFunctions::print("Cached: ", node->get_name());
		set_as_top_level(true);
	}
}

// ================================
// Interface
// ================================

void PhysicsInterpolation3D::skip_next_frame() {
	skip = true;
	return;
}

void PhysicsInterpolation3D::set_interp_mode(const INTERP_MODE p_mode) {
	mode = p_mode;
	return;
}

PhysicsInterpolation3D::INTERP_MODE PhysicsInterpolation3D::get_interp_mode() const {
	return mode;
}

void PhysicsInterpolation3D::set_target(const NodePath p_remote_target) {
	target = p_remote_target;
	if (is_inside_tree()) {
		_update_cache();
	}
	return;
}

NodePath PhysicsInterpolation3D::get_target() const {
	return target;
}

VARIANT_ENUM_CAST(PhysicsInterpolation3D::INTERP_MODE);

physics_interpolation_3d.h

#ifndef PHYSICS_INTERPOLATION_3D_H
#define PHYSICS_INTERPOLATION_3D_H

#include <godot_cpp/classes/object.hpp>
#include <godot_cpp/classes/engine.hpp>
#include <godot_cpp/variant/node_path.hpp>
#include <godot_cpp/classes/node3d.hpp>

namespace godot {

	class PhysicsInterpolation3D : public Node3D {
		GDCLASS(PhysicsInterpolation3D, Node3D);

	public:
		enum INTERP_MODE {
			TRANSFORM,
			CAMERA
		};

		void _process(double delta) override;
		void _physics_process(double delta) override;

		void _interpolate();
		void _update_xform_cache();

		void skip_next_frame();

		void set_interp_mode(const INTERP_MODE p_mode);
		INTERP_MODE get_interp_mode() const;

		void set_target(const NodePath p_target);
		NodePath get_target() const;

		PhysicsInterpolation3D();
		~PhysicsInterpolation3D();


	protected:
		static void _bind_methods();
		void _notification(int p_what);

	private:
		bool skip = false;

		INTERP_MODE mode = TRANSFORM;

		NodePath target = "";
		ObjectID cache = ObjectID();

		Transform3D curr;
		Transform3D prev;

		void _update_cache();
	};

} //namespace godot

#endif

A work in progress.