Add Jolt Physics as an alternative 3D physics engine

Co-authored-by: Jorrit Rouwe <jrouwe@gmail.com>

modules/jolt_physics/spaces/jolt_body_accessor_3d.cpp

@@ -0,0 +1,196 @@
+/**************************************************************************/
+/*  jolt_body_accessor_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_accessor_3d.h"
+
+#include "jolt_space_3d.h"
+
+namespace {
+
+template <class... TTypes>
+struct VariantVisitors : TTypes... {
+	using TTypes::operator()...;
+};
+
+template <class... TTypes>
+VariantVisitors(TTypes...) -> VariantVisitors<TTypes...>;
+
+} // namespace
+
+JoltBodyAccessor3D::JoltBodyAccessor3D(const JoltSpace3D *p_space) :
+		space(p_space) {
+}
+
+JoltBodyAccessor3D::~JoltBodyAccessor3D() = default;
+
+void JoltBodyAccessor3D::acquire(const JPH::BodyID *p_ids, int p_id_count) {
+	ERR_FAIL_NULL(space);
+
+	lock_iface = &space->get_lock_iface();
+	ids = BodyIDSpan(p_ids, p_id_count);
+	_acquire_internal(p_ids, p_id_count);
+}
+
+void JoltBodyAccessor3D::acquire(const JPH::BodyID &p_id) {
+	ERR_FAIL_NULL(space);
+
+	lock_iface = &space->get_lock_iface();
+	ids = p_id;
+	_acquire_internal(&p_id, 1);
+}
+
+void JoltBodyAccessor3D::acquire_active() {
+	const JPH::PhysicsSystem &physics_system = space->get_physics_system();
+
+	acquire(physics_system.GetActiveBodiesUnsafe(JPH::EBodyType::RigidBody), (int)physics_system.GetNumActiveBodies(JPH::EBodyType::RigidBody));
+}
+
+void JoltBodyAccessor3D::acquire_all() {
+	ERR_FAIL_NULL(space);
+
+	lock_iface = &space->get_lock_iface();
+
+	JPH::BodyIDVector *vector = std::get_if<JPH::BodyIDVector>(&ids);
+
+	if (vector == nullptr) {
+		ids = JPH::BodyIDVector();
+		vector = std::get_if<JPH::BodyIDVector>(&ids);
+	}
+
+	space->get_physics_system().GetBodies(*vector);
+
+	_acquire_internal(vector->data(), (int)vector->size());
+}
+
+void JoltBodyAccessor3D::release() {
+	_release_internal();
+	lock_iface = nullptr;
+}
+
+const JPH::BodyID *JoltBodyAccessor3D::get_ids() const {
+	ERR_FAIL_COND_V(not_acquired(), nullptr);
+
+	return std::visit(
+			VariantVisitors{
+					[](const JPH::BodyID &p_id) { return &p_id; },
+					[](const JPH::BodyIDVector &p_vector) { return p_vector.data(); },
+					[](const BodyIDSpan &p_span) { return p_span.ptr; } },
+			ids);
+}
+
+int JoltBodyAccessor3D::get_count() const {
+	ERR_FAIL_COND_V(not_acquired(), 0);
+
+	return std::visit(
+			VariantVisitors{
+					[](const JPH::BodyID &p_id) { return 1; },
+					[](const JPH::BodyIDVector &p_vector) { return (int)p_vector.size(); },
+					[](const BodyIDSpan &p_span) { return p_span.count; } },
+			ids);
+}
+
+const JPH::BodyID &JoltBodyAccessor3D::get_at(int p_index) const {
+	CRASH_BAD_INDEX(p_index, get_count());
+	return get_ids()[p_index];
+}
+
+void JoltBodyReader3D::_acquire_internal(const JPH::BodyID *p_ids, int p_id_count) {
+	mutex_mask = lock_iface->GetMutexMask(p_ids, p_id_count);
+	lock_iface->LockRead(mutex_mask);
+}
+
+void JoltBodyReader3D::_release_internal() {
+	ERR_FAIL_COND(not_acquired());
+	lock_iface->UnlockRead(mutex_mask);
+}
+
+JoltBodyReader3D::JoltBodyReader3D(const JoltSpace3D *p_space) :
+		JoltBodyAccessor3D(p_space) {
+}
+
+const JPH::Body *JoltBodyReader3D::try_get(const JPH::BodyID &p_id) const {
+	if (unlikely(p_id.IsInvalid())) {
+		return nullptr;
+	}
+
+	ERR_FAIL_COND_V(not_acquired(), nullptr);
+
+	return lock_iface->TryGetBody(p_id);
+}
+
+const JPH::Body *JoltBodyReader3D::try_get(int p_index) const {
+	const int count = get_count();
+	if (unlikely(p_index < 0 || p_index >= count)) {
+		return nullptr;
+	}
+
+	return try_get(get_at(p_index));
+}
+
+const JPH::Body *JoltBodyReader3D::try_get() const {
+	return try_get(0);
+}
+
+void JoltBodyWriter3D::_acquire_internal(const JPH::BodyID *p_ids, int p_id_count) {
+	mutex_mask = lock_iface->GetMutexMask(p_ids, p_id_count);
+	lock_iface->LockWrite(mutex_mask);
+}
+
+void JoltBodyWriter3D::_release_internal() {
+	ERR_FAIL_COND(not_acquired());
+	lock_iface->UnlockWrite(mutex_mask);
+}
+
+JoltBodyWriter3D::JoltBodyWriter3D(const JoltSpace3D *p_space) :
+		JoltBodyAccessor3D(p_space) {
+}
+
+JPH::Body *JoltBodyWriter3D::try_get(const JPH::BodyID &p_id) const {
+	if (unlikely(p_id.IsInvalid())) {
+		return nullptr;
+	}
+
+	ERR_FAIL_COND_V(not_acquired(), nullptr);
+
+	return lock_iface->TryGetBody(p_id);
+}
+
+JPH::Body *JoltBodyWriter3D::try_get(int p_index) const {
+	const int count = get_count();
+	if (unlikely(p_index < 0 || p_index >= count)) {
+		return nullptr;
+	}
+
+	return try_get(get_at(p_index));
+}
+
+JPH::Body *JoltBodyWriter3D::try_get() const {
+	return try_get(0);
+}

modules/jolt_physics/spaces/jolt_body_accessor_3d.h

@@ -0,0 +1,214 @@
+/**************************************************************************/
+/*  jolt_body_accessor_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.                 */
+/**************************************************************************/
+
+#ifndef JOLT_BODY_ACCESSOR_3D_H
+#define JOLT_BODY_ACCESSOR_3D_H
+
+#include "../objects/jolt_object_3d.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Body/BodyLockInterface.h"
+
+#include <variant>
+
+class JoltArea3D;
+class JoltBody3D;
+class JoltShapedObject3D;
+class JoltSpace3D;
+
+class JoltBodyAccessor3D {
+protected:
+	struct BodyIDSpan {
+		BodyIDSpan(const JPH::BodyID *p_ptr, int p_count) :
+				ptr(p_ptr), count(p_count) {}
+
+		const JPH::BodyID *ptr;
+		int count;
+	};
+
+	virtual void _acquire_internal(const JPH::BodyID *p_ids, int p_id_count) = 0;
+	virtual void _release_internal() = 0;
+
+	const JoltSpace3D *space = nullptr;
+
+	const JPH::BodyLockInterface *lock_iface = nullptr;
+
+	std::variant<JPH::BodyID, JPH::BodyIDVector, BodyIDSpan> ids;
+
+public:
+	explicit JoltBodyAccessor3D(const JoltSpace3D *p_space);
+	virtual ~JoltBodyAccessor3D() = 0;
+
+	void acquire(const JPH::BodyID *p_ids, int p_id_count);
+	void acquire(const JPH::BodyID &p_id);
+	void acquire_active();
+	void acquire_all();
+	void release();
+
+	bool is_acquired() const { return lock_iface != nullptr; }
+	bool not_acquired() const { return lock_iface == nullptr; }
+
+	const JoltSpace3D &get_space() const { return *space; }
+	const JPH::BodyID *get_ids() const;
+	int get_count() const;
+
+	const JPH::BodyID &get_at(int p_index) const;
+};
+
+class JoltBodyReader3D final : public JoltBodyAccessor3D {
+	virtual void _acquire_internal(const JPH::BodyID *p_ids, int p_id_count) override;
+	virtual void _release_internal() override;
+
+	JPH::BodyLockInterface::MutexMask mutex_mask = 0;
+
+public:
+	explicit JoltBodyReader3D(const JoltSpace3D *p_space);
+
+	const JPH::Body *try_get(const JPH::BodyID &p_id) const;
+	const JPH::Body *try_get(int p_index) const;
+	const JPH::Body *try_get() const;
+};
+
+class JoltBodyWriter3D final : public JoltBodyAccessor3D {
+	virtual void _acquire_internal(const JPH::BodyID *p_ids, int p_id_count) override;
+	virtual void _release_internal() override;
+
+	JPH::BodyLockInterface::MutexMask mutex_mask = 0;
+
+public:
+	explicit JoltBodyWriter3D(const JoltSpace3D *p_space);
+
+	JPH::Body *try_get(const JPH::BodyID &p_id) const;
+	JPH::Body *try_get(int p_index) const;
+	JPH::Body *try_get() const;
+};
+
+template <typename TBodyAccessor>
+class JoltScopedBodyAccessor3D {
+	TBodyAccessor inner;
+
+public:
+	JoltScopedBodyAccessor3D(const JoltSpace3D &p_space, const JPH::BodyID *p_ids, int p_id_count) :
+			inner(&p_space) { inner.acquire(p_ids, p_id_count); }
+
+	JoltScopedBodyAccessor3D(const JoltSpace3D &p_space, const JPH::BodyID &p_id) :
+			inner(&p_space) { inner.acquire(p_id); }
+
+	JoltScopedBodyAccessor3D(const JoltScopedBodyAccessor3D &p_other) = delete;
+	JoltScopedBodyAccessor3D(JoltScopedBodyAccessor3D &&p_other) = default;
+	~JoltScopedBodyAccessor3D() { inner.release(); }
+
+	const JoltSpace3D &get_space() const { return inner.get_space(); }
+	int get_count() const { return inner.get_count(); }
+	const JPH::BodyID &get_at(int p_index) const { return inner.get_at(p_index); }
+
+	JoltScopedBodyAccessor3D &operator=(const JoltScopedBodyAccessor3D &p_other) = delete;
+	JoltScopedBodyAccessor3D &operator=(JoltScopedBodyAccessor3D &&p_other) = default;
+
+	decltype(auto) try_get(const JPH::BodyID &p_id) const { return inner.try_get(p_id); }
+	decltype(auto) try_get(int p_index) const { return inner.try_get(p_index); }
+	decltype(auto) try_get() const { return inner.try_get(); }
+};
+
+template <typename TAccessor, typename TBody>
+class JoltAccessibleBody3D {
+	TAccessor accessor;
+	TBody *body = nullptr;
+
+public:
+	JoltAccessibleBody3D(const JoltSpace3D &p_space, const JPH::BodyID &p_id) :
+			accessor(p_space, p_id), body(accessor.try_get()) {}
+
+	bool is_valid() const { return body != nullptr; }
+	bool is_invalid() const { return body == nullptr; }
+
+	JoltObject3D *as_object() const {
+		if (body != nullptr) {
+			return reinterpret_cast<JoltObject3D *>(body->GetUserData());
+		} else {
+			return nullptr;
+		}
+	}
+
+	JoltShapedObject3D *as_shaped() const {
+		if (JoltObject3D *object = as_object(); object != nullptr && object->is_shaped()) {
+			return reinterpret_cast<JoltShapedObject3D *>(body->GetUserData());
+		} else {
+			return nullptr;
+		}
+	}
+
+	JoltBody3D *as_body() const {
+		if (JoltObject3D *object = as_object(); object != nullptr && object->is_body()) {
+			return reinterpret_cast<JoltBody3D *>(body->GetUserData());
+		} else {
+			return nullptr;
+		}
+	}
+
+	JoltArea3D *as_area() const {
+		if (JoltObject3D *object = as_object(); object != nullptr && object->is_area()) {
+			return reinterpret_cast<JoltArea3D *>(body->GetUserData());
+		} else {
+			return nullptr;
+		}
+	}
+
+	TBody *operator->() const { return body; }
+	TBody &operator*() const { return *body; }
+
+	explicit operator TBody *() const { return body; }
+};
+
+template <typename TAccessor, typename TBody>
+class JoltAccessibleBodies3D {
+	TAccessor accessor;
+
+public:
+	JoltAccessibleBodies3D(const JoltSpace3D &p_space, const JPH::BodyID *p_ids, int p_id_count) :
+			accessor(p_space, p_ids, p_id_count) {}
+
+	JoltAccessibleBody3D<TAccessor, TBody> operator[](int p_index) const {
+		const JPH::BodyID &body_id = p_index < accessor.get_count() ? accessor.get_at(p_index) : JPH::BodyID();
+		return JoltAccessibleBody3D<TAccessor, TBody>(accessor.get_space(), body_id);
+	}
+};
+
+typedef JoltScopedBodyAccessor3D<JoltBodyReader3D> JoltScopedBodyReader3D;
+typedef JoltScopedBodyAccessor3D<JoltBodyWriter3D> JoltScopedBodyWriter3D;
+
+typedef JoltAccessibleBody3D<JoltScopedBodyReader3D, const JPH::Body> JoltReadableBody3D;
+typedef JoltAccessibleBody3D<JoltScopedBodyWriter3D, JPH::Body> JoltWritableBody3D;
+
+typedef JoltAccessibleBodies3D<JoltScopedBodyReader3D, const JPH::Body> JoltReadableBodies3D;
+typedef JoltAccessibleBodies3D<JoltScopedBodyWriter3D, JPH::Body> JoltWritableBodies3D;
+
+#endif // JOLT_BODY_ACCESSOR_3D_H

modules/jolt_physics/spaces/jolt_broad_phase_layer.h

@@ -0,0 +1,52 @@
+/**************************************************************************/
+/*  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.                 */
+/**************************************************************************/
+
+#ifndef JOLT_BROAD_PHASE_LAYER_H
+#define JOLT_BROAD_PHASE_LAYER_H
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h"
+
+#include <stdint.h>
+
+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
+
+#endif // JOLT_BROAD_PHASE_LAYER_H

modules/jolt_physics/spaces/jolt_contact_listener_3d.cpp

@@ -0,0 +1,548 @@
+/**************************************************************************/
+/*  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::_is_listening_for(const JPH::Body &p_body) const {
+	return listening_for.has(p_body.GetID());
+}
+
+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_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold, JPH::ContactSettings &p_settings) {
+	if (p_body1.IsSensor() || p_body2.IsSensor()) {
+		return false;
+	}
+
+	if (!_is_listening_for(p_body1) && !_is_listening_for(p_body2)) {
+		return false;
+	}
+
+	const JPH::SubShapeIDPair shape_pair(p_body1.GetID(), p_manifold.mSubShapeID1, p_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_body1, p_body2, p_manifold, collision, p_settings.mCombinedFriction, p_settings.mCombinedRestitution, JoltProjectSettings::get_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_body1.GetPointVelocity(world_point1);
+		const JPH::Vec3 velocity2 = p_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 evaluate = [&](const auto &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 (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;
+
+		const JPH::BodyID body_ids[2] = { shape_pair.GetBody1ID(), shape_pair.GetBody2ID() };
+		const JoltReadableBodies3D jolt_bodies = space->read_bodies(body_ids, 2);
+
+		JoltBody3D *body1 = jolt_bodies[0].as_body();
+		ERR_FAIL_NULL(body1);
+
+		JoltBody3D *body2 = jolt_bodies[1].as_body();
+		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();
+
+		const JPH::SubShapeID &sub_shape_id1 = shape_pair.GetSubShapeID1();
+		const JPH::SubShapeID &sub_shape_id2 = shape_pair.GetSubShapeID2();
+
+		const JPH::BodyID body_ids[2] = { body_id1, body_id2 };
+		const JoltReadableBodies3D jolt_bodies = space->read_bodies(body_ids, 2);
+
+		const JoltReadableBody3D jolt_body1 = jolt_bodies[0];
+		const JoltReadableBody3D jolt_body2 = jolt_bodies[1];
+
+		if (jolt_body1.is_invalid() || jolt_body2.is_invalid()) {
+			continue;
+		}
+
+		JoltArea3D *area1 = jolt_body1.as_area();
+		JoltArea3D *area2 = jolt_body2.as_area();
+
+		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_shifts() {
+	for (const JPH::SubShapeIDPair &shape_pair : area_overlaps) {
+		auto is_shifted = [&](const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_sub_shape_id) {
+			const JoltReadableBody3D jolt_body = space->read_body(p_body_id);
+			const JoltShapedObject3D *object = jolt_body.as_shaped();
+			ERR_FAIL_NULL_V(object, false);
+
+			if (object->get_previous_jolt_shape() == nullptr) {
+				return false;
+			}
+
+			const JPH::Shape &current_shape = *object->get_jolt_shape();
+			const JPH::Shape &previous_shape = *object->get_previous_jolt_shape();
+
+			const uint32_t current_id = (uint32_t)current_shape.GetSubShapeUserData(p_sub_shape_id);
+			const uint32_t previous_id = (uint32_t)previous_shape.GetSubShapeUserData(p_sub_shape_id);
+
+			return current_id != previous_id;
+		};
+
+		if (is_shifted(shape_pair.GetBody1ID(), shape_pair.GetSubShapeID1()) || is_shifted(shape_pair.GetBody2ID(), shape_pair.GetSubShapeID2())) {
+			area_enters.insert(shape_pair);
+			area_exits.insert(shape_pair);
+		}
+	}
+}
+
+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();
+
+		const JPH::SubShapeID &sub_shape_id1 = shape_pair.GetSubShapeID1();
+		const JPH::SubShapeID &sub_shape_id2 = shape_pair.GetSubShapeID2();
+
+		const JPH::BodyID body_ids[2] = { body_id1, body_id2 };
+		const JoltReadableBodies3D jolt_bodies = space->read_bodies(body_ids, 2);
+
+		const JoltReadableBody3D jolt_body1 = jolt_bodies[0];
+		const JoltReadableBody3D jolt_body2 = jolt_bodies[1];
+
+		JoltArea3D *area1 = jolt_body1.as_area();
+		JoltArea3D *area2 = jolt_body2.as_area();
+
+		const JoltBody3D *body1 = jolt_body1.as_body();
+		const JoltBody3D *body2 = jolt_body2.as_body();
+
+		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::listen_for(JoltShapedObject3D *p_object) {
+	listening_for.insert(p_object->get_jolt_id());
+}
+
+void JoltContactListener3D::pre_step() {
+	listening_for.clear();
+
+#ifdef DEBUG_ENABLED
+	debug_contact_count = 0;
+#endif
+}
+
+void JoltContactListener3D::post_step() {
+	_flush_contacts();
+	_flush_area_shifts();
+	_flush_area_exits();
+	_flush_area_enters();
+}

modules/jolt_physics/spaces/jolt_contact_listener_3d.h

@@ -0,0 +1,147 @@
+/**************************************************************************/
+/*  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.                 */
+/**************************************************************************/
+
+#ifndef JOLT_CONTACT_LISTENER_3D_H
+#define JOLT_CONTACT_LISTENER_3D_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 <stdint.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::BodyID, BodyIDHasher> listening_for;
+	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;
+#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 _is_listening_for(const JPH::Body &p_body) const;
+
+	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_body1, const JPH::Body &p_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_shifts();
+	void _flush_area_exits();
+
+public:
+	explicit JoltContactListener3D(JoltSpace3D *p_space) :
+			space(p_space) {}
+
+	void listen_for(JoltShapedObject3D *p_object);
+
+	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
+};
+
+#endif // JOLT_CONTACT_LISTENER_3D_H

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,107 @@
+/**************************************************************************/
+/*  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.                 */
+/**************************************************************************/
+
+#ifndef JOLT_JOB_SYSTEM_H
+#define JOLT_JOB_SYSTEM_H
+
+#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 <stdint.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
+};
+
+#endif // JOLT_JOB_SYSTEM_H

modules/jolt_physics/spaces/jolt_layers.cpp

@@ -0,0 +1,223 @@
+/**************************************************************************/
+/*  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_BIG, BODY_DYNAMIC);
+		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, AREA_DETECTABLE);
+		allow_collision(AREA_DETECTABLE, AREA_UNDETECTABLE);
+		allow_collision(AREA_UNDETECTABLE, BODY_DYNAMIC);
+		allow_collision(AREA_UNDETECTABLE, AREA_DETECTABLE);
+
+		if (JoltProjectSettings::areas_detect_static_bodies()) {
+			allow_collision(BODY_STATIC, AREA_DETECTABLE);
+			allow_collision(BODY_STATIC, AREA_UNDETECTABLE);
+			allow_collision(BODY_STATIC_BIG, AREA_DETECTABLE);
+			allow_collision(BODY_STATIC_BIG, AREA_UNDETECTABLE);
+			allow_collision(AREA_DETECTABLE, BODY_STATIC);
+			allow_collision(AREA_DETECTABLE, BODY_STATIC_BIG);
+			allow_collision(AREA_UNDETECTABLE, BODY_STATIC);
+			allow_collision(AREA_UNDETECTABLE, BODY_STATIC_BIG);
+		}
+	}
+
+	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);
+}
+
+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,71 @@
+/**************************************************************************/
+/*  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.                 */
+/**************************************************************************/
+
+#ifndef JOLT_LAYERS_H
+#define JOLT_LAYERS_H
+
+#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"
+
+#include <stdint.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();
+
+	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;
+};
+
+#endif // JOLT_LAYERS_H

modules/jolt_physics/spaces/jolt_motion_filter_3d.cpp

@@ -0,0 +1,114 @@
+/**************************************************************************/
+/*  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;
+	}
+
+	const JoltReadableBody3D jolt_body_self = space.read_body(body_self);
+	return jolt_body_self->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,72 @@
+/**************************************************************************/
+/*  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.                 */
+/**************************************************************************/
+
+#ifndef JOLT_MOTION_FILTER_3D_H
+#define JOLT_MOTION_FILTER_3D_H
+
+#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;
+};
+
+#endif // JOLT_MOTION_FILTER_3D_H

modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.cpp

@@ -0,0 +1,936 @@
+/**************************************************************************/
+/*  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_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, 2048> 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(logf(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 JoltReadableBody3D other_jolt_body = space->read_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 int recovery_iterations = JoltProjectSettings::get_motion_query_recovery_iterations();
+	const float recovery_amount = JoltProjectSettings::get_motion_query_recovery_amount();
+
+	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 < recovery_iterations; ++i) {
+		collector.reset();
+
+		_collide_shape_kinematics(jolt_shape, JPH::Vec3::sReplicate(1.0f), 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 JoltReadableBody3D other_jolt_body = space->read_body(hit.mBodyID2);
+			const JoltBody3D *other_body = other_jolt_body.as_body();
+			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 JoltReadableBody3D other_jolt_body = space->read_body(hit.mBodyID2);
+			const JoltBody3D *other_body = other_jolt_body.as_body();
+			ERR_CONTINUE(other_body == nullptr);
+
+			const float recovery_distance = penetration_depth * 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 = transform_com.basis.get_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.");
+
+		transform_com.basis.orthonormalize();
+
+		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::sReplicate(1.0f), 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 JoltReadableBody3D collider_jolt_body = space->read_body(hit.mBodyID2);
+		const JoltShapedObject3D *collider = collider_jolt_body.as_shaped();
+		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.mManifoldToleranceSq, 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->try_optimize();
+
+	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 JoltReadableBody3D body = space->read_body(body_id);
+	const JoltObject3D *object = body.as_object();
+	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 = 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(*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->try_optimize();
+
+	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 JoltReadableBody3D body = space->read_body(hit.mBodyID);
+		const JoltObject3D *object = body.as_object();
+		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->try_optimize();
+
+	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 = transform.basis.get_scale();
+	JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "intersect_shape was passed an invalid transform.");
+
+	transform.basis.orthonormalize();
+
+	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 JoltReadableBody3D body = space->read_body(hit.mBodyID2);
+		const JoltObject3D *object = body.as_object();
+		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->try_optimize();
+
+	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 = transform.basis.get_scale();
+	JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "cast_motion (maybe from ShapeCast3D?) was passed an invalid transform.");
+
+	transform.basis.orthonormalize();
+
+	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->try_optimize();
+
+	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 = transform.basis.get_scale();
+	JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "collide_shape was passed an invalid transform.");
+
+	transform.basis.orthonormalize();
+
+	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->try_optimize();
+
+	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 = transform.basis.get_scale();
+	JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "get_rest_info (maybe from ShapeCast3D?) was passed an invalid transform.");
+
+	transform.basis.orthonormalize();
+
+	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 JoltReadableBody3D body = space->read_body(hit.mBodyID2);
+	const JoltObject3D *object = body.as_object();
+	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->shape = 0;
+	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->try_optimize();
+
+	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());
+
+	const JoltReadableBody3D body = space->read_body(*object);
+	const JPH::TransformedShape root_shape = body->GetTransformedShape();
+
+	JoltQueryCollectorAll<JPH::TransformedShapeCollector, 32> collector;
+	root_shape.CollectTransformedShapes(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::Quat &shape_rotation = shape_transformed.mShapeRotation;
+		const JPH::RVec3 &shape_pos_com = shape_transformed.mShapePositionCOM;
+		const JPH::RMat44 shape_3x3 = JPH::RMat44::sRotation(shape_rotation);
+		const JPH::Vec3 shape_com_local = shape.GetCenterOfMass();
+		const JPH::Vec3 shape_com = shape_3x3.Multiply3x3(shape_com_local);
+		const JPH::RVec3 shape_pos = shape_pos_com - JPH::RVec3(shape_com);
+		const JPH::RMat44 shape_4x4 = shape_3x3.PostTranslated(shape_pos);
+		const JPH::RMat44 shape_4x4_inv = shape_4x4.InversedRotationTranslation();
+
+		JPH::PointConvexSupport point_support;
+		point_support.mPoint = JPH::Vec3(shape_4x4_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_4x4 * point_on_a;
+			found_point = true;
+		}
+	}
+
+	if (found_point) {
+		return to_godot(closest_point);
+	} else {
+		return to_godot(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.");
+
+	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 = transform.basis.get_scale();
+	transform.basis.orthonormalize();
+
+	space->try_optimize();
+
+	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,87 @@
+/**************************************************************************/
+/*  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.                 */
+/**************************************************************************/
+
+#ifndef JOLT_PHYSICS_DIRECT_SPACE_STATE_3D_H
+#define JOLT_PHYSICS_DIRECT_SPACE_STATE_3D_H
+
+#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; }
+};
+
+#endif // JOLT_PHYSICS_DIRECT_SPACE_STATE_3D_H

modules/jolt_physics/spaces/jolt_query_collectors.h

@@ -0,0 +1,238 @@
+/**************************************************************************/
+/*  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.                 */
+/**************************************************************************/
+
+#ifndef JOLT_QUERY_COLLECTORS_H
+#define JOLT_QUERY_COLLECTORS_H
+
+#include "../jolt_project_settings.h"
+#include "jolt_space_3d.h"
+
+#include "core/templates/local_vector.h"
+
+#include "Jolt/Jolt.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;
+
+private:
+	JPH::Array<Hit> hits;
+
+public:
+	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;
+
+private:
+	JPH::Array<Hit> hits;
+	int max_hits = 0;
+
+public:
+	explicit JoltQueryCollectorAnyMulti(int p_max_hits = TDefaultCapacity) :
+			max_hits(p_max_hits) {}
+
+	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;
+
+private:
+	JPH::Array<Hit> hits;
+	int max_hits = 0;
+
+public:
+	explicit JoltQueryCollectorClosestMulti(int p_max_hits = TDefaultCapacity) :
+			max_hits(p_max_hits) {}
+
+	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 JPH::Array<Hit>::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);
+		}
+	}
+};
+
+#endif // JOLT_QUERY_COLLECTORS_H

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,67 @@
+/**************************************************************************/
+/*  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.                 */
+/**************************************************************************/
+
+#ifndef JOLT_QUERY_FILTER_3D_H
+#define JOLT_QUERY_FILTER_3D_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"
+
+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;
+};
+
+#endif // JOLT_QUERY_FILTER_3D_H

modules/jolt_physics/spaces/jolt_space_3d.cpp

@@ -0,0 +1,514 @@
+/**************************************************************************/
+/*  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_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/PhysicsScene.h"
+
+namespace {
+
+constexpr double DEFAULT_CONTACT_RECYCLE_RADIUS = 0.01;
+constexpr double DEFAULT_CONTACT_MAX_SEPARATION = 0.05;
+constexpr double DEFAULT_CONTACT_MAX_ALLOWED_PENETRATION = 0.01;
+constexpr double DEFAULT_CONTACT_DEFAULT_BIAS = 0.8;
+constexpr double DEFAULT_SLEEP_THRESHOLD_LINEAR = 0.1;
+constexpr double DEFAULT_SLEEP_THRESHOLD_ANGULAR = 8.0 * Math_PI / 180;
+constexpr double DEFAULT_SOLVER_ITERATIONS = 8;
+
+} // namespace
+
+void JoltSpace3D::_pre_step(float p_step) {
+	body_accessor.acquire_all();
+
+	contact_listener->pre_step();
+
+	const int body_count = body_accessor.get_count();
+
+	for (int i = 0; i < body_count; ++i) {
+		if (JPH::Body *jolt_body = body_accessor.try_get(i)) {
+			if (jolt_body->IsSoftBody()) {
+				continue;
+			}
+
+			JoltShapedObject3D *object = reinterpret_cast<JoltShapedObject3D *>(jolt_body->GetUserData());
+
+			object->pre_step(p_step, *jolt_body);
+
+			if (object->reports_contacts()) {
+				contact_listener->listen_for(object);
+			}
+		}
+	}
+
+	body_accessor.release();
+}
+
+void JoltSpace3D::_post_step(float p_step) {
+	body_accessor.acquire_all();
+
+	contact_listener->post_step();
+
+	const int body_count = body_accessor.get_count();
+
+	for (int i = 0; i < body_count; ++i) {
+		if (JPH::Body *jolt_body = body_accessor.try_get(i)) {
+			if (jolt_body->IsSoftBody()) {
+				continue;
+			}
+
+			JoltObject3D *object = reinterpret_cast<JoltObject3D *>(jolt_body->GetUserData());
+
+			object->post_step(p_step, *jolt_body);
+		}
+	}
+
+	body_accessor.release();
+}
+
+JoltSpace3D::JoltSpace3D(JPH::JobSystem *p_job_system) :
+		body_accessor(this),
+		job_system(p_job_system),
+		temp_allocator(new JoltTempAllocator()),
+		layers(new JoltLayers()),
+		contact_listener(new JoltContactListener3D(this)),
+		physics_system(new JPH::PhysicsSystem()) {
+	physics_system->Init((JPH::uint)JoltProjectSettings::get_max_bodies(), 0, (JPH::uint)JoltProjectSettings::get_max_pairs(), (JPH::uint)JoltProjectSettings::get_max_contact_constraints(), *layers, *layers, *layers);
+
+	JPH::PhysicsSettings settings;
+	settings.mBaumgarte = JoltProjectSettings::get_baumgarte_stabilization_factor();
+	settings.mSpeculativeContactDistance = JoltProjectSettings::get_speculative_contact_distance();
+	settings.mPenetrationSlop = JoltProjectSettings::get_penetration_slop();
+	settings.mLinearCastThreshold = JoltProjectSettings::get_ccd_movement_threshold();
+	settings.mLinearCastMaxPenetration = JoltProjectSettings::get_ccd_max_penetration();
+	settings.mBodyPairCacheMaxDeltaPositionSq = JoltProjectSettings::get_body_pair_cache_distance_sq();
+	settings.mBodyPairCacheCosMaxDeltaRotationDiv2 = JoltProjectSettings::get_body_pair_cache_angle_cos_div2();
+	settings.mNumVelocitySteps = (JPH::uint)JoltProjectSettings::get_simulation_velocity_steps();
+	settings.mNumPositionSteps = (JPH::uint)JoltProjectSettings::get_simulation_position_steps();
+	settings.mMinVelocityForRestitution = JoltProjectSettings::get_bounce_velocity_threshold();
+	settings.mTimeBeforeSleep = JoltProjectSettings::get_sleep_time_threshold();
+	settings.mPointVelocitySleepThreshold = JoltProjectSettings::get_sleep_velocity_threshold();
+	settings.mUseBodyPairContactCache = JoltProjectSettings::is_body_pair_contact_cache_enabled();
+	settings.mAllowSleeping = JoltProjectSettings::is_sleep_allowed();
+
+	physics_system->SetPhysicsSettings(settings);
+	physics_system->SetGravity(JPH::Vec3::sZero());
+	physics_system->SetContactListener(contact_listener);
+	physics_system->SetSoftBodyContactListener(contact_listener);
+
+	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 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 (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::get_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::get_max_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::get_max_contact_constraints()));
+	}
+
+	_post_step(p_step);
+
+	bodies_added_since_optimizing = 0;
+	has_stepped = true;
+	stepping = false;
+}
+
+void JoltSpace3D::call_queries() {
+	if (!has_stepped) {
+		// We need to skip the first invocation of this method, because there will be pending notifications that need to
+		// be flushed first, which can cause weird conflicts with things like `_integrate_forces`. This happens to also
+		// emulate the behavior of Godot Physics, where (active) collision objects must register to have `call_queries`
+		// invoked, which they don't do until the physics step, which happens after this.
+		//
+		// TODO: This would be better solved by just doing what Godot Physics does with `GodotSpace*D::active_list`.
+		return;
+	}
+
+	body_accessor.acquire_all();
+
+	const int body_count = body_accessor.get_count();
+
+	for (int i = 0; i < body_count; ++i) {
+		if (JPH::Body *jolt_body = body_accessor.try_get(i)) {
+			if (!jolt_body->IsSensor() && !jolt_body->IsSoftBody()) {
+				JoltBody3D *body = reinterpret_cast<JoltBody3D *>(jolt_body->GetUserData());
+				body->call_queries(*jolt_body);
+			}
+		}
+	}
+
+	for (int i = 0; i < body_count; ++i) {
+		if (JPH::Body *jolt_body = body_accessor.try_get(i)) {
+			if (jolt_body->IsSensor()) {
+				JoltArea3D *area = reinterpret_cast<JoltArea3D *>(jolt_body->GetUserData());
+				area->call_queries(*jolt_body);
+			}
+		}
+	}
+
+	body_accessor.release();
+}
+
+double JoltSpace3D::get_param(PhysicsServer3D::SpaceParameter p_param) const {
+	switch (p_param) {
+		case PhysicsServer3D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: {
+			return DEFAULT_CONTACT_RECYCLE_RADIUS;
+		}
+		case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_SEPARATION: {
+			return DEFAULT_CONTACT_MAX_SEPARATION;
+		}
+		case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION: {
+			return DEFAULT_CONTACT_MAX_ALLOWED_PENETRATION;
+		}
+		case PhysicsServer3D::SPACE_PARAM_CONTACT_DEFAULT_BIAS: {
+			return DEFAULT_CONTACT_DEFAULT_BIAS;
+		}
+		case PhysicsServer3D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: {
+			return DEFAULT_SLEEP_THRESHOLD_LINEAR;
+		}
+		case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: {
+			return DEFAULT_SLEEP_THRESHOLD_ANGULAR;
+		}
+		case PhysicsServer3D::SPACE_PARAM_BODY_TIME_TO_SLEEP: {
+			return JoltProjectSettings::get_sleep_time_threshold();
+		}
+		case PhysicsServer3D::SPACE_PARAM_SOLVER_ITERATIONS: {
+			return 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);
+}
+
+JoltReadableBody3D JoltSpace3D::read_body(const JPH::BodyID &p_body_id) const {
+	return JoltReadableBody3D(*this, p_body_id);
+}
+
+JoltReadableBody3D JoltSpace3D::read_body(const JoltObject3D &p_object) const {
+	return read_body(p_object.get_jolt_id());
+}
+
+JoltWritableBody3D JoltSpace3D::write_body(const JPH::BodyID &p_body_id) const {
+	return JoltWritableBody3D(*this, p_body_id);
+}
+
+JoltWritableBody3D JoltSpace3D::write_body(const JoltObject3D &p_object) const {
+	return write_body(p_object.get_jolt_id());
+}
+
+JoltReadableBodies3D JoltSpace3D::read_bodies(const JPH::BodyID *p_body_ids, int p_body_count) const {
+	return JoltReadableBodies3D(*this, p_body_ids, p_body_count);
+}
+
+JoltWritableBodies3D JoltSpace3D::write_bodies(const JPH::BodyID *p_body_ids, int p_body_count) const {
+	return JoltWritableBodies3D(*this, p_body_ids, p_body_count);
+}
+
+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::BodyID JoltSpace3D::add_rigid_body(const JoltObject3D &p_object, const JPH::BodyCreationSettings &p_settings, bool p_sleeping) {
+	const JPH::BodyID body_id = get_body_iface().CreateAndAddBody(p_settings, p_sleeping ? JPH::EActivation::DontActivate : JPH::EActivation::Activate);
+
+	if (unlikely(body_id.IsInvalid())) {
+		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::get_max_bodies()));
+
+		return JPH::BodyID();
+	}
+
+	bodies_added_since_optimizing += 1;
+
+	return body_id;
+}
+
+JPH::BodyID JoltSpace3D::add_soft_body(const JoltObject3D &p_object, const JPH::SoftBodyCreationSettings &p_settings, bool p_sleeping) {
+	const JPH::BodyID body_id = get_body_iface().CreateAndAddSoftBody(p_settings, p_sleeping ? JPH::EActivation::DontActivate : JPH::EActivation::Activate);
+
+	if (unlikely(body_id.IsInvalid())) {
+		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::get_max_bodies()));
+
+		return JPH::BodyID();
+	}
+
+	bodies_added_since_optimizing += 1;
+
+	return body_id;
+}
+
+void JoltSpace3D::remove_body(const JPH::BodyID &p_body_id) {
+	JPH::BodyInterface &body_iface = get_body_iface();
+
+	body_iface.RemoveBody(p_body_id);
+	body_iface.DestroyBody(p_body_id);
+}
+
+void JoltSpace3D::try_optimize() {
+	// This makes assumptions about the underlying acceleration structure of Jolt's broad-phase, which currently uses a
+	// quadtree, and which gets walked with a fixed-size node stack of 128. This means that when the quadtree is
+	// completely unbalanced, as is the case if we add bodies one-by-one without ever stepping the simulation, like in
+	// the editor viewport, we would exceed this stack size (resulting in an incomplete search) as soon as we perform a
+	// physics query after having added somewhere in the order of 128 * 3 bodies. We leave a hefty margin just in case.
+
+	if (likely(bodies_added_since_optimizing < 128)) {
+		return;
+	}
+
+	physics_system->OptimizeBroadPhase();
+
+	bodies_added_since_optimizing = 0;
+}
+
+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,149 @@
+/**************************************************************************/
+/*  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.                 */
+/**************************************************************************/
+
+#ifndef JOLT_SPACE_3D_H
+#define JOLT_SPACE_3D_H
+
+#include "jolt_body_accessor_3d.h"
+
+#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"
+
+#include <stdint.h>
+
+class JoltArea3D;
+class JoltContactListener3D;
+class JoltJoint3D;
+class JoltLayers;
+class JoltObject3D;
+class JoltPhysicsDirectSpaceState3D;
+
+class JoltSpace3D {
+	JoltBodyWriter3D body_accessor;
+
+	RID rid;
+
+	JPH::JobSystem *job_system = nullptr;
+	JPH::TempAllocator *temp_allocator = nullptr;
+	JoltLayers *layers = nullptr;
+	JoltContactListener3D *contact_listener = nullptr;
+	JPH::PhysicsSystem *physics_system = nullptr;
+	JoltPhysicsDirectSpaceState3D *direct_state = nullptr;
+	JoltArea3D *default_area = nullptr;
+
+	float last_step = 0.0f;
+
+	int bodies_added_since_optimizing = 0;
+
+	bool active = false;
+	bool stepping = false;
+	bool has_stepped = 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::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;
+
+	JoltReadableBody3D read_body(const JPH::BodyID &p_body_id) const;
+	JoltReadableBody3D read_body(const JoltObject3D &p_object) const;
+
+	JoltWritableBody3D write_body(const JPH::BodyID &p_body_id) const;
+	JoltWritableBody3D write_body(const JoltObject3D &p_object) const;
+
+	JoltReadableBodies3D read_bodies(const JPH::BodyID *p_body_ids, int p_body_count) const;
+	JoltWritableBodies3D write_bodies(const JPH::BodyID *p_body_ids, int p_body_count) 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::BodyID add_rigid_body(const JoltObject3D &p_object, const JPH::BodyCreationSettings &p_settings, bool p_sleeping = false);
+	JPH::BodyID add_soft_body(const JoltObject3D &p_object, const JPH::SoftBodyCreationSettings &p_settings, bool p_sleeping = false);
+
+	void remove_body(const JPH::BodyID &p_body_id);
+
+	void try_optimize();
+
+	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
+};
+
+#endif // JOLT_SPACE_3D_H

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::get_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::get_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,53 @@
+/**************************************************************************/
+/*  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.                 */
+/**************************************************************************/
+
+#ifndef JOLT_TEMP_ALLOCATOR_H
+#define JOLT_TEMP_ALLOCATOR_H
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Core/TempAllocator.h"
+
+#include <stdint.h>
+
+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;
+};
+
+#endif // JOLT_TEMP_ALLOCATOR_H