initial commit, 4.5 stable

modules/jolt_physics/spaces/jolt_body_activation_listener_3d.cpp

@@ -0,0 +1,41 @@
+/**************************************************************************/
+/*  jolt_body_activation_listener_3d.cpp                                  */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* 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 "jolt_body_activation_listener_3d.h"
+
+#include "../objects/jolt_body_3d.h"
+
+void JoltBodyActivationListener3D::OnBodyActivated(const JPH::BodyID &p_body_id, JPH::uint64 p_body_user_data) {
+	// This method will be called on multiple threads during the simulation step.
+
+	if (JoltBody3D *body = reinterpret_cast<JoltObject3D *>(p_body_user_data)->as_body()) {
+		body->_on_wake_up();
+	}
+}

modules/jolt_physics/spaces/jolt_body_activation_listener_3d.h

@@ -0,0 +1,42 @@
+/**************************************************************************/
+/*  jolt_body_activation_listener_3d.h                                    */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* 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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Body/BodyActivationListener.h"
+
+class JoltBodyActivationListener3D final
+		: public JPH::BodyActivationListener {
+public:
+	virtual void OnBodyActivated(const JPH::BodyID &p_body_id, JPH::uint64 p_body_user_data) override;
+	virtual void OnBodyDeactivated(const JPH::BodyID &p_body_id, JPH::uint64 p_body_user_data) override {}
+};

modules/jolt_physics/spaces/jolt_broad_phase_layer.h

@@ -0,0 +1,49 @@
+/**************************************************************************/
+/*  jolt_broad_phase_layer.h                                              */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* 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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h"
+
+#include <cstdint>
+
+namespace JoltBroadPhaseLayer {
+
+constexpr JPH::BroadPhaseLayer BODY_STATIC(0);
+constexpr JPH::BroadPhaseLayer BODY_STATIC_BIG(1);
+constexpr JPH::BroadPhaseLayer BODY_DYNAMIC(2);
+constexpr JPH::BroadPhaseLayer AREA_DETECTABLE(3);
+constexpr JPH::BroadPhaseLayer AREA_UNDETECTABLE(4);
+
+constexpr uint32_t COUNT = 5;
+
+} // namespace JoltBroadPhaseLayer

modules/jolt_physics/spaces/jolt_contact_listener_3d.cpp

@@ -0,0 +1,511 @@
+/**************************************************************************/
+/*  jolt_contact_listener_3d.cpp                                          */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* 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 "jolt_contact_listener_3d.h"
+
+#include "../jolt_project_settings.h"
+#include "../misc/jolt_type_conversions.h"
+#include "../objects/jolt_area_3d.h"
+#include "../objects/jolt_body_3d.h"
+#include "../objects/jolt_soft_body_3d.h"
+#include "jolt_space_3d.h"
+
+#include "Jolt/Physics/Collision/EstimateCollisionResponse.h"
+#include "Jolt/Physics/SoftBody/SoftBodyManifold.h"
+
+void JoltContactListener3D::OnContactAdded(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold, JPH::ContactSettings &p_settings) {
+	_try_override_collision_response(p_body1, p_body2, p_settings);
+	_try_apply_surface_velocities(p_body1, p_body2, p_settings);
+	_try_add_contacts(p_body1, p_body2, p_manifold, p_settings);
+	_try_evaluate_area_overlap(p_body1, p_body2, p_manifold);
+
+#ifdef DEBUG_ENABLED
+	_try_add_debug_contacts(p_body1, p_body2, p_manifold);
+#endif
+}
+
+void JoltContactListener3D::OnContactPersisted(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold, JPH::ContactSettings &p_settings) {
+	_try_override_collision_response(p_body1, p_body2, p_settings);
+	_try_apply_surface_velocities(p_body1, p_body2, p_settings);
+	_try_add_contacts(p_body1, p_body2, p_manifold, p_settings);
+	_try_evaluate_area_overlap(p_body1, p_body2, p_manifold);
+
+#ifdef DEBUG_ENABLED
+	_try_add_debug_contacts(p_body1, p_body2, p_manifold);
+#endif
+}
+
+void JoltContactListener3D::OnContactRemoved(const JPH::SubShapeIDPair &p_shape_pair) {
+	if (_try_remove_contacts(p_shape_pair)) {
+		return;
+	}
+
+	if (_try_remove_area_overlap(p_shape_pair)) {
+		return;
+	}
+}
+
+JPH::SoftBodyValidateResult JoltContactListener3D::OnSoftBodyContactValidate(const JPH::Body &p_soft_body, const JPH::Body &p_other_body, JPH::SoftBodyContactSettings &p_settings) {
+	_try_override_collision_response(p_soft_body, p_other_body, p_settings);
+
+	return JPH::SoftBodyValidateResult::AcceptContact;
+}
+
+#ifdef DEBUG_ENABLED
+
+void JoltContactListener3D::OnSoftBodyContactAdded(const JPH::Body &p_soft_body, const JPH::SoftBodyManifold &p_manifold) {
+	_try_add_debug_contacts(p_soft_body, p_manifold);
+}
+
+#endif
+
+bool JoltContactListener3D::_try_override_collision_response(const JPH::Body &p_jolt_body1, const JPH::Body &p_jolt_body2, JPH::ContactSettings &p_settings) {
+	if (p_jolt_body1.IsSensor() || p_jolt_body2.IsSensor()) {
+		return false;
+	}
+
+	if (!p_jolt_body1.IsDynamic() && !p_jolt_body2.IsDynamic()) {
+		return false;
+	}
+
+	const JoltBody3D *body1 = reinterpret_cast<JoltBody3D *>(p_jolt_body1.GetUserData());
+	const JoltBody3D *body2 = reinterpret_cast<JoltBody3D *>(p_jolt_body2.GetUserData());
+
+	const bool can_collide1 = body1->can_collide_with(*body2);
+	const bool can_collide2 = body2->can_collide_with(*body1);
+
+	if (can_collide1 && !can_collide2) {
+		p_settings.mInvMassScale2 = 0.0f;
+		p_settings.mInvInertiaScale2 = 0.0f;
+	} else if (can_collide2 && !can_collide1) {
+		p_settings.mInvMassScale1 = 0.0f;
+		p_settings.mInvInertiaScale1 = 0.0f;
+	}
+
+	return true;
+}
+
+bool JoltContactListener3D::_try_override_collision_response(const JPH::Body &p_jolt_soft_body, const JPH::Body &p_jolt_other_body, JPH::SoftBodyContactSettings &p_settings) {
+	if (p_jolt_other_body.IsSensor()) {
+		return false;
+	}
+
+	const JoltSoftBody3D *soft_body = reinterpret_cast<JoltSoftBody3D *>(p_jolt_soft_body.GetUserData());
+	const JoltBody3D *other_body = reinterpret_cast<JoltBody3D *>(p_jolt_other_body.GetUserData());
+
+	const bool can_collide1 = soft_body->can_collide_with(*other_body);
+	const bool can_collide2 = other_body->can_collide_with(*soft_body);
+
+	if (can_collide1 && !can_collide2) {
+		p_settings.mInvMassScale2 = 0.0f;
+		p_settings.mInvInertiaScale2 = 0.0f;
+	} else if (can_collide2 && !can_collide1) {
+		p_settings.mInvMassScale1 = 0.0f;
+	}
+
+	return true;
+}
+
+bool JoltContactListener3D::_try_apply_surface_velocities(const JPH::Body &p_jolt_body1, const JPH::Body &p_jolt_body2, JPH::ContactSettings &p_settings) {
+	if (p_jolt_body1.IsSensor() || p_jolt_body2.IsSensor()) {
+		return false;
+	}
+
+	const bool supports_surface_velocity1 = !p_jolt_body1.IsDynamic();
+	const bool supports_surface_velocity2 = !p_jolt_body2.IsDynamic();
+
+	if (supports_surface_velocity1 == supports_surface_velocity2) {
+		return false;
+	}
+
+	const JoltBody3D *body1 = reinterpret_cast<JoltBody3D *>(p_jolt_body1.GetUserData());
+	const JoltBody3D *body2 = reinterpret_cast<JoltBody3D *>(p_jolt_body2.GetUserData());
+
+	const bool has_surface_velocity1 = supports_surface_velocity1 && (body1->get_linear_surface_velocity() != Vector3() || body1->get_angular_surface_velocity() != Vector3());
+	const bool has_surface_velocity2 = supports_surface_velocity2 && (body2->get_linear_surface_velocity() != Vector3() || body2->get_angular_surface_velocity() != Vector3());
+
+	if (has_surface_velocity1 == has_surface_velocity2) {
+		return false;
+	}
+
+	const JPH::Vec3 linear_velocity1 = to_jolt(body1->get_linear_surface_velocity());
+	const JPH::Vec3 angular_velocity1 = to_jolt(body1->get_angular_surface_velocity());
+
+	const JPH::Vec3 linear_velocity2 = to_jolt(body2->get_linear_surface_velocity());
+	const JPH::Vec3 angular_velocity2 = to_jolt(body2->get_angular_surface_velocity());
+
+	const JPH::RVec3 com1 = p_jolt_body1.GetCenterOfMassPosition();
+	const JPH::RVec3 com2 = p_jolt_body2.GetCenterOfMassPosition();
+	const JPH::Vec3 rel_com2 = JPH::Vec3(com2 - com1);
+
+	const JPH::Vec3 angular_linear_velocity2 = rel_com2.Cross(angular_velocity2);
+	const JPH::Vec3 total_linear_velocity2 = linear_velocity2 + angular_linear_velocity2;
+
+	p_settings.mRelativeLinearSurfaceVelocity = total_linear_velocity2 - linear_velocity1;
+	p_settings.mRelativeAngularSurfaceVelocity = angular_velocity2 - angular_velocity1;
+
+	return true;
+}
+
+bool JoltContactListener3D::_try_add_contacts(const JPH::Body &p_jolt_body1, const JPH::Body &p_jolt_body2, const JPH::ContactManifold &p_manifold, JPH::ContactSettings &p_settings) {
+	if (p_jolt_body1.IsSensor() || p_jolt_body2.IsSensor()) {
+		return false;
+	}
+
+	const JoltBody3D *body1 = reinterpret_cast<JoltBody3D *>(p_jolt_body1.GetUserData());
+	const JoltBody3D *body2 = reinterpret_cast<JoltBody3D *>(p_jolt_body2.GetUserData());
+
+	if (!body1->reports_contacts() && !body2->reports_contacts()) {
+		return false;
+	}
+
+	const JPH::SubShapeIDPair shape_pair(p_jolt_body1.GetID(), p_manifold.mSubShapeID1, p_jolt_body2.GetID(), p_manifold.mSubShapeID2);
+
+	Manifold &manifold = [&]() -> Manifold & {
+		const MutexLock write_lock(write_mutex);
+		return manifolds_by_shape_pair[shape_pair];
+	}();
+
+	const JPH::uint contact_count = p_manifold.mRelativeContactPointsOn1.size();
+
+	manifold.contacts1.reserve((uint32_t)contact_count);
+	manifold.contacts2.reserve((uint32_t)contact_count);
+	manifold.depth = p_manifold.mPenetrationDepth;
+
+	JPH::CollisionEstimationResult collision;
+	JPH::EstimateCollisionResponse(p_jolt_body1, p_jolt_body2, p_manifold, collision, p_settings.mCombinedFriction, p_settings.mCombinedRestitution, JoltProjectSettings::bounce_velocity_threshold, 5);
+
+	for (JPH::uint i = 0; i < contact_count; ++i) {
+		const JPH::RVec3 relative_point1 = JPH::RVec3(p_manifold.mRelativeContactPointsOn1[i]);
+		const JPH::RVec3 relative_point2 = JPH::RVec3(p_manifold.mRelativeContactPointsOn2[i]);
+
+		const JPH::RVec3 world_point1 = p_manifold.mBaseOffset + relative_point1;
+		const JPH::RVec3 world_point2 = p_manifold.mBaseOffset + relative_point2;
+
+		const JPH::Vec3 velocity1 = p_jolt_body1.GetPointVelocity(world_point1);
+		const JPH::Vec3 velocity2 = p_jolt_body2.GetPointVelocity(world_point2);
+
+		const JPH::CollisionEstimationResult::Impulse &impulse = collision.mImpulses[i];
+
+		const JPH::Vec3 contact_impulse = p_manifold.mWorldSpaceNormal * impulse.mContactImpulse;
+		const JPH::Vec3 friction_impulse1 = collision.mTangent1 * impulse.mFrictionImpulse1;
+		const JPH::Vec3 friction_impulse2 = collision.mTangent2 * impulse.mFrictionImpulse2;
+		const JPH::Vec3 combined_impulse = contact_impulse + friction_impulse1 + friction_impulse2;
+
+		Contact contact1;
+		contact1.point_self = to_godot(world_point1);
+		contact1.point_other = to_godot(world_point2);
+		contact1.normal = to_godot(-p_manifold.mWorldSpaceNormal);
+		contact1.velocity_self = to_godot(velocity1);
+		contact1.velocity_other = to_godot(velocity2);
+		contact1.impulse = to_godot(-combined_impulse);
+		manifold.contacts1.push_back(contact1);
+
+		Contact contact2;
+		contact2.point_self = to_godot(world_point2);
+		contact2.point_other = to_godot(world_point1);
+		contact2.normal = to_godot(p_manifold.mWorldSpaceNormal);
+		contact2.velocity_self = to_godot(velocity2);
+		contact2.velocity_other = to_godot(velocity1);
+		contact2.impulse = to_godot(combined_impulse);
+		manifold.contacts2.push_back(contact2);
+	}
+
+	return true;
+}
+
+bool JoltContactListener3D::_try_evaluate_area_overlap(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold) {
+	if (!p_body1.IsSensor() && !p_body2.IsSensor()) {
+		return false;
+	}
+
+	auto has_shifted = [](const JoltShapedObject3D &p_object, const JPH::SubShapeID &p_sub_shape_id) {
+		return p_object.get_previous_jolt_shape() != nullptr && p_object.get_jolt_shape()->GetSubShapeUserData(p_sub_shape_id) != p_object.get_previous_jolt_shape()->GetSubShapeUserData(p_sub_shape_id);
+	};
+
+	auto evaluate = [&](const JoltArea3D &p_area, const auto &p_object, const JPH::SubShapeIDPair &p_shape_pair) {
+		const MutexLock write_lock(write_mutex);
+
+		if (p_area.can_monitor(p_object)) {
+			if (!area_overlaps.has(p_shape_pair)) {
+				area_overlaps.insert(p_shape_pair);
+				area_enters.insert(p_shape_pair);
+			} else if (has_shifted(p_area, p_shape_pair.GetSubShapeID1()) || has_shifted(p_object, p_shape_pair.GetSubShapeID2())) {
+				// A shape has taken on the `JPH::SubShapeID` value of another shape, likely because of the other shape having been replaced or moved
+				// in some way, so we force the area to refresh its internal mappings by exiting and entering this shape pair.
+				area_exits.insert(p_shape_pair);
+				area_enters.insert(p_shape_pair);
+			}
+		} else {
+			if (area_overlaps.erase(p_shape_pair)) {
+				area_exits.insert(p_shape_pair);
+			}
+		}
+	};
+
+	const JPH::SubShapeIDPair shape_pair1(p_body1.GetID(), p_manifold.mSubShapeID1, p_body2.GetID(), p_manifold.mSubShapeID2);
+	const JPH::SubShapeIDPair shape_pair2(p_body2.GetID(), p_manifold.mSubShapeID2, p_body1.GetID(), p_manifold.mSubShapeID1);
+
+	const JoltObject3D *object1 = reinterpret_cast<JoltObject3D *>(p_body1.GetUserData());
+	const JoltObject3D *object2 = reinterpret_cast<JoltObject3D *>(p_body2.GetUserData());
+
+	const JoltArea3D *area1 = object1->as_area();
+	const JoltArea3D *area2 = object2->as_area();
+
+	const JoltBody3D *body1 = object1->as_body();
+	const JoltBody3D *body2 = object2->as_body();
+
+	if (area1 != nullptr && area2 != nullptr) {
+		evaluate(*area1, *area2, shape_pair1);
+		evaluate(*area2, *area1, shape_pair2);
+	} else if (area1 != nullptr && body2 != nullptr) {
+		evaluate(*area1, *body2, shape_pair1);
+	} else if (area2 != nullptr && body1 != nullptr) {
+		evaluate(*area2, *body1, shape_pair2);
+	}
+
+	return true;
+}
+
+bool JoltContactListener3D::_try_remove_contacts(const JPH::SubShapeIDPair &p_shape_pair) {
+	const MutexLock write_lock(write_mutex);
+
+	return manifolds_by_shape_pair.erase(p_shape_pair);
+}
+
+bool JoltContactListener3D::_try_remove_area_overlap(const JPH::SubShapeIDPair &p_shape_pair) {
+	const JPH::SubShapeIDPair swapped_shape_pair(p_shape_pair.GetBody2ID(), p_shape_pair.GetSubShapeID2(), p_shape_pair.GetBody1ID(), p_shape_pair.GetSubShapeID1());
+
+	const MutexLock write_lock(write_mutex);
+
+	bool removed = false;
+
+	if (area_overlaps.erase(p_shape_pair)) {
+		area_exits.insert(p_shape_pair);
+		removed = true;
+	}
+
+	if (area_overlaps.erase(swapped_shape_pair)) {
+		area_exits.insert(swapped_shape_pair);
+		removed = true;
+	}
+
+	return removed;
+}
+
+#ifdef DEBUG_ENABLED
+
+bool JoltContactListener3D::_try_add_debug_contacts(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold) {
+	if (p_body1.IsSensor() || p_body2.IsSensor()) {
+		return false;
+	}
+
+	const int64_t max_count = debug_contacts.size();
+
+	if (max_count == 0) {
+		return false;
+	}
+
+	const int additional_pairs = (int)p_manifold.mRelativeContactPointsOn1.size();
+	const int additional_contacts = additional_pairs * 2;
+
+	int current_count = debug_contact_count.load(std::memory_order_relaxed);
+	bool exchanged = false;
+
+	do {
+		const int new_count = current_count + additional_contacts;
+
+		if (new_count > max_count) {
+			return false;
+		}
+
+		exchanged = debug_contact_count.compare_exchange_weak(current_count, new_count, std::memory_order_release, std::memory_order_relaxed);
+	} while (!exchanged);
+
+	for (int i = 0; i < additional_pairs; ++i) {
+		const int pair_index = current_count + i * 2;
+
+		const JPH::RVec3 point_on_1 = p_manifold.GetWorldSpaceContactPointOn1((JPH::uint)i);
+		const JPH::RVec3 point_on_2 = p_manifold.GetWorldSpaceContactPointOn2((JPH::uint)i);
+
+		debug_contacts.write[pair_index + 0] = to_godot(point_on_1);
+		debug_contacts.write[pair_index + 1] = to_godot(point_on_2);
+	}
+
+	return true;
+}
+
+bool JoltContactListener3D::_try_add_debug_contacts(const JPH::Body &p_soft_body, const JPH::SoftBodyManifold &p_manifold) {
+	const int64_t max_count = debug_contacts.size();
+	if (max_count == 0) {
+		return false;
+	}
+
+	int additional_contacts = 0;
+
+	for (const JPH::SoftBodyVertex &vertex : p_manifold.GetVertices()) {
+		if (p_manifold.HasContact(vertex)) {
+			additional_contacts += 1;
+		}
+	}
+
+	int current_count = debug_contact_count.load(std::memory_order_relaxed);
+	bool exchanged = false;
+
+	do {
+		const int new_count = current_count + additional_contacts;
+
+		if (new_count > max_count) {
+			return false;
+		}
+
+		exchanged = debug_contact_count.compare_exchange_weak(current_count, new_count, std::memory_order_release, std::memory_order_relaxed);
+	} while (!exchanged);
+
+	int contact_index = current_count;
+
+	for (const JPH::SoftBodyVertex &vertex : p_manifold.GetVertices()) {
+		if (!p_manifold.HasContact(vertex)) {
+			continue;
+		}
+
+		const JPH::RMat44 body_com_transform = p_soft_body.GetCenterOfMassTransform();
+		const JPH::Vec3 local_contact_point = p_manifold.GetLocalContactPoint(vertex);
+		const JPH::RVec3 contact_point = body_com_transform * local_contact_point;
+
+		debug_contacts.write[contact_index++] = to_godot(contact_point);
+	}
+
+	return true;
+}
+
+#endif
+
+void JoltContactListener3D::_flush_contacts() {
+	for (KeyValue<JPH::SubShapeIDPair, Manifold> &E : manifolds_by_shape_pair) {
+		const JPH::SubShapeIDPair &shape_pair = E.key;
+		Manifold &manifold = E.value;
+
+		JoltBody3D *body1 = space->try_get_body(shape_pair.GetBody1ID());
+		ERR_FAIL_NULL(body1);
+
+		JoltBody3D *body2 = space->try_get_body(shape_pair.GetBody2ID());
+		ERR_FAIL_NULL(body2);
+
+		const int shape_index1 = body1->find_shape_index(shape_pair.GetSubShapeID1());
+		const int shape_index2 = body2->find_shape_index(shape_pair.GetSubShapeID2());
+
+		for (const Contact &contact : manifold.contacts1) {
+			body1->add_contact(body2, manifold.depth, shape_index1, shape_index2, contact.normal, contact.point_self, contact.point_other, contact.velocity_self, contact.velocity_other, contact.impulse);
+		}
+
+		for (const Contact &contact : manifold.contacts2) {
+			body2->add_contact(body1, manifold.depth, shape_index2, shape_index1, contact.normal, contact.point_self, contact.point_other, contact.velocity_self, contact.velocity_other, contact.impulse);
+		}
+
+		manifold.contacts1.clear();
+		manifold.contacts2.clear();
+	}
+}
+
+void JoltContactListener3D::_flush_area_enters() {
+	for (const JPH::SubShapeIDPair &shape_pair : area_enters) {
+		const JPH::BodyID &body_id1 = shape_pair.GetBody1ID();
+		const JPH::BodyID &body_id2 = shape_pair.GetBody2ID();
+
+		JoltObject3D *object1 = space->try_get_object(body_id1);
+		JoltObject3D *object2 = space->try_get_object(body_id2);
+
+		if (object1 == nullptr || object2 == nullptr) {
+			continue;
+		}
+
+		JoltArea3D *area1 = object1->as_area();
+		JoltArea3D *area2 = object2->as_area();
+
+		const JPH::SubShapeID &sub_shape_id1 = shape_pair.GetSubShapeID1();
+		const JPH::SubShapeID &sub_shape_id2 = shape_pair.GetSubShapeID2();
+
+		if (area1 != nullptr && area2 != nullptr) {
+			area1->area_shape_entered(body_id2, sub_shape_id2, sub_shape_id1);
+		} else if (area1 != nullptr && area2 == nullptr) {
+			area1->body_shape_entered(body_id2, sub_shape_id2, sub_shape_id1);
+		} else if (area1 == nullptr && area2 != nullptr) {
+			area2->body_shape_entered(body_id1, sub_shape_id1, sub_shape_id2);
+		}
+	}
+
+	area_enters.clear();
+}
+
+void JoltContactListener3D::_flush_area_exits() {
+	for (const JPH::SubShapeIDPair &shape_pair : area_exits) {
+		const JPH::BodyID &body_id1 = shape_pair.GetBody1ID();
+		const JPH::BodyID &body_id2 = shape_pair.GetBody2ID();
+
+		JoltObject3D *object1 = space->try_get_object(body_id1);
+		JoltObject3D *object2 = space->try_get_object(body_id2);
+
+		JoltArea3D *area1 = object1 != nullptr ? object1->as_area() : nullptr;
+		JoltArea3D *area2 = object2 != nullptr ? object2->as_area() : nullptr;
+
+		const JoltBody3D *body1 = object1 != nullptr ? object1->as_body() : nullptr;
+		const JoltBody3D *body2 = object2 != nullptr ? object2->as_body() : nullptr;
+
+		const JPH::SubShapeID &sub_shape_id1 = shape_pair.GetSubShapeID1();
+		const JPH::SubShapeID &sub_shape_id2 = shape_pair.GetSubShapeID2();
+
+		if (area1 != nullptr && area2 != nullptr) {
+			area1->area_shape_exited(body_id2, sub_shape_id2, sub_shape_id1);
+		} else if (area1 != nullptr && body2 != nullptr) {
+			area1->body_shape_exited(body_id2, sub_shape_id2, sub_shape_id1);
+		} else if (body1 != nullptr && area2 != nullptr) {
+			area2->body_shape_exited(body_id1, sub_shape_id1, sub_shape_id2);
+		} else if (area1 != nullptr) {
+			area1->shape_exited(body_id2, sub_shape_id2, sub_shape_id1);
+		} else if (area2 != nullptr) {
+			area2->shape_exited(body_id1, sub_shape_id1, sub_shape_id2);
+		}
+	}
+
+	area_exits.clear();
+}
+
+void JoltContactListener3D::pre_step() {
+#ifdef DEBUG_ENABLED
+	debug_contact_count = 0;
+#endif
+}
+
+void JoltContactListener3D::post_step() {
+	_flush_contacts();
+	_flush_area_exits();
+	_flush_area_enters();
+}

modules/jolt_physics/spaces/jolt_contact_listener_3d.h

@@ -0,0 +1,138 @@
+/**************************************************************************/
+/*  jolt_contact_listener_3d.h                                            */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* 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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/os/mutex.h"
+#include "core/templates/hash_map.h"
+#include "core/templates/hash_set.h"
+#include "core/templates/hashfuncs.h"
+#include "core/templates/local_vector.h"
+#include "core/templates/safe_refcount.h"
+#include "core/variant/variant.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Body/Body.h"
+#include "Jolt/Physics/Collision/ContactListener.h"
+#include "Jolt/Physics/SoftBody/SoftBodyContactListener.h"
+
+#include <new>
+
+class JoltShapedObject3D;
+class JoltSpace3D;
+
+class JoltContactListener3D final
+		: public JPH::ContactListener,
+		  public JPH::SoftBodyContactListener {
+	struct BodyIDHasher {
+		static uint32_t hash(const JPH::BodyID &p_id) { return hash_fmix32(p_id.GetIndexAndSequenceNumber()); }
+	};
+
+	struct ShapePairHasher {
+		static uint32_t hash(const JPH::SubShapeIDPair &p_pair) {
+			uint32_t hash = hash_murmur3_one_32(p_pair.GetBody1ID().GetIndexAndSequenceNumber());
+			hash = hash_murmur3_one_32(p_pair.GetSubShapeID1().GetValue(), hash);
+			hash = hash_murmur3_one_32(p_pair.GetBody2ID().GetIndexAndSequenceNumber(), hash);
+			hash = hash_murmur3_one_32(p_pair.GetSubShapeID2().GetValue(), hash);
+			return hash_fmix32(hash);
+		}
+	};
+
+	struct Contact {
+		Vector3 point_self;
+		Vector3 point_other;
+		Vector3 normal;
+		Vector3 velocity_self;
+		Vector3 velocity_other;
+		Vector3 impulse;
+	};
+
+	typedef LocalVector<Contact> Contacts;
+
+	struct Manifold {
+		Contacts contacts1;
+		Contacts contacts2;
+		float depth = 0.0f;
+	};
+
+	HashMap<JPH::SubShapeIDPair, Manifold, ShapePairHasher> manifolds_by_shape_pair;
+	HashSet<JPH::SubShapeIDPair, ShapePairHasher> area_overlaps;
+	HashSet<JPH::SubShapeIDPair, ShapePairHasher> area_enters;
+	HashSet<JPH::SubShapeIDPair, ShapePairHasher> area_exits;
+	Mutex write_mutex;
+	JoltSpace3D *space = nullptr;
+
+#ifdef DEBUG_ENABLED
+	PackedVector3Array debug_contacts;
+	std::atomic_int debug_contact_count = 0;
+#endif
+
+	virtual void OnContactAdded(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold, JPH::ContactSettings &p_settings) override;
+	virtual void OnContactPersisted(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold, JPH::ContactSettings &p_settings) override;
+	virtual void OnContactRemoved(const JPH::SubShapeIDPair &p_shape_pair) override;
+
+	virtual JPH::SoftBodyValidateResult OnSoftBodyContactValidate(const JPH::Body &p_soft_body, const JPH::Body &p_other_body, JPH::SoftBodyContactSettings &p_settings) override;
+
+#ifdef DEBUG_ENABLED
+	virtual void OnSoftBodyContactAdded(const JPH::Body &p_soft_body, const JPH::SoftBodyManifold &p_manifold) override;
+#endif
+
+	bool _try_override_collision_response(const JPH::Body &p_jolt_body1, const JPH::Body &p_jolt_body2, JPH::ContactSettings &p_settings);
+	bool _try_override_collision_response(const JPH::Body &p_jolt_soft_body, const JPH::Body &p_jolt_other_body, JPH::SoftBodyContactSettings &p_settings);
+	bool _try_apply_surface_velocities(const JPH::Body &p_jolt_body1, const JPH::Body &p_jolt_body2, JPH::ContactSettings &p_settings);
+	bool _try_add_contacts(const JPH::Body &p_jolt_body1, const JPH::Body &p_jolt_body2, const JPH::ContactManifold &p_manifold, JPH::ContactSettings &p_settings);
+	bool _try_evaluate_area_overlap(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold);
+	bool _try_remove_contacts(const JPH::SubShapeIDPair &p_shape_pair);
+	bool _try_remove_area_overlap(const JPH::SubShapeIDPair &p_shape_pair);
+
+#ifdef DEBUG_ENABLED
+	bool _try_add_debug_contacts(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold);
+	bool _try_add_debug_contacts(const JPH::Body &p_soft_body, const JPH::SoftBodyManifold &p_manifold);
+#endif
+
+	void _flush_contacts();
+	void _flush_area_enters();
+	void _flush_area_exits();
+
+public:
+	explicit JoltContactListener3D(JoltSpace3D *p_space) :
+			space(p_space) {}
+
+	void pre_step();
+	void post_step();
+
+#ifdef DEBUG_ENABLED
+	const PackedVector3Array &get_debug_contacts() const { return debug_contacts; }
+	int get_debug_contact_count() const { return debug_contact_count.load(std::memory_order_acquire); }
+	int get_max_debug_contacts() const { return (int)debug_contacts.size(); }
+	void set_max_debug_contacts(int p_count) { debug_contacts.resize(p_count); }
+#endif
+};

modules/jolt_physics/spaces/jolt_job_system.cpp

@@ -0,0 +1,201 @@
+/**************************************************************************/
+/*  jolt_job_system.cpp                                                   */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* 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 "jolt_job_system.h"
+
+#include "../jolt_project_settings.h"
+
+#include "core/debugger/engine_debugger.h"
+#include "core/object/worker_thread_pool.h"
+#include "core/os/os.h"
+#include "core/os/time.h"
+
+#include "Jolt/Physics/PhysicsSettings.h"
+
+void JoltJobSystem::Job::_execute(void *p_user_data) {
+	Job *job = static_cast<Job *>(p_user_data);
+
+#ifdef DEBUG_ENABLED
+	const uint64_t time_start = Time::get_singleton()->get_ticks_usec();
+#endif
+
+	job->Execute();
+
+#ifdef DEBUG_ENABLED
+	const uint64_t time_end = Time::get_singleton()->get_ticks_usec();
+	const uint64_t time_elapsed = time_end - time_start;
+
+	timings_lock.lock();
+	timings_by_job[job->name] += time_elapsed;
+	timings_lock.unlock();
+#endif
+
+	job->Release();
+}
+
+JoltJobSystem::Job::Job(const char *p_name, JPH::ColorArg p_color, JPH::JobSystem *p_job_system, const JPH::JobSystem::JobFunction &p_job_function, JPH::uint32 p_dependency_count) :
+		JPH::JobSystem::Job(p_name, p_color, p_job_system, p_job_function, p_dependency_count)
+#ifdef DEBUG_ENABLED
+		,
+		name(p_name)
+#endif
+{
+}
+
+JoltJobSystem::Job::~Job() {
+	if (task_id != -1) {
+		WorkerThreadPool::get_singleton()->wait_for_task_completion(task_id);
+	}
+}
+
+void JoltJobSystem::Job::push_completed(Job *p_job) {
+	Job *prev_head = nullptr;
+
+	do {
+		prev_head = completed_head.load(std::memory_order_relaxed);
+		p_job->completed_next = prev_head;
+	} while (!completed_head.compare_exchange_weak(prev_head, p_job, std::memory_order_release, std::memory_order_relaxed));
+}
+
+JoltJobSystem::Job *JoltJobSystem::Job::pop_completed() {
+	Job *prev_head = nullptr;
+
+	do {
+		prev_head = completed_head.load(std::memory_order_relaxed);
+		if (prev_head == nullptr) {
+			return nullptr;
+		}
+	} while (!completed_head.compare_exchange_weak(prev_head, prev_head->completed_next, std::memory_order_acquire, std::memory_order_relaxed));
+
+	return prev_head;
+}
+
+void JoltJobSystem::Job::queue() {
+	AddRef();
+
+	// Ideally we would use Jolt's actual job name here, but I'd rather not incur the overhead of a memory allocation or
+	// thread-safe lookup every time we create/queue a task. So instead we use the same cached description for all of them.
+	static const String task_name("Jolt Physics");
+
+	task_id = WorkerThreadPool::get_singleton()->add_native_task(&_execute, this, true, task_name);
+}
+
+int JoltJobSystem::GetMaxConcurrency() const {
+	return thread_count;
+}
+
+JPH::JobHandle JoltJobSystem::CreateJob(const char *p_name, JPH::ColorArg p_color, const JPH::JobSystem::JobFunction &p_job_function, JPH::uint32 p_dependency_count) {
+	Job *job = nullptr;
+
+	while (true) {
+		JPH::uint32 job_index = jobs.ConstructObject(p_name, p_color, this, p_job_function, p_dependency_count);
+
+		if (job_index != JPH::FixedSizeFreeList<Job>::cInvalidObjectIndex) {
+			job = &jobs.Get(job_index);
+			break;
+		}
+
+		WARN_PRINT_ONCE("Jolt Physics job system exceeded the maximum number of jobs. This should not happen. Please report this. Waiting for jobs to become available...");
+
+		OS::get_singleton()->delay_usec(100);
+
+		_reclaim_jobs();
+	}
+
+	// This will increment the job's reference count, so must happen before we queue the job
+	JPH::JobHandle job_handle(job);
+
+	if (p_dependency_count == 0) {
+		QueueJob(job);
+	}
+
+	return job_handle;
+}
+
+void JoltJobSystem::QueueJob(JPH::JobSystem::Job *p_job) {
+	static_cast<Job *>(p_job)->queue();
+}
+
+void JoltJobSystem::QueueJobs(JPH::JobSystem::Job **p_jobs, JPH::uint p_job_count) {
+	for (JPH::uint i = 0; i < p_job_count; ++i) {
+		QueueJob(p_jobs[i]);
+	}
+}
+
+void JoltJobSystem::FreeJob(JPH::JobSystem::Job *p_job) {
+	Job::push_completed(static_cast<Job *>(p_job));
+}
+
+void JoltJobSystem::_reclaim_jobs() {
+	while (Job *job = Job::pop_completed()) {
+		jobs.DestructObject(job);
+	}
+}
+
+JoltJobSystem::JoltJobSystem() :
+		JPH::JobSystemWithBarrier(JPH::cMaxPhysicsBarriers),
+		thread_count(MAX(1, WorkerThreadPool::get_singleton()->get_thread_count())) {
+	jobs.Init(JPH::cMaxPhysicsJobs, JPH::cMaxPhysicsJobs);
+}
+
+void JoltJobSystem::pre_step() {
+	// Nothing to do.
+}
+
+void JoltJobSystem::post_step() {
+	_reclaim_jobs();
+}
+
+#ifdef DEBUG_ENABLED
+
+void JoltJobSystem::flush_timings() {
+	static const StringName profiler_name("servers");
+
+	EngineDebugger *engine_debugger = EngineDebugger::get_singleton();
+
+	if (engine_debugger->is_profiling(profiler_name)) {
+		Array timings;
+
+		for (const KeyValue<const void *, uint64_t> &E : timings_by_job) {
+			timings.push_back(static_cast<const char *>(E.key));
+			timings.push_back(USEC_TO_SEC(E.value));
+		}
+
+		timings.push_front("physics_3d");
+
+		engine_debugger->profiler_add_frame_data(profiler_name, timings);
+	}
+
+	for (KeyValue<const void *, uint64_t> &E : timings_by_job) {
+		E.value = 0;
+	}
+}
+
+#endif

modules/jolt_physics/spaces/jolt_job_system.h

@@ -0,0 +1,103 @@
+/**************************************************************************/
+/*  jolt_job_system.h                                                     */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* 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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/os/spin_lock.h"
+#include "core/templates/hash_map.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Core/FixedSizeFreeList.h"
+#include "Jolt/Core/JobSystemWithBarrier.h"
+
+#include <atomic>
+
+class JoltJobSystem final : public JPH::JobSystemWithBarrier {
+	class Job : public JPH::JobSystem::Job {
+		inline static std::atomic<Job *> completed_head = nullptr;
+
+#ifdef DEBUG_ENABLED
+		const char *name = nullptr;
+#endif
+
+		int64_t task_id = -1;
+
+		std::atomic<Job *> completed_next = nullptr;
+
+		static void _execute(void *p_user_data);
+
+	public:
+		Job(const char *p_name, JPH::ColorArg p_color, JPH::JobSystem *p_job_system, const JPH::JobSystem::JobFunction &p_job_function, JPH::uint32 p_dependency_count);
+		Job(const Job &p_other) = delete;
+		Job(Job &&p_other) = delete;
+		~Job();
+
+		static void push_completed(Job *p_job);
+		static Job *pop_completed();
+
+		void queue();
+
+		Job &operator=(const Job &p_other) = delete;
+		Job &operator=(Job &&p_other) = delete;
+	};
+
+#ifdef DEBUG_ENABLED
+	// We use `const void*` here to avoid the cost of hashing the actual string, since the job names
+	// are always literals and as such will point to the same address every time.
+	inline static HashMap<const void *, uint64_t> timings_by_job;
+
+	// TODO: Check whether the usage of SpinLock is justified or if this should be a mutex instead.
+	inline static SpinLock timings_lock;
+#endif
+
+	JPH::FixedSizeFreeList<Job> jobs;
+
+	int thread_count = 0;
+
+	virtual int GetMaxConcurrency() const override;
+
+	virtual JPH::JobHandle CreateJob(const char *p_name, JPH::ColorArg p_color, const JPH::JobSystem::JobFunction &p_job_function, JPH::uint32 p_dependency_count = 0) override;
+	virtual void QueueJob(JPH::JobSystem::Job *p_job) override;
+	virtual void QueueJobs(JPH::JobSystem::Job **p_jobs, JPH::uint p_job_count) override;
+	virtual void FreeJob(JPH::JobSystem::Job *p_job) override;
+
+	void _reclaim_jobs();
+
+public:
+	JoltJobSystem();
+
+	void pre_step();
+	void post_step();
+
+#ifdef DEBUG_ENABLED
+	void flush_timings();
+#endif
+};

modules/jolt_physics/spaces/jolt_layers.cpp

@@ -0,0 +1,224 @@
+/**************************************************************************/
+/*  jolt_layers.cpp                                                       */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* 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 "jolt_layers.h"
+
+#include "../jolt_project_settings.h"
+#include "jolt_broad_phase_layer.h"
+
+#include "core/error/error_macros.h"
+#include "core/variant/variant.h"
+
+static_assert(sizeof(JPH::ObjectLayer) == 2, "Size of Jolt's object layer has changed.");
+static_assert(sizeof(JPH::BroadPhaseLayer::Type) == 1, "Size of Jolt's broadphase layer has changed.");
+static_assert(JoltBroadPhaseLayer::COUNT <= 8, "Maximum number of broadphase layers exceeded.");
+
+namespace {
+
+template <uint8_t TSize = JoltBroadPhaseLayer::COUNT>
+class JoltBroadPhaseMatrix {
+	typedef JPH::BroadPhaseLayer LayerType;
+	typedef LayerType::Type UnderlyingType;
+
+public:
+	JoltBroadPhaseMatrix() {
+		using namespace JoltBroadPhaseLayer;
+
+		allow_collision(BODY_STATIC, BODY_DYNAMIC);
+		allow_collision(BODY_STATIC, AREA_DETECTABLE);
+		allow_collision(BODY_STATIC, AREA_UNDETECTABLE);
+		allow_collision(BODY_STATIC_BIG, BODY_DYNAMIC);
+		allow_collision(BODY_STATIC_BIG, AREA_DETECTABLE);
+		allow_collision(BODY_STATIC_BIG, AREA_UNDETECTABLE);
+		allow_collision(BODY_DYNAMIC, BODY_STATIC);
+		allow_collision(BODY_DYNAMIC, BODY_STATIC_BIG);
+		allow_collision(BODY_DYNAMIC, BODY_DYNAMIC);
+		allow_collision(BODY_DYNAMIC, AREA_DETECTABLE);
+		allow_collision(BODY_DYNAMIC, AREA_UNDETECTABLE);
+		allow_collision(AREA_DETECTABLE, BODY_DYNAMIC);
+		allow_collision(AREA_DETECTABLE, BODY_STATIC);
+		allow_collision(AREA_DETECTABLE, BODY_STATIC_BIG);
+		allow_collision(AREA_DETECTABLE, AREA_DETECTABLE);
+		allow_collision(AREA_DETECTABLE, AREA_UNDETECTABLE);
+		allow_collision(AREA_UNDETECTABLE, BODY_DYNAMIC);
+		allow_collision(AREA_UNDETECTABLE, BODY_STATIC);
+		allow_collision(AREA_UNDETECTABLE, BODY_STATIC_BIG);
+		allow_collision(AREA_UNDETECTABLE, AREA_DETECTABLE);
+	}
+
+	void allow_collision(UnderlyingType p_layer1, UnderlyingType p_layer2) { masks[p_layer1] |= uint8_t(1U << p_layer2); }
+	void allow_collision(LayerType p_layer1, LayerType p_layer2) { allow_collision((UnderlyingType)p_layer1, (UnderlyingType)p_layer2); }
+
+	bool should_collide(UnderlyingType p_layer1, UnderlyingType p_layer2) const { return (masks[p_layer1] & uint8_t(1U << p_layer2)) != 0; }
+	bool should_collide(LayerType p_layer1, LayerType p_layer2) const { return should_collide((UnderlyingType)p_layer1, (UnderlyingType)p_layer2); }
+
+private:
+	uint8_t masks[TSize] = {};
+};
+
+constexpr JPH::ObjectLayer encode_layers(JPH::BroadPhaseLayer p_broad_phase_layer, JPH::ObjectLayer p_object_layer) {
+	const uint16_t upper_bits = uint16_t((uint8_t)p_broad_phase_layer << 13U);
+	const uint16_t lower_bits = uint16_t(p_object_layer);
+	return JPH::ObjectLayer(upper_bits | lower_bits);
+}
+
+constexpr void decode_layers(JPH::ObjectLayer p_encoded_layers, JPH::BroadPhaseLayer &r_broad_phase_layer, JPH::ObjectLayer &r_object_layer) {
+	r_broad_phase_layer = JPH::BroadPhaseLayer(uint8_t(p_encoded_layers >> 13U));
+	r_object_layer = JPH::ObjectLayer(p_encoded_layers & 0b0001'1111'1111'1111U);
+}
+
+constexpr uint64_t encode_collision(uint32_t p_collision_layer, uint32_t p_collision_mask) {
+	const uint64_t upper_bits = (uint64_t)p_collision_layer << 32U;
+	const uint64_t lower_bits = (uint64_t)p_collision_mask;
+	return upper_bits | lower_bits;
+}
+
+constexpr void decode_collision(uint64_t p_collision, uint32_t &r_collision_layer, uint32_t &r_collision_mask) {
+	r_collision_layer = uint32_t(p_collision >> 32U);
+	r_collision_mask = uint32_t(p_collision & 0xFFFFFFFFU);
+}
+
+} // namespace
+
+uint32_t JoltLayers::GetNumBroadPhaseLayers() const {
+	return JoltBroadPhaseLayer::COUNT;
+}
+
+JPH::BroadPhaseLayer JoltLayers::GetBroadPhaseLayer(JPH::ObjectLayer p_layer) const {
+	JPH::BroadPhaseLayer broad_phase_layer = JoltBroadPhaseLayer::BODY_STATIC;
+	JPH::ObjectLayer object_layer = 0;
+	decode_layers(p_layer, broad_phase_layer, object_layer);
+
+	return broad_phase_layer;
+}
+
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+
+const char *JoltLayers::GetBroadPhaseLayerName(JPH::BroadPhaseLayer p_layer) const {
+	switch ((JPH::BroadPhaseLayer::Type)p_layer) {
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::BODY_STATIC: {
+			return "BODY_STATIC";
+		}
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::BODY_STATIC_BIG: {
+			return "BODY_STATIC_BIG";
+		}
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::BODY_DYNAMIC: {
+			return "BODY_DYNAMIC";
+		}
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::AREA_DETECTABLE: {
+			return "AREA_DETECTABLE";
+		}
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::AREA_UNDETECTABLE: {
+			return "AREA_UNDETECTABLE";
+		}
+		default: {
+			return "UNKNOWN";
+		}
+	}
+}
+
+#endif
+
+bool JoltLayers::ShouldCollide(JPH::ObjectLayer p_encoded_layer1, JPH::ObjectLayer p_encoded_layer2) const {
+	JPH::BroadPhaseLayer broad_phase_layer1 = JoltBroadPhaseLayer::BODY_STATIC;
+	uint32_t collision_layer1 = 0;
+	uint32_t collision_mask1 = 0;
+	from_object_layer(p_encoded_layer1, broad_phase_layer1, collision_layer1, collision_mask1);
+
+	JPH::BroadPhaseLayer broad_phase_layer2 = JoltBroadPhaseLayer::BODY_STATIC;
+	uint32_t collision_layer2 = 0;
+	uint32_t collision_mask2 = 0;
+	from_object_layer(p_encoded_layer2, broad_phase_layer2, collision_layer2, collision_mask2);
+
+	const bool first_scans_second = (collision_mask1 & collision_layer2) != 0;
+	const bool second_scans_first = (collision_mask2 & collision_layer1) != 0;
+
+	return first_scans_second || second_scans_first;
+}
+
+bool JoltLayers::ShouldCollide(JPH::ObjectLayer p_encoded_layer1, JPH::BroadPhaseLayer p_broad_phase_layer2) const {
+	static const JoltBroadPhaseMatrix matrix;
+
+	JPH::BroadPhaseLayer broad_phase_layer1 = JoltBroadPhaseLayer::BODY_STATIC;
+	JPH::ObjectLayer object_layer1 = 0;
+	decode_layers(p_encoded_layer1, broad_phase_layer1, object_layer1);
+
+	return matrix.should_collide(broad_phase_layer1, p_broad_phase_layer2);
+}
+
+JPH::ObjectLayer JoltLayers::_allocate_object_layer(uint64_t p_collision) {
+	const JPH::ObjectLayer new_object_layer = next_object_layer++;
+
+	collisions_by_layer.resize(new_object_layer + 1);
+	collisions_by_layer[new_object_layer] = p_collision;
+
+	layers_by_collision[p_collision] = new_object_layer;
+
+	return new_object_layer;
+}
+
+JoltLayers::JoltLayers() {
+	_allocate_object_layer(0);
+}
+
+// MinGW GCC using LTO will emit errors during linking if this is defined in the header file, implicitly or otherwise.
+// Likely caused by this GCC bug: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=94156
+JoltLayers::~JoltLayers() = default;
+
+JPH::ObjectLayer JoltLayers::to_object_layer(JPH::BroadPhaseLayer p_broad_phase_layer, uint32_t p_collision_layer, uint32_t p_collision_mask) {
+	const uint64_t collision = encode_collision(p_collision_layer, p_collision_mask);
+
+	JPH::ObjectLayer object_layer = 0;
+
+	HashMap<uint64_t, JPH::ObjectLayer>::Iterator iter = layers_by_collision.find(collision);
+	if (iter != layers_by_collision.end()) {
+		object_layer = iter->value;
+	} else {
+		constexpr uint16_t object_layer_count = 1U << 13U;
+
+		ERR_FAIL_COND_V_MSG(next_object_layer == object_layer_count, 0,
+				vformat("Maximum number of object layers (%d) reached. "
+						"This means there are %d combinations of collision layers and masks."
+						"This should not happen under normal circumstances. Consider reporting this.",
+						object_layer_count, object_layer_count));
+
+		object_layer = _allocate_object_layer(collision);
+	}
+
+	return encode_layers(p_broad_phase_layer, object_layer);
+}
+
+void JoltLayers::from_object_layer(JPH::ObjectLayer p_encoded_layer, JPH::BroadPhaseLayer &r_broad_phase_layer, uint32_t &r_collision_layer, uint32_t &r_collision_mask) const {
+	JPH::ObjectLayer object_layer = 0;
+	decode_layers(p_encoded_layer, r_broad_phase_layer, object_layer);
+
+	const uint64_t collision = collisions_by_layer[object_layer];
+	decode_collision(collision, r_collision_layer, r_collision_mask);
+}

modules/jolt_physics/spaces/jolt_layers.h

@@ -0,0 +1,67 @@
+/**************************************************************************/
+/*  jolt_layers.h                                                         */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* 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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/templates/hash_map.h"
+#include "core/templates/local_vector.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h"
+#include "Jolt/Physics/Collision/ObjectLayer.h"
+
+class JoltLayers final
+		: public JPH::BroadPhaseLayerInterface,
+		  public JPH::ObjectLayerPairFilter,
+		  public JPH::ObjectVsBroadPhaseLayerFilter {
+	LocalVector<uint64_t> collisions_by_layer;
+	HashMap<uint64_t, JPH::ObjectLayer> layers_by_collision;
+	JPH::ObjectLayer next_object_layer = 0;
+
+	virtual uint32_t GetNumBroadPhaseLayers() const override;
+	virtual JPH::BroadPhaseLayer GetBroadPhaseLayer(JPH::ObjectLayer p_layer) const override;
+
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+	virtual const char *GetBroadPhaseLayerName(JPH::BroadPhaseLayer p_layer) const override;
+#endif
+
+	virtual bool ShouldCollide(JPH::ObjectLayer p_encoded_layer1, JPH::ObjectLayer p_encoded_layer2) const override;
+	virtual bool ShouldCollide(JPH::ObjectLayer p_encoded_layer1, JPH::BroadPhaseLayer p_broad_phase_layer2) const override;
+
+	JPH::ObjectLayer _allocate_object_layer(uint64_t p_collision);
+
+public:
+	JoltLayers();
+	virtual ~JoltLayers();
+
+	JPH::ObjectLayer to_object_layer(JPH::BroadPhaseLayer p_broad_phase_layer, uint32_t p_collision_layer, uint32_t p_collision_mask);
+	void from_object_layer(JPH::ObjectLayer p_encoded_layer, JPH::BroadPhaseLayer &r_broad_phase_layer, uint32_t &r_collision_layer, uint32_t &r_collision_mask) const;
+};

modules/jolt_physics/spaces/jolt_motion_filter_3d.cpp

@@ -0,0 +1,113 @@
+/**************************************************************************/
+/*  jolt_motion_filter_3d.cpp                                             */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* 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 "jolt_motion_filter_3d.h"
+
+#include "../objects/jolt_body_3d.h"
+#include "../objects/jolt_object_3d.h"
+#include "../shapes/jolt_custom_motion_shape.h"
+#include "../shapes/jolt_custom_ray_shape.h"
+#include "../shapes/jolt_custom_shape_type.h"
+#include "../shapes/jolt_shape_3d.h"
+#include "jolt_broad_phase_layer.h"
+#include "jolt_space_3d.h"
+
+JoltMotionFilter3D::JoltMotionFilter3D(const JoltBody3D &p_body, const HashSet<RID> &p_excluded_bodies, const HashSet<ObjectID> &p_excluded_objects, bool p_collide_separation_ray) :
+		body_self(p_body),
+		space(*body_self.get_space()),
+		excluded_bodies(p_excluded_bodies),
+		excluded_objects(p_excluded_objects),
+		collide_separation_ray(p_collide_separation_ray) {
+}
+
+bool JoltMotionFilter3D::ShouldCollide(JPH::BroadPhaseLayer p_broad_phase_layer) const {
+	const JPH::BroadPhaseLayer::Type broad_phase_layer = (JPH::BroadPhaseLayer::Type)p_broad_phase_layer;
+
+	switch (broad_phase_layer) {
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::BODY_STATIC:
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::BODY_STATIC_BIG:
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::BODY_DYNAMIC: {
+			return true;
+		} break;
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::AREA_DETECTABLE:
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::AREA_UNDETECTABLE: {
+			return false;
+		} break;
+		default: {
+			ERR_FAIL_V_MSG(false, vformat("Unhandled broad phase layer: '%d'. This should not happen. Please report this.", broad_phase_layer));
+		}
+	}
+}
+
+bool JoltMotionFilter3D::ShouldCollide(JPH::ObjectLayer p_object_layer) const {
+	JPH::BroadPhaseLayer object_broad_phase_layer = JoltBroadPhaseLayer::BODY_STATIC;
+	uint32_t object_collision_layer = 0;
+	uint32_t object_collision_mask = 0;
+
+	space.map_from_object_layer(p_object_layer, object_broad_phase_layer, object_collision_layer, object_collision_mask);
+
+	return (body_self.get_collision_mask() & object_collision_layer) != 0;
+}
+
+bool JoltMotionFilter3D::ShouldCollide(const JPH::BodyID &p_jolt_id) const {
+	return p_jolt_id != body_self.get_jolt_id();
+}
+
+bool JoltMotionFilter3D::ShouldCollideLocked(const JPH::Body &p_jolt_body) const {
+	if (p_jolt_body.IsSoftBody()) {
+		return false;
+	}
+
+	const JoltObject3D *object = reinterpret_cast<const JoltObject3D *>(p_jolt_body.GetUserData());
+	if (excluded_objects.has(object->get_instance_id()) || excluded_bodies.has(object->get_rid())) {
+		return false;
+	}
+
+	return body_self.get_jolt_body()->GetCollisionGroup().CanCollide(p_jolt_body.GetCollisionGroup());
+}
+
+bool JoltMotionFilter3D::ShouldCollide(const JPH::Shape *p_jolt_shape, const JPH::SubShapeID &p_jolt_shape_id) const {
+	return true;
+}
+
+bool JoltMotionFilter3D::ShouldCollide(const JPH::Shape *p_jolt_shape_self, const JPH::SubShapeID &p_jolt_shape_id_self, const JPH::Shape *p_jolt_shape_other, const JPH::SubShapeID &p_jolt_shape_id_other) const {
+	if (collide_separation_ray) {
+		return true;
+	}
+
+	const JoltCustomMotionShape *motion_shape = static_cast<const JoltCustomMotionShape *>(p_jolt_shape_self);
+	const JPH::ConvexShape &actual_shape_self = motion_shape->get_inner_shape();
+	if (actual_shape_self.GetSubType() == JoltCustomShapeSubType::RAY) {
+		// When `slide_on_slope` is enabled the ray shape acts as a regular shape.
+		return static_cast<const JoltCustomRayShape &>(actual_shape_self).slide_on_slope;
+	}
+
+	return true;
+}

modules/jolt_physics/spaces/jolt_motion_filter_3d.h

@@ -0,0 +1,69 @@
+/**************************************************************************/
+/*  jolt_motion_filter_3d.h                                               */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* 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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/object/object_id.h"
+#include "core/templates/hash_set.h"
+#include "core/templates/rid.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Body/Body.h"
+#include "Jolt/Physics/Body/BodyFilter.h"
+#include "Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h"
+#include "Jolt/Physics/Collision/ObjectLayer.h"
+#include "Jolt/Physics/Collision/ShapeFilter.h"
+
+class JoltBody3D;
+class JoltPhysicsServer3D;
+class JoltSpace3D;
+
+class JoltMotionFilter3D final
+		: public JPH::BroadPhaseLayerFilter,
+		  public JPH::ObjectLayerFilter,
+		  public JPH::BodyFilter,
+		  public JPH::ShapeFilter {
+	const JoltBody3D &body_self;
+	const JoltSpace3D &space;
+	const HashSet<RID> &excluded_bodies;
+	const HashSet<ObjectID> &excluded_objects;
+	bool collide_separation_ray = false;
+
+public:
+	explicit JoltMotionFilter3D(const JoltBody3D &p_body, const HashSet<RID> &p_excluded_bodies, const HashSet<ObjectID> &p_excluded_objects, bool p_collide_separation_ray = true);
+
+	virtual bool ShouldCollide(JPH::BroadPhaseLayer p_broad_phase_layer) const override;
+	virtual bool ShouldCollide(JPH::ObjectLayer p_object_layer) const override;
+	virtual bool ShouldCollide(const JPH::BodyID &p_jolt_id) const override;
+	virtual bool ShouldCollideLocked(const JPH::Body &p_jolt_body) const override;
+	virtual bool ShouldCollide(const JPH::Shape *p_jolt_shape, const JPH::SubShapeID &p_jolt_shape_id) const override;
+	virtual bool ShouldCollide(const JPH::Shape *p_jolt_shape_self, const JPH::SubShapeID &p_jolt_shape_id_self, const JPH::Shape *p_jolt_shape_other, const JPH::SubShapeID &p_jolt_shape_id_other) const override;
+};

modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.cpp

@@ -0,0 +1,913 @@
+/**************************************************************************/
+/*  jolt_physics_direct_space_state_3d.cpp                                */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* 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 "jolt_physics_direct_space_state_3d.h"
+
+#include "../jolt_physics_server_3d.h"
+#include "../jolt_project_settings.h"
+#include "../misc/jolt_math_funcs.h"
+#include "../misc/jolt_type_conversions.h"
+#include "../objects/jolt_area_3d.h"
+#include "../objects/jolt_body_3d.h"
+#include "../objects/jolt_object_3d.h"
+#include "../shapes/jolt_custom_motion_shape.h"
+#include "../shapes/jolt_shape_3d.h"
+#include "jolt_motion_filter_3d.h"
+#include "jolt_query_collectors.h"
+#include "jolt_query_filter_3d.h"
+#include "jolt_space_3d.h"
+
+#include "Jolt/Geometry/GJKClosestPoint.h"
+#include "Jolt/Physics/Body/Body.h"
+#include "Jolt/Physics/Body/BodyFilter.h"
+#include "Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h"
+#include "Jolt/Physics/Collision/CastResult.h"
+#include "Jolt/Physics/Collision/CollidePointResult.h"
+#include "Jolt/Physics/Collision/NarrowPhaseQuery.h"
+#include "Jolt/Physics/Collision/RayCast.h"
+#include "Jolt/Physics/Collision/Shape/MeshShape.h"
+#include "Jolt/Physics/PhysicsSystem.h"
+
+bool JoltPhysicsDirectSpaceState3D::_cast_motion_impl(const JPH::Shape &p_jolt_shape, const Transform3D &p_transform_com, const Vector3 &p_scale, const Vector3 &p_motion, bool p_use_edge_removal, bool p_ignore_overlaps, const JPH::CollideShapeSettings &p_settings, const JPH::BroadPhaseLayerFilter &p_broad_phase_layer_filter, const JPH::ObjectLayerFilter &p_object_layer_filter, const JPH::BodyFilter &p_body_filter, const JPH::ShapeFilter &p_shape_filter, real_t &r_closest_safe, real_t &r_closest_unsafe) const {
+	r_closest_safe = 1.0f;
+	r_closest_unsafe = 1.0f;
+
+	ERR_FAIL_COND_V_MSG(p_jolt_shape.GetType() != JPH::EShapeType::Convex, false, "Shape-casting with non-convex shapes is not supported.");
+
+	const float motion_length = (float)p_motion.length();
+
+	if (p_ignore_overlaps && motion_length == 0.0f) {
+		return false;
+	}
+
+	const JPH::RMat44 transform_com = to_jolt_r(p_transform_com);
+	const JPH::Vec3 scale = to_jolt(p_scale);
+	const JPH::Vec3 motion = to_jolt(p_motion);
+	const JPH::Vec3 motion_local = transform_com.Multiply3x3Transposed(motion);
+
+	JPH::AABox aabb = p_jolt_shape.GetWorldSpaceBounds(transform_com, scale);
+	JPH::AABox aabb_translated = aabb;
+	aabb_translated.Translate(motion);
+	aabb.Encapsulate(aabb_translated);
+
+	JoltQueryCollectorAnyMulti<JPH::CollideShapeBodyCollector, 1024> aabb_collector;
+	space->get_broad_phase_query().CollideAABox(aabb, aabb_collector, p_broad_phase_layer_filter, p_object_layer_filter);
+
+	if (!aabb_collector.had_hit()) {
+		return false;
+	}
+
+	const JPH::RVec3 base_offset = transform_com.GetTranslation();
+
+	JoltCustomMotionShape motion_shape(static_cast<const JPH::ConvexShape &>(p_jolt_shape));
+
+	auto collides = [&](const JPH::Body &p_other_body, float p_fraction) {
+		motion_shape.set_motion(motion_local * p_fraction);
+
+		const JPH::TransformedShape other_shape = p_other_body.GetTransformedShape();
+
+		JoltQueryCollectorAny<JPH::CollideShapeCollector> collector;
+
+		if (p_use_edge_removal) {
+			JPH::CollideShapeSettings eier_settings = p_settings;
+			eier_settings.mActiveEdgeMode = JPH::EActiveEdgeMode::CollideWithAll;
+			eier_settings.mCollectFacesMode = JPH::ECollectFacesMode::CollectFaces;
+
+			JPH::InternalEdgeRemovingCollector eier_collector(collector);
+			other_shape.CollideShape(&motion_shape, scale, transform_com, eier_settings, base_offset, eier_collector, p_shape_filter);
+			eier_collector.Flush();
+		} else {
+			other_shape.CollideShape(&motion_shape, scale, transform_com, p_settings, base_offset, collector, p_shape_filter);
+		}
+
+		return collector.had_hit();
+	};
+
+	// Figure out the number of steps we need in our binary search in order to achieve millimeter precision, within reason.
+	const int step_count = CLAMP(int(std::log(1000.0f * motion_length) / (float)Math::LN2), 4, 16);
+
+	bool collided = false;
+
+	for (int i = 0; i < aabb_collector.get_hit_count(); ++i) {
+		const JPH::BodyID other_jolt_id = aabb_collector.get_hit(i);
+		if (!p_body_filter.ShouldCollide(other_jolt_id)) {
+			continue;
+		}
+
+		const JPH::Body *other_jolt_body = space->try_get_jolt_body(other_jolt_id);
+		if (!p_body_filter.ShouldCollideLocked(*other_jolt_body)) {
+			continue;
+		}
+
+		if (!collides(*other_jolt_body, 1.0f)) {
+			continue;
+		}
+
+		if (p_ignore_overlaps && collides(*other_jolt_body, 0.0f)) {
+			continue;
+		}
+
+		float lo = 0.0f;
+		float hi = 1.0f;
+		float coeff = 0.5f;
+
+		for (int j = 0; j < step_count; ++j) {
+			const float fraction = lo + (hi - lo) * coeff;
+
+			if (collides(*other_jolt_body, fraction)) {
+				collided = true;
+
+				hi = fraction;
+
+				if (j == 0 || lo > 0.0f) {
+					coeff = 0.5f;
+				} else {
+					coeff = 0.25f;
+				}
+			} else {
+				lo = fraction;
+
+				if (j == 0 || hi < 1.0f) {
+					coeff = 0.5f;
+				} else {
+					coeff = 0.75f;
+				}
+			}
+		}
+
+		if (lo < r_closest_safe) {
+			r_closest_safe = lo;
+			r_closest_unsafe = hi;
+		}
+	}
+
+	return collided;
+}
+
+bool JoltPhysicsDirectSpaceState3D::_body_motion_recover(const JoltBody3D &p_body, const Transform3D &p_transform, float p_margin, const HashSet<RID> &p_excluded_bodies, const HashSet<ObjectID> &p_excluded_objects, Vector3 &r_recovery) const {
+	const JPH::Shape *jolt_shape = p_body.get_jolt_shape();
+
+	const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
+	Transform3D transform_com = p_transform.translated_local(com_scaled);
+
+	JPH::CollideShapeSettings settings;
+	settings.mMaxSeparationDistance = p_margin;
+
+	const Vector3 &base_offset = transform_com.origin;
+
+	const JoltMotionFilter3D motion_filter(p_body, p_excluded_bodies, p_excluded_objects);
+	JoltQueryCollectorAnyMulti<JPH::CollideShapeCollector, 32> collector;
+
+	bool recovered = false;
+
+	for (int i = 0; i < JoltProjectSettings::motion_query_recovery_iterations; ++i) {
+		collector.reset();
+
+		_collide_shape_kinematics(jolt_shape, JPH::Vec3::sOne(), to_jolt_r(transform_com), settings, to_jolt_r(base_offset), collector, motion_filter, motion_filter, motion_filter, motion_filter);
+
+		if (!collector.had_hit()) {
+			break;
+		}
+
+		const int hit_count = collector.get_hit_count();
+
+		float combined_priority = 0.0;
+
+		for (int j = 0; j < hit_count; j++) {
+			const JPH::CollideShapeResult &hit = collector.get_hit(j);
+			const JoltBody3D *other_body = space->try_get_body(hit.mBodyID2);
+			ERR_CONTINUE(other_body == nullptr);
+
+			combined_priority += other_body->get_collision_priority();
+		}
+
+		const float average_priority = MAX(combined_priority / (float)hit_count, (float)CMP_EPSILON);
+
+		recovered = true;
+
+		Vector3 recovery;
+
+		for (int j = 0; j < hit_count; ++j) {
+			const JPH::CollideShapeResult &hit = collector.get_hit(j);
+
+			const Vector3 penetration_axis = to_godot(hit.mPenetrationAxis.Normalized());
+			const Vector3 margin_offset = penetration_axis * p_margin;
+
+			const Vector3 point_on_1 = base_offset + to_godot(hit.mContactPointOn1) + margin_offset;
+			const Vector3 point_on_2 = base_offset + to_godot(hit.mContactPointOn2);
+
+			const real_t distance_to_1 = penetration_axis.dot(point_on_1 + recovery);
+			const real_t distance_to_2 = penetration_axis.dot(point_on_2);
+
+			const float penetration_depth = float(distance_to_1 - distance_to_2);
+
+			if (penetration_depth <= 0.0f) {
+				continue;
+			}
+
+			const JoltBody3D *other_body = space->try_get_body(hit.mBodyID2);
+			ERR_CONTINUE(other_body == nullptr);
+
+			const float recovery_distance = penetration_depth * JoltProjectSettings::motion_query_recovery_amount;
+			const float other_priority = other_body->get_collision_priority();
+			const float other_priority_normalized = other_priority / average_priority;
+			const float scaled_recovery_distance = recovery_distance * other_priority_normalized;
+
+			recovery -= penetration_axis * scaled_recovery_distance;
+		}
+
+		if (recovery == Vector3()) {
+			break;
+		}
+
+		r_recovery += recovery;
+		transform_com.origin += recovery;
+	}
+
+	return recovered;
+}
+
+bool JoltPhysicsDirectSpaceState3D::_body_motion_cast(const JoltBody3D &p_body, const Transform3D &p_transform, const Vector3 &p_scale, const Vector3 &p_motion, bool p_collide_separation_ray, const HashSet<RID> &p_excluded_bodies, const HashSet<ObjectID> &p_excluded_objects, real_t &r_safe_fraction, real_t &r_unsafe_fraction) const {
+	const Transform3D body_transform = p_transform.scaled_local(p_scale);
+
+	const JPH::CollideShapeSettings settings;
+	const JoltMotionFilter3D motion_filter(p_body, p_excluded_bodies, p_excluded_objects, p_collide_separation_ray);
+
+	bool collided = false;
+
+	for (int i = 0; i < p_body.get_shape_count(); ++i) {
+		if (p_body.is_shape_disabled(i)) {
+			continue;
+		}
+
+		JoltShape3D *shape = p_body.get_shape(i);
+
+		if (!shape->is_convex()) {
+			continue;
+		}
+
+		const JPH::ShapeRefC jolt_shape = shape->try_build();
+		if (unlikely(jolt_shape == nullptr)) {
+			return false;
+		}
+
+		const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
+		const Transform3D transform_local = p_body.get_shape_transform_scaled(i);
+		const Transform3D transform_com_local = transform_local.translated_local(com_scaled);
+		Transform3D transform_com = body_transform * transform_com_local;
+
+		Vector3 scale;
+		JoltMath::decompose(transform_com, scale);
+		JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "body_test_motion was passed an invalid transform along with body '%s'. This results in invalid scaling for shape at index %d.");
+
+		real_t shape_safe_fraction = 1.0;
+		real_t shape_unsafe_fraction = 1.0;
+
+		collided |= _cast_motion_impl(*jolt_shape, transform_com, scale, p_motion, JoltProjectSettings::use_enhanced_internal_edge_removal_for_motion_queries, false, settings, motion_filter, motion_filter, motion_filter, motion_filter, shape_safe_fraction, shape_unsafe_fraction);
+
+		r_safe_fraction = MIN(r_safe_fraction, shape_safe_fraction);
+		r_unsafe_fraction = MIN(r_unsafe_fraction, shape_unsafe_fraction);
+	}
+
+	return collided;
+}
+
+bool JoltPhysicsDirectSpaceState3D::_body_motion_collide(const JoltBody3D &p_body, const Transform3D &p_transform, const Vector3 &p_motion, float p_margin, int p_max_collisions, const HashSet<RID> &p_excluded_bodies, const HashSet<ObjectID> &p_excluded_objects, PhysicsServer3D::MotionResult *p_result) const {
+	if (p_max_collisions == 0) {
+		return false;
+	}
+
+	const JPH::Shape *jolt_shape = p_body.get_jolt_shape();
+
+	const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
+	const Transform3D transform_com = p_transform.translated_local(com_scaled);
+
+	JPH::CollideShapeSettings settings;
+	settings.mCollectFacesMode = JPH::ECollectFacesMode::CollectFaces;
+	settings.mMaxSeparationDistance = p_margin;
+
+	const Vector3 &base_offset = transform_com.origin;
+
+	const JoltMotionFilter3D motion_filter(p_body, p_excluded_bodies, p_excluded_objects);
+	JoltQueryCollectorClosestMulti<JPH::CollideShapeCollector, 32> collector(p_max_collisions);
+	_collide_shape_kinematics(jolt_shape, JPH::Vec3::sOne(), to_jolt_r(transform_com), settings, to_jolt_r(base_offset), collector, motion_filter, motion_filter, motion_filter, motion_filter);
+
+	if (!collector.had_hit() || p_result == nullptr) {
+		return collector.had_hit();
+	}
+
+	int count = 0;
+
+	for (int i = 0; i < collector.get_hit_count(); ++i) {
+		const JPH::CollideShapeResult &hit = collector.get_hit(i);
+
+		const float penetration_depth = hit.mPenetrationDepth + p_margin;
+
+		if (penetration_depth <= 0.0f) {
+			continue;
+		}
+
+		const Vector3 normal = to_godot(-hit.mPenetrationAxis.Normalized());
+
+		if (p_motion.length_squared() > 0) {
+			const Vector3 direction = p_motion.normalized();
+
+			if (direction.dot(normal) >= -CMP_EPSILON) {
+				continue;
+			}
+		}
+
+		JPH::ContactPoints contact_points1;
+		JPH::ContactPoints contact_points2;
+
+		if (p_max_collisions > 1) {
+			_generate_manifold(hit, contact_points1, contact_points2 JPH_IF_DEBUG_RENDERER(, to_jolt_r(base_offset)));
+		} else {
+			contact_points2.push_back(hit.mContactPointOn2);
+		}
+
+		const JoltShapedObject3D *collider = space->try_get_shaped(hit.mBodyID2);
+		ERR_FAIL_NULL_V(collider, false);
+
+		const int local_shape = p_body.find_shape_index(hit.mSubShapeID1);
+		ERR_FAIL_COND_V(local_shape == -1, false);
+
+		const int collider_shape = collider->find_shape_index(hit.mSubShapeID2);
+		ERR_FAIL_COND_V(collider_shape == -1, false);
+
+		for (JPH::Vec3 contact_point : contact_points2) {
+			const Vector3 position = base_offset + to_godot(contact_point);
+
+			PhysicsServer3D::MotionCollision &collision = p_result->collisions[count++];
+
+			collision.position = position;
+			collision.normal = normal;
+			collision.collider_velocity = collider->get_velocity_at_position(position);
+			collision.collider_angular_velocity = collider->get_angular_velocity();
+			collision.depth = penetration_depth;
+			collision.local_shape = local_shape;
+			collision.collider_id = collider->get_instance_id();
+			collision.collider = collider->get_rid();
+			collision.collider_shape = collider_shape;
+
+			if (count == p_max_collisions) {
+				break;
+			}
+		}
+
+		if (count == p_max_collisions) {
+			break;
+		}
+	}
+
+	p_result->collision_count = count;
+
+	return count > 0;
+}
+
+int JoltPhysicsDirectSpaceState3D::_try_get_face_index(const JPH::Body &p_body, const JPH::SubShapeID &p_sub_shape_id) {
+	if (!JoltProjectSettings::enable_ray_cast_face_index) {
+		return -1;
+	}
+
+	const JPH::Shape *root_shape = p_body.GetShape();
+	JPH::SubShapeID sub_shape_id_remainder;
+	const JPH::Shape *leaf_shape = root_shape->GetLeafShape(p_sub_shape_id, sub_shape_id_remainder);
+
+	if (leaf_shape->GetType() != JPH::EShapeType::Mesh) {
+		return -1;
+	}
+
+	const JPH::MeshShape *mesh_shape = static_cast<const JPH::MeshShape *>(leaf_shape);
+	return (int)mesh_shape->GetTriangleUserData(sub_shape_id_remainder);
+}
+
+void JoltPhysicsDirectSpaceState3D::_generate_manifold(const JPH::CollideShapeResult &p_hit, JPH::ContactPoints &r_contact_points1, JPH::ContactPoints &r_contact_points2 JPH_IF_DEBUG_RENDERER(, JPH::RVec3Arg p_center_of_mass)) const {
+	const JPH::PhysicsSystem &physics_system = space->get_physics_system();
+	const JPH::PhysicsSettings &physics_settings = physics_system.GetPhysicsSettings();
+	const JPH::Vec3 penetration_axis = p_hit.mPenetrationAxis.Normalized();
+
+	JPH::ManifoldBetweenTwoFaces(p_hit.mContactPointOn1, p_hit.mContactPointOn2, penetration_axis, physics_settings.mManifoldTolerance, p_hit.mShape1Face, p_hit.mShape2Face, r_contact_points1, r_contact_points2 JPH_IF_DEBUG_RENDERER(, p_center_of_mass));
+
+	if (r_contact_points1.size() > 4) {
+		JPH::PruneContactPoints(penetration_axis, r_contact_points1, r_contact_points2 JPH_IF_DEBUG_RENDERER(, p_center_of_mass));
+	}
+}
+
+void JoltPhysicsDirectSpaceState3D::_collide_shape_queries(
+		const JPH::Shape *p_shape,
+		JPH::Vec3Arg p_scale,
+		JPH::RMat44Arg p_transform_com,
+		const JPH::CollideShapeSettings &p_settings,
+		JPH::RVec3Arg p_base_offset,
+		JPH::CollideShapeCollector &p_collector,
+		const JPH::BroadPhaseLayerFilter &p_broad_phase_layer_filter,
+		const JPH::ObjectLayerFilter &p_object_layer_filter,
+		const JPH::BodyFilter &p_body_filter,
+		const JPH::ShapeFilter &p_shape_filter) const {
+	if (JoltProjectSettings::use_enhanced_internal_edge_removal_for_queries) {
+		space->get_narrow_phase_query().CollideShapeWithInternalEdgeRemoval(p_shape, p_scale, p_transform_com, p_settings, p_base_offset, p_collector, p_broad_phase_layer_filter, p_object_layer_filter, p_body_filter, p_shape_filter);
+	} else {
+		space->get_narrow_phase_query().CollideShape(p_shape, p_scale, p_transform_com, p_settings, p_base_offset, p_collector, p_broad_phase_layer_filter, p_object_layer_filter, p_body_filter, p_shape_filter);
+	}
+}
+
+void JoltPhysicsDirectSpaceState3D::_collide_shape_kinematics(
+		const JPH::Shape *p_shape,
+		JPH::Vec3Arg p_scale,
+		JPH::RMat44Arg p_transform_com,
+		const JPH::CollideShapeSettings &p_settings,
+		JPH::RVec3Arg p_base_offset,
+		JPH::CollideShapeCollector &p_collector,
+		const JPH::BroadPhaseLayerFilter &p_broad_phase_layer_filter,
+		const JPH::ObjectLayerFilter &p_object_layer_filter,
+		const JPH::BodyFilter &p_body_filter,
+		const JPH::ShapeFilter &p_shape_filter) const {
+	if (JoltProjectSettings::use_enhanced_internal_edge_removal_for_motion_queries) {
+		space->get_narrow_phase_query().CollideShapeWithInternalEdgeRemoval(p_shape, p_scale, p_transform_com, p_settings, p_base_offset, p_collector, p_broad_phase_layer_filter, p_object_layer_filter, p_body_filter, p_shape_filter);
+	} else {
+		space->get_narrow_phase_query().CollideShape(p_shape, p_scale, p_transform_com, p_settings, p_base_offset, p_collector, p_broad_phase_layer_filter, p_object_layer_filter, p_body_filter, p_shape_filter);
+	}
+}
+
+JoltPhysicsDirectSpaceState3D::JoltPhysicsDirectSpaceState3D(JoltSpace3D *p_space) :
+		space(p_space) {
+}
+
+bool JoltPhysicsDirectSpaceState3D::intersect_ray(const RayParameters &p_parameters, RayResult &r_result) {
+	ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "intersect_ray must not be called while the physics space is being stepped.");
+
+	space->flush_pending_objects();
+
+	const JoltQueryFilter3D query_filter(*this, p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas, p_parameters.exclude, p_parameters.pick_ray);
+
+	const JPH::RVec3 from = to_jolt_r(p_parameters.from);
+	const JPH::RVec3 to = to_jolt_r(p_parameters.to);
+	const JPH::Vec3 vector = JPH::Vec3(to - from);
+	const JPH::RRayCast ray(from, vector);
+
+	const JPH::EBackFaceMode back_face_mode = p_parameters.hit_back_faces ? JPH::EBackFaceMode::CollideWithBackFaces : JPH::EBackFaceMode::IgnoreBackFaces;
+
+	JPH::RayCastSettings settings;
+	settings.mTreatConvexAsSolid = p_parameters.hit_from_inside;
+	settings.mBackFaceModeTriangles = back_face_mode;
+
+	JoltQueryCollectorClosest<JPH::CastRayCollector> collector;
+	space->get_narrow_phase_query().CastRay(ray, settings, collector, query_filter, query_filter, query_filter);
+
+	if (!collector.had_hit()) {
+		return false;
+	}
+
+	const JPH::RayCastResult &hit = collector.get_hit();
+
+	const JPH::BodyID &body_id = hit.mBodyID;
+	const JPH::SubShapeID &sub_shape_id = hit.mSubShapeID2;
+
+	const JoltObject3D *object = space->try_get_object(body_id);
+	ERR_FAIL_NULL_V(object, false);
+
+	const JPH::RVec3 position = ray.GetPointOnRay(hit.mFraction);
+
+	JPH::Vec3 normal = JPH::Vec3::sZero();
+
+	if (!p_parameters.hit_from_inside || hit.mFraction > 0.0f) {
+		normal = object->get_jolt_body()->GetWorldSpaceSurfaceNormal(sub_shape_id, position);
+
+		// If we got a back-face normal we need to flip it.
+		if (normal.Dot(vector) > 0) {
+			normal = -normal;
+		}
+	}
+
+	r_result.position = to_godot(position);
+	r_result.normal = to_godot(normal);
+	r_result.rid = object->get_rid();
+	r_result.collider_id = object->get_instance_id();
+	r_result.collider = object->get_instance();
+	r_result.shape = 0;
+
+	if (const JoltShapedObject3D *shaped_object = object->as_shaped()) {
+		const int shape_index = shaped_object->find_shape_index(sub_shape_id);
+		ERR_FAIL_COND_V(shape_index == -1, false);
+		r_result.shape = shape_index;
+		r_result.face_index = _try_get_face_index(*object->get_jolt_body(), sub_shape_id);
+	}
+
+	return true;
+}
+
+int JoltPhysicsDirectSpaceState3D::intersect_point(const PointParameters &p_parameters, ShapeResult *r_results, int p_result_max) {
+	ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "intersect_point must not be called while the physics space is being stepped.");
+
+	if (p_result_max == 0) {
+		return 0;
+	}
+
+	space->flush_pending_objects();
+
+	const JoltQueryFilter3D query_filter(*this, p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas, p_parameters.exclude);
+	JoltQueryCollectorAnyMulti<JPH::CollidePointCollector, 32> collector(p_result_max);
+	space->get_narrow_phase_query().CollidePoint(to_jolt_r(p_parameters.position), collector, query_filter, query_filter, query_filter);
+
+	const int hit_count = collector.get_hit_count();
+
+	for (int i = 0; i < hit_count; ++i) {
+		const JPH::CollidePointResult &hit = collector.get_hit(i);
+		const JoltObject3D *object = space->try_get_object(hit.mBodyID);
+		ERR_FAIL_NULL_V(object, 0);
+
+		ShapeResult &result = *r_results++;
+
+		result.shape = 0;
+
+		if (const JoltShapedObject3D *shaped_object = object->as_shaped()) {
+			const int shape_index = shaped_object->find_shape_index(hit.mSubShapeID2);
+			ERR_FAIL_COND_V(shape_index == -1, 0);
+			result.shape = shape_index;
+		}
+
+		result.rid = object->get_rid();
+		result.collider_id = object->get_instance_id();
+		result.collider = object->get_instance();
+	}
+
+	return hit_count;
+}
+
+int JoltPhysicsDirectSpaceState3D::intersect_shape(const ShapeParameters &p_parameters, ShapeResult *r_results, int p_result_max) {
+	ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "intersect_shape must not be called while the physics space is being stepped.");
+
+	if (p_result_max == 0) {
+		return 0;
+	}
+
+	space->flush_pending_objects();
+
+	JoltShape3D *shape = JoltPhysicsServer3D::get_singleton()->get_shape(p_parameters.shape_rid);
+	ERR_FAIL_NULL_V(shape, 0);
+
+	const JPH::ShapeRefC jolt_shape = shape->try_build();
+	ERR_FAIL_NULL_V(jolt_shape, 0);
+
+	Transform3D transform = p_parameters.transform;
+	JOLT_ENSURE_SCALE_NOT_ZERO(transform, "intersect_shape was passed an invalid transform.");
+
+	Vector3 scale;
+	JoltMath::decompose(transform, scale);
+	JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "intersect_shape was passed an invalid transform.");
+
+	const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
+	const Transform3D transform_com = transform.translated_local(com_scaled);
+
+	JPH::CollideShapeSettings settings;
+	settings.mMaxSeparationDistance = (float)p_parameters.margin;
+
+	const JoltQueryFilter3D query_filter(*this, p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas, p_parameters.exclude);
+	JoltQueryCollectorAnyMulti<JPH::CollideShapeCollector, 32> collector(p_result_max);
+	_collide_shape_queries(jolt_shape, to_jolt(scale), to_jolt_r(transform_com), settings, to_jolt_r(transform_com.origin), collector, query_filter, query_filter, query_filter);
+
+	const int hit_count = collector.get_hit_count();
+
+	for (int i = 0; i < hit_count; ++i) {
+		const JPH::CollideShapeResult &hit = collector.get_hit(i);
+		const JoltObject3D *object = space->try_get_object(hit.mBodyID2);
+		ERR_FAIL_NULL_V(object, 0);
+
+		ShapeResult &result = *r_results++;
+
+		result.shape = 0;
+
+		if (const JoltShapedObject3D *shaped_object = object->as_shaped()) {
+			const int shape_index = shaped_object->find_shape_index(hit.mSubShapeID2);
+			ERR_FAIL_COND_V(shape_index == -1, 0);
+			result.shape = shape_index;
+		}
+
+		result.rid = object->get_rid();
+		result.collider_id = object->get_instance_id();
+		result.collider = object->get_instance();
+	}
+
+	return hit_count;
+}
+
+bool JoltPhysicsDirectSpaceState3D::cast_motion(const ShapeParameters &p_parameters, real_t &r_closest_safe, real_t &r_closest_unsafe, ShapeRestInfo *r_info) {
+	ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "cast_motion must not be called while the physics space is being stepped.");
+	ERR_FAIL_COND_V_MSG(r_info != nullptr, false, "Providing rest info as part of cast_motion is not supported when using Jolt Physics.");
+
+	space->flush_pending_objects();
+
+	JoltShape3D *shape = JoltPhysicsServer3D::get_singleton()->get_shape(p_parameters.shape_rid);
+	ERR_FAIL_NULL_V(shape, false);
+
+	const JPH::ShapeRefC jolt_shape = shape->try_build();
+	ERR_FAIL_NULL_V(jolt_shape, false);
+
+	Transform3D transform = p_parameters.transform;
+	JOLT_ENSURE_SCALE_NOT_ZERO(transform, "cast_motion (maybe from ShapeCast3D?) was passed an invalid transform.");
+
+	Vector3 scale;
+	JoltMath::decompose(transform, scale);
+	JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "cast_motion (maybe from ShapeCast3D?) was passed an invalid transform.");
+
+	const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
+	Transform3D transform_com = transform.translated_local(com_scaled);
+
+	JPH::CollideShapeSettings settings;
+	settings.mMaxSeparationDistance = (float)p_parameters.margin;
+
+	const JoltQueryFilter3D query_filter(*this, p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas, p_parameters.exclude);
+	_cast_motion_impl(*jolt_shape, transform_com, scale, p_parameters.motion, JoltProjectSettings::use_enhanced_internal_edge_removal_for_queries, true, settings, query_filter, query_filter, query_filter, JPH::ShapeFilter(), r_closest_safe, r_closest_unsafe);
+
+	return true;
+}
+
+bool JoltPhysicsDirectSpaceState3D::collide_shape(const ShapeParameters &p_parameters, Vector3 *r_results, int p_result_max, int &r_result_count) {
+	r_result_count = 0;
+
+	ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "collide_shape must not be called while the physics space is being stepped.");
+
+	if (p_result_max == 0) {
+		return false;
+	}
+
+	space->flush_pending_objects();
+
+	JoltShape3D *shape = JoltPhysicsServer3D::get_singleton()->get_shape(p_parameters.shape_rid);
+	ERR_FAIL_NULL_V(shape, false);
+
+	const JPH::ShapeRefC jolt_shape = shape->try_build();
+	ERR_FAIL_NULL_V(jolt_shape, false);
+
+	Transform3D transform = p_parameters.transform;
+	JOLT_ENSURE_SCALE_NOT_ZERO(transform, "collide_shape was passed an invalid transform.");
+
+	Vector3 scale;
+	JoltMath::decompose(transform, scale);
+	JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "collide_shape was passed an invalid transform.");
+
+	const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
+	const Transform3D transform_com = transform.translated_local(com_scaled);
+
+	JPH::CollideShapeSettings settings;
+	settings.mCollectFacesMode = JPH::ECollectFacesMode::CollectFaces;
+	settings.mMaxSeparationDistance = (float)p_parameters.margin;
+
+	const Vector3 &base_offset = transform_com.origin;
+
+	const JoltQueryFilter3D query_filter(*this, p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas, p_parameters.exclude);
+	JoltQueryCollectorAnyMulti<JPH::CollideShapeCollector, 32> collector(p_result_max);
+	_collide_shape_queries(jolt_shape, to_jolt(scale), to_jolt_r(transform_com), settings, to_jolt_r(base_offset), collector, query_filter, query_filter, query_filter);
+
+	if (!collector.had_hit()) {
+		return false;
+	}
+
+	const int max_points = p_result_max * 2;
+
+	int point_count = 0;
+
+	for (int i = 0; i < collector.get_hit_count(); ++i) {
+		const JPH::CollideShapeResult &hit = collector.get_hit(i);
+
+		const Vector3 penetration_axis = to_godot(hit.mPenetrationAxis.Normalized());
+		const Vector3 margin_offset = penetration_axis * (float)p_parameters.margin;
+
+		JPH::ContactPoints contact_points1;
+		JPH::ContactPoints contact_points2;
+
+		_generate_manifold(hit, contact_points1, contact_points2 JPH_IF_DEBUG_RENDERER(, to_jolt_r(base_offset)));
+
+		for (JPH::uint j = 0; j < contact_points1.size(); ++j) {
+			r_results[point_count++] = base_offset + to_godot(contact_points1[j]) + margin_offset;
+			r_results[point_count++] = base_offset + to_godot(contact_points2[j]);
+
+			if (point_count >= max_points) {
+				break;
+			}
+		}
+
+		if (point_count >= max_points) {
+			break;
+		}
+	}
+
+	r_result_count = point_count / 2;
+
+	return true;
+}
+
+bool JoltPhysicsDirectSpaceState3D::rest_info(const ShapeParameters &p_parameters, ShapeRestInfo *r_info) {
+	ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "get_rest_info must not be called while the physics space is being stepped.");
+
+	space->flush_pending_objects();
+
+	JoltShape3D *shape = JoltPhysicsServer3D::get_singleton()->get_shape(p_parameters.shape_rid);
+	ERR_FAIL_NULL_V(shape, false);
+
+	const JPH::ShapeRefC jolt_shape = shape->try_build();
+	ERR_FAIL_NULL_V(jolt_shape, false);
+
+	Transform3D transform = p_parameters.transform;
+	JOLT_ENSURE_SCALE_NOT_ZERO(transform, "get_rest_info (maybe from ShapeCast3D?) was passed an invalid transform.");
+
+	Vector3 scale;
+	JoltMath::decompose(transform, scale);
+	JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "get_rest_info (maybe from ShapeCast3D?) was passed an invalid transform.");
+
+	const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
+	const Transform3D transform_com = transform.translated_local(com_scaled);
+
+	JPH::CollideShapeSettings settings;
+	settings.mMaxSeparationDistance = (float)p_parameters.margin;
+
+	const Vector3 &base_offset = transform_com.origin;
+
+	const JoltQueryFilter3D query_filter(*this, p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas, p_parameters.exclude);
+	JoltQueryCollectorClosest<JPH::CollideShapeCollector> collector;
+	_collide_shape_queries(jolt_shape, to_jolt(scale), to_jolt_r(transform_com), settings, to_jolt_r(base_offset), collector, query_filter, query_filter, query_filter);
+
+	if (!collector.had_hit()) {
+		return false;
+	}
+
+	const JPH::CollideShapeResult &hit = collector.get_hit();
+	const JoltObject3D *object = space->try_get_object(hit.mBodyID2);
+	ERR_FAIL_NULL_V(object, false);
+
+	r_info->shape = 0;
+
+	if (const JoltShapedObject3D *shaped_object = object->as_shaped()) {
+		const int shape_index = shaped_object->find_shape_index(hit.mSubShapeID2);
+		ERR_FAIL_COND_V(shape_index == -1, false);
+		r_info->shape = shape_index;
+	}
+
+	const Vector3 hit_point = base_offset + to_godot(hit.mContactPointOn2);
+
+	r_info->point = hit_point;
+	r_info->normal = to_godot(-hit.mPenetrationAxis.Normalized());
+	r_info->rid = object->get_rid();
+	r_info->collider_id = object->get_instance_id();
+	r_info->linear_velocity = object->get_velocity_at_position(hit_point);
+
+	return true;
+}
+
+Vector3 JoltPhysicsDirectSpaceState3D::get_closest_point_to_object_volume(RID p_object, Vector3 p_point) const {
+	ERR_FAIL_COND_V_MSG(space->is_stepping(), Vector3(), "get_closest_point_to_object_volume must not be called while the physics space is being stepped.");
+
+	space->flush_pending_objects();
+
+	JoltPhysicsServer3D *physics_server = JoltPhysicsServer3D::get_singleton();
+	JoltObject3D *object = physics_server->get_area(p_object);
+
+	if (object == nullptr) {
+		object = physics_server->get_body(p_object);
+	}
+
+	ERR_FAIL_NULL_V(object, Vector3());
+	ERR_FAIL_COND_V(object->get_space() != space, Vector3());
+
+	JoltQueryCollectorAll<JPH::TransformedShapeCollector, 32> collector;
+	const JPH::TransformedShape root_shape = object->get_jolt_body()->GetTransformedShape();
+	root_shape.CollectTransformedShapes(object->get_jolt_body()->GetWorldSpaceBounds(), collector);
+
+	const JPH::RVec3 point = to_jolt_r(p_point);
+
+	float closest_distance_sq = FLT_MAX;
+	JPH::RVec3 closest_point = JPH::RVec3::sZero();
+
+	bool found_point = false;
+
+	for (int i = 0; i < collector.get_hit_count(); ++i) {
+		const JPH::TransformedShape &shape_transformed = collector.get_hit(i);
+		const JPH::Shape &shape = *shape_transformed.mShape;
+
+		if (shape.GetType() != JPH::EShapeType::Convex) {
+			continue;
+		}
+
+		const JPH::ConvexShape &shape_convex = static_cast<const JPH::ConvexShape &>(shape);
+
+		JPH::GJKClosestPoint gjk;
+
+		JPH::ConvexShape::SupportBuffer shape_support_buffer;
+		const JPH::ConvexShape::Support *shape_support = shape_convex.GetSupportFunction(JPH::ConvexShape::ESupportMode::IncludeConvexRadius, shape_support_buffer, shape_transformed.GetShapeScale());
+
+		const JPH::RMat44 shape_rotation = JPH::RMat44::sRotation(shape_transformed.mShapeRotation);
+		const JPH::Vec3 shape_com = shape_rotation.Multiply3x3(shape.GetCenterOfMass());
+		const JPH::RVec3 shape_pos = shape_transformed.mShapePositionCOM - JPH::RVec3(shape_com);
+		const JPH::RMat44 shape_xform = shape_rotation.PostTranslated(shape_pos);
+		const JPH::RMat44 shape_xform_inv = shape_xform.InversedRotationTranslation();
+
+		JPH::PointConvexSupport point_support;
+		point_support.mPoint = JPH::Vec3(shape_xform_inv * point);
+
+		JPH::Vec3 separating_axis = JPH::Vec3::sAxisX();
+		JPH::Vec3 point_on_a = JPH::Vec3::sZero();
+		JPH::Vec3 point_on_b = JPH::Vec3::sZero();
+
+		const float distance_sq = gjk.GetClosestPoints(*shape_support, point_support, JPH::cDefaultCollisionTolerance, FLT_MAX, separating_axis, point_on_a, point_on_b);
+
+		if (distance_sq == 0.0f) {
+			closest_point = point;
+			found_point = true;
+			break;
+		}
+
+		if (distance_sq < closest_distance_sq) {
+			closest_distance_sq = distance_sq;
+			closest_point = shape_xform * point_on_a;
+			found_point = true;
+		}
+	}
+
+	if (found_point) {
+		return to_godot(closest_point);
+	} else {
+		return to_godot(object->get_jolt_body()->GetPosition());
+	}
+}
+
+bool JoltPhysicsDirectSpaceState3D::body_test_motion(const JoltBody3D &p_body, const PhysicsServer3D::MotionParameters &p_parameters, PhysicsServer3D::MotionResult *r_result) const {
+	ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "body_test_motion (maybe from move_and_slide?) must not be called while the physics space is being stepped.");
+
+	space->flush_pending_objects();
+
+	const float margin = MAX((float)p_parameters.margin, 0.0001f);
+	const int max_collisions = MIN(p_parameters.max_collisions, 32);
+
+	Transform3D transform = p_parameters.from;
+	JOLT_ENSURE_SCALE_NOT_ZERO(transform, vformat("body_test_motion (maybe from move_and_slide?) was passed an invalid transform along with body '%s'.", p_body.to_string()));
+
+	Vector3 scale;
+	JoltMath::decompose(transform, scale);
+
+	Vector3 recovery;
+	const bool recovered = _body_motion_recover(p_body, transform, margin, p_parameters.exclude_bodies, p_parameters.exclude_objects, recovery);
+
+	transform.origin += recovery;
+
+	real_t safe_fraction = 1.0;
+	real_t unsafe_fraction = 1.0;
+
+	const bool hit = _body_motion_cast(p_body, transform, scale, p_parameters.motion, p_parameters.collide_separation_ray, p_parameters.exclude_bodies, p_parameters.exclude_objects, safe_fraction, unsafe_fraction);
+
+	bool collided = false;
+
+	if (hit || (recovered && p_parameters.recovery_as_collision)) {
+		collided = _body_motion_collide(p_body, transform.translated(p_parameters.motion * unsafe_fraction), p_parameters.motion, margin, max_collisions, p_parameters.exclude_bodies, p_parameters.exclude_objects, r_result);
+	}
+
+	if (r_result == nullptr) {
+		return collided;
+	}
+
+	if (collided) {
+		const PhysicsServer3D::MotionCollision &deepest = r_result->collisions[0];
+
+		r_result->travel = recovery + p_parameters.motion * safe_fraction;
+		r_result->remainder = p_parameters.motion - p_parameters.motion * safe_fraction;
+		r_result->collision_depth = deepest.depth;
+		r_result->collision_safe_fraction = safe_fraction;
+		r_result->collision_unsafe_fraction = unsafe_fraction;
+	} else {
+		r_result->travel = recovery + p_parameters.motion;
+		r_result->remainder = Vector3();
+		r_result->collision_depth = 0.0f;
+		r_result->collision_safe_fraction = 1.0f;
+		r_result->collision_unsafe_fraction = 1.0f;
+		r_result->collision_count = 0;
+	}
+
+	return collided;
+}

modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.h

@@ -0,0 +1,84 @@
+/**************************************************************************/
+/*  jolt_physics_direct_space_state_3d.h                                  */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* 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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "servers/physics_server_3d.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Body/Body.h"
+#include "Jolt/Physics/Body/BodyFilter.h"
+#include "Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h"
+#include "Jolt/Physics/Collision/ContactListener.h"
+#include "Jolt/Physics/Collision/ObjectLayer.h"
+#include "Jolt/Physics/Collision/Shape/Shape.h"
+#include "Jolt/Physics/Collision/ShapeFilter.h"
+
+class JoltBody3D;
+class JoltShape3D;
+class JoltSpace3D;
+
+class JoltPhysicsDirectSpaceState3D final : public PhysicsDirectSpaceState3D {
+	GDCLASS(JoltPhysicsDirectSpaceState3D, PhysicsDirectSpaceState3D)
+
+	JoltSpace3D *space = nullptr;
+
+	static void _bind_methods() {}
+
+	bool _cast_motion_impl(const JPH::Shape &p_jolt_shape, const Transform3D &p_transform_com, const Vector3 &p_scale, const Vector3 &p_motion, bool p_use_edge_removal, bool p_ignore_overlaps, const JPH::CollideShapeSettings &p_settings, const JPH::BroadPhaseLayerFilter &p_broad_phase_layer_filter, const JPH::ObjectLayerFilter &p_object_layer_filter, const JPH::BodyFilter &p_body_filter, const JPH::ShapeFilter &p_shape_filter, real_t &r_closest_safe, real_t &r_closest_unsafe) const;
+
+	bool _body_motion_recover(const JoltBody3D &p_body, const Transform3D &p_transform, float p_margin, const HashSet<RID> &p_excluded_bodies, const HashSet<ObjectID> &p_excluded_objects, Vector3 &r_recovery) const;
+	bool _body_motion_cast(const JoltBody3D &p_body, const Transform3D &p_transform, const Vector3 &p_scale, const Vector3 &p_motion, bool p_collide_separation_ray, const HashSet<RID> &p_excluded_bodies, const HashSet<ObjectID> &p_excluded_objects, real_t &r_safe_fraction, real_t &r_unsafe_fraction) const;
+	bool _body_motion_collide(const JoltBody3D &p_body, const Transform3D &p_transform, const Vector3 &p_motion, float p_margin, int p_max_collisions, const HashSet<RID> &p_excluded_bodies, const HashSet<ObjectID> &p_excluded_objects, PhysicsServer3D::MotionResult *r_result) const;
+
+	int _try_get_face_index(const JPH::Body &p_body, const JPH::SubShapeID &p_sub_shape_id);
+
+	void _generate_manifold(const JPH::CollideShapeResult &p_hit, JPH::ContactPoints &r_contact_points1, JPH::ContactPoints &r_contact_points2 JPH_IF_DEBUG_RENDERER(, JPH::RVec3Arg p_center_of_mass)) const;
+
+	void _collide_shape_queries(const JPH::Shape *p_shape, JPH::Vec3Arg p_scale, JPH::RMat44Arg p_transform_com, const JPH::CollideShapeSettings &p_settings, JPH::RVec3Arg p_base_offset, JPH::CollideShapeCollector &p_collector, const JPH::BroadPhaseLayerFilter &p_broad_phase_layer_filter = JPH::BroadPhaseLayerFilter(), const JPH::ObjectLayerFilter &p_object_layer_filter = JPH::ObjectLayerFilter(), const JPH::BodyFilter &p_body_filter = JPH::BodyFilter(), const JPH::ShapeFilter &p_shape_filter = JPH::ShapeFilter()) const;
+	void _collide_shape_kinematics(const JPH::Shape *p_shape, JPH::Vec3Arg p_scale, JPH::RMat44Arg p_transform_com, const JPH::CollideShapeSettings &p_settings, JPH::RVec3Arg p_base_offset, JPH::CollideShapeCollector &p_collector, const JPH::BroadPhaseLayerFilter &p_broad_phase_layer_filter = JPH::BroadPhaseLayerFilter(), const JPH::ObjectLayerFilter &p_object_layer_filter = JPH::ObjectLayerFilter(), const JPH::BodyFilter &p_body_filter = JPH::BodyFilter(), const JPH::ShapeFilter &p_shape_filter = JPH::ShapeFilter()) const;
+
+public:
+	JoltPhysicsDirectSpaceState3D() = default;
+	explicit JoltPhysicsDirectSpaceState3D(JoltSpace3D *p_space);
+
+	virtual bool intersect_ray(const RayParameters &p_parameters, RayResult &r_result) override;
+	virtual int intersect_point(const PointParameters &p_parameters, ShapeResult *r_results, int p_result_max) override;
+	virtual int intersect_shape(const ShapeParameters &p_parameters, ShapeResult *r_results, int p_result_max) override;
+	virtual bool cast_motion(const ShapeParameters &p_parameters, real_t &r_closest_safe, real_t &r_closest_unsafe, ShapeRestInfo *r_info = nullptr) override;
+	virtual bool collide_shape(const ShapeParameters &p_parameters, Vector3 *r_results, int p_result_max, int &r_result_count) override;
+	virtual bool rest_info(const ShapeParameters &p_parameters, ShapeRestInfo *r_info) override;
+	virtual Vector3 get_closest_point_to_object_volume(RID p_object, Vector3 p_point) const override;
+
+	bool body_test_motion(const JoltBody3D &p_body, const PhysicsServer3D::MotionParameters &p_parameters, PhysicsServer3D::MotionResult *r_result) const;
+
+	JoltSpace3D &get_space() const { return *space; }
+};

modules/jolt_physics/spaces/jolt_query_collectors.h

@@ -0,0 +1,245 @@
+/**************************************************************************/
+/*  jolt_query_collectors.h                                               */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* 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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "../jolt_project_settings.h"
+#include "jolt_space_3d.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Core/STLLocalAllocator.h"
+#include "Jolt/Physics/Collision/InternalEdgeRemovingCollector.h"
+#include "Jolt/Physics/Collision/Shape/Shape.h"
+
+template <typename TBase, int TDefaultCapacity>
+class JoltQueryCollectorAll final : public TBase {
+public:
+	typedef typename TBase::ResultType Hit;
+	typedef JPH::Array<Hit, JPH::STLLocalAllocator<Hit, TDefaultCapacity>> HitArray;
+
+private:
+	HitArray hits;
+
+public:
+	JoltQueryCollectorAll() {
+		hits.reserve(TDefaultCapacity);
+	}
+
+	bool had_hit() const {
+		return !hits.is_empty();
+	}
+
+	int get_hit_count() const {
+		return hits.size();
+	}
+
+	const Hit &get_hit(int p_index) const {
+		return hits[p_index];
+	}
+
+	void reset() { Reset(); }
+
+	virtual void Reset() override {
+		TBase::Reset();
+		hits.clear();
+	}
+
+	virtual void AddHit(const Hit &p_hit) override {
+		hits.push_back(p_hit);
+	}
+};
+
+template <typename TBase>
+class JoltQueryCollectorAny final : public TBase {
+public:
+	typedef typename TBase::ResultType Hit;
+
+private:
+	Hit hit;
+	bool valid = false;
+
+public:
+	bool had_hit() const { return valid; }
+
+	const Hit &get_hit() const { return hit; }
+
+	void reset() {
+		Reset();
+	}
+
+	virtual void Reset() override {
+		TBase::Reset();
+		valid = false;
+	}
+
+	virtual void AddHit(const Hit &p_hit) override {
+		hit = p_hit;
+		valid = true;
+
+		TBase::ForceEarlyOut();
+	}
+};
+
+template <typename TBase, int TDefaultCapacity>
+class JoltQueryCollectorAnyMulti final : public TBase {
+public:
+	typedef typename TBase::ResultType Hit;
+	typedef JPH::Array<Hit, JPH::STLLocalAllocator<Hit, TDefaultCapacity>> HitArray;
+
+private:
+	HitArray hits;
+	int max_hits = 0;
+
+public:
+	explicit JoltQueryCollectorAnyMulti(int p_max_hits = TDefaultCapacity) :
+			max_hits(p_max_hits) {
+		hits.reserve(TDefaultCapacity);
+	}
+
+	bool had_hit() const {
+		return hits.size() > 0;
+	}
+
+	int get_hit_count() const {
+		return hits.size();
+	}
+
+	const Hit &get_hit(int p_index) const {
+		return hits[p_index];
+	}
+
+	void reset() {
+		Reset();
+	}
+
+	virtual void Reset() override {
+		TBase::Reset();
+		hits.clear();
+	}
+
+	virtual void AddHit(const Hit &p_hit) override {
+		if ((int)hits.size() < max_hits) {
+			hits.push_back(p_hit);
+		}
+
+		if ((int)hits.size() == max_hits) {
+			TBase::ForceEarlyOut();
+		}
+	}
+};
+
+template <typename TBase>
+class JoltQueryCollectorClosest final : public TBase {
+public:
+	typedef typename TBase::ResultType Hit;
+
+private:
+	Hit hit;
+	bool valid = false;
+
+public:
+	bool had_hit() const { return valid; }
+
+	const Hit &get_hit() const { return hit; }
+
+	void reset() {
+		Reset();
+	}
+
+	virtual void Reset() override {
+		TBase::Reset();
+		valid = false;
+	}
+
+	virtual void AddHit(const Hit &p_hit) override {
+		const float early_out = p_hit.GetEarlyOutFraction();
+
+		if (!valid || early_out < hit.GetEarlyOutFraction()) {
+			TBase::UpdateEarlyOutFraction(early_out);
+
+			hit = p_hit;
+			valid = true;
+		}
+	}
+};
+
+template <typename TBase, int TDefaultCapacity>
+class JoltQueryCollectorClosestMulti final : public TBase {
+public:
+	typedef typename TBase::ResultType Hit;
+	typedef JPH::Array<Hit, JPH::STLLocalAllocator<Hit, TDefaultCapacity + 1>> HitArray;
+
+private:
+	HitArray hits;
+	int max_hits = 0;
+
+public:
+	explicit JoltQueryCollectorClosestMulti(int p_max_hits = TDefaultCapacity) :
+			max_hits(p_max_hits) {
+		hits.reserve(TDefaultCapacity + 1);
+	}
+
+	bool had_hit() const {
+		return hits.size() > 0;
+	}
+
+	int get_hit_count() const {
+		return hits.size();
+	}
+
+	const Hit &get_hit(int p_index) const {
+		return hits[p_index];
+	}
+
+	void reset() {
+		Reset();
+	}
+
+	virtual void Reset() override {
+		TBase::Reset();
+		hits.clear();
+	}
+
+	virtual void AddHit(const Hit &p_hit) override {
+		typename HitArray::const_iterator E = hits.cbegin();
+		for (; E != hits.cend(); ++E) {
+			if (p_hit.GetEarlyOutFraction() < E->GetEarlyOutFraction()) {
+				break;
+			}
+		}
+
+		hits.insert(E, p_hit);
+
+		if ((int)hits.size() > max_hits) {
+			hits.resize(max_hits);
+		}
+	}
+};

modules/jolt_physics/spaces/jolt_query_filter_3d.cpp

@@ -0,0 +1,83 @@
+/**************************************************************************/
+/*  jolt_query_filter_3d.cpp                                              */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* 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 "jolt_query_filter_3d.h"
+
+#include "../objects/jolt_object_3d.h"
+#include "jolt_broad_phase_layer.h"
+#include "jolt_physics_direct_space_state_3d.h"
+#include "jolt_space_3d.h"
+
+JoltQueryFilter3D::JoltQueryFilter3D(const JoltPhysicsDirectSpaceState3D &p_space_state, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, const HashSet<RID> &p_excluded, bool p_picking) :
+		space(p_space_state.get_space()),
+		excluded(p_excluded),
+		collision_mask(p_collision_mask),
+		collide_with_bodies(p_collide_with_bodies),
+		collide_with_areas(p_collide_with_areas),
+		picking(p_picking) {
+}
+
+bool JoltQueryFilter3D::ShouldCollide(JPH::BroadPhaseLayer p_broad_phase_layer) const {
+	const JPH::BroadPhaseLayer::Type broad_phase_layer = (JPH::BroadPhaseLayer::Type)p_broad_phase_layer;
+
+	switch (broad_phase_layer) {
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::BODY_STATIC:
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::BODY_STATIC_BIG:
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::BODY_DYNAMIC: {
+			return collide_with_bodies;
+		} break;
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::AREA_DETECTABLE:
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::AREA_UNDETECTABLE: {
+			return collide_with_areas;
+		} break;
+		default: {
+			ERR_FAIL_V_MSG(false, vformat("Unhandled broad phase layer: '%d'. This should not happen. Please report this.", broad_phase_layer));
+		}
+	}
+}
+
+bool JoltQueryFilter3D::ShouldCollide(JPH::ObjectLayer p_object_layer) const {
+	JPH::BroadPhaseLayer object_broad_phase_layer = JoltBroadPhaseLayer::BODY_STATIC;
+	uint32_t object_collision_layer = 0;
+	uint32_t object_collision_mask = 0;
+
+	space.map_from_object_layer(p_object_layer, object_broad_phase_layer, object_collision_layer, object_collision_mask);
+
+	return (collision_mask & object_collision_layer) != 0;
+}
+
+bool JoltQueryFilter3D::ShouldCollide(const JPH::BodyID &p_body_id) const {
+	return true;
+}
+
+bool JoltQueryFilter3D::ShouldCollideLocked(const JPH::Body &p_body) const {
+	JoltObject3D *object = reinterpret_cast<JoltObject3D *>(p_body.GetUserData());
+	return (!picking || object->is_pickable()) && !excluded.has(object->get_rid());
+}

modules/jolt_physics/spaces/jolt_query_filter_3d.h

@@ -0,0 +1,64 @@
+/**************************************************************************/
+/*  jolt_query_filter_3d.h                                                */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* 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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/templates/hash_set.h"
+#include "core/templates/rid.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Body/Body.h"
+#include "Jolt/Physics/Body/BodyFilter.h"
+#include "Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h"
+#include "Jolt/Physics/Collision/ObjectLayer.h"
+
+class JoltPhysicsDirectSpaceState3D;
+class JoltSpace3D;
+
+class JoltQueryFilter3D final
+		: public JPH::BroadPhaseLayerFilter,
+		  public JPH::ObjectLayerFilter,
+		  public JPH::BodyFilter {
+	const JoltSpace3D &space;
+	const HashSet<RID> &excluded;
+	uint32_t collision_mask = 0;
+	bool collide_with_bodies = false;
+	bool collide_with_areas = false;
+	bool picking = false;
+
+public:
+	JoltQueryFilter3D(const JoltPhysicsDirectSpaceState3D &p_space_state, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, const HashSet<RID> &p_excluded, bool p_picking = false);
+
+	virtual bool ShouldCollide(JPH::BroadPhaseLayer p_broad_phase_layer) const override;
+	virtual bool ShouldCollide(JPH::ObjectLayer p_object_layer) const override;
+	virtual bool ShouldCollide(const JPH::BodyID &p_body_id) const override;
+	virtual bool ShouldCollideLocked(const JPH::Body &p_body) const override;
+};

modules/jolt_physics/spaces/jolt_space_3d.cpp

@@ -0,0 +1,615 @@
+/**************************************************************************/
+/*  jolt_space_3d.cpp                                                     */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* 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 "jolt_space_3d.h"
+
+#include "../joints/jolt_joint_3d.h"
+#include "../jolt_physics_server_3d.h"
+#include "../jolt_project_settings.h"
+#include "../misc/jolt_stream_wrappers.h"
+#include "../objects/jolt_area_3d.h"
+#include "../objects/jolt_body_3d.h"
+#include "../shapes/jolt_custom_shape_type.h"
+#include "../shapes/jolt_shape_3d.h"
+#include "jolt_body_activation_listener_3d.h"
+#include "jolt_contact_listener_3d.h"
+#include "jolt_layers.h"
+#include "jolt_physics_direct_space_state_3d.h"
+#include "jolt_temp_allocator.h"
+
+#include "core/io/file_access.h"
+#include "core/os/time.h"
+#include "core/string/print_string.h"
+#include "core/variant/variant_utility.h"
+
+#include "Jolt/Physics/Collision/CollideShapeVsShapePerLeaf.h"
+#include "Jolt/Physics/Collision/CollisionCollectorImpl.h"
+#include "Jolt/Physics/PhysicsScene.h"
+
+namespace {
+
+constexpr double SPACE_DEFAULT_CONTACT_RECYCLE_RADIUS = 0.01;
+constexpr double SPACE_DEFAULT_CONTACT_MAX_SEPARATION = 0.05;
+constexpr double SPACE_DEFAULT_CONTACT_MAX_ALLOWED_PENETRATION = 0.01;
+constexpr double SPACE_DEFAULT_CONTACT_DEFAULT_BIAS = 0.8;
+constexpr double SPACE_DEFAULT_SLEEP_THRESHOLD_LINEAR = 0.1;
+constexpr double SPACE_DEFAULT_SLEEP_THRESHOLD_ANGULAR = 8.0 * Math::PI / 180;
+constexpr double SPACE_DEFAULT_SOLVER_ITERATIONS = 8;
+
+} // namespace
+
+void JoltSpace3D::_pre_step(float p_step) {
+	flush_pending_objects();
+
+	while (needs_optimization_list.first()) {
+		JoltShapedObject3D *object = needs_optimization_list.first()->self();
+		needs_optimization_list.remove(needs_optimization_list.first());
+		object->commit_shapes(true);
+	}
+
+	contact_listener->pre_step();
+
+	const JPH::BodyLockInterface &lock_iface = get_lock_iface();
+	const JPH::BodyID *active_rigid_bodies = physics_system->GetActiveBodiesUnsafe(JPH::EBodyType::RigidBody);
+	const JPH::uint32 active_rigid_body_count = physics_system->GetNumActiveBodies(JPH::EBodyType::RigidBody);
+
+	for (JPH::uint32 i = 0; i < active_rigid_body_count; i++) {
+		JPH::Body *jolt_body = lock_iface.TryGetBody(active_rigid_bodies[i]);
+		JoltObject3D *object = reinterpret_cast<JoltObject3D *>(jolt_body->GetUserData());
+		object->pre_step(p_step, *jolt_body);
+	}
+}
+
+void JoltSpace3D::_post_step(float p_step) {
+	contact_listener->post_step();
+
+	while (shapes_changed_list.first()) {
+		JoltShapedObject3D *object = shapes_changed_list.first()->self();
+		shapes_changed_list.remove(shapes_changed_list.first());
+		object->clear_previous_shape();
+	}
+}
+
+JoltSpace3D::JoltSpace3D(JPH::JobSystem *p_job_system) :
+		job_system(p_job_system),
+		temp_allocator(new JoltTempAllocator()),
+		layers(new JoltLayers()),
+		contact_listener(new JoltContactListener3D(this)),
+		body_activation_listener(new JoltBodyActivationListener3D()),
+		physics_system(new JPH::PhysicsSystem()) {
+	physics_system->Init((JPH::uint)JoltProjectSettings::max_bodies, 0, (JPH::uint)JoltProjectSettings::max_body_pairs, (JPH::uint)JoltProjectSettings::max_contact_constraints, *layers, *layers, *layers);
+
+	JPH::PhysicsSettings settings;
+	settings.mBaumgarte = JoltProjectSettings::baumgarte_stabilization_factor;
+	settings.mSpeculativeContactDistance = JoltProjectSettings::speculative_contact_distance;
+	settings.mPenetrationSlop = JoltProjectSettings::penetration_slop;
+	settings.mLinearCastThreshold = JoltProjectSettings::ccd_movement_threshold;
+	settings.mLinearCastMaxPenetration = JoltProjectSettings::ccd_max_penetration;
+	settings.mBodyPairCacheMaxDeltaPositionSq = JoltProjectSettings::body_pair_cache_distance_sq;
+	settings.mBodyPairCacheCosMaxDeltaRotationDiv2 = JoltProjectSettings::body_pair_cache_angle_cos_div2;
+	settings.mNumVelocitySteps = (JPH::uint)JoltProjectSettings::simulation_velocity_steps;
+	settings.mNumPositionSteps = (JPH::uint)JoltProjectSettings::simulation_position_steps;
+	settings.mMinVelocityForRestitution = JoltProjectSettings::bounce_velocity_threshold;
+	settings.mTimeBeforeSleep = JoltProjectSettings::sleep_time_threshold;
+	settings.mPointVelocitySleepThreshold = JoltProjectSettings::sleep_velocity_threshold;
+	settings.mUseBodyPairContactCache = JoltProjectSettings::body_pair_contact_cache_enabled;
+	settings.mAllowSleeping = JoltProjectSettings::sleep_allowed;
+
+	physics_system->SetPhysicsSettings(settings);
+	physics_system->SetGravity(JPH::Vec3::sZero());
+	physics_system->SetContactListener(contact_listener);
+	physics_system->SetSoftBodyContactListener(contact_listener);
+	physics_system->SetBodyActivationListener(body_activation_listener);
+
+	physics_system->SetSimCollideBodyVsBody([](const JPH::Body &p_body1, const JPH::Body &p_body2, JPH::Mat44Arg p_transform_com1, JPH::Mat44Arg p_transform_com2, JPH::CollideShapeSettings &p_collide_shape_settings, JPH::CollideShapeCollector &p_collector, const JPH::ShapeFilter &p_shape_filter) {
+		if (p_body1.IsSensor() || p_body2.IsSensor()) {
+			JPH::CollideShapeSettings new_collide_shape_settings = p_collide_shape_settings;
+			// Since we're breaking the sensor down into leaf shapes we'll end up stripping away our `JoltCustomDoubleSidedShape` decorator shape and thus any back-face collision, so we simply force-enable it like this rather than going through the trouble of reapplying the decorator.
+			new_collide_shape_settings.mBackFaceMode = JPH::EBackFaceMode::CollideWithBackFaces;
+			JPH::SubShapeIDCreator part1, part2;
+			JPH::CollideShapeVsShapePerLeaf<JPH::AnyHitCollisionCollector<JPH::CollideShapeCollector>>(p_body1.GetShape(), p_body2.GetShape(), JPH::Vec3::sOne(), JPH::Vec3::sOne(), p_transform_com1, p_transform_com2, part1, part2, new_collide_shape_settings, p_collector, p_shape_filter);
+		} else {
+			JPH::PhysicsSystem::sDefaultSimCollideBodyVsBody(p_body1, p_body2, p_transform_com1, p_transform_com2, p_collide_shape_settings, p_collector, p_shape_filter);
+		}
+	});
+
+	physics_system->SetCombineFriction([](const JPH::Body &p_body1, const JPH::SubShapeID &p_sub_shape_id1, const JPH::Body &p_body2, const JPH::SubShapeID &p_sub_shape_id2) {
+		return Math::abs(MIN(p_body1.GetFriction(), p_body2.GetFriction()));
+	});
+
+	physics_system->SetCombineRestitution([](const JPH::Body &p_body1, const JPH::SubShapeID &p_sub_shape_id1, const JPH::Body &p_body2, const JPH::SubShapeID &p_sub_shape_id2) {
+		return CLAMP(p_body1.GetRestitution() + p_body2.GetRestitution(), 0.0f, 1.0f);
+	});
+}
+
+JoltSpace3D::~JoltSpace3D() {
+	if (direct_state != nullptr) {
+		memdelete(direct_state);
+		direct_state = nullptr;
+	}
+
+	if (physics_system != nullptr) {
+		delete physics_system;
+		physics_system = nullptr;
+	}
+
+	if (body_activation_listener != nullptr) {
+		delete body_activation_listener;
+		body_activation_listener = nullptr;
+	}
+
+	if (contact_listener != nullptr) {
+		delete contact_listener;
+		contact_listener = nullptr;
+	}
+
+	if (layers != nullptr) {
+		delete layers;
+		layers = nullptr;
+	}
+
+	if (temp_allocator != nullptr) {
+		delete temp_allocator;
+		temp_allocator = nullptr;
+	}
+}
+
+void JoltSpace3D::step(float p_step) {
+	stepping = true;
+	last_step = p_step;
+
+	_pre_step(p_step);
+
+	const JPH::EPhysicsUpdateError update_error = physics_system->Update(p_step, 1, temp_allocator, job_system);
+
+	if ((update_error & JPH::EPhysicsUpdateError::ManifoldCacheFull) != JPH::EPhysicsUpdateError::None) {
+		WARN_PRINT_ONCE(vformat("Jolt Physics manifold cache exceeded capacity and contacts were ignored. "
+								"Consider increasing maximum number of contact constraints in project settings. "
+								"Maximum number of contact constraints is currently set to %d.",
+				JoltProjectSettings::max_contact_constraints));
+	}
+
+	if ((update_error & JPH::EPhysicsUpdateError::BodyPairCacheFull) != JPH::EPhysicsUpdateError::None) {
+		WARN_PRINT_ONCE(vformat("Jolt Physics body pair cache exceeded capacity and contacts were ignored. "
+								"Consider increasing maximum number of body pairs in project settings. "
+								"Maximum number of body pairs is currently set to %d.",
+				JoltProjectSettings::max_body_pairs));
+	}
+
+	if ((update_error & JPH::EPhysicsUpdateError::ContactConstraintsFull) != JPH::EPhysicsUpdateError::None) {
+		WARN_PRINT_ONCE(vformat("Jolt Physics contact constraint buffer exceeded capacity and contacts were ignored. "
+								"Consider increasing maximum number of contact constraints in project settings. "
+								"Maximum number of contact constraints is currently set to %d.",
+				JoltProjectSettings::max_contact_constraints));
+	}
+
+	_post_step(p_step);
+
+	stepping = false;
+}
+
+void JoltSpace3D::call_queries() {
+	while (body_call_queries_list.first()) {
+		JoltBody3D *body = body_call_queries_list.first()->self();
+		body_call_queries_list.remove(body_call_queries_list.first());
+		body->call_queries();
+	}
+
+	while (area_call_queries_list.first()) {
+		JoltArea3D *body = area_call_queries_list.first()->self();
+		area_call_queries_list.remove(area_call_queries_list.first());
+		body->call_queries();
+	}
+}
+
+double JoltSpace3D::get_param(PhysicsServer3D::SpaceParameter p_param) const {
+	switch (p_param) {
+		case PhysicsServer3D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: {
+			return SPACE_DEFAULT_CONTACT_RECYCLE_RADIUS;
+		}
+		case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_SEPARATION: {
+			return SPACE_DEFAULT_CONTACT_MAX_SEPARATION;
+		}
+		case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION: {
+			return SPACE_DEFAULT_CONTACT_MAX_ALLOWED_PENETRATION;
+		}
+		case PhysicsServer3D::SPACE_PARAM_CONTACT_DEFAULT_BIAS: {
+			return SPACE_DEFAULT_CONTACT_DEFAULT_BIAS;
+		}
+		case PhysicsServer3D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: {
+			return SPACE_DEFAULT_SLEEP_THRESHOLD_LINEAR;
+		}
+		case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: {
+			return SPACE_DEFAULT_SLEEP_THRESHOLD_ANGULAR;
+		}
+		case PhysicsServer3D::SPACE_PARAM_BODY_TIME_TO_SLEEP: {
+			return JoltProjectSettings::sleep_time_threshold;
+		}
+		case PhysicsServer3D::SPACE_PARAM_SOLVER_ITERATIONS: {
+			return SPACE_DEFAULT_SOLVER_ITERATIONS;
+		}
+		default: {
+			ERR_FAIL_V_MSG(0.0, vformat("Unhandled space parameter: '%d'. This should not happen. Please report this.", p_param));
+		}
+	}
+}
+
+void JoltSpace3D::set_param(PhysicsServer3D::SpaceParameter p_param, double p_value) {
+	switch (p_param) {
+		case PhysicsServer3D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: {
+			WARN_PRINT("Space-specific contact recycle radius is not supported when using Jolt Physics. Any such value will be ignored.");
+		} break;
+		case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_SEPARATION: {
+			WARN_PRINT("Space-specific contact max separation is not supported when using Jolt Physics. Any such value will be ignored.");
+		} break;
+		case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION: {
+			WARN_PRINT("Space-specific contact max allowed penetration is not supported when using Jolt Physics. Any such value will be ignored.");
+		} break;
+		case PhysicsServer3D::SPACE_PARAM_CONTACT_DEFAULT_BIAS: {
+			WARN_PRINT("Space-specific contact default bias is not supported when using Jolt Physics. Any such value will be ignored.");
+		} break;
+		case PhysicsServer3D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: {
+			WARN_PRINT("Space-specific linear velocity sleep threshold is not supported when using Jolt Physics. Any such value will be ignored.");
+		} break;
+		case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: {
+			WARN_PRINT("Space-specific angular velocity sleep threshold is not supported when using Jolt Physics. Any such value will be ignored.");
+		} break;
+		case PhysicsServer3D::SPACE_PARAM_BODY_TIME_TO_SLEEP: {
+			WARN_PRINT("Space-specific body sleep time is not supported when using Jolt Physics. Any such value will be ignored.");
+		} break;
+		case PhysicsServer3D::SPACE_PARAM_SOLVER_ITERATIONS: {
+			WARN_PRINT("Space-specific solver iterations is not supported when using Jolt Physics. Any such value will be ignored.");
+		} break;
+		default: {
+			ERR_FAIL_MSG(vformat("Unhandled space parameter: '%d'. This should not happen. Please report this.", p_param));
+		} break;
+	}
+}
+
+JPH::BodyInterface &JoltSpace3D::get_body_iface() {
+	return physics_system->GetBodyInterfaceNoLock();
+}
+
+const JPH::BodyInterface &JoltSpace3D::get_body_iface() const {
+	return physics_system->GetBodyInterfaceNoLock();
+}
+
+const JPH::BodyLockInterface &JoltSpace3D::get_lock_iface() const {
+	return physics_system->GetBodyLockInterfaceNoLock();
+}
+
+const JPH::BroadPhaseQuery &JoltSpace3D::get_broad_phase_query() const {
+	return physics_system->GetBroadPhaseQuery();
+}
+
+const JPH::NarrowPhaseQuery &JoltSpace3D::get_narrow_phase_query() const {
+	return physics_system->GetNarrowPhaseQueryNoLock();
+}
+
+JPH::ObjectLayer JoltSpace3D::map_to_object_layer(JPH::BroadPhaseLayer p_broad_phase_layer, uint32_t p_collision_layer, uint32_t p_collision_mask) {
+	return layers->to_object_layer(p_broad_phase_layer, p_collision_layer, p_collision_mask);
+}
+
+void JoltSpace3D::map_from_object_layer(JPH::ObjectLayer p_object_layer, JPH::BroadPhaseLayer &r_broad_phase_layer, uint32_t &r_collision_layer, uint32_t &r_collision_mask) const {
+	layers->from_object_layer(p_object_layer, r_broad_phase_layer, r_collision_layer, r_collision_mask);
+}
+
+JPH::Body *JoltSpace3D::try_get_jolt_body(const JPH::BodyID &p_body_id) const {
+	return get_lock_iface().TryGetBody(p_body_id);
+}
+
+JoltObject3D *JoltSpace3D::try_get_object(const JPH::BodyID &p_body_id) const {
+	const JPH::Body *jolt_body = try_get_jolt_body(p_body_id);
+	if (unlikely(jolt_body == nullptr)) {
+		return nullptr;
+	}
+
+	return reinterpret_cast<JoltObject3D *>(jolt_body->GetUserData());
+}
+
+JoltShapedObject3D *JoltSpace3D::try_get_shaped(const JPH::BodyID &p_body_id) const {
+	JoltObject3D *object = try_get_object(p_body_id);
+	if (unlikely(object == nullptr)) {
+		return nullptr;
+	}
+
+	return object->as_shaped();
+}
+
+JoltBody3D *JoltSpace3D::try_get_body(const JPH::BodyID &p_body_id) const {
+	JoltObject3D *object = try_get_object(p_body_id);
+	if (unlikely(object == nullptr)) {
+		return nullptr;
+	}
+
+	return object->as_body();
+}
+
+JoltArea3D *JoltSpace3D::try_get_area(const JPH::BodyID &p_body_id) const {
+	JoltObject3D *object = try_get_object(p_body_id);
+	if (unlikely(object == nullptr)) {
+		return nullptr;
+	}
+
+	return object->as_area();
+}
+
+JoltSoftBody3D *JoltSpace3D::try_get_soft_body(const JPH::BodyID &p_body_id) const {
+	JoltObject3D *object = try_get_object(p_body_id);
+	if (unlikely(object == nullptr)) {
+		return nullptr;
+	}
+
+	return object->as_soft_body();
+}
+
+JoltPhysicsDirectSpaceState3D *JoltSpace3D::get_direct_state() {
+	if (direct_state == nullptr) {
+		direct_state = memnew(JoltPhysicsDirectSpaceState3D(this));
+	}
+
+	return direct_state;
+}
+
+void JoltSpace3D::set_default_area(JoltArea3D *p_area) {
+	if (default_area == p_area) {
+		return;
+	}
+
+	if (default_area != nullptr) {
+		default_area->set_default_area(false);
+	}
+
+	default_area = p_area;
+
+	if (default_area != nullptr) {
+		default_area->set_default_area(true);
+	}
+}
+
+JPH::Body *JoltSpace3D::add_object(const JoltObject3D &p_object, const JPH::BodyCreationSettings &p_settings, bool p_sleeping) {
+	JPH::BodyInterface &body_iface = get_body_iface();
+	JPH::Body *jolt_body = body_iface.CreateBody(p_settings);
+	if (unlikely(jolt_body == nullptr)) {
+		ERR_PRINT_ONCE(vformat("Failed to create underlying Jolt Physics body for '%s'. "
+							   "Consider increasing maximum number of bodies in project settings. "
+							   "Maximum number of bodies is currently set to %d.",
+				p_object.to_string(), JoltProjectSettings::max_bodies));
+
+		return nullptr;
+	}
+
+	if (p_sleeping) {
+		pending_objects_sleeping.push_back(jolt_body->GetID());
+	} else {
+		pending_objects_awake.push_back(jolt_body->GetID());
+	}
+
+	return jolt_body;
+}
+
+JPH::Body *JoltSpace3D::add_object(const JoltObject3D &p_object, const JPH::SoftBodyCreationSettings &p_settings, bool p_sleeping) {
+	JPH::BodyInterface &body_iface = get_body_iface();
+	JPH::Body *jolt_body = body_iface.CreateSoftBody(p_settings);
+	if (unlikely(jolt_body == nullptr)) {
+		ERR_PRINT_ONCE(vformat("Failed to create underlying Jolt Physics body for '%s'. "
+							   "Consider increasing maximum number of bodies in project settings. "
+							   "Maximum number of bodies is currently set to %d.",
+				p_object.to_string(), JoltProjectSettings::max_bodies));
+
+		return nullptr;
+	}
+
+	if (p_sleeping) {
+		pending_objects_sleeping.push_back(jolt_body->GetID());
+	} else {
+		pending_objects_awake.push_back(jolt_body->GetID());
+	}
+
+	return jolt_body;
+}
+
+void JoltSpace3D::remove_object(const JPH::BodyID &p_jolt_id) {
+	JPH::BodyInterface &body_iface = get_body_iface();
+
+	if (!pending_objects_sleeping.erase_unordered(p_jolt_id) && !pending_objects_awake.erase_unordered(p_jolt_id)) {
+		body_iface.RemoveBody(p_jolt_id);
+	}
+
+	body_iface.DestroyBody(p_jolt_id);
+
+	// If we're never going to step this space, like in the editor viewport, we need to manually clean up Jolt's broad phase instead, otherwise performance can degrade when doing things like switching scenes.
+	// We'll never actually have zero bodies in any space though, since we always have the default area, so we check if there's one or fewer left instead.
+	if (!JoltPhysicsServer3D::get_singleton()->is_active() && physics_system->GetNumBodies() <= 1) {
+		physics_system->OptimizeBroadPhase();
+	}
+}
+
+void JoltSpace3D::flush_pending_objects() {
+	if (pending_objects_sleeping.is_empty() && pending_objects_awake.is_empty()) {
+		return;
+	}
+
+	// We only care about locking within this method, because it's called when performing queries, which aren't covered by `PhysicsServer3DWrapMT`.
+	MutexLock pending_objects_lock(pending_objects_mutex);
+
+	JPH::BodyInterface &body_iface = get_body_iface();
+
+	if (!pending_objects_sleeping.is_empty()) {
+		JPH::BodyInterface::AddState add_state = body_iface.AddBodiesPrepare(pending_objects_sleeping.ptr(), pending_objects_sleeping.size());
+		body_iface.AddBodiesFinalize(pending_objects_sleeping.ptr(), pending_objects_sleeping.size(), add_state, JPH::EActivation::DontActivate);
+		pending_objects_sleeping.reset();
+	}
+
+	if (!pending_objects_awake.is_empty()) {
+		JPH::BodyInterface::AddState add_state = body_iface.AddBodiesPrepare(pending_objects_awake.ptr(), pending_objects_awake.size());
+		body_iface.AddBodiesFinalize(pending_objects_awake.ptr(), pending_objects_awake.size(), add_state, JPH::EActivation::Activate);
+		pending_objects_awake.reset();
+	}
+}
+
+void JoltSpace3D::set_is_object_sleeping(const JPH::BodyID &p_jolt_id, bool p_enable) {
+	if (p_enable) {
+		if (pending_objects_awake.erase_unordered(p_jolt_id)) {
+			pending_objects_sleeping.push_back(p_jolt_id);
+		} else if (pending_objects_sleeping.has(p_jolt_id)) {
+			// Do nothing.
+		} else {
+			get_body_iface().DeactivateBody(p_jolt_id);
+		}
+	} else {
+		if (pending_objects_sleeping.erase_unordered(p_jolt_id)) {
+			pending_objects_awake.push_back(p_jolt_id);
+		} else if (pending_objects_awake.has(p_jolt_id)) {
+			// Do nothing.
+		} else {
+			get_body_iface().ActivateBody(p_jolt_id);
+		}
+	}
+}
+
+void JoltSpace3D::enqueue_call_queries(SelfList<JoltBody3D> *p_body) {
+	// This method will be called from the body activation listener on multiple threads during the simulation step.
+	MutexLock body_call_queries_lock(body_call_queries_mutex);
+
+	if (!p_body->in_list()) {
+		body_call_queries_list.add(p_body);
+	}
+}
+
+void JoltSpace3D::enqueue_call_queries(SelfList<JoltArea3D> *p_area) {
+	if (!p_area->in_list()) {
+		area_call_queries_list.add(p_area);
+	}
+}
+
+void JoltSpace3D::dequeue_call_queries(SelfList<JoltBody3D> *p_body) {
+	if (p_body->in_list()) {
+		body_call_queries_list.remove(p_body);
+	}
+}
+
+void JoltSpace3D::dequeue_call_queries(SelfList<JoltArea3D> *p_area) {
+	if (p_area->in_list()) {
+		area_call_queries_list.remove(p_area);
+	}
+}
+
+void JoltSpace3D::enqueue_shapes_changed(SelfList<JoltShapedObject3D> *p_object) {
+	if (!p_object->in_list()) {
+		shapes_changed_list.add(p_object);
+	}
+}
+
+void JoltSpace3D::dequeue_shapes_changed(SelfList<JoltShapedObject3D> *p_object) {
+	if (p_object->in_list()) {
+		shapes_changed_list.remove(p_object);
+	}
+}
+
+void JoltSpace3D::enqueue_needs_optimization(SelfList<JoltShapedObject3D> *p_object) {
+	if (!p_object->in_list()) {
+		needs_optimization_list.add(p_object);
+	}
+}
+
+void JoltSpace3D::dequeue_needs_optimization(SelfList<JoltShapedObject3D> *p_object) {
+	if (p_object->in_list()) {
+		needs_optimization_list.remove(p_object);
+	}
+}
+
+void JoltSpace3D::add_joint(JPH::Constraint *p_jolt_ref) {
+	physics_system->AddConstraint(p_jolt_ref);
+}
+
+void JoltSpace3D::add_joint(JoltJoint3D *p_joint) {
+	add_joint(p_joint->get_jolt_ref());
+}
+
+void JoltSpace3D::remove_joint(JPH::Constraint *p_jolt_ref) {
+	physics_system->RemoveConstraint(p_jolt_ref);
+}
+
+void JoltSpace3D::remove_joint(JoltJoint3D *p_joint) {
+	remove_joint(p_joint->get_jolt_ref());
+}
+
+#ifdef DEBUG_ENABLED
+
+void JoltSpace3D::dump_debug_snapshot(const String &p_dir) {
+	const Dictionary datetime = Time::get_singleton()->get_datetime_dict_from_system();
+	const String datetime_str = vformat("%04d-%02d-%02d_%02d-%02d-%02d", datetime["year"], datetime["month"], datetime["day"], datetime["hour"], datetime["minute"], datetime["second"]);
+	const String path = p_dir + vformat("/jolt_snapshot_%s_%d.bin", datetime_str, rid.get_id());
+
+	Ref<FileAccess> file_access = FileAccess::open(path, FileAccess::ModeFlags::WRITE);
+	ERR_FAIL_COND_MSG(file_access.is_null(), vformat("Failed to open '%s' for writing when saving snapshot of physics space with RID '%d'.", path, rid.get_id()));
+
+	JPH::PhysicsScene physics_scene;
+	physics_scene.FromPhysicsSystem(physics_system);
+
+	for (JPH::BodyCreationSettings &settings : physics_scene.GetBodies()) {
+		const JoltObject3D *object = reinterpret_cast<const JoltObject3D *>(settings.mUserData);
+
+		if (const JoltBody3D *body = object->as_body()) {
+			// Since we do our own integration of gravity and damping, while leaving Jolt's own values at zero, we need to transfer over the correct values.
+			settings.mGravityFactor = body->get_gravity_scale();
+			settings.mLinearDamping = body->get_total_linear_damp();
+			settings.mAngularDamping = body->get_total_angular_damp();
+		}
+
+		settings.SetShape(JoltShape3D::without_custom_shapes(settings.GetShape()));
+	}
+
+	JoltStreamOutputWrapper output_stream(file_access);
+	physics_scene.SaveBinaryState(output_stream, true, false);
+
+	ERR_FAIL_COND_MSG(file_access->get_error() != OK, vformat("Writing snapshot of physics space with RID '%d' to '%s' failed with error '%s'.", rid.get_id(), path, VariantUtilityFunctions::error_string(file_access->get_error())));
+
+	print_line(vformat("Snapshot of physics space with RID '%d' saved to '%s'.", rid.get_id(), path));
+}
+
+const PackedVector3Array &JoltSpace3D::get_debug_contacts() const {
+	return contact_listener->get_debug_contacts();
+}
+
+int JoltSpace3D::get_debug_contact_count() const {
+	return contact_listener->get_debug_contact_count();
+}
+
+int JoltSpace3D::get_max_debug_contacts() const {
+	return contact_listener->get_max_debug_contacts();
+}
+
+void JoltSpace3D::set_max_debug_contacts(int p_count) {
+	contact_listener->set_max_debug_contacts(p_count);
+}
+
+#endif

modules/jolt_physics/spaces/jolt_space_3d.h

@@ -0,0 +1,164 @@
+/**************************************************************************/
+/*  jolt_space_3d.h                                                       */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* 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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "servers/physics_server_3d.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Core/JobSystem.h"
+#include "Jolt/Core/TempAllocator.h"
+#include "Jolt/Physics/Body/BodyInterface.h"
+#include "Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h"
+#include "Jolt/Physics/Collision/NarrowPhaseQuery.h"
+#include "Jolt/Physics/Constraints/Constraint.h"
+#include "Jolt/Physics/PhysicsSystem.h"
+
+class JoltArea3D;
+class JoltBody3D;
+class JoltBodyActivationListener3D;
+class JoltContactListener3D;
+class JoltJoint3D;
+class JoltLayers;
+class JoltObject3D;
+class JoltPhysicsDirectSpaceState3D;
+class JoltShapedObject3D;
+class JoltSoftBody3D;
+
+class JoltSpace3D {
+	Mutex pending_objects_mutex;
+	Mutex body_call_queries_mutex;
+
+	SelfList<JoltBody3D>::List body_call_queries_list;
+	SelfList<JoltArea3D>::List area_call_queries_list;
+	SelfList<JoltShapedObject3D>::List shapes_changed_list;
+	SelfList<JoltShapedObject3D>::List needs_optimization_list;
+
+	LocalVector<JPH::BodyID> pending_objects_sleeping;
+	LocalVector<JPH::BodyID> pending_objects_awake;
+
+	RID rid;
+
+	JPH::JobSystem *job_system = nullptr;
+	JPH::TempAllocator *temp_allocator = nullptr;
+	JoltLayers *layers = nullptr;
+	JoltContactListener3D *contact_listener = nullptr;
+	JoltBodyActivationListener3D *body_activation_listener = nullptr;
+	JPH::PhysicsSystem *physics_system = nullptr;
+	JoltPhysicsDirectSpaceState3D *direct_state = nullptr;
+	JoltArea3D *default_area = nullptr;
+
+	float last_step = 0.0f;
+
+	bool active = false;
+	bool stepping = false;
+
+	void _pre_step(float p_step);
+	void _post_step(float p_step);
+
+public:
+	explicit JoltSpace3D(JPH::JobSystem *p_job_system);
+	~JoltSpace3D();
+
+	void step(float p_step);
+
+	void call_queries();
+
+	RID get_rid() const { return rid; }
+	void set_rid(const RID &p_rid) { rid = p_rid; }
+
+	bool is_active() const { return active; }
+	void set_active(bool p_active) { active = p_active; }
+
+	bool is_stepping() const { return stepping; }
+
+	double get_param(PhysicsServer3D::SpaceParameter p_param) const;
+	void set_param(PhysicsServer3D::SpaceParameter p_param, double p_value);
+
+	JPH::PhysicsSystem &get_physics_system() const { return *physics_system; }
+
+	JPH::TempAllocator &get_temp_allocator() const { return *temp_allocator; }
+
+	JPH::BodyInterface &get_body_iface();
+	const JPH::BodyInterface &get_body_iface() const;
+	const JPH::BodyLockInterface &get_lock_iface() const;
+
+	const JPH::BroadPhaseQuery &get_broad_phase_query() const;
+	const JPH::NarrowPhaseQuery &get_narrow_phase_query() const;
+
+	JPH::ObjectLayer map_to_object_layer(JPH::BroadPhaseLayer p_broad_phase_layer, uint32_t p_collision_layer, uint32_t p_collision_mask);
+	void map_from_object_layer(JPH::ObjectLayer p_object_layer, JPH::BroadPhaseLayer &r_broad_phase_layer, uint32_t &r_collision_layer, uint32_t &r_collision_mask) const;
+
+	JPH::Body *try_get_jolt_body(const JPH::BodyID &p_body_id) const;
+	JoltObject3D *try_get_object(const JPH::BodyID &p_body_id) const;
+	JoltShapedObject3D *try_get_shaped(const JPH::BodyID &p_body_id) const;
+	JoltBody3D *try_get_body(const JPH::BodyID &p_body_id) const;
+	JoltArea3D *try_get_area(const JPH::BodyID &p_body_id) const;
+	JoltSoftBody3D *try_get_soft_body(const JPH::BodyID &p_body_id) const;
+
+	JoltPhysicsDirectSpaceState3D *get_direct_state();
+
+	JoltArea3D *get_default_area() const { return default_area; }
+	void set_default_area(JoltArea3D *p_area);
+
+	float get_last_step() const { return last_step; }
+
+	JPH::Body *add_object(const JoltObject3D &p_object, const JPH::BodyCreationSettings &p_settings, bool p_sleeping = false);
+	JPH::Body *add_object(const JoltObject3D &p_object, const JPH::SoftBodyCreationSettings &p_settings, bool p_sleeping = false);
+	void remove_object(const JPH::BodyID &p_jolt_id);
+	void flush_pending_objects();
+
+	void set_is_object_sleeping(const JPH::BodyID &p_jolt_id, bool p_enable);
+
+	void enqueue_call_queries(SelfList<JoltBody3D> *p_body);
+	void enqueue_call_queries(SelfList<JoltArea3D> *p_area);
+	void dequeue_call_queries(SelfList<JoltBody3D> *p_body);
+	void dequeue_call_queries(SelfList<JoltArea3D> *p_area);
+
+	void enqueue_shapes_changed(SelfList<JoltShapedObject3D> *p_object);
+	void dequeue_shapes_changed(SelfList<JoltShapedObject3D> *p_object);
+
+	void enqueue_needs_optimization(SelfList<JoltShapedObject3D> *p_object);
+	void dequeue_needs_optimization(SelfList<JoltShapedObject3D> *p_object);
+
+	void add_joint(JPH::Constraint *p_jolt_ref);
+	void add_joint(JoltJoint3D *p_joint);
+	void remove_joint(JPH::Constraint *p_jolt_ref);
+	void remove_joint(JoltJoint3D *p_joint);
+
+#ifdef DEBUG_ENABLED
+	void dump_debug_snapshot(const String &p_dir);
+	const PackedVector3Array &get_debug_contacts() const;
+	int get_debug_contact_count() const;
+	int get_max_debug_contacts() const;
+	void set_max_debug_contacts(int p_count);
+#endif
+};

modules/jolt_physics/spaces/jolt_temp_allocator.cpp

@@ -0,0 +1,120 @@
+/**************************************************************************/
+/*  jolt_temp_allocator.cpp                                               */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* 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.                 */
+/**************************************************************************/
+
+/*
+Adapted to Godot from the Jolt Physics library.
+*/
+
+/*
+Copyright 2021 Jorrit Rouwe
+
+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 "jolt_temp_allocator.h"
+
+#include "../jolt_project_settings.h"
+
+#include "core/variant/variant.h"
+
+#include "Jolt/Core/Memory.h"
+
+namespace {
+
+template <typename TValue, typename TAlignment>
+constexpr TValue align_up(TValue p_value, TAlignment p_alignment) {
+	return (p_value + p_alignment - 1) & ~(p_alignment - 1);
+}
+
+} //namespace
+
+JoltTempAllocator::JoltTempAllocator() :
+		capacity((uint64_t)JoltProjectSettings::temp_memory_b),
+		base(static_cast<uint8_t *>(JPH::Allocate((size_t)capacity))) {
+}
+
+JoltTempAllocator::~JoltTempAllocator() {
+	JPH::Free(base);
+}
+
+void *JoltTempAllocator::Allocate(uint32_t p_size) {
+	if (p_size == 0) {
+		return nullptr;
+	}
+
+	p_size = align_up(p_size, 16U);
+
+	const uint64_t new_top = top + p_size;
+
+	void *ptr = nullptr;
+
+	if (new_top <= capacity) {
+		ptr = base + top;
+	} else {
+		WARN_PRINT_ONCE(vformat("Jolt Physics temporary memory allocator exceeded capacity of %d MiB. "
+								"Falling back to slower general-purpose allocator. "
+								"Consider increasing maximum temporary memory in project settings.",
+				JoltProjectSettings::temp_memory_mib));
+
+		ptr = JPH::Allocate(p_size);
+	}
+
+	top = new_top;
+
+	return ptr;
+}
+
+void JoltTempAllocator::Free(void *p_ptr, uint32_t p_size) {
+	if (p_ptr == nullptr) {
+		return;
+	}
+
+	p_size = align_up(p_size, 16U);
+
+	const uint64_t new_top = top - p_size;
+
+	if (top <= capacity) {
+		if (base + new_top != p_ptr) {
+			CRASH_NOW_MSG("Jolt Physics temporary memory was freed in the wrong order.");
+		}
+	} else {
+		JPH::Free(p_ptr);
+	}
+
+	top = new_top;
+}

modules/jolt_physics/spaces/jolt_temp_allocator.h

@@ -0,0 +1,50 @@
+/**************************************************************************/
+/*  jolt_temp_allocator.h                                                 */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* 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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Core/TempAllocator.h"
+
+#include <cstdint>
+
+class JoltTempAllocator final : public JPH::TempAllocator {
+	uint64_t capacity = 0;
+	uint64_t top = 0;
+	uint8_t *base = nullptr;
+
+public:
+	explicit JoltTempAllocator();
+	virtual ~JoltTempAllocator() override;
+
+	virtual void *Allocate(JPH::uint p_size) override;
+	virtual void Free(void *p_ptr, JPH::uint p_size) override;
+};