From 785b8a6c921ac78e2bd0282eaaf9b357fabefc83 Mon Sep 17 00:00:00 2001
From: Sofox <sofoxx@gmail.com>
Date: Thu, 23 Nov 2023 17:58:33 +0000
Subject: [PATCH 001/124] Undoing an Anchor Preset on a Control will correctly
 restore its position

---
 editor/editor_inspector.cpp | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/editor/editor_inspector.cpp b/editor/editor_inspector.cpp
index c5ce815d6b..74f4481d20 100644
--- a/editor/editor_inspector.cpp
+++ b/editor/editor_inspector.cpp
@@ -3747,7 +3747,11 @@ void EditorInspector::_edit_set(const String &p_name, const Variant &p_value, bo
 		bool valid = false;
 		Variant value = object->get(p_name, &valid);
 		if (valid) {
-			undo_redo->add_undo_property(object, p_name, value);
+			if (Object::cast_to<Control>(object) && (p_name == "anchors_preset" || p_name == "layout_mode")) {
+				undo_redo->add_undo_method(object, "_edit_set_state", Object::cast_to<Control>(object)->_edit_get_state());
+			} else {
+				undo_redo->add_undo_property(object, p_name, value);
+			}
 		}
 
 		List<StringName> linked_properties;

From c15633139f057ee77a022da49ccd9d855df6292b Mon Sep 17 00:00:00 2001
From: Hugo Locurcio <hugo.locurcio@hugo.pro>
Date: Mon, 22 Jul 2024 21:54:08 +0200
Subject: [PATCH 002/124] Document `_process()` and `_physics_process()` delta
 behavior at low FPS

---
 doc/classes/MainLoop.xml | 2 ++
 doc/classes/Node.xml     | 6 +++++-
 2 files changed, 7 insertions(+), 1 deletion(-)

diff --git a/doc/classes/MainLoop.xml b/doc/classes/MainLoop.xml
index 17cc0d78d3..ec000c3c3a 100644
--- a/doc/classes/MainLoop.xml
+++ b/doc/classes/MainLoop.xml
@@ -78,6 +78,7 @@
 			<description>
 				Called each physics frame with the time since the last physics frame as argument ([param delta], in seconds). Equivalent to [method Node._physics_process].
 				If implemented, the method must return a boolean value. [code]true[/code] ends the main loop, while [code]false[/code] lets it proceed to the next frame.
+				[b]Note:[/b] [param delta] will be larger than expected if running at a framerate lower than [member Engine.physics_ticks_per_second] / [member Engine.max_physics_steps_per_frame] FPS. This is done to avoid "spiral of death" scenarios where performance would plummet due to an ever-increasing number of physics steps per frame. This behavior affects both [method _process] and [method _physics_process]. As a result, avoid using [param delta] for time measurements in real-world seconds. Use the [Time] singleton's methods for this purpose instead, such as [method Time.get_ticks_usec].
 			</description>
 		</method>
 		<method name="_process" qualifiers="virtual">
@@ -86,6 +87,7 @@
 			<description>
 				Called each process (idle) frame with the time since the last process frame as argument (in seconds). Equivalent to [method Node._process].
 				If implemented, the method must return a boolean value. [code]true[/code] ends the main loop, while [code]false[/code] lets it proceed to the next frame.
+				[b]Note:[/b] [param delta] will be larger than expected if running at a framerate lower than [member Engine.physics_ticks_per_second] / [member Engine.max_physics_steps_per_frame] FPS. This is done to avoid "spiral of death" scenarios where performance would plummet due to an ever-increasing number of physics steps per frame. This behavior affects both [method _process] and [method _physics_process]. As a result, avoid using [param delta] for time measurements in real-world seconds. Use the [Time] singleton's methods for this purpose instead, such as [method Time.get_ticks_usec].
 			</description>
 		</method>
 	</methods>
diff --git a/doc/classes/Node.xml b/doc/classes/Node.xml
index c54219c056..b7c4000296 100644
--- a/doc/classes/Node.xml
+++ b/doc/classes/Node.xml
@@ -71,10 +71,11 @@
 			<return type="void" />
 			<param index="0" name="delta" type="float" />
 			<description>
-				Called during the physics processing step of the main loop. Physics processing means that the frame rate is synced to the physics, i.e. the [param delta] variable should be constant. [param delta] is in seconds.
+				Called during the physics processing step of the main loop. Physics processing means that the frame rate is synced to the physics, i.e. the [param delta] parameter will [i]generally[/i] be constant (see exceptions below). [param delta] is in seconds.
 				It is only called if physics processing is enabled, which is done automatically if this method is overridden, and can be toggled with [method set_physics_process].
 				Corresponds to the [constant NOTIFICATION_PHYSICS_PROCESS] notification in [method Object._notification].
 				[b]Note:[/b] This method is only called if the node is present in the scene tree (i.e. if it's not an orphan).
+				[b]Note:[/b] [param delta] will be larger than expected if running at a framerate lower than [member Engine.physics_ticks_per_second] / [member Engine.max_physics_steps_per_frame] FPS. This is done to avoid "spiral of death" scenarios where performance would plummet due to an ever-increasing number of physics steps per frame. This behavior affects both [method _process] and [method _physics_process]. As a result, avoid using [param delta] for time measurements in real-world seconds. Use the [Time] singleton's methods for this purpose instead, such as [method Time.get_ticks_usec].
 			</description>
 		</method>
 		<method name="_process" qualifiers="virtual">
@@ -85,6 +86,7 @@
 				It is only called if processing is enabled, which is done automatically if this method is overridden, and can be toggled with [method set_process].
 				Corresponds to the [constant NOTIFICATION_PROCESS] notification in [method Object._notification].
 				[b]Note:[/b] This method is only called if the node is present in the scene tree (i.e. if it's not an orphan).
+				[b]Note:[/b] [param delta] will be larger than expected if running at a framerate lower than [member Engine.physics_ticks_per_second] / [member Engine.max_physics_steps_per_frame] FPS. This is done to avoid "spiral of death" scenarios where performance would plummet due to an ever-increasing number of physics steps per frame. This behavior affects both [method _process] and [method _physics_process]. As a result, avoid using [param delta] for time measurements in real-world seconds. Use the [Time] singleton's methods for this purpose instead, such as [method Time.get_ticks_usec].
 			</description>
 		</method>
 		<method name="_ready" qualifiers="virtual">
@@ -490,12 +492,14 @@
 			<return type="float" />
 			<description>
 				Returns the time elapsed (in seconds) since the last physics callback. This value is identical to [method _physics_process]'s [code]delta[/code] parameter, and is often consistent at run-time, unless [member Engine.physics_ticks_per_second] is changed. See also [constant NOTIFICATION_PHYSICS_PROCESS].
+				[b]Note:[/b] The returned value will be larger than expected if running at a framerate lower than [member Engine.physics_ticks_per_second] / [member Engine.max_physics_steps_per_frame] FPS. This is done to avoid "spiral of death" scenarios where performance would plummet due to an ever-increasing number of physics steps per frame. This behavior affects both [method _process] and [method _physics_process]. As a result, avoid using [code]delta[/code] for time measurements in real-world seconds. Use the [Time] singleton's methods for this purpose instead, such as [method Time.get_ticks_usec].
 			</description>
 		</method>
 		<method name="get_process_delta_time" qualifiers="const">
 			<return type="float" />
 			<description>
 				Returns the time elapsed (in seconds) since the last process callback. This value is identical to [method _process]'s [code]delta[/code] parameter, and may vary from frame to frame. See also [constant NOTIFICATION_PROCESS].
+				[b]Note:[/b] The returned value will be larger than expected if running at a framerate lower than [member Engine.physics_ticks_per_second] / [member Engine.max_physics_steps_per_frame] FPS. This is done to avoid "spiral of death" scenarios where performance would plummet due to an ever-increasing number of physics steps per frame. This behavior affects both [method _process] and [method _physics_process]. As a result, avoid using [code]delta[/code] for time measurements in real-world seconds. Use the [Time] singleton's methods for this purpose instead, such as [method Time.get_ticks_usec].
 			</description>
 		</method>
 		<method name="get_scene_instance_load_placeholder" qualifiers="const">

From 3669eb0b69020ac97e711c1cc0ab4259adaa0ba2 Mon Sep 17 00:00:00 2001
From: Zi Ye <major.mcdoom@gmail.com>
Date: Thu, 15 Aug 2024 18:28:25 -0500
Subject: [PATCH 003/124] Added an "Edit Now" option to project dialog.

---
 editor/project_manager.cpp                |  7 +++++--
 editor/project_manager.h                  |  2 +-
 editor/project_manager/project_dialog.cpp | 20 ++++++++++++++++----
 editor/project_manager/project_dialog.h   |  3 +++
 4 files changed, 25 insertions(+), 7 deletions(-)

diff --git a/editor/project_manager.cpp b/editor/project_manager.cpp
index 59f45ef5db..acb8b5d78f 100644
--- a/editor/project_manager.cpp
+++ b/editor/project_manager.cpp
@@ -711,14 +711,17 @@ void ProjectManager::_on_projects_updated() {
 	project_list->update_dock_menu();
 }
 
-void ProjectManager::_on_project_created(const String &dir) {
+void ProjectManager::_on_project_created(const String &dir, bool edit) {
 	project_list->add_project(dir, false);
 	project_list->save_config();
 	search_box->clear();
 	int i = project_list->refresh_project(dir);
 	project_list->select_project(i);
 	project_list->ensure_project_visible(i);
-	_open_selected_projects_ask();
+
+	if (edit) {
+		_open_selected_projects_ask();
+	}
 
 	project_list->update_dock_menu();
 }
diff --git a/editor/project_manager.h b/editor/project_manager.h
index 669b5d8b6c..52e300f1a5 100644
--- a/editor/project_manager.h
+++ b/editor/project_manager.h
@@ -187,7 +187,7 @@ class ProjectManager : public Control {
 	void _erase_missing_projects_confirm();
 	void _update_project_buttons();
 
-	void _on_project_created(const String &dir);
+	void _on_project_created(const String &dir, bool edit);
 	void _on_projects_updated();
 
 	void _on_order_option_changed(int p_idx);
diff --git a/editor/project_manager/project_dialog.cpp b/editor/project_manager/project_dialog.cpp
index b4aa00ee0a..78ff9379d8 100644
--- a/editor/project_manager/project_dialog.cpp
+++ b/editor/project_manager/project_dialog.cpp
@@ -671,7 +671,7 @@ void ProjectDialog::ok_pressed() {
 
 	hide();
 	if (mode == MODE_NEW || mode == MODE_IMPORT || mode == MODE_INSTALL) {
-		emit_signal(SNAME("project_created"), path);
+		emit_signal(SNAME("project_created"), path, edit_check_box->is_pressed());
 	} else if (mode == MODE_RENAME) {
 		emit_signal(SNAME("projects_updated"));
 	}
@@ -713,6 +713,7 @@ void ProjectDialog::show_dialog(bool p_reset_name) {
 		create_dir->hide();
 		project_status_rect->hide();
 		project_browse->hide();
+		edit_check_box->hide();
 
 		name_container->show();
 		install_path_container->hide();
@@ -742,10 +743,11 @@ void ProjectDialog::show_dialog(bool p_reset_name) {
 		create_dir->show();
 		project_status_rect->show();
 		project_browse->show();
+		edit_check_box->show();
 
 		if (mode == MODE_IMPORT) {
 			set_title(TTR("Import Existing Project"));
-			set_ok_button_text(TTR("Import & Edit"));
+			set_ok_button_text(TTR("Import"));
 
 			name_container->hide();
 			install_path_container->hide();
@@ -755,7 +757,7 @@ void ProjectDialog::show_dialog(bool p_reset_name) {
 			// Project path dialog is also opened; no need to change focus.
 		} else if (mode == MODE_NEW) {
 			set_title(TTR("Create New Project"));
-			set_ok_button_text(TTR("Create & Edit"));
+			set_ok_button_text(TTR("Create"));
 
 			name_container->show();
 			install_path_container->hide();
@@ -766,7 +768,7 @@ void ProjectDialog::show_dialog(bool p_reset_name) {
 			callable_mp(project_name, &LineEdit::select_all).call_deferred();
 		} else if (mode == MODE_INSTALL) {
 			set_title(TTR("Install Project:") + " " + zip_title);
-			set_ok_button_text(TTR("Install & Edit"));
+			set_ok_button_text(TTR("Install"));
 
 			project_name->set_text(zip_title);
 
@@ -993,6 +995,16 @@ ProjectDialog::ProjectDialog() {
 	fdialog_install->set_access(EditorFileDialog::ACCESS_FILESYSTEM);
 	add_child(fdialog_install);
 
+	Control *spacer2 = memnew(Control);
+	spacer2->set_v_size_flags(Control::SIZE_EXPAND_FILL);
+	vb->add_child(spacer2);
+
+	edit_check_box = memnew(CheckBox);
+	edit_check_box->set_text(TTR("Edit Now"));
+	edit_check_box->set_h_size_flags(Control::SIZE_SHRINK_CENTER);
+	edit_check_box->set_pressed(true);
+	vb->add_child(edit_check_box);
+
 	project_name->connect(SceneStringName(text_changed), callable_mp(this, &ProjectDialog::_project_name_changed).unbind(1));
 	project_path->connect(SceneStringName(text_changed), callable_mp(this, &ProjectDialog::_project_path_changed).unbind(1));
 	install_path->connect(SceneStringName(text_changed), callable_mp(this, &ProjectDialog::_install_path_changed).unbind(1));
diff --git a/editor/project_manager/project_dialog.h b/editor/project_manager/project_dialog.h
index b985492f84..6b39eb786e 100644
--- a/editor/project_manager/project_dialog.h
+++ b/editor/project_manager/project_dialog.h
@@ -34,6 +34,7 @@
 #include "scene/gui/dialogs.h"
 
 class Button;
+class CheckBox;
 class CheckButton;
 class EditorFileDialog;
 class LineEdit;
@@ -87,6 +88,8 @@ private:
 
 	OptionButton *vcs_metadata_selection = nullptr;
 
+	CheckBox *edit_check_box = nullptr;
+
 	EditorFileDialog *fdialog_project = nullptr;
 	EditorFileDialog *fdialog_install = nullptr;
 	AcceptDialog *dialog_error = nullptr;

From 93ae8d8f0de3560512bcf392c43952f3c9b2d361 Mon Sep 17 00:00:00 2001
From: HolonProduction <holonproduction@gmail.com>
Date: Mon, 30 Sep 2024 21:46:28 +0200
Subject: [PATCH 004/124] GUI: Only cancel completion with the `ui_cancel`
 action

---
 scene/gui/code_edit.cpp | 4 ----
 1 file changed, 4 deletions(-)

diff --git a/scene/gui/code_edit.cpp b/scene/gui/code_edit.cpp
index 635228670d..d906a6e014 100644
--- a/scene/gui/code_edit.cpp
+++ b/scene/gui/code_edit.cpp
@@ -570,10 +570,6 @@ void CodeEdit::gui_input(const Ref<InputEvent> &p_gui_input) {
 		} else {
 			update_code_completion = (allow_unicode_handling && k->get_unicode() >= 32);
 		}
-
-		if (!update_code_completion) {
-			cancel_code_completion();
-		}
 	}
 
 	/* MISC */

From bb813ba7bc1e0358ade36034ba81f88deacdbb5e Mon Sep 17 00:00:00 2001
From: Hugo Locurcio <hugo.locurcio@hugo.pro>
Date: Sat, 2 Nov 2024 18:45:28 +0100
Subject: [PATCH 005/124] Fix Tree drag-and-drop scrolling having low FPS at
 low Physics Ticks per Second

Scrolling is now performed in process instead of physics process.
This makes scrolling much smoother if Physics Ticks per Second is lower
than the rendered FPS.
---
 scene/gui/tree.cpp | 22 +++++++++++-----------
 1 file changed, 11 insertions(+), 11 deletions(-)

diff --git a/scene/gui/tree.cpp b/scene/gui/tree.cpp
index aab6f672f0..307b0caef4 100644
--- a/scene/gui/tree.cpp
+++ b/scene/gui/tree.cpp
@@ -3879,7 +3879,7 @@ void Tree::gui_input(const Ref<InputEvent> &p_event) {
 					if (drag_speed == 0) {
 						drag_touching_deaccel = false;
 						drag_touching = false;
-						set_physics_process_internal(false);
+						set_process_internal(false);
 					} else {
 						drag_touching_deaccel = true;
 					}
@@ -3966,7 +3966,7 @@ void Tree::gui_input(const Ref<InputEvent> &p_event) {
 				}
 
 				if (drag_touching) {
-					set_physics_process_internal(false);
+					set_process_internal(false);
 					drag_touching_deaccel = false;
 					drag_touching = false;
 					drag_speed = 0;
@@ -3981,7 +3981,7 @@ void Tree::gui_input(const Ref<InputEvent> &p_event) {
 					drag_touching = DisplayServer::get_singleton()->is_touchscreen_available();
 					drag_touching_deaccel = false;
 					if (drag_touching) {
-						set_physics_process_internal(true);
+						set_process_internal(true);
 					}
 
 					if (mb->get_button_index() == MouseButton::LEFT) {
@@ -4461,7 +4461,7 @@ void Tree::_notification(int p_what) {
 		case NOTIFICATION_DRAG_END: {
 			drop_mode_flags = 0;
 			scrolling = false;
-			set_physics_process_internal(false);
+			set_process_internal(false);
 			queue_redraw();
 		} break;
 
@@ -4469,21 +4469,21 @@ void Tree::_notification(int p_what) {
 			single_select_defer = nullptr;
 			if (theme_cache.scroll_speed > 0) {
 				scrolling = true;
-				set_physics_process_internal(true);
+				set_process_internal(true);
 			}
 		} break;
 
-		case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: {
+		case NOTIFICATION_INTERNAL_PROCESS: {
 			if (drag_touching) {
 				if (drag_touching_deaccel) {
 					float pos = v_scroll->get_value();
-					pos += drag_speed * get_physics_process_delta_time();
+					pos += drag_speed * get_process_delta_time();
 
 					bool turnoff = false;
 					if (pos < 0) {
 						pos = 0;
 						turnoff = true;
-						set_physics_process_internal(false);
+						set_process_internal(false);
 						drag_touching = false;
 						drag_touching_deaccel = false;
 					}
@@ -4495,7 +4495,7 @@ void Tree::_notification(int p_what) {
 					v_scroll->set_value(pos);
 					float sgn = drag_speed < 0 ? -1 : 1;
 					float val = Math::abs(drag_speed);
-					val -= 1000 * get_physics_process_delta_time();
+					val -= 1000 * get_process_delta_time();
 
 					if (val < 0) {
 						turnoff = true;
@@ -4503,7 +4503,7 @@ void Tree::_notification(int p_what) {
 					drag_speed = sgn * val;
 
 					if (turnoff) {
-						set_physics_process_internal(false);
+						set_process_internal(false);
 						drag_touching = false;
 						drag_touching_deaccel = false;
 					}
@@ -4526,7 +4526,7 @@ void Tree::_notification(int p_what) {
 					point.y = mouse_position.y - (get_size().height - theme_cache.scroll_border);
 				}
 
-				point *= theme_cache.scroll_speed * get_physics_process_delta_time();
+				point *= theme_cache.scroll_speed * get_process_delta_time();
 				point += get_scroll();
 				h_scroll->set_value(point.x);
 				v_scroll->set_value(point.y);

From 899f5151c3f90c7dbb40bf9826998c1fae7c7a9e Mon Sep 17 00:00:00 2001
From: tetrapod00 <145553014+tetrapod00@users.noreply.github.com>
Date: Fri, 1 Nov 2024 17:00:35 -0700
Subject: [PATCH 006/124] Standardize terms for renderers in error strings

Use "Forward+", "Mobile", "Compatibility", and "renderer" or "rendering method".
---
 drivers/gles3/rasterizer_canvas_gles3.h            |  2 +-
 drivers/gles3/rasterizer_scene_gles3.cpp           |  2 +-
 drivers/gles3/storage/material_storage.cpp         | 10 +++++-----
 drivers/gles3/storage/particles_storage.cpp        | 14 +++++++-------
 drivers/gles3/storage/texture_storage.cpp          |  6 +++---
 editor/import/resource_importer_shader_file.cpp    |  2 +-
 scene/2d/gpu_particles_2d.cpp                      |  4 ++--
 scene/3d/decal.cpp                                 |  2 +-
 scene/3d/fog_volume.cpp                            |  2 +-
 scene/3d/gpu_particles_3d.cpp                      |  4 ++--
 scene/3d/light_3d.cpp                              |  4 ++--
 scene/3d/voxel_gi.cpp                              |  2 +-
 scene/resources/particle_process_material.cpp      |  2 +-
 .../forward_mobile/scene_shader_forward_mobile.cpp |  4 ++--
 servers/rendering/rendering_device_binds.cpp       |  2 +-
 servers/rendering/shader_language.cpp              |  2 +-
 .../storage/camera_attributes_storage.cpp          |  4 ++--
 servers/rendering/storage/environment_storage.cpp  | 10 +++++-----
 18 files changed, 39 insertions(+), 39 deletions(-)

diff --git a/drivers/gles3/rasterizer_canvas_gles3.h b/drivers/gles3/rasterizer_canvas_gles3.h
index a82e2713e0..fa584a2e6c 100644
--- a/drivers/gles3/rasterizer_canvas_gles3.h
+++ b/drivers/gles3/rasterizer_canvas_gles3.h
@@ -375,7 +375,7 @@ public:
 
 	virtual void set_debug_redraw(bool p_enabled, double p_time, const Color &p_color) override {
 		if (p_enabled) {
-			WARN_PRINT_ONCE("Debug CanvasItem Redraw is not available yet when using the GL Compatibility backend.");
+			WARN_PRINT_ONCE("Debug CanvasItem Redraw is not available yet when using the Compatibility renderer.");
 		}
 	}
 
diff --git a/drivers/gles3/rasterizer_scene_gles3.cpp b/drivers/gles3/rasterizer_scene_gles3.cpp
index 537076d4ed..ced2d20e43 100644
--- a/drivers/gles3/rasterizer_scene_gles3.cpp
+++ b/drivers/gles3/rasterizer_scene_gles3.cpp
@@ -2135,7 +2135,7 @@ void RasterizerSceneGLES3::_render_shadow_pass(RID p_light, RID p_shadow_atlas,
 				light_transform = light_storage->light_instance_get_shadow_transform(p_light, p_pass);
 				shadow_size = shadow_size / 2;
 			} else {
-				ERR_FAIL_MSG("Dual paraboloid shadow mode not supported in GL Compatibility renderer. Please use Cubemap shadow mode instead.");
+				ERR_FAIL_MSG("Dual paraboloid shadow mode not supported in the Compatibility renderer. Please use CubeMap shadow mode instead.");
 			}
 
 			shadow_bias = light_storage->light_get_param(base, RS::LIGHT_PARAM_SHADOW_BIAS);
diff --git a/drivers/gles3/storage/material_storage.cpp b/drivers/gles3/storage/material_storage.cpp
index 04cbf7f2cd..493f80a22b 100644
--- a/drivers/gles3/storage/material_storage.cpp
+++ b/drivers/gles3/storage/material_storage.cpp
@@ -945,7 +945,7 @@ void MaterialData::update_textures(const HashMap<StringName, Variant> &p_paramet
 					}
 				} break;
 				case ShaderLanguage::TYPE_SAMPLERCUBEARRAY: {
-					ERR_PRINT_ONCE("Type: SamplerCubeArray not supported in GL Compatibility rendering backend, please use another type.");
+					ERR_PRINT_ONCE("Type: SamplerCubeArray is not supported in the Compatibility renderer, please use another type.");
 				} break;
 				case ShaderLanguage::TYPE_SAMPLEREXT: {
 					gl_texture = texture_storage->texture_gl_get_default(DEFAULT_GL_TEXTURE_EXT);
@@ -3013,19 +3013,19 @@ void SceneShaderData::set_code(const String &p_code) {
 
 #ifdef DEBUG_ENABLED
 	if (uses_particle_trails) {
-		WARN_PRINT_ONCE_ED("Particle trails are only available when using the Forward+ or Mobile rendering backends.");
+		WARN_PRINT_ONCE_ED("Particle trails are only available when using the Forward+ or Mobile renderers.");
 	}
 
 	if (uses_sss) {
-		WARN_PRINT_ONCE_ED("Sub-surface scattering is only available when using the Forward+ rendering backend.");
+		WARN_PRINT_ONCE_ED("Subsurface scattering is only available when using the Forward+ renderer.");
 	}
 
 	if (uses_transmittance) {
-		WARN_PRINT_ONCE_ED("Transmittance is only available when using the Forward+ rendering backend.");
+		WARN_PRINT_ONCE_ED("Transmittance is only available when using the Forward+ renderer.");
 	}
 
 	if (uses_normal_texture) {
-		WARN_PRINT_ONCE_ED("Reading from the normal-roughness texture is only available when using the Forward+ or Mobile rendering backends.");
+		WARN_PRINT_ONCE_ED("Reading from the normal-roughness texture is only available when using the Forward+ or Mobile renderers.");
 	}
 #endif
 
diff --git a/drivers/gles3/storage/particles_storage.cpp b/drivers/gles3/storage/particles_storage.cpp
index b600681280..5f919c867f 100644
--- a/drivers/gles3/storage/particles_storage.cpp
+++ b/drivers/gles3/storage/particles_storage.cpp
@@ -287,13 +287,13 @@ void ParticlesStorage::particles_set_fractional_delta(RID p_particles, bool p_en
 
 void ParticlesStorage::particles_set_trails(RID p_particles, bool p_enable, double p_length) {
 	if (p_enable) {
-		WARN_PRINT_ONCE_ED("The GL Compatibility rendering backend does not support particle trails.");
+		WARN_PRINT_ONCE_ED("The Compatibility renderer does not support particle trails.");
 	}
 }
 
 void ParticlesStorage::particles_set_trail_bind_poses(RID p_particles, const Vector<Transform3D> &p_bind_poses) {
 	if (p_bind_poses.size() != 0) {
-		WARN_PRINT_ONCE_ED("The GL Compatibility rendering backend does not support particle trails.");
+		WARN_PRINT_ONCE_ED("The Compatibility renderer does not support particle trails.");
 	}
 }
 
@@ -357,12 +357,12 @@ void ParticlesStorage::particles_restart(RID p_particles) {
 
 void ParticlesStorage::particles_set_subemitter(RID p_particles, RID p_subemitter_particles) {
 	if (p_subemitter_particles.is_valid()) {
-		WARN_PRINT_ONCE_ED("The GL Compatibility rendering backend does not support particle sub-emitters.");
+		WARN_PRINT_ONCE_ED("The Compatibility renderer does not support particle sub-emitters.");
 	}
 }
 
 void ParticlesStorage::particles_emit(RID p_particles, const Transform3D &p_transform, const Vector3 &p_velocity, const Color &p_color, const Color &p_custom, uint32_t p_emit_flags) {
-	WARN_PRINT_ONCE_ED("The GL Compatibility rendering backend does not support manually emitting particles.");
+	WARN_PRINT_ONCE_ED("The Compatibility renderer does not support manually emitting particles.");
 }
 
 void ParticlesStorage::particles_request_process(RID p_particles) {
@@ -645,7 +645,7 @@ void ParticlesStorage::_particles_process(Particles *p_particles, double p_delta
 						attr.extents[2] = extents.z;
 					} break;
 					case RS::PARTICLES_COLLISION_TYPE_VECTOR_FIELD_ATTRACT: {
-						WARN_PRINT_ONCE_ED("Vector field particle attractors are not available in the GL Compatibility rendering backend.");
+						WARN_PRINT_ONCE_ED("Vector field particle attractors are not available in the Compatibility renderer.");
 					} break;
 					default: {
 					}
@@ -678,7 +678,7 @@ void ParticlesStorage::_particles_process(Particles *p_particles, double p_delta
 						col.extents[2] = extents.z;
 					} break;
 					case RS::PARTICLES_COLLISION_TYPE_SDF_COLLIDE: {
-						WARN_PRINT_ONCE_ED("SDF Particle Colliders are not available in the GL Compatibility rendering backend.");
+						WARN_PRINT_ONCE_ED("SDF Particle Colliders are not available in the Compatibility renderer.");
 					} break;
 					case RS::PARTICLES_COLLISION_TYPE_HEIGHTFIELD_COLLIDE: {
 						if (collision_heightmap_texture != 0) { //already taken
@@ -1341,7 +1341,7 @@ void ParticlesStorage::particles_collision_set_attractor_attenuation(RID p_parti
 }
 
 void ParticlesStorage::particles_collision_set_field_texture(RID p_particles_collision, RID p_texture) {
-	WARN_PRINT_ONCE_ED("The GL Compatibility rendering backend does not support SDF collisions in 3D particle shaders");
+	WARN_PRINT_ONCE_ED("The Compatibility renderer does not support SDF collisions in 3D particle shaders");
 }
 
 void ParticlesStorage::particles_collision_height_field_update(RID p_particles_collision) {
diff --git a/drivers/gles3/storage/texture_storage.cpp b/drivers/gles3/storage/texture_storage.cpp
index 14bbe635a4..abfa65b3f5 100644
--- a/drivers/gles3/storage/texture_storage.cpp
+++ b/drivers/gles3/storage/texture_storage.cpp
@@ -681,7 +681,7 @@ Ref<Image> TextureStorage::_get_gl_image_and_format(const Ref<Image> &p_image, I
 			}
 		} break;
 		default: {
-			ERR_FAIL_V_MSG(Ref<Image>(), "The image format " + itos(p_format) + " is not supported by the GL Compatibility rendering backend.");
+			ERR_FAIL_V_MSG(Ref<Image>(), vformat("The image format %d is not supported by the Compatibility renderer.", p_format));
 		}
 	}
 
@@ -841,7 +841,7 @@ void TextureStorage::texture_2d_layered_initialize(RID p_texture, const Vector<R
 	ERR_FAIL_COND(p_layers.is_empty());
 
 	ERR_FAIL_COND(p_layered_type == RS::TEXTURE_LAYERED_CUBEMAP && p_layers.size() != 6);
-	ERR_FAIL_COND_MSG(p_layered_type == RS::TEXTURE_LAYERED_CUBEMAP_ARRAY, "Cubemap Arrays are not supported in the GL Compatibility backend.");
+	ERR_FAIL_COND_MSG(p_layered_type == RS::TEXTURE_LAYERED_CUBEMAP_ARRAY, "Cubemap Arrays are not supported in the Compatibility renderer.");
 
 	const Ref<Image> &image = p_layers[0];
 	{
@@ -1623,7 +1623,7 @@ void TextureStorage::_texture_set_3d_data(RID p_texture, const Vector<Ref<Image>
 	Ref<Image> img = _get_gl_image_and_format(p_data[0], p_data[0]->get_format(), real_format, format, internal_format, type, compressed, texture->resize_to_po2);
 	ERR_FAIL_COND(img.is_null());
 
-	ERR_FAIL_COND_MSG(compressed, "Compressed 3D textures are not supported in the GL Compatibility backend.");
+	ERR_FAIL_COND_MSG(compressed, "Compressed 3D textures are not supported in the Compatibility renderer.");
 
 	glActiveTexture(GL_TEXTURE0);
 	glBindTexture(texture->target, texture->tex_id);
diff --git a/editor/import/resource_importer_shader_file.cpp b/editor/import/resource_importer_shader_file.cpp
index b7508e7644..fc77526942 100644
--- a/editor/import/resource_importer_shader_file.cpp
+++ b/editor/import/resource_importer_shader_file.cpp
@@ -91,7 +91,7 @@ static String _include_function(const String &p_path, void *userpointer) {
 
 Error ResourceImporterShaderFile::import(const String &p_source_file, const String &p_save_path, const HashMap<StringName, Variant> &p_options, List<String> *r_platform_variants, List<String> *r_gen_files, Variant *r_metadata) {
 	/* STEP 1, Read shader code */
-	ERR_FAIL_COND_V_EDMSG((OS::get_singleton()->get_current_rendering_method() == "gl_compatibility"), ERR_UNAVAILABLE, "Cannot import custom .glsl shaders when using the gl_compatibility rendering_method. Please switch to the forward_plus or mobile rendering methods to use custom shaders.");
+	ERR_FAIL_COND_V_EDMSG((OS::get_singleton()->get_current_rendering_method() == "gl_compatibility"), ERR_UNAVAILABLE, "Cannot import custom .glsl shaders when using the Compatibility renderer. Please switch to the Forward+ or Mobile renderer to use custom shaders.");
 	ERR_FAIL_COND_V_EDMSG((DisplayServer::get_singleton()->get_name() == "headless"), ERR_UNAVAILABLE, "Cannot import custom .glsl shaders when running in headless mode.");
 
 	Error err;
diff --git a/scene/2d/gpu_particles_2d.cpp b/scene/2d/gpu_particles_2d.cpp
index b50881cb89..808648194f 100644
--- a/scene/2d/gpu_particles_2d.cpp
+++ b/scene/2d/gpu_particles_2d.cpp
@@ -345,11 +345,11 @@ PackedStringArray GPUParticles2D::get_configuration_warnings() const {
 	}
 
 	if (trail_enabled && OS::get_singleton()->get_current_rendering_method() == "gl_compatibility") {
-		warnings.push_back(RTR("Particle trails are only available when using the Forward+ or Mobile rendering backends."));
+		warnings.push_back(RTR("Particle trails are only available when using the Forward+ or Mobile renderers."));
 	}
 
 	if (sub_emitter != NodePath() && OS::get_singleton()->get_current_rendering_method() == "gl_compatibility") {
-		warnings.push_back(RTR("Particle sub-emitters are not available when using the GL Compatibility rendering backend."));
+		warnings.push_back(RTR("Particle sub-emitters are not available when using the Compatibility renderer."));
 	}
 
 	return warnings;
diff --git a/scene/3d/decal.cpp b/scene/3d/decal.cpp
index 8702b1d3da..41403a269c 100644
--- a/scene/3d/decal.cpp
+++ b/scene/3d/decal.cpp
@@ -166,7 +166,7 @@ PackedStringArray Decal::get_configuration_warnings() const {
 	PackedStringArray warnings = VisualInstance3D::get_configuration_warnings();
 
 	if (OS::get_singleton()->get_current_rendering_method() == "gl_compatibility") {
-		warnings.push_back(RTR("Decals are only available when using the Forward+ or Mobile rendering backends."));
+		warnings.push_back(RTR("Decals are only available when using the Forward+ or Mobile renderers."));
 		return warnings;
 	}
 
diff --git a/scene/3d/fog_volume.cpp b/scene/3d/fog_volume.cpp
index 195074ba2f..ede33bed01 100644
--- a/scene/3d/fog_volume.cpp
+++ b/scene/3d/fog_volume.cpp
@@ -121,7 +121,7 @@ PackedStringArray FogVolume::get_configuration_warnings() const {
 	Ref<Environment> environment = get_viewport()->find_world_3d()->get_environment();
 
 	if (OS::get_singleton()->get_current_rendering_method() != "forward_plus") {
-		warnings.push_back(RTR("Fog Volumes are only visible when using the Forward+ backend."));
+		warnings.push_back(RTR("Fog Volumes are only visible when using the Forward+ renderer."));
 		return warnings;
 	}
 
diff --git a/scene/3d/gpu_particles_3d.cpp b/scene/3d/gpu_particles_3d.cpp
index b48a3a87c7..5f5fcec5c3 100644
--- a/scene/3d/gpu_particles_3d.cpp
+++ b/scene/3d/gpu_particles_3d.cpp
@@ -385,12 +385,12 @@ PackedStringArray GPUParticles3D::get_configuration_warnings() const {
 			warnings.push_back(RTR("Trails enabled, but one or more mesh materials are either missing or not set for trails rendering."));
 		}
 		if (OS::get_singleton()->get_current_rendering_method() == "gl_compatibility") {
-			warnings.push_back(RTR("Particle trails are only available when using the Forward+ or Mobile rendering backends."));
+			warnings.push_back(RTR("Particle trails are only available when using the Forward+ or Mobile renderers."));
 		}
 	}
 
 	if (sub_emitter != NodePath() && OS::get_singleton()->get_current_rendering_method() == "gl_compatibility") {
-		warnings.push_back(RTR("Particle sub-emitters are only available when using the Forward+ or Mobile rendering backends."));
+		warnings.push_back(RTR("Particle sub-emitters are only available when using the Forward+ or Mobile renderers."));
 	}
 
 	return warnings;
diff --git a/scene/3d/light_3d.cpp b/scene/3d/light_3d.cpp
index 2d18e62b10..6cc81da76f 100644
--- a/scene/3d/light_3d.cpp
+++ b/scene/3d/light_3d.cpp
@@ -617,7 +617,7 @@ PackedStringArray OmniLight3D::get_configuration_warnings() const {
 	}
 
 	if (get_projector().is_valid() && OS::get_singleton()->get_current_rendering_method() == "gl_compatibility") {
-		warnings.push_back(RTR("Projector textures are not supported when using the GL Compatibility backend yet. Support will be added in a future release."));
+		warnings.push_back(RTR("Projector textures are not supported when using the Compatibility renderer yet. Support will be added in a future release."));
 	}
 
 	return warnings;
@@ -653,7 +653,7 @@ PackedStringArray SpotLight3D::get_configuration_warnings() const {
 	}
 
 	if (get_projector().is_valid() && OS::get_singleton()->get_current_rendering_method() == "gl_compatibility") {
-		warnings.push_back(RTR("Projector textures are not supported when using the GL Compatibility backend yet. Support will be added in a future release."));
+		warnings.push_back(RTR("Projector textures are not supported when using the Compatibility renderer yet. Support will be added in a future release."));
 	}
 
 	return warnings;
diff --git a/scene/3d/voxel_gi.cpp b/scene/3d/voxel_gi.cpp
index 80ff176a98..5c429702ed 100644
--- a/scene/3d/voxel_gi.cpp
+++ b/scene/3d/voxel_gi.cpp
@@ -521,7 +521,7 @@ PackedStringArray VoxelGI::get_configuration_warnings() const {
 	PackedStringArray warnings = VisualInstance3D::get_configuration_warnings();
 
 	if (OS::get_singleton()->get_current_rendering_method() == "gl_compatibility") {
-		warnings.push_back(RTR("VoxelGI nodes are not supported when using the GL Compatibility backend yet. Support will be added in a future release."));
+		warnings.push_back(RTR("VoxelGI nodes are not supported when using the Compatibility renderer yet. Support will be added in a future release."));
 	} else if (probe_data.is_null()) {
 		warnings.push_back(RTR("No VoxelGI data set, so this node is disabled. Bake static objects to enable GI."));
 	}
diff --git a/scene/resources/particle_process_material.cpp b/scene/resources/particle_process_material.cpp
index 09bc1fa8e4..a829acd0c7 100644
--- a/scene/resources/particle_process_material.cpp
+++ b/scene/resources/particle_process_material.cpp
@@ -1858,7 +1858,7 @@ void ParticleProcessMaterial::set_sub_emitter_mode(SubEmitterMode p_sub_emitter_
 	_queue_shader_change();
 	notify_property_list_changed();
 	if (sub_emitter_mode != SUB_EMITTER_DISABLED && RenderingServer::get_singleton()->is_low_end()) {
-		WARN_PRINT_ONCE_ED("Sub-emitter modes other than SUB_EMITTER_DISABLED are not supported in the GL Compatibility rendering backend.");
+		WARN_PRINT_ONCE_ED("Sub-emitter modes other than SUB_EMITTER_DISABLED are not supported in the Compatibility renderer.");
 	}
 }
 
diff --git a/servers/rendering/renderer_rd/forward_mobile/scene_shader_forward_mobile.cpp b/servers/rendering/renderer_rd/forward_mobile/scene_shader_forward_mobile.cpp
index 4525b50b6e..fbc60e3c45 100644
--- a/servers/rendering/renderer_rd/forward_mobile/scene_shader_forward_mobile.cpp
+++ b/servers/rendering/renderer_rd/forward_mobile/scene_shader_forward_mobile.cpp
@@ -162,11 +162,11 @@ void SceneShaderForwardMobile::ShaderData::set_code(const String &p_code) {
 
 #ifdef DEBUG_ENABLED
 	if (uses_sss) {
-		WARN_PRINT_ONCE_ED("Sub-surface scattering is only available when using the Forward+ rendering backend.");
+		WARN_PRINT_ONCE_ED("Subsurface scattering is only available when using the Forward+ renderer.");
 	}
 
 	if (uses_transmittance) {
-		WARN_PRINT_ONCE_ED("Transmittance is only available when using the Forward+ rendering backend.");
+		WARN_PRINT_ONCE_ED("Transmittance is only available when using the Forward+ renderer.");
 	}
 #endif
 
diff --git a/servers/rendering/rendering_device_binds.cpp b/servers/rendering/rendering_device_binds.cpp
index e41a56b0a3..507f070ba6 100644
--- a/servers/rendering/rendering_device_binds.cpp
+++ b/servers/rendering/rendering_device_binds.cpp
@@ -34,7 +34,7 @@ Error RDShaderFile::parse_versions_from_text(const String &p_text, const String
 	ERR_FAIL_NULL_V_MSG(
 			RenderingDevice::get_singleton(),
 			ERR_UNAVAILABLE,
-			"Cannot import custom .glsl shaders when running without a RenderingDevice. This can happen if you are using the headless more or the Compatibility backend.");
+			"Cannot import custom .glsl shaders when running without a RenderingDevice. This can happen if you are using the headless more or the Compatibility renderer.");
 
 	Vector<String> lines = p_text.split("\n");
 
diff --git a/servers/rendering/shader_language.cpp b/servers/rendering/shader_language.cpp
index 7c4128b0e3..4b9e1c27a5 100644
--- a/servers/rendering/shader_language.cpp
+++ b/servers/rendering/shader_language.cpp
@@ -9686,7 +9686,7 @@ Error ShaderLanguage::_parse_shader(const HashMap<StringName, FunctionInfo> &p_f
 									--texture_uniforms;
 									--texture_binding;
 									if (OS::get_singleton()->get_current_rendering_method() != "forward_plus") {
-										_set_error(RTR("'hint_normal_roughness_texture' is only available when using the Forward+ backend."));
+										_set_error(RTR("'hint_normal_roughness_texture' is only available when using the Forward+ renderer."));
 										return ERR_PARSE_ERROR;
 									}
 									if (shader_type_identifier != StringName() && String(shader_type_identifier) != "spatial") {
diff --git a/servers/rendering/storage/camera_attributes_storage.cpp b/servers/rendering/storage/camera_attributes_storage.cpp
index 2fece3ce0f..3e33954f19 100644
--- a/servers/rendering/storage/camera_attributes_storage.cpp
+++ b/servers/rendering/storage/camera_attributes_storage.cpp
@@ -67,7 +67,7 @@ void RendererCameraAttributes::camera_attributes_set_dof_blur(RID p_camera_attri
 	ERR_FAIL_NULL(cam_attributes);
 #ifdef DEBUG_ENABLED
 	if (OS::get_singleton()->get_current_rendering_method() == "gl_compatibility" && (p_far_enable || p_near_enable)) {
-		WARN_PRINT_ONCE_ED("DoF blur is only available when using the Forward+ or Mobile rendering backends.");
+		WARN_PRINT_ONCE_ED("DoF blur is only available when using the Forward+ or Mobile renderers.");
 	}
 #endif
 	cam_attributes->dof_blur_far_enabled = p_far_enable;
@@ -145,7 +145,7 @@ void RendererCameraAttributes::camera_attributes_set_auto_exposure(RID p_camera_
 	}
 #ifdef DEBUG_ENABLED
 	if (OS::get_singleton()->get_current_rendering_method() == "gl_compatibility" && p_enable) {
-		WARN_PRINT_ONCE_ED("Auto exposure is only available when using the Forward+ or Mobile rendering backends.");
+		WARN_PRINT_ONCE_ED("Auto exposure is only available when using the Forward+ or Mobile renderers.");
 	}
 #endif
 	cam_attributes->use_auto_exposure = p_enable;
diff --git a/servers/rendering/storage/environment_storage.cpp b/servers/rendering/storage/environment_storage.cpp
index e7556f9000..bfb2852d5f 100644
--- a/servers/rendering/storage/environment_storage.cpp
+++ b/servers/rendering/storage/environment_storage.cpp
@@ -341,7 +341,7 @@ void RendererEnvironmentStorage::environment_set_volumetric_fog(RID p_env, bool
 	ERR_FAIL_NULL(env);
 #ifdef DEBUG_ENABLED
 	if (OS::get_singleton()->get_current_rendering_method() != "forward_plus" && p_enable) {
-		WARN_PRINT_ONCE_ED("Volumetric fog can only be enabled when using the Forward+ rendering backend.");
+		WARN_PRINT_ONCE_ED("Volumetric fog can only be enabled when using the Forward+ renderer.");
 	}
 #endif
 	env->volumetric_fog_enabled = p_enable;
@@ -536,7 +536,7 @@ void RendererEnvironmentStorage::environment_set_ssr(RID p_env, bool p_enable, i
 	ERR_FAIL_NULL(env);
 #ifdef DEBUG_ENABLED
 	if (OS::get_singleton()->get_current_rendering_method() != "forward_plus" && p_enable) {
-		WARN_PRINT_ONCE_ED("Screen-space reflections (SSR) can only be enabled when using the Forward+ rendering backend.");
+		WARN_PRINT_ONCE_ED("Screen-space reflections (SSR) can only be enabled when using the Forward+ renderer.");
 	}
 #endif
 	env->ssr_enabled = p_enable;
@@ -583,7 +583,7 @@ void RendererEnvironmentStorage::environment_set_ssao(RID p_env, bool p_enable,
 	ERR_FAIL_NULL(env);
 #ifdef DEBUG_ENABLED
 	if (OS::get_singleton()->get_current_rendering_method() != "forward_plus" && p_enable) {
-		WARN_PRINT_ONCE_ED("Screen-space ambient occlusion (SSAO) can only be enabled when using the Forward+ rendering backend.");
+		WARN_PRINT_ONCE_ED("Screen-space ambient occlusion (SSAO) can only be enabled when using the Forward+ renderer.");
 	}
 #endif
 	env->ssao_enabled = p_enable;
@@ -658,7 +658,7 @@ void RendererEnvironmentStorage::environment_set_ssil(RID p_env, bool p_enable,
 	ERR_FAIL_NULL(env);
 #ifdef DEBUG_ENABLED
 	if (OS::get_singleton()->get_current_rendering_method() != "forward_plus" && p_enable) {
-		WARN_PRINT_ONCE_ED("Screen-space indirect lighting (SSIL) can only be enabled when using the Forward+ rendering backend.");
+		WARN_PRINT_ONCE_ED("Screen-space indirect lighting (SSIL) can only be enabled when using the Forward+ renderer.");
 	}
 #endif
 	env->ssil_enabled = p_enable;
@@ -705,7 +705,7 @@ void RendererEnvironmentStorage::environment_set_sdfgi(RID p_env, bool p_enable,
 	ERR_FAIL_NULL(env);
 #ifdef DEBUG_ENABLED
 	if (OS::get_singleton()->get_current_rendering_method() != "forward_plus" && p_enable) {
-		WARN_PRINT_ONCE_ED("SDFGI can only be enabled when using the Forward+ rendering backend.");
+		WARN_PRINT_ONCE_ED("SDFGI can only be enabled when using the Forward+ renderer.");
 	}
 #endif
 	env->sdfgi_enabled = p_enable;

From fcd32dcde63767b244f5ccc70a13164e49070ec8 Mon Sep 17 00:00:00 2001
From: tetrapod00 <145553014+tetrapod00@users.noreply.github.com>
Date: Mon, 9 Sep 2024 18:14:40 -0700
Subject: [PATCH 007/124] [Editor] Use toast (notification) instead of dialog
 when saving with no open scene

---
 editor/editor_node.cpp | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/editor/editor_node.cpp b/editor/editor_node.cpp
index 36b43b7e9b..7e4491a4a8 100644
--- a/editor/editor_node.cpp
+++ b/editor/editor_node.cpp
@@ -2768,9 +2768,7 @@ void EditorNode::_menu_option_confirm(int p_option, bool p_confirmed) {
 
 				const int saved = _save_external_resources(true);
 				if (saved > 0) {
-					show_accept(
-							vformat(TTR("The current scene has no root node, but %d modified external resource(s) and/or plugin data were saved anyway."), saved),
-							TTR("OK"));
+					EditorToaster::get_singleton()->popup_str(vformat(TTR("The current scene has no root node, but %d modified external resource(s) and/or plugin data were saved anyway."), saved), EditorToaster::SEVERITY_INFO);
 				} else if (p_option == FILE_SAVE_AS_SCENE) {
 					// Don't show this dialog when pressing Ctrl + S to avoid interfering with script saving.
 					show_accept(

From 0c2296fb1b416d0dedf2bd6afe047746c14d32fa Mon Sep 17 00:00:00 2001
From: Micky <micheledevita2@gmail.com>
Date: Mon, 11 Nov 2024 21:19:34 +0100
Subject: [PATCH 008/124] Fix C# boolean "Prints" comments in documentation

---
 doc/classes/Array.xml             | 18 +++++++++---------
 doc/classes/Dictionary.xml        |  8 ++++----
 doc/classes/Engine.xml            |  8 ++++----
 doc/classes/OS.xml                |  8 ++++----
 doc/classes/PolygonPathFinder.xml |  4 ++--
 doc/classes/String.xml            |  4 ++--
 doc/classes/StringName.xml        |  4 ++--
 7 files changed, 27 insertions(+), 27 deletions(-)

diff --git a/doc/classes/Array.xml b/doc/classes/Array.xml
index 1b7dbf7c41..983e8fc98f 100644
--- a/doc/classes/Array.xml
+++ b/doc/classes/Array.xml
@@ -183,17 +183,17 @@
 
 				public override void _Ready()
 				{
-				    // Prints true (3/3 elements evaluate to true).
+				    // Prints True (3/3 elements evaluate to true).
 				    GD.Print(new Godot.Collections.Array&gt;int&lt; { 6, 10, 6 }.All(GreaterThan5));
-				    // Prints false (1/3 elements evaluate to true).
+				    // Prints False (1/3 elements evaluate to true).
 				    GD.Print(new Godot.Collections.Array&gt;int&lt; { 4, 10, 4 }.All(GreaterThan5));
-				    // Prints false (0/3 elements evaluate to true).
+				    // Prints False (0/3 elements evaluate to true).
 				    GD.Print(new Godot.Collections.Array&gt;int&lt; { 4, 4, 4 }.All(GreaterThan5));
-				    // Prints true (0/0 elements evaluate to true).
+				    // Prints True (0/0 elements evaluate to true).
 				    GD.Print(new Godot.Collections.Array&gt;int&lt; { }.All(GreaterThan5));
 
 				    // Same as the first line above, but using a lambda function.
-				    GD.Print(new Godot.Collections.Array&gt;int&lt; { 6, 10, 6 }.All(element =&gt; element &gt; 5)); // Prints true
+				    GD.Print(new Godot.Collections.Array&gt;int&lt; { 6, 10, 6 }.All(element =&gt; element &gt; 5)); // Prints True
 				}
 				[/csharp]
 				[/codeblocks]
@@ -462,10 +462,10 @@
 				[csharp]
 				var arr = new Godot.Collections.Array { "inside", 7 };
 				// By C# convention, this method is renamed to `Contains`.
-				GD.Print(arr.Contains("inside"));  // Prints true
-				GD.Print(arr.Contains("outside")); // Prints false
-				GD.Print(arr.Contains(7));         // Prints true
-				GD.Print(arr.Contains("7"));       // Prints false
+				GD.Print(arr.Contains("inside"));  // Prints True
+				GD.Print(arr.Contains("outside")); // Prints False
+				GD.Print(arr.Contains(7));         // Prints True
+				GD.Print(arr.Contains("7"));       // Prints False
 				[/csharp]
 				[/codeblocks]
 				In GDScript, this is equivalent to the [code]in[/code] operator:
diff --git a/doc/classes/Dictionary.xml b/doc/classes/Dictionary.xml
index 5c9b22fe4a..2e02de54a8 100644
--- a/doc/classes/Dictionary.xml
+++ b/doc/classes/Dictionary.xml
@@ -281,9 +281,9 @@
 				    { 210, default },
 				};
 
-				GD.Print(myDict.ContainsKey("Godot")); // Prints true
-				GD.Print(myDict.ContainsKey(210));     // Prints true
-				GD.Print(myDict.ContainsKey(4));       // Prints false
+				GD.Print(myDict.ContainsKey("Godot")); // Prints True
+				GD.Print(myDict.ContainsKey(210));     // Prints True
+				GD.Print(myDict.ContainsKey(4));       // Prints False
 				[/csharp]
 				[/codeblocks]
 				In GDScript, this is equivalent to the [code]in[/code] operator:
@@ -321,7 +321,7 @@
 				var dict2 = new Godot.Collections.Dictionary{{"A", 10}, {"B", 2}};
 
 				// Godot.Collections.Dictionary has no Hash() method. Use GD.Hash() instead.
-				GD.Print(GD.Hash(dict1) == GD.Hash(dict2)); // Prints true
+				GD.Print(GD.Hash(dict1) == GD.Hash(dict2)); // Prints True
 				[/csharp]
 				[/codeblocks]
 				[b]Note:[/b] Dictionaries with the same entries but in a different order will not have the same hash.
diff --git a/doc/classes/Engine.xml b/doc/classes/Engine.xml
index bba5157053..34ba5ec655 100644
--- a/doc/classes/Engine.xml
+++ b/doc/classes/Engine.xml
@@ -223,10 +223,10 @@
 				print(Engine.has_singleton("Unknown"))     # Prints false
 				[/gdscript]
 				[csharp]
-				GD.Print(Engine.HasSingleton("OS"));          // Prints true
-				GD.Print(Engine.HasSingleton("Engine"));      // Prints true
-				GD.Print(Engine.HasSingleton("AudioServer")); // Prints true
-				GD.Print(Engine.HasSingleton("Unknown"));     // Prints false
+				GD.Print(Engine.HasSingleton("OS"));          // Prints True
+				GD.Print(Engine.HasSingleton("Engine"));      // Prints True
+				GD.Print(Engine.HasSingleton("AudioServer")); // Prints True
+				GD.Print(Engine.HasSingleton("Unknown"));     // Prints False
 				[/csharp]
 				[/codeblocks]
 				[b]Note:[/b] Global singletons are not the same as autoloaded nodes, which are configurable in the project settings.
diff --git a/doc/classes/OS.xml b/doc/classes/OS.xml
index 777950c075..90cf1871cc 100644
--- a/doc/classes/OS.xml
+++ b/doc/classes/OS.xml
@@ -605,10 +605,10 @@
 				print(OS.is_keycode_unicode(KEY_ESCAPE)) # Prints false
 				[/gdscript]
 				[csharp]
-				GD.Print(OS.IsKeycodeUnicode((long)Key.G));      // Prints true
-				GD.Print(OS.IsKeycodeUnicode((long)Key.Kp4));    // Prints true
-				GD.Print(OS.IsKeycodeUnicode((long)Key.Tab));    // Prints false
-				GD.Print(OS.IsKeycodeUnicode((long)Key.Escape)); // Prints false
+				GD.Print(OS.IsKeycodeUnicode((long)Key.G));      // Prints True
+				GD.Print(OS.IsKeycodeUnicode((long)Key.Kp4));    // Prints True
+				GD.Print(OS.IsKeycodeUnicode((long)Key.Tab));    // Prints False
+				GD.Print(OS.IsKeycodeUnicode((long)Key.Escape)); // Prints False
 				[/csharp]
 				[/codeblocks]
 			</description>
diff --git a/doc/classes/PolygonPathFinder.xml b/doc/classes/PolygonPathFinder.xml
index b70633883c..98734c3e9b 100644
--- a/doc/classes/PolygonPathFinder.xml
+++ b/doc/classes/PolygonPathFinder.xml
@@ -62,8 +62,8 @@
 				};
 				var connections = new int[] { 0, 1, 1, 2, 2, 0 };
 				polygonPathFinder.Setup(points, connections);
-				GD.Print(polygonPathFinder.IsPointInside(new Vector2(0.2f, 0.2f))); // Prints true
-				GD.Print(polygonPathFinder.IsPointInside(new Vector2(1.0f, 1.0f))); // Prints false
+				GD.Print(polygonPathFinder.IsPointInside(new Vector2(0.2f, 0.2f))); // Prints True
+				GD.Print(polygonPathFinder.IsPointInside(new Vector2(1.0f, 1.0f))); // Prints False
 				[/csharp]
 				[/codeblocks]
 			</description>
diff --git a/doc/classes/String.xml b/doc/classes/String.xml
index 44795af473..e02b1d76cc 100644
--- a/doc/classes/String.xml
+++ b/doc/classes/String.xml
@@ -139,8 +139,8 @@
 				print("I" in "team")         # Prints false
 				[/gdscript]
 				[csharp]
-				GD.Print("Node".Contains("de")); // Prints true
-				GD.Print("team".Contains("I"));  // Prints false
+				GD.Print("Node".Contains("de")); // Prints True
+				GD.Print("team".Contains("I"));  // Prints False
 				[/csharp]
 				[/codeblocks]
 				If you need to know where [param what] is within the string, use [method find]. See also [method containsn].
diff --git a/doc/classes/StringName.xml b/doc/classes/StringName.xml
index 3a2b492496..163a2303af 100644
--- a/doc/classes/StringName.xml
+++ b/doc/classes/StringName.xml
@@ -122,8 +122,8 @@
 				print("I" in "team")         # Prints false
 				[/gdscript]
 				[csharp]
-				GD.Print("Node".Contains("de")); // Prints true
-				GD.Print("team".Contains("I"));  // Prints false
+				GD.Print("Node".Contains("de")); // Prints True
+				GD.Print("team".Contains("I"));  // Prints False
 				[/csharp]
 				[/codeblocks]
 				If you need to know where [param what] is within the string, use [method find]. See also [method containsn].

From 9b8833dbbfa5461f7de3410914d9e17127870053 Mon Sep 17 00:00:00 2001
From: Hugo Locurcio <hugo.locurcio@hugo.pro>
Date: Fri, 2 Aug 2024 17:55:58 +0200
Subject: [PATCH 009/124] Tweak warning/error formatting in EditorLog to be
 closer to console output

This makes the output easier to understand when it's copied to a plain
text file (without visible colors).
---
 editor/editor_log.cpp | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/editor/editor_log.cpp b/editor/editor_log.cpp
index db26a75cb8..e738b6e366 100644
--- a/editor/editor_log.cpp
+++ b/editor/editor_log.cpp
@@ -358,14 +358,18 @@ void EditorLog::_add_log_line(LogMessage &p_message, bool p_replace_previous) {
 			log->push_color(theme_cache.error_color);
 			Ref<Texture2D> icon = theme_cache.error_icon;
 			log->add_image(icon);
-			log->add_text(" ");
+			log->push_bold();
+			log->add_text(" ERROR: ");
+			log->pop(); // bold
 			tool_button->set_button_icon(icon);
 		} break;
 		case MSG_TYPE_WARNING: {
 			log->push_color(theme_cache.warning_color);
 			Ref<Texture2D> icon = theme_cache.warning_icon;
 			log->add_image(icon);
-			log->add_text(" ");
+			log->push_bold();
+			log->add_text(" WARNING: ");
+			log->pop(); // bold
 			tool_button->set_button_icon(icon);
 		} break;
 		case MSG_TYPE_EDITOR: {

From d6a4fe6c052ec5a71d1a25d175ddfb17763d6bf1 Mon Sep 17 00:00:00 2001
From: David Snopek <dsnopek@gmail.com>
Date: Tue, 12 Nov 2024 14:36:58 -0600
Subject: [PATCH 010/124] XR: Allow locking the camera to the `XROrigin3D` for
 benchmarking or automated testing

---
 doc/classes/XRServer.xml                  |  4 ++++
 servers/rendering/renderer_scene_cull.cpp | 12 +++++++++++-
 servers/xr_server.cpp                     |  7 +++++++
 servers/xr_server.h                       |  4 ++++
 4 files changed, 26 insertions(+), 1 deletion(-)

diff --git a/doc/classes/XRServer.xml b/doc/classes/XRServer.xml
index 49b13986ca..247d2ab938 100644
--- a/doc/classes/XRServer.xml
+++ b/doc/classes/XRServer.xml
@@ -111,6 +111,10 @@
 		</method>
 	</methods>
 	<members>
+		<member name="camera_locked_to_origin" type="bool" setter="set_camera_locked_to_origin" getter="is_camera_locked_to_origin" default="false">
+			If set to [code]true[/code], the scene will be rendered as if the camera is locked to the [XROrigin3D].
+			[b]Note:[/b] This doesn't provide a very comfortable experience for users. This setting exists for doing benchmarking or automated testing, where you want to control what is rendered via code.
+		</member>
 		<member name="primary_interface" type="XRInterface" setter="set_primary_interface" getter="get_primary_interface">
 			The primary [XRInterface] currently bound to the [XRServer].
 		</member>
diff --git a/servers/rendering/renderer_scene_cull.cpp b/servers/rendering/renderer_scene_cull.cpp
index 11ca7de44f..eec3ef4c2f 100644
--- a/servers/rendering/renderer_scene_cull.cpp
+++ b/servers/rendering/renderer_scene_cull.cpp
@@ -2773,6 +2773,8 @@ void RendererSceneCull::render_camera(const Ref<RenderSceneBuffers> &p_render_bu
 
 		camera_data.set_camera(transform, projection, is_orthogonal, is_frustum, vaspect, jitter, taa_frame_count, camera->visible_layers);
 	} else {
+		XRServer *xr_server = XRServer::get_singleton();
+
 		// Setup our camera for our XR interface.
 		// We can support multiple views here each with their own camera
 		Transform3D transforms[RendererSceneRender::MAX_RENDER_VIEWS];
@@ -2783,7 +2785,7 @@ void RendererSceneCull::render_camera(const Ref<RenderSceneBuffers> &p_render_bu
 
 		float aspect = p_viewport_size.width / (float)p_viewport_size.height;
 
-		Transform3D world_origin = XRServer::get_singleton()->get_world_origin();
+		Transform3D world_origin = xr_server->get_world_origin();
 
 		// We ignore our camera position, it will have been positioned with a slightly old tracking position.
 		// Instead we take our origin point and have our XR interface add fresh tracking data! Whoohoo!
@@ -2792,6 +2794,14 @@ void RendererSceneCull::render_camera(const Ref<RenderSceneBuffers> &p_render_bu
 			projections[v] = p_xr_interface->get_projection_for_view(v, aspect, camera->znear, camera->zfar);
 		}
 
+		// If requested, we move the views to be rendered as if the HMD is at the XROrigin.
+		if (unlikely(xr_server->is_camera_locked_to_origin())) {
+			Transform3D camera_reset = p_xr_interface->get_camera_transform().affine_inverse() * xr_server->get_reference_frame().affine_inverse();
+			for (uint32_t v = 0; v < view_count; v++) {
+				transforms[v] *= camera_reset;
+			}
+		}
+
 		if (view_count == 1) {
 			camera_data.set_camera(transforms[0], projections[0], false, false, camera->vaspect, jitter, p_jitter_phase_count, camera->visible_layers);
 		} else if (view_count == 2) {
diff --git a/servers/xr_server.cpp b/servers/xr_server.cpp
index 4c4781fdef..2f9479433a 100644
--- a/servers/xr_server.cpp
+++ b/servers/xr_server.cpp
@@ -62,9 +62,12 @@ void XRServer::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("clear_reference_frame"), &XRServer::clear_reference_frame);
 	ClassDB::bind_method(D_METHOD("center_on_hmd", "rotation_mode", "keep_height"), &XRServer::center_on_hmd);
 	ClassDB::bind_method(D_METHOD("get_hmd_transform"), &XRServer::get_hmd_transform);
+	ClassDB::bind_method(D_METHOD("set_camera_locked_to_origin", "enabled"), &XRServer::set_camera_locked_to_origin);
+	ClassDB::bind_method(D_METHOD("is_camera_locked_to_origin"), &XRServer::is_camera_locked_to_origin);
 
 	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "world_scale"), "set_world_scale", "get_world_scale");
 	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "world_origin"), "set_world_origin", "get_world_origin");
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "camera_locked_to_origin"), "set_camera_locked_to_origin", "is_camera_locked_to_origin");
 
 	ClassDB::bind_method(D_METHOD("add_interface", "interface"), &XRServer::add_interface);
 	ClassDB::bind_method(D_METHOD("get_interface_count"), &XRServer::get_interface_count);
@@ -241,6 +244,10 @@ Transform3D XRServer::get_hmd_transform() {
 	return hmd_transform;
 }
 
+void XRServer::set_camera_locked_to_origin(bool p_enable) {
+	camera_locked_to_origin = p_enable;
+}
+
 void XRServer::add_interface(const Ref<XRInterface> &p_interface) {
 	ERR_FAIL_COND(p_interface.is_null());
 
diff --git a/servers/xr_server.h b/servers/xr_server.h
index cd9c241bb0..6867ce9aa0 100644
--- a/servers/xr_server.h
+++ b/servers/xr_server.h
@@ -96,6 +96,7 @@ private:
 	double world_scale = 1.0; /* scale by which we multiply our tracker positions */
 	Transform3D world_origin; /* our world origin point, maps a location in our virtual world to the origin point in our real world tracking volume */
 	Transform3D reference_frame; /* our reference frame */
+	bool camera_locked_to_origin = false;
 
 	// As we may be updating our main state for our next frame while we're still rendering our previous frame,
 	// we need to keep copies around.
@@ -198,6 +199,9 @@ public:
 	*/
 	Transform3D get_hmd_transform();
 
+	void set_camera_locked_to_origin(bool p_enable);
+	inline bool is_camera_locked_to_origin() const { return camera_locked_to_origin; }
+
 	/*
 		Interfaces are objects that 'glue' Godot to an AR or VR SDK such as the Oculus SDK, OpenVR, OpenHMD, etc.
 	*/

From fb7507830863943ecc39b427551792136ded1b29 Mon Sep 17 00:00:00 2001
From: Chris <9290150+ChrisBase@users.noreply.github.com>
Date: Fri, 15 Nov 2024 20:42:03 +0100
Subject: [PATCH 011/124] Changed some image error messages to print the file
 path

---
 drivers/gles3/storage/texture_storage.cpp        | 16 +++++++++++++---
 .../renderer_rd/storage_rd/texture_storage.cpp   | 15 +++++++++++++--
 2 files changed, 26 insertions(+), 5 deletions(-)

diff --git a/drivers/gles3/storage/texture_storage.cpp b/drivers/gles3/storage/texture_storage.cpp
index 14bbe635a4..bdcd9fe484 100644
--- a/drivers/gles3/storage/texture_storage.cpp
+++ b/drivers/gles3/storage/texture_storage.cpp
@@ -1099,7 +1099,11 @@ Ref<Image> TextureStorage::texture_2d_get(RID p_texture) const {
 
 		ERR_FAIL_COND_V(data.is_empty(), Ref<Image>());
 		image = Image::create_from_data(texture->alloc_width, texture->alloc_height, texture->mipmaps > 1, texture->real_format, data);
-		ERR_FAIL_COND_V(image->is_empty(), Ref<Image>());
+		if (image->is_empty()) {
+			const String &path_str = texture->path.is_empty() ? "with no path" : vformat("with path '%s'", texture->path);
+			ERR_FAIL_V_MSG(Ref<Image>(), vformat("Texture %s has no data.", path_str));
+		}
+
 		if (texture->format != texture->real_format) {
 			image->convert(texture->format);
 		}
@@ -1155,7 +1159,10 @@ Ref<Image> TextureStorage::texture_2d_get(RID p_texture) const {
 
 		ERR_FAIL_COND_V(data.is_empty(), Ref<Image>());
 		image = Image::create_from_data(texture->alloc_width, texture->alloc_height, false, Image::FORMAT_RGBA8, data);
-		ERR_FAIL_COND_V(image->is_empty(), Ref<Image>());
+		if (image->is_empty()) {
+			const String &path_str = texture->path.is_empty() ? "with no path" : vformat("with path '%s'", texture->path);
+			ERR_FAIL_V_MSG(Ref<Image>(), vformat("Texture %s has no data.", path_str));
+		}
 
 		if (texture->format != Image::FORMAT_RGBA8) {
 			image->convert(texture->format);
@@ -1227,7 +1234,10 @@ Ref<Image> TextureStorage::texture_2d_layer_get(RID p_texture, int p_layer) cons
 
 	ERR_FAIL_COND_V(data.is_empty(), Ref<Image>());
 	Ref<Image> image = Image::create_from_data(texture->width, texture->height, false, Image::FORMAT_RGBA8, data);
-	ERR_FAIL_COND_V(image->is_empty(), Ref<Image>());
+	if (image->is_empty()) {
+		const String &path_str = texture->path.is_empty() ? "with no path" : vformat("with path '%s'", texture->path);
+		ERR_FAIL_V_MSG(Ref<Image>(), vformat("Texture %s has no data.", path_str));
+	}
 
 	if (texture->format != Image::FORMAT_RGBA8) {
 		image->convert(texture->format);
diff --git a/servers/rendering/renderer_rd/storage_rd/texture_storage.cpp b/servers/rendering/renderer_rd/storage_rd/texture_storage.cpp
index 8a9499dfc9..2b3fb6d384 100644
--- a/servers/rendering/renderer_rd/storage_rd/texture_storage.cpp
+++ b/servers/rendering/renderer_rd/storage_rd/texture_storage.cpp
@@ -1446,7 +1446,11 @@ Ref<Image> TextureStorage::texture_2d_get(RID p_texture) const {
 		image = Image::create_from_data(tex->width, tex->height, tex->mipmaps > 1, tex->validated_format, data);
 	}
 
-	ERR_FAIL_COND_V(image->is_empty(), Ref<Image>());
+	if (image->is_empty()) {
+		const String &path_str = tex->path.is_empty() ? "with no path" : vformat("with path '%s'", tex->path);
+		ERR_FAIL_V_MSG(Ref<Image>(), vformat("Texture %s has no data.", path_str));
+	}
+
 	if (tex->format != tex->validated_format) {
 		image->convert(tex->format);
 	}
@@ -1467,7 +1471,10 @@ Ref<Image> TextureStorage::texture_2d_layer_get(RID p_texture, int p_layer) cons
 	Vector<uint8_t> data = RD::get_singleton()->texture_get_data(tex->rd_texture, p_layer);
 	ERR_FAIL_COND_V(data.is_empty(), Ref<Image>());
 	Ref<Image> image = Image::create_from_data(tex->width, tex->height, tex->mipmaps > 1, tex->validated_format, data);
-	ERR_FAIL_COND_V(image->is_empty(), Ref<Image>());
+	if (image->is_empty()) {
+		const String &path_str = tex->path.is_empty() ? "with no path" : vformat("with path '%s'", tex->path);
+		ERR_FAIL_V_MSG(Ref<Image>(), vformat("Texture %s has no data.", path_str));
+	}
 	if (tex->format != tex->validated_format) {
 		image->convert(tex->format);
 	}
@@ -1494,6 +1501,10 @@ Vector<Ref<Image>> TextureStorage::texture_3d_get(RID p_texture) const {
 
 		Ref<Image> img = Image::create_from_data(bs.size.width, bs.size.height, false, tex->validated_format, sub_region);
 		ERR_FAIL_COND_V(img->is_empty(), Vector<Ref<Image>>());
+		if (img->is_empty()) {
+			const String &path_str = tex->path.is_empty() ? "with no path" : vformat("with path '%s'", tex->path);
+			ERR_FAIL_V_MSG(Vector<Ref<Image>>(), vformat("Texture %s has no data.", path_str));
+		}
 		if (tex->format != tex->validated_format) {
 			img->convert(tex->format);
 		}

From f03e081efb24219d01bff60663c3ba00828249c2 Mon Sep 17 00:00:00 2001
From: Giganzo <158825920+Giganzo@users.noreply.github.com>
Date: Fri, 9 Aug 2024 15:29:35 +0200
Subject: [PATCH 012/124] Fix Lock and Group for canvas items not updated in
 editor after changed in SceenTree

---
 editor/plugins/canvas_item_editor_plugin.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/editor/plugins/canvas_item_editor_plugin.cpp b/editor/plugins/canvas_item_editor_plugin.cpp
index f91a052a24..632c2709f1 100644
--- a/editor/plugins/canvas_item_editor_plugin.cpp
+++ b/editor/plugins/canvas_item_editor_plugin.cpp
@@ -4063,6 +4063,7 @@ void CanvasItemEditor::_notification(int p_what) {
 		case NOTIFICATION_READY: {
 			_update_lock_and_group_button();
 
+			SceneTreeDock::get_singleton()->get_tree_editor()->connect("node_changed", callable_mp(this, &CanvasItemEditor::_update_lock_and_group_button));
 			ProjectSettings::get_singleton()->connect("settings_changed", callable_mp(this, &CanvasItemEditor::_project_settings_changed));
 		} break;
 

From ce512d3eeaf82ef06fa40e3b43514dfaa930e573 Mon Sep 17 00:00:00 2001
From: kobewi <kobewi4e@gmail.com>
Date: Sat, 16 Nov 2024 23:07:23 +0100
Subject: [PATCH 013/124] Clarify Button's text clipping

---
 doc/classes/Button.xml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/doc/classes/Button.xml b/doc/classes/Button.xml
index 5b3f86c9f5..5103134f65 100644
--- a/doc/classes/Button.xml
+++ b/doc/classes/Button.xml
@@ -47,7 +47,7 @@
 			If set to something other than [constant TextServer.AUTOWRAP_OFF], the text gets wrapped inside the node's bounding rectangle.
 		</member>
 		<member name="clip_text" type="bool" setter="set_clip_text" getter="get_clip_text" default="false">
-			When this property is enabled, text that is too large to fit the button is clipped, when disabled the Button will always be wide enough to hold the text.
+			If [code]true[/code], text that is too large to fit the button is clipped horizontally. If [code]false[/code], the button will always be wide enough to hold the text. The text is not vertically clipped, and the button's height is not affected by this property.
 		</member>
 		<member name="expand_icon" type="bool" setter="set_expand_icon" getter="is_expand_icon" default="false">
 			When enabled, the button's icon will expand/shrink to fit the button's size while keeping its aspect. See also [theme_item icon_max_width].

From 6f76ef4bddcb8d01d66c99621e67267444aa16a2 Mon Sep 17 00:00:00 2001
From: Mateus Reis <mateus.sureis@gmail.com>
Date: Wed, 20 Nov 2024 04:28:31 +0200
Subject: [PATCH 014/124] Allow dragging to specific folders in filesystem dock

---
 editor/editor_node.cpp     | 6 +++++-
 editor/filesystem_dock.cpp | 9 +++++++++
 editor/filesystem_dock.h   | 1 +
 3 files changed, 15 insertions(+), 1 deletion(-)

diff --git a/editor/editor_node.cpp b/editor/editor_node.cpp
index f056a477c4..817f193ac0 100644
--- a/editor/editor_node.cpp
+++ b/editor/editor_node.cpp
@@ -5828,7 +5828,11 @@ PopupMenu *EditorNode::get_export_as_menu() {
 }
 
 void EditorNode::_dropped_files(const Vector<String> &p_files) {
-	String to_path = ProjectSettings::get_singleton()->globalize_path(FileSystemDock::get_singleton()->get_current_directory());
+	String to_path = FileSystemDock::get_singleton()->get_folder_path_at_mouse_position();
+	if (to_path.is_empty()) {
+		to_path = FileSystemDock::get_singleton()->get_current_directory();
+	}
+	to_path = ProjectSettings::get_singleton()->globalize_path(to_path);
 
 	_add_dropped_files_recursive(p_files, to_path);
 
diff --git a/editor/filesystem_dock.cpp b/editor/filesystem_dock.cpp
index c7e12d1f3b..ce00ff2ba9 100644
--- a/editor/filesystem_dock.cpp
+++ b/editor/filesystem_dock.cpp
@@ -2749,6 +2749,15 @@ void FileSystemDock::remove_resource_tooltip_plugin(const Ref<EditorResourceTool
 	tooltip_plugins.remove_at(index);
 }
 
+String FileSystemDock::get_folder_path_at_mouse_position() const {
+	TreeItem *item = tree->get_item_at_position(tree->get_local_mouse_position());
+	if (!item) {
+		return String();
+	}
+	String fpath = item->get_metadata(0);
+	return fpath.get_base_dir();
+}
+
 Control *FileSystemDock::create_tooltip_for_path(const String &p_path) const {
 	if (p_path == "Favorites") {
 		// No tooltip for the "Favorites" group.
diff --git a/editor/filesystem_dock.h b/editor/filesystem_dock.h
index d2e403a8af..2be99a084e 100644
--- a/editor/filesystem_dock.h
+++ b/editor/filesystem_dock.h
@@ -380,6 +380,7 @@ public:
 
 	String get_current_path() const;
 	String get_current_directory() const;
+	String get_folder_path_at_mouse_position() const;
 
 	void navigate_to_path(const String &p_path);
 	void focus_on_path();

From daa665c4cb971c0bec24e58418982cf9249ee764 Mon Sep 17 00:00:00 2001
From: Rob Mayoff <mayoff@dqd.com>
Date: Fri, 22 Nov 2024 13:13:41 -0600
Subject: [PATCH 015/124] fix copy/paste error (fixes #99518)

Replace erroneous use of `/` with `%`.
---
 core/variant/variant_op.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/core/variant/variant_op.h b/core/variant/variant_op.h
index 0bd8b830e0..e95f2b49bf 100644
--- a/core/variant/variant_op.h
+++ b/core/variant/variant_op.h
@@ -317,7 +317,7 @@ public:
 		*VariantGetInternalPtr<Vector2i>::get_ptr(r_ret) = *VariantGetInternalPtr<Vector2i>::get_ptr(left) % *VariantGetInternalPtr<Vector2i>::get_ptr(right);
 	}
 	static void ptr_evaluate(const void *left, const void *right, void *r_ret) {
-		PtrToArg<Vector2i>::encode(PtrToArg<Vector2i>::convert(left) / PtrToArg<Vector2i>::convert(right), r_ret);
+		PtrToArg<Vector2i>::encode(PtrToArg<Vector2i>::convert(left) % PtrToArg<Vector2i>::convert(right), r_ret);
 	}
 	static Variant::Type get_return_type() { return GetTypeInfo<Vector2i>::VARIANT_TYPE; }
 };

From 7ecdfc8b5289722f53dc46be8a7d54e7a692a8d9 Mon Sep 17 00:00:00 2001
From: Chaosus <chaosus89@gmail.com>
Date: Sun, 24 Nov 2024 14:18:53 +0300
Subject: [PATCH 016/124] Add `samplerExternalOES` type to shader globals

---
 editor/shader_globals_editor.cpp       | 9 +++++++++
 scene/main/shader_globals_override.cpp | 5 +++++
 servers/rendering_server.cpp           | 2 ++
 3 files changed, 16 insertions(+)

diff --git a/editor/shader_globals_editor.cpp b/editor/shader_globals_editor.cpp
index a53b621097..2b368d0c90 100644
--- a/editor/shader_globals_editor.cpp
+++ b/editor/shader_globals_editor.cpp
@@ -65,6 +65,7 @@ static const char *global_var_type_names[RS::GLOBAL_VAR_TYPE_MAX] = {
 	"sampler2DArray",
 	"sampler3D",
 	"samplerCube",
+	"samplerExternalOES",
 };
 
 class ShaderGlobalsEditorInterface : public Object {
@@ -232,6 +233,11 @@ protected:
 					pinfo.hint = PROPERTY_HINT_RESOURCE_TYPE;
 					pinfo.hint_string = "Cubemap,CompressedCubemap";
 				} break;
+				case RS::GLOBAL_VAR_TYPE_SAMPLEREXT: {
+					pinfo.type = Variant::OBJECT;
+					pinfo.hint = PROPERTY_HINT_RESOURCE_TYPE;
+					pinfo.hint_string = "ExternalTexture";
+				} break;
 				default: {
 				} break;
 			}
@@ -339,6 +345,9 @@ static Variant create_var(RS::GlobalShaderParameterType p_type) {
 		case RS::GLOBAL_VAR_TYPE_SAMPLERCUBE: {
 			return "";
 		}
+		case RS::GLOBAL_VAR_TYPE_SAMPLEREXT: {
+			return "";
+		}
 		default: {
 			return Variant();
 		}
diff --git a/scene/main/shader_globals_override.cpp b/scene/main/shader_globals_override.cpp
index 41e0aa739e..4007cd58e9 100644
--- a/scene/main/shader_globals_override.cpp
+++ b/scene/main/shader_globals_override.cpp
@@ -197,6 +197,11 @@ void ShaderGlobalsOverride::_get_property_list(List<PropertyInfo> *p_list) const
 				pinfo.hint = PROPERTY_HINT_RESOURCE_TYPE;
 				pinfo.hint_string = "Cubemap";
 			} break;
+			case RS::GLOBAL_VAR_TYPE_SAMPLEREXT: {
+				pinfo.type = Variant::OBJECT;
+				pinfo.hint = PROPERTY_HINT_RESOURCE_TYPE;
+				pinfo.hint_string = "ExternalTexture";
+			} break;
 			default: {
 			} break;
 		}
diff --git a/servers/rendering_server.cpp b/servers/rendering_server.cpp
index 2051d0caac..fa52d2025e 100644
--- a/servers/rendering_server.cpp
+++ b/servers/rendering_server.cpp
@@ -1868,6 +1868,8 @@ int RenderingServer::global_shader_uniform_type_get_shader_datatype(GlobalShader
 			return ShaderLanguage::TYPE_SAMPLER3D;
 		case RS::GLOBAL_VAR_TYPE_SAMPLERCUBE:
 			return ShaderLanguage::TYPE_SAMPLERCUBE;
+		case RS::GLOBAL_VAR_TYPE_SAMPLEREXT:
+			return ShaderLanguage::TYPE_SAMPLEREXT;
 		default:
 			return ShaderLanguage::TYPE_MAX; // Invalid or not found.
 	}

From 344d678bbecf15fe76407e635c30713aef79fc3d Mon Sep 17 00:00:00 2001
From: A Thousand Ships <96648715+AThousandShips@users.noreply.github.com>
Date: Thu, 21 Nov 2024 13:32:53 +0100
Subject: [PATCH 017/124] [Buildsystem] Tweak cache sizes for CI

---
 .github/workflows/android_builds.yml | 4 ++++
 .github/workflows/ios_builds.yml     | 1 +
 .github/workflows/linux_builds.yml   | 7 +++++++
 .github/workflows/macos_builds.yml   | 4 ++++
 .github/workflows/web_builds.yml     | 1 +
 .github/workflows/windows_builds.yml | 5 +++++
 6 files changed, 22 insertions(+)

diff --git a/.github/workflows/android_builds.yml b/.github/workflows/android_builds.yml
index 950e1e51cc..2803ff4b5e 100644
--- a/.github/workflows/android_builds.yml
+++ b/.github/workflows/android_builds.yml
@@ -25,18 +25,21 @@ jobs:
             target: editor
             tests: false
             sconsflags: arch=arm64 production=yes swappy=yes
+            cache-limit: 1
 
           - name: Template arm32 (target=template_release, arch=arm32)
             cache-name: android-template-arm32
             target: template_release
             tests: false
             sconsflags: arch=arm32 swappy=yes
+            cache-limit: 1
 
           - name: Template arm64 (target=template_release, arch=arm64)
             cache-name: android-template-arm64
             target: template_release
             tests: false
             sconsflags: arch=arm64 swappy=yes
+            cache-limit: 1
 
     steps:
       - name: Checkout
@@ -77,6 +80,7 @@ jobs:
           platform: android
           target: ${{ matrix.target }}
           tests: ${{ matrix.tests }}
+          scons-cache-limit: ${{ matrix.cache-limit }}
 
       - name: Save Godot build cache
         uses: ./.github/actions/godot-cache-save
diff --git a/.github/workflows/ios_builds.yml b/.github/workflows/ios_builds.yml
index 8513429f9a..270204ebe5 100644
--- a/.github/workflows/ios_builds.yml
+++ b/.github/workflows/ios_builds.yml
@@ -37,6 +37,7 @@ jobs:
           platform: ios
           target: template_release
           tests: false
+          scons-cache-limit: 1
 
       - name: Save Godot build cache
         uses: ./.github/actions/godot-cache-save
diff --git a/.github/workflows/linux_builds.yml b/.github/workflows/linux_builds.yml
index bd4e2856e3..972e29b7e5 100644
--- a/.github/workflows/linux_builds.yml
+++ b/.github/workflows/linux_builds.yml
@@ -35,6 +35,7 @@ jobs:
             proj-conv: true
             api-compat: true
             artifact: true
+            cache-limit: 1
 
           - name: Editor with doubles and GCC sanitizers (target=editor, tests=yes, dev_build=yes, scu_build=yes, precision=double, use_asan=yes, use_ubsan=yes, linker=gold)
             cache-name: linux-editor-double-sanitizers
@@ -49,6 +50,7 @@ jobs:
             api-dump: true
             # Skip 2GiB artifact speeding up action.
             artifact: false
+            cache-limit: 7
 
           - name: Editor with clang sanitizers (target=editor, tests=yes, dev_build=yes, use_asan=yes, use_ubsan=yes, use_llvm=yes, linker=lld)
             cache-name: linux-editor-llvm-sanitizers
@@ -61,6 +63,7 @@ jobs:
             artifact: false
             # Test our oldest supported SCons/Python versions on one arbitrary editor build.
             legacy-scons: true
+            cache-limit: 7
 
           - name: Editor with ThreadSanitizer (target=editor, tests=yes, dev_build=yes, use_tsan=yes, use_llvm=yes, linker=lld)
             cache-name: linux-editor-thread-sanitizer
@@ -71,6 +74,7 @@ jobs:
             build-mono: false
             # Skip 2GiB artifact speeding up action.
             artifact: false
+            cache-limit: 5
 
           - name: Template w/ Mono (target=template_release, tests=yes)
             cache-name: linux-template-mono
@@ -80,6 +84,7 @@ jobs:
             build-mono: false
             tests: true
             artifact: true
+            cache-limit: 1
 
           - name: Minimal template (target=template_release, tests=yes, everything disabled)
             cache-name: linux-template-minimal
@@ -88,6 +93,7 @@ jobs:
             bin: ./bin/godot.linuxbsd.template_release.x86_64
             tests: true
             artifact: true
+            cache-limit: 1
 
     steps:
       - name: Checkout
@@ -143,6 +149,7 @@ jobs:
           platform: linuxbsd
           target: ${{ matrix.target }}
           tests: ${{ matrix.tests }}
+          scons-cache-limit: ${{ matrix.cache-limit }}
 
       - name: Save Godot build cache
         uses: ./.github/actions/godot-cache-save
diff --git a/.github/workflows/macos_builds.yml b/.github/workflows/macos_builds.yml
index fcf4b00afb..3fedc2a5c9 100644
--- a/.github/workflows/macos_builds.yml
+++ b/.github/workflows/macos_builds.yml
@@ -25,6 +25,7 @@ jobs:
             target: editor
             tests: true
             bin: ./bin/godot.macos.editor.universal
+            cache-limit: 1
 
           - name: Template (target=template_release, tests=yes)
             cache-name: macos-template
@@ -32,6 +33,7 @@ jobs:
             tests: true
             sconsflags: debug_symbols=no
             bin: ./bin/godot.macos.template_release.universal
+            cache-limit: 1
 
     steps:
       - name: Checkout
@@ -59,6 +61,7 @@ jobs:
           platform: macos
           target: ${{ matrix.target }}
           tests: ${{ matrix.tests }}
+          scons-cache-limit: 0 # Only cap on second run to avoid purging unnecessarily
 
       - name: Compilation (arm64)
         uses: ./.github/actions/godot-build
@@ -67,6 +70,7 @@ jobs:
           platform: macos
           target: ${{ matrix.target }}
           tests: ${{ matrix.tests }}
+          scons-cache-limit: ${{ matrix.cache-limit }}
 
       - name: Save Godot build cache
         uses: ./.github/actions/godot-cache-save
diff --git a/.github/workflows/web_builds.yml b/.github/workflows/web_builds.yml
index 9ed8475769..dac6270b2b 100644
--- a/.github/workflows/web_builds.yml
+++ b/.github/workflows/web_builds.yml
@@ -67,6 +67,7 @@ jobs:
           platform: web
           target: ${{ matrix.target }}
           tests: ${{ matrix.tests }}
+          scons-cache-limit: 0.5
 
       - name: Save Godot build cache
         uses: ./.github/actions/godot-cache-save
diff --git a/.github/workflows/windows_builds.yml b/.github/workflows/windows_builds.yml
index 384284b604..26b7715437 100644
--- a/.github/workflows/windows_builds.yml
+++ b/.github/workflows/windows_builds.yml
@@ -31,6 +31,7 @@ jobs:
             sconsflags: debug_symbols=no vsproj=yes vsproj_gen_only=no windows_subsystem=console
             bin: ./bin/godot.windows.editor.x86_64.exe
             compiler: msvc
+            cache-limit: 2
 
           - name: Editor w/ clang-cl (target=editor, tests=yes, use_llvm=yes)
             cache-name: windows-editor-clang
@@ -39,6 +40,7 @@ jobs:
             sconsflags: debug_symbols=no windows_subsystem=console use_llvm=yes
             bin: ./bin/godot.windows.editor.x86_64.llvm.exe
             compiler: clang
+            cache-limit: 1
 
           - name: Template (target=template_release, tests=yes)
             cache-name: windows-template
@@ -47,6 +49,7 @@ jobs:
             sconsflags: debug_symbols=no
             bin: ./bin/godot.windows.template_release.x86_64.console.exe
             compiler: msvc
+            cache-limit: 2
 
           - name: Template w/ GCC (target=template_release, tests=yes, use_mingw=yes)
             cache-name: windows-template-gcc
@@ -56,6 +59,7 @@ jobs:
             sconsflags: debug_symbols=no use_mingw=yes
             bin: ./bin/godot.windows.template_release.x86_64.console.exe
             compiler: gcc
+            cache-limit: 1
 
     steps:
       - name: Checkout
@@ -101,6 +105,7 @@ jobs:
           platform: windows
           target: ${{ matrix.target }}
           tests: ${{ matrix.tests }}
+          scons-cache-limit: ${{ matrix.cache-limit }}
 
       - name: Save Godot build cache
         uses: ./.github/actions/godot-cache-save

From bd1a35ce9e6f7e6e4c87fa28278ed54e8f84ff3f Mon Sep 17 00:00:00 2001
From: Danil Alexeev <dalexeev12@yandex.ru>
Date: Wed, 27 Nov 2024 18:15:49 +0300
Subject: [PATCH 018/124] Core: Fix `JSON.{from,to}_native()` issues

---
 core/io/json.cpp                       | 1598 ++++++++++++++----------
 core/io/json.h                         |   20 +-
 core/io/marshalls.cpp                  |   66 +-
 core/math/expression.cpp               |    7 +-
 core/variant/array.cpp                 |   15 +-
 core/variant/array.h                   |   12 +-
 core/variant/container_type_validate.h |    6 +
 core/variant/dictionary.cpp            |   20 +
 core/variant/dictionary.h              |    6 +
 core/variant/variant.cpp               |   12 +
 core/variant/variant.h                 |    1 +
 doc/classes/JSON.xml                   |   24 +-
 editor/editor_autoload_settings.cpp    |   12 +-
 modules/gdscript/gdscript_editor.cpp   |   11 +-
 tests/core/io/test_json_native.h       |  301 +++--
 15 files changed, 1237 insertions(+), 874 deletions(-)

diff --git a/core/io/json.cpp b/core/io/json.cpp
index e73677be9c..b6b1a88479 100644
--- a/core/io/json.cpp
+++ b/core/io/json.cpp
@@ -31,7 +31,8 @@
 #include "json.h"
 
 #include "core/config/engine.h"
-#include "core/string/print_string.h"
+#include "core/object/script_language.h"
+#include "core/variant/container_type_validate.h"
 
 const char *JSON::tk_name[TK_MAX] = {
 	"'{'",
@@ -563,18 +564,18 @@ String JSON::get_parsed_text() const {
 }
 
 String JSON::stringify(const Variant &p_var, const String &p_indent, bool p_sort_keys, bool p_full_precision) {
-	Ref<JSON> jason;
-	jason.instantiate();
+	Ref<JSON> json;
+	json.instantiate();
 	HashSet<const void *> markers;
-	return jason->_stringify(p_var, p_indent, 0, p_sort_keys, markers, p_full_precision);
+	return json->_stringify(p_var, p_indent, 0, p_sort_keys, markers, p_full_precision);
 }
 
 Variant JSON::parse_string(const String &p_json_string) {
-	Ref<JSON> jason;
-	jason.instantiate();
-	Error error = jason->parse(p_json_string);
-	ERR_FAIL_COND_V_MSG(error != Error::OK, Variant(), vformat("Parse JSON failed. Error at line %d: %s", jason->get_error_line(), jason->get_error_message()));
-	return jason->get_data();
+	Ref<JSON> json;
+	json.instantiate();
+	Error error = json->parse(p_json_string);
+	ERR_FAIL_COND_V_MSG(error != Error::OK, Variant(), vformat("Parse JSON failed. Error at line %d: %s", json->get_error_line(), json->get_error_message()));
+	return json->get_data();
 }
 
 void JSON::_bind_methods() {
@@ -588,756 +589,1015 @@ void JSON::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("get_error_line"), &JSON::get_error_line);
 	ClassDB::bind_method(D_METHOD("get_error_message"), &JSON::get_error_message);
 
-	ClassDB::bind_static_method("JSON", D_METHOD("to_native", "json", "allow_classes", "allow_scripts"), &JSON::to_native, DEFVAL(false), DEFVAL(false));
-	ClassDB::bind_static_method("JSON", D_METHOD("from_native", "variant", "allow_classes", "allow_scripts"), &JSON::from_native, DEFVAL(false), DEFVAL(false));
+	ClassDB::bind_static_method("JSON", D_METHOD("from_native", "variant", "full_objects"), &JSON::from_native, DEFVAL(false));
+	ClassDB::bind_static_method("JSON", D_METHOD("to_native", "json", "allow_objects"), &JSON::to_native, DEFVAL(false));
 
 	ADD_PROPERTY(PropertyInfo(Variant::NIL, "data", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_DEFAULT | PROPERTY_USAGE_NIL_IS_VARIANT), "set_data", "get_data"); // Ensures that it can be serialized as binary.
 }
 
-#define GDTYPE "__gdtype"
-#define VALUES "values"
-#define PASS_ARG p_allow_classes, p_allow_scripts
+#define TYPE "type"
+#define ELEM_TYPE "elem_type"
+#define KEY_TYPE "key_type"
+#define VALUE_TYPE "value_type"
+#define ARGS "args"
+#define PROPS "props"
+
+static bool _encode_container_type(Dictionary &r_dict, const String &p_key, const ContainerType &p_type, bool p_full_objects) {
+	if (p_type.builtin_type != Variant::NIL) {
+		if (p_type.script.is_valid()) {
+			ERR_FAIL_COND_V(!p_full_objects, false);
+			const String path = p_type.script->get_path();
+			ERR_FAIL_COND_V_MSG(path.is_empty() || !path.begins_with("res://"), false, "Failed to encode a path to a custom script for a container type.");
+			r_dict[p_key] = path;
+		} else if (p_type.class_name != StringName()) {
+			ERR_FAIL_COND_V(!p_full_objects, false);
+			r_dict[p_key] = String(p_type.class_name);
+		} else {
+			// No need to check `p_full_objects` since `class_name` should be non-empty for `builtin_type == Variant::OBJECT`.
+			r_dict[p_key] = Variant::get_type_name(p_type.builtin_type);
+		}
+	}
+	return true;
+}
+
+Variant JSON::_from_native(const Variant &p_variant, bool p_full_objects, int p_depth) {
+#define RETURN_ARGS                                           \
+	Dictionary ret;                                           \
+	ret[TYPE] = Variant::get_type_name(p_variant.get_type()); \
+	ret[ARGS] = args;                                         \
+	return ret
 
-Variant JSON::from_native(const Variant &p_variant, bool p_allow_classes, bool p_allow_scripts) {
 	switch (p_variant.get_type()) {
-		case Variant::NIL: {
-			Dictionary nil;
-			nil[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return nil;
-		} break;
+		case Variant::NIL:
 		case Variant::BOOL: {
 			return p_variant;
 		} break;
+
 		case Variant::INT: {
-			return p_variant;
+			return "i:" + String(p_variant);
 		} break;
 		case Variant::FLOAT: {
-			return p_variant;
+			return "f:" + String(p_variant);
 		} break;
 		case Variant::STRING: {
-			return p_variant;
-		} break;
-		case Variant::VECTOR2: {
-			Dictionary d;
-			Vector2 v = p_variant;
-			Array values;
-			values.push_back(v.x);
-			values.push_back(v.y);
-			d[VALUES] = values;
-			d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return d;
-		} break;
-		case Variant::VECTOR2I: {
-			Dictionary d;
-			Vector2i v = p_variant;
-			Array values;
-			values.push_back(v.x);
-			values.push_back(v.y);
-			d[VALUES] = values;
-			d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return d;
-		} break;
-		case Variant::RECT2: {
-			Dictionary d;
-			Rect2 r = p_variant;
-			d["position"] = from_native(r.position);
-			d["size"] = from_native(r.size);
-			d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return d;
-		} break;
-		case Variant::RECT2I: {
-			Dictionary d;
-			Rect2i r = p_variant;
-			d["position"] = from_native(r.position);
-			d["size"] = from_native(r.size);
-			d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return d;
-		} break;
-		case Variant::VECTOR3: {
-			Dictionary d;
-			Vector3 v = p_variant;
-			Array values;
-			values.push_back(v.x);
-			values.push_back(v.y);
-			values.push_back(v.z);
-			d[VALUES] = values;
-			d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return d;
-		} break;
-		case Variant::VECTOR3I: {
-			Dictionary d;
-			Vector3i v = p_variant;
-			Array values;
-			values.push_back(v.x);
-			values.push_back(v.y);
-			values.push_back(v.z);
-			d[VALUES] = values;
-			d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return d;
-		} break;
-		case Variant::TRANSFORM2D: {
-			Dictionary d;
-			Transform2D t = p_variant;
-			d["x"] = from_native(t[0]);
-			d["y"] = from_native(t[1]);
-			d["origin"] = from_native(t[2]);
-			d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return d;
-		} break;
-		case Variant::VECTOR4: {
-			Dictionary d;
-			Vector4 v = p_variant;
-			Array values;
-			values.push_back(v.x);
-			values.push_back(v.y);
-			values.push_back(v.z);
-			values.push_back(v.w);
-			d[VALUES] = values;
-			d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return d;
-		} break;
-		case Variant::VECTOR4I: {
-			Dictionary d;
-			Vector4i v = p_variant;
-			Array values;
-			values.push_back(v.x);
-			values.push_back(v.y);
-			values.push_back(v.z);
-			values.push_back(v.w);
-			d[VALUES] = values;
-			d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return d;
-		} break;
-		case Variant::PLANE: {
-			Dictionary d;
-			Plane p = p_variant;
-			d["normal"] = from_native(p.normal);
-			d["d"] = p.d;
-			d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return d;
-		} break;
-		case Variant::QUATERNION: {
-			Dictionary d;
-			Quaternion q = p_variant;
-			Array values;
-			values.push_back(q.x);
-			values.push_back(q.y);
-			values.push_back(q.z);
-			values.push_back(q.w);
-			d[VALUES] = values;
-			d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return d;
-		} break;
-		case Variant::AABB: {
-			Dictionary d;
-			AABB aabb = p_variant;
-			d["position"] = from_native(aabb.position);
-			d["size"] = from_native(aabb.size);
-			d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return d;
-		} break;
-		case Variant::BASIS: {
-			Dictionary d;
-			Basis t = p_variant;
-			d["x"] = from_native(t.get_column(0));
-			d["y"] = from_native(t.get_column(1));
-			d["z"] = from_native(t.get_column(2));
-			d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return d;
-		} break;
-		case Variant::TRANSFORM3D: {
-			Dictionary d;
-			Transform3D t = p_variant;
-			d["basis"] = from_native(t.basis);
-			d["origin"] = from_native(t.origin);
-			d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return d;
-		} break;
-		case Variant::PROJECTION: {
-			Dictionary d;
-			Projection t = p_variant;
-			d["x"] = from_native(t[0]);
-			d["y"] = from_native(t[1]);
-			d["z"] = from_native(t[2]);
-			d["w"] = from_native(t[3]);
-			d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return d;
-		} break;
-		case Variant::COLOR: {
-			Dictionary d;
-			Color c = p_variant;
-			Array values;
-			values.push_back(c.r);
-			values.push_back(c.g);
-			values.push_back(c.b);
-			values.push_back(c.a);
-			d[VALUES] = values;
-			d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return d;
+			return "s:" + String(p_variant);
 		} break;
 		case Variant::STRING_NAME: {
-			Dictionary d;
-			d["name"] = String(p_variant);
-			d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return d;
+			return "sn:" + String(p_variant);
 		} break;
 		case Variant::NODE_PATH: {
-			Dictionary d;
-			d["path"] = String(p_variant);
-			d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return d;
+			return "np:" + String(p_variant);
 		} break;
-		case Variant::RID: {
-			Dictionary d;
-			d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return d;
-		} break;
-		case Variant::OBJECT: {
-			Object *obj = p_variant.get_validated_object();
 
-			if (p_allow_classes && obj) {
-				Dictionary d;
-				List<PropertyInfo> property_list;
-				obj->get_property_list(&property_list);
-
-				d["type"] = obj->get_class();
-				Dictionary p;
-				for (const PropertyInfo &P : property_list) {
-					if (P.usage & PROPERTY_USAGE_STORAGE) {
-						if (P.name == "script" && !p_allow_scripts) {
-							continue;
-						}
-						p[P.name] = from_native(obj->get(P.name), PASS_ARG);
-					}
-				}
-				d["properties"] = p;
-				d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-				return d;
-			} else {
-				Dictionary nil;
-				nil[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-				return nil;
-			}
-		} break;
+		case Variant::RID:
 		case Variant::CALLABLE:
 		case Variant::SIGNAL: {
-			Dictionary nil;
-			nil[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return nil;
-		} break;
-		case Variant::DICTIONARY: {
-			Dictionary d = p_variant;
-			List<Variant> keys;
-			d.get_key_list(&keys);
-			bool all_strings = true;
-			for (const Variant &K : keys) {
-				if (K.get_type() != Variant::STRING) {
-					all_strings = false;
-					break;
-				}
-			}
-
-			if (all_strings) {
-				Dictionary ret_dict;
-				for (const Variant &K : keys) {
-					ret_dict[K] = from_native(d[K], PASS_ARG);
-				}
-				return ret_dict;
-			} else {
-				Dictionary ret;
-				Array pairs;
-				for (const Variant &K : keys) {
-					Dictionary pair;
-					pair["key"] = from_native(K, PASS_ARG);
-					pair["value"] = from_native(d[K], PASS_ARG);
-					pairs.push_back(pair);
-				}
-				ret["pairs"] = pairs;
-				ret[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-				return ret;
-			}
-		} break;
-		case Variant::ARRAY: {
-			Array arr = p_variant;
-			Array ret;
-			for (int i = 0; i < arr.size(); i++) {
-				ret.push_back(from_native(arr[i], PASS_ARG));
-			}
+			Dictionary ret;
+			ret[TYPE] = Variant::get_type_name(p_variant.get_type());
 			return ret;
 		} break;
-		case Variant::PACKED_BYTE_ARRAY: {
-			Dictionary d;
-			PackedByteArray arr = p_variant;
-			Array values;
-			for (int i = 0; i < arr.size(); i++) {
-				values.push_back(arr[i]);
+
+		case Variant::VECTOR2: {
+			const Vector2 v = p_variant;
+
+			Array args;
+			args.push_back(v.x);
+			args.push_back(v.y);
+
+			RETURN_ARGS;
+		} break;
+		case Variant::VECTOR2I: {
+			const Vector2i v = p_variant;
+
+			Array args;
+			args.push_back(v.x);
+			args.push_back(v.y);
+
+			RETURN_ARGS;
+		} break;
+		case Variant::RECT2: {
+			const Rect2 r = p_variant;
+
+			Array args;
+			args.push_back(r.position.x);
+			args.push_back(r.position.y);
+			args.push_back(r.size.width);
+			args.push_back(r.size.height);
+
+			RETURN_ARGS;
+		} break;
+		case Variant::RECT2I: {
+			const Rect2i r = p_variant;
+
+			Array args;
+			args.push_back(r.position.x);
+			args.push_back(r.position.y);
+			args.push_back(r.size.width);
+			args.push_back(r.size.height);
+
+			RETURN_ARGS;
+		} break;
+		case Variant::VECTOR3: {
+			const Vector3 v = p_variant;
+
+			Array args;
+			args.push_back(v.x);
+			args.push_back(v.y);
+			args.push_back(v.z);
+
+			RETURN_ARGS;
+		} break;
+		case Variant::VECTOR3I: {
+			const Vector3i v = p_variant;
+
+			Array args;
+			args.push_back(v.x);
+			args.push_back(v.y);
+			args.push_back(v.z);
+
+			RETURN_ARGS;
+		} break;
+		case Variant::TRANSFORM2D: {
+			const Transform2D t = p_variant;
+
+			Array args;
+			args.push_back(t[0].x);
+			args.push_back(t[0].y);
+			args.push_back(t[1].x);
+			args.push_back(t[1].y);
+			args.push_back(t[2].x);
+			args.push_back(t[2].y);
+
+			RETURN_ARGS;
+		} break;
+		case Variant::VECTOR4: {
+			const Vector4 v = p_variant;
+
+			Array args;
+			args.push_back(v.x);
+			args.push_back(v.y);
+			args.push_back(v.z);
+			args.push_back(v.w);
+
+			RETURN_ARGS;
+		} break;
+		case Variant::VECTOR4I: {
+			const Vector4i v = p_variant;
+
+			Array args;
+			args.push_back(v.x);
+			args.push_back(v.y);
+			args.push_back(v.z);
+			args.push_back(v.w);
+
+			RETURN_ARGS;
+		} break;
+		case Variant::PLANE: {
+			const Plane p = p_variant;
+
+			Array args;
+			args.push_back(p.normal.x);
+			args.push_back(p.normal.y);
+			args.push_back(p.normal.z);
+			args.push_back(p.d);
+
+			RETURN_ARGS;
+		} break;
+		case Variant::QUATERNION: {
+			const Quaternion q = p_variant;
+
+			Array args;
+			args.push_back(q.x);
+			args.push_back(q.y);
+			args.push_back(q.z);
+			args.push_back(q.w);
+
+			RETURN_ARGS;
+		} break;
+		case Variant::AABB: {
+			const AABB aabb = p_variant;
+
+			Array args;
+			args.push_back(aabb.position.x);
+			args.push_back(aabb.position.y);
+			args.push_back(aabb.position.z);
+			args.push_back(aabb.size.x);
+			args.push_back(aabb.size.y);
+			args.push_back(aabb.size.z);
+
+			RETURN_ARGS;
+		} break;
+		case Variant::BASIS: {
+			const Basis b = p_variant;
+
+			Array args;
+			args.push_back(b.get_column(0).x);
+			args.push_back(b.get_column(0).y);
+			args.push_back(b.get_column(0).z);
+			args.push_back(b.get_column(1).x);
+			args.push_back(b.get_column(1).y);
+			args.push_back(b.get_column(1).z);
+			args.push_back(b.get_column(2).x);
+			args.push_back(b.get_column(2).y);
+			args.push_back(b.get_column(2).z);
+
+			RETURN_ARGS;
+		} break;
+		case Variant::TRANSFORM3D: {
+			const Transform3D t = p_variant;
+
+			Array args;
+			args.push_back(t.basis.get_column(0).x);
+			args.push_back(t.basis.get_column(0).y);
+			args.push_back(t.basis.get_column(0).z);
+			args.push_back(t.basis.get_column(1).x);
+			args.push_back(t.basis.get_column(1).y);
+			args.push_back(t.basis.get_column(1).z);
+			args.push_back(t.basis.get_column(2).x);
+			args.push_back(t.basis.get_column(2).y);
+			args.push_back(t.basis.get_column(2).z);
+			args.push_back(t.origin.x);
+			args.push_back(t.origin.y);
+			args.push_back(t.origin.z);
+
+			RETURN_ARGS;
+		} break;
+		case Variant::PROJECTION: {
+			const Projection p = p_variant;
+
+			Array args;
+			args.push_back(p[0].x);
+			args.push_back(p[0].y);
+			args.push_back(p[0].z);
+			args.push_back(p[0].w);
+			args.push_back(p[1].x);
+			args.push_back(p[1].y);
+			args.push_back(p[1].z);
+			args.push_back(p[1].w);
+			args.push_back(p[2].x);
+			args.push_back(p[2].y);
+			args.push_back(p[2].z);
+			args.push_back(p[2].w);
+			args.push_back(p[3].x);
+			args.push_back(p[3].y);
+			args.push_back(p[3].z);
+			args.push_back(p[3].w);
+
+			RETURN_ARGS;
+		} break;
+		case Variant::COLOR: {
+			const Color c = p_variant;
+
+			Array args;
+			args.push_back(c.r);
+			args.push_back(c.g);
+			args.push_back(c.b);
+			args.push_back(c.a);
+
+			RETURN_ARGS;
+		} break;
+
+		case Variant::OBJECT: {
+			ERR_FAIL_COND_V(!p_full_objects, Variant());
+
+			ERR_FAIL_COND_V_MSG(p_depth > Variant::MAX_RECURSION_DEPTH, Variant(), "Variant is too deep. Bailing.");
+
+			const Object *obj = p_variant.get_validated_object();
+			if (obj == nullptr) {
+				return Variant();
 			}
-			d[VALUES] = values;
-			d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return d;
+
+			ERR_FAIL_COND_V(!ClassDB::can_instantiate(obj->get_class()), Variant());
+
+			List<PropertyInfo> prop_list;
+			obj->get_property_list(&prop_list);
+
+			Array props;
+			for (const PropertyInfo &pi : prop_list) {
+				if (!(pi.usage & PROPERTY_USAGE_STORAGE)) {
+					continue;
+				}
+
+				Variant value;
+				if (pi.name == CoreStringName(script)) {
+					const Ref<Script> script = obj->get_script();
+					if (script.is_valid()) {
+						const String path = script->get_path();
+						ERR_FAIL_COND_V_MSG(path.is_empty() || !path.begins_with("res://"), Variant(), "Failed to encode a path to a custom script.");
+						value = path;
+					}
+				} else {
+					value = obj->get(pi.name);
+				}
+
+				props.push_back(pi.name);
+				props.push_back(_from_native(value, p_full_objects, p_depth + 1));
+			}
+
+			Dictionary ret;
+			ret[TYPE] = obj->get_class();
+			ret[PROPS] = props;
+			return ret;
+		} break;
+
+		case Variant::DICTIONARY: {
+			const Dictionary dict = p_variant;
+
+			Array args;
+
+			Dictionary ret;
+			ret[TYPE] = Variant::get_type_name(p_variant.get_type());
+			if (!_encode_container_type(ret, KEY_TYPE, dict.get_key_type(), p_full_objects)) {
+				return Variant();
+			}
+			if (!_encode_container_type(ret, VALUE_TYPE, dict.get_value_type(), p_full_objects)) {
+				return Variant();
+			}
+			ret[ARGS] = args;
+
+			ERR_FAIL_COND_V_MSG(p_depth > Variant::MAX_RECURSION_DEPTH, ret, "Variant is too deep. Bailing.");
+
+			List<Variant> keys;
+			dict.get_key_list(&keys);
+
+			for (const Variant &key : keys) {
+				args.push_back(_from_native(key, p_full_objects, p_depth + 1));
+				args.push_back(_from_native(dict[key], p_full_objects, p_depth + 1));
+			}
+
+			return ret;
+		} break;
+
+		case Variant::ARRAY: {
+			const Array arr = p_variant;
+
+			Variant ret;
+			Array args;
+
+			if (arr.is_typed()) {
+				Dictionary d;
+				d[TYPE] = Variant::get_type_name(p_variant.get_type());
+				if (!_encode_container_type(d, ELEM_TYPE, arr.get_element_type(), p_full_objects)) {
+					return Variant();
+				}
+				d[ARGS] = args;
+				ret = d;
+			} else {
+				ret = args;
+			}
+
+			ERR_FAIL_COND_V_MSG(p_depth > Variant::MAX_RECURSION_DEPTH, ret, "Variant is too deep. Bailing.");
+
+			for (int i = 0; i < arr.size(); i++) {
+				args.push_back(_from_native(arr[i], p_full_objects, p_depth + 1));
+			}
+
+			return ret;
+		} break;
+
+		case Variant::PACKED_BYTE_ARRAY: {
+			const PackedByteArray arr = p_variant;
+
+			Array args;
+			for (int i = 0; i < arr.size(); i++) {
+				args.push_back(arr[i]);
+			}
+
+			RETURN_ARGS;
 		} break;
 		case Variant::PACKED_INT32_ARRAY: {
-			Dictionary d;
-			PackedInt32Array arr = p_variant;
-			Array values;
-			for (int i = 0; i < arr.size(); i++) {
-				values.push_back(arr[i]);
-			}
-			d[VALUES] = values;
-			d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return d;
+			const PackedInt32Array arr = p_variant;
 
+			Array args;
+			for (int i = 0; i < arr.size(); i++) {
+				args.push_back(arr[i]);
+			}
+
+			RETURN_ARGS;
 		} break;
 		case Variant::PACKED_INT64_ARRAY: {
-			Dictionary d;
-			PackedInt64Array arr = p_variant;
-			Array values;
+			const PackedInt64Array arr = p_variant;
+
+			Array args;
 			for (int i = 0; i < arr.size(); i++) {
-				values.push_back(arr[i]);
+				args.push_back(arr[i]);
 			}
-			d[VALUES] = values;
-			d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return d;
+
+			RETURN_ARGS;
 		} break;
 		case Variant::PACKED_FLOAT32_ARRAY: {
-			Dictionary d;
-			PackedFloat32Array arr = p_variant;
-			Array values;
+			const PackedFloat32Array arr = p_variant;
+
+			Array args;
 			for (int i = 0; i < arr.size(); i++) {
-				values.push_back(arr[i]);
+				args.push_back(arr[i]);
 			}
-			d[VALUES] = values;
-			d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return d;
+
+			RETURN_ARGS;
 		} break;
 		case Variant::PACKED_FLOAT64_ARRAY: {
-			Dictionary d;
-			PackedFloat64Array arr = p_variant;
-			Array values;
+			const PackedFloat64Array arr = p_variant;
+
+			Array args;
 			for (int i = 0; i < arr.size(); i++) {
-				values.push_back(arr[i]);
+				args.push_back(arr[i]);
 			}
-			d[VALUES] = values;
-			d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return d;
+
+			RETURN_ARGS;
 		} break;
 		case Variant::PACKED_STRING_ARRAY: {
-			Dictionary d;
-			PackedStringArray arr = p_variant;
-			Array values;
+			const PackedStringArray arr = p_variant;
+
+			Array args;
 			for (int i = 0; i < arr.size(); i++) {
-				values.push_back(arr[i]);
+				args.push_back(arr[i]);
 			}
-			d[VALUES] = values;
-			d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return d;
+
+			RETURN_ARGS;
 		} break;
 		case Variant::PACKED_VECTOR2_ARRAY: {
-			Dictionary d;
-			PackedVector2Array arr = p_variant;
-			Array values;
+			const PackedVector2Array arr = p_variant;
+
+			Array args;
 			for (int i = 0; i < arr.size(); i++) {
 				Vector2 v = arr[i];
-				values.push_back(v.x);
-				values.push_back(v.y);
+				args.push_back(v.x);
+				args.push_back(v.y);
 			}
-			d[VALUES] = values;
-			d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return d;
+
+			RETURN_ARGS;
 		} break;
 		case Variant::PACKED_VECTOR3_ARRAY: {
-			Dictionary d;
-			PackedVector3Array arr = p_variant;
-			Array values;
+			const PackedVector3Array arr = p_variant;
+
+			Array args;
 			for (int i = 0; i < arr.size(); i++) {
 				Vector3 v = arr[i];
-				values.push_back(v.x);
-				values.push_back(v.y);
-				values.push_back(v.z);
+				args.push_back(v.x);
+				args.push_back(v.y);
+				args.push_back(v.z);
 			}
-			d[VALUES] = values;
-			d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return d;
+
+			RETURN_ARGS;
 		} break;
 		case Variant::PACKED_COLOR_ARRAY: {
-			Dictionary d;
-			PackedColorArray arr = p_variant;
-			Array values;
+			const PackedColorArray arr = p_variant;
+
+			Array args;
 			for (int i = 0; i < arr.size(); i++) {
 				Color v = arr[i];
-				values.push_back(v.r);
-				values.push_back(v.g);
-				values.push_back(v.b);
-				values.push_back(v.a);
+				args.push_back(v.r);
+				args.push_back(v.g);
+				args.push_back(v.b);
+				args.push_back(v.a);
 			}
-			d[VALUES] = values;
-			d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return d;
+
+			RETURN_ARGS;
 		} break;
 		case Variant::PACKED_VECTOR4_ARRAY: {
-			Dictionary d;
-			PackedVector4Array arr = p_variant;
-			Array values;
+			const PackedVector4Array arr = p_variant;
+
+			Array args;
 			for (int i = 0; i < arr.size(); i++) {
 				Vector4 v = arr[i];
-				values.push_back(v.x);
-				values.push_back(v.y);
-				values.push_back(v.z);
-				values.push_back(v.w);
+				args.push_back(v.x);
+				args.push_back(v.y);
+				args.push_back(v.z);
+				args.push_back(v.w);
 			}
-			d[VALUES] = values;
-			d[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-			return d;
+
+			RETURN_ARGS;
 		} break;
-		default: {
-			ERR_PRINT(vformat("Unhandled conversion from native Variant type '%s' to JSON.", Variant::get_type_name(p_variant.get_type())));
+
+		case Variant::VARIANT_MAX: {
+			// Nothing to do.
 		} break;
 	}
 
-	Dictionary nil;
-	nil[GDTYPE] = Variant::get_type_name(p_variant.get_type());
-	return nil;
+#undef RETURN_ARGS
+
+	ERR_FAIL_V_MSG(Variant(), vformat(R"(Unhandled Variant type "%s".)", Variant::get_type_name(p_variant.get_type())));
 }
 
-Variant JSON::to_native(const Variant &p_json, bool p_allow_classes, bool p_allow_scripts) {
+static bool _decode_container_type(const Dictionary &p_dict, const String &p_key, ContainerType &r_type, bool p_allow_objects) {
+	if (!p_dict.has(p_key)) {
+		return true;
+	}
+
+	const String type_name = p_dict[p_key];
+
+	const Variant::Type builtin_type = Variant::get_type_by_name(type_name);
+	if (builtin_type < Variant::VARIANT_MAX && builtin_type != Variant::OBJECT) {
+		r_type.builtin_type = builtin_type;
+		return true;
+	}
+
+	if (ClassDB::class_exists(type_name)) {
+		ERR_FAIL_COND_V(!p_allow_objects, false);
+
+		r_type.builtin_type = Variant::OBJECT;
+		r_type.class_name = type_name;
+		return true;
+	}
+
+	if (type_name.begins_with("res://")) {
+		ERR_FAIL_COND_V(!p_allow_objects, false);
+
+		ERR_FAIL_COND_V_MSG(!ResourceLoader::exists(type_name, "Script"), false, vformat(R"(Invalid script path "%s".)", type_name));
+		const Ref<Script> script = ResourceLoader::load(type_name, "Script");
+		ERR_FAIL_COND_V_MSG(script.is_null(), false, vformat(R"(Can't load script at path "%s".)", type_name));
+
+		r_type.builtin_type = Variant::OBJECT;
+		r_type.class_name = script->get_instance_base_type();
+		r_type.script = script;
+		return true;
+	}
+
+	ERR_FAIL_V_MSG(false, vformat(R"(Invalid type "%s".)", type_name));
+}
+
+Variant JSON::_to_native(const Variant &p_json, bool p_allow_objects, int p_depth) {
 	switch (p_json.get_type()) {
+		case Variant::NIL:
 		case Variant::BOOL: {
 			return p_json;
 		} break;
-		case Variant::INT: {
-			return p_json;
-		} break;
-		case Variant::FLOAT: {
-			return p_json;
-		} break;
+
 		case Variant::STRING: {
-			return p_json;
-		} break;
-		case Variant::STRING_NAME: {
-			return p_json;
-		} break;
-		case Variant::CALLABLE: {
-			return p_json;
-		} break;
-		case Variant::DICTIONARY: {
-			Dictionary d = p_json;
-			if (d.has(GDTYPE)) {
-				// Specific Godot Variant types serialized to JSON.
-				String type = d[GDTYPE];
-				if (type == Variant::get_type_name(Variant::VECTOR2)) {
-					ERR_FAIL_COND_V(!d.has(VALUES), Variant());
-					Array values = d[VALUES];
-					ERR_FAIL_COND_V(values.size() != 2, Variant());
-					Vector2 v;
-					v.x = values[0];
-					v.y = values[1];
-					return v;
-				} else if (type == Variant::get_type_name(Variant::VECTOR2I)) {
-					ERR_FAIL_COND_V(!d.has(VALUES), Variant());
-					Array values = d[VALUES];
-					ERR_FAIL_COND_V(values.size() != 2, Variant());
-					Vector2i v;
-					v.x = values[0];
-					v.y = values[1];
-					return v;
-				} else if (type == Variant::get_type_name(Variant::RECT2)) {
-					ERR_FAIL_COND_V(!d.has("position"), Variant());
-					ERR_FAIL_COND_V(!d.has("size"), Variant());
-					Rect2 r;
-					r.position = to_native(d["position"]);
-					r.size = to_native(d["size"]);
-					return r;
-				} else if (type == Variant::get_type_name(Variant::RECT2I)) {
-					ERR_FAIL_COND_V(!d.has("position"), Variant());
-					ERR_FAIL_COND_V(!d.has("size"), Variant());
-					Rect2i r;
-					r.position = to_native(d["position"]);
-					r.size = to_native(d["size"]);
-					return r;
-				} else if (type == Variant::get_type_name(Variant::VECTOR3)) {
-					ERR_FAIL_COND_V(!d.has(VALUES), Variant());
-					Array values = d[VALUES];
-					ERR_FAIL_COND_V(values.size() != 3, Variant());
-					Vector3 v;
-					v.x = values[0];
-					v.y = values[1];
-					v.z = values[2];
-					return v;
-				} else if (type == Variant::get_type_name(Variant::VECTOR3I)) {
-					ERR_FAIL_COND_V(!d.has(VALUES), Variant());
-					Array values = d[VALUES];
-					ERR_FAIL_COND_V(values.size() != 3, Variant());
-					Vector3i v;
-					v.x = values[0];
-					v.y = values[1];
-					v.z = values[2];
-					return v;
-				} else if (type == Variant::get_type_name(Variant::TRANSFORM2D)) {
-					ERR_FAIL_COND_V(!d.has("x"), Variant());
-					ERR_FAIL_COND_V(!d.has("y"), Variant());
-					ERR_FAIL_COND_V(!d.has("origin"), Variant());
-					Transform2D t;
-					t[0] = to_native(d["x"]);
-					t[1] = to_native(d["y"]);
-					t[2] = to_native(d["origin"]);
-					return t;
-				} else if (type == Variant::get_type_name(Variant::VECTOR4)) {
-					ERR_FAIL_COND_V(!d.has(VALUES), Variant());
-					Array values = d[VALUES];
-					ERR_FAIL_COND_V(values.size() != 4, Variant());
-					Vector4 v;
-					v.x = values[0];
-					v.y = values[1];
-					v.z = values[2];
-					v.w = values[3];
-					return v;
-				} else if (type == Variant::get_type_name(Variant::VECTOR4I)) {
-					ERR_FAIL_COND_V(!d.has(VALUES), Variant());
-					Array values = d[VALUES];
-					ERR_FAIL_COND_V(values.size() != 4, Variant());
-					Vector4i v;
-					v.x = values[0];
-					v.y = values[1];
-					v.z = values[2];
-					v.w = values[3];
-					return v;
-				} else if (type == Variant::get_type_name(Variant::PLANE)) {
-					ERR_FAIL_COND_V(!d.has("normal"), Variant());
-					ERR_FAIL_COND_V(!d.has("d"), Variant());
-					Plane p;
-					p.normal = to_native(d["normal"]);
-					p.d = d["d"];
-					return p;
-				} else if (type == Variant::get_type_name(Variant::QUATERNION)) {
-					ERR_FAIL_COND_V(!d.has(VALUES), Variant());
-					Array values = d[VALUES];
-					ERR_FAIL_COND_V(values.size() != 4, Variant());
-					Quaternion v;
-					v.x = values[0];
-					v.y = values[1];
-					v.z = values[2];
-					v.w = values[3];
-					return v;
-				} else if (type == Variant::get_type_name(Variant::AABB)) {
-					ERR_FAIL_COND_V(!d.has("position"), Variant());
-					ERR_FAIL_COND_V(!d.has("size"), Variant());
-					AABB r;
-					r.position = to_native(d["position"]);
-					r.size = to_native(d["size"]);
-					return r;
-				} else if (type == Variant::get_type_name(Variant::BASIS)) {
-					ERR_FAIL_COND_V(!d.has("x"), Variant());
-					ERR_FAIL_COND_V(!d.has("y"), Variant());
-					ERR_FAIL_COND_V(!d.has("z"), Variant());
-					Basis b;
-					b.set_column(0, to_native(d["x"]));
-					b.set_column(1, to_native(d["y"]));
-					b.set_column(2, to_native(d["z"]));
-					return b;
-				} else if (type == Variant::get_type_name(Variant::TRANSFORM3D)) {
-					ERR_FAIL_COND_V(!d.has("basis"), Variant());
-					ERR_FAIL_COND_V(!d.has("origin"), Variant());
-					Transform3D t;
-					t.basis = to_native(d["basis"]);
-					t.origin = to_native(d["origin"]);
-					return t;
-				} else if (type == Variant::get_type_name(Variant::PROJECTION)) {
-					ERR_FAIL_COND_V(!d.has("x"), Variant());
-					ERR_FAIL_COND_V(!d.has("y"), Variant());
-					ERR_FAIL_COND_V(!d.has("z"), Variant());
-					ERR_FAIL_COND_V(!d.has("w"), Variant());
-					Projection p;
-					p[0] = to_native(d["x"]);
-					p[1] = to_native(d["y"]);
-					p[2] = to_native(d["z"]);
-					p[3] = to_native(d["w"]);
-					return p;
-				} else if (type == Variant::get_type_name(Variant::COLOR)) {
-					ERR_FAIL_COND_V(!d.has(VALUES), Variant());
-					Array values = d[VALUES];
-					ERR_FAIL_COND_V(values.size() != 4, Variant());
-					Color c;
-					c.r = values[0];
-					c.g = values[1];
-					c.b = values[2];
-					c.a = values[3];
-					return c;
-				} else if (type == Variant::get_type_name(Variant::NODE_PATH)) {
-					ERR_FAIL_COND_V(!d.has("path"), Variant());
-					NodePath np = d["path"];
-					return np;
-				} else if (type == Variant::get_type_name(Variant::STRING_NAME)) {
-					ERR_FAIL_COND_V(!d.has("name"), Variant());
-					StringName s = d["name"];
-					return s;
-				} else if (type == Variant::get_type_name(Variant::OBJECT)) {
-					ERR_FAIL_COND_V(!d.has("type"), Variant());
-					ERR_FAIL_COND_V(!d.has("properties"), Variant());
+			const String s = p_json;
 
-					ERR_FAIL_COND_V(!p_allow_classes, Variant());
-
-					String obj_type = d["type"];
-					bool is_script = obj_type == "Script" || ClassDB::is_parent_class(obj_type, "Script");
-					ERR_FAIL_COND_V(!p_allow_scripts && is_script, Variant());
-					Object *obj = ClassDB::instantiate(obj_type);
-					ERR_FAIL_NULL_V(obj, Variant());
-
-					Dictionary p = d["properties"];
-
-					List<Variant> keys;
-					p.get_key_list(&keys);
-
-					for (const Variant &K : keys) {
-						String property = K;
-						Variant value = to_native(p[K], PASS_ARG);
-						obj->set(property, value);
-					}
-
-					Variant v(obj);
-
-					return v;
-				} else if (type == Variant::get_type_name(Variant::DICTIONARY)) {
-					ERR_FAIL_COND_V(!d.has("pairs"), Variant());
-					Array pairs = d["pairs"];
-					Dictionary r;
-					for (int i = 0; i < pairs.size(); i++) {
-						Dictionary p = pairs[i];
-						ERR_CONTINUE(!p.has("key"));
-						ERR_CONTINUE(!p.has("value"));
-						r[to_native(p["key"], PASS_ARG)] = to_native(p["value"]);
-					}
-					return r;
-				} else if (type == Variant::get_type_name(Variant::ARRAY)) {
-					ERR_PRINT(vformat("Unexpected Array with '%s' key. Arrays are supported natively.", GDTYPE));
-				} else if (type == Variant::get_type_name(Variant::PACKED_BYTE_ARRAY)) {
-					ERR_FAIL_COND_V(!d.has(VALUES), Variant());
-					Array values = d[VALUES];
-					PackedByteArray pbarr;
-					pbarr.resize(values.size());
-					for (int i = 0; i < pbarr.size(); i++) {
-						pbarr.write[i] = values[i];
-					}
-					return pbarr;
-				} else if (type == Variant::get_type_name(Variant::PACKED_INT32_ARRAY)) {
-					ERR_FAIL_COND_V(!d.has(VALUES), Variant());
-					Array values = d[VALUES];
-					PackedInt32Array arr;
-					arr.resize(values.size());
-					for (int i = 0; i < arr.size(); i++) {
-						arr.write[i] = values[i];
-					}
-					return arr;
-				} else if (type == Variant::get_type_name(Variant::PACKED_INT64_ARRAY)) {
-					ERR_FAIL_COND_V(!d.has(VALUES), Variant());
-					Array values = d[VALUES];
-					PackedInt64Array arr;
-					arr.resize(values.size());
-					for (int i = 0; i < arr.size(); i++) {
-						arr.write[i] = values[i];
-					}
-					return arr;
-				} else if (type == Variant::get_type_name(Variant::PACKED_FLOAT32_ARRAY)) {
-					ERR_FAIL_COND_V(!d.has(VALUES), Variant());
-					Array values = d[VALUES];
-					PackedFloat32Array arr;
-					arr.resize(values.size());
-					for (int i = 0; i < arr.size(); i++) {
-						arr.write[i] = values[i];
-					}
-					return arr;
-				} else if (type == Variant::get_type_name(Variant::PACKED_FLOAT64_ARRAY)) {
-					ERR_FAIL_COND_V(!d.has(VALUES), Variant());
-					Array values = d[VALUES];
-					PackedFloat64Array arr;
-					arr.resize(values.size());
-					for (int i = 0; i < arr.size(); i++) {
-						arr.write[i] = values[i];
-					}
-					return arr;
-				} else if (type == Variant::get_type_name(Variant::PACKED_STRING_ARRAY)) {
-					ERR_FAIL_COND_V(!d.has(VALUES), Variant());
-					Array values = d[VALUES];
-					PackedStringArray arr;
-					arr.resize(values.size());
-					for (int i = 0; i < arr.size(); i++) {
-						arr.write[i] = values[i];
-					}
-					return arr;
-				} else if (type == Variant::get_type_name(Variant::PACKED_VECTOR2_ARRAY)) {
-					ERR_FAIL_COND_V(!d.has(VALUES), Variant());
-					Array values = d[VALUES];
-					ERR_FAIL_COND_V(values.size() % 2 != 0, Variant());
-					PackedVector2Array arr;
-					arr.resize(values.size() / 2);
-					for (int i = 0; i < arr.size(); i++) {
-						arr.write[i] = Vector2(values[i * 2 + 0], values[i * 2 + 1]);
-					}
-					return arr;
-				} else if (type == Variant::get_type_name(Variant::PACKED_VECTOR3_ARRAY)) {
-					ERR_FAIL_COND_V(!d.has(VALUES), Variant());
-					Array values = d[VALUES];
-					ERR_FAIL_COND_V(values.size() % 3 != 0, Variant());
-					PackedVector3Array arr;
-					arr.resize(values.size() / 3);
-					for (int i = 0; i < arr.size(); i++) {
-						arr.write[i] = Vector3(values[i * 3 + 0], values[i * 3 + 1], values[i * 3 + 2]);
-					}
-					return arr;
-				} else if (type == Variant::get_type_name(Variant::PACKED_COLOR_ARRAY)) {
-					ERR_FAIL_COND_V(!d.has(VALUES), Variant());
-					Array values = d[VALUES];
-					ERR_FAIL_COND_V(values.size() % 4 != 0, Variant());
-					PackedColorArray arr;
-					arr.resize(values.size() / 4);
-					for (int i = 0; i < arr.size(); i++) {
-						arr.write[i] = Color(values[i * 4 + 0], values[i * 4 + 1], values[i * 4 + 2], values[i * 4 + 3]);
-					}
-					return arr;
-				} else if (type == Variant::get_type_name(Variant::PACKED_VECTOR4_ARRAY)) {
-					ERR_FAIL_COND_V(!d.has(VALUES), Variant());
-					Array values = d[VALUES];
-					ERR_FAIL_COND_V(values.size() % 4 != 0, Variant());
-					PackedVector4Array arr;
-					arr.resize(values.size() / 4);
-					for (int i = 0; i < arr.size(); i++) {
-						arr.write[i] = Vector4(values[i * 4 + 0], values[i * 4 + 1], values[i * 4 + 2], values[i * 4 + 3]);
-					}
-					return arr;
-				} else {
-					return Variant();
-				}
-			} else {
-				// Regular dictionary with string keys.
-				List<Variant> keys;
-				d.get_key_list(&keys);
-				Dictionary r;
-				for (const Variant &K : keys) {
-					r[K] = to_native(d[K], PASS_ARG);
-				}
-				return r;
+			if (s.begins_with("i:")) {
+				return s.substr(2).to_int();
+			} else if (s.begins_with("f:")) {
+				return s.substr(2).to_float();
+			} else if (s.begins_with("s:")) {
+				return s.substr(2);
+			} else if (s.begins_with("sn:")) {
+				return StringName(s.substr(3));
+			} else if (s.begins_with("np:")) {
+				return NodePath(s.substr(3));
 			}
+
+			ERR_FAIL_V_MSG(Variant(), "Invalid string, the type prefix is not recognized.");
 		} break;
+
+		case Variant::DICTIONARY: {
+			const Dictionary dict = p_json;
+
+			ERR_FAIL_COND_V(!dict.has(TYPE), Variant());
+
+#define LOAD_ARGS()                              \
+	ERR_FAIL_COND_V(!dict.has(ARGS), Variant()); \
+	const Array args = dict[ARGS]
+
+#define LOAD_ARGS_CHECK_SIZE(m_size)             \
+	ERR_FAIL_COND_V(!dict.has(ARGS), Variant()); \
+	const Array args = dict[ARGS];               \
+	ERR_FAIL_COND_V(args.size() != (m_size), Variant())
+
+#define LOAD_ARGS_CHECK_FACTOR(m_factor)         \
+	ERR_FAIL_COND_V(!dict.has(ARGS), Variant()); \
+	const Array args = dict[ARGS];               \
+	ERR_FAIL_COND_V(args.size() % (m_factor) != 0, Variant())
+
+			switch (Variant::get_type_by_name(dict[TYPE])) {
+				case Variant::NIL:
+				case Variant::BOOL: {
+					ERR_FAIL_V_MSG(Variant(), vformat(R"(Unexpected "%s": Variant type "%s" is JSON-compliant.)", TYPE, dict[TYPE]));
+				} break;
+
+				case Variant::INT:
+				case Variant::FLOAT:
+				case Variant::STRING:
+				case Variant::STRING_NAME:
+				case Variant::NODE_PATH: {
+					ERR_FAIL_V_MSG(Variant(), vformat(R"(Unexpected "%s": Variant type "%s" must be represented as a string.)", TYPE, dict[TYPE]));
+				} break;
+
+				case Variant::RID: {
+					return RID();
+				} break;
+				case Variant::CALLABLE: {
+					return Callable();
+				} break;
+				case Variant::SIGNAL: {
+					return Signal();
+				} break;
+
+				case Variant::VECTOR2: {
+					LOAD_ARGS_CHECK_SIZE(2);
+
+					Vector2 v;
+					v.x = args[0];
+					v.y = args[1];
+
+					return v;
+				} break;
+				case Variant::VECTOR2I: {
+					LOAD_ARGS_CHECK_SIZE(2);
+
+					Vector2i v;
+					v.x = args[0];
+					v.y = args[1];
+
+					return v;
+				} break;
+				case Variant::RECT2: {
+					LOAD_ARGS_CHECK_SIZE(4);
+
+					Rect2 r;
+					r.position = Point2(args[0], args[1]);
+					r.size = Size2(args[2], args[3]);
+
+					return r;
+				} break;
+				case Variant::RECT2I: {
+					LOAD_ARGS_CHECK_SIZE(4);
+
+					Rect2i r;
+					r.position = Point2i(args[0], args[1]);
+					r.size = Size2i(args[2], args[3]);
+
+					return r;
+				} break;
+				case Variant::VECTOR3: {
+					LOAD_ARGS_CHECK_SIZE(3);
+
+					Vector3 v;
+					v.x = args[0];
+					v.y = args[1];
+					v.z = args[2];
+
+					return v;
+				} break;
+				case Variant::VECTOR3I: {
+					LOAD_ARGS_CHECK_SIZE(3);
+
+					Vector3i v;
+					v.x = args[0];
+					v.y = args[1];
+					v.z = args[2];
+
+					return v;
+				} break;
+				case Variant::TRANSFORM2D: {
+					LOAD_ARGS_CHECK_SIZE(6);
+
+					Transform2D t;
+					t[0] = Vector2(args[0], args[1]);
+					t[1] = Vector2(args[2], args[3]);
+					t[2] = Vector2(args[4], args[5]);
+
+					return t;
+				} break;
+				case Variant::VECTOR4: {
+					LOAD_ARGS_CHECK_SIZE(4);
+
+					Vector4 v;
+					v.x = args[0];
+					v.y = args[1];
+					v.z = args[2];
+					v.w = args[3];
+
+					return v;
+				} break;
+				case Variant::VECTOR4I: {
+					LOAD_ARGS_CHECK_SIZE(4);
+
+					Vector4i v;
+					v.x = args[0];
+					v.y = args[1];
+					v.z = args[2];
+					v.w = args[3];
+
+					return v;
+				} break;
+				case Variant::PLANE: {
+					LOAD_ARGS_CHECK_SIZE(4);
+
+					Plane p;
+					p.normal = Vector3(args[0], args[1], args[2]);
+					p.d = args[3];
+
+					return p;
+				} break;
+				case Variant::QUATERNION: {
+					LOAD_ARGS_CHECK_SIZE(4);
+
+					Quaternion q;
+					q.x = args[0];
+					q.y = args[1];
+					q.z = args[2];
+					q.w = args[3];
+
+					return q;
+				} break;
+				case Variant::AABB: {
+					LOAD_ARGS_CHECK_SIZE(6);
+
+					AABB aabb;
+					aabb.position = Vector3(args[0], args[1], args[2]);
+					aabb.size = Vector3(args[3], args[4], args[5]);
+
+					return aabb;
+				} break;
+				case Variant::BASIS: {
+					LOAD_ARGS_CHECK_SIZE(9);
+
+					Basis b;
+					b.set_column(0, Vector3(args[0], args[1], args[2]));
+					b.set_column(1, Vector3(args[3], args[4], args[5]));
+					b.set_column(2, Vector3(args[6], args[7], args[8]));
+
+					return b;
+				} break;
+				case Variant::TRANSFORM3D: {
+					LOAD_ARGS_CHECK_SIZE(12);
+
+					Transform3D t;
+					t.basis.set_column(0, Vector3(args[0], args[1], args[2]));
+					t.basis.set_column(1, Vector3(args[3], args[4], args[5]));
+					t.basis.set_column(2, Vector3(args[6], args[7], args[8]));
+					t.origin = Vector3(args[9], args[10], args[11]);
+
+					return t;
+				} break;
+				case Variant::PROJECTION: {
+					LOAD_ARGS_CHECK_SIZE(16);
+
+					Projection p;
+					p[0] = Vector4(args[0], args[1], args[2], args[3]);
+					p[1] = Vector4(args[4], args[5], args[6], args[7]);
+					p[2] = Vector4(args[8], args[9], args[10], args[11]);
+					p[3] = Vector4(args[12], args[13], args[14], args[15]);
+
+					return p;
+				} break;
+				case Variant::COLOR: {
+					LOAD_ARGS_CHECK_SIZE(4);
+
+					Color c;
+					c.r = args[0];
+					c.g = args[1];
+					c.b = args[2];
+					c.a = args[3];
+
+					return c;
+				} break;
+
+				case Variant::OBJECT: {
+					// Nothing to do at this stage. `Object` should be treated as a class, not as a built-in type.
+				} break;
+
+				case Variant::DICTIONARY: {
+					LOAD_ARGS_CHECK_FACTOR(2);
+
+					ContainerType key_type;
+					if (!_decode_container_type(dict, KEY_TYPE, key_type, p_allow_objects)) {
+						return Variant();
+					}
+
+					ContainerType value_type;
+					if (!_decode_container_type(dict, VALUE_TYPE, value_type, p_allow_objects)) {
+						return Variant();
+					}
+
+					Dictionary ret;
+
+					if (key_type.builtin_type != Variant::NIL || value_type.builtin_type != Variant::NIL) {
+						ret.set_typed(key_type, value_type);
+					}
+
+					ERR_FAIL_COND_V_MSG(p_depth > Variant::MAX_RECURSION_DEPTH, ret, "Variant is too deep. Bailing.");
+
+					for (int i = 0; i < args.size() / 2; i++) {
+						ret[_to_native(args[i * 2 + 0], p_allow_objects, p_depth + 1)] = _to_native(args[i * 2 + 1], p_allow_objects, p_depth + 1);
+					}
+
+					return ret;
+				} break;
+
+				case Variant::ARRAY: {
+					LOAD_ARGS();
+
+					ContainerType elem_type;
+					if (!_decode_container_type(dict, ELEM_TYPE, elem_type, p_allow_objects)) {
+						return Variant();
+					}
+
+					Array ret;
+
+					if (elem_type.builtin_type != Variant::NIL) {
+						ret.set_typed(elem_type);
+					}
+
+					ERR_FAIL_COND_V_MSG(p_depth > Variant::MAX_RECURSION_DEPTH, ret, "Variant is too deep. Bailing.");
+
+					ret.resize(args.size());
+					for (int i = 0; i < args.size(); i++) {
+						ret[i] = _to_native(args[i], p_allow_objects, p_depth + 1);
+					}
+
+					return ret;
+				} break;
+
+				case Variant::PACKED_BYTE_ARRAY: {
+					LOAD_ARGS();
+
+					PackedByteArray arr;
+					arr.resize(args.size());
+					for (int i = 0; i < arr.size(); i++) {
+						arr.write[i] = args[i];
+					}
+
+					return arr;
+				} break;
+				case Variant::PACKED_INT32_ARRAY: {
+					LOAD_ARGS();
+
+					PackedInt32Array arr;
+					arr.resize(args.size());
+					for (int i = 0; i < arr.size(); i++) {
+						arr.write[i] = args[i];
+					}
+
+					return arr;
+				} break;
+				case Variant::PACKED_INT64_ARRAY: {
+					LOAD_ARGS();
+
+					PackedInt64Array arr;
+					arr.resize(args.size());
+					for (int i = 0; i < arr.size(); i++) {
+						arr.write[i] = args[i];
+					}
+
+					return arr;
+				} break;
+				case Variant::PACKED_FLOAT32_ARRAY: {
+					LOAD_ARGS();
+
+					PackedFloat32Array arr;
+					arr.resize(args.size());
+					for (int i = 0; i < arr.size(); i++) {
+						arr.write[i] = args[i];
+					}
+
+					return arr;
+				} break;
+				case Variant::PACKED_FLOAT64_ARRAY: {
+					LOAD_ARGS();
+
+					PackedFloat64Array arr;
+					arr.resize(args.size());
+					for (int i = 0; i < arr.size(); i++) {
+						arr.write[i] = args[i];
+					}
+
+					return arr;
+				} break;
+				case Variant::PACKED_STRING_ARRAY: {
+					LOAD_ARGS();
+
+					PackedStringArray arr;
+					arr.resize(args.size());
+					for (int i = 0; i < arr.size(); i++) {
+						arr.write[i] = args[i];
+					}
+
+					return arr;
+				} break;
+				case Variant::PACKED_VECTOR2_ARRAY: {
+					LOAD_ARGS_CHECK_FACTOR(2);
+
+					PackedVector2Array arr;
+					arr.resize(args.size() / 2);
+					for (int i = 0; i < arr.size(); i++) {
+						arr.write[i] = Vector2(args[i * 2 + 0], args[i * 2 + 1]);
+					}
+
+					return arr;
+				} break;
+				case Variant::PACKED_VECTOR3_ARRAY: {
+					LOAD_ARGS_CHECK_FACTOR(3);
+
+					PackedVector3Array arr;
+					arr.resize(args.size() / 3);
+					for (int i = 0; i < arr.size(); i++) {
+						arr.write[i] = Vector3(args[i * 3 + 0], args[i * 3 + 1], args[i * 3 + 2]);
+					}
+
+					return arr;
+				} break;
+				case Variant::PACKED_COLOR_ARRAY: {
+					LOAD_ARGS_CHECK_FACTOR(4);
+
+					PackedColorArray arr;
+					arr.resize(args.size() / 4);
+					for (int i = 0; i < arr.size(); i++) {
+						arr.write[i] = Color(args[i * 4 + 0], args[i * 4 + 1], args[i * 4 + 2], args[i * 4 + 3]);
+					}
+
+					return arr;
+				} break;
+				case Variant::PACKED_VECTOR4_ARRAY: {
+					LOAD_ARGS_CHECK_FACTOR(4);
+
+					PackedVector4Array arr;
+					arr.resize(args.size() / 4);
+					for (int i = 0; i < arr.size(); i++) {
+						arr.write[i] = Vector4(args[i * 4 + 0], args[i * 4 + 1], args[i * 4 + 2], args[i * 4 + 3]);
+					}
+
+					return arr;
+				} break;
+
+				case Variant::VARIANT_MAX: {
+					// Nothing to do.
+				} break;
+			}
+
+#undef LOAD_ARGS
+#undef LOAD_ARGS_CHECK_SIZE
+#undef LOAD_ARGS_CHECK_FACTOR
+
+			if (ClassDB::class_exists(dict[TYPE])) {
+				ERR_FAIL_COND_V(!p_allow_objects, Variant());
+
+				ERR_FAIL_COND_V_MSG(p_depth > Variant::MAX_RECURSION_DEPTH, Variant(), "Variant is too deep. Bailing.");
+
+				ERR_FAIL_COND_V(!dict.has(PROPS), Variant());
+				const Array props = dict[PROPS];
+				ERR_FAIL_COND_V(props.size() % 2 != 0, Variant());
+
+				ERR_FAIL_COND_V(!ClassDB::can_instantiate(dict[TYPE]), Variant());
+
+				Object *obj = ClassDB::instantiate(dict[TYPE]);
+				ERR_FAIL_NULL_V(obj, Variant());
+
+				// Avoid premature free `RefCounted`. This must be done before properties are initialized,
+				// since script functions (setters, implicit initializer) may be called. See GH-68666.
+				Variant variant;
+				if (Object::cast_to<RefCounted>(obj)) {
+					const Ref<RefCounted> ref = Ref<RefCounted>(Object::cast_to<RefCounted>(obj));
+					variant = ref;
+				} else {
+					variant = obj;
+				}
+
+				for (int i = 0; i < props.size() / 2; i++) {
+					const StringName name = props[i * 2 + 0];
+					const Variant value = _to_native(props[i * 2 + 1], p_allow_objects, p_depth + 1);
+
+					if (name == CoreStringName(script) && value.get_type() != Variant::NIL) {
+						const String path = value;
+						ERR_FAIL_COND_V_MSG(path.is_empty() || !path.begins_with("res://") || !ResourceLoader::exists(path, "Script"),
+								Variant(),
+								vformat(R"(Invalid script path "%s".)", path));
+
+						const Ref<Script> script = ResourceLoader::load(path, "Script");
+						ERR_FAIL_COND_V_MSG(script.is_null(), Variant(), vformat(R"(Can't load script at path "%s".)", path));
+
+						obj->set_script(script);
+					} else {
+						obj->set(name, value);
+					}
+				}
+
+				return variant;
+			}
+
+			ERR_FAIL_V_MSG(Variant(), vformat(R"(Invalid type "%s".)", dict[TYPE]));
+		} break;
+
 		case Variant::ARRAY: {
-			Array arr = p_json;
+			ERR_FAIL_COND_V_MSG(p_depth > Variant::MAX_RECURSION_DEPTH, Array(), "Variant is too deep. Bailing.");
+
+			const Array arr = p_json;
+
 			Array ret;
 			ret.resize(arr.size());
 			for (int i = 0; i < arr.size(); i++) {
-				ret[i] = to_native(arr[i], PASS_ARG);
+				ret[i] = _to_native(arr[i], p_allow_objects, p_depth + 1);
 			}
+
 			return ret;
 		} break;
+
 		default: {
-			ERR_PRINT(vformat("Unhandled conversion from JSON type '%s' to native Variant type.", Variant::get_type_name(p_json.get_type())));
-			return Variant();
-		}
+			// Nothing to do.
+		} break;
 	}
 
-	return Variant();
+	ERR_FAIL_V_MSG(Variant(), vformat(R"(Variant type "%s" is not JSON-compliant.)", Variant::get_type_name(p_json.get_type())));
 }
 
-#undef GDTYPE
-#undef VALUES
-#undef PASS_ARG
+#undef TYPE
+#undef ELEM_TYPE
+#undef KEY_TYPE
+#undef VALUE_TYPE
+#undef ARGS
+#undef PROPS
 
 ////////////
 
diff --git a/core/io/json.h b/core/io/json.h
index 67b5e09afa..c6dfa31175 100644
--- a/core/io/json.h
+++ b/core/io/json.h
@@ -80,6 +80,9 @@ class JSON : public Resource {
 	static Error _parse_object(Dictionary &object, const char32_t *p_str, int &index, int p_len, int &line, int p_depth, String &r_err_str);
 	static Error _parse_string(const String &p_json, Variant &r_ret, String &r_err_str, int &r_err_line);
 
+	static Variant _from_native(const Variant &p_variant, bool p_full_objects, int p_depth);
+	static Variant _to_native(const Variant &p_json, bool p_allow_objects, int p_depth);
+
 protected:
 	static void _bind_methods();
 
@@ -90,13 +93,18 @@ public:
 	static String stringify(const Variant &p_var, const String &p_indent = "", bool p_sort_keys = true, bool p_full_precision = false);
 	static Variant parse_string(const String &p_json_string);
 
-	inline Variant get_data() const { return data; }
-	void set_data(const Variant &p_data);
-	inline int get_error_line() const { return err_line; }
-	inline String get_error_message() const { return err_str; }
+	_FORCE_INLINE_ static Variant from_native(const Variant &p_variant, bool p_full_objects = false) {
+		return _from_native(p_variant, p_full_objects, 0);
+	}
+	_FORCE_INLINE_ static Variant to_native(const Variant &p_json, bool p_allow_objects = false) {
+		return _to_native(p_json, p_allow_objects, 0);
+	}
 
-	static Variant from_native(const Variant &p_variant, bool p_allow_classes = false, bool p_allow_scripts = false);
-	static Variant to_native(const Variant &p_json, bool p_allow_classes = false, bool p_allow_scripts = false);
+	void set_data(const Variant &p_data);
+	_FORCE_INLINE_ Variant get_data() const { return data; }
+
+	_FORCE_INLINE_ int get_error_line() const { return err_line; }
+	_FORCE_INLINE_ String get_error_message() const { return err_str; }
 };
 
 class ResourceFormatLoaderJSON : public ResourceFormatLoader {
diff --git a/core/io/marshalls.cpp b/core/io/marshalls.cpp
index d9664e7370..d9149a6e73 100644
--- a/core/io/marshalls.cpp
+++ b/core/io/marshalls.cpp
@@ -33,6 +33,7 @@
 #include "core/io/resource_loader.h"
 #include "core/object/ref_counted.h"
 #include "core/object/script_language.h"
+#include "core/variant/container_type_validate.h"
 
 #include <limits.h>
 #include <stdio.h>
@@ -84,12 +85,6 @@ enum ContainerTypeKind {
 	CONTAINER_TYPE_KIND_SCRIPT = 0b11,
 };
 
-struct ContainerType {
-	Variant::Type builtin_type = Variant::NIL;
-	StringName class_name;
-	Ref<Script> script;
-};
-
 #define GET_CONTAINER_TYPE_KIND(m_header, m_field) \
 	((ContainerTypeKind)(((m_header) & HEADER_DATA_FIELD_##m_field##_MASK) >> HEADER_DATA_FIELD_##m_field##_SHIFT))
 
@@ -844,7 +839,7 @@ Error decode_variant(Variant &r_variant, const uint8_t *p_buffer, int p_len, int
 
 			Dictionary dict;
 			if (key_type.builtin_type != Variant::NIL || value_type.builtin_type != Variant::NIL) {
-				dict.set_typed(key_type.builtin_type, key_type.class_name, key_type.script, value_type.builtin_type, value_type.class_name, value_type.script);
+				dict.set_typed(key_type, value_type);
 			}
 
 			for (int i = 0; i < count; i++) {
@@ -901,7 +896,7 @@ Error decode_variant(Variant &r_variant, const uint8_t *p_buffer, int p_len, int
 
 			Array array;
 			if (type.builtin_type != Variant::NIL) {
-				array.set_typed(type.builtin_type, type.class_name, type.script);
+				array.set_typed(type);
 			}
 
 			for (int i = 0; i < count; i++) {
@@ -1402,31 +1397,13 @@ Error encode_variant(const Variant &p_variant, uint8_t *r_buffer, int &r_len, bo
 			}
 		} break;
 		case Variant::DICTIONARY: {
-			Dictionary dict = p_variant;
-
-			ContainerType key_type;
-			key_type.builtin_type = (Variant::Type)dict.get_typed_key_builtin();
-			key_type.class_name = dict.get_typed_key_class_name();
-			key_type.script = dict.get_typed_key_script();
-
-			_encode_container_type_header(key_type, header, HEADER_DATA_FIELD_TYPED_DICTIONARY_KEY_SHIFT, p_full_objects);
-
-			ContainerType value_type;
-			value_type.builtin_type = (Variant::Type)dict.get_typed_value_builtin();
-			value_type.class_name = dict.get_typed_value_class_name();
-			value_type.script = dict.get_typed_value_script();
-
-			_encode_container_type_header(value_type, header, HEADER_DATA_FIELD_TYPED_DICTIONARY_VALUE_SHIFT, p_full_objects);
+			const Dictionary dict = p_variant;
+			_encode_container_type_header(dict.get_key_type(), header, HEADER_DATA_FIELD_TYPED_DICTIONARY_KEY_SHIFT, p_full_objects);
+			_encode_container_type_header(dict.get_value_type(), header, HEADER_DATA_FIELD_TYPED_DICTIONARY_VALUE_SHIFT, p_full_objects);
 		} break;
 		case Variant::ARRAY: {
-			Array array = p_variant;
-
-			ContainerType type;
-			type.builtin_type = (Variant::Type)array.get_typed_builtin();
-			type.class_name = array.get_typed_class_name();
-			type.script = array.get_typed_script();
-
-			_encode_container_type_header(type, header, HEADER_DATA_FIELD_TYPED_ARRAY_SHIFT, p_full_objects);
+			const Array array = p_variant;
+			_encode_container_type_header(array.get_element_type(), header, HEADER_DATA_FIELD_TYPED_ARRAY_SHIFT, p_full_objects);
 		} break;
 #ifdef REAL_T_IS_DOUBLE
 		case Variant::VECTOR2:
@@ -1850,27 +1827,17 @@ Error encode_variant(const Variant &p_variant, uint8_t *r_buffer, int &r_len, bo
 			r_len += 8;
 		} break;
 		case Variant::DICTIONARY: {
-			Dictionary dict = p_variant;
+			const Dictionary dict = p_variant;
 
 			{
-				ContainerType key_type;
-				key_type.builtin_type = (Variant::Type)dict.get_typed_key_builtin();
-				key_type.class_name = dict.get_typed_key_class_name();
-				key_type.script = dict.get_typed_key_script();
-
-				Error err = _encode_container_type(key_type, buf, r_len, p_full_objects);
+				Error err = _encode_container_type(dict.get_key_type(), buf, r_len, p_full_objects);
 				if (err) {
 					return err;
 				}
 			}
 
 			{
-				ContainerType value_type;
-				value_type.builtin_type = (Variant::Type)dict.get_typed_value_builtin();
-				value_type.class_name = dict.get_typed_value_class_name();
-				value_type.script = dict.get_typed_value_script();
-
-				Error err = _encode_container_type(value_type, buf, r_len, p_full_objects);
+				Error err = _encode_container_type(dict.get_value_type(), buf, r_len, p_full_objects);
 				if (err) {
 					return err;
 				}
@@ -1894,7 +1861,7 @@ Error encode_variant(const Variant &p_variant, uint8_t *r_buffer, int &r_len, bo
 				if (buf) {
 					buf += len;
 				}
-				Variant *value = dict.getptr(key);
+				const Variant *value = dict.getptr(key);
 				ERR_FAIL_NULL_V(value, ERR_BUG);
 				err = encode_variant(*value, buf, len, p_full_objects, p_depth + 1);
 				ERR_FAIL_COND_V(err, err);
@@ -1907,15 +1874,10 @@ Error encode_variant(const Variant &p_variant, uint8_t *r_buffer, int &r_len, bo
 
 		} break;
 		case Variant::ARRAY: {
-			Array array = p_variant;
+			const Array array = p_variant;
 
 			{
-				ContainerType type;
-				type.builtin_type = (Variant::Type)array.get_typed_builtin();
-				type.class_name = array.get_typed_class_name();
-				type.script = array.get_typed_script();
-
-				Error err = _encode_container_type(type, buf, r_len, p_full_objects);
+				Error err = _encode_container_type(array.get_element_type(), buf, r_len, p_full_objects);
 				if (err) {
 					return err;
 				}
diff --git a/core/math/expression.cpp b/core/math/expression.cpp
index 35303fe9ac..b7fd26ae36 100644
--- a/core/math/expression.cpp
+++ b/core/math/expression.cpp
@@ -473,10 +473,11 @@ Error Expression::_get_token(Token &r_token) {
 					} else if (id == "self") {
 						r_token.type = TK_SELF;
 					} else {
-						for (int i = 0; i < Variant::VARIANT_MAX; i++) {
-							if (id == Variant::get_type_name(Variant::Type(i))) {
+						{
+							const Variant::Type type = Variant::get_type_by_name(id);
+							if (type < Variant::VARIANT_MAX) {
 								r_token.type = TK_BASIC_TYPE;
-								r_token.value = i;
+								r_token.value = type;
 								return OK;
 							}
 						}
diff --git a/core/variant/array.cpp b/core/variant/array.cpp
index f7a86b8fa3..abde597f20 100644
--- a/core/variant/array.cpp
+++ b/core/variant/array.cpp
@@ -41,8 +41,7 @@
 #include "core/variant/dictionary.h"
 #include "core/variant/variant.h"
 
-class ArrayPrivate {
-public:
+struct ArrayPrivate {
 	SafeRefCount refcount;
 	Vector<Variant> array;
 	Variant *read_only = nullptr; // If enabled, a pointer is used to a temporary value that is used to return read-only values.
@@ -843,6 +842,10 @@ Array::Array(const Array &p_from, uint32_t p_type, const StringName &p_class_nam
 	assign(p_from);
 }
 
+void Array::set_typed(const ContainerType &p_element_type) {
+	set_typed(p_element_type.builtin_type, p_element_type.class_name, p_element_type.script);
+}
+
 void Array::set_typed(uint32_t p_type, const StringName &p_class_name, const Variant &p_script) {
 	ERR_FAIL_COND_MSG(_p->read_only, "Array is in read-only state.");
 	ERR_FAIL_COND_MSG(_p->array.size() > 0, "Type can only be set when array is empty.");
@@ -870,6 +873,14 @@ bool Array::is_same_instance(const Array &p_other) const {
 	return _p == p_other._p;
 }
 
+ContainerType Array::get_element_type() const {
+	ContainerType type;
+	type.builtin_type = _p->typed.type;
+	type.class_name = _p->typed.class_name;
+	type.script = _p->typed.script;
+	return type;
+}
+
 uint32_t Array::get_typed_builtin() const {
 	return _p->typed.type;
 }
diff --git a/core/variant/array.h b/core/variant/array.h
index 6c3bae6ccb..78b5d71c88 100644
--- a/core/variant/array.h
+++ b/core/variant/array.h
@@ -35,11 +35,13 @@
 
 #include <climits>
 
-class Variant;
-class ArrayPrivate;
+class Callable;
 class Object;
 class StringName;
-class Callable;
+class Variant;
+
+struct ArrayPrivate;
+struct ContainerType;
 
 class Array {
 	mutable ArrayPrivate *_p;
@@ -185,10 +187,14 @@ public:
 
 	const void *id() const;
 
+	void set_typed(const ContainerType &p_element_type);
 	void set_typed(uint32_t p_type, const StringName &p_class_name, const Variant &p_script);
+
 	bool is_typed() const;
 	bool is_same_typed(const Array &p_other) const;
 	bool is_same_instance(const Array &p_other) const;
+
+	ContainerType get_element_type() const;
 	uint32_t get_typed_builtin() const;
 	StringName get_typed_class_name() const;
 	Variant get_typed_script() const;
diff --git a/core/variant/container_type_validate.h b/core/variant/container_type_validate.h
index 8971fadf73..18761a639a 100644
--- a/core/variant/container_type_validate.h
+++ b/core/variant/container_type_validate.h
@@ -34,6 +34,12 @@
 #include "core/object/script_language.h"
 #include "core/variant/variant.h"
 
+struct ContainerType {
+	Variant::Type builtin_type = Variant::NIL;
+	StringName class_name;
+	Ref<Script> script;
+};
+
 struct ContainerTypeValidate {
 	Variant::Type type = Variant::NIL;
 	StringName class_name;
diff --git a/core/variant/dictionary.cpp b/core/variant/dictionary.cpp
index 501ca69205..233e45f876 100644
--- a/core/variant/dictionary.cpp
+++ b/core/variant/dictionary.cpp
@@ -595,6 +595,10 @@ Dictionary Dictionary::recursive_duplicate(bool p_deep, int recursion_count) con
 	return n;
 }
 
+void Dictionary::set_typed(const ContainerType &p_key_type, const ContainerType &p_value_type) {
+	set_typed(p_key_type.builtin_type, p_key_type.class_name, p_key_type.script, p_value_type.builtin_type, p_value_type.class_name, p_key_type.script);
+}
+
 void Dictionary::set_typed(uint32_t p_key_type, const StringName &p_key_class_name, const Variant &p_key_script, uint32_t p_value_type, const StringName &p_value_class_name, const Variant &p_value_script) {
 	ERR_FAIL_COND_MSG(_p->read_only, "Dictionary is in read-only state.");
 	ERR_FAIL_COND_MSG(_p->variant_map.size() > 0, "Type can only be set when dictionary is empty.");
@@ -641,6 +645,22 @@ bool Dictionary::is_same_typed_value(const Dictionary &p_other) const {
 	return _p->typed_value == p_other._p->typed_value;
 }
 
+ContainerType Dictionary::get_key_type() const {
+	ContainerType type;
+	type.builtin_type = _p->typed_key.type;
+	type.class_name = _p->typed_key.class_name;
+	type.script = _p->typed_key.script;
+	return type;
+}
+
+ContainerType Dictionary::get_value_type() const {
+	ContainerType type;
+	type.builtin_type = _p->typed_value.type;
+	type.class_name = _p->typed_value.class_name;
+	type.script = _p->typed_value.script;
+	return type;
+}
+
 uint32_t Dictionary::get_typed_key_builtin() const {
 	return _p->typed_key.type;
 }
diff --git a/core/variant/dictionary.h b/core/variant/dictionary.h
index bbfb5b3083..c24dff7c4d 100644
--- a/core/variant/dictionary.h
+++ b/core/variant/dictionary.h
@@ -37,6 +37,7 @@
 
 class Variant;
 
+struct ContainerType;
 struct DictionaryPrivate;
 
 class Dictionary {
@@ -91,13 +92,18 @@ public:
 	Dictionary duplicate(bool p_deep = false) const;
 	Dictionary recursive_duplicate(bool p_deep, int recursion_count) const;
 
+	void set_typed(const ContainerType &p_key_type, const ContainerType &p_value_type);
 	void set_typed(uint32_t p_key_type, const StringName &p_key_class_name, const Variant &p_key_script, uint32_t p_value_type, const StringName &p_value_class_name, const Variant &p_value_script);
+
 	bool is_typed() const;
 	bool is_typed_key() const;
 	bool is_typed_value() const;
 	bool is_same_typed(const Dictionary &p_other) const;
 	bool is_same_typed_key(const Dictionary &p_other) const;
 	bool is_same_typed_value(const Dictionary &p_other) const;
+
+	ContainerType get_key_type() const;
+	ContainerType get_value_type() const;
 	uint32_t get_typed_key_builtin() const;
 	uint32_t get_typed_value_builtin() const;
 	StringName get_typed_key_class_name() const;
diff --git a/core/variant/variant.cpp b/core/variant/variant.cpp
index 8aca56c1d5..c879efecb0 100644
--- a/core/variant/variant.cpp
+++ b/core/variant/variant.cpp
@@ -176,6 +176,18 @@ String Variant::get_type_name(Variant::Type p_type) {
 	return "";
 }
 
+Variant::Type Variant::get_type_by_name(const String &p_type_name) {
+	static HashMap<String, Type> type_names;
+	if (unlikely(type_names.is_empty())) {
+		for (int i = 0; i < VARIANT_MAX; i++) {
+			type_names[get_type_name((Type)i)] = (Type)i;
+		}
+	}
+
+	const Type *ptr = type_names.getptr(p_type_name);
+	return (ptr == nullptr) ? VARIANT_MAX : *ptr;
+}
+
 bool Variant::can_convert(Variant::Type p_type_from, Variant::Type p_type_to) {
 	if (p_type_from == p_type_to) {
 		return true;
diff --git a/core/variant/variant.h b/core/variant/variant.h
index babd2cf084..12528ae515 100644
--- a/core/variant/variant.h
+++ b/core/variant/variant.h
@@ -354,6 +354,7 @@ public:
 		return type;
 	}
 	static String get_type_name(Variant::Type p_type);
+	static Variant::Type get_type_by_name(const String &p_type_name);
 	static bool can_convert(Type p_type_from, Type p_type_to);
 	static bool can_convert_strict(Type p_type_from, Type p_type_to);
 	static bool is_type_shared(Variant::Type p_type);
diff --git a/doc/classes/JSON.xml b/doc/classes/JSON.xml
index fe5fdfa89a..6cd3d2c2e4 100644
--- a/doc/classes/JSON.xml
+++ b/doc/classes/JSON.xml
@@ -40,11 +40,15 @@
 		<method name="from_native" qualifiers="static">
 			<return type="Variant" />
 			<param index="0" name="variant" type="Variant" />
-			<param index="1" name="allow_classes" type="bool" default="false" />
-			<param index="2" name="allow_scripts" type="bool" default="false" />
+			<param index="1" name="full_objects" type="bool" default="false" />
 			<description>
-				Converts a native engine type to a JSON-compliant dictionary.
-				By default, classes and scripts are ignored for security reasons, unless [param allow_classes] or [param allow_scripts] are specified.
+				Converts a native engine type to a JSON-compliant value.
+				By default, objects are ignored for security reasons, unless [param full_objects] is [code]true[/code].
+				You can convert a native value to a JSON string like this:
+				[codeblock]
+				func encode_data(value, full_objects = false):
+				    return JSON.stringify(JSON.from_native(value, full_objects))
+				[/codeblock]
 			</description>
 		</method>
 		<method name="get_error_line" qualifiers="const">
@@ -136,11 +140,15 @@
 		<method name="to_native" qualifiers="static">
 			<return type="Variant" />
 			<param index="0" name="json" type="Variant" />
-			<param index="1" name="allow_classes" type="bool" default="false" />
-			<param index="2" name="allow_scripts" type="bool" default="false" />
+			<param index="1" name="allow_objects" type="bool" default="false" />
 			<description>
-				Converts a JSON-compliant dictionary that was created with [method from_native] back to native engine types.
-				By default, classes and scripts are ignored for security reasons, unless [param allow_classes] or [param allow_scripts] are specified.
+				Converts a JSON-compliant value that was created with [method from_native] back to native engine types.
+				By default, objects are ignored for security reasons, unless [param allow_objects] is [code]true[/code].
+				You can convert a JSON string back to a native value like this:
+				[codeblock]
+				func decode_data(string, allow_objects = false):
+				    return JSON.to_native(JSON.parse_string(string), allow_objects)
+				[/codeblock]
 			</description>
 		</method>
 	</methods>
diff --git a/editor/editor_autoload_settings.cpp b/editor/editor_autoload_settings.cpp
index 23a2f5b13c..24d98e4c6d 100644
--- a/editor/editor_autoload_settings.cpp
+++ b/editor/editor_autoload_settings.cpp
@@ -112,14 +112,12 @@ bool EditorAutoloadSettings::_autoload_name_is_valid(const String &p_name, Strin
 		return false;
 	}
 
-	for (int i = 0; i < Variant::VARIANT_MAX; i++) {
-		if (Variant::get_type_name(Variant::Type(i)) == p_name) {
-			if (r_error) {
-				*r_error = TTR("Invalid name.") + " " + TTR("Must not collide with an existing built-in type name.");
-			}
-
-			return false;
+	if (Variant::get_type_by_name(p_name) < Variant::VARIANT_MAX) {
+		if (r_error) {
+			*r_error = TTR("Invalid name.") + " " + TTR("Must not collide with an existing built-in type name.");
 		}
+
+		return false;
 	}
 
 	for (int i = 0; i < CoreConstants::get_global_constant_count(); i++) {
diff --git a/modules/gdscript/gdscript_editor.cpp b/modules/gdscript/gdscript_editor.cpp
index cfff20f6d3..88749b8684 100644
--- a/modules/gdscript/gdscript_editor.cpp
+++ b/modules/gdscript/gdscript_editor.cpp
@@ -3858,13 +3858,10 @@ static Error _lookup_symbol_from_base(const GDScriptParser::DataType &p_base, co
 		return OK;
 	}
 
-	for (int i = 0; i < Variant::VARIANT_MAX; i++) {
-		Variant::Type t = Variant::Type(i);
-		if (Variant::get_type_name(t) == p_symbol) {
-			r_result.type = ScriptLanguage::LOOKUP_RESULT_CLASS;
-			r_result.class_name = Variant::get_type_name(t);
-			return OK;
-		}
+	if (Variant::get_type_by_name(p_symbol) < Variant::VARIANT_MAX) {
+		r_result.type = ScriptLanguage::LOOKUP_RESULT_CLASS;
+		r_result.class_name = p_symbol;
+		return OK;
 	}
 
 	if ("Variant" == p_symbol) {
diff --git a/tests/core/io/test_json_native.h b/tests/core/io/test_json_native.h
index 819078ac57..f30c5b1d4e 100644
--- a/tests/core/io/test_json_native.h
+++ b/tests/core/io/test_json_native.h
@@ -33,128 +33,195 @@
 
 #include "core/io/json.h"
 
+#include "core/variant/typed_array.h"
+#include "core/variant/typed_dictionary.h"
+#include "tests/test_macros.h"
+
 namespace TestJSONNative {
 
-bool compare_variants(Variant variant_1, Variant variant_2, int depth = 0) {
-	if (depth > 100) {
-		return false;
-	}
-	if (variant_1.get_type() == Variant::RID && variant_2.get_type() == Variant::RID) {
-		return true;
-	}
-	if (variant_1.get_type() == Variant::CALLABLE || variant_2.get_type() == Variant::CALLABLE) {
-		return true;
-	}
-
-	List<PropertyInfo> variant_1_properties;
-	variant_1.get_property_list(&variant_1_properties);
-	List<PropertyInfo> variant_2_properties;
-	variant_2.get_property_list(&variant_2_properties);
-
-	if (variant_1_properties.size() != variant_2_properties.size()) {
-		return false;
-	}
-
-	for (List<PropertyInfo>::Element *E = variant_1_properties.front(); E; E = E->next()) {
-		String name = E->get().name;
-		Variant variant_1_value = variant_1.get(name);
-		Variant variant_2_value = variant_2.get(name);
-
-		if (!compare_variants(variant_1_value, variant_2_value, depth + 1)) {
-			return false;
-		}
-	}
-
-	return true;
+String encode(const Variant &p_variant, bool p_full_objects = false) {
+	return JSON::stringify(JSON::from_native(p_variant, p_full_objects), "", false);
 }
 
-TEST_CASE("[JSON][Native][SceneTree] Conversion between native and JSON formats") {
-	for (int variant_i = 0; variant_i < Variant::VARIANT_MAX; variant_i++) {
-		Variant::Type type = static_cast<Variant::Type>(variant_i);
-		Variant native_data;
-		Callable::CallError error;
-
-		if (type == Variant::Type::INT || type == Variant::Type::FLOAT) {
-			Variant value = int64_t(INT64_MAX);
-			const Variant *args[] = { &value };
-			Variant::construct(type, native_data, args, 1, error);
-		} else if (type == Variant::Type::OBJECT) {
-			Ref<JSON> json = memnew(JSON);
-			native_data = json;
-		} else if (type == Variant::Type::DICTIONARY) {
-			Dictionary dictionary;
-			dictionary["key"] = "value";
-			native_data = dictionary;
-		} else if (type == Variant::Type::ARRAY) {
-			Array array;
-			array.push_back("element1");
-			array.push_back("element2");
-			native_data = array;
-		} else if (type == Variant::Type::PACKED_BYTE_ARRAY) {
-			PackedByteArray packed_array;
-			packed_array.push_back(1);
-			packed_array.push_back(2);
-			native_data = packed_array;
-		} else if (type == Variant::Type::PACKED_INT32_ARRAY) {
-			PackedInt32Array packed_array;
-			packed_array.push_back(INT32_MIN);
-			packed_array.push_back(INT32_MAX);
-			native_data = packed_array;
-		} else if (type == Variant::Type::PACKED_INT64_ARRAY) {
-			PackedInt64Array packed_array;
-			packed_array.push_back(INT64_MIN);
-			packed_array.push_back(INT64_MAX);
-			native_data = packed_array;
-		} else if (type == Variant::Type::PACKED_FLOAT32_ARRAY) {
-			PackedFloat32Array packed_array;
-			packed_array.push_back(FLT_MIN);
-			packed_array.push_back(FLT_MAX);
-			native_data = packed_array;
-		} else if (type == Variant::Type::PACKED_FLOAT64_ARRAY) {
-			PackedFloat64Array packed_array;
-			packed_array.push_back(DBL_MIN);
-			packed_array.push_back(DBL_MAX);
-			native_data = packed_array;
-		} else if (type == Variant::Type::PACKED_STRING_ARRAY) {
-			PackedStringArray packed_array;
-			packed_array.push_back("string1");
-			packed_array.push_back("string2");
-			native_data = packed_array;
-		} else if (type == Variant::Type::PACKED_VECTOR2_ARRAY) {
-			PackedVector2Array packed_array;
-			Vector2 vector(1.0, 2.0);
-			packed_array.push_back(vector);
-			native_data = packed_array;
-		} else if (type == Variant::Type::PACKED_VECTOR3_ARRAY) {
-			PackedVector3Array packed_array;
-			Vector3 vector(1.0, 2.0, 3.0);
-			packed_array.push_back(vector);
-			native_data = packed_array;
-		} else if (type == Variant::Type::PACKED_COLOR_ARRAY) {
-			PackedColorArray packed_array;
-			Color color(1.0, 1.0, 1.0);
-			packed_array.push_back(color);
-			native_data = packed_array;
-		} else if (type == Variant::Type::PACKED_VECTOR4_ARRAY) {
-			PackedVector4Array packed_array;
-			Vector4 vector(1.0, 2.0, 3.0, 4.0);
-			packed_array.push_back(vector);
-			native_data = packed_array;
-		} else {
-			Variant::construct(type, native_data, nullptr, 0, error);
-		}
-		Variant json_converted_from_native = JSON::from_native(native_data, true, true);
-		Variant variant_native_converted = JSON::to_native(json_converted_from_native, true, true);
-		CHECK_MESSAGE(compare_variants(native_data, variant_native_converted),
-				vformat("Conversion from native to JSON type %s and back successful. \nNative: %s \nNative Converted: %s \nError: %s\nConversion from native to JSON type %s successful: %s",
-						Variant::get_type_name(type),
-						native_data,
-						variant_native_converted,
-						itos(error.error),
-						Variant::get_type_name(type),
-						json_converted_from_native));
-	}
+Variant decode(const String &p_string, bool p_allow_objects = false) {
+	return JSON::to_native(JSON::parse_string(p_string), p_allow_objects);
 }
+
+void test(const Variant &p_variant, const String &p_string, bool p_with_objects = false) {
+	CHECK(encode(p_variant, p_with_objects) == p_string);
+	CHECK(decode(p_string, p_with_objects).get_construct_string() == p_variant.get_construct_string());
+}
+
+TEST_CASE("[JSON][Native] Conversion between native and JSON formats") {
+	// `Nil` and `bool` (represented as JSON keyword literals).
+	test(Variant(), "null");
+	test(false, "false");
+	test(true, "true");
+
+	// Numbers and strings (represented as JSON strings).
+	test(1, R"("i:1")");
+	test(1.0, R"("f:1.0")");
+	test(String("abc"), R"("s:abc")");
+	test(StringName("abc"), R"("sn:abc")");
+	test(NodePath("abc"), R"("np:abc")");
+
+	// Non-serializable types (always empty after deserialization).
+	test(RID(), R"({"type":"RID"})");
+	test(Callable(), R"({"type":"Callable"})");
+	test(Signal(), R"({"type":"Signal"})");
+
+	// Math types.
+
+	test(Vector2(1, 2), R"({"type":"Vector2","args":[1.0,2.0]})");
+	test(Vector2i(1, 2), R"({"type":"Vector2i","args":[1,2]})");
+	test(Rect2(1, 2, 3, 4), R"({"type":"Rect2","args":[1.0,2.0,3.0,4.0]})");
+	test(Rect2i(1, 2, 3, 4), R"({"type":"Rect2i","args":[1,2,3,4]})");
+	test(Vector3(1, 2, 3), R"({"type":"Vector3","args":[1.0,2.0,3.0]})");
+	test(Vector3i(1, 2, 3), R"({"type":"Vector3i","args":[1,2,3]})");
+	test(Transform2D(1, 2, 3, 4, 5, 6), R"({"type":"Transform2D","args":[1.0,2.0,3.0,4.0,5.0,6.0]})");
+	test(Vector4(1, 2, 3, 4), R"({"type":"Vector4","args":[1.0,2.0,3.0,4.0]})");
+	test(Vector4i(1, 2, 3, 4), R"({"type":"Vector4i","args":[1,2,3,4]})");
+	test(Plane(1, 2, 3, 4), R"({"type":"Plane","args":[1.0,2.0,3.0,4.0]})");
+	test(Quaternion(1, 2, 3, 4), R"({"type":"Quaternion","args":[1.0,2.0,3.0,4.0]})");
+	test(AABB(Vector3(1, 2, 3), Vector3(4, 5, 6)), R"({"type":"AABB","args":[1.0,2.0,3.0,4.0,5.0,6.0]})");
+
+	const Basis b(Vector3(1, 2, 3), Vector3(4, 5, 6), Vector3(7, 8, 9));
+	test(b, R"({"type":"Basis","args":[1.0,2.0,3.0,4.0,5.0,6.0,7.0,8.0,9.0]})");
+
+	const Transform3D tr3d(Vector3(1, 2, 3), Vector3(4, 5, 6), Vector3(7, 8, 9), Vector3(10, 11, 12));
+	test(tr3d, R"({"type":"Transform3D","args":[1.0,2.0,3.0,4.0,5.0,6.0,7.0,8.0,9.0,10.0,11.0,12.0]})");
+
+	const Projection p(Vector4(1, 2, 3, 4), Vector4(5, 6, 7, 8), Vector4(9, 10, 11, 12), Vector4(13, 14, 15, 16));
+	test(p, R"({"type":"Projection","args":[1.0,2.0,3.0,4.0,5.0,6.0,7.0,8.0,9.0,10.0,11.0,12.0,13.0,14.0,15.0,16.0]})");
+
+	test(Color(1, 2, 3, 4), R"({"type":"Color","args":[1.0,2.0,3.0,4.0]})");
+
+	// `Object`.
+
+	Ref<Resource> res;
+	res.instantiate();
+
+	// The properties are stored in an array because the order in which they are assigned may be important during initialization.
+	const String res_repr = R"({"type":"Resource","props":["resource_local_to_scene",false,"resource_name","s:","script",null]})";
+
+	test(res, res_repr, true);
+	ERR_PRINT_OFF;
+	CHECK(encode(res) == "null");
+	CHECK(decode(res_repr).get_type() == Variant::NIL);
+	ERR_PRINT_ON;
+
+	// `Dictionary`.
+
+	Dictionary dict;
+	dict[false] = true;
+	dict[0] = 1;
+	dict[0.0] = 1.0;
+
+	// Godot dictionaries preserve insertion order, so an array is used for keys/values.
+	test(dict, R"({"type":"Dictionary","args":[false,true,"i:0","i:1","f:0.0","f:1.0"]})");
+
+	TypedDictionary<int64_t, int64_t> int_int_dict;
+	int_int_dict[1] = 2;
+	int_int_dict[3] = 4;
+
+	test(int_int_dict, R"({"type":"Dictionary","key_type":"int","value_type":"int","args":["i:1","i:2","i:3","i:4"]})");
+
+	TypedDictionary<int64_t, Variant> int_var_dict;
+	int_var_dict[1] = "2";
+	int_var_dict[3] = "4";
+
+	test(int_var_dict, R"({"type":"Dictionary","key_type":"int","args":["i:1","s:2","i:3","s:4"]})");
+
+	TypedDictionary<Variant, int64_t> var_int_dict;
+	var_int_dict["1"] = 2;
+	var_int_dict["3"] = 4;
+
+	test(var_int_dict, R"({"type":"Dictionary","value_type":"int","args":["s:1","i:2","s:3","i:4"]})");
+
+	Dictionary dict2;
+	dict2["x"] = res;
+
+	const String dict2_repr = vformat(R"({"type":"Dictionary","args":["s:x",%s]})", res_repr);
+
+	test(dict2, dict2_repr, true);
+	ERR_PRINT_OFF;
+	CHECK(encode(dict2) == R"({"type":"Dictionary","args":["s:x",null]})");
+	CHECK(decode(dict2_repr).get_construct_string() == "{\n\"x\": null\n}");
+	ERR_PRINT_ON;
+
+	TypedDictionary<String, Resource> res_dict;
+	res_dict["x"] = res;
+
+	const String res_dict_repr = vformat(R"({"type":"Dictionary","key_type":"String","value_type":"Resource","args":["s:x",%s]})", res_repr);
+
+	test(res_dict, res_dict_repr, true);
+	ERR_PRINT_OFF;
+	CHECK(encode(res_dict) == "null");
+	CHECK(decode(res_dict_repr).get_type() == Variant::NIL);
+	ERR_PRINT_ON;
+
+	// `Array`.
+
+	Array arr;
+	arr.push_back(true);
+	arr.push_back(1);
+	arr.push_back("abc");
+
+	test(arr, R"([true,"i:1","s:abc"])");
+
+	TypedArray<int64_t> int_arr;
+	int_arr.push_back(1);
+	int_arr.push_back(2);
+	int_arr.push_back(3);
+
+	test(int_arr, R"({"type":"Array","elem_type":"int","args":["i:1","i:2","i:3"]})");
+
+	Array arr2;
+	arr2.push_back(1);
+	arr2.push_back(res);
+	arr2.push_back(9);
+
+	const String arr2_repr = vformat(R"(["i:1",%s,"i:9"])", res_repr);
+
+	test(arr2, arr2_repr, true);
+	ERR_PRINT_OFF;
+	CHECK(encode(arr2) == R"(["i:1",null,"i:9"])");
+	CHECK(decode(arr2_repr).get_construct_string() == "[1, null, 9]");
+	ERR_PRINT_ON;
+
+	TypedArray<Resource> res_arr;
+	res_arr.push_back(res);
+
+	const String res_arr_repr = vformat(R"({"type":"Array","elem_type":"Resource","args":[%s]})", res_repr);
+
+	test(res_arr, res_arr_repr, true);
+	ERR_PRINT_OFF;
+	CHECK(encode(res_arr) == "null");
+	CHECK(decode(res_arr_repr).get_type() == Variant::NIL);
+	ERR_PRINT_ON;
+
+	// Packed arrays.
+
+	test(PackedByteArray({ 1, 2, 3 }), R"({"type":"PackedByteArray","args":[1,2,3]})");
+	test(PackedInt32Array({ 1, 2, 3 }), R"({"type":"PackedInt32Array","args":[1,2,3]})");
+	test(PackedInt64Array({ 1, 2, 3 }), R"({"type":"PackedInt64Array","args":[1,2,3]})");
+	test(PackedFloat32Array({ 1, 2, 3 }), R"({"type":"PackedFloat32Array","args":[1.0,2.0,3.0]})");
+	test(PackedFloat64Array({ 1, 2, 3 }), R"({"type":"PackedFloat64Array","args":[1.0,2.0,3.0]})");
+	test(PackedStringArray({ "a", "b", "c" }), R"({"type":"PackedStringArray","args":["a","b","c"]})");
+
+	const PackedVector2Array pv2arr({ Vector2(1, 2), Vector2(3, 4), Vector2(5, 6) });
+	test(pv2arr, R"({"type":"PackedVector2Array","args":[1.0,2.0,3.0,4.0,5.0,6.0]})");
+
+	const PackedVector3Array pv3arr({ Vector3(1, 2, 3), Vector3(4, 5, 6), Vector3(7, 8, 9) });
+	test(pv3arr, R"({"type":"PackedVector3Array","args":[1.0,2.0,3.0,4.0,5.0,6.0,7.0,8.0,9.0]})");
+
+	const PackedColorArray pcolarr({ Color(1, 2, 3, 4), Color(5, 6, 7, 8), Color(9, 10, 11, 12) });
+	test(pcolarr, R"({"type":"PackedColorArray","args":[1.0,2.0,3.0,4.0,5.0,6.0,7.0,8.0,9.0,10.0,11.0,12.0]})");
+
+	const PackedVector4Array pv4arr({ Vector4(1, 2, 3, 4), Vector4(5, 6, 7, 8), Vector4(9, 10, 11, 12) });
+	test(pv4arr, R"({"type":"PackedVector4Array","args":[1.0,2.0,3.0,4.0,5.0,6.0,7.0,8.0,9.0,10.0,11.0,12.0]})");
+}
+
 } // namespace TestJSONNative
 
 #endif // TEST_JSON_NATIVE_H

From f367a6c231da9b754615417953bc3d62eaa7708b Mon Sep 17 00:00:00 2001
From: Naoto Kondo <cgigcp3yqt@gmail.com>
Date: Mon, 25 Nov 2024 00:23:59 +0900
Subject: [PATCH 019/124] Fix underscore input not working with jp106 keyboard
 on wayland platform

---
 platform/linuxbsd/wayland/wayland_thread.cpp | 11 +++++------
 1 file changed, 5 insertions(+), 6 deletions(-)

diff --git a/platform/linuxbsd/wayland/wayland_thread.cpp b/platform/linuxbsd/wayland/wayland_thread.cpp
index d8d58ba54b..c6a288b93f 100644
--- a/platform/linuxbsd/wayland/wayland_thread.cpp
+++ b/platform/linuxbsd/wayland/wayland_thread.cpp
@@ -193,10 +193,7 @@ bool WaylandThread::_seat_state_configure_key_event(SeatState &p_ss, Ref<InputEv
 	Key keycode = KeyMappingXKB::get_keycode(xkb_state_key_get_one_sym(p_ss.xkb_state, p_keycode));
 	Key physical_keycode = KeyMappingXKB::get_scancode(p_keycode);
 	KeyLocation key_location = KeyMappingXKB::get_location(p_keycode);
-
-	if (physical_keycode == Key::NONE) {
-		return false;
-	}
+	uint32_t unicode = xkb_state_key_get_utf32(p_ss.xkb_state, p_keycode);
 
 	if (keycode == Key::NONE) {
 		keycode = physical_keycode;
@@ -206,6 +203,10 @@ bool WaylandThread::_seat_state_configure_key_event(SeatState &p_ss, Ref<InputEv
 		keycode -= 'a' - 'A';
 	}
 
+	if (physical_keycode == Key::NONE && keycode == Key::NONE && unicode == 0) {
+		return false;
+	}
+
 	p_event->set_window_id(DisplayServer::MAIN_WINDOW_ID);
 
 	// Set all pressed modifiers.
@@ -219,8 +220,6 @@ bool WaylandThread::_seat_state_configure_key_event(SeatState &p_ss, Ref<InputEv
 	p_event->set_physical_keycode(physical_keycode);
 	p_event->set_location(key_location);
 
-	uint32_t unicode = xkb_state_key_get_utf32(p_ss.xkb_state, p_keycode);
-
 	if (unicode != 0) {
 		p_event->set_key_label(fix_key_label(unicode, keycode));
 	} else {

From d90f045d2482725e605b2d4802eda101f8c8281b Mon Sep 17 00:00:00 2001
From: Micky <micheledevita2@gmail.com>
Date: Mon, 11 Nov 2024 18:21:40 +0100
Subject: [PATCH 020/124] Update documentation's "Prints" comments after #47502

---
 doc/classes/AABB.xml        | 36 ++++++++++++++++++------------------
 doc/classes/AStarGrid2D.xml |  4 ++--
 doc/classes/Basis.xml       | 24 ++++++++++++------------
 doc/classes/Callable.xml    |  2 +-
 doc/classes/Geometry2D.xml  |  4 ++--
 doc/classes/Object.xml      |  8 ++++----
 doc/classes/Transform2D.xml |  2 +-
 doc/classes/Vector2.xml     | 14 +++++++-------
 doc/classes/Vector2i.xml    | 16 ++++++++--------
 doc/classes/Vector3.xml     |  8 ++++----
 doc/classes/Vector3i.xml    | 16 ++++++++--------
 doc/classes/Vector4.xml     | 12 ++++++------
 doc/classes/Vector4i.xml    | 16 ++++++++--------
 doc/classes/float.xml       |  6 +++---
 14 files changed, 84 insertions(+), 84 deletions(-)

diff --git a/doc/classes/AABB.xml b/doc/classes/AABB.xml
index 57ac241eb2..cb43c196cd 100644
--- a/doc/classes/AABB.xml
+++ b/doc/classes/AABB.xml
@@ -46,8 +46,8 @@
 				[gdscript]
 				var box = AABB(Vector3(5, 0, 5), Vector3(-20, -10, -5))
 				var absolute = box.abs()
-				print(absolute.position) # Prints (-15, -10, 0)
-				print(absolute.size)     # Prints (20, 10, 5)
+				print(absolute.position) # Prints (-15.0, -10.0, 0.0)
+				print(absolute.size)     # Prints (20.0, 10.0, 5.0)
 				[/gdscript]
 				[csharp]
 				var box = new Aabb(new Vector3(5, 0, 5), new Vector3(-20, -10, -5));
@@ -96,12 +96,12 @@
 				var box = AABB(Vector3(0, 0, 0), Vector3(5, 2, 5))
 
 				box = box.expand(Vector3(10, 0, 0))
-				print(box.position) # Prints (0, 0, 0)
-				print(box.size)     # Prints (10, 2, 5)
+				print(box.position) # Prints (0.0, 0.0, 0.0)
+				print(box.size)     # Prints (10.0, 2.0, 5.0)
 
 				box = box.expand(Vector3(-5, 0, 5))
-				print(box.position) # Prints (-5, 0, 0)
-				print(box.size)     # Prints (15, 2, 5)
+				print(box.position) # Prints (-5.0, 0.0, 0.0)
+				print(box.size)     # Prints (15.0, 2.0, 5.0)
 				[/gdscript]
 				[csharp]
 				var box = new Aabb(new Vector3(0, 0, 0), new Vector3(5, 2, 5));
@@ -138,15 +138,15 @@
 				[gdscript]
 				var box = AABB(Vector3(0, 0, 0), Vector3(2, 4, 8))
 
-				print(box.get_longest_axis())       # Prints (0, 0, 1)
+				print(box.get_longest_axis())       # Prints (0.0, 0.0, 1.0)
 				print(box.get_longest_axis_index()) # Prints 2
-				print(box.get_longest_axis_size())  # Prints 8
+				print(box.get_longest_axis_size())  # Prints 8.0
 				[/gdscript]
 				[csharp]
 				var box = new Aabb(new Vector3(0, 0, 0), new Vector3(2, 4, 8));
 
 				GD.Print(box.GetLongestAxis());      // Prints (0, 0, 1)
-				GD.Print(box.GetLongestAxisIndex()); // Prints 2
+				GD.Print(box.GetLongestAxisIndex()); // Prints Z
 				GD.Print(box.GetLongestAxisSize());  // Prints 8
 				[/csharp]
 				[/codeblocks]
@@ -175,15 +175,15 @@
 				[gdscript]
 				var box = AABB(Vector3(0, 0, 0), Vector3(2, 4, 8))
 
-				print(box.get_shortest_axis())       # Prints (1, 0, 0)
+				print(box.get_shortest_axis())       # Prints (1.0, 0.0, 0.0)
 				print(box.get_shortest_axis_index()) # Prints 0
-				print(box.get_shortest_axis_size())  # Prints 2
+				print(box.get_shortest_axis_size())  # Prints 2.0
 				[/gdscript]
 				[csharp]
 				var box = new Aabb(new Vector3(0, 0, 0), new Vector3(2, 4, 8));
 
 				GD.Print(box.GetShortestAxis());      // Prints (1, 0, 0)
-				GD.Print(box.GetShortestAxisIndex()); // Prints 0
+				GD.Print(box.GetShortestAxisIndex()); // Prints X
 				GD.Print(box.GetShortestAxisSize());  // Prints 2
 				[/csharp]
 				[/codeblocks]
@@ -225,12 +225,12 @@
 				[codeblocks]
 				[gdscript]
 				var a = AABB(Vector3(4, 4, 4), Vector3(8, 8, 8)).grow(4)
-				print(a.position) # Prints (0, 0, 0)
-				print(a.size)     # Prints (16, 16, 16)
+				print(a.position) # Prints (0.0, 0.0, 0.0)
+				print(a.size)     # Prints (16.0, 16.0, 16.0)
 
 				var b = AABB(Vector3(0, 0, 0), Vector3(8, 4, 2)).grow(2)
-				print(b.position) # Prints (-2, -2, -2)
-				print(b.size)     # Prints (12, 8, 6)
+				print(b.position) # Prints (-2.0, -2.0, -2.0)
+				print(b.size)     # Prints (12.0, 8.0, 6.0)
 				[/gdscript]
 				[csharp]
 				var a = new Aabb(new Vector3(4, 4, 4), new Vector3(8, 8, 8)).Grow(4);
@@ -275,8 +275,8 @@
 				var box2 = AABB(Vector3(2, 0, 2), Vector3(8, 4, 4))
 
 				var intersection = box1.intersection(box2)
-				print(intersection.position) # Prints (2, 0, 2)
-				print(intersection.size)     # Prints (3, 2, 4)
+				print(intersection.position) # Prints (2.0, 0.0, 2.0)
+				print(intersection.size)     # Prints (3.0, 2.0, 4.0)
 				[/gdscript]
 				[csharp]
 				var box1 = new Aabb(new Vector3(0, 0, 0), new Vector3(5, 2, 8));
diff --git a/doc/classes/AStarGrid2D.xml b/doc/classes/AStarGrid2D.xml
index 8e1972af11..ac37a18a42 100644
--- a/doc/classes/AStarGrid2D.xml
+++ b/doc/classes/AStarGrid2D.xml
@@ -12,8 +12,8 @@
 		astar_grid.region = Rect2i(0, 0, 32, 32)
 		astar_grid.cell_size = Vector2(16, 16)
 		astar_grid.update()
-		print(astar_grid.get_id_path(Vector2i(0, 0), Vector2i(3, 4))) # prints (0, 0), (1, 1), (2, 2), (3, 3), (3, 4)
-		print(astar_grid.get_point_path(Vector2i(0, 0), Vector2i(3, 4))) # prints (0, 0), (16, 16), (32, 32), (48, 48), (48, 64)
+		print(astar_grid.get_id_path(Vector2i(0, 0), Vector2i(3, 4))) # Prints [(0, 0), (1, 1), (2, 2), (3, 3), (3, 4)]
+		print(astar_grid.get_point_path(Vector2i(0, 0), Vector2i(3, 4))) # Prints [(0, 0), (16, 16), (32, 32), (48, 48), (48, 64)]
 		[/gdscript]
 		[csharp]
 		AStarGrid2D astarGrid = new AStarGrid2D();
diff --git a/doc/classes/Basis.xml b/doc/classes/Basis.xml
index b8823cf188..81f63addd4 100644
--- a/doc/classes/Basis.xml
+++ b/doc/classes/Basis.xml
@@ -91,7 +91,7 @@
 				# Creates a Basis whose z axis points down.
 				var my_basis = Basis.from_euler(Vector3(TAU / 4, 0, 0))
 
-				print(my_basis.z) # Prints (0, -1, 0)
+				print(my_basis.z) # Prints (0.0, -1.0, 0.0)
 				[/gdscript]
 				[csharp]
 				// Creates a Basis whose z axis points down.
@@ -112,9 +112,9 @@
 				[gdscript]
 				var my_basis = Basis.from_scale(Vector3(2, 4, 8))
 
-				print(my_basis.x) # Prints (2, 0, 0)
-				print(my_basis.y) # Prints (0, 4, 0)
-				print(my_basis.z) # Prints (0, 0, 8)
+				print(my_basis.x) # Prints (2.0, 0.0, 0.0)
+				print(my_basis.y) # Prints (0.0, 4.0, 0.0)
+				print(my_basis.z) # Prints (0.0, 0.0, 8.0)
 				[/gdscript]
 				[csharp]
 				var myBasis = Basis.FromScale(new Vector3(2.0f, 4.0f, 8.0f));
@@ -163,7 +163,7 @@
 				my_basis = my_basis.rotated(Vector3.UP, TAU / 2)
 				my_basis = my_basis.rotated(Vector3.RIGHT, TAU / 4)
 
-				print(my_basis.get_scale()) # Prints (2, 4, 8)
+				print(my_basis.get_scale()) # Prints (2.0, 4.0, 8.0)
 				[/gdscript]
 				[csharp]
 				var myBasis = new Basis(
@@ -285,9 +285,9 @@
 				)
 				my_basis = my_basis.scaled(Vector3(0, 2, -2))
 
-				print(my_basis.x) # Prints (0, 2, -2)
-				print(my_basis.y) # Prints (0, 4, -4)
-				print(my_basis.z) # Prints (0, 6, -6)
+				print(my_basis.x) # Prints (0.0, 2.0, -2.0)
+				print(my_basis.y) # Prints (0.0, 4.0, -4.0)
+				print(my_basis.z) # Prints (0.0, 6.0, -6.0)
 				[/gdscript]
 				[csharp]
 				var myBasis = new Basis(
@@ -360,9 +360,9 @@
 				)
 				my_basis = my_basis.transposed()
 
-				print(my_basis.x) # Prints (1, 4, 7)
-				print(my_basis.y) # Prints (2, 5, 8)
-				print(my_basis.z) # Prints (3, 6, 9)
+				print(my_basis.x) # Prints (1.0, 4.0, 7.0)
+				print(my_basis.y) # Prints (2.0, 5.0, 8.0)
+				print(my_basis.z) # Prints (3.0, 6.0, 9.0)
 				[/gdscript]
 				[csharp]
 				var myBasis = new Basis(
@@ -454,7 +454,7 @@
 				[gdscript]
 				# Basis that swaps the X/Z axes and doubles the scale.
 				var my_basis = Basis(Vector3(0, 2, 0), Vector3(2, 0, 0), Vector3(0, 0, 2))
-				print(my_basis * Vector3(1, 2, 3)) # Prints (4, 2, 6)
+				print(my_basis * Vector3(1, 2, 3)) # Prints (4.0, 2.0, 6.0)
 				[/gdscript]
 				[csharp]
 				// Basis that swaps the X/Z axes and doubles the scale.
diff --git a/doc/classes/Callable.xml b/doc/classes/Callable.xml
index cf3c3e06fd..d5978a42f6 100644
--- a/doc/classes/Callable.xml
+++ b/doc/classes/Callable.xml
@@ -13,7 +13,7 @@
 		func test():
 		    var callable = Callable(self, "print_args")
 		    callable.call("hello", "world")  # Prints "hello world ".
-		    callable.call(Vector2.UP, 42, callable)  # Prints "(0, -1) 42 Node(node.gd)::print_args".
+		    callable.call(Vector2.UP, 42, callable)  # Prints "(0.0, -1.0) 42 Node(node.gd)::print_args".
 		    callable.call("invalid")  # Invalid call, should have at least 2 arguments.
 		[/gdscript]
 		[csharp]
diff --git a/doc/classes/Geometry2D.xml b/doc/classes/Geometry2D.xml
index 71e6cf93ae..c86861fd13 100644
--- a/doc/classes/Geometry2D.xml
+++ b/doc/classes/Geometry2D.xml
@@ -201,13 +201,13 @@
 				var polygon = PackedVector2Array([Vector2(0, 0), Vector2(100, 0), Vector2(100, 100), Vector2(0, 100)])
 				var offset = Vector2(50, 50)
 				polygon = Transform2D(0, offset) * polygon
-				print(polygon) # prints [(50, 50), (150, 50), (150, 150), (50, 150)]
+				print(polygon) # Prints [(50.0, 50.0), (150.0, 50.0), (150.0, 150.0), (50.0, 150.0)]
 				[/gdscript]
 				[csharp]
 				var polygon = new Vector2[] { new Vector2(0, 0), new Vector2(100, 0), new Vector2(100, 100), new Vector2(0, 100) };
 				var offset = new Vector2(50, 50);
 				polygon = new Transform2D(0, offset) * polygon;
-				GD.Print((Variant)polygon); // prints [(50, 50), (150, 50), (150, 150), (50, 150)]
+				GD.Print((Variant)polygon); // Prints [(50, 50), (150, 50), (150, 150), (50, 150)]
 				[/csharp]
 				[/codeblocks]
 			</description>
diff --git a/doc/classes/Object.xml b/doc/classes/Object.xml
index 73fd7e1943..febc81dd81 100644
--- a/doc/classes/Object.xml
+++ b/doc/classes/Object.xml
@@ -987,12 +987,12 @@
 				[gdscript]
 				var node = Node2D.new()
 				node.set("global_scale", Vector2(8, 2.5))
-				print(node.global_scale) # Prints (8, 2.5)
+				print(node.global_scale) # Prints (8.0, 2.5)
 				[/gdscript]
 				[csharp]
 				var node = new Node2D();
-				node.Set(Node2D.PropertyName.GlobalScale, new Vector2(8, 2.5));
-				GD.Print(node.GlobalScale); // Prints Vector2(8, 2.5)
+				node.Set(Node2D.PropertyName.GlobalScale, new Vector2(8, 2.5f));
+				GD.Print(node.GlobalScale); // Prints (8, 2.5)
 				[/csharp]
 				[/codeblocks]
 				[b]Note:[/b] In C#, [param property] must be in snake_case when referring to built-in Godot properties. Prefer using the names exposed in the [code]PropertyName[/code] class to avoid allocating a new [StringName] on each call.
@@ -1047,7 +1047,7 @@
 				var node = Node2D.new()
 				node.set_indexed("position", Vector2(42, 0))
 				node.set_indexed("position:y", -10)
-				print(node.position) # Prints (42, -10)
+				print(node.position) # Prints (42.0, -10.0)
 				[/gdscript]
 				[csharp]
 				var node = new Node2D();
diff --git a/doc/classes/Transform2D.xml b/doc/classes/Transform2D.xml
index bdc908d387..c55b5f1c90 100644
--- a/doc/classes/Transform2D.xml
+++ b/doc/classes/Transform2D.xml
@@ -116,7 +116,7 @@
 				# Rotating the Transform2D in any way preserves its scale.
 				my_transform = my_transform.rotated(TAU / 2)
 
-				print(my_transform.get_scale()) # Prints (2, 4)
+				print(my_transform.get_scale()) # Prints (2.0, 4.0)
 				[/gdscript]
 				[csharp]
 				var myTransform = new Transform2D(
diff --git a/doc/classes/Vector2.xml b/doc/classes/Vector2.xml
index c03262bb33..630dfe67e8 100644
--- a/doc/classes/Vector2.xml
+++ b/doc/classes/Vector2.xml
@@ -213,9 +213,9 @@
 			<description>
 				Creates a unit [Vector2] rotated to the given [param angle] in radians. This is equivalent to doing [code]Vector2(cos(angle), sin(angle))[/code] or [code]Vector2.RIGHT.rotated(angle)[/code].
 				[codeblock]
-				print(Vector2.from_angle(0)) # Prints (1, 0).
-				print(Vector2(1, 0).angle()) # Prints 0, which is the angle used above.
-				print(Vector2.from_angle(PI / 2)) # Prints (0, 1).
+				print(Vector2.from_angle(0)) # Prints (1.0, 0.0).
+				print(Vector2(1, 0).angle()) # Prints 0.0, which is the angle used above.
+				print(Vector2.from_angle(PI / 2)) # Prints (0.0, 1.0).
 				[/codeblock]
 			</description>
 		</method>
@@ -477,7 +477,7 @@
 			<description>
 				Multiplies each component of the [Vector2] by the components of the given [Vector2].
 				[codeblock]
-				print(Vector2(10, 20) * Vector2(3, 4)) # Prints "(30, 80)"
+				print(Vector2(10, 20) * Vector2(3, 4)) # Prints (30.0, 80.0)
 				[/codeblock]
 			</description>
 		</operator>
@@ -501,7 +501,7 @@
 			<description>
 				Adds each component of the [Vector2] by the components of the given [Vector2].
 				[codeblock]
-				print(Vector2(10, 20) + Vector2(3, 4)) # Prints "(13, 24)"
+				print(Vector2(10, 20) + Vector2(3, 4)) # Prints (13.0, 24.0)
 				[/codeblock]
 			</description>
 		</operator>
@@ -511,7 +511,7 @@
 			<description>
 				Subtracts each component of the [Vector2] by the components of the given [Vector2].
 				[codeblock]
-				print(Vector2(10, 20) - Vector2(3, 4)) # Prints "(7, 16)"
+				print(Vector2(10, 20) - Vector2(3, 4)) # Prints (7.0, 16.0)
 				[/codeblock]
 			</description>
 		</operator>
@@ -521,7 +521,7 @@
 			<description>
 				Divides each component of the [Vector2] by the components of the given [Vector2].
 				[codeblock]
-				print(Vector2(10, 20) / Vector2(2, 5)) # Prints "(5, 4)"
+				print(Vector2(10, 20) / Vector2(2, 5)) # Prints (5.0, 4.0)
 				[/codeblock]
 			</description>
 		</operator>
diff --git a/doc/classes/Vector2i.xml b/doc/classes/Vector2i.xml
index 53c7c92ca3..74dfc3efe9 100644
--- a/doc/classes/Vector2i.xml
+++ b/doc/classes/Vector2i.xml
@@ -215,7 +215,7 @@
 			<description>
 				Gets the remainder of each component of the [Vector2i] with the components of the given [Vector2i]. This operation uses truncated division, which is often not desired as it does not work well with negative numbers. Consider using [method @GlobalScope.posmod] instead if you want to handle negative numbers.
 				[codeblock]
-				print(Vector2i(10, -20) % Vector2i(7, 8)) # Prints "(3, -4)"
+				print(Vector2i(10, -20) % Vector2i(7, 8)) # Prints (3, -4)
 				[/codeblock]
 			</description>
 		</operator>
@@ -225,7 +225,7 @@
 			<description>
 				Gets the remainder of each component of the [Vector2i] with the given [int]. This operation uses truncated division, which is often not desired as it does not work well with negative numbers. Consider using [method @GlobalScope.posmod] instead if you want to handle negative numbers.
 				[codeblock]
-				print(Vector2i(10, -20) % 7) # Prints "(3, -6)"
+				print(Vector2i(10, -20) % 7) # Prints (3, -6)
 				[/codeblock]
 			</description>
 		</operator>
@@ -235,7 +235,7 @@
 			<description>
 				Multiplies each component of the [Vector2i] by the components of the given [Vector2i].
 				[codeblock]
-				print(Vector2i(10, 20) * Vector2i(3, 4)) # Prints "(30, 80)"
+				print(Vector2i(10, 20) * Vector2i(3, 4)) # Prints (30, 80)
 				[/codeblock]
 			</description>
 		</operator>
@@ -245,7 +245,7 @@
 			<description>
 				Multiplies each component of the [Vector2i] by the given [float]. Returns a [Vector2].
 				[codeblock]
-				print(Vector2i(10, 15) * 0.9) # Prints "(9, 13.5)"
+				print(Vector2i(10, 15) * 0.9) # Prints (9.0, 13.5)
 				[/codeblock]
 			</description>
 		</operator>
@@ -262,7 +262,7 @@
 			<description>
 				Adds each component of the [Vector2i] by the components of the given [Vector2i].
 				[codeblock]
-				print(Vector2i(10, 20) + Vector2i(3, 4)) # Prints "(13, 24)"
+				print(Vector2i(10, 20) + Vector2i(3, 4)) # Prints (13, 24)
 				[/codeblock]
 			</description>
 		</operator>
@@ -272,7 +272,7 @@
 			<description>
 				Subtracts each component of the [Vector2i] by the components of the given [Vector2i].
 				[codeblock]
-				print(Vector2i(10, 20) - Vector2i(3, 4)) # Prints "(7, 16)"
+				print(Vector2i(10, 20) - Vector2i(3, 4)) # Prints (7, 16)
 				[/codeblock]
 			</description>
 		</operator>
@@ -282,7 +282,7 @@
 			<description>
 				Divides each component of the [Vector2i] by the components of the given [Vector2i].
 				[codeblock]
-				print(Vector2i(10, 20) / Vector2i(2, 5)) # Prints "(5, 4)"
+				print(Vector2i(10, 20) / Vector2i(2, 5)) # Prints (5, 4)
 				[/codeblock]
 			</description>
 		</operator>
@@ -292,7 +292,7 @@
 			<description>
 				Divides each component of the [Vector2i] by the given [float]. Returns a [Vector2].
 				[codeblock]
-				print(Vector2i(10, 20) / 2.9) # Prints "(5, 10)"
+				print(Vector2i(10, 20) / 2.9) # Prints (5.0, 10.0)
 				[/codeblock]
 			</description>
 		</operator>
diff --git a/doc/classes/Vector3.xml b/doc/classes/Vector3.xml
index 4ab3140eb6..63ad143c14 100644
--- a/doc/classes/Vector3.xml
+++ b/doc/classes/Vector3.xml
@@ -518,7 +518,7 @@
 			<description>
 				Multiplies each component of the [Vector3] by the components of the given [Vector3].
 				[codeblock]
-				print(Vector3(10, 20, 30) * Vector3(3, 4, 5)) # Prints "(30, 80, 150)"
+				print(Vector3(10, 20, 30) * Vector3(3, 4, 5)) # Prints (30.0, 80.0, 150.0)
 				[/codeblock]
 			</description>
 		</operator>
@@ -542,7 +542,7 @@
 			<description>
 				Adds each component of the [Vector3] by the components of the given [Vector3].
 				[codeblock]
-				print(Vector3(10, 20, 30) + Vector3(3, 4, 5)) # Prints "(13, 24, 35)"
+				print(Vector3(10, 20, 30) + Vector3(3, 4, 5)) # Prints (13.0, 24.0, 35.0)
 				[/codeblock]
 			</description>
 		</operator>
@@ -552,7 +552,7 @@
 			<description>
 				Subtracts each component of the [Vector3] by the components of the given [Vector3].
 				[codeblock]
-				print(Vector3(10, 20, 30) - Vector3(3, 4, 5)) # Prints "(7, 16, 25)"
+				print(Vector3(10, 20, 30) - Vector3(3, 4, 5)) # Prints (7.0, 16.0, 25.0)
 				[/codeblock]
 			</description>
 		</operator>
@@ -562,7 +562,7 @@
 			<description>
 				Divides each component of the [Vector3] by the components of the given [Vector3].
 				[codeblock]
-				print(Vector3(10, 20, 30) / Vector3(2, 5, 3)) # Prints "(5, 4, 10)"
+				print(Vector3(10, 20, 30) / Vector3(2, 5, 3)) # Prints (5.0, 4.0, 10.0)
 				[/codeblock]
 			</description>
 		</operator>
diff --git a/doc/classes/Vector3i.xml b/doc/classes/Vector3i.xml
index 7fe469aec0..64c2137b7d 100644
--- a/doc/classes/Vector3i.xml
+++ b/doc/classes/Vector3i.xml
@@ -222,7 +222,7 @@
 			<description>
 				Gets the remainder of each component of the [Vector3i] with the components of the given [Vector3i]. This operation uses truncated division, which is often not desired as it does not work well with negative numbers. Consider using [method @GlobalScope.posmod] instead if you want to handle negative numbers.
 				[codeblock]
-				print(Vector3i(10, -20, 30) % Vector3i(7, 8, 9)) # Prints "(3, -4, 3)"
+				print(Vector3i(10, -20, 30) % Vector3i(7, 8, 9)) # Prints (3, -4, 3)
 				[/codeblock]
 			</description>
 		</operator>
@@ -232,7 +232,7 @@
 			<description>
 				Gets the remainder of each component of the [Vector3i] with the given [int]. This operation uses truncated division, which is often not desired as it does not work well with negative numbers. Consider using [method @GlobalScope.posmod] instead if you want to handle negative numbers.
 				[codeblock]
-				print(Vector3i(10, -20, 30) % 7) # Prints "(3, -6, 2)"
+				print(Vector3i(10, -20, 30) % 7) # Prints (3, -6, 2)
 				[/codeblock]
 			</description>
 		</operator>
@@ -242,7 +242,7 @@
 			<description>
 				Multiplies each component of the [Vector3i] by the components of the given [Vector3i].
 				[codeblock]
-				print(Vector3i(10, 20, 30) * Vector3i(3, 4, 5)) # Prints "(30, 80, 150)"
+				print(Vector3i(10, 20, 30) * Vector3i(3, 4, 5)) # Prints (30, 80, 150)
 				[/codeblock]
 			</description>
 		</operator>
@@ -252,7 +252,7 @@
 			<description>
 				Multiplies each component of the [Vector3i] by the given [float]. Returns a [Vector3].
 				[codeblock]
-				print(Vector3i(10, 15, 20) * 0.9) # Prints "(9, 13.5, 18)"
+				print(Vector3i(10, 15, 20) * 0.9) # Prints (9.0, 13.5, 18.0)
 				[/codeblock]
 			</description>
 		</operator>
@@ -269,7 +269,7 @@
 			<description>
 				Adds each component of the [Vector3i] by the components of the given [Vector3i].
 				[codeblock]
-				print(Vector3i(10, 20, 30) + Vector3i(3, 4, 5)) # Prints "(13, 24, 35)"
+				print(Vector3i(10, 20, 30) + Vector3i(3, 4, 5)) # Prints (13, 24, 35)
 				[/codeblock]
 			</description>
 		</operator>
@@ -279,7 +279,7 @@
 			<description>
 				Subtracts each component of the [Vector3i] by the components of the given [Vector3i].
 				[codeblock]
-				print(Vector3i(10, 20, 30) - Vector3i(3, 4, 5)) # Prints "(7, 16, 25)"
+				print(Vector3i(10, 20, 30) - Vector3i(3, 4, 5)) # Prints (7, 16, 25)
 				[/codeblock]
 			</description>
 		</operator>
@@ -289,7 +289,7 @@
 			<description>
 				Divides each component of the [Vector3i] by the components of the given [Vector3i].
 				[codeblock]
-				print(Vector3i(10, 20, 30) / Vector3i(2, 5, 3)) # Prints "(5, 4, 10)"
+				print(Vector3i(10, 20, 30) / Vector3i(2, 5, 3)) # Prints (5, 4, 10)
 				[/codeblock]
 			</description>
 		</operator>
@@ -299,7 +299,7 @@
 			<description>
 				Divides each component of the [Vector3i] by the given [float]. Returns a [Vector3].
 				[codeblock]
-				print(Vector3i(10, 20, 30) / 2.9) # Prints "(5, 10, 15)"
+				print(Vector3i(10, 20, 30) / 2.9) # Prints (5.0, 10.0, 15.0)
 				[/codeblock]
 			</description>
 		</operator>
diff --git a/doc/classes/Vector4.xml b/doc/classes/Vector4.xml
index 8fa17b57e6..45780a1aed 100644
--- a/doc/classes/Vector4.xml
+++ b/doc/classes/Vector4.xml
@@ -333,7 +333,7 @@
 			<description>
 				Multiplies each component of the [Vector4] by the components of the given [Vector4].
 				[codeblock]
-				print(Vector4(10, 20, 30, 40) * Vector4(3, 4, 5, 6)) # Prints "(30, 80, 150, 240)"
+				print(Vector4(10, 20, 30, 40) * Vector4(3, 4, 5, 6)) # Prints (30.0, 80.0, 150.0, 240.0)
 				[/codeblock]
 			</description>
 		</operator>
@@ -343,7 +343,7 @@
 			<description>
 				Multiplies each component of the [Vector4] by the given [float].
 				[codeblock]
-				print(Vector4(10, 20, 30, 40) * 2) # Prints "(20, 40, 60, 80)"
+				print(Vector4(10, 20, 30, 40) * 2) # Prints (20.0, 40.0, 60.0, 80.0)
 				[/codeblock]
 			</description>
 		</operator>
@@ -360,7 +360,7 @@
 			<description>
 				Adds each component of the [Vector4] by the components of the given [Vector4].
 				[codeblock]
-				print(Vector4(10, 20, 30, 40) + Vector4(3, 4, 5, 6)) # Prints "(13, 24, 35, 46)"
+				print(Vector4(10, 20, 30, 40) + Vector4(3, 4, 5, 6)) # Prints (13.0, 24.0, 35.0, 46.0)
 				[/codeblock]
 			</description>
 		</operator>
@@ -370,7 +370,7 @@
 			<description>
 				Subtracts each component of the [Vector4] by the components of the given [Vector4].
 				[codeblock]
-				print(Vector4(10, 20, 30, 40) - Vector4(3, 4, 5, 6)) # Prints "(7, 16, 25, 34)"
+				print(Vector4(10, 20, 30, 40) - Vector4(3, 4, 5, 6)) # Prints (7.0, 16.0, 25.0, 34.0)
 				[/codeblock]
 			</description>
 		</operator>
@@ -380,7 +380,7 @@
 			<description>
 				Divides each component of the [Vector4] by the components of the given [Vector4].
 				[codeblock]
-				print(Vector4(10, 20, 30, 40) / Vector4(2, 5, 3, 4)) # Prints "(5, 4, 10, 10)"
+				print(Vector4(10, 20, 30, 40) / Vector4(2, 5, 3, 4)) # Prints (5.0, 4.0, 10.0, 10.0)
 				[/codeblock]
 			</description>
 		</operator>
@@ -390,7 +390,7 @@
 			<description>
 				Divides each component of the [Vector4] by the given [float].
 				[codeblock]
-				print(Vector4(10, 20, 30, 40) / 2 # Prints "(5, 10, 15, 20)"
+				print(Vector4(10, 20, 30, 40) / 2 # Prints (5.0, 10.0, 15.0, 20.0)
 				[/codeblock]
 			</description>
 		</operator>
diff --git a/doc/classes/Vector4i.xml b/doc/classes/Vector4i.xml
index e1d65eb1b5..6c46f82a3b 100644
--- a/doc/classes/Vector4i.xml
+++ b/doc/classes/Vector4i.xml
@@ -208,7 +208,7 @@
 			<description>
 				Gets the remainder of each component of the [Vector4i] with the components of the given [Vector4i]. This operation uses truncated division, which is often not desired as it does not work well with negative numbers. Consider using [method @GlobalScope.posmod] instead if you want to handle negative numbers.
 				[codeblock]
-				print(Vector4i(10, -20, 30, -40) % Vector4i(7, 8, 9, 10))  # Prints "(3, -4, 3, 0)"
+				print(Vector4i(10, -20, 30, -40) % Vector4i(7, 8, 9, 10))  # Prints (3, -4, 3, 0)
 				[/codeblock]
 			</description>
 		</operator>
@@ -218,7 +218,7 @@
 			<description>
 				Gets the remainder of each component of the [Vector4i] with the given [int]. This operation uses truncated division, which is often not desired as it does not work well with negative numbers. Consider using [method @GlobalScope.posmod] instead if you want to handle negative numbers.
 				[codeblock]
-				print(Vector4i(10, -20, 30, -40) % 7)  # Prints "(3, -6, 2, -5)"
+				print(Vector4i(10, -20, 30, -40) % 7)  # Prints (3, -6, 2, -5)
 				[/codeblock]
 			</description>
 		</operator>
@@ -228,7 +228,7 @@
 			<description>
 				Multiplies each component of the [Vector4i] by the components of the given [Vector4i].
 				[codeblock]
-				print(Vector4i(10, 20, 30, 40) * Vector4i(3, 4, 5, 6)) # Prints "(30, 80, 150, 240)"
+				print(Vector4i(10, 20, 30, 40) * Vector4i(3, 4, 5, 6)) # Prints (30, 80, 150, 240)
 				[/codeblock]
 			</description>
 		</operator>
@@ -239,7 +239,7 @@
 				Multiplies each component of the [Vector4i] by the given [float].
 				Returns a Vector4 value due to floating-point operations.
 				[codeblock]
-				print(Vector4i(10, 20, 30, 40) * 2) # Prints "(20, 40, 60, 80)"
+				print(Vector4i(10, 20, 30, 40) * 2) # Prints (20.0, 40.0, 60.0, 80.0)
 				[/codeblock]
 			</description>
 		</operator>
@@ -256,7 +256,7 @@
 			<description>
 				Adds each component of the [Vector4i] by the components of the given [Vector4i].
 				[codeblock]
-				print(Vector4i(10, 20, 30, 40) + Vector4i(3, 4, 5, 6)) # Prints "(13, 24, 35, 46)"
+				print(Vector4i(10, 20, 30, 40) + Vector4i(3, 4, 5, 6)) # Prints (13, 24, 35, 46)
 				[/codeblock]
 			</description>
 		</operator>
@@ -266,7 +266,7 @@
 			<description>
 				Subtracts each component of the [Vector4i] by the components of the given [Vector4i].
 				[codeblock]
-				print(Vector4i(10, 20, 30, 40) - Vector4i(3, 4, 5, 6)) # Prints "(7, 16, 25, 34)"
+				print(Vector4i(10, 20, 30, 40) - Vector4i(3, 4, 5, 6)) # Prints (7, 16, 25, 34)
 				[/codeblock]
 			</description>
 		</operator>
@@ -276,7 +276,7 @@
 			<description>
 				Divides each component of the [Vector4i] by the components of the given [Vector4i].
 				[codeblock]
-				print(Vector4i(10, 20, 30, 40) / Vector4i(2, 5, 3, 4)) # Prints "(5, 4, 10, 10)"
+				print(Vector4i(10, 20, 30, 40) / Vector4i(2, 5, 3, 4)) # Prints (5, 4, 10, 10)
 				[/codeblock]
 			</description>
 		</operator>
@@ -287,7 +287,7 @@
 				Divides each component of the [Vector4i] by the given [float].
 				Returns a Vector4 value due to floating-point operations.
 				[codeblock]
-				print(Vector4i(10, 20, 30, 40) / 2 # Prints "(5, 10, 15, 20)"
+				print(Vector4i(10, 20, 30, 40) / 2 # Prints (5.0, 10.0, 15.0, 20.0)
 				[/codeblock]
 			</description>
 		</operator>
diff --git a/doc/classes/float.xml b/doc/classes/float.xml
index 56f78bdc8a..96ac9a8049 100644
--- a/doc/classes/float.xml
+++ b/doc/classes/float.xml
@@ -97,7 +97,7 @@
 			<description>
 				Multiplies each component of the [Vector2i] by the given [float]. Returns a [Vector2].
 				[codeblock]
-				print(0.9 * Vector2i(10, 15)) # Prints "(9, 13.5)"
+				print(0.9 * Vector2i(10, 15)) # Prints (9.0, 13.5)
 				[/codeblock]
 			</description>
 		</operator>
@@ -114,7 +114,7 @@
 			<description>
 				Multiplies each component of the [Vector3i] by the given [float]. Returns a [Vector3].
 				[codeblock]
-				print(0.9 * Vector3i(10, 15, 20)) # Prints "(9, 13.5, 18)"
+				print(0.9 * Vector3i(10, 15, 20)) # Prints (9.0, 13.5, 18.0)
 				[/codeblock]
 			</description>
 		</operator>
@@ -131,7 +131,7 @@
 			<description>
 				Multiplies each component of the [Vector4i] by the given [float]. Returns a [Vector4].
 				[codeblock]
-				print(0.9 * Vector4i(10, 15, 20, -10)) # Prints "(9, 13.5, 18, -9)"
+				print(0.9 * Vector4i(10, 15, 20, -10)) # Prints (9.0, 13.5, 18.0, -9.0)
 				[/codeblock]
 			</description>
 		</operator>

From 3ac4e363df3a66ccae25b9038fb2ccd09860a569 Mon Sep 17 00:00:00 2001
From: Peter <peterhdangg@gmail.com>
Date: Mon, 2 Dec 2024 11:06:10 -0500
Subject: [PATCH 021/124] Added missing argument to deferred call to
 restart_editor in surface_upgrade_tool

---
 editor/surface_upgrade_tool.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/editor/surface_upgrade_tool.cpp b/editor/surface_upgrade_tool.cpp
index f532535034..ccac7925d8 100644
--- a/editor/surface_upgrade_tool.cpp
+++ b/editor/surface_upgrade_tool.cpp
@@ -105,7 +105,7 @@ void SurfaceUpgradeTool::prepare_upgrade() {
 	EditorSettings::get_singleton()->set_project_metadata("surface_upgrade_tool", "resave_paths", resave_paths);
 
 	// Delay to avoid deadlocks, since this dialog can be triggered by loading a scene.
-	callable_mp(EditorNode::get_singleton(), &EditorNode::restart_editor).call_deferred();
+	callable_mp(EditorNode::get_singleton(), &EditorNode::restart_editor).call_deferred(false);
 }
 
 // Ensure that the warnings and popups are skipped.

From 93e06ff16c2134c49e26b0f5403fb8526a908aea Mon Sep 17 00:00:00 2001
From: kit <kitbdev@gmail.com>
Date: Mon, 2 Dec 2024 15:36:17 -0500
Subject: [PATCH 022/124] Fix select and edit invisble items in SubViewports

---
 editor/plugins/abstract_polygon_2d_editor.cpp | 10 ++++++++
 editor/plugins/canvas_item_editor_plugin.cpp  | 24 ++++++++++++++++---
 editor/plugins/cast_2d_editor_plugin.cpp      | 11 +++++++++
 .../collision_shape_2d_editor_plugin.cpp      | 10 ++++++++
 .../navigation_link_2d_editor_plugin.cpp      | 11 +++++++++
 editor/plugins/path_2d_editor_plugin.cpp      | 10 ++++++++
 scene/main/viewport.cpp                       | 10 ++++++++
 scene/main/viewport.h                         |  4 ++++
 8 files changed, 87 insertions(+), 3 deletions(-)

diff --git a/editor/plugins/abstract_polygon_2d_editor.cpp b/editor/plugins/abstract_polygon_2d_editor.cpp
index 01154ee6b0..c81ddf7c71 100644
--- a/editor/plugins/abstract_polygon_2d_editor.cpp
+++ b/editor/plugins/abstract_polygon_2d_editor.cpp
@@ -260,6 +260,11 @@ bool AbstractPolygon2DEditor::forward_gui_input(const Ref<InputEvent> &p_event)
 		return false;
 	}
 
+	Viewport *vp = _get_node()->get_viewport();
+	if (vp && !vp->is_visible_subviewport()) {
+		return false;
+	}
+
 	EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton();
 	Ref<InputEventMouseButton> mb = p_event;
 
@@ -501,6 +506,11 @@ void AbstractPolygon2DEditor::forward_canvas_draw_over_viewport(Control *p_overl
 		return;
 	}
 
+	Viewport *vp = _get_node()->get_viewport();
+	if (vp && !vp->is_visible_subviewport()) {
+		return;
+	}
+
 	Transform2D xform = canvas_item_editor->get_canvas_transform() * _get_node()->get_screen_transform();
 	// All polygon points are sharp, so use the sharp handle icon
 	const Ref<Texture2D> handle = get_editor_theme_icon(SNAME("EditorPathSharpHandle"));
diff --git a/editor/plugins/canvas_item_editor_plugin.cpp b/editor/plugins/canvas_item_editor_plugin.cpp
index 5b948d9b7c..9596ac64d0 100644
--- a/editor/plugins/canvas_item_editor_plugin.cpp
+++ b/editor/plugins/canvas_item_editor_plugin.cpp
@@ -618,6 +618,9 @@ void CanvasItemEditor::_find_canvas_items_at_pos(const Point2 &p_pos, Node *p_no
 	if (CanvasLayer *cl = Object::cast_to<CanvasLayer>(p_node)) {
 		xform = cl->get_transform();
 	} else if (Viewport *vp = Object::cast_to<Viewport>(p_node)) {
+		if (!vp->is_visible_subviewport()) {
+			return;
+		}
 		xform = vp->get_popup_base_transform();
 		if (!vp->get_visible_rect().has_point(xform.xform_inv(p_pos))) {
 			return;
@@ -718,6 +721,9 @@ void CanvasItemEditor::_find_canvas_items_in_rect(const Rect2 &p_rect, Node *p_n
 	if (CanvasLayer *cl = Object::cast_to<CanvasLayer>(p_node)) {
 		xform = cl->get_transform();
 	} else if (Viewport *vp = Object::cast_to<Viewport>(p_node)) {
+		if (!vp->is_visible_subviewport()) {
+			return;
+		}
 		xform = vp->get_popup_base_transform();
 		if (!vp->get_visible_rect().intersects(xform.xform_inv(p_rect))) {
 			return;
@@ -797,6 +803,10 @@ List<CanvasItem *> CanvasItemEditor::_get_edited_canvas_items(bool p_retrieve_lo
 		CanvasItem *ci = Object::cast_to<CanvasItem>(E.key);
 		if (ci) {
 			if (ci->is_visible_in_tree() && (p_retrieve_locked || !_is_node_locked(ci))) {
+				Viewport *vp = ci->get_viewport();
+				if (vp && !vp->is_visible_subviewport()) {
+					continue;
+				}
 				CanvasItemEditorSelectedItem *se = editor_selection->get_node_editor_data<CanvasItemEditorSelectedItem>(ci);
 				if (se) {
 					selection.push_back(ci);
@@ -3819,6 +3829,9 @@ void CanvasItemEditor::_draw_invisible_nodes_positions(Node *p_node, const Trans
 		parent_xform = Transform2D();
 		canvas_xform = cl->get_transform();
 	} else if (Viewport *vp = Object::cast_to<Viewport>(p_node)) {
+		if (!vp->is_visible_subviewport()) {
+			return;
+		}
 		parent_xform = Transform2D();
 		canvas_xform = vp->get_popup_base_transform();
 	}
@@ -3947,10 +3960,15 @@ void CanvasItemEditor::_draw_locks_and_groups(Node *p_node, const Transform2D &p
 
 	if (ci && !ci->is_set_as_top_level()) {
 		parent_xform = parent_xform * ci->get_transform();
-	} else {
-		CanvasLayer *cl = Object::cast_to<CanvasLayer>(p_node);
+	} else if (CanvasLayer *cl = Object::cast_to<CanvasLayer>(p_node)) {
 		parent_xform = Transform2D();
-		canvas_xform = cl ? cl->get_transform() : p_canvas_xform;
+		canvas_xform = cl->get_transform();
+	} else if (Viewport *vp = Object::cast_to<Viewport>(p_node)) {
+		if (!vp->is_visible_subviewport()) {
+			return;
+		}
+		parent_xform = Transform2D();
+		canvas_xform = vp->get_popup_base_transform();
 	}
 
 	for (int i = p_node->get_child_count() - 1; i >= 0; i--) {
diff --git a/editor/plugins/cast_2d_editor_plugin.cpp b/editor/plugins/cast_2d_editor_plugin.cpp
index d02b9579c8..3839671e21 100644
--- a/editor/plugins/cast_2d_editor_plugin.cpp
+++ b/editor/plugins/cast_2d_editor_plugin.cpp
@@ -35,6 +35,7 @@
 #include "editor/editor_undo_redo_manager.h"
 #include "scene/2d/physics/ray_cast_2d.h"
 #include "scene/2d/physics/shape_cast_2d.h"
+#include "scene/main/viewport.h"
 
 void Cast2DEditor::_notification(int p_what) {
 	switch (p_what) {
@@ -59,6 +60,11 @@ bool Cast2DEditor::forward_canvas_gui_input(const Ref<InputEvent> &p_event) {
 		return false;
 	}
 
+	Viewport *vp = node->get_viewport();
+	if (vp && !vp->is_visible_subviewport()) {
+		return false;
+	}
+
 	Transform2D xform = canvas_item_editor->get_canvas_transform() * node->get_screen_transform();
 
 	Ref<InputEventMouseButton> mb = p_event;
@@ -114,6 +120,11 @@ void Cast2DEditor::forward_canvas_draw_over_viewport(Control *p_overlay) {
 		return;
 	}
 
+	Viewport *vp = node->get_viewport();
+	if (vp && !vp->is_visible_subviewport()) {
+		return;
+	}
+
 	Transform2D gt = canvas_item_editor->get_canvas_transform() * node->get_screen_transform();
 
 	const Ref<Texture2D> handle = get_editor_theme_icon(SNAME("EditorHandle"));
diff --git a/editor/plugins/collision_shape_2d_editor_plugin.cpp b/editor/plugins/collision_shape_2d_editor_plugin.cpp
index 40c13ea3c4..46b2c8d2d9 100644
--- a/editor/plugins/collision_shape_2d_editor_plugin.cpp
+++ b/editor/plugins/collision_shape_2d_editor_plugin.cpp
@@ -300,6 +300,11 @@ bool CollisionShape2DEditor::forward_canvas_gui_input(const Ref<InputEvent> &p_e
 		return false;
 	}
 
+	Viewport *vp = node->get_viewport();
+	if (vp && !vp->is_visible_subviewport()) {
+		return false;
+	}
+
 	if (shape_type == -1) {
 		return false;
 	}
@@ -433,6 +438,11 @@ void CollisionShape2DEditor::forward_canvas_draw_over_viewport(Control *p_overla
 		return;
 	}
 
+	Viewport *vp = node->get_viewport();
+	if (vp && !vp->is_visible_subviewport()) {
+		return;
+	}
+
 	if (shape_type == -1) {
 		return;
 	}
diff --git a/editor/plugins/navigation_link_2d_editor_plugin.cpp b/editor/plugins/navigation_link_2d_editor_plugin.cpp
index ea873d8390..969dc89a1f 100644
--- a/editor/plugins/navigation_link_2d_editor_plugin.cpp
+++ b/editor/plugins/navigation_link_2d_editor_plugin.cpp
@@ -34,6 +34,7 @@
 #include "editor/editor_node.h"
 #include "editor/editor_settings.h"
 #include "editor/editor_undo_redo_manager.h"
+#include "scene/main/viewport.h"
 #include "servers/navigation_server_3d.h"
 
 void NavigationLink2DEditor::_notification(int p_what) {
@@ -59,6 +60,11 @@ bool NavigationLink2DEditor::forward_canvas_gui_input(const Ref<InputEvent> &p_e
 		return false;
 	}
 
+	Viewport *vp = node->get_viewport();
+	if (vp && !vp->is_visible_subviewport()) {
+		return false;
+	}
+
 	real_t grab_threshold = EDITOR_GET("editors/polygon_editor/point_grab_radius");
 	Transform2D xform = canvas_item_editor->get_canvas_transform() * node->get_screen_transform();
 
@@ -142,6 +148,11 @@ void NavigationLink2DEditor::forward_canvas_draw_over_viewport(Control *p_overla
 		return;
 	}
 
+	Viewport *vp = node->get_viewport();
+	if (vp && !vp->is_visible_subviewport()) {
+		return;
+	}
+
 	Transform2D gt = canvas_item_editor->get_canvas_transform() * node->get_screen_transform();
 	Vector2 global_start_position = gt.xform(node->get_start_position());
 	Vector2 global_end_position = gt.xform(node->get_end_position());
diff --git a/editor/plugins/path_2d_editor_plugin.cpp b/editor/plugins/path_2d_editor_plugin.cpp
index 524f3d86c2..da65bc8e18 100644
--- a/editor/plugins/path_2d_editor_plugin.cpp
+++ b/editor/plugins/path_2d_editor_plugin.cpp
@@ -71,6 +71,11 @@ bool Path2DEditor::forward_gui_input(const Ref<InputEvent> &p_event) {
 		return false;
 	}
 
+	Viewport *vp = node->get_viewport();
+	if (vp && !vp->is_visible_subviewport()) {
+		return false;
+	}
+
 	if (node->get_curve().is_null()) {
 		return false;
 	}
@@ -393,6 +398,11 @@ void Path2DEditor::forward_canvas_draw_over_viewport(Control *p_overlay) {
 		return;
 	}
 
+	Viewport *vp = node->get_viewport();
+	if (vp && !vp->is_visible_subviewport()) {
+		return;
+	}
+
 	Transform2D xform = canvas_item_editor->get_canvas_transform() * node->get_screen_transform();
 
 	const Ref<Texture2D> path_sharp_handle = get_editor_theme_icon(SNAME("EditorPathSharpHandle"));
diff --git a/scene/main/viewport.cpp b/scene/main/viewport.cpp
index 6c83c7843f..c3bed93ad0 100644
--- a/scene/main/viewport.cpp
+++ b/scene/main/viewport.cpp
@@ -3893,6 +3893,16 @@ bool Viewport::get_canvas_cull_mask_bit(uint32_t p_layer) const {
 	return (canvas_cull_mask & (1 << p_layer));
 }
 
+#ifdef TOOLS_ENABLED
+bool Viewport::is_visible_subviewport() const {
+	if (!is_sub_viewport()) {
+		return true;
+	}
+	SubViewportContainer *container = Object::cast_to<SubViewportContainer>(get_parent());
+	return container && container->is_visible_in_tree();
+}
+#endif // TOOLS_ENABLED
+
 void Viewport::_update_audio_listener_2d() {
 	if (AudioServer::get_singleton()) {
 		AudioServer::get_singleton()->notify_listener_changed();
diff --git a/scene/main/viewport.h b/scene/main/viewport.h
index 3a5ad2d83c..8667bc53c0 100644
--- a/scene/main/viewport.h
+++ b/scene/main/viewport.h
@@ -672,6 +672,10 @@ public:
 	void set_canvas_cull_mask_bit(uint32_t p_layer, bool p_enable);
 	bool get_canvas_cull_mask_bit(uint32_t p_layer) const;
 
+#ifdef TOOLS_ENABLED
+	bool is_visible_subviewport() const;
+#endif // TOOLS_ENABLED
+
 	virtual bool is_size_2d_override_stretch_enabled() const { return true; }
 
 	Transform2D get_screen_transform() const;

From ca12f350dbb718312ef6875a5d20bca38c07a24d Mon Sep 17 00:00:00 2001
From: David Snopek <dsnopek@gmail.com>
Date: Tue, 3 Dec 2024 12:57:56 -0600
Subject: [PATCH 023/124] `CompositorEffect` should use `GDVIRTUAL_CALL()` so
 it works with GDExtension

---
 scene/resources/compositor.cpp | 6 +++++-
 scene/resources/compositor.h   | 2 ++
 2 files changed, 7 insertions(+), 1 deletion(-)

diff --git a/scene/resources/compositor.cpp b/scene/resources/compositor.cpp
index 60b7ed0563..d9fdb93c5d 100644
--- a/scene/resources/compositor.cpp
+++ b/scene/resources/compositor.cpp
@@ -85,6 +85,10 @@ void CompositorEffect::_validate_property(PropertyInfo &p_property) const {
 	}
 }
 
+void CompositorEffect::_call_render_callback(int p_effect_callback_type, const RenderData *p_render_data) {
+	GDVIRTUAL_CALL(_render_callback, p_effect_callback_type, p_render_data);
+}
+
 void CompositorEffect::set_enabled(bool p_enabled) {
 	enabled = p_enabled;
 	if (rid.is_valid()) {
@@ -105,7 +109,7 @@ void CompositorEffect::set_effect_callback_type(EffectCallbackType p_callback_ty
 	if (rid.is_valid()) {
 		RenderingServer *rs = RenderingServer::get_singleton();
 		ERR_FAIL_NULL(rs);
-		rs->compositor_effect_set_callback(rid, RenderingServer::CompositorEffectCallbackType(effect_callback_type), Callable(this, "_render_callback"));
+		rs->compositor_effect_set_callback(rid, RenderingServer::CompositorEffectCallbackType(effect_callback_type), callable_mp(this, &CompositorEffect::_call_render_callback));
 	}
 }
 
diff --git a/scene/resources/compositor.h b/scene/resources/compositor.h
index ff840c8878..e7d56878df 100644
--- a/scene/resources/compositor.h
+++ b/scene/resources/compositor.h
@@ -65,6 +65,8 @@ protected:
 	static void _bind_methods();
 	void _validate_property(PropertyInfo &p_property) const;
 
+	void _call_render_callback(int p_effect_callback_type, const RenderData *p_render_data);
+
 	GDVIRTUAL2(_render_callback, int, const RenderData *)
 
 public:

From 56922db85b7211bb7da889d02a5a320150b34594 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?R=C3=A9mi=20Verschelde?= <rverschelde@gmail.com>
Date: Wed, 4 Dec 2024 02:14:05 +0100
Subject: [PATCH 024/124] mbedtls: Update to upstream 3.6.2

---
 thirdparty/README.md                            |  2 +-
 thirdparty/mbedtls/include/mbedtls/build_info.h |  8 ++++----
 thirdparty/mbedtls/library/pkwrite.c            | 14 +++++++++++---
 3 files changed, 16 insertions(+), 8 deletions(-)

diff --git a/thirdparty/README.md b/thirdparty/README.md
index 79d7e5b7b6..83121c735d 100644
--- a/thirdparty/README.md
+++ b/thirdparty/README.md
@@ -561,7 +561,7 @@ File extracted from upstream source:
 ## mbedtls
 
 - Upstream: https://github.com/Mbed-TLS/mbedtls
-- Version: 3.6.1 (71c569d44bf3a8bd53d874c81ee8ac644dd6e9e3, 2024)
+- Version: 3.6.2 (107ea89daaefb9867ea9121002fbbdf926780e98, 2024)
 - License: Apache 2.0
 
 File extracted from upstream release tarball:
diff --git a/thirdparty/mbedtls/include/mbedtls/build_info.h b/thirdparty/mbedtls/include/mbedtls/build_info.h
index 8242ec6828..d91d2964b6 100644
--- a/thirdparty/mbedtls/include/mbedtls/build_info.h
+++ b/thirdparty/mbedtls/include/mbedtls/build_info.h
@@ -26,16 +26,16 @@
  */
 #define MBEDTLS_VERSION_MAJOR  3
 #define MBEDTLS_VERSION_MINOR  6
-#define MBEDTLS_VERSION_PATCH  1
+#define MBEDTLS_VERSION_PATCH  2
 
 /**
  * The single version number has the following structure:
  *    MMNNPP00
  *    Major version | Minor version | Patch version
  */
-#define MBEDTLS_VERSION_NUMBER         0x03060100
-#define MBEDTLS_VERSION_STRING         "3.6.1"
-#define MBEDTLS_VERSION_STRING_FULL    "Mbed TLS 3.6.1"
+#define MBEDTLS_VERSION_NUMBER         0x03060200
+#define MBEDTLS_VERSION_STRING         "3.6.2"
+#define MBEDTLS_VERSION_STRING_FULL    "Mbed TLS 3.6.2"
 
 /* Macros for build-time platform detection */
 
diff --git a/thirdparty/mbedtls/library/pkwrite.c b/thirdparty/mbedtls/library/pkwrite.c
index 5e009c565e..2a698448be 100644
--- a/thirdparty/mbedtls/library/pkwrite.c
+++ b/thirdparty/mbedtls/library/pkwrite.c
@@ -65,17 +65,21 @@ static int pk_write_rsa_der(unsigned char **p, unsigned char *buf,
 #if defined(MBEDTLS_USE_PSA_CRYPTO)
     if (mbedtls_pk_get_type(pk) == MBEDTLS_PK_OPAQUE) {
         uint8_t tmp[PSA_EXPORT_KEY_PAIR_MAX_SIZE];
-        size_t len = 0, tmp_len = 0;
+        size_t tmp_len = 0;
 
         if (psa_export_key(pk->priv_id, tmp, sizeof(tmp), &tmp_len) != PSA_SUCCESS) {
             return MBEDTLS_ERR_PK_BAD_INPUT_DATA;
         }
+        /* Ensure there's enough space in the provided buffer before copying data into it. */
+        if (tmp_len > (size_t) (*p - buf)) {
+            mbedtls_platform_zeroize(tmp, sizeof(tmp));
+            return MBEDTLS_ERR_ASN1_BUF_TOO_SMALL;
+        }
         *p -= tmp_len;
         memcpy(*p, tmp, tmp_len);
-        len += tmp_len;
         mbedtls_platform_zeroize(tmp, sizeof(tmp));
 
-        return (int) len;
+        return (int) tmp_len;
     }
 #endif /* MBEDTLS_USE_PSA_CRYPTO */
     return mbedtls_rsa_write_key(mbedtls_pk_rsa(*pk), buf, p);
@@ -125,6 +129,10 @@ static int pk_write_ec_pubkey(unsigned char **p, unsigned char *start,
         if (psa_export_public_key(pk->priv_id, buf, sizeof(buf), &len) != PSA_SUCCESS) {
             return MBEDTLS_ERR_PK_BAD_INPUT_DATA;
         }
+        /* Ensure there's enough space in the provided buffer before copying data into it. */
+        if (len > (size_t) (*p - start)) {
+            return MBEDTLS_ERR_ASN1_BUF_TOO_SMALL;
+        }
         *p -= len;
         memcpy(*p, buf, len);
         return (int) len;

From bd47e4f1ba5a0d8623944582eaf9b21a1fa68c3a Mon Sep 17 00:00:00 2001
From: Matt Enad <m22mail@comcast.net>
Date: Tue, 8 Oct 2024 13:09:13 -0400
Subject: [PATCH 025/124] Add `Tablet/Trackpad` nav preset

This commit adds a new navigation preset called `Tablet/Trackpad` which enables "Emulate 3 Button Mouse" to more quickly set up good default keys for tablet users.

It also adds support for mouse buttons 4 and 5 in the navigation settings which will be helpful if users want to customize 3D navigation further for specific pens/mice.
---
 doc/classes/EditorSettings.xml           |  5 +++--
 editor/editor_settings.cpp               |  8 ++++----
 editor/editor_settings_dialog.cpp        | 25 ++++++++++++++++++++++--
 editor/plugins/node_3d_editor_plugin.cpp | 12 ++++++++++++
 editor/plugins/node_3d_editor_plugin.h   | 11 +++++++----
 5 files changed, 49 insertions(+), 12 deletions(-)

diff --git a/doc/classes/EditorSettings.xml b/doc/classes/EditorSettings.xml
index 550933ce64..25e2e959f3 100644
--- a/doc/classes/EditorSettings.xml
+++ b/doc/classes/EditorSettings.xml
@@ -360,12 +360,13 @@
 			If [code]true[/code], invert the vertical mouse axis when panning, orbiting, or using freelook mode in the 3D editor.
 		</member>
 		<member name="editors/3d/navigation/navigation_scheme" type="int" setter="" getter="">
-			The navigation scheme preset to use in the 3D editor. Changing this setting will affect the mouse button and modifier controls used to navigate the 3D editor viewport.
+			The navigation scheme preset to use in the 3D editor. Changing this setting will affect the mouse button and modifier keys used to navigate the 3D editor viewport.
 			All schemes can use [kbd]Mouse wheel[/kbd] to zoom.
 			- [b]Godot:[/b] [kbd]Middle mouse button[/kbd] to orbit. [kbd]Shift + Middle mouse button[/kbd] to pan. [kbd]Ctrl + Middle mouse button[/kbd] to zoom.
 			- [b]Maya:[/b] [kbd]Alt + Left mouse button[/kbd] to orbit. [kbd]Middle mouse button[/kbd] to pan, [kbd]Shift + Middle mouse button[/kbd] to pan 10 times faster. [kbd]Alt + Right mouse button[/kbd] to zoom.
 			- [b]Modo:[/b] [kbd]Alt + Left mouse button[/kbd] to orbit. [kbd]Alt + Shift + Left mouse button[/kbd] to pan. [kbd]Ctrl + Alt + Left mouse button[/kbd] to zoom.
-			See also [member editors/3d/navigation/orbit_mouse_button], [member editors/3d/navigation/pan_mouse_button], [member editors/3d/navigation/zoom_mouse_button], and [member editors/3d/freelook/freelook_navigation_scheme].
+			- [b]Tablet/Trackpad:[/b] [kbd]Alt[/kbd] to orbit. [kbd]Shift[/kbd] to pan. [kbd]Ctrl[/kbd] to zoom. Enables 3-button mouse emulation mode.
+			See also [member editors/3d/navigation/orbit_mouse_button], [member editors/3d/navigation/pan_mouse_button], [member editors/3d/navigation/zoom_mouse_button], [member editors/3d/freelook/freelook_navigation_scheme], and [member editors/3d/navigation/emulate_3_button_mouse].
 			[b]Note:[/b] On certain window managers on Linux, the [kbd]Alt[/kbd] key will be intercepted by the window manager when clicking a mouse button at the same time. This means Godot will not see the modifier key as being pressed.
 		</member>
 		<member name="editors/3d/navigation/orbit_mouse_button" type="int" setter="" getter="">
diff --git a/editor/editor_settings.cpp b/editor/editor_settings.cpp
index 153cb74b74..ea0d5804e1 100644
--- a/editor/editor_settings.cpp
+++ b/editor/editor_settings.cpp
@@ -816,10 +816,10 @@ void EditorSettings::_load_defaults(Ref<ConfigFile> p_extra_config) {
 	// 3D: Navigation
 	_initial_set("editors/3d/navigation/invert_x_axis", false, true);
 	_initial_set("editors/3d/navigation/invert_y_axis", false, true);
-	EDITOR_SETTING_BASIC(Variant::INT, PROPERTY_HINT_ENUM, "editors/3d/navigation/navigation_scheme", 0, "Godot,Maya,Modo,Custom")
-	EDITOR_SETTING_BASIC(Variant::INT, PROPERTY_HINT_ENUM, "editors/3d/navigation/orbit_mouse_button", 1, "Left Mouse,Middle Mouse,Right Mouse")
-	EDITOR_SETTING_BASIC(Variant::INT, PROPERTY_HINT_ENUM, "editors/3d/navigation/pan_mouse_button", 1, "Left Mouse,Middle Mouse,Right Mouse")
-	EDITOR_SETTING_BASIC(Variant::INT, PROPERTY_HINT_ENUM, "editors/3d/navigation/zoom_mouse_button", 1, "Left Mouse,Middle Mouse,Right Mouse")
+	EDITOR_SETTING_BASIC(Variant::INT, PROPERTY_HINT_ENUM, "editors/3d/navigation/navigation_scheme", 0, "Godot:0,Maya:1,Modo:2,Tablet/Trackpad:4,Custom:3")
+	EDITOR_SETTING_BASIC(Variant::INT, PROPERTY_HINT_ENUM, "editors/3d/navigation/orbit_mouse_button", 1, "Left Mouse,Middle Mouse,Right Mouse,Mouse Button 4,Mouse Button 5")
+	EDITOR_SETTING_BASIC(Variant::INT, PROPERTY_HINT_ENUM, "editors/3d/navigation/pan_mouse_button", 1, "Left Mouse,Middle Mouse,Right Mouse,Mouse Button 4,Mouse Button 5")
+	EDITOR_SETTING_BASIC(Variant::INT, PROPERTY_HINT_ENUM, "editors/3d/navigation/zoom_mouse_button", 1, "Left Mouse,Middle Mouse,Right Mouse,Mouse Button 4,Mouse Button 5")
 	EDITOR_SETTING_BASIC(Variant::INT, PROPERTY_HINT_ENUM, "editors/3d/navigation/zoom_style", 0, "Vertical,Horizontal")
 
 	_initial_set("editors/3d/navigation/emulate_numpad", false, true);
diff --git a/editor/editor_settings_dialog.cpp b/editor/editor_settings_dialog.cpp
index 8989b9cf9b..877b18697b 100644
--- a/editor/editor_settings_dialog.cpp
+++ b/editor/editor_settings_dialog.cpp
@@ -77,7 +77,7 @@ void EditorSettingsDialog::_settings_property_edited(const String &p_name) {
 		EditorSettings::get_singleton()->set_manually("text_editor/theme/color_theme", "Custom");
 	} else if (full_name.begins_with("editors/visual_editors/connection_colors") || full_name.begins_with("editors/visual_editors/category_colors")) {
 		EditorSettings::get_singleton()->set_manually("editors/visual_editors/color_theme", "Custom");
-	} else if (full_name == "editors/3d/navigation/orbit_mouse_button" || full_name == "editors/3d/navigation/pan_mouse_button" || full_name == "editors/3d/navigation/zoom_mouse_button") {
+	} else if (full_name == "editors/3d/navigation/orbit_mouse_button" || full_name == "editors/3d/navigation/pan_mouse_button" || full_name == "editors/3d/navigation/zoom_mouse_button" || full_name == "editors/3d/navigation/emulate_3_button_mouse") {
 		EditorSettings::get_singleton()->set_manually("editors/3d/navigation/navigation_scheme", (int)Node3DEditorViewport::NAVIGATION_CUSTOM);
 	} else if (full_name == "editors/3d/navigation/navigation_scheme") {
 		update_navigation_preset();
@@ -89,6 +89,7 @@ void EditorSettingsDialog::update_navigation_preset() {
 	Node3DEditorViewport::ViewportNavMouseButton set_orbit_mouse_button = Node3DEditorViewport::NAVIGATION_LEFT_MOUSE;
 	Node3DEditorViewport::ViewportNavMouseButton set_pan_mouse_button = Node3DEditorViewport::NAVIGATION_LEFT_MOUSE;
 	Node3DEditorViewport::ViewportNavMouseButton set_zoom_mouse_button = Node3DEditorViewport::NAVIGATION_LEFT_MOUSE;
+	bool set_3_button_mouse = false;
 	Ref<InputEventKey> orbit_mod_key_1;
 	Ref<InputEventKey> orbit_mod_key_2;
 	Ref<InputEventKey> pan_mod_key_1;
@@ -102,6 +103,7 @@ void EditorSettingsDialog::update_navigation_preset() {
 		set_orbit_mouse_button = Node3DEditorViewport::NAVIGATION_MIDDLE_MOUSE;
 		set_pan_mouse_button = Node3DEditorViewport::NAVIGATION_MIDDLE_MOUSE;
 		set_zoom_mouse_button = Node3DEditorViewport::NAVIGATION_MIDDLE_MOUSE;
+		set_3_button_mouse = false;
 		orbit_mod_key_1 = InputEventKey::create_reference(Key::NONE);
 		orbit_mod_key_2 = InputEventKey::create_reference(Key::NONE);
 		pan_mod_key_1 = InputEventKey::create_reference(Key::SHIFT);
@@ -113,6 +115,7 @@ void EditorSettingsDialog::update_navigation_preset() {
 		set_orbit_mouse_button = Node3DEditorViewport::NAVIGATION_LEFT_MOUSE;
 		set_pan_mouse_button = Node3DEditorViewport::NAVIGATION_MIDDLE_MOUSE;
 		set_zoom_mouse_button = Node3DEditorViewport::NAVIGATION_RIGHT_MOUSE;
+		set_3_button_mouse = false;
 		orbit_mod_key_1 = InputEventKey::create_reference(Key::ALT);
 		orbit_mod_key_2 = InputEventKey::create_reference(Key::NONE);
 		pan_mod_key_1 = InputEventKey::create_reference(Key::NONE);
@@ -124,18 +127,32 @@ void EditorSettingsDialog::update_navigation_preset() {
 		set_orbit_mouse_button = Node3DEditorViewport::NAVIGATION_LEFT_MOUSE;
 		set_pan_mouse_button = Node3DEditorViewport::NAVIGATION_LEFT_MOUSE;
 		set_zoom_mouse_button = Node3DEditorViewport::NAVIGATION_LEFT_MOUSE;
+		set_3_button_mouse = false;
 		orbit_mod_key_1 = InputEventKey::create_reference(Key::ALT);
 		orbit_mod_key_2 = InputEventKey::create_reference(Key::NONE);
 		pan_mod_key_1 = InputEventKey::create_reference(Key::SHIFT);
 		pan_mod_key_2 = InputEventKey::create_reference(Key::ALT);
 		zoom_mod_key_1 = InputEventKey::create_reference(Key::ALT);
 		zoom_mod_key_2 = InputEventKey::create_reference(Key::CTRL);
+	} else if (nav_scheme == Node3DEditorViewport::NAVIGATION_TABLET) {
+		set_preset = true;
+		set_orbit_mouse_button = Node3DEditorViewport::NAVIGATION_MIDDLE_MOUSE;
+		set_pan_mouse_button = Node3DEditorViewport::NAVIGATION_MIDDLE_MOUSE;
+		set_zoom_mouse_button = Node3DEditorViewport::NAVIGATION_MIDDLE_MOUSE;
+		set_3_button_mouse = true;
+		orbit_mod_key_1 = InputEventKey::create_reference(Key::ALT);
+		orbit_mod_key_2 = InputEventKey::create_reference(Key::NONE);
+		pan_mod_key_1 = InputEventKey::create_reference(Key::SHIFT);
+		pan_mod_key_2 = InputEventKey::create_reference(Key::NONE);
+		zoom_mod_key_1 = InputEventKey::create_reference(Key::CTRL);
+		zoom_mod_key_2 = InputEventKey::create_reference(Key::NONE);
 	}
 	// Set settings to the desired preset values.
 	if (set_preset) {
 		EditorSettings::get_singleton()->set_manually("editors/3d/navigation/orbit_mouse_button", (int)set_orbit_mouse_button);
 		EditorSettings::get_singleton()->set_manually("editors/3d/navigation/pan_mouse_button", (int)set_pan_mouse_button);
 		EditorSettings::get_singleton()->set_manually("editors/3d/navigation/zoom_mouse_button", (int)set_zoom_mouse_button);
+		EditorSettings::get_singleton()->set_manually("editors/3d/navigation/emulate_3_button_mouse", set_3_button_mouse);
 		_set_shortcut_input("spatial_editor/viewport_orbit_modifier_1", orbit_mod_key_1);
 		_set_shortcut_input("spatial_editor/viewport_orbit_modifier_2", orbit_mod_key_2);
 		_set_shortcut_input("spatial_editor/viewport_pan_modifier_1", pan_mod_key_1);
@@ -775,7 +792,11 @@ PropertyInfo EditorSettingsDialog::_create_mouse_shortcut_property_info(const St
 	hint_string += _get_shortcut_button_string(p_shortcut_1_name) + _get_shortcut_button_string(p_shortcut_2_name);
 	hint_string += "Middle Mouse,";
 	hint_string += _get_shortcut_button_string(p_shortcut_1_name) + _get_shortcut_button_string(p_shortcut_2_name);
-	hint_string += "Right Mouse";
+	hint_string += "Right Mouse,";
+	hint_string += _get_shortcut_button_string(p_shortcut_1_name) + _get_shortcut_button_string(p_shortcut_2_name);
+	hint_string += "Mouse Button 4,";
+	hint_string += _get_shortcut_button_string(p_shortcut_1_name) + _get_shortcut_button_string(p_shortcut_2_name);
+	hint_string += "Mouse Button 5";
 
 	return PropertyInfo(Variant::INT, p_property_name, PROPERTY_HINT_ENUM, hint_string);
 }
diff --git a/editor/plugins/node_3d_editor_plugin.cpp b/editor/plugins/node_3d_editor_plugin.cpp
index 5d11cc7bc5..1ac83408b9 100644
--- a/editor/plugins/node_3d_editor_plugin.cpp
+++ b/editor/plugins/node_3d_editor_plugin.cpp
@@ -2125,6 +2125,18 @@ void Node3DEditorViewport::_sinput(const Ref<InputEvent> &p_event) {
 				nav_mode = change_nav_from_shortcut;
 			}
 
+		} else if (m->get_button_mask().has_flag(MouseButtonMask::MB_XBUTTON1)) {
+			NavigationMode change_nav_from_shortcut = _get_nav_mode_from_shortcut_check(NAVIGATION_MOUSE_4, shortcut_check_sets, false);
+			if (change_nav_from_shortcut != NAVIGATION_NONE) {
+				nav_mode = change_nav_from_shortcut;
+			}
+
+		} else if (m->get_button_mask().has_flag(MouseButtonMask::MB_XBUTTON2)) {
+			NavigationMode change_nav_from_shortcut = _get_nav_mode_from_shortcut_check(NAVIGATION_MOUSE_5, shortcut_check_sets, false);
+			if (change_nav_from_shortcut != NAVIGATION_NONE) {
+				nav_mode = change_nav_from_shortcut;
+			}
+
 		} else if (EDITOR_GET("editors/3d/navigation/emulate_3_button_mouse")) {
 			// Handle trackpad (no external mouse) use case
 			NavigationMode change_nav_from_shortcut = _get_nav_mode_from_shortcut_check(NAVIGATION_LEFT_MOUSE, shortcut_check_sets, true);
diff --git a/editor/plugins/node_3d_editor_plugin.h b/editor/plugins/node_3d_editor_plugin.h
index 243a712c3a..0b5ed554b1 100644
--- a/editor/plugins/node_3d_editor_plugin.h
+++ b/editor/plugins/node_3d_editor_plugin.h
@@ -190,10 +190,11 @@ public:
 	};
 
 	enum NavigationScheme {
-		NAVIGATION_GODOT,
-		NAVIGATION_MAYA,
-		NAVIGATION_MODO,
-		NAVIGATION_CUSTOM,
+		NAVIGATION_GODOT = 0,
+		NAVIGATION_MAYA = 1,
+		NAVIGATION_MODO = 2,
+		NAVIGATION_CUSTOM = 3,
+		NAVIGATION_TABLET = 4,
 	};
 
 	enum FreelookNavigationScheme {
@@ -206,6 +207,8 @@ public:
 		NAVIGATION_LEFT_MOUSE,
 		NAVIGATION_MIDDLE_MOUSE,
 		NAVIGATION_RIGHT_MOUSE,
+		NAVIGATION_MOUSE_4,
+		NAVIGATION_MOUSE_5,
 	};
 
 private:

From 614a51f2933ec0448ea49cf870ba31e0d71a374a Mon Sep 17 00:00:00 2001
From: Daylily-Zeleen <daylily-zeleen@foxmail.com>
Date: Thu, 5 Dec 2024 14:24:39 +0800
Subject: [PATCH 026/124] Optimize callable's stringify text.

---
 core/variant/callable.cpp                     | 8 ++++++--
 modules/gdscript/gdscript_lambda_callable.cpp | 6 +++---
 modules/gdscript/gdscript_rpc_callable.cpp    | 9 ++++++++-
 3 files changed, 17 insertions(+), 6 deletions(-)

diff --git a/core/variant/callable.cpp b/core/variant/callable.cpp
index ddeea27118..402dc6823c 100644
--- a/core/variant/callable.cpp
+++ b/core/variant/callable.cpp
@@ -361,8 +361,12 @@ Callable::operator String() const {
 		if (base) {
 			String class_name = base->get_class();
 			Ref<Script> script = base->get_script();
-			if (script.is_valid() && script->get_path().is_resource_file()) {
-				class_name += "(" + script->get_path().get_file() + ")";
+			if (script.is_valid()) {
+				if (!script->get_global_name().is_empty()) {
+					class_name += "(" + script->get_global_name() + ")";
+				} else if (script->get_path().is_resource_file()) {
+					class_name += "(" + script->get_path().get_file() + ")";
+				}
 			}
 			return class_name + "::" + String(method);
 		} else {
diff --git a/modules/gdscript/gdscript_lambda_callable.cpp b/modules/gdscript/gdscript_lambda_callable.cpp
index d6fd5d043b..d767324b42 100644
--- a/modules/gdscript/gdscript_lambda_callable.cpp
+++ b/modules/gdscript/gdscript_lambda_callable.cpp
@@ -178,12 +178,12 @@ uint32_t GDScriptLambdaSelfCallable::hash() const {
 
 String GDScriptLambdaSelfCallable::get_as_text() const {
 	if (function == nullptr) {
-		return "<invalid lambda>";
+		return "<invalid self lambda>";
 	}
 	if (function->get_name() != StringName()) {
-		return function->get_name().operator String() + "(lambda)";
+		return function->get_name().operator String() + "(self lambda)";
 	}
-	return "(anonymous lambda)";
+	return "(anonymous self lambda)";
 }
 
 CallableCustom::CompareEqualFunc GDScriptLambdaSelfCallable::get_compare_equal_func() const {
diff --git a/modules/gdscript/gdscript_rpc_callable.cpp b/modules/gdscript/gdscript_rpc_callable.cpp
index 3139371eb5..05629b1c00 100644
--- a/modules/gdscript/gdscript_rpc_callable.cpp
+++ b/modules/gdscript/gdscript_rpc_callable.cpp
@@ -49,7 +49,14 @@ uint32_t GDScriptRPCCallable::hash() const {
 String GDScriptRPCCallable::get_as_text() const {
 	String class_name = object->get_class();
 	Ref<Script> script = object->get_script();
-	return class_name + "(" + script->get_path().get_file() + ")::" + String(method) + " (rpc)";
+	if (script.is_valid()) {
+		if (!script->get_global_name().is_empty()) {
+			class_name += "(" + script->get_global_name() + ")";
+		} else if (script->get_path().is_resource_file()) {
+			class_name += "(" + script->get_path().get_file() + ")";
+		}
+	}
+	return class_name + "::" + String(method) + " (rpc)";
 }
 
 CallableCustom::CompareEqualFunc GDScriptRPCCallable::get_compare_equal_func() const {

From d7462d223ca655d0924ce9688151175d9c901935 Mon Sep 17 00:00:00 2001
From: Guilherme Sousa <guilherme.sousa1994@gmail.com>
Date: Wed, 13 Nov 2024 17:31:37 +0100
Subject: [PATCH 027/124] Add animation node extension

---
 doc/classes/AnimationNode.xml                | 15 +++-
 doc/classes/AnimationNodeExtension.xml       | 38 +++++++++
 scene/animation/animation_node_extension.cpp | 83 ++++++++++++++++++++
 scene/animation/animation_node_extension.h   | 55 +++++++++++++
 scene/animation/animation_tree.cpp           | 14 ++++
 scene/animation/animation_tree.h             |  4 +
 scene/register_scene_types.cpp               |  2 +
 7 files changed, 210 insertions(+), 1 deletion(-)
 create mode 100644 doc/classes/AnimationNodeExtension.xml
 create mode 100644 scene/animation/animation_node_extension.cpp
 create mode 100644 scene/animation/animation_node_extension.h

diff --git a/doc/classes/AnimationNode.xml b/doc/classes/AnimationNode.xml
index 3b7cd9adad..4948ad94f4 100644
--- a/doc/classes/AnimationNode.xml
+++ b/doc/classes/AnimationNode.xml
@@ -152,11 +152,24 @@
 				Gets the value of a parameter. Parameters are custom local memory used for your animation nodes, given a resource can be reused in multiple trees.
 			</description>
 		</method>
+		<method name="get_processing_animation_tree_instance_id" qualifiers="const">
+			<return type="int" />
+			<description>
+				Returns the object id of the [AnimationTree] that owns this node.
+				[b]Note:[/b] This method should only be called from within the [method AnimationNodeExtension._process] method, and will return an invalid id otherwise.
+			</description>
+		</method>
 		<method name="is_path_filtered" qualifiers="const">
 			<return type="bool" />
 			<param index="0" name="path" type="NodePath" />
 			<description>
-				Returns whether the given path is filtered.
+				Returns [code]true[/code] if the given path is filtered.
+			</description>
+		</method>
+		<method name="is_process_testing" qualifiers="const">
+			<return type="bool" />
+			<description>
+				Returns [code]true[/code] if this animation node is being processed in test-only mode.
 			</description>
 		</method>
 		<method name="remove_input">
diff --git a/doc/classes/AnimationNodeExtension.xml b/doc/classes/AnimationNodeExtension.xml
new file mode 100644
index 0000000000..3373b95a33
--- /dev/null
+++ b/doc/classes/AnimationNodeExtension.xml
@@ -0,0 +1,38 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<class name="AnimationNodeExtension" inherits="AnimationRootNode" experimental="" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="../class.xsd">
+	<brief_description>
+		Base class for extending [AnimationRootNode]s from GDScript, C#, or C++.
+	</brief_description>
+	<description>
+		[AnimationNodeExtension] exposes the APIs of [AnimationRootNode] to allow users to extend it from GDScript, C#, or C++. This class is not meant to be used directly, but to be extended by other classes. It is used to create custom nodes for the [AnimationTree] system.
+	</description>
+	<tutorials>
+	</tutorials>
+	<methods>
+		<method name="_process" qualifiers="virtual">
+			<return type="PackedFloat32Array" />
+			<param index="0" name="playback_info" type="PackedFloat64Array" />
+			<param index="1" name="test_only" type="bool" />
+			<description>
+				A version of the [method AnimationNode._process] method that is meant to be overridden by custom nodes. It returns a [PackedFloat32Array] with the processed animation data.
+				The [PackedFloat64Array] parameter contains the playback information, containing the following values encoded as floating point numbers (in order): playback time and delta, start and end times, whether a seek was requested (encoded as a float greater than [code]0[/code]), whether the seek request was externally requested (encoded as a float greater than [code]0[/code]), the current [enum Animation.LoopedFlag] (encoded as a float), and the current blend weight.
+				The function must return a [PackedFloat32Array] of the node's time info, containing the following values (in order): animation length, time position, delta, [enum Animation.LoopMode] (encoded as a float), whether the animation is about to end (encoded as a float greater than [code]0[/code]) and whether the animation is infinite (encoded as a float greater than [code]0[/code]). All values must be included in the returned array.
+			</description>
+		</method>
+		<method name="get_remaining_time" qualifiers="static">
+			<return type="float" />
+			<param index="0" name="node_info" type="PackedFloat32Array" />
+			<param index="1" name="break_loop" type="bool" />
+			<description>
+				Returns the animation's remaining time for the given node info. For looping animations, it will only return the remaining time if [param break_loop] is [code]true[/code], a large integer value will be returned otherwise.
+			</description>
+		</method>
+		<method name="is_looping" qualifiers="static">
+			<return type="bool" />
+			<param index="0" name="node_info" type="PackedFloat32Array" />
+			<description>
+				Returns [code]true[/code] if the animation for the given [param node_info] is looping.
+			</description>
+		</method>
+	</methods>
+</class>
diff --git a/scene/animation/animation_node_extension.cpp b/scene/animation/animation_node_extension.cpp
new file mode 100644
index 0000000000..8536da9d0f
--- /dev/null
+++ b/scene/animation/animation_node_extension.cpp
@@ -0,0 +1,83 @@
+/**************************************************************************/
+/*  animation_node_extension.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 "animation_node_extension.h"
+
+AnimationNode::NodeTimeInfo AnimationNodeExtension::_process(const AnimationMixer::PlaybackInfo p_playback_info, bool p_test_only) {
+	PackedFloat32Array r_ret;
+
+	GDVIRTUAL_CALL(
+			_process,
+			_playback_info_to_array(p_playback_info),
+			p_test_only,
+			r_ret);
+
+	return _array_to_node_time_info(r_ret);
+}
+
+bool AnimationNodeExtension::is_looping(const PackedFloat32Array &p_node_info) {
+	return _array_to_node_time_info(p_node_info).is_looping();
+}
+
+double AnimationNodeExtension::get_remaining_time(const PackedFloat32Array &p_node_info, bool p_break_loop) {
+	return _array_to_node_time_info(p_node_info).get_remain(p_break_loop);
+}
+
+void AnimationNodeExtension::_bind_methods() {
+	ClassDB::bind_static_method("AnimationNodeExtension", D_METHOD("is_looping", "node_info"), &AnimationNodeExtension::is_looping);
+	ClassDB::bind_static_method("AnimationNodeExtension", D_METHOD("get_remaining_time", "node_info", "break_loop"), &AnimationNodeExtension::get_remaining_time);
+	GDVIRTUAL_BIND(_process, "playback_info", "test_only");
+}
+
+AnimationNode::NodeTimeInfo AnimationNodeExtension::_array_to_node_time_info(const PackedFloat32Array &p_node_info) {
+	ERR_FAIL_COND_V_MSG(p_node_info.size() != 6, AnimationNode::NodeTimeInfo(), "Invalid node info.");
+	AnimationNode::NodeTimeInfo ret_val;
+	ret_val.length = p_node_info[0];
+	ret_val.position = p_node_info[1];
+	ret_val.delta = p_node_info[2];
+	ret_val.loop_mode = static_cast<Animation::LoopMode>(p_node_info[3]);
+	ret_val.will_end = p_node_info[4] > 0.0;
+	ret_val.is_infinity = p_node_info[5] > 0.0;
+	return ret_val;
+}
+
+PackedFloat64Array AnimationNodeExtension::_playback_info_to_array(const AnimationMixer::PlaybackInfo &p_playback_info) {
+	PackedFloat64Array playback_info_array;
+	playback_info_array.push_back(p_playback_info.time);
+	playback_info_array.push_back(p_playback_info.delta);
+	playback_info_array.push_back(p_playback_info.start);
+	playback_info_array.push_back(p_playback_info.end);
+	playback_info_array.push_back(p_playback_info.seeked);
+	playback_info_array.push_back(p_playback_info.is_external_seeking);
+	playback_info_array.push_back(p_playback_info.looped_flag);
+	playback_info_array.push_back(p_playback_info.weight);
+
+	return playback_info_array;
+}
diff --git a/scene/animation/animation_node_extension.h b/scene/animation/animation_node_extension.h
new file mode 100644
index 0000000000..08e4b4f00c
--- /dev/null
+++ b/scene/animation/animation_node_extension.h
@@ -0,0 +1,55 @@
+/**************************************************************************/
+/*  animation_node_extension.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 ANIMATION_NODE_EXTENSION_H
+#define ANIMATION_NODE_EXTENSION_H
+
+#include "scene/animation/animation_tree.h"
+
+class AnimationNodeExtension : public AnimationRootNode {
+	GDCLASS(AnimationNodeExtension, AnimationRootNode);
+
+public:
+	virtual NodeTimeInfo _process(const AnimationMixer::PlaybackInfo p_playback_info, bool p_test_only = false) override;
+
+	static bool is_looping(const PackedFloat32Array &p_node_info);
+	static double get_remaining_time(const PackedFloat32Array &p_node_info, bool p_break_loop = false);
+
+protected:
+	static void _bind_methods();
+
+	GDVIRTUAL2R_REQUIRED(PackedFloat32Array, _process, PackedFloat64Array, bool);
+
+private:
+	static AnimationNode::NodeTimeInfo _array_to_node_time_info(const PackedFloat32Array &p_array);
+	static PackedFloat64Array _playback_info_to_array(const AnimationMixer::PlaybackInfo &p_playback_info);
+};
+
+#endif // ANIMATION_NODE_EXTENSION_H
diff --git a/scene/animation/animation_tree.cpp b/scene/animation/animation_tree.cpp
index d676e2acf4..dc0a27b005 100644
--- a/scene/animation/animation_tree.cpp
+++ b/scene/animation/animation_tree.cpp
@@ -417,6 +417,16 @@ bool AnimationNode::is_deletable() const {
 	return closable;
 }
 
+ObjectID AnimationNode::get_processing_animation_tree_instance_id() const {
+	ERR_FAIL_NULL_V(process_state, ObjectID());
+	return process_state->tree->get_instance_id();
+}
+
+bool AnimationNode::is_process_testing() const {
+	ERR_FAIL_NULL_V(process_state, false);
+	return process_state->is_testing;
+}
+
 bool AnimationNode::is_path_filtered(const NodePath &p_path) const {
 	return filter.has(p_path);
 }
@@ -542,6 +552,10 @@ void AnimationNode::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_filter_enabled", "enable"), &AnimationNode::set_filter_enabled);
 	ClassDB::bind_method(D_METHOD("is_filter_enabled"), &AnimationNode::is_filter_enabled);
 
+	ClassDB::bind_method(D_METHOD("get_processing_animation_tree_instance_id"), &AnimationNode::get_processing_animation_tree_instance_id);
+
+	ClassDB::bind_method(D_METHOD("is_process_testing"), &AnimationNode::is_process_testing);
+
 	ClassDB::bind_method(D_METHOD("_set_filters", "filters"), &AnimationNode::_set_filters);
 	ClassDB::bind_method(D_METHOD("_get_filters"), &AnimationNode::_get_filters);
 
diff --git a/scene/animation/animation_tree.h b/scene/animation/animation_tree.h
index eb25a8db1b..08e9f87e87 100644
--- a/scene/animation/animation_tree.h
+++ b/scene/animation/animation_tree.h
@@ -226,6 +226,10 @@ public:
 	void set_deletable(bool p_closable);
 	bool is_deletable() const;
 
+	ObjectID get_processing_animation_tree_instance_id() const;
+
+	bool is_process_testing() const;
+
 	virtual bool has_filter() const;
 
 #ifdef TOOLS_ENABLED
diff --git a/scene/register_scene_types.cpp b/scene/register_scene_types.cpp
index 8a048e9cc3..87f1ce56c1 100644
--- a/scene/register_scene_types.cpp
+++ b/scene/register_scene_types.cpp
@@ -37,6 +37,7 @@
 #include "scene/animation/animation_blend_space_2d.h"
 #include "scene/animation/animation_blend_tree.h"
 #include "scene/animation/animation_mixer.h"
+#include "scene/animation/animation_node_extension.h"
 #include "scene/animation/animation_node_state_machine.h"
 #include "scene/animation/animation_player.h"
 #include "scene/animation/animation_tree.h"
@@ -512,6 +513,7 @@ void register_scene_types() {
 	GDREGISTER_CLASS(AnimationNodeBlendSpace2D);
 	GDREGISTER_CLASS(AnimationNodeStateMachine);
 	GDREGISTER_CLASS(AnimationNodeStateMachinePlayback);
+	GDREGISTER_VIRTUAL_CLASS(AnimationNodeExtension);
 
 	GDREGISTER_INTERNAL_CLASS(AnimationNodeStartState);
 	GDREGISTER_INTERNAL_CLASS(AnimationNodeEndState);

From cec037b1a81a8fc4b3083dac207e03c59d3fbc0f Mon Sep 17 00:00:00 2001
From: Hugo Locurcio <hugo.locurcio@hugo.pro>
Date: Thu, 5 Dec 2024 14:03:19 +0100
Subject: [PATCH 028/124] GitHub Actions: Use quiet xmllint output to only
 print error lines

Success lines are no longer printed, making the CI log shorter
and faster to read.
---
 .github/workflows/static_checks.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/static_checks.yml b/.github/workflows/static_checks.yml
index 9b326cb43e..e832faaca4 100644
--- a/.github/workflows/static_checks.yml
+++ b/.github/workflows/static_checks.yml
@@ -56,7 +56,7 @@ jobs:
 
       - name: Class reference schema checks
         run: |
-          xmllint --noout --schema doc/class.xsd doc/classes/*.xml modules/*/doc_classes/*.xml platform/*/doc_classes/*.xml
+          xmllint --quiet --noout --schema doc/class.xsd doc/classes/*.xml modules/*/doc_classes/*.xml platform/*/doc_classes/*.xml
 
       - name: Run C compiler on `gdextension_interface.h`
         run: |

From a9881f99a027f1bdbe65dc1b7e561bc49b744a58 Mon Sep 17 00:00:00 2001
From: Hugo Locurcio <hugo.locurcio@hugo.pro>
Date: Thu, 5 Dec 2024 18:05:15 +0100
Subject: [PATCH 029/124] Enable Emulate Numpad by default in the 3D editor
 settings

This allows using the top row keys as 3D navigation shortcuts.

This provides a better out of the box experience, as most laptops
don't have a numpad nowadays.
---
 editor/editor_settings.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/editor/editor_settings.cpp b/editor/editor_settings.cpp
index 153cb74b74..b9a5648b59 100644
--- a/editor/editor_settings.cpp
+++ b/editor/editor_settings.cpp
@@ -822,7 +822,7 @@ void EditorSettings::_load_defaults(Ref<ConfigFile> p_extra_config) {
 	EDITOR_SETTING_BASIC(Variant::INT, PROPERTY_HINT_ENUM, "editors/3d/navigation/zoom_mouse_button", 1, "Left Mouse,Middle Mouse,Right Mouse")
 	EDITOR_SETTING_BASIC(Variant::INT, PROPERTY_HINT_ENUM, "editors/3d/navigation/zoom_style", 0, "Vertical,Horizontal")
 
-	_initial_set("editors/3d/navigation/emulate_numpad", false, true);
+	_initial_set("editors/3d/navigation/emulate_numpad", true, true);
 	_initial_set("editors/3d/navigation/emulate_3_button_mouse", false, true);
 	_initial_set("editors/3d/navigation/warped_mouse_panning", true, true);
 

From 25354237190897c64ab8b13f0dba97a375136afc Mon Sep 17 00:00:00 2001
From: clayjohn <claynjohn@gmail.com>
Date: Tue, 3 Dec 2024 17:22:48 -0800
Subject: [PATCH 030/124] Increase the size of the uniform set cache to 4096 to
 avoid cache thrashing from projects with high numbers of unique textures.

---
 doc/classes/ProjectSettings.xml | 2 +-
 servers/rendering_server.cpp    | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/doc/classes/ProjectSettings.xml b/doc/classes/ProjectSettings.xml
index 1da578167c..78d25809c7 100644
--- a/doc/classes/ProjectSettings.xml
+++ b/doc/classes/ProjectSettings.xml
@@ -2367,7 +2367,7 @@
 		<member name="rendering/2d/batching/item_buffer_size" type="int" setter="" getter="" default="16384">
 			Maximum number of canvas item commands that can be batched into a single draw call.
 		</member>
-		<member name="rendering/2d/batching/uniform_set_cache_size" type="int" setter="" getter="" default="256">
+		<member name="rendering/2d/batching/uniform_set_cache_size" type="int" setter="" getter="" default="4096">
 			Maximum number of uniform sets that will be cached by the 2D renderer when batching draw calls.
 			[b]Note:[/b] A project that uses a large number of unique sprite textures per frame may benefit from increasing this value.
 		</member>
diff --git a/servers/rendering_server.cpp b/servers/rendering_server.cpp
index 2051d0caac..5e0a1daf2e 100644
--- a/servers/rendering_server.cpp
+++ b/servers/rendering_server.cpp
@@ -3584,7 +3584,7 @@ void RenderingServer::init() {
 
 	GLOBAL_DEF(PropertyInfo(Variant::INT, "rendering/2d/shadow_atlas/size", PROPERTY_HINT_RANGE, "128,16384"), 2048);
 	GLOBAL_DEF_RST(PropertyInfo(Variant::INT, "rendering/2d/batching/item_buffer_size", PROPERTY_HINT_RANGE, "128,1048576,1"), 16384);
-	GLOBAL_DEF_RST(PropertyInfo(Variant::INT, "rendering/2d/batching/uniform_set_cache_size", PROPERTY_HINT_RANGE, "256,1048576,1"), 256);
+	GLOBAL_DEF_RST(PropertyInfo(Variant::INT, "rendering/2d/batching/uniform_set_cache_size", PROPERTY_HINT_RANGE, "256,1048576,1"), 4096);
 
 	// Number of commands that can be drawn per frame.
 	GLOBAL_DEF_RST(PropertyInfo(Variant::INT, "rendering/gl_compatibility/item_buffer_size", PROPERTY_HINT_RANGE, "128,1048576,1"), 16384);

From 05010180ce269246bb408525dcda8945d393ea7c Mon Sep 17 00:00:00 2001
From: landervr <31851431+CpnWaffle@users.noreply.github.com>
Date: Tue, 3 Dec 2024 12:08:36 +0100
Subject: [PATCH 031/124] ReflectionProbe add Blend Distance

---
 doc/classes/ReflectionProbe.xml               |  3 +++
 doc/classes/RenderingServer.xml               |  8 ++++++
 drivers/gles3/rasterizer_scene_gles3.cpp      |  2 ++
 drivers/gles3/shaders/scene.glsl              | 24 ++++++++++++-----
 drivers/gles3/storage/light_storage.cpp       |  7 +++++
 drivers/gles3/storage/light_storage.h         |  2 ++
 .../gizmos/reflection_probe_gizmo_plugin.cpp  | 26 +++++++++++++++++++
 scene/3d/reflection_probe.cpp                 | 14 ++++++++++
 scene/3d/reflection_probe.h                   |  4 +++
 .../rendering/dummy/storage/light_storage.h   |  1 +
 .../renderer_rd/shaders/light_data_inc.glsl   |  4 +++
 .../shaders/scene_forward_lights_inc.glsl     | 17 +++++++++---
 .../renderer_rd/storage_rd/light_storage.cpp  | 15 +++++++++++
 .../renderer_rd/storage_rd/light_storage.h    |  7 +++++
 servers/rendering/rendering_server_default.h  |  1 +
 servers/rendering/storage/light_storage.h     |  1 +
 servers/rendering_server.cpp                  |  1 +
 servers/rendering_server.h                    |  1 +
 18 files changed, 128 insertions(+), 10 deletions(-)

diff --git a/doc/classes/ReflectionProbe.xml b/doc/classes/ReflectionProbe.xml
index 367942682e..0af58ad08d 100644
--- a/doc/classes/ReflectionProbe.xml
+++ b/doc/classes/ReflectionProbe.xml
@@ -23,6 +23,9 @@
 		<member name="ambient_mode" type="int" setter="set_ambient_mode" getter="get_ambient_mode" enum="ReflectionProbe.AmbientMode" default="1">
 			The ambient color to use within the [ReflectionProbe]'s box defined by its [member size]. The ambient color will smoothly blend with other [ReflectionProbe]s and the rest of the scene (outside the [ReflectionProbe]'s box defined by its [member size]).
 		</member>
+		<member name="blend_distance" type="float" setter="set_blend_distance" getter="get_blend_distance" default="1.0">
+			Defines the distance in meters over which a probe blends into the scene.
+		</member>
 		<member name="box_projection" type="bool" setter="set_enable_box_projection" getter="is_box_projection_enabled" default="false">
 			If [code]true[/code], enables box projection. This makes reflections look more correct in rectangle-shaped rooms by offsetting the reflection center depending on the camera's location.
 			[b]Note:[/b] To better fit rectangle-shaped rooms that are not aligned to the grid, you can rotate the [ReflectionProbe] node.
diff --git a/doc/classes/RenderingServer.xml b/doc/classes/RenderingServer.xml
index c245ac77af..6593d69a20 100644
--- a/doc/classes/RenderingServer.xml
+++ b/doc/classes/RenderingServer.xml
@@ -3163,6 +3163,14 @@
 				If [code]true[/code], reflections will ignore sky contribution. Equivalent to [member ReflectionProbe.interior].
 			</description>
 		</method>
+		<method name="reflection_probe_set_blend_distance">
+			<return type="void" />
+			<param index="0" name="probe" type="RID" />
+			<param index="1" name="blend_distance" type="float" />
+			<description>
+				Sets the distance in meters over which a probe blends into the scene.
+			</description>
+		</method>
 		<method name="reflection_probe_set_cull_mask">
 			<return type="void" />
 			<param index="0" name="probe" type="RID" />
diff --git a/drivers/gles3/rasterizer_scene_gles3.cpp b/drivers/gles3/rasterizer_scene_gles3.cpp
index 17bdcbcad3..81347245c0 100644
--- a/drivers/gles3/rasterizer_scene_gles3.cpp
+++ b/drivers/gles3/rasterizer_scene_gles3.cpp
@@ -3435,6 +3435,7 @@ void RasterizerSceneGLES3::_render_list_template(RenderListParameters *p_params,
 						material_storage->shaders.scene_shader.version_set_uniform(SceneShaderGLES3::REFPROBE1_AMBIENT_MODE, int(probe->ambient_mode), shader->version, instance_variant, spec_constants);
 						material_storage->shaders.scene_shader.version_set_uniform(SceneShaderGLES3::REFPROBE1_AMBIENT_COLOR, probe->ambient_color * probe->ambient_color_energy, shader->version, instance_variant, spec_constants);
 						material_storage->shaders.scene_shader.version_set_uniform(SceneShaderGLES3::REFPROBE1_LOCAL_MATRIX, inst->reflection_probes_local_transform_cache[0], shader->version, instance_variant, spec_constants);
+						material_storage->shaders.scene_shader.version_set_uniform(SceneShaderGLES3::REFPROBE1_BLEND_DISTANCE, probe->blend_distance, shader->version, instance_variant, spec_constants);
 
 						glActiveTexture(GL_TEXTURE0 + config->max_texture_image_units - 7);
 						glBindTexture(GL_TEXTURE_CUBE_MAP, light_storage->reflection_probe_instance_get_texture(inst->reflection_probe_rid_cache[0]));
@@ -3453,6 +3454,7 @@ void RasterizerSceneGLES3::_render_list_template(RenderListParameters *p_params,
 						material_storage->shaders.scene_shader.version_set_uniform(SceneShaderGLES3::REFPROBE2_AMBIENT_MODE, int(probe->ambient_mode), shader->version, instance_variant, spec_constants);
 						material_storage->shaders.scene_shader.version_set_uniform(SceneShaderGLES3::REFPROBE2_AMBIENT_COLOR, probe->ambient_color * probe->ambient_color_energy, shader->version, instance_variant, spec_constants);
 						material_storage->shaders.scene_shader.version_set_uniform(SceneShaderGLES3::REFPROBE2_LOCAL_MATRIX, inst->reflection_probes_local_transform_cache[1], shader->version, instance_variant, spec_constants);
+						material_storage->shaders.scene_shader.version_set_uniform(SceneShaderGLES3::REFPROBE2_BLEND_DISTANCE, probe->blend_distance, shader->version, instance_variant, spec_constants);
 
 						glActiveTexture(GL_TEXTURE0 + config->max_texture_image_units - 8);
 						glBindTexture(GL_TEXTURE_CUBE_MAP, light_storage->reflection_probe_instance_get_texture(inst->reflection_probe_rid_cache[1]));
diff --git a/drivers/gles3/shaders/scene.glsl b/drivers/gles3/shaders/scene.glsl
index 2022c8ee43..a6be3af437 100644
--- a/drivers/gles3/shaders/scene.glsl
+++ b/drivers/gles3/shaders/scene.glsl
@@ -875,6 +875,7 @@ uniform vec3 refprobe1_box_offset;
 uniform highp mat4 refprobe1_local_matrix;
 uniform bool refprobe1_exterior;
 uniform float refprobe1_intensity;
+uniform float refprobe1_blend_distance;
 uniform int refprobe1_ambient_mode;
 uniform vec4 refprobe1_ambient_color;
 
@@ -888,6 +889,7 @@ uniform vec3 refprobe2_box_offset;
 uniform highp mat4 refprobe2_local_matrix;
 uniform bool refprobe2_exterior;
 uniform float refprobe2_intensity;
+uniform float refprobe2_blend_distance;
 uniform int refprobe2_ambient_mode;
 uniform vec4 refprobe2_ambient_color;
 
@@ -1589,7 +1591,7 @@ void reflection_process(samplerCube reflection_map,
 		vec3 normal, vec3 vertex,
 		mat4 local_matrix,
 		bool use_box_project, vec3 box_extents, vec3 box_offset,
-		bool exterior, float intensity, int ref_ambient_mode, vec4 ref_ambient_color,
+		bool exterior, float intensity, float blend_distance, int ref_ambient_mode, vec4 ref_ambient_color,
 		float roughness, vec3 ambient, vec3 skybox,
 		inout highp vec4 reflection_accum, inout highp vec4 ambient_accum) {
 	vec4 reflection;
@@ -1601,10 +1603,20 @@ void reflection_process(samplerCube reflection_map,
 	}
 
 	vec3 inner_pos = abs(local_pos / box_extents);
-	float blend = max(inner_pos.x, max(inner_pos.y, inner_pos.z));
-	blend = mix(length(inner_pos), blend, blend);
-	blend *= blend;
+	vec3 blend_axes = vec3(0.0, 0.0, 0.0);
+	float blend = 0.0;
+	if (blend_distance != 0) {
+		for (int i = 0; i < 3; i++) {
+			float axis_blend_distance = min(blend_distance, box_extents[i]);
+			blend_axes[i] = (inner_pos[i] * box_extents[i]) - box_extents[i] + axis_blend_distance;
+			blend_axes[i] = blend_axes[i] / axis_blend_distance;
+			blend_axes[i] = clamp(blend_axes[i], 0.0, 1.0);
+		}
+		blend = pow((1.0 - blend_axes.x) * (1.0 - blend_axes.y) * (1.0 - blend_axes.z), 2);
+		blend = 1 - blend;
+	}
 	blend = max(0.0, 1.0 - blend);
+	blend = clamp(blend, 0.0, 1.0);
 
 	//reflect and make local
 	vec3 ref_normal = normalize(reflect(vertex, normal));
@@ -1963,14 +1975,14 @@ void main() {
 
 		reflection_process(refprobe1_texture, normal, vertex_interp, refprobe1_local_matrix,
 				refprobe1_use_box_project, refprobe1_box_extents, refprobe1_box_offset,
-				refprobe1_exterior, refprobe1_intensity, refprobe1_ambient_mode, refprobe1_ambient_color,
+				refprobe1_exterior, refprobe1_intensity, refprobe1_blend_distance, refprobe1_ambient_mode, refprobe1_ambient_color,
 				roughness, ambient_light, specular_light, reflection_accum, ambient_accum);
 
 #ifdef SECOND_REFLECTION_PROBE
 
 		reflection_process(refprobe2_texture, normal, vertex_interp, refprobe2_local_matrix,
 				refprobe2_use_box_project, refprobe2_box_extents, refprobe2_box_offset,
-				refprobe2_exterior, refprobe2_intensity, refprobe2_ambient_mode, refprobe2_ambient_color,
+				refprobe2_exterior, refprobe2_intensity, refprobe2_blend_distance, refprobe2_ambient_mode, refprobe2_ambient_color,
 				roughness, ambient_light, specular_light, reflection_accum, ambient_accum);
 
 #endif // SECOND_REFLECTION_PROBE
diff --git a/drivers/gles3/storage/light_storage.cpp b/drivers/gles3/storage/light_storage.cpp
index 886918a2f7..4b99b289c6 100644
--- a/drivers/gles3/storage/light_storage.cpp
+++ b/drivers/gles3/storage/light_storage.cpp
@@ -471,6 +471,13 @@ void LightStorage::reflection_probe_set_intensity(RID p_probe, float p_intensity
 	reflection_probe->intensity = p_intensity;
 }
 
+void LightStorage::reflection_probe_set_blend_distance(RID p_probe, float p_blend_distance) {
+	ReflectionProbe *reflection_probe = reflection_probe_owner.get_or_null(p_probe);
+	ERR_FAIL_NULL(reflection_probe);
+
+	reflection_probe->blend_distance = p_blend_distance;
+}
+
 void LightStorage::reflection_probe_set_ambient_mode(RID p_probe, RS::ReflectionProbeAmbientMode p_mode) {
 	ReflectionProbe *reflection_probe = reflection_probe_owner.get_or_null(p_probe);
 	ERR_FAIL_NULL(reflection_probe);
diff --git a/drivers/gles3/storage/light_storage.h b/drivers/gles3/storage/light_storage.h
index 0695102640..ec1c6ef3b9 100644
--- a/drivers/gles3/storage/light_storage.h
+++ b/drivers/gles3/storage/light_storage.h
@@ -118,6 +118,7 @@ struct ReflectionProbe {
 	RS::ReflectionProbeUpdateMode update_mode = RS::REFLECTION_PROBE_UPDATE_ONCE;
 	int resolution = 256;
 	float intensity = 1.0;
+	float blend_distance = 1.0;
 	RS::ReflectionProbeAmbientMode ambient_mode = RS::REFLECTION_PROBE_AMBIENT_ENVIRONMENT;
 	Color ambient_color;
 	float ambient_color_energy = 1.0;
@@ -642,6 +643,7 @@ public:
 
 	virtual void reflection_probe_set_update_mode(RID p_probe, RS::ReflectionProbeUpdateMode p_mode) override;
 	virtual void reflection_probe_set_intensity(RID p_probe, float p_intensity) override;
+	virtual void reflection_probe_set_blend_distance(RID p_probe, float p_blend_distance) override;
 	virtual void reflection_probe_set_ambient_mode(RID p_probe, RS::ReflectionProbeAmbientMode p_mode) override;
 	virtual void reflection_probe_set_ambient_color(RID p_probe, const Color &p_color) override;
 	virtual void reflection_probe_set_ambient_energy(RID p_probe, float p_energy) override;
diff --git a/editor/plugins/gizmos/reflection_probe_gizmo_plugin.cpp b/editor/plugins/gizmos/reflection_probe_gizmo_plugin.cpp
index d74c434410..6a0a7592f7 100644
--- a/editor/plugins/gizmos/reflection_probe_gizmo_plugin.cpp
+++ b/editor/plugins/gizmos/reflection_probe_gizmo_plugin.cpp
@@ -165,6 +165,32 @@ void ReflectionProbeGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) {
 		aabb.position = -size / 2;
 		aabb.size = size;
 
+		AABB blend_aabb;
+		for (int i = 0; i < 3; i++) {
+			blend_aabb.position[i] = aabb.position[i] + probe->get_blend_distance();
+			blend_aabb.size[i] = aabb.size[i] - probe->get_blend_distance() * 2.0;
+			if (blend_aabb.size[i] < blend_aabb.position[i]) {
+				blend_aabb.position[i] = aabb.position[i] + aabb.size[i] / 2.0;
+				blend_aabb.size[i] = 0.0;
+			}
+		}
+
+		if (probe->get_blend_distance() != 0.0) {
+			for (int i = 0; i < 12; i++) {
+				Vector3 a;
+				Vector3 b;
+				blend_aabb.get_edge(i, a, b);
+				lines.push_back(a);
+				lines.push_back(b);
+			}
+
+			for (int i = 0; i < 8; i++) {
+				Vector3 ep = aabb.get_endpoint(i);
+				internal_lines.push_back(blend_aabb.get_endpoint(i));
+				internal_lines.push_back(ep);
+			}
+		}
+
 		Vector<Vector3> handles = helper->box_get_handles(probe->get_size());
 
 		if (probe->get_origin_offset() != Vector3(0.0, 0.0, 0.0)) {
diff --git a/scene/3d/reflection_probe.cpp b/scene/3d/reflection_probe.cpp
index c5654d0d77..a2c7bf428d 100644
--- a/scene/3d/reflection_probe.cpp
+++ b/scene/3d/reflection_probe.cpp
@@ -39,6 +39,16 @@ float ReflectionProbe::get_intensity() const {
 	return intensity;
 }
 
+void ReflectionProbe::set_blend_distance(float p_blend_distance) {
+	blend_distance = p_blend_distance;
+	RS::get_singleton()->reflection_probe_set_blend_distance(probe, p_blend_distance);
+	update_gizmos();
+}
+
+float ReflectionProbe::get_blend_distance() const {
+	return blend_distance;
+}
+
 void ReflectionProbe::set_ambient_mode(AmbientMode p_mode) {
 	ambient_mode = p_mode;
 	RS::get_singleton()->reflection_probe_set_ambient_mode(probe, RS::ReflectionProbeAmbientMode(p_mode));
@@ -202,6 +212,9 @@ void ReflectionProbe::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_intensity", "intensity"), &ReflectionProbe::set_intensity);
 	ClassDB::bind_method(D_METHOD("get_intensity"), &ReflectionProbe::get_intensity);
 
+	ClassDB::bind_method(D_METHOD("set_blend_distance", "blend_distance"), &ReflectionProbe::set_blend_distance);
+	ClassDB::bind_method(D_METHOD("get_blend_distance"), &ReflectionProbe::get_blend_distance);
+
 	ClassDB::bind_method(D_METHOD("set_ambient_mode", "ambient"), &ReflectionProbe::set_ambient_mode);
 	ClassDB::bind_method(D_METHOD("get_ambient_mode"), &ReflectionProbe::get_ambient_mode);
 
@@ -243,6 +256,7 @@ void ReflectionProbe::_bind_methods() {
 
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "update_mode", PROPERTY_HINT_ENUM, "Once (Fast),Always (Slow)"), "set_update_mode", "get_update_mode");
 	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "intensity", PROPERTY_HINT_RANGE, "0,1,0.01"), "set_intensity", "get_intensity");
+	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "blend_distance", PROPERTY_HINT_RANGE, "0,8,0.01,or_greater,suffix:m"), "set_blend_distance", "get_blend_distance");
 	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "max_distance", PROPERTY_HINT_RANGE, "0,16384,0.1,or_greater,exp,suffix:m"), "set_max_distance", "get_max_distance");
 	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "size", PROPERTY_HINT_NONE, "suffix:m"), "set_size", "get_size");
 	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "origin_offset", PROPERTY_HINT_NONE, "suffix:m"), "set_origin_offset", "get_origin_offset");
diff --git a/scene/3d/reflection_probe.h b/scene/3d/reflection_probe.h
index 7221294228..3c3a38d7a6 100644
--- a/scene/3d/reflection_probe.h
+++ b/scene/3d/reflection_probe.h
@@ -51,6 +51,7 @@ public:
 private:
 	RID probe;
 	float intensity = 1.0;
+	float blend_distance = 1.0;
 	float max_distance = 0.0;
 	Vector3 size = Vector3(20, 20, 20);
 	Vector3 origin_offset = Vector3(0, 0, 0);
@@ -78,6 +79,9 @@ public:
 	void set_intensity(float p_intensity);
 	float get_intensity() const;
 
+	void set_blend_distance(float p_blend_distance);
+	float get_blend_distance() const;
+
 	void set_ambient_mode(AmbientMode p_mode);
 	AmbientMode get_ambient_mode() const;
 
diff --git a/servers/rendering/dummy/storage/light_storage.h b/servers/rendering/dummy/storage/light_storage.h
index d25523753c..5ea479de99 100644
--- a/servers/rendering/dummy/storage/light_storage.h
+++ b/servers/rendering/dummy/storage/light_storage.h
@@ -124,6 +124,7 @@ public:
 
 	virtual void reflection_probe_set_update_mode(RID p_probe, RS::ReflectionProbeUpdateMode p_mode) override {}
 	virtual void reflection_probe_set_intensity(RID p_probe, float p_intensity) override {}
+	virtual void reflection_probe_set_blend_distance(RID p_probe, float p_blend_distance) override {}
 	virtual void reflection_probe_set_ambient_mode(RID p_probe, RS::ReflectionProbeAmbientMode p_mode) override {}
 	virtual void reflection_probe_set_ambient_color(RID p_probe, const Color &p_color) override {}
 	virtual void reflection_probe_set_ambient_energy(RID p_probe, float p_energy) override {}
diff --git a/servers/rendering/renderer_rd/shaders/light_data_inc.glsl b/servers/rendering/renderer_rd/shaders/light_data_inc.glsl
index 7488a3f2c7..6f0ca2176d 100644
--- a/servers/rendering/renderer_rd/shaders/light_data_inc.glsl
+++ b/servers/rendering/renderer_rd/shaders/light_data_inc.glsl
@@ -41,10 +41,14 @@ struct ReflectionData {
 	uint mask;
 	mediump vec3 ambient; // ambient color
 	mediump float intensity;
+	mediump float blend_distance;
 	bool exterior;
 	bool box_project;
 	uint ambient_mode;
 	float exposure_normalization;
+	float pad0;
+	float pad1;
+	float pad2;
 	//0-8 is intensity,8-9 is ambient, mode
 	highp mat4 local_matrix; // up to here for spot and omni, rest is for directional
 	// notes: for ambientblend, use distance to edge to blend between already existing global environment
diff --git a/servers/rendering/renderer_rd/shaders/scene_forward_lights_inc.glsl b/servers/rendering/renderer_rd/shaders/scene_forward_lights_inc.glsl
index f58304aa8d..44d8429a34 100644
--- a/servers/rendering/renderer_rd/shaders/scene_forward_lights_inc.glsl
+++ b/servers/rendering/renderer_rd/shaders/scene_forward_lights_inc.glsl
@@ -876,11 +876,20 @@ void reflection_process(uint ref_index, vec3 vertex, vec3 ref_vec, vec3 normal,
 	}
 
 	vec3 inner_pos = abs(local_pos / box_extents);
-	float blend = max(inner_pos.x, max(inner_pos.y, inner_pos.z));
-	//make blend more rounded
-	blend = mix(length(inner_pos), blend, blend);
-	blend *= blend;
+	vec3 blend_axes = vec3(0.0, 0.0, 0.0);
+	float blend = 0.0;
+	if (reflections.data[ref_index].blend_distance != 0) {
+		for (int i = 0; i < 3; i++) {
+			float axis_blend_distance = min(reflections.data[ref_index].blend_distance, box_extents[i]);
+			blend_axes[i] = (inner_pos[i] * box_extents[i]) - box_extents[i] + axis_blend_distance;
+			blend_axes[i] = blend_axes[i] / axis_blend_distance;
+			blend_axes[i] = clamp(blend_axes[i], 0.0, 1.0);
+		}
+		blend = pow((1.0 - blend_axes.x) * (1.0 - blend_axes.y) * (1.0 - blend_axes.z), 2);
+		blend = 1 - blend;
+	}
 	blend = max(0.0, 1.0 - blend);
+	blend = clamp(blend, 0.0, 1.0);
 
 	if (reflections.data[ref_index].intensity > 0.0) { // compute reflection
 
diff --git a/servers/rendering/renderer_rd/storage_rd/light_storage.cpp b/servers/rendering/renderer_rd/storage_rd/light_storage.cpp
index 1d309a8006..5fbd983e03 100644
--- a/servers/rendering/renderer_rd/storage_rd/light_storage.cpp
+++ b/servers/rendering/renderer_rd/storage_rd/light_storage.cpp
@@ -1066,6 +1066,13 @@ void LightStorage::reflection_probe_set_intensity(RID p_probe, float p_intensity
 	reflection_probe->intensity = p_intensity;
 }
 
+void LightStorage::reflection_probe_set_blend_distance(RID p_probe, float p_blend_distance) {
+	ReflectionProbe *reflection_probe = reflection_probe_owner.get_or_null(p_probe);
+	ERR_FAIL_NULL(reflection_probe);
+
+	reflection_probe->blend_distance = p_blend_distance;
+}
+
 void LightStorage::reflection_probe_set_ambient_mode(RID p_probe, RS::ReflectionProbeAmbientMode p_mode) {
 	ReflectionProbe *reflection_probe = reflection_probe_owner.get_or_null(p_probe);
 	ERR_FAIL_NULL(reflection_probe);
@@ -1266,6 +1273,13 @@ float LightStorage::reflection_probe_get_intensity(RID p_probe) const {
 	return reflection_probe->intensity;
 }
 
+float LightStorage::reflection_probe_get_blend_distance(RID p_probe) const {
+	const ReflectionProbe *reflection_probe = reflection_probe_owner.get_or_null(p_probe);
+	ERR_FAIL_NULL_V(reflection_probe, 0);
+
+	return reflection_probe->blend_distance;
+}
+
 bool LightStorage::reflection_probe_is_interior(RID p_probe) const {
 	const ReflectionProbe *reflection_probe = reflection_probe_owner.get_or_null(p_probe);
 	ERR_FAIL_NULL_V(reflection_probe, false);
@@ -1755,6 +1769,7 @@ void LightStorage::update_reflection_probe_buffer(RenderDataRD *p_render_data, c
 		reflection_ubo.mask = probe->reflection_mask;
 
 		reflection_ubo.intensity = probe->intensity;
+		reflection_ubo.blend_distance = probe->blend_distance;
 		reflection_ubo.ambient_mode = probe->ambient_mode;
 
 		reflection_ubo.exterior = !probe->interior;
diff --git a/servers/rendering/renderer_rd/storage_rd/light_storage.h b/servers/rendering/renderer_rd/storage_rd/light_storage.h
index b5f1523123..ea22484f4c 100644
--- a/servers/rendering/renderer_rd/storage_rd/light_storage.h
+++ b/servers/rendering/renderer_rd/storage_rd/light_storage.h
@@ -223,6 +223,7 @@ private:
 		RS::ReflectionProbeUpdateMode update_mode = RS::REFLECTION_PROBE_UPDATE_ONCE;
 		int resolution = 256;
 		float intensity = 1.0;
+		float blend_distance = 1.0;
 		RS::ReflectionProbeAmbientMode ambient_mode = RS::REFLECTION_PROBE_AMBIENT_ENVIRONMENT;
 		Color ambient_color;
 		float ambient_color_energy = 1.0;
@@ -303,10 +304,14 @@ private:
 		uint32_t mask;
 		float ambient[3]; // ambient color,
 		float intensity;
+		float blend_distance;
 		uint32_t exterior;
 		uint32_t box_project;
 		uint32_t ambient_mode;
 		float exposure_normalization;
+		uint32_t pad0;
+		uint32_t pad1;
+		uint32_t pad2;
 		float local_matrix[16]; // up to here for spot and omni, rest is for directional
 	};
 
@@ -829,6 +834,7 @@ public:
 
 	virtual void reflection_probe_set_update_mode(RID p_probe, RS::ReflectionProbeUpdateMode p_mode) override;
 	virtual void reflection_probe_set_intensity(RID p_probe, float p_intensity) override;
+	virtual void reflection_probe_set_blend_distance(RID p_probe, float p_blend_distance) override;
 	virtual void reflection_probe_set_ambient_mode(RID p_probe, RS::ReflectionProbeAmbientMode p_mode) override;
 	virtual void reflection_probe_set_ambient_color(RID p_probe, const Color &p_color) override;
 	virtual void reflection_probe_set_ambient_energy(RID p_probe, float p_energy) override;
@@ -859,6 +865,7 @@ public:
 	virtual bool reflection_probe_renders_shadows(RID p_probe) const override;
 
 	float reflection_probe_get_intensity(RID p_probe) const;
+	float reflection_probe_get_blend_distance(RID p_probe) const;
 	bool reflection_probe_is_interior(RID p_probe) const;
 	bool reflection_probe_is_box_projection(RID p_probe) const;
 	RS::ReflectionProbeAmbientMode reflection_probe_get_ambient_mode(RID p_probe) const;
diff --git a/servers/rendering/rendering_server_default.h b/servers/rendering/rendering_server_default.h
index 29b1e163c7..1e1dae88ee 100644
--- a/servers/rendering/rendering_server_default.h
+++ b/servers/rendering/rendering_server_default.h
@@ -460,6 +460,7 @@ public:
 
 	FUNC2(reflection_probe_set_update_mode, RID, ReflectionProbeUpdateMode)
 	FUNC2(reflection_probe_set_intensity, RID, float)
+	FUNC2(reflection_probe_set_blend_distance, RID, float)
 	FUNC2(reflection_probe_set_ambient_color, RID, const Color &)
 	FUNC2(reflection_probe_set_ambient_energy, RID, float)
 	FUNC2(reflection_probe_set_ambient_mode, RID, ReflectionProbeAmbientMode)
diff --git a/servers/rendering/storage/light_storage.h b/servers/rendering/storage/light_storage.h
index 1e149e3a97..624d7cb89f 100644
--- a/servers/rendering/storage/light_storage.h
+++ b/servers/rendering/storage/light_storage.h
@@ -111,6 +111,7 @@ public:
 	virtual void reflection_probe_set_update_mode(RID p_probe, RS::ReflectionProbeUpdateMode p_mode) = 0;
 	virtual void reflection_probe_set_resolution(RID p_probe, int p_resolution) = 0;
 	virtual void reflection_probe_set_intensity(RID p_probe, float p_intensity) = 0;
+	virtual void reflection_probe_set_blend_distance(RID p_probe, float p_blend_distance) = 0;
 	virtual void reflection_probe_set_ambient_mode(RID p_probe, RS::ReflectionProbeAmbientMode p_mode) = 0;
 	virtual void reflection_probe_set_ambient_color(RID p_probe, const Color &p_color) = 0;
 	virtual void reflection_probe_set_ambient_energy(RID p_probe, float p_energy) = 0;
diff --git a/servers/rendering_server.cpp b/servers/rendering_server.cpp
index 2051d0caac..7bcb626808 100644
--- a/servers/rendering_server.cpp
+++ b/servers/rendering_server.cpp
@@ -2569,6 +2569,7 @@ void RenderingServer::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("reflection_probe_create"), &RenderingServer::reflection_probe_create);
 	ClassDB::bind_method(D_METHOD("reflection_probe_set_update_mode", "probe", "mode"), &RenderingServer::reflection_probe_set_update_mode);
 	ClassDB::bind_method(D_METHOD("reflection_probe_set_intensity", "probe", "intensity"), &RenderingServer::reflection_probe_set_intensity);
+	ClassDB::bind_method(D_METHOD("reflection_probe_set_blend_distance", "probe", "blend_distance"), &RenderingServer::reflection_probe_set_blend_distance);
 	ClassDB::bind_method(D_METHOD("reflection_probe_set_ambient_mode", "probe", "mode"), &RenderingServer::reflection_probe_set_ambient_mode);
 	ClassDB::bind_method(D_METHOD("reflection_probe_set_ambient_color", "probe", "color"), &RenderingServer::reflection_probe_set_ambient_color);
 	ClassDB::bind_method(D_METHOD("reflection_probe_set_ambient_energy", "probe", "energy"), &RenderingServer::reflection_probe_set_ambient_energy);
diff --git a/servers/rendering_server.h b/servers/rendering_server.h
index 0917af73c6..531cdab0db 100644
--- a/servers/rendering_server.h
+++ b/servers/rendering_server.h
@@ -622,6 +622,7 @@ public:
 
 	virtual void reflection_probe_set_update_mode(RID p_probe, ReflectionProbeUpdateMode p_mode) = 0;
 	virtual void reflection_probe_set_intensity(RID p_probe, float p_intensity) = 0;
+	virtual void reflection_probe_set_blend_distance(RID p_probe, float p_blend_distance) = 0;
 
 	enum ReflectionProbeAmbientMode {
 		REFLECTION_PROBE_AMBIENT_DISABLED,

From f92018288feedb3113ef831044c60e4ad2da1fec Mon Sep 17 00:00:00 2001
From: clayjohn <claynjohn@gmail.com>
Date: Thu, 5 Dec 2024 19:11:03 -0800
Subject: [PATCH 032/124] Ensure schlick is available when using clearcoat with
 GGX

---
 .../renderer_rd/shaders/scene_forward_lights_inc.glsl       | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/servers/rendering/renderer_rd/shaders/scene_forward_lights_inc.glsl b/servers/rendering/renderer_rd/shaders/scene_forward_lights_inc.glsl
index f58304aa8d..40804948b9 100644
--- a/servers/rendering/renderer_rd/shaders/scene_forward_lights_inc.glsl
+++ b/servers/rendering/renderer_rd/shaders/scene_forward_lights_inc.glsl
@@ -134,10 +134,8 @@ void light_compute(vec3 N, vec3 L, vec3 V, float A, vec3 light_color, bool is_di
 		float ccNdotL = max(min(A + dot(vertex_normal, L), 1.0), 0.0);
 		float ccNdotH = clamp(A + dot(vertex_normal, H), 0.0, 1.0);
 		float ccNdotV = max(dot(vertex_normal, V), 1e-4);
-
-#if !defined(SPECULAR_SCHLICK_GGX)
 		float cLdotH5 = SchlickFresnel(cLdotH);
-#endif
+
 		float Dr = D_GGX(ccNdotH, mix(0.001, 0.1, clearcoat_roughness));
 		float Gr = 0.25 / (cLdotH * cLdotH);
 		float Fr = mix(.04, 1.0, cLdotH5);
@@ -212,7 +210,9 @@ void light_compute(vec3 N, vec3 L, vec3 V, float A, vec3 light_color, bool is_di
 			float G = V_GGX(cNdotL, cNdotV, alpha_ggx);
 #endif // LIGHT_ANISOTROPY_USED
 	   // F
+#if !defined(LIGHT_CLEARCOAT_USED)
 			float cLdotH5 = SchlickFresnel(cLdotH);
+#endif
 			// Calculate Fresnel using specular occlusion term from Filament:
 			// https://google.github.io/filament/Filament.html#lighting/occlusion/specularocclusion
 			float f90 = clamp(dot(f0, vec3(50.0 * 0.33)), metallic, 1.0);

From 93208657968082f73a9f000ab9f0f839d89fccd6 Mon Sep 17 00:00:00 2001
From: clayjohn <claynjohn@gmail.com>
Date: Mon, 9 Sep 2024 18:33:19 -0300
Subject: [PATCH 033/124] Avoid calculating dynamic lights when lights are
 baked into LightmapGI using the static bake mode

---
 drivers/gles3/rasterizer_scene_gles3.cpp      | 28 ++++++++-----------
 drivers/gles3/shaders/scene.glsl              | 21 +++++++-------
 .../scene_forward_clustered.glsl              |  4 +++
 .../forward_mobile/scene_forward_mobile.glsl  |  8 ++++++
 .../renderer_rd/storage_rd/light_storage.cpp  |  3 +-
 servers/rendering/renderer_scene_cull.cpp     |  4 +++
 6 files changed, 40 insertions(+), 28 deletions(-)

diff --git a/drivers/gles3/rasterizer_scene_gles3.cpp b/drivers/gles3/rasterizer_scene_gles3.cpp
index 17bdcbcad3..cd4c7dd82f 100644
--- a/drivers/gles3/rasterizer_scene_gles3.cpp
+++ b/drivers/gles3/rasterizer_scene_gles3.cpp
@@ -1302,15 +1302,12 @@ void RasterizerSceneGLES3::_fill_render_list(RenderListType p_render_list, const
 					int32_t shadow_id = light_storage->light_instance_get_shadow_id(light_instance);
 
 					if (light_storage->light_has_shadow(light) && shadow_id >= 0) {
-						// Skip static lights when a lightmap is used.
-						if (!inst->lightmap_instance.is_valid() || light_storage->light_get_bake_mode(light) != RenderingServer::LIGHT_BAKE_STATIC) {
-							GeometryInstanceGLES3::LightPass pass;
-							pass.light_id = light_storage->light_instance_get_gl_id(light_instance);
-							pass.shadow_id = shadow_id;
-							pass.light_instance_rid = light_instance;
-							pass.is_omni = true;
-							inst->light_passes.push_back(pass);
-						}
+						GeometryInstanceGLES3::LightPass pass;
+						pass.light_id = light_storage->light_instance_get_gl_id(light_instance);
+						pass.shadow_id = shadow_id;
+						pass.light_instance_rid = light_instance;
+						pass.is_omni = true;
+						inst->light_passes.push_back(pass);
 					} else {
 						// Lights without shadow can all go in base pass.
 						inst->omni_light_gl_cache.push_back((uint32_t)light_storage->light_instance_get_gl_id(light_instance));
@@ -1328,14 +1325,11 @@ void RasterizerSceneGLES3::_fill_render_list(RenderListType p_render_list, const
 					int32_t shadow_id = light_storage->light_instance_get_shadow_id(light_instance);
 
 					if (light_storage->light_has_shadow(light) && shadow_id >= 0) {
-						// Skip static lights when a lightmap is used.
-						if (!inst->lightmap_instance.is_valid() || light_storage->light_get_bake_mode(light) != RenderingServer::LIGHT_BAKE_STATIC) {
-							GeometryInstanceGLES3::LightPass pass;
-							pass.light_id = light_storage->light_instance_get_gl_id(light_instance);
-							pass.shadow_id = shadow_id;
-							pass.light_instance_rid = light_instance;
-							inst->light_passes.push_back(pass);
-						}
+						GeometryInstanceGLES3::LightPass pass;
+						pass.light_id = light_storage->light_instance_get_gl_id(light_instance);
+						pass.shadow_id = shadow_id;
+						pass.light_instance_rid = light_instance;
+						inst->light_passes.push_back(pass);
 					} else {
 						// Lights without shadow can all go in base pass.
 						inst->spot_light_gl_cache.push_back((uint32_t)light_storage->light_instance_get_gl_id(light_instance));
diff --git a/drivers/gles3/shaders/scene.glsl b/drivers/gles3/shaders/scene.glsl
index 2022c8ee43..fd195fe7c4 100644
--- a/drivers/gles3/shaders/scene.glsl
+++ b/drivers/gles3/shaders/scene.glsl
@@ -275,6 +275,10 @@ struct DirectionalLightData {
 layout(std140) uniform DirectionalLights { // ubo:7
 	DirectionalLightData directional_lights[MAX_DIRECTIONAL_LIGHT_DATA_STRUCTS];
 };
+
+#define LIGHT_BAKE_DISABLED 0u
+#define LIGHT_BAKE_STATIC 1u
+#define LIGHT_BAKE_DYNAMIC 2u
 #endif // !DISABLE_LIGHT_DIRECTIONAL
 
 // Omni and spot light data.
@@ -723,6 +727,11 @@ void main() {
 #ifdef BASE_PASS
 #ifndef DISABLE_LIGHT_DIRECTIONAL
 	for (uint i = uint(0); i < scene_data.directional_light_count; i++) {
+#if defined(USE_LIGHTMAP) && !defined(DISABLE_LIGHTMAP)
+		if (directional_lights[i].bake_mode == LIGHT_BAKE_STATIC) {
+			continue;
+		}
+#endif
 		light_compute(normal_interp, normalize(directional_lights[i].direction), normalize(view), directional_lights[i].color * directional_lights[i].energy, true, roughness,
 				diffuse_light_interp.rgb,
 				specular_light_interp.rgb);
@@ -2138,11 +2147,7 @@ void main() {
 		if (i >= omni_light_count) {
 			break;
 		}
-#if defined(USE_LIGHTMAP) && !defined(DISABLE_LIGHTMAP)
-		if (omni_lights[omni_light_indices[i]].bake_mode == LIGHT_BAKE_STATIC) {
-			continue;
-		}
-#endif
+
 		light_process_omni(omni_light_indices[i], vertex, view, normal, f0, roughness, metallic, 1.0, albedo, alpha, screen_uv,
 #ifdef LIGHT_BACKLIGHT_USED
 				backlight,
@@ -2166,11 +2171,7 @@ void main() {
 		if (i >= spot_light_count) {
 			break;
 		}
-#if defined(USE_LIGHTMAP) && !defined(DISABLE_LIGHTMAP)
-		if (spot_lights[spot_light_indices[i]].bake_mode == LIGHT_BAKE_STATIC) {
-			continue;
-		}
-#endif
+
 		light_process_spot(spot_light_indices[i], vertex, view, normal, f0, roughness, metallic, 1.0, albedo, alpha, screen_uv,
 #ifdef LIGHT_BACKLIGHT_USED
 				backlight,
diff --git a/servers/rendering/renderer_rd/shaders/forward_clustered/scene_forward_clustered.glsl b/servers/rendering/renderer_rd/shaders/forward_clustered/scene_forward_clustered.glsl
index 52b3d5d1e3..5b4d746b52 100644
--- a/servers/rendering/renderer_rd/shaders/forward_clustered/scene_forward_clustered.glsl
+++ b/servers/rendering/renderer_rd/shaders/forward_clustered/scene_forward_clustered.glsl
@@ -2211,6 +2211,10 @@ void fragment_shader(in SceneData scene_data) {
 				continue; //not masked
 			}
 
+			if (directional_lights.data[i].bake_mode == LIGHT_BAKE_STATIC && bool(instances.data[instance_index].flags & INSTANCE_FLAGS_USE_LIGHTMAP)) {
+				continue; // Statically baked light and object uses lightmap, skip
+			}
+
 #ifdef LIGHT_TRANSMITTANCE_USED
 			float transmittance_z = transmittance_depth;
 #ifndef SHADOWS_DISABLED
diff --git a/servers/rendering/renderer_rd/shaders/forward_mobile/scene_forward_mobile.glsl b/servers/rendering/renderer_rd/shaders/forward_mobile/scene_forward_mobile.glsl
index 144b994a52..dbca038d3a 100644
--- a/servers/rendering/renderer_rd/shaders/forward_mobile/scene_forward_mobile.glsl
+++ b/servers/rendering/renderer_rd/shaders/forward_mobile/scene_forward_mobile.glsl
@@ -1445,6 +1445,10 @@ void main() {
 				continue; //not masked
 			}
 
+			if (directional_lights.data[i].bake_mode == LIGHT_BAKE_STATIC && bool(instances.data[draw_call.instance_index].flags & INSTANCE_FLAGS_USE_LIGHTMAP)) {
+				continue; // Statically baked light and object uses lightmap, skip.
+			}
+
 			float shadow = 1.0;
 
 			if (directional_lights.data[i].shadow_opacity > 0.001) {
@@ -1560,6 +1564,10 @@ void main() {
 				continue; //not masked
 			}
 
+			if (directional_lights.data[i].bake_mode == LIGHT_BAKE_STATIC && bool(instances.data[draw_call.instance_index].flags & INSTANCE_FLAGS_USE_LIGHTMAP)) {
+				continue; // Statically baked light and object uses lightmap, skip.
+			}
+
 			// We're not doing light transmittence
 
 			float shadow = 1.0;
diff --git a/servers/rendering/renderer_rd/storage_rd/light_storage.cpp b/servers/rendering/renderer_rd/storage_rd/light_storage.cpp
index 1d309a8006..a812c894b9 100644
--- a/servers/rendering/renderer_rd/storage_rd/light_storage.cpp
+++ b/servers/rendering/renderer_rd/storage_rd/light_storage.cpp
@@ -676,12 +676,13 @@ void LightStorage::update_light_buffers(RenderDataRD *p_render_data, const Paged
 					angular_diameter = 0.0;
 				}
 
+				light_data.bake_mode = light->bake_mode;
+
 				if (light_data.shadow_opacity > 0.001) {
 					RS::LightDirectionalShadowMode smode = light->directional_shadow_mode;
 
 					light_data.soft_shadow_scale = light->param[RS::LIGHT_PARAM_SHADOW_BLUR];
 					light_data.softshadow_angle = angular_diameter;
-					light_data.bake_mode = light->bake_mode;
 
 					if (angular_diameter <= 0.0) {
 						light_data.soft_shadow_scale *= RendererSceneRenderRD::get_singleton()->directional_shadow_quality_radius_get(); // Only use quality radius for PCF
diff --git a/servers/rendering/renderer_scene_cull.cpp b/servers/rendering/renderer_scene_cull.cpp
index af17a35535..1fcbcb9853 100644
--- a/servers/rendering/renderer_scene_cull.cpp
+++ b/servers/rendering/renderer_scene_cull.cpp
@@ -3044,6 +3044,10 @@ void RendererSceneCull::_scene_cull(CullData &cull_data, InstanceCullResult &cul
 								continue;
 							}
 
+							if ((RSG::light_storage->light_get_bake_mode(E->base) == RS::LIGHT_BAKE_STATIC) && idata.instance->lightmap) {
+								continue;
+							}
+
 							instance_pair_buffer[idx++] = light->instance;
 							if (idx == MAX_INSTANCE_PAIRS) {
 								break;

From 5601e4c18c2497d41a4f748f9cb3e503ce7d8b33 Mon Sep 17 00:00:00 2001
From: Allen Pestaluky <allenpestaluky@gmail.com>
Date: Thu, 5 Dec 2024 12:20:14 -0500
Subject: [PATCH 034/124] Forced fixed undo history to make editor shortcuts
 use global history. Fixes #76851.

---
 editor/editor_settings_dialog.cpp | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/editor/editor_settings_dialog.cpp b/editor/editor_settings_dialog.cpp
index 8989b9cf9b..1149d91ebd 100644
--- a/editor/editor_settings_dialog.cpp
+++ b/editor/editor_settings_dialog.cpp
@@ -332,7 +332,10 @@ void EditorSettingsDialog::_update_shortcut_events(const String &p_path, const A
 	Ref<Shortcut> current_sc = EditorSettings::get_singleton()->get_shortcut(p_path);
 
 	EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton();
-	undo_redo->create_action(vformat(TTR("Edit Shortcut: %s"), p_path));
+	undo_redo->create_action(vformat(TTR("Edit Shortcut: %s"), p_path), UndoRedo::MERGE_DISABLE, EditorSettings::get_singleton());
+	// History must be fixed based on the EditorSettings object because current_sc would
+	// incorrectly make this action use the scene history.
+	undo_redo->force_fixed_history();
 	undo_redo->add_do_method(current_sc.ptr(), "set_events", p_events);
 	undo_redo->add_undo_method(current_sc.ptr(), "set_events", current_sc->get_events());
 	undo_redo->add_do_method(EditorSettings::get_singleton(), "mark_setting_changed", "shortcuts");

From c153c5b3cf288f9e378d29b4756fa3132302fffc Mon Sep 17 00:00:00 2001
From: Alessandro <alessandrogenovese1@hotmail.com>
Date: Fri, 6 Dec 2024 17:39:31 -0500
Subject: [PATCH 035/124] Moved uniform variables up so that they are available
 within the #GLOBAL scope

---
 drivers/gles3/shaders/scene.glsl | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/drivers/gles3/shaders/scene.glsl b/drivers/gles3/shaders/scene.glsl
index 2022c8ee43..1867930b29 100644
--- a/drivers/gles3/shaders/scene.glsl
+++ b/drivers/gles3/shaders/scene.glsl
@@ -974,6 +974,10 @@ layout(std140) uniform MultiviewData { // ubo:8
 multiview_data;
 #endif
 
+uniform highp mat4 world_transform;
+uniform highp uint instance_offset;
+uniform highp uint model_flags;
+
 /* clang-format off */
 
 #GLOBALS
@@ -1199,10 +1203,7 @@ ivec2 multiview_uv(ivec2 uv) {
 }
 #endif
 
-uniform highp mat4 world_transform;
 uniform mediump float opaque_prepass_threshold;
-uniform highp uint model_flags;
-uniform highp uint instance_offset;
 
 #if defined(RENDER_MATERIAL)
 layout(location = 0) out vec4 albedo_output_buffer;

From 72650f9787f880353245a0630ba5ba25f814fe71 Mon Sep 17 00:00:00 2001
From: YoSoyFreeman <yosoyfreeman@gmail.com>
Date: Mon, 18 Nov 2024 15:53:52 +0100
Subject: [PATCH 036/124] Allow apply_floor_snap to preserve the horizontal
 position after the snapping independently of stop_on_slopes

---
 scene/2d/physics/character_body_2d.cpp | 16 ++++++++--------
 scene/3d/physics/character_body_3d.cpp | 16 ++++++++--------
 2 files changed, 16 insertions(+), 16 deletions(-)

diff --git a/scene/2d/physics/character_body_2d.cpp b/scene/2d/physics/character_body_2d.cpp
index a503f3cb78..32582cc120 100644
--- a/scene/2d/physics/character_body_2d.cpp
+++ b/scene/2d/physics/character_body_2d.cpp
@@ -352,14 +352,14 @@ void CharacterBody2D::_apply_floor_snap(bool p_wall_as_floor) {
 			floor_normal = result.collision_normal;
 			_set_platform_data(result);
 
-			if (floor_stop_on_slope) {
-				// move and collide may stray the object a bit because of pre un-stucking,
-				// so only ensure that motion happens on floor direction in this case.
-				if (result.travel.length() > margin) {
-					result.travel = up_direction * up_direction.dot(result.travel);
-				} else {
-					result.travel = Vector2();
-				}
+			// Ensure that we only move the body along the up axis, because
+			// move_and_collide may stray the object a bit when getting it unstuck.
+			// Canceling this motion should not affect move_and_slide, as previous
+			// calls to move_and_collide already took care of freeing the body.
+			if (result.travel.length() > margin) {
+				result.travel = up_direction * up_direction.dot(result.travel);
+			} else {
+				result.travel = Vector2();
 			}
 
 			parameters.from.columns[2] += result.travel;
diff --git a/scene/3d/physics/character_body_3d.cpp b/scene/3d/physics/character_body_3d.cpp
index e3815e8219..1aea2cbd9e 100644
--- a/scene/3d/physics/character_body_3d.cpp
+++ b/scene/3d/physics/character_body_3d.cpp
@@ -469,14 +469,14 @@ void CharacterBody3D::apply_floor_snap() {
 		_set_collision_direction(result, result_state, CollisionState(true, false, false));
 
 		if (result_state.floor) {
-			if (floor_stop_on_slope) {
-				// move and collide may stray the object a bit because of pre un-stucking,
-				// so only ensure that motion happens on floor direction in this case.
-				if (result.travel.length() > margin) {
-					result.travel = up_direction * up_direction.dot(result.travel);
-				} else {
-					result.travel = Vector3();
-				}
+			// Ensure that we only move the body along the up axis, because
+			// move_and_collide may stray the object a bit when getting it unstuck.
+			// Canceling this motion should not affect move_and_slide, as previous
+			// calls to move_and_collide already took care of freeing the body.
+			if (result.travel.length() > margin) {
+				result.travel = up_direction * up_direction.dot(result.travel);
+			} else {
+				result.travel = Vector3();
 			}
 
 			parameters.from.origin += result.travel;

From 00a791f04e9b936e158172cd78984cd4b485b0dd Mon Sep 17 00:00:00 2001
From: ArchercatNEO <tururu.pompella@gmail.com>
Date: Sat, 7 Dec 2024 17:31:22 +0000
Subject: [PATCH 037/124] Use temp dirs instead of cache dirs for export

Fixes #95897
During CI scenarios $HOME may be set to an invalid value (such as
`/var/empty`).
Using temp dirs fits better with godot's usage of these paths and is
independent from the user's $HOME.
---
 editor/export/codesign.cpp                 |  4 +--
 editor/export/editor_export_platform.cpp   |  8 +++---
 platform/android/export/export_plugin.cpp  |  4 +--
 platform/ios/export/export_plugin.cpp      | 32 +++++++++++-----------
 platform/linuxbsd/export/export_plugin.cpp |  4 +--
 platform/macos/export/export_plugin.cpp    | 10 +++----
 platform/web/export/editor_http_server.cpp |  2 +-
 platform/web/export/export_plugin.cpp      |  2 +-
 platform/windows/export/export_plugin.cpp  |  6 ++--
 9 files changed, 36 insertions(+), 36 deletions(-)

diff --git a/editor/export/codesign.cpp b/editor/export/codesign.cpp
index cc53068d48..f02895fadf 100644
--- a/editor/export/codesign.cpp
+++ b/editor/export/codesign.cpp
@@ -214,7 +214,7 @@ bool CodeSignCodeResources::add_nested_file(const String &p_root, const String &
 
 	Vector<String> files_to_add;
 	if (LipO::is_lipo(p_exepath)) {
-		String tmp_path_name = EditorPaths::get_singleton()->get_cache_dir().path_join("_lipo");
+		String tmp_path_name = EditorPaths::get_singleton()->get_temp_dir().path_join("_lipo");
 		Error err = da->make_dir_recursive(tmp_path_name);
 		ERR_FAIL_COND_V_MSG(err != OK, false, vformat("CodeSign/CodeResources: Failed to create \"%s\" subfolder.", tmp_path_name));
 		LipO lip;
@@ -1248,7 +1248,7 @@ Error CodeSign::_codesign_file(bool p_use_hardened_runtime, bool p_force, const
 	Vector<String> files_to_sign;
 	if (LipO::is_lipo(main_exe)) {
 		print_verbose(vformat("CodeSign: Executable is fat, extracting..."));
-		String tmp_path_name = EditorPaths::get_singleton()->get_cache_dir().path_join("_lipo");
+		String tmp_path_name = EditorPaths::get_singleton()->get_temp_dir().path_join("_lipo");
 		Error err = da->make_dir_recursive(tmp_path_name);
 		if (err != OK) {
 			r_error_msg = vformat(TTR("Failed to create \"%s\" subfolder."), tmp_path_name);
diff --git a/editor/export/editor_export_platform.cpp b/editor/export/editor_export_platform.cpp
index 72fa20f671..cb496fc9f0 100644
--- a/editor/export/editor_export_platform.cpp
+++ b/editor/export/editor_export_platform.cpp
@@ -1528,7 +1528,7 @@ Error EditorExportPlatform::export_project_files(const Ref<EditorExportPreset> &
 	}
 
 	String config_file = "project.binary";
-	String engine_cfb = EditorPaths::get_singleton()->get_cache_dir().path_join("tmp" + config_file);
+	String engine_cfb = EditorPaths::get_singleton()->get_temp_dir().path_join("tmp" + config_file);
 	ProjectSettings::get_singleton()->save_custom(engine_cfb, custom_map, custom_list);
 	Vector<uint8_t> data = FileAccess::get_file_as_bytes(engine_cfb);
 	DirAccess::remove_file_or_error(engine_cfb);
@@ -1832,9 +1832,9 @@ Error EditorExportPlatform::save_pack(const Ref<EditorExportPreset> &p_preset, b
 
 	// Create the temporary export directory if it doesn't exist.
 	Ref<DirAccess> da = DirAccess::create(DirAccess::ACCESS_FILESYSTEM);
-	da->make_dir_recursive(EditorPaths::get_singleton()->get_cache_dir());
+	da->make_dir_recursive(EditorPaths::get_singleton()->get_temp_dir());
 
-	String tmppath = EditorPaths::get_singleton()->get_cache_dir().path_join("packtmp");
+	String tmppath = EditorPaths::get_singleton()->get_temp_dir().path_join("packtmp");
 	Ref<FileAccess> ftmp = FileAccess::open(tmppath, FileAccess::WRITE);
 	if (ftmp.is_null()) {
 		add_message(EXPORT_MESSAGE_ERROR, TTR("Save PCK"), vformat(TTR("Cannot create file \"%s\"."), tmppath));
@@ -2094,7 +2094,7 @@ Error EditorExportPlatform::save_zip(const Ref<EditorExportPreset> &p_preset, bo
 		p_save_func = _save_zip_file;
 	}
 
-	String tmppath = EditorPaths::get_singleton()->get_cache_dir().path_join("packtmp");
+	String tmppath = EditorPaths::get_singleton()->get_temp_dir().path_join("packtmp");
 
 	Ref<FileAccess> io_fa;
 	zlib_filefunc_def io = zipio_create_io(&io_fa);
diff --git a/platform/android/export/export_plugin.cpp b/platform/android/export/export_plugin.cpp
index 293044a8fa..cc0f75802b 100644
--- a/platform/android/export/export_plugin.cpp
+++ b/platform/android/export/export_plugin.cpp
@@ -2085,7 +2085,7 @@ Error EditorExportPlatformAndroid::run(const Ref<EditorExportPreset> &p_preset,
 		p_debug_flags.set_flag(DEBUG_FLAG_REMOTE_DEBUG_LOCALHOST);
 	}
 
-	String tmp_export_path = EditorPaths::get_singleton()->get_cache_dir().path_join("tmpexport." + uitos(OS::get_singleton()->get_unix_time()) + ".apk");
+	String tmp_export_path = EditorPaths::get_singleton()->get_temp_dir().path_join("tmpexport." + uitos(OS::get_singleton()->get_unix_time()) + ".apk");
 
 #define CLEANUP_AND_RETURN(m_err)                         \
 	{                                                     \
@@ -3445,7 +3445,7 @@ Error EditorExportPlatformAndroid::export_project_helper(const Ref<EditorExportP
 	Ref<FileAccess> io2_fa;
 	zlib_filefunc_def io2 = zipio_create_io(&io2_fa);
 
-	String tmp_unaligned_path = EditorPaths::get_singleton()->get_cache_dir().path_join("tmpexport-unaligned." + uitos(OS::get_singleton()->get_unix_time()) + ".apk");
+	String tmp_unaligned_path = EditorPaths::get_singleton()->get_temp_dir().path_join("tmpexport-unaligned." + uitos(OS::get_singleton()->get_unix_time()) + ".apk");
 
 #define CLEANUP_AND_RETURN(m_err)                            \
 	{                                                        \
diff --git a/platform/ios/export/export_plugin.cpp b/platform/ios/export/export_plugin.cpp
index 742127a10e..667c883e38 100644
--- a/platform/ios/export/export_plugin.cpp
+++ b/platform/ios/export/export_plugin.cpp
@@ -2433,7 +2433,7 @@ Error EditorExportPlatformIOS::_export_project_helper(const Ref<EditorExportPres
 	if (p_preset->get("application/generate_simulator_library_if_missing").operator bool()) {
 		String sim_lib_path = dest_dir + String(binary_name + ".xcframework").path_join("ios-arm64_x86_64-simulator").path_join("libgodot.a");
 		String dev_lib_path = dest_dir + String(binary_name + ".xcframework").path_join("ios-arm64").path_join("libgodot.a");
-		String tmp_lib_path = EditorPaths::get_singleton()->get_cache_dir().path_join(binary_name + "_lipo_");
+		String tmp_lib_path = EditorPaths::get_singleton()->get_temp_dir().path_join(binary_name + "_lipo_");
 		uint32_t cputype = 0;
 		uint32_t cpusubtype = 0;
 		if (!_archive_has_arm64(sim_lib_path, &cputype, &cpusubtype) && _archive_has_arm64(dev_lib_path) && FileAccess::exists(dev_lib_path)) {
@@ -3111,19 +3111,19 @@ Error EditorExportPlatformIOS::run(const Ref<EditorExportPreset> &p_preset, int
 	String id = "tmpexport." + uitos(OS::get_singleton()->get_unix_time());
 
 	Ref<DirAccess> filesystem_da = DirAccess::create(DirAccess::ACCESS_FILESYSTEM);
-	ERR_FAIL_COND_V_MSG(filesystem_da.is_null(), ERR_CANT_CREATE, "Cannot create DirAccess for path '" + EditorPaths::get_singleton()->get_cache_dir() + "'.");
-	filesystem_da->make_dir_recursive(EditorPaths::get_singleton()->get_cache_dir().path_join(id));
-	String tmp_export_path = EditorPaths::get_singleton()->get_cache_dir().path_join(id).path_join("export.ipa");
+	ERR_FAIL_COND_V_MSG(filesystem_da.is_null(), ERR_CANT_CREATE, "Cannot create DirAccess for path '" + EditorPaths::get_singleton()->get_temp_dir() + "'.");
+	filesystem_da->make_dir_recursive(EditorPaths::get_singleton()->get_temp_dir().path_join(id));
+	String tmp_export_path = EditorPaths::get_singleton()->get_temp_dir().path_join(id).path_join("export.ipa");
 
-#define CLEANUP_AND_RETURN(m_err)                                                                           \
-	{                                                                                                       \
-		if (filesystem_da->change_dir(EditorPaths::get_singleton()->get_cache_dir().path_join(id)) == OK) { \
-			filesystem_da->erase_contents_recursive();                                                      \
-			filesystem_da->change_dir("..");                                                                \
-			filesystem_da->remove(id);                                                                      \
-		}                                                                                                   \
-		return m_err;                                                                                       \
-	}                                                                                                       \
+#define CLEANUP_AND_RETURN(m_err)                                                                          \
+	{                                                                                                      \
+		if (filesystem_da->change_dir(EditorPaths::get_singleton()->get_temp_dir().path_join(id)) == OK) { \
+			filesystem_da->erase_contents_recursive();                                                     \
+			filesystem_da->change_dir("..");                                                               \
+			filesystem_da->remove(id);                                                                     \
+		}                                                                                                  \
+		return m_err;                                                                                      \
+	}                                                                                                      \
 	((void)0)
 
 	Device dev = devices[p_device];
@@ -3193,7 +3193,7 @@ Error EditorExportPlatformIOS::run(const Ref<EditorExportPreset> &p_preset, int
 			args.push_back("simctl");
 			args.push_back("install");
 			args.push_back(dev.id);
-			args.push_back(EditorPaths::get_singleton()->get_cache_dir().path_join(id).path_join("export.xcarchive/Products/Applications/export.app"));
+			args.push_back(EditorPaths::get_singleton()->get_temp_dir().path_join(id).path_join("export.xcarchive/Products/Applications/export.app"));
 
 			String log;
 			int ec;
@@ -3246,7 +3246,7 @@ Error EditorExportPlatformIOS::run(const Ref<EditorExportPreset> &p_preset, int
 			args.push_back(dev.id);
 			args.push_back("--justlaunch");
 			args.push_back("--bundle");
-			args.push_back(EditorPaths::get_singleton()->get_cache_dir().path_join(id).path_join("export.xcarchive/Products/Applications/export.app"));
+			args.push_back(EditorPaths::get_singleton()->get_temp_dir().path_join(id).path_join("export.xcarchive/Products/Applications/export.app"));
 			String app_args;
 			for (const String &E : cmd_args_list) {
 				app_args += E + " ";
@@ -3285,7 +3285,7 @@ Error EditorExportPlatformIOS::run(const Ref<EditorExportPreset> &p_preset, int
 			args.push_back("app");
 			args.push_back("-d");
 			args.push_back(dev.id);
-			args.push_back(EditorPaths::get_singleton()->get_cache_dir().path_join(id).path_join("export.xcarchive/Products/Applications/export.app"));
+			args.push_back(EditorPaths::get_singleton()->get_temp_dir().path_join(id).path_join("export.xcarchive/Products/Applications/export.app"));
 
 			String log;
 			int ec;
diff --git a/platform/linuxbsd/export/export_plugin.cpp b/platform/linuxbsd/export/export_plugin.cpp
index 7cd77dd593..9362aee5e4 100644
--- a/platform/linuxbsd/export/export_plugin.cpp
+++ b/platform/linuxbsd/export/export_plugin.cpp
@@ -88,7 +88,7 @@ Error EditorExportPlatformLinuxBSD::export_project(const Ref<EditorExportPreset>
 
 	// Setup temp folder.
 	String path = p_path;
-	String tmp_dir_path = EditorPaths::get_singleton()->get_cache_dir().path_join(pkg_name);
+	String tmp_dir_path = EditorPaths::get_singleton()->get_temp_dir().path_join(pkg_name);
 
 	Ref<DirAccess> tmp_app_dir = DirAccess::create_for_path(tmp_dir_path);
 	if (export_as_zip) {
@@ -468,7 +468,7 @@ Error EditorExportPlatformLinuxBSD::run(const Ref<EditorExportPreset> &p_preset,
 
 	EditorProgress ep("run", TTR("Running..."), 5);
 
-	const String dest = EditorPaths::get_singleton()->get_cache_dir().path_join("linuxbsd");
+	const String dest = EditorPaths::get_singleton()->get_temp_dir().path_join("linuxbsd");
 	Ref<DirAccess> da = DirAccess::create(DirAccess::ACCESS_FILESYSTEM);
 	if (!da->dir_exists(dest)) {
 		Error err = da->make_dir_recursive(dest);
diff --git a/platform/macos/export/export_plugin.cpp b/platform/macos/export/export_plugin.cpp
index b9f9d8d613..d795adbe33 100644
--- a/platform/macos/export/export_plugin.cpp
+++ b/platform/macos/export/export_plugin.cpp
@@ -1592,7 +1592,7 @@ Error EditorExportPlatformMacOS::export_project(const Ref<EditorExportPreset> &p
 		tmp_app_path_name = p_path;
 		scr_path = p_path.get_basename() + ".command";
 	} else {
-		tmp_base_path_name = EditorPaths::get_singleton()->get_cache_dir().path_join(pkg_name);
+		tmp_base_path_name = EditorPaths::get_singleton()->get_temp_dir().path_join(pkg_name);
 		tmp_app_path_name = tmp_base_path_name.path_join(tmp_app_dir_name);
 		scr_path = tmp_base_path_name.path_join(pkg_name + ".command");
 	}
@@ -1987,9 +1987,9 @@ Error EditorExportPlatformMacOS::export_project(const Ref<EditorExportPreset> &p
 
 		bool sandbox = p_preset->get("codesign/entitlements/app_sandbox/enabled");
 		String ent_path = p_preset->get("codesign/entitlements/custom_file");
-		String hlp_ent_path = sandbox ? EditorPaths::get_singleton()->get_cache_dir().path_join(pkg_name + "_helper.entitlements") : ent_path;
+		String hlp_ent_path = sandbox ? EditorPaths::get_singleton()->get_temp_dir().path_join(pkg_name + "_helper.entitlements") : ent_path;
 		if (sign_enabled && (ent_path.is_empty())) {
-			ent_path = EditorPaths::get_singleton()->get_cache_dir().path_join(pkg_name + ".entitlements");
+			ent_path = EditorPaths::get_singleton()->get_temp_dir().path_join(pkg_name + ".entitlements");
 
 			Ref<FileAccess> ent_f = FileAccess::open(ent_path, FileAccess::WRITE);
 			if (ent_f.is_valid()) {
@@ -2269,7 +2269,7 @@ Error EditorExportPlatformMacOS::export_project(const Ref<EditorExportPreset> &p
 		} else if (export_format == "app" && noto_enabled) {
 			// Create temporary ZIP.
 			if (err == OK) {
-				noto_path = EditorPaths::get_singleton()->get_cache_dir().path_join(pkg_name + ".zip");
+				noto_path = EditorPaths::get_singleton()->get_temp_dir().path_join(pkg_name + ".zip");
 
 				if (ep.step(TTR("Making ZIP"), 3)) {
 					return ERR_SKIP;
@@ -2554,7 +2554,7 @@ Error EditorExportPlatformMacOS::run(const Ref<EditorExportPreset> &p_preset, in
 
 	EditorProgress ep("run", TTR("Running..."), 5);
 
-	const String dest = EditorPaths::get_singleton()->get_cache_dir().path_join("macos");
+	const String dest = EditorPaths::get_singleton()->get_temp_dir().path_join("macos");
 	Ref<DirAccess> da = DirAccess::create(DirAccess::ACCESS_FILESYSTEM);
 	if (!da->dir_exists(dest)) {
 		Error err = da->make_dir_recursive(dest);
diff --git a/platform/web/export/editor_http_server.cpp b/platform/web/export/editor_http_server.cpp
index 9cf862eb1e..26b9a626db 100644
--- a/platform/web/export/editor_http_server.cpp
+++ b/platform/web/export/editor_http_server.cpp
@@ -86,7 +86,7 @@ void EditorHTTPServer::_send_response() {
 
 	const String req_file = path.get_file();
 	const String req_ext = path.get_extension();
-	const String cache_path = EditorPaths::get_singleton()->get_cache_dir().path_join("web");
+	const String cache_path = EditorPaths::get_singleton()->get_temp_dir().path_join("web");
 	const String filepath = cache_path.path_join(req_file);
 
 	if (!mimes.has(req_ext) || !FileAccess::exists(filepath)) {
diff --git a/platform/web/export/export_plugin.cpp b/platform/web/export/export_plugin.cpp
index 9e60a76fdc..9d212d81ff 100644
--- a/platform/web/export/export_plugin.cpp
+++ b/platform/web/export/export_plugin.cpp
@@ -817,7 +817,7 @@ Error EditorExportPlatformWeb::run(const Ref<EditorExportPreset> &p_preset, int
 }
 
 Error EditorExportPlatformWeb::_export_project(const Ref<EditorExportPreset> &p_preset, int p_debug_flags) {
-	const String dest = EditorPaths::get_singleton()->get_cache_dir().path_join("web");
+	const String dest = EditorPaths::get_singleton()->get_temp_dir().path_join("web");
 	Ref<DirAccess> da = DirAccess::create(DirAccess::ACCESS_FILESYSTEM);
 	if (!da->dir_exists(dest)) {
 		Error err = da->make_dir_recursive(dest);
diff --git a/platform/windows/export/export_plugin.cpp b/platform/windows/export/export_plugin.cpp
index 8d3f4bb269..65fbc59310 100644
--- a/platform/windows/export/export_plugin.cpp
+++ b/platform/windows/export/export_plugin.cpp
@@ -257,7 +257,7 @@ Error EditorExportPlatformWindows::export_project(const Ref<EditorExportPreset>
 
 	// Setup temp folder.
 	String path = p_path;
-	String tmp_dir_path = EditorPaths::get_singleton()->get_cache_dir().path_join(pkg_name);
+	String tmp_dir_path = EditorPaths::get_singleton()->get_temp_dir().path_join(pkg_name);
 	Ref<DirAccess> tmp_app_dir = DirAccess::create_for_path(tmp_dir_path);
 	if (export_as_zip) {
 		if (tmp_app_dir.is_null()) {
@@ -501,7 +501,7 @@ Error EditorExportPlatformWindows::_rcedit_add_data(const Ref<EditorExportPreset
 		}
 	}
 
-	String tmp_icon_path = EditorPaths::get_singleton()->get_cache_dir().path_join("_rcedit.ico");
+	String tmp_icon_path = EditorPaths::get_singleton()->get_temp_dir().path_join("_rcedit.ico");
 	if (!icon_path.is_empty()) {
 		if (_process_icon(p_preset, icon_path, tmp_icon_path) != OK) {
 			add_message(EXPORT_MESSAGE_WARNING, TTR("Resources Modification"), vformat(TTR("Invalid icon file \"%s\"."), icon_path));
@@ -1004,7 +1004,7 @@ Error EditorExportPlatformWindows::run(const Ref<EditorExportPreset> &p_preset,
 
 	EditorProgress ep("run", TTR("Running..."), 5);
 
-	const String dest = EditorPaths::get_singleton()->get_cache_dir().path_join("windows");
+	const String dest = EditorPaths::get_singleton()->get_temp_dir().path_join("windows");
 	Ref<DirAccess> da = DirAccess::create(DirAccess::ACCESS_FILESYSTEM);
 	if (!da->dir_exists(dest)) {
 		Error err = da->make_dir_recursive(dest);

From 875b48366c0b03c74bd9473fec6b439ff1268132 Mon Sep 17 00:00:00 2001
From: Lukas Tenbrink <lukas.tenbrink@gmail.com>
Date: Thu, 5 Dec 2024 13:24:05 +0100
Subject: [PATCH 038/124] Optimize String.similarity by avoiding allocation for
 bigrams.

---
 core/string/ustring.cpp | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/core/string/ustring.cpp b/core/string/ustring.cpp
index 0e553a5680..ff9ae07328 100644
--- a/core/string/ustring.cpp
+++ b/core/string/ustring.cpp
@@ -4002,17 +4002,17 @@ float String::similarity(const String &p_string) const {
 		return 0.0f;
 	}
 
-	Vector<String> src_bigrams = bigrams();
-	Vector<String> tgt_bigrams = p_string.bigrams();
+	const int src_size = length() - 1;
+	const int tgt_size = p_string.length() - 1;
 
-	int src_size = src_bigrams.size();
-	int tgt_size = tgt_bigrams.size();
-
-	int sum = src_size + tgt_size;
+	const int sum = src_size + tgt_size;
 	int inter = 0;
 	for (int i = 0; i < src_size; i++) {
+		const char32_t i0 = get(i);
+		const char32_t i1 = get(i + 1);
+
 		for (int j = 0; j < tgt_size; j++) {
-			if (src_bigrams[i] == tgt_bigrams[j]) {
+			if (i0 == p_string.get(j) && i1 == p_string.get(j + 1)) {
 				inter++;
 				break;
 			}

From 3866a7f818d3975d6051a59f116e4f196b261005 Mon Sep 17 00:00:00 2001
From: David Snopek <dsnopek@gmail.com>
Date: Fri, 6 Dec 2024 16:58:50 -0600
Subject: [PATCH 039/124] Add missing `GDVIRTUAL_BIND()` for
 `AudioStream::_has_loop()` and `::_get_bar_beats()`

---
 doc/classes/AudioStream.xml    | 12 ++++++++++++
 servers/audio/audio_stream.cpp |  2 ++
 2 files changed, 14 insertions(+)

diff --git a/doc/classes/AudioStream.xml b/doc/classes/AudioStream.xml
index d11e070d89..52e9951281 100644
--- a/doc/classes/AudioStream.xml
+++ b/doc/classes/AudioStream.xml
@@ -13,6 +13,12 @@
 		<link title="Audio Spectrum Visualizer Demo">https://godotengine.org/asset-library/asset/2762</link>
 	</tutorials>
 	<methods>
+		<method name="_get_bar_beats" qualifiers="virtual const">
+			<return type="int" />
+			<description>
+				Override this method to return the bar beats of this stream.
+			</description>
+		</method>
 		<method name="_get_beat_count" qualifiers="virtual const">
 			<return type="int" />
 			<description>
@@ -45,6 +51,12 @@
 				Override this method to customize the name assigned to this audio stream. Unused by the engine.
 			</description>
 		</method>
+		<method name="_has_loop" qualifiers="virtual const">
+			<return type="bool" />
+			<description>
+				Override this method to return [code]true[/code] if this stream has a loop.
+			</description>
+		</method>
 		<method name="_instantiate_playback" qualifiers="virtual const">
 			<return type="AudioStreamPlayback" />
 			<description>
diff --git a/servers/audio/audio_stream.cpp b/servers/audio/audio_stream.cpp
index d5c37c2acc..70f005930b 100644
--- a/servers/audio/audio_stream.cpp
+++ b/servers/audio/audio_stream.cpp
@@ -352,6 +352,8 @@ void AudioStream::_bind_methods() {
 	GDVIRTUAL_BIND(_get_bpm)
 	GDVIRTUAL_BIND(_get_beat_count)
 	GDVIRTUAL_BIND(_get_parameter_list)
+	GDVIRTUAL_BIND(_has_loop);
+	GDVIRTUAL_BIND(_get_bar_beats);
 
 	ADD_SIGNAL(MethodInfo("parameter_list_changed"));
 }

From 8df2dbe2f6e95852c858d6831fa8e8ef04455f4a Mon Sep 17 00:00:00 2001
From: Lukas Tenbrink <lukas.tenbrink@gmail.com>
Date: Thu, 28 Nov 2024 01:20:58 +0100
Subject: [PATCH 040/124] Optimize StringBuilder append for static strings, and
 as_string().

---
 core/string/string_builder.cpp | 10 ++++++----
 core/string/string_builder.h   |  8 ++++----
 2 files changed, 10 insertions(+), 8 deletions(-)

diff --git a/core/string/string_builder.cpp b/core/string/string_builder.cpp
index 4f74a2e42d..7f0a6abc04 100644
--- a/core/string/string_builder.cpp
+++ b/core/string/string_builder.cpp
@@ -68,8 +68,10 @@ String StringBuilder::as_string() const {
 	int godot_string_elem = 0;
 	int c_string_elem = 0;
 
-	for (int i = 0; i < appended_strings.size(); i++) {
-		if (appended_strings[i] == -1) {
+	for (uint32_t i = 0; i < appended_strings.size(); i++) {
+		const int32_t str_len = appended_strings[i];
+
+		if (str_len == -1) {
 			// Godot string
 			const String &s = strings[godot_string_elem];
 
@@ -81,11 +83,11 @@ String StringBuilder::as_string() const {
 		} else {
 			const char *s = c_strings[c_string_elem];
 
-			for (int32_t j = 0; j < appended_strings[i]; j++) {
+			for (int32_t j = 0; j < str_len; j++) {
 				buffer[current_position + j] = s[j];
 			}
 
-			current_position += appended_strings[i];
+			current_position += str_len;
 
 			c_string_elem++;
 		}
diff --git a/core/string/string_builder.h b/core/string/string_builder.h
index 1a989e8422..3ba279b4fd 100644
--- a/core/string/string_builder.h
+++ b/core/string/string_builder.h
@@ -32,17 +32,17 @@
 #define STRING_BUILDER_H
 
 #include "core/string/ustring.h"
-#include "core/templates/vector.h"
+#include "core/templates/local_vector.h"
 
 class StringBuilder {
 	uint32_t string_length = 0;
 
-	Vector<String> strings;
-	Vector<const char *> c_strings;
+	LocalVector<String> strings;
+	LocalVector<const char *> c_strings;
 
 	// -1 means it's a Godot String
 	// a natural number means C string.
-	Vector<int32_t> appended_strings;
+	LocalVector<int32_t> appended_strings;
 
 public:
 	StringBuilder &append(const String &p_string);

From a7bb85d2b7c07962f39b2dd419ae96f173e3efe2 Mon Sep 17 00:00:00 2001
From: Chaosus <chaosus89@gmail.com>
Date: Sun, 18 Aug 2024 10:19:16 +0300
Subject: [PATCH 041/124] Fix shader crash when using varyings with non-`flat`
 integer type

---
 servers/rendering/shader_compiler.cpp |  2 +-
 servers/rendering/shader_language.cpp | 56 +++++++++------------------
 servers/rendering/shader_language.h   |  6 +--
 3 files changed, 21 insertions(+), 43 deletions(-)

diff --git a/servers/rendering/shader_compiler.cpp b/servers/rendering/shader_compiler.cpp
index f52963f98f..cba889f1e4 100644
--- a/servers/rendering/shader_compiler.cpp
+++ b/servers/rendering/shader_compiler.cpp
@@ -674,7 +674,7 @@ String ShaderCompiler::_dump_node_code(const SL::Node *p_node, int p_level, Gene
 				const StringName &varying_name = varying_names[k];
 				const SL::ShaderNode::Varying &varying = pnode->varyings[varying_name];
 
-				if (varying.stage == SL::ShaderNode::Varying::STAGE_FRAGMENT_TO_LIGHT || varying.stage == SL::ShaderNode::Varying::STAGE_FRAGMENT) {
+				if (varying.stage == SL::ShaderNode::Varying::STAGE_FRAGMENT) {
 					var_frag_to_light.push_back(Pair<StringName, SL::ShaderNode::Varying>(varying_name, varying));
 					fragment_varyings.insert(varying_name);
 					continue;
diff --git a/servers/rendering/shader_language.cpp b/servers/rendering/shader_language.cpp
index 764d420a23..cfa8a85234 100644
--- a/servers/rendering/shader_language.cpp
+++ b/servers/rendering/shader_language.cpp
@@ -5062,7 +5062,7 @@ bool ShaderLanguage::_validate_varying_assign(ShaderNode::Varying &p_varying, St
 		case ShaderNode::Varying::STAGE_UNKNOWN: // first assign
 			if (current_function == varying_function_names.vertex) {
 				if (p_varying.type < TYPE_INT) {
-					*r_message = vformat(RTR("Varying with '%s' data type may only be assigned in the 'fragment' function."), get_datatype_name(p_varying.type));
+					*r_message = vformat(RTR("Varying with '%s' data type may only be assigned in the '%s' function."), get_datatype_name(p_varying.type), "fragment");
 					return false;
 				}
 				p_varying.stage = ShaderNode::Varying::STAGE_VERTEX;
@@ -5070,17 +5070,15 @@ bool ShaderLanguage::_validate_varying_assign(ShaderNode::Varying &p_varying, St
 				p_varying.stage = ShaderNode::Varying::STAGE_FRAGMENT;
 			}
 			break;
-		case ShaderNode::Varying::STAGE_VERTEX_TO_FRAGMENT_LIGHT:
 		case ShaderNode::Varying::STAGE_VERTEX:
 			if (current_function == varying_function_names.fragment) {
-				*r_message = RTR("Varyings which assigned in 'vertex' function may not be reassigned in 'fragment' or 'light'.");
+				*r_message = vformat(RTR("Varyings which assigned in '%s' function may not be reassigned in '%s' or '%s'."), "vertex", "fragment", "light");
 				return false;
 			}
 			break;
-		case ShaderNode::Varying::STAGE_FRAGMENT_TO_LIGHT:
 		case ShaderNode::Varying::STAGE_FRAGMENT:
 			if (current_function == varying_function_names.vertex) {
-				*r_message = RTR("Varyings which assigned in 'fragment' function may not be reassigned in 'vertex' or 'light'.");
+				*r_message = vformat(RTR("Varyings which assigned in '%s' function may not be reassigned in '%s' or '%s'."), "fragment", "vertex", "light");
 				return false;
 			}
 			break;
@@ -6038,15 +6036,11 @@ ShaderLanguage::Node *ShaderLanguage::_parse_expression(BlockNode *p_block, cons
 														error = true;
 													}
 													break;
-												case ShaderNode::Varying::STAGE_VERTEX_TO_FRAGMENT_LIGHT:
-													[[fallthrough]];
 												case ShaderNode::Varying::STAGE_VERTEX:
 													if (is_out_arg && current_function != varying_function_names.vertex) { // inout/out
 														error = true;
 													}
 													break;
-												case ShaderNode::Varying::STAGE_FRAGMENT_TO_LIGHT:
-													[[fallthrough]];
 												case ShaderNode::Varying::STAGE_FRAGMENT:
 													if (!is_out_arg) {
 														if (current_function != varying_function_names.fragment && current_function != varying_function_names.light) {
@@ -6246,37 +6240,15 @@ ShaderLanguage::Node *ShaderLanguage::_parse_expression(BlockNode *p_block, cons
 								return nullptr;
 							}
 						} else {
-							switch (var.stage) {
-								case ShaderNode::Varying::STAGE_UNKNOWN: {
-									if (var.type < TYPE_INT) {
-										if (current_function == varying_function_names.vertex) {
-											_set_error(vformat(RTR("Varying with '%s' data type may only be used in the 'fragment' function."), get_datatype_name(var.type)));
-										} else {
-											_set_error(vformat(RTR("Varying '%s' must be assigned in the 'fragment' function first."), identifier));
-										}
-										return nullptr;
-									}
-								} break;
-								case ShaderNode::Varying::STAGE_VERTEX:
-									if (current_function == varying_function_names.fragment || current_function == varying_function_names.light) {
-										var.stage = ShaderNode::Varying::STAGE_VERTEX_TO_FRAGMENT_LIGHT;
-									}
-									break;
-								case ShaderNode::Varying::STAGE_FRAGMENT:
-									if (current_function == varying_function_names.light) {
-										var.stage = ShaderNode::Varying::STAGE_FRAGMENT_TO_LIGHT;
-									}
-									break;
-								default:
-									break;
+							if (var.stage == ShaderNode::Varying::STAGE_UNKNOWN && var.type < TYPE_INT) {
+								if (current_function == varying_function_names.vertex) {
+									_set_error(vformat(RTR("Varying with '%s' data type may only be used in the '%s' function."), get_datatype_name(var.type), "fragment"));
+								} else {
+									_set_error(vformat(RTR("Varying '%s' must be assigned in the '%s' function first."), identifier, "fragment"));
+								}
+								return nullptr;
 							}
 						}
-
-						if ((var.stage != ShaderNode::Varying::STAGE_FRAGMENT && var.stage != ShaderNode::Varying::STAGE_FRAGMENT_TO_LIGHT) && var.type < TYPE_FLOAT && var.interpolation != INTERPOLATION_FLAT) {
-							_set_tkpos(var.tkpos);
-							_set_error(RTR("Varying with integer data type must be declared with `flat` interpolation qualifier."));
-							return nullptr;
-						}
 					}
 
 					if (ident_type == IDENTIFIER_FUNCTION) {
@@ -10697,6 +10669,14 @@ Error ShaderLanguage::_parse_shader(const HashMap<StringName, FunctionInfo> &p_f
 		tk = _get_token();
 	}
 
+	for (const KeyValue<StringName, ShaderNode::Varying> &kv : shader->varyings) {
+		if (kv.value.stage != ShaderNode::Varying::STAGE_FRAGMENT && (kv.value.type > TYPE_BVEC4 && kv.value.type < TYPE_FLOAT) && kv.value.interpolation != INTERPOLATION_FLAT) {
+			_set_tkpos(kv.value.tkpos);
+			_set_error(vformat(RTR("Varying with integer data type must be declared with `%s` interpolation qualifier."), "flat"));
+			return ERR_PARSE_ERROR;
+		}
+	}
+
 #ifdef DEBUG_ENABLED
 	if (check_device_limit_warnings && uniform_buffer_exceeded_line != -1) {
 		_add_warning(ShaderWarning::DEVICE_LIMIT_EXCEEDED, uniform_buffer_exceeded_line, RTR("uniform buffer"), { uniform_buffer_size, max_uniform_buffer_size });
diff --git a/servers/rendering/shader_language.h b/servers/rendering/shader_language.h
index 4e7642a781..61a3f1fa02 100644
--- a/servers/rendering/shader_language.h
+++ b/servers/rendering/shader_language.h
@@ -620,10 +620,8 @@ public:
 		struct Varying {
 			enum Stage {
 				STAGE_UNKNOWN,
-				STAGE_VERTEX, // transition stage to STAGE_VERTEX_TO_FRAGMENT_LIGHT, emits warning if it's not used
-				STAGE_FRAGMENT, // transition stage to STAGE_FRAGMENT_TO_LIGHT, emits warning if it's not used
-				STAGE_VERTEX_TO_FRAGMENT_LIGHT,
-				STAGE_FRAGMENT_TO_LIGHT,
+				STAGE_VERTEX,
+				STAGE_FRAGMENT,
 			};
 
 			Stage stage = STAGE_UNKNOWN;

From c77cbf096b15d3828768c78a0a1b573c748f9219 Mon Sep 17 00:00:00 2001
From: "Matias N. Goldberg" <dark_sylinc@yahoo.com.ar>
Date: Thu, 14 Nov 2024 13:03:14 -0300
Subject: [PATCH 042/124] Improvements from TheForge (see description)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

The work was performed by collaboration of TheForge and Google. I am
merely splitting it up into smaller PRs and cleaning it up.

This is the most "risky" PR so far because the previous ones have been
miscellaneous stuff aimed at either [improve
debugging](https://github.com/godotengine/godot/pull/90993) (e.g. device
lost), [improve Android
experience](https://github.com/godotengine/godot/pull/96439) (add Swappy
for better Frame Pacing + Pre-Transformed Swapchains for slightly better
performance), or harmless [ASTC
improvements](https://github.com/godotengine/godot/pull/96045) (better
performance by simply toggling a feature when available).

However this PR contains larger modifications aimed at improving
performance or reducing memory fragmentation. With greater
modifications, come greater risks of bugs or breakage.

Changes introduced by this PR:

TBDR GPUs (e.g. most of Android + iOS + M1 Apple) support rendering to
Render Targets that are not backed by actual GPU memory (everything
stays in cache). This works as long as load action isn't `LOAD`, and
store action must be `DONT_CARE`. This saves VRAM (it also makes
painfully obvious when a mistake introduces a performance regression).
Of particular usefulness is when doing MSAA and keeping the raw MSAA
content is not necessary.

Some GPUs get faster when the sampler settings are hard-coded into the
GLSL shaders (instead of being dynamically bound at runtime). This
required changes to the GLSL shaders, PSO creation routines, Descriptor
creation routines, and Descriptor binding routines.

 - `bool immutable_samplers_enabled = true`

Setting it to false enforces the old behavior. Useful for debugging bugs
and regressions.

Immutable samplers requires that the samplers stay... immutable, hence
this boolean is useful if the promise gets broken. We might want to turn
this into a `GLOBAL_DEF` setting.

Instead of creating dozen/hundreds/thousands of `VkDescriptorSet` every
frame that need to be freed individually when they are no longer needed,
they all get freed at once by resetting the whole pool. Once the whole
pool is no longer in use by the GPU, it gets reset and its memory
recycled. Descriptor sets that are created to be kept around for longer
or forever (i.e. not created and freed within the same frame) **must
not** use linear pools. There may be more than one pool per frame. How
many pools per frame Godot ends up with depends on its capacity, and
that is controlled by
`rendering/rendering_device/vulkan/max_descriptors_per_pool`.

- **Possible improvement for later:** It should be possible for Godot
to adapt to how many descriptors per pool are needed on a per-key basis
(i.e. grow their capacity like `std::vector` does) after rendering a few
frames; which would be better than the current solution of having a
single global value for all pools (`max_descriptors_per_pool`) that the
user needs to tweak.

 - `bool linear_descriptor_pools_enabled = true`

Setting it to false enforces the old behavior. Useful for debugging bugs
and regressions.
Setting it to false is required when workarounding driver bugs (e.g.
Adreno 730).

A ridiculous optimization. Ridiculous because the original code
should've done this in the first place. Previously Godot was doing the
following:

  1. Create a command buffer **pool**. One per frame.
  2. Create multiple command buffers from the pool in point 1.
3. Call `vkBeginCommandBuffer` on the cmd buffer in point 2. This
resets the cmd buffer because Godot requests the
`VK_COMMAND_POOL_CREATE_RESET_COMMAND_BUFFER_BIT` flag.
  4. Add commands to the cmd buffers from point 2.
  5. Submit those commands.
6. On frame N + 2, recycle the buffer pool and cmd buffers from pt 1 &
2, and repeat from step 3.

The problem here is that step 3 resets each command buffer individually.
Initially Godot used to have 1 cmd buffer per pool, thus the impact is
very low.

But not anymore (specially with Adreno workarounds to force splitting
compute dispatches into a new cmd buffer, more on this later). However
Godot keeps around a very low amount of command buffers per frame.

The recommended method is to reset the whole pool, to reset all cmd
buffers at once. Hence the new steps would be:

  1. Create a command buffer **pool**. One per frame.
  2. Create multiple command buffers from the pool in point 1.
3. Call `vkBeginCommandBuffer` on the cmd buffer in point 2, which is
already reset/empty (see step 6).
  4. Add commands to the cmd buffers from point 2.
  5. Submit those commands.
6. On frame N + 2, recycle the buffer pool and cmd buffers from pt 1 &
2, call `vkResetCommandPool` and repeat from step 3.

**Possible issues:** @dariosamo added `transfer_worker` which creates a
command buffer pool:

```cpp
transfer_worker->command_pool =
driver->command_pool_create(transfer_queue_family,
RDD::COMMAND_BUFFER_TYPE_PRIMARY);
```

As expected, validation was complaining that command buffers were being
reused without being reset (that's good, we now know Validation Layers
will warn us of wrong use).
I fixed it by adding:

```cpp
void RenderingDevice::_wait_for_transfer_worker(TransferWorker
*p_transfer_worker) {
	driver->fence_wait(p_transfer_worker->command_fence);
	driver->command_pool_reset(p_transfer_worker->command_pool); //
! New line !
```

**Secondary cmd buffers are subject to the same issue but I didn't alter
them. I talked this with Dario and he is aware of this.**
Secondary cmd buffers are currently disabled due to other issues (it's
disabled on master).

 - `bool RenderingDeviceCommons::command_pool_reset_enabled`

Setting it to false enforces the old behavior. Useful for debugging bugs
and regressions.

There's no other reason for this boolean. Possibly once it becomes well
tested, the boolean could be removed entirely.

Adds `command_bind_render_uniform_sets` and
`add_draw_list_bind_uniform_sets` (+ compute variants).

It performs the same as `add_draw_list_bind_uniform_set` (notice
singular vs plural), but on multiple consecutive uniform sets, thus
reducing graph and draw call overhead.

 - `bool descriptor_set_batching = true;`

Setting it to false enforces the old behavior. Useful for debugging bugs
and regressions.

There's no other reason for this boolean. Possibly once it becomes well
tested, the boolean could be removed entirely.

Godot currently does the following:

 1. Fill the entire cmd buffer with commands.
 2. `submit()`
    - Wait with a semaphore for the swapchain.
- Trigger a semaphore to indicate when we're done (so the swapchain
can submit).
 3. `present()`

The optimization opportunity here is that 95% of Godot's rendering is
done offscreen.
Then a fullscreen pass copies everything to the swapchain. Godot doesn't
practically render directly to the swapchain.

The problem with this is that the GPU has to wait for the swapchain to
be released **to start anything**, when we could start *much earlier*.
Only the final blit pass must wait for the swapchain.

TheForge changed it to the following (more complicated, I'm simplifying
the idea):

 1. Fill the entire cmd buffer with commands.
 2. In `screen_prepare_for_drawing` do `submit()`
    - There are no semaphore waits for the swapchain.
    - Trigger a semaphore to indicate when we're done.
3. Fill a new cmd buffer that only does the final blit to the
swapchain.
 4. `submit()`
    - Wait with a semaphore for the submit() from step 2.
- Wait with a semaphore for the swapchain (so the swapchain can
submit).
- Trigger a semaphore to indicate when we're done (so the swapchain
can submit).
 5. `present()`

Dario discovered this problem independently while working on a different
platform.

**However TheForge's solution had to be rewritten from scratch:** The
complexity to achieve the solution was high and quite difficult to
maintain with the way Godot works now (after Übershaders PR).
But on the other hand, re-implementing the solution became much simpler
because Dario already had to do something similar: To fix an Adreno 730
driver bug, he had to implement splitting command buffers. **This is
exactly what we need!**. Thus it was re-written using this existing
functionality for a new purpose.

To achieve this, I added a new argument, `bool p_split_cmd_buffer`, to
`RenderingDeviceGraph::add_draw_list_begin`, which is only set to true
by `RenderingDevice::draw_list_begin_for_screen`.

The graph will split the draw list into its own command buffer.

 - `bool split_swapchain_into_its_own_cmd_buffer = true;`

Setting it to false enforces the old behavior. This might be necessary
for consoles which follow an alternate solution to the same problem.
If not, then we should consider removing it.

PR #90993 added `shader_destroy_modules()` but it was not actually in
use.

This PR adds several places where `shader_destroy_modules()` is called
after initialization to free up memory of SPIR-V structures that are no
longer needed.
---
 doc/classes/ProjectSettings.xml               |   3 +
 .../d3d12/rendering_device_driver_d3d12.cpp   |  28 +-
 drivers/d3d12/rendering_device_driver_d3d12.h |   9 +-
 drivers/metal/metal_objects.h                 |   2 +
 drivers/metal/metal_objects.mm                |  54 +++
 drivers/metal/rendering_device_driver_metal.h |   8 +-
 .../metal/rendering_device_driver_metal.mm    |  40 ++-
 .../vulkan/rendering_device_driver_vulkan.cpp | 302 +++++++++++++---
 .../vulkan/rendering_device_driver_vulkan.h   |  23 +-
 .../forward_mobile/render_forward_mobile.cpp  |  10 +-
 .../scene_shader_forward_mobile.cpp           |  27 +-
 .../renderer_rd/renderer_canvas_render_rd.cpp |   4 +
 .../renderer_rd/renderer_compositor_rd.cpp    |   3 +
 servers/rendering/renderer_rd/shader_rd.cpp   |   7 +-
 servers/rendering/renderer_rd/shader_rd.h     |   4 +-
 .../storage_rd/render_scene_buffers_rd.cpp    |   3 +
 servers/rendering/rendering_device.cpp        | 324 ++++++++++++++----
 servers/rendering/rendering_device.h          |  45 ++-
 servers/rendering/rendering_device_commons.h  |  18 +
 servers/rendering/rendering_device_driver.h   |  27 +-
 servers/rendering/rendering_device_graph.cpp  | 108 ++++--
 servers/rendering/rendering_device_graph.h    |  42 ++-
 ...add-vmaCalculateLazilyAllocatedBytes.patch |  55 +++
 thirdparty/vulkan/vk_mem_alloc.h              |  37 ++
 24 files changed, 983 insertions(+), 200 deletions(-)
 create mode 100644 thirdparty/vulkan/patches/0003-VMA-add-vmaCalculateLazilyAllocatedBytes.patch

diff --git a/doc/classes/ProjectSettings.xml b/doc/classes/ProjectSettings.xml
index 1da578167c..d31c726829 100644
--- a/doc/classes/ProjectSettings.xml
+++ b/doc/classes/ProjectSettings.xml
@@ -2861,6 +2861,9 @@
 			[b]Note:[/b] Some platforms may restrict the actual value.
 		</member>
 		<member name="rendering/rendering_device/vulkan/max_descriptors_per_pool" type="int" setter="" getter="" default="64">
+			The number of descriptors per pool. Godot's Vulkan backend uses linear pools for descriptors that will be created and destroyed within a single frame. Instead of destroying every single descriptor every frame, they all can be destroyed at once by resetting the pool they belong to.
+			A larger number is more efficient up to a limit, after that it will only waste RAM (maximum efficiency is achieved when there is no more than 1 pool per frame). A small number could end up with one pool per descriptor, which negatively impacts performance.
+			[b]Note:[/b] Changing this property requires a restart to take effect.
 		</member>
 		<member name="rendering/scaling_3d/fsr_sharpness" type="float" setter="" getter="" default="0.2">
 			Determines how sharp the upscaled image will be when using the FSR upscaling mode. Sharpness halves with every whole number. Values go from 0.0 (sharpest) to 2.0. Values above 2.0 won't make a visible difference.
diff --git a/drivers/d3d12/rendering_device_driver_d3d12.cpp b/drivers/d3d12/rendering_device_driver_d3d12.cpp
index f78233b9b3..917104d0df 100644
--- a/drivers/d3d12/rendering_device_driver_d3d12.cpp
+++ b/drivers/d3d12/rendering_device_driver_d3d12.cpp
@@ -2286,6 +2286,10 @@ RDD::CommandPoolID RenderingDeviceDriverD3D12::command_pool_create(CommandQueueF
 	return CommandPoolID(command_pool);
 }
 
+bool RenderingDeviceDriverD3D12::command_pool_reset(CommandPoolID p_cmd_pool) {
+	return true;
+}
+
 void RenderingDeviceDriverD3D12::command_pool_free(CommandPoolID p_cmd_pool) {
 	CommandPoolInfo *command_pool = (CommandPoolInfo *)(p_cmd_pool.id);
 	memdelete(command_pool);
@@ -3616,7 +3620,7 @@ Vector<uint8_t> RenderingDeviceDriverD3D12::shader_compile_binary_from_spirv(Vec
 	return ret;
 }
 
-RDD::ShaderID RenderingDeviceDriverD3D12::shader_create_from_bytecode(const Vector<uint8_t> &p_shader_binary, ShaderDescription &r_shader_desc, String &r_name) {
+RDD::ShaderID RenderingDeviceDriverD3D12::shader_create_from_bytecode(const Vector<uint8_t> &p_shader_binary, ShaderDescription &r_shader_desc, String &r_name, const Vector<ImmutableSampler> &p_immutable_samplers) {
 	r_shader_desc = {}; // Driver-agnostic.
 	ShaderInfo shader_info_in; // Driver-specific.
 
@@ -3825,7 +3829,9 @@ static void _add_descriptor_count_for_uniform(RenderingDevice::UniformType p_typ
 	}
 }
 
-RDD::UniformSetID RenderingDeviceDriverD3D12::uniform_set_create(VectorView<BoundUniform> p_uniforms, ShaderID p_shader, uint32_t p_set_index) {
+RDD::UniformSetID RenderingDeviceDriverD3D12::uniform_set_create(VectorView<BoundUniform> p_uniforms, ShaderID p_shader, uint32_t p_set_index, int p_linear_pool_index) {
+	// p_linear_pool_index = -1; // TODO:? Linear pools not implemented or not supported by API backend.
+
 	// Pre-bookkeep.
 	UniformSetInfo *uniform_set_info = VersatileResource::allocate<UniformSetInfo>(resources_allocator);
 
@@ -5352,6 +5358,13 @@ void RenderingDeviceDriverD3D12::command_bind_render_uniform_set(CommandBufferID
 	_command_bind_uniform_set(p_cmd_buffer, p_uniform_set, p_shader, p_set_index, false);
 }
 
+void RenderingDeviceDriverD3D12::command_bind_render_uniform_sets(CommandBufferID p_cmd_buffer, VectorView<UniformSetID> p_uniform_sets, ShaderID p_shader, uint32_t p_first_set_index, uint32_t p_set_count) {
+	for (uint32_t i = 0u; i < p_set_count; ++i) {
+		// TODO: _command_bind_uniform_set() does WAAAAY too much stuff. A lot of it should be already cached in UniformSetID when uniform_set_create() was called. Binding is supposed to be a cheap operation, ideally a memcpy.
+		_command_bind_uniform_set(p_cmd_buffer, p_uniform_sets[i], p_shader, p_first_set_index + i, false);
+	}
+}
+
 void RenderingDeviceDriverD3D12::command_render_draw(CommandBufferID p_cmd_buffer, uint32_t p_vertex_count, uint32_t p_instance_count, uint32_t p_base_vertex, uint32_t p_first_instance) {
 	CommandBufferInfo *cmd_buf_info = (CommandBufferInfo *)p_cmd_buffer.id;
 	_bind_vertex_buffers(cmd_buf_info);
@@ -5856,6 +5869,13 @@ void RenderingDeviceDriverD3D12::command_bind_compute_uniform_set(CommandBufferI
 	_command_bind_uniform_set(p_cmd_buffer, p_uniform_set, p_shader, p_set_index, true);
 }
 
+void RenderingDeviceDriverD3D12::command_bind_compute_uniform_sets(CommandBufferID p_cmd_buffer, VectorView<UniformSetID> p_uniform_sets, ShaderID p_shader, uint32_t p_first_set_index, uint32_t p_set_count) {
+	for (uint32_t i = 0u; i < p_set_count; ++i) {
+		// TODO: _command_bind_uniform_set() does WAAAAY too much stuff. A lot of it should be already cached in UniformSetID when uniform_set_create() was called. Binding is supposed to be a cheap operation, ideally a memcpy.
+		_command_bind_uniform_set(p_cmd_buffer, p_uniform_sets[i], p_shader, p_first_set_index + i, true);
+	}
+}
+
 void RenderingDeviceDriverD3D12::command_compute_dispatch(CommandBufferID p_cmd_buffer, uint32_t p_x_groups, uint32_t p_y_groups, uint32_t p_z_groups) {
 	CommandBufferInfo *cmd_buf_info = (CommandBufferInfo *)p_cmd_buffer.id;
 	if (!barrier_capabilities.enhanced_barriers_supported) {
@@ -6139,6 +6159,10 @@ uint64_t RenderingDeviceDriverD3D12::get_total_memory_used() {
 	return stats.Total.Stats.BlockBytes;
 }
 
+uint64_t RenderingDeviceDriverD3D12::get_lazily_memory_used() {
+	return 0;
+}
+
 uint64_t RenderingDeviceDriverD3D12::limit_get(Limit p_limit) {
 	uint64_t safe_unbounded = ((uint64_t)1 << 30);
 	switch (p_limit) {
diff --git a/drivers/d3d12/rendering_device_driver_d3d12.h b/drivers/d3d12/rendering_device_driver_d3d12.h
index b449a90876..59c81db726 100644
--- a/drivers/d3d12/rendering_device_driver_d3d12.h
+++ b/drivers/d3d12/rendering_device_driver_d3d12.h
@@ -434,6 +434,7 @@ private:
 
 public:
 	virtual CommandPoolID command_pool_create(CommandQueueFamilyID p_cmd_queue_family, CommandBufferType p_cmd_buffer_type) override final;
+	virtual bool command_pool_reset(CommandPoolID p_cmd_pool) override final;
 	virtual void command_pool_free(CommandPoolID p_cmd_pool) override final;
 
 	// ----- BUFFER -----
@@ -697,7 +698,7 @@ private:
 public:
 	virtual String shader_get_binary_cache_key() override final;
 	virtual Vector<uint8_t> shader_compile_binary_from_spirv(VectorView<ShaderStageSPIRVData> p_spirv, const String &p_shader_name) override final;
-	virtual ShaderID shader_create_from_bytecode(const Vector<uint8_t> &p_shader_binary, ShaderDescription &r_shader_desc, String &r_name) override final;
+	virtual ShaderID shader_create_from_bytecode(const Vector<uint8_t> &p_shader_binary, ShaderDescription &r_shader_desc, String &r_name, const Vector<ImmutableSampler> &p_immutable_samplers) override final;
 	virtual uint32_t shader_get_layout_hash(ShaderID p_shader) override final;
 	virtual void shader_free(ShaderID p_shader) override final;
 	virtual void shader_destroy_modules(ShaderID p_shader) override final;
@@ -747,7 +748,7 @@ private:
 	};
 
 public:
-	virtual UniformSetID uniform_set_create(VectorView<BoundUniform> p_uniforms, ShaderID p_shader, uint32_t p_set_index) override final;
+	virtual UniformSetID uniform_set_create(VectorView<BoundUniform> p_uniforms, ShaderID p_shader, uint32_t p_set_index, int p_linear_pool_index) override final;
 	virtual void uniform_set_free(UniformSetID p_uniform_set) override final;
 
 	// ----- COMMANDS -----
@@ -757,6 +758,7 @@ public:
 private:
 	void _command_check_descriptor_sets(CommandBufferID p_cmd_buffer);
 	void _command_bind_uniform_set(CommandBufferID p_cmd_buffer, UniformSetID p_uniform_set, ShaderID p_shader, uint32_t p_set_index, bool p_for_compute);
+	void _command_bind_uniform_sets(CommandBufferID p_cmd_buffer, VectorView<UniformSetID> p_uniform_sets, ShaderID p_shader, uint32_t p_first_set_index, uint32_t p_set_count, bool p_for_compute);
 
 public:
 	/******************/
@@ -846,6 +848,7 @@ public:
 	// Binding.
 	virtual void command_bind_render_pipeline(CommandBufferID p_cmd_buffer, PipelineID p_pipeline) override final;
 	virtual void command_bind_render_uniform_set(CommandBufferID p_cmd_buffer, UniformSetID p_uniform_set, ShaderID p_shader, uint32_t p_set_index) override final;
+	virtual void command_bind_render_uniform_sets(CommandBufferID p_cmd_buffer, VectorView<UniformSetID> p_uniform_sets, ShaderID p_shader, uint32_t p_first_set_index, uint32_t p_set_count) override final;
 
 	// Drawing.
 	virtual void command_render_draw(CommandBufferID p_cmd_buffer, uint32_t p_vertex_count, uint32_t p_instance_count, uint32_t p_base_vertex, uint32_t p_first_instance) override final;
@@ -893,6 +896,7 @@ public:
 	// Binding.
 	virtual void command_bind_compute_pipeline(CommandBufferID p_cmd_buffer, PipelineID p_pipeline) override final;
 	virtual void command_bind_compute_uniform_set(CommandBufferID p_cmd_buffer, UniformSetID p_uniform_set, ShaderID p_shader, uint32_t p_set_index) override final;
+	virtual void command_bind_compute_uniform_sets(CommandBufferID p_cmd_buffer, VectorView<UniformSetID> p_uniform_sets, ShaderID p_shader, uint32_t p_first_set_index, uint32_t p_set_count) override final;
 
 	// Dispatching.
 	virtual void command_compute_dispatch(CommandBufferID p_cmd_buffer, uint32_t p_x_groups, uint32_t p_y_groups, uint32_t p_z_groups) override final;
@@ -986,6 +990,7 @@ public:
 	virtual void set_object_name(ObjectType p_type, ID p_driver_id, const String &p_name) override final;
 	virtual uint64_t get_resource_native_handle(DriverResource p_type, ID p_driver_id) override final;
 	virtual uint64_t get_total_memory_used() override final;
+	virtual uint64_t get_lazily_memory_used() override final;
 	virtual uint64_t limit_get(Limit p_limit) override final;
 	virtual uint64_t api_trait_get(ApiTrait p_trait) override final;
 	virtual bool has_feature(Features p_feature) override final;
diff --git a/drivers/metal/metal_objects.h b/drivers/metal/metal_objects.h
index 5eda826d2a..1870221b22 100644
--- a/drivers/metal/metal_objects.h
+++ b/drivers/metal/metal_objects.h
@@ -502,6 +502,7 @@ public:
 #pragma mark - Render Commands
 
 	void render_bind_uniform_set(RDD::UniformSetID p_uniform_set, RDD::ShaderID p_shader, uint32_t p_set_index);
+	void render_bind_uniform_sets(VectorView<RDD::UniformSetID> p_uniform_sets, RDD::ShaderID p_shader, uint32_t p_first_set_index, uint32_t p_set_count);
 	void render_clear_attachments(VectorView<RDD::AttachmentClear> p_attachment_clears, VectorView<Rect2i> p_rects);
 	void render_set_viewport(VectorView<Rect2i> p_viewports);
 	void render_set_scissor(VectorView<Rect2i> p_scissors);
@@ -535,6 +536,7 @@ public:
 #pragma mark - Compute Commands
 
 	void compute_bind_uniform_set(RDD::UniformSetID p_uniform_set, RDD::ShaderID p_shader, uint32_t p_set_index);
+	void compute_bind_uniform_sets(VectorView<RDD::UniformSetID> p_uniform_sets, RDD::ShaderID p_shader, uint32_t p_first_set_index, uint32_t p_set_count);
 	void compute_dispatch(uint32_t p_x_groups, uint32_t p_y_groups, uint32_t p_z_groups);
 	void compute_dispatch_indirect(RDD::BufferID p_indirect_buffer, uint64_t p_offset);
 
diff --git a/drivers/metal/metal_objects.mm b/drivers/metal/metal_objects.mm
index 11ab209d60..a3a2f75fef 100644
--- a/drivers/metal/metal_objects.mm
+++ b/drivers/metal/metal_objects.mm
@@ -223,6 +223,26 @@ void MDCommandBuffer::render_bind_uniform_set(RDD::UniformSetID p_uniform_set, R
 	}
 }
 
+void MDCommandBuffer::render_bind_uniform_sets(VectorView<RDD::UniformSetID> p_uniform_sets, RDD::ShaderID p_shader, uint32_t p_first_set_index, uint32_t p_set_count) {
+	DEV_ASSERT(type == MDCommandBufferStateType::Render);
+
+	for (size_t i = 0u; i < p_set_count; ++i) {
+		MDUniformSet *set = (MDUniformSet *)(p_uniform_sets[i].id);
+		if (render.uniform_sets.size() <= set->index) {
+			uint32_t s = render.uniform_sets.size();
+			render.uniform_sets.resize(set->index + 1);
+			// Set intermediate values to null.
+			std::fill(&render.uniform_sets[s], &render.uniform_sets[set->index] + 1, nullptr);
+		}
+
+		if (render.uniform_sets[set->index] != set) {
+			render.dirty.set_flag(RenderState::DIRTY_UNIFORMS);
+			render.uniform_set_mask |= 1ULL << set->index;
+			render.uniform_sets[set->index] = set;
+		}
+	}
+}
+
 void MDCommandBuffer::render_clear_attachments(VectorView<RDD::AttachmentClear> p_attachment_clears, VectorView<Rect2i> p_rects) {
 	DEV_ASSERT(type == MDCommandBufferStateType::Render);
 
@@ -964,6 +984,40 @@ void MDCommandBuffer::compute_bind_uniform_set(RDD::UniformSetID p_uniform_set,
 	}
 }
 
+void MDCommandBuffer::compute_bind_uniform_sets(VectorView<RDD::UniformSetID> p_uniform_sets, RDD::ShaderID p_shader, uint32_t p_first_set_index, uint32_t p_set_count) {
+	DEV_ASSERT(type == MDCommandBufferStateType::Compute);
+
+	id<MTLComputeCommandEncoder> enc = compute.encoder;
+	id<MTLDevice> device = enc.device;
+
+	MDShader *shader = (MDShader *)(p_shader.id);
+
+	thread_local LocalVector<__unsafe_unretained id<MTLBuffer>> buffers;
+	thread_local LocalVector<NSUInteger> offsets;
+
+	buffers.resize(p_set_count);
+	offsets.resize(p_set_count);
+
+	for (size_t i = 0u; i < p_set_count; ++i) {
+		UniformSet const &set_info = shader->sets[p_first_set_index + i];
+
+		MDUniformSet *set = (MDUniformSet *)(p_uniform_sets[i].id);
+		BoundUniformSet &bus = set->boundUniformSetForShader(shader, device);
+		bus.merge_into(compute.resource_usage);
+
+		uint32_t const *offset = set_info.offsets.getptr(RDD::SHADER_STAGE_COMPUTE);
+		if (offset) {
+			buffers[i] = bus.buffer;
+			offsets[i] = *offset;
+		} else {
+			buffers[i] = nullptr;
+			offsets[i] = 0u;
+		}
+	}
+
+	[enc setBuffers:buffers.ptr() offsets:offsets.ptr() withRange:NSMakeRange(p_first_set_index, p_set_count)];
+}
+
 void MDCommandBuffer::compute_dispatch(uint32_t p_x_groups, uint32_t p_y_groups, uint32_t p_z_groups) {
 	DEV_ASSERT(type == MDCommandBufferStateType::Compute);
 
diff --git a/drivers/metal/rendering_device_driver_metal.h b/drivers/metal/rendering_device_driver_metal.h
index 09ab7601e9..52eb0f7930 100644
--- a/drivers/metal/rendering_device_driver_metal.h
+++ b/drivers/metal/rendering_device_driver_metal.h
@@ -185,6 +185,7 @@ public:
 	// ----- POOL -----
 
 	virtual CommandPoolID command_pool_create(CommandQueueFamilyID p_cmd_queue_family, CommandBufferType p_cmd_buffer_type) override final;
+	virtual bool command_pool_reset(CommandPoolID p_cmd_pool) override final;
 	virtual void command_pool_free(CommandPoolID p_cmd_pool) override final;
 
 	// ----- BUFFER -----
@@ -251,14 +252,14 @@ private:
 public:
 	virtual String shader_get_binary_cache_key() override final;
 	virtual Vector<uint8_t> shader_compile_binary_from_spirv(VectorView<ShaderStageSPIRVData> p_spirv, const String &p_shader_name) override final;
-	virtual ShaderID shader_create_from_bytecode(const Vector<uint8_t> &p_shader_binary, ShaderDescription &r_shader_desc, String &r_name) override final;
+	virtual ShaderID shader_create_from_bytecode(const Vector<uint8_t> &p_shader_binary, ShaderDescription &r_shader_desc, String &r_name, const Vector<ImmutableSampler> &p_immutable_samplers) override final;
 	virtual void shader_free(ShaderID p_shader) override final;
 	virtual void shader_destroy_modules(ShaderID p_shader) override final;
 
 #pragma mark - Uniform Set
 
 public:
-	virtual UniformSetID uniform_set_create(VectorView<BoundUniform> p_uniforms, ShaderID p_shader, uint32_t p_set_index) override final;
+	virtual UniformSetID uniform_set_create(VectorView<BoundUniform> p_uniforms, ShaderID p_shader, uint32_t p_set_index, int p_linear_pool_index) override final;
 	virtual void uniform_set_free(UniformSetID p_uniform_set) override final;
 
 #pragma mark - Commands
@@ -331,6 +332,7 @@ public:
 	// Binding.
 	virtual void command_bind_render_pipeline(CommandBufferID p_cmd_buffer, PipelineID p_pipeline) override final;
 	virtual void command_bind_render_uniform_set(CommandBufferID p_cmd_buffer, UniformSetID p_uniform_set, ShaderID p_shader, uint32_t p_set_index) override final;
+	virtual void command_bind_render_uniform_sets(CommandBufferID p_cmd_buffer, VectorView<UniformSetID> p_uniform_sets, ShaderID p_shader, uint32_t p_first_set_index, uint32_t p_set_count) override final;
 
 	// Drawing.
 	virtual void command_render_draw(CommandBufferID p_cmd_buffer, uint32_t p_vertex_count, uint32_t p_instance_count, uint32_t p_base_vertex, uint32_t p_first_instance) override final;
@@ -371,6 +373,7 @@ public:
 	// Binding.
 	virtual void command_bind_compute_pipeline(CommandBufferID p_cmd_buffer, PipelineID p_pipeline) override final;
 	virtual void command_bind_compute_uniform_set(CommandBufferID p_cmd_buffer, UniformSetID p_uniform_set, ShaderID p_shader, uint32_t p_set_index) override final;
+	virtual void command_bind_compute_uniform_sets(CommandBufferID p_cmd_buffer, VectorView<UniformSetID> p_uniform_sets, ShaderID p_shader, uint32_t p_first_set_index, uint32_t p_set_count) override final;
 
 	// Dispatching.
 	virtual void command_compute_dispatch(CommandBufferID p_cmd_buffer, uint32_t p_x_groups, uint32_t p_y_groups, uint32_t p_z_groups) override final;
@@ -413,6 +416,7 @@ public:
 	virtual void set_object_name(ObjectType p_type, ID p_driver_id, const String &p_name) override final;
 	virtual uint64_t get_resource_native_handle(DriverResource p_type, ID p_driver_id) override final;
 	virtual uint64_t get_total_memory_used() override final;
+	virtual uint64_t get_lazily_memory_used() override final;
 	virtual uint64_t limit_get(Limit p_limit) override final;
 	virtual uint64_t api_trait_get(ApiTrait p_trait) override final;
 	virtual bool has_feature(Features p_feature) override final;
diff --git a/drivers/metal/rendering_device_driver_metal.mm b/drivers/metal/rendering_device_driver_metal.mm
index 976020abf1..302e4dc552 100644
--- a/drivers/metal/rendering_device_driver_metal.mm
+++ b/drivers/metal/rendering_device_driver_metal.mm
@@ -275,11 +275,19 @@ RDD::TextureID RenderingDeviceDriverMetal::texture_create(const TextureFormat &p
 	}
 
 	// Usage.
-	MTLResourceOptions options = MTLResourceCPUCacheModeDefaultCache | MTLResourceHazardTrackingModeTracked;
-	if (p_format.usage_bits & TEXTURE_USAGE_CPU_READ_BIT) {
-		options |= MTLResourceStorageModeShared;
+
+	MTLResourceOptions options = 0;
+	const bool supports_memoryless = (*metal_device_properties).features.highestFamily >= MTLGPUFamilyApple2 && (*metal_device_properties).features.highestFamily < MTLGPUFamilyMac1;
+	if (supports_memoryless && p_format.usage_bits & TEXTURE_USAGE_TRANSIENT_BIT) {
+		options = MTLResourceStorageModeMemoryless | MTLResourceHazardTrackingModeTracked;
+		desc.storageMode = MTLStorageModeMemoryless;
 	} else {
-		options |= MTLResourceStorageModePrivate;
+		options = MTLResourceCPUCacheModeDefaultCache | MTLResourceHazardTrackingModeTracked;
+		if (p_format.usage_bits & TEXTURE_USAGE_CPU_READ_BIT) {
+			options |= MTLResourceStorageModeShared;
+		} else {
+			options |= MTLResourceStorageModePrivate;
+		}
 	}
 	desc.resourceOptions = options;
 
@@ -890,6 +898,10 @@ RDD::CommandPoolID RenderingDeviceDriverMetal::command_pool_create(CommandQueueF
 	return rid::make(device_queue);
 }
 
+bool RenderingDeviceDriverMetal::command_pool_reset(CommandPoolID p_cmd_pool) {
+	return true;
+}
+
 void RenderingDeviceDriverMetal::command_pool_free(CommandPoolID p_cmd_pool) {
 	rid::release(p_cmd_pool);
 }
@@ -2347,7 +2359,7 @@ void RenderingDeviceDriverMetal::shader_cache_free_entry(const SHA256Digest &key
 	}
 }
 
-RDD::ShaderID RenderingDeviceDriverMetal::shader_create_from_bytecode(const Vector<uint8_t> &p_shader_binary, ShaderDescription &r_shader_desc, String &r_name) {
+RDD::ShaderID RenderingDeviceDriverMetal::shader_create_from_bytecode(const Vector<uint8_t> &p_shader_binary, ShaderDescription &r_shader_desc, String &r_name, const Vector<ImmutableSampler> &p_immutable_samplers) {
 	r_shader_desc = {}; // Driver-agnostic.
 
 	const uint8_t *binptr = p_shader_binary.ptr();
@@ -2557,7 +2569,9 @@ void RenderingDeviceDriverMetal::shader_destroy_modules(ShaderID p_shader) {
 /**** UNIFORM SET ****/
 /*********************/
 
-RDD::UniformSetID RenderingDeviceDriverMetal::uniform_set_create(VectorView<BoundUniform> p_uniforms, ShaderID p_shader, uint32_t p_set_index) {
+RDD::UniformSetID RenderingDeviceDriverMetal::uniform_set_create(VectorView<BoundUniform> p_uniforms, ShaderID p_shader, uint32_t p_set_index, int p_linear_pool_index) {
+	// p_linear_pool_index = -1; // TODO:? Linear pools not implemented or not supported by API backend.
+
 	MDUniformSet *set = new MDUniformSet();
 	Vector<BoundUniform> bound_uniforms;
 	bound_uniforms.resize(p_uniforms.size());
@@ -3112,6 +3126,11 @@ void RenderingDeviceDriverMetal::command_bind_render_uniform_set(CommandBufferID
 	cb->render_bind_uniform_set(p_uniform_set, p_shader, p_set_index);
 }
 
+void RenderingDeviceDriverMetal::command_bind_render_uniform_sets(CommandBufferID p_cmd_buffer, VectorView<UniformSetID> p_uniform_sets, ShaderID p_shader, uint32_t p_first_set_index, uint32_t p_set_count) {
+	MDCommandBuffer *cb = (MDCommandBuffer *)(p_cmd_buffer.id);
+	cb->render_bind_uniform_sets(p_uniform_sets, p_shader, p_first_set_index, p_set_count);
+}
+
 void RenderingDeviceDriverMetal::command_render_draw(CommandBufferID p_cmd_buffer, uint32_t p_vertex_count, uint32_t p_instance_count, uint32_t p_base_vertex, uint32_t p_first_instance) {
 	MDCommandBuffer *cb = (MDCommandBuffer *)(p_cmd_buffer.id);
 	cb->render_draw(p_vertex_count, p_instance_count, p_base_vertex, p_first_instance);
@@ -3583,6 +3602,11 @@ void RenderingDeviceDriverMetal::command_bind_compute_uniform_set(CommandBufferI
 	cb->compute_bind_uniform_set(p_uniform_set, p_shader, p_set_index);
 }
 
+void RenderingDeviceDriverMetal::command_bind_compute_uniform_sets(CommandBufferID p_cmd_buffer, VectorView<UniformSetID> p_uniform_sets, ShaderID p_shader, uint32_t p_first_set_index, uint32_t p_set_count) {
+	MDCommandBuffer *cb = (MDCommandBuffer *)(p_cmd_buffer.id);
+	cb->compute_bind_uniform_sets(p_uniform_sets, p_shader, p_first_set_index, p_set_count);
+}
+
 void RenderingDeviceDriverMetal::command_compute_dispatch(CommandBufferID p_cmd_buffer, uint32_t p_x_groups, uint32_t p_y_groups, uint32_t p_z_groups) {
 	MDCommandBuffer *cb = (MDCommandBuffer *)(p_cmd_buffer.id);
 	cb->compute_dispatch(p_x_groups, p_y_groups, p_z_groups);
@@ -3786,6 +3810,10 @@ uint64_t RenderingDeviceDriverMetal::get_total_memory_used() {
 	return device.currentAllocatedSize;
 }
 
+uint64_t RenderingDeviceDriverMetal::get_lazily_memory_used() {
+	return 0; // TODO: Track this (grep for memoryless in Godot's Metal backend).
+}
+
 uint64_t RenderingDeviceDriverMetal::limit_get(Limit p_limit) {
 	MetalDeviceProperties const &props = (*metal_device_properties);
 	MetalLimits const &limits = props.limits;
diff --git a/drivers/vulkan/rendering_device_driver_vulkan.cpp b/drivers/vulkan/rendering_device_driver_vulkan.cpp
index b6e5ed0287..ae6cddc987 100644
--- a/drivers/vulkan/rendering_device_driver_vulkan.cpp
+++ b/drivers/vulkan/rendering_device_driver_vulkan.cpp
@@ -1383,6 +1383,15 @@ Error RenderingDeviceDriverVulkan::initialize(uint32_t p_device_index, uint32_t
 	physical_device = context_driver->physical_device_get(p_device_index);
 	vkGetPhysicalDeviceProperties(physical_device, &physical_device_properties);
 
+	// Workaround a driver bug on Adreno 730 GPUs that keeps leaking memory on each call to vkResetDescriptorPool.
+	// Which eventually run out of memory. in such case we should not be using linear allocated pools
+	// Bug introduced in driver 512.597.0 and fixed in 512.671.0
+	// Confirmed by Qualcomm
+	if (linear_descriptor_pools_enabled) {
+		const uint32_t reset_descriptor_pool_broken_driver_begin = VK_MAKE_VERSION(512u, 597u, 0u);
+		const uint32_t reset_descriptor_pool_fixed_driver_begin = VK_MAKE_VERSION(512u, 671u, 0u);
+		linear_descriptor_pools_enabled = physical_device_properties.driverVersion < reset_descriptor_pool_broken_driver_begin || physical_device_properties.driverVersion > reset_descriptor_pool_fixed_driver_begin;
+	}
 	frame_count = p_frame_count;
 
 	// Copy the queue family properties the context already retrieved.
@@ -1728,7 +1737,27 @@ RDD::TextureID RenderingDeviceDriverVulkan::texture_create(const TextureFormat &
 
 	VmaAllocationCreateInfo alloc_create_info = {};
 	alloc_create_info.flags = (p_format.usage_bits & TEXTURE_USAGE_CPU_READ_BIT) ? VMA_ALLOCATION_CREATE_HOST_ACCESS_RANDOM_BIT : 0;
-	alloc_create_info.preferredFlags = VK_MEMORY_PROPERTY_DEVICE_LOCAL_BIT;
+
+	if (p_format.usage_bits & TEXTURE_USAGE_TRANSIENT_BIT) {
+		uint32_t memory_type_index = 0;
+		VmaAllocationCreateInfo lazy_memory_requirements = alloc_create_info;
+		lazy_memory_requirements.usage = VMA_MEMORY_USAGE_GPU_LAZILY_ALLOCATED;
+		VkResult result = vmaFindMemoryTypeIndex(allocator, UINT32_MAX, &lazy_memory_requirements, &memory_type_index);
+		if (VK_SUCCESS == result) {
+			alloc_create_info = lazy_memory_requirements;
+			create_info.usage |= VK_IMAGE_USAGE_TRANSIENT_ATTACHMENT_BIT;
+			// VUID-VkImageCreateInfo-usage-00963 :
+			// If usage includes VK_IMAGE_USAGE_TRANSIENT_ATTACHMENT_BIT,
+			// then bits other than VK_IMAGE_USAGE_COLOR_ATTACHMENT_BIT, VK_IMAGE_USAGE_DEPTH_STENCIL_ATTACHMENT_BIT,
+			// and VK_IMAGE_USAGE_INPUT_ATTACHMENT_BIT must not be set
+			create_info.usage &= (VK_IMAGE_USAGE_TRANSIENT_ATTACHMENT_BIT | VK_IMAGE_USAGE_COLOR_ATTACHMENT_BIT | VK_IMAGE_USAGE_DEPTH_STENCIL_ATTACHMENT_BIT | VK_IMAGE_USAGE_INPUT_ATTACHMENT_BIT);
+		} else {
+			alloc_create_info.preferredFlags = VK_MEMORY_PROPERTY_DEVICE_LOCAL_BIT;
+		}
+	} else {
+		alloc_create_info.preferredFlags = VK_MEMORY_PROPERTY_DEVICE_LOCAL_BIT;
+	}
+
 	if (image_size <= SMALL_ALLOCATION_MAX_SIZE) {
 		uint32_t mem_type_index = 0;
 		vmaFindMemoryTypeIndexForImageInfo(allocator, &create_info, &alloc_create_info, &mem_type_index);
@@ -1794,6 +1823,9 @@ RDD::TextureID RenderingDeviceDriverVulkan::texture_create(const TextureFormat &
 	tex_info->vk_create_info = create_info;
 	tex_info->vk_view_create_info = image_view_create_info;
 	tex_info->allocation.handle = allocation;
+#ifdef DEBUG_ENABLED
+	tex_info->transient = (p_format.usage_bits & TEXTURE_USAGE_TRANSIENT_BIT) != 0;
+#endif
 	vmaGetAllocationInfo(allocator, tex_info->allocation.handle, &tex_info->allocation.info);
 
 #if PRINT_NATIVE_COMMANDS
@@ -2659,7 +2691,10 @@ RDD::CommandPoolID RenderingDeviceDriverVulkan::command_pool_create(CommandQueue
 	VkCommandPoolCreateInfo cmd_pool_info = {};
 	cmd_pool_info.sType = VK_STRUCTURE_TYPE_COMMAND_POOL_CREATE_INFO;
 	cmd_pool_info.queueFamilyIndex = family_index;
-	cmd_pool_info.flags = VK_COMMAND_POOL_CREATE_RESET_COMMAND_BUFFER_BIT;
+
+	if (!command_pool_reset_enabled) {
+		cmd_pool_info.flags = VK_COMMAND_POOL_CREATE_RESET_COMMAND_BUFFER_BIT;
+	}
 
 	VkCommandPool vk_command_pool = VK_NULL_HANDLE;
 	VkResult res = vkCreateCommandPool(vk_device, &cmd_pool_info, VKC::get_allocation_callbacks(VK_OBJECT_TYPE_COMMAND_POOL), &vk_command_pool);
@@ -2671,6 +2706,16 @@ RDD::CommandPoolID RenderingDeviceDriverVulkan::command_pool_create(CommandQueue
 	return CommandPoolID(command_pool);
 }
 
+bool RenderingDeviceDriverVulkan::command_pool_reset(CommandPoolID p_cmd_pool) {
+	DEV_ASSERT(p_cmd_pool);
+
+	CommandPool *command_pool = (CommandPool *)(p_cmd_pool.id);
+	VkResult err = vkResetCommandPool(vk_device, command_pool->vk_command_pool, 0);
+	ERR_FAIL_COND_V_MSG(err, false, "vkResetCommandPool failed with error " + itos(err) + ".");
+
+	return true;
+}
+
 void RenderingDeviceDriverVulkan::command_pool_free(CommandPoolID p_cmd_pool) {
 	DEV_ASSERT(p_cmd_pool);
 
@@ -2704,8 +2749,6 @@ RDD::CommandBufferID RenderingDeviceDriverVulkan::command_buffer_create(CommandP
 }
 
 bool RenderingDeviceDriverVulkan::command_buffer_begin(CommandBufferID p_cmd_buffer) {
-	// Reset is implicit (VK_COMMAND_POOL_CREATE_RESET_COMMAND_BUFFER_BIT).
-
 	VkCommandBufferBeginInfo cmd_buf_begin_info = {};
 	cmd_buf_begin_info.sType = VK_STRUCTURE_TYPE_COMMAND_BUFFER_BEGIN_INFO;
 	cmd_buf_begin_info.flags = VK_COMMAND_BUFFER_USAGE_ONE_TIME_SUBMIT_BIT;
@@ -2717,8 +2760,6 @@ bool RenderingDeviceDriverVulkan::command_buffer_begin(CommandBufferID p_cmd_buf
 }
 
 bool RenderingDeviceDriverVulkan::command_buffer_begin_secondary(CommandBufferID p_cmd_buffer, RenderPassID p_render_pass, uint32_t p_subpass, FramebufferID p_framebuffer) {
-	// Reset is implicit (VK_COMMAND_POOL_CREATE_RESET_COMMAND_BUFFER_BIT).
-
 	Framebuffer *framebuffer = (Framebuffer *)(p_framebuffer.id);
 
 	VkCommandBufferInheritanceInfo inheritance_info = {};
@@ -3477,7 +3518,7 @@ Vector<uint8_t> RenderingDeviceDriverVulkan::shader_compile_binary_from_spirv(Ve
 	return ret;
 }
 
-RDD::ShaderID RenderingDeviceDriverVulkan::shader_create_from_bytecode(const Vector<uint8_t> &p_shader_binary, ShaderDescription &r_shader_desc, String &r_name) {
+RDD::ShaderID RenderingDeviceDriverVulkan::shader_create_from_bytecode(const Vector<uint8_t> &p_shader_binary, ShaderDescription &r_shader_desc, String &r_name, const Vector<ImmutableSampler> &p_immutable_samplers) {
 	r_shader_desc = {}; // Driver-agnostic.
 	ShaderInfo shader_info; // Driver-specific.
 
@@ -3549,6 +3590,19 @@ RDD::ShaderID RenderingDeviceDriverVulkan::shader_create_from_bytecode(const Vec
 				case UNIFORM_TYPE_SAMPLER: {
 					layout_binding.descriptorType = VK_DESCRIPTOR_TYPE_SAMPLER;
 					layout_binding.descriptorCount = set_ptr[j].length;
+					// Immutable samplers: here they get set in the layoutbinding, given that they will not be changed later.
+					int immutable_bind_index = -1;
+					if (immutable_samplers_enabled && p_immutable_samplers.size() > 0) {
+						for (int k = 0; k < p_immutable_samplers.size(); k++) {
+							if (p_immutable_samplers[k].binding == layout_binding.binding) {
+								immutable_bind_index = k;
+								break;
+							}
+						}
+						if (immutable_bind_index >= 0) {
+							layout_binding.pImmutableSamplers = (VkSampler *)&p_immutable_samplers[immutable_bind_index].ids[0].id;
+						}
+					}
 				} break;
 				case UNIFORM_TYPE_SAMPLER_WITH_TEXTURE: {
 					layout_binding.descriptorType = VK_DESCRIPTOR_TYPE_COMBINED_IMAGE_SAMPLER;
@@ -3770,9 +3824,9 @@ void RenderingDeviceDriverVulkan::shader_destroy_modules(ShaderID p_shader) {
 /*********************/
 /**** UNIFORM SET ****/
 /*********************/
-
-VkDescriptorPool RenderingDeviceDriverVulkan::_descriptor_set_pool_find_or_create(const DescriptorSetPoolKey &p_key, DescriptorSetPools::Iterator *r_pool_sets_it) {
-	DescriptorSetPools::Iterator pool_sets_it = descriptor_set_pools.find(p_key);
+VkDescriptorPool RenderingDeviceDriverVulkan::_descriptor_set_pool_find_or_create(const DescriptorSetPoolKey &p_key, DescriptorSetPools::Iterator *r_pool_sets_it, int p_linear_pool_index) {
+	bool linear_pool = p_linear_pool_index >= 0;
+	DescriptorSetPools::Iterator pool_sets_it = linear_pool ? linear_descriptor_set_pools[p_linear_pool_index].find(p_key) : descriptor_set_pools.find(p_key);
 
 	if (pool_sets_it) {
 		for (KeyValue<VkDescriptorPool, uint32_t> &E : pool_sets_it->value) {
@@ -3858,7 +3912,11 @@ VkDescriptorPool RenderingDeviceDriverVulkan::_descriptor_set_pool_find_or_creat
 
 	VkDescriptorPoolCreateInfo descriptor_set_pool_create_info = {};
 	descriptor_set_pool_create_info.sType = VK_STRUCTURE_TYPE_DESCRIPTOR_POOL_CREATE_INFO;
-	descriptor_set_pool_create_info.flags = VK_DESCRIPTOR_POOL_CREATE_FREE_DESCRIPTOR_SET_BIT; // Can't think how somebody may NOT need this flag.
+	if (linear_descriptor_pools_enabled && linear_pool) {
+		descriptor_set_pool_create_info.flags = 0;
+	} else {
+		descriptor_set_pool_create_info.flags = VK_DESCRIPTOR_POOL_CREATE_FREE_DESCRIPTOR_SET_BIT; // Can't think how somebody may NOT need this flag.
+	}
 	descriptor_set_pool_create_info.maxSets = max_descriptor_sets_per_pool;
 	descriptor_set_pool_create_info.poolSizeCount = vk_sizes_count;
 	descriptor_set_pool_create_info.pPoolSizes = vk_sizes;
@@ -3872,7 +3930,11 @@ VkDescriptorPool RenderingDeviceDriverVulkan::_descriptor_set_pool_find_or_creat
 	// Bookkeep.
 
 	if (!pool_sets_it) {
-		pool_sets_it = descriptor_set_pools.insert(p_key, HashMap<VkDescriptorPool, uint32_t>());
+		if (linear_pool) {
+			pool_sets_it = linear_descriptor_set_pools[p_linear_pool_index].insert(p_key, HashMap<VkDescriptorPool, uint32_t>());
+		} else {
+			pool_sets_it = descriptor_set_pools.insert(p_key, HashMap<VkDescriptorPool, uint32_t>());
+		}
 	}
 	HashMap<VkDescriptorPool, uint32_t> &pool_rcs = pool_sets_it->value;
 	pool_rcs.insert(vk_pool, 0);
@@ -3880,34 +3942,43 @@ VkDescriptorPool RenderingDeviceDriverVulkan::_descriptor_set_pool_find_or_creat
 	return vk_pool;
 }
 
-void RenderingDeviceDriverVulkan::_descriptor_set_pool_unreference(DescriptorSetPools::Iterator p_pool_sets_it, VkDescriptorPool p_vk_descriptor_pool) {
+void RenderingDeviceDriverVulkan::_descriptor_set_pool_unreference(DescriptorSetPools::Iterator p_pool_sets_it, VkDescriptorPool p_vk_descriptor_pool, int p_linear_pool_index) {
 	HashMap<VkDescriptorPool, uint32_t>::Iterator pool_rcs_it = p_pool_sets_it->value.find(p_vk_descriptor_pool);
 	pool_rcs_it->value--;
 	if (pool_rcs_it->value == 0) {
 		vkDestroyDescriptorPool(vk_device, p_vk_descriptor_pool, VKC::get_allocation_callbacks(VK_OBJECT_TYPE_DESCRIPTOR_POOL));
 		p_pool_sets_it->value.erase(p_vk_descriptor_pool);
 		if (p_pool_sets_it->value.is_empty()) {
-			descriptor_set_pools.remove(p_pool_sets_it);
+			if (linear_descriptor_pools_enabled && p_linear_pool_index >= 0) {
+				linear_descriptor_set_pools[p_linear_pool_index].remove(p_pool_sets_it);
+			} else {
+				descriptor_set_pools.remove(p_pool_sets_it);
+			}
 		}
 	}
 }
 
-RDD::UniformSetID RenderingDeviceDriverVulkan::uniform_set_create(VectorView<BoundUniform> p_uniforms, ShaderID p_shader, uint32_t p_set_index) {
+RDD::UniformSetID RenderingDeviceDriverVulkan::uniform_set_create(VectorView<BoundUniform> p_uniforms, ShaderID p_shader, uint32_t p_set_index, int p_linear_pool_index) {
+	if (!linear_descriptor_pools_enabled) {
+		p_linear_pool_index = -1;
+	}
 	DescriptorSetPoolKey pool_key;
-
+	// Immutable samplers will be skipped so we need to track the number of vk_writes used.
 	VkWriteDescriptorSet *vk_writes = ALLOCA_ARRAY(VkWriteDescriptorSet, p_uniforms.size());
+	uint32_t writes_amount = 0;
 	for (uint32_t i = 0; i < p_uniforms.size(); i++) {
 		const BoundUniform &uniform = p_uniforms[i];
 
-		vk_writes[i] = {};
-		vk_writes[i].sType = VK_STRUCTURE_TYPE_WRITE_DESCRIPTOR_SET;
-		vk_writes[i].dstBinding = uniform.binding;
-		vk_writes[i].descriptorType = VK_DESCRIPTOR_TYPE_MAX_ENUM; // Invalid value.
+		vk_writes[writes_amount] = {};
+		vk_writes[writes_amount].sType = VK_STRUCTURE_TYPE_WRITE_DESCRIPTOR_SET;
 
 		uint32_t num_descriptors = 1;
 
 		switch (uniform.type) {
 			case UNIFORM_TYPE_SAMPLER: {
+				if (uniform.immutable_sampler && immutable_samplers_enabled) {
+					continue; // Skipping immutable samplers.
+				}
 				num_descriptors = uniform.ids.size();
 				VkDescriptorImageInfo *vk_img_infos = ALLOCA_ARRAY(VkDescriptorImageInfo, num_descriptors);
 
@@ -3918,48 +3989,63 @@ RDD::UniformSetID RenderingDeviceDriverVulkan::uniform_set_create(VectorView<Bou
 					vk_img_infos[j].imageLayout = VK_IMAGE_LAYOUT_UNDEFINED;
 				}
 
-				vk_writes[i].descriptorType = VK_DESCRIPTOR_TYPE_SAMPLER;
-				vk_writes[i].pImageInfo = vk_img_infos;
+				vk_writes[writes_amount].descriptorType = VK_DESCRIPTOR_TYPE_SAMPLER;
+				vk_writes[writes_amount].pImageInfo = vk_img_infos;
 			} break;
 			case UNIFORM_TYPE_SAMPLER_WITH_TEXTURE: {
 				num_descriptors = uniform.ids.size() / 2;
 				VkDescriptorImageInfo *vk_img_infos = ALLOCA_ARRAY(VkDescriptorImageInfo, num_descriptors);
 
 				for (uint32_t j = 0; j < num_descriptors; j++) {
+#ifdef DEBUG_ENABLED
+					if (((const TextureInfo *)uniform.ids[j * 2 + 1].id)->transient) {
+						ERR_PRINT("TEXTURE_USAGE_TRANSIENT_BIT texture must not be used for sampling in a shader.");
+					}
+#endif
 					vk_img_infos[j] = {};
 					vk_img_infos[j].sampler = (VkSampler)uniform.ids[j * 2 + 0].id;
 					vk_img_infos[j].imageView = ((const TextureInfo *)uniform.ids[j * 2 + 1].id)->vk_view;
 					vk_img_infos[j].imageLayout = VK_IMAGE_LAYOUT_SHADER_READ_ONLY_OPTIMAL;
 				}
 
-				vk_writes[i].descriptorType = VK_DESCRIPTOR_TYPE_COMBINED_IMAGE_SAMPLER;
-				vk_writes[i].pImageInfo = vk_img_infos;
+				vk_writes[writes_amount].descriptorType = VK_DESCRIPTOR_TYPE_COMBINED_IMAGE_SAMPLER;
+				vk_writes[writes_amount].pImageInfo = vk_img_infos;
 			} break;
 			case UNIFORM_TYPE_TEXTURE: {
 				num_descriptors = uniform.ids.size();
 				VkDescriptorImageInfo *vk_img_infos = ALLOCA_ARRAY(VkDescriptorImageInfo, num_descriptors);
 
 				for (uint32_t j = 0; j < num_descriptors; j++) {
+#ifdef DEBUG_ENABLED
+					if (((const TextureInfo *)uniform.ids[j].id)->transient) {
+						ERR_PRINT("TEXTURE_USAGE_TRANSIENT_BIT texture must not be used for sampling in a shader.");
+					}
+#endif
 					vk_img_infos[j] = {};
 					vk_img_infos[j].imageView = ((const TextureInfo *)uniform.ids[j].id)->vk_view;
 					vk_img_infos[j].imageLayout = VK_IMAGE_LAYOUT_SHADER_READ_ONLY_OPTIMAL;
 				}
 
-				vk_writes[i].descriptorType = VK_DESCRIPTOR_TYPE_SAMPLED_IMAGE;
-				vk_writes[i].pImageInfo = vk_img_infos;
+				vk_writes[writes_amount].descriptorType = VK_DESCRIPTOR_TYPE_SAMPLED_IMAGE;
+				vk_writes[writes_amount].pImageInfo = vk_img_infos;
 			} break;
 			case UNIFORM_TYPE_IMAGE: {
 				num_descriptors = uniform.ids.size();
 				VkDescriptorImageInfo *vk_img_infos = ALLOCA_ARRAY(VkDescriptorImageInfo, num_descriptors);
 
 				for (uint32_t j = 0; j < num_descriptors; j++) {
+#ifdef DEBUG_ENABLED
+					if (((const TextureInfo *)uniform.ids[j].id)->transient) {
+						ERR_PRINT("TEXTURE_USAGE_TRANSIENT_BIT texture must not be used for sampling in a shader.");
+					}
+#endif
 					vk_img_infos[j] = {};
 					vk_img_infos[j].imageView = ((const TextureInfo *)uniform.ids[j].id)->vk_view;
 					vk_img_infos[j].imageLayout = VK_IMAGE_LAYOUT_GENERAL;
 				}
 
-				vk_writes[i].descriptorType = VK_DESCRIPTOR_TYPE_STORAGE_IMAGE;
-				vk_writes[i].pImageInfo = vk_img_infos;
+				vk_writes[writes_amount].descriptorType = VK_DESCRIPTOR_TYPE_STORAGE_IMAGE;
+				vk_writes[writes_amount].pImageInfo = vk_img_infos;
 			} break;
 			case UNIFORM_TYPE_TEXTURE_BUFFER: {
 				num_descriptors = uniform.ids.size();
@@ -3975,9 +4061,9 @@ RDD::UniformSetID RenderingDeviceDriverVulkan::uniform_set_create(VectorView<Bou
 					vk_buf_views[j] = buf_info->vk_view;
 				}
 
-				vk_writes[i].descriptorType = VK_DESCRIPTOR_TYPE_UNIFORM_TEXEL_BUFFER;
-				vk_writes[i].pBufferInfo = vk_buf_infos;
-				vk_writes[i].pTexelBufferView = vk_buf_views;
+				vk_writes[writes_amount].descriptorType = VK_DESCRIPTOR_TYPE_UNIFORM_TEXEL_BUFFER;
+				vk_writes[writes_amount].pBufferInfo = vk_buf_infos;
+				vk_writes[writes_amount].pTexelBufferView = vk_buf_views;
 			} break;
 			case UNIFORM_TYPE_SAMPLER_WITH_TEXTURE_BUFFER: {
 				num_descriptors = uniform.ids.size() / 2;
@@ -3997,10 +4083,10 @@ RDD::UniformSetID RenderingDeviceDriverVulkan::uniform_set_create(VectorView<Bou
 					vk_buf_views[j] = buf_info->vk_view;
 				}
 
-				vk_writes[i].descriptorType = VK_DESCRIPTOR_TYPE_UNIFORM_TEXEL_BUFFER;
-				vk_writes[i].pImageInfo = vk_img_infos;
-				vk_writes[i].pBufferInfo = vk_buf_infos;
-				vk_writes[i].pTexelBufferView = vk_buf_views;
+				vk_writes[writes_amount].descriptorType = VK_DESCRIPTOR_TYPE_UNIFORM_TEXEL_BUFFER;
+				vk_writes[writes_amount].pImageInfo = vk_img_infos;
+				vk_writes[writes_amount].pBufferInfo = vk_buf_infos;
+				vk_writes[writes_amount].pTexelBufferView = vk_buf_views;
 			} break;
 			case UNIFORM_TYPE_IMAGE_BUFFER: {
 				CRASH_NOW_MSG("Unimplemented!"); // TODO.
@@ -4012,8 +4098,8 @@ RDD::UniformSetID RenderingDeviceDriverVulkan::uniform_set_create(VectorView<Bou
 				vk_buf_info->buffer = buf_info->vk_buffer;
 				vk_buf_info->range = buf_info->size;
 
-				vk_writes[i].descriptorType = VK_DESCRIPTOR_TYPE_UNIFORM_BUFFER;
-				vk_writes[i].pBufferInfo = vk_buf_info;
+				vk_writes[writes_amount].descriptorType = VK_DESCRIPTOR_TYPE_UNIFORM_BUFFER;
+				vk_writes[writes_amount].pBufferInfo = vk_buf_info;
 			} break;
 			case UNIFORM_TYPE_STORAGE_BUFFER: {
 				const BufferInfo *buf_info = (const BufferInfo *)uniform.ids[0].id;
@@ -4022,8 +4108,8 @@ RDD::UniformSetID RenderingDeviceDriverVulkan::uniform_set_create(VectorView<Bou
 				vk_buf_info->buffer = buf_info->vk_buffer;
 				vk_buf_info->range = buf_info->size;
 
-				vk_writes[i].descriptorType = VK_DESCRIPTOR_TYPE_STORAGE_BUFFER;
-				vk_writes[i].pBufferInfo = vk_buf_info;
+				vk_writes[writes_amount].descriptorType = VK_DESCRIPTOR_TYPE_STORAGE_BUFFER;
+				vk_writes[writes_amount].pBufferInfo = vk_buf_info;
 			} break;
 			case UNIFORM_TYPE_INPUT_ATTACHMENT: {
 				num_descriptors = uniform.ids.size();
@@ -4035,24 +4121,26 @@ RDD::UniformSetID RenderingDeviceDriverVulkan::uniform_set_create(VectorView<Bou
 					vk_img_infos[j].imageLayout = VK_IMAGE_LAYOUT_SHADER_READ_ONLY_OPTIMAL;
 				}
 
-				vk_writes[i].descriptorType = VK_DESCRIPTOR_TYPE_INPUT_ATTACHMENT;
-				vk_writes[i].pImageInfo = vk_img_infos;
+				vk_writes[writes_amount].descriptorType = VK_DESCRIPTOR_TYPE_INPUT_ATTACHMENT;
+				vk_writes[writes_amount].pImageInfo = vk_img_infos;
 			} break;
 			default: {
 				DEV_ASSERT(false);
 			}
 		}
 
-		vk_writes[i].descriptorCount = num_descriptors;
+		vk_writes[writes_amount].dstBinding = uniform.binding;
+		vk_writes[writes_amount].descriptorCount = num_descriptors;
 
 		ERR_FAIL_COND_V_MSG(pool_key.uniform_type[uniform.type] == MAX_UNIFORM_POOL_ELEMENT, UniformSetID(),
 				"Uniform set reached the limit of bindings for the same type (" + itos(MAX_UNIFORM_POOL_ELEMENT) + ").");
 		pool_key.uniform_type[uniform.type] += num_descriptors;
+		writes_amount++;
 	}
 
 	// Need a descriptor pool.
 	DescriptorSetPools::Iterator pool_sets_it;
-	VkDescriptorPool vk_pool = _descriptor_set_pool_find_or_create(pool_key, &pool_sets_it);
+	VkDescriptorPool vk_pool = _descriptor_set_pool_find_or_create(pool_key, &pool_sets_it, p_linear_pool_index);
 	DEV_ASSERT(vk_pool);
 	pool_sets_it->value[vk_pool]++;
 
@@ -4064,22 +4152,27 @@ RDD::UniformSetID RenderingDeviceDriverVulkan::uniform_set_create(VectorView<Bou
 	descriptor_set_allocate_info.pSetLayouts = &shader_info->vk_descriptor_set_layouts[p_set_index];
 
 	VkDescriptorSet vk_descriptor_set = VK_NULL_HANDLE;
+
 	VkResult res = vkAllocateDescriptorSets(vk_device, &descriptor_set_allocate_info, &vk_descriptor_set);
 	if (res) {
-		_descriptor_set_pool_unreference(pool_sets_it, vk_pool);
+		_descriptor_set_pool_unreference(pool_sets_it, vk_pool, p_linear_pool_index);
 		ERR_FAIL_V_MSG(UniformSetID(), "Cannot allocate descriptor sets, error " + itos(res) + ".");
 	}
 
-	for (uint32_t i = 0; i < p_uniforms.size(); i++) {
+	for (uint32_t i = 0; i < writes_amount; i++) {
 		vk_writes[i].dstSet = vk_descriptor_set;
 	}
-	vkUpdateDescriptorSets(vk_device, p_uniforms.size(), vk_writes, 0, nullptr);
+	vkUpdateDescriptorSets(vk_device, writes_amount, vk_writes, 0, nullptr);
 
 	// Bookkeep.
 
 	UniformSetInfo *usi = VersatileResource::allocate<UniformSetInfo>(resources_allocator);
 	usi->vk_descriptor_set = vk_descriptor_set;
-	usi->vk_descriptor_pool = vk_pool;
+	if (p_linear_pool_index >= 0) {
+		usi->vk_linear_descriptor_pool = vk_pool;
+	} else {
+		usi->vk_descriptor_pool = vk_pool;
+	}
 	usi->pool_sets_it = pool_sets_it;
 
 	return UniformSetID(usi);
@@ -4087,13 +4180,43 @@ RDD::UniformSetID RenderingDeviceDriverVulkan::uniform_set_create(VectorView<Bou
 
 void RenderingDeviceDriverVulkan::uniform_set_free(UniformSetID p_uniform_set) {
 	UniformSetInfo *usi = (UniformSetInfo *)p_uniform_set.id;
-	vkFreeDescriptorSets(vk_device, usi->vk_descriptor_pool, 1, &usi->vk_descriptor_set);
 
-	_descriptor_set_pool_unreference(usi->pool_sets_it, usi->vk_descriptor_pool);
+	if (usi->vk_linear_descriptor_pool) {
+		// Nothing to do. All sets are freed at once using vkResetDescriptorPool.
+		//
+		// We can NOT decrease the reference count (i.e. call _descriptor_set_pool_unreference())
+		// because the pool is linear (i.e. the freed set can't be recycled) and further calls to
+		// _descriptor_set_pool_find_or_create() need usi->pool_sets_it->value to stay so that we can
+		// tell if the pool has ran out of space and we need to create a new pool.
+	} else {
+		vkFreeDescriptorSets(vk_device, usi->vk_descriptor_pool, 1, &usi->vk_descriptor_set);
+		_descriptor_set_pool_unreference(usi->pool_sets_it, usi->vk_descriptor_pool, -1);
+	}
 
 	VersatileResource::free(resources_allocator, usi);
 }
 
+bool RenderingDeviceDriverVulkan::uniform_sets_have_linear_pools() const {
+	return true;
+}
+
+void RenderingDeviceDriverVulkan::linear_uniform_set_pools_reset(int p_linear_pool_index) {
+	if (linear_descriptor_pools_enabled) {
+		DescriptorSetPools &pools_to_reset = linear_descriptor_set_pools[p_linear_pool_index];
+		DescriptorSetPools::Iterator curr_pool = pools_to_reset.begin();
+
+		while (curr_pool != pools_to_reset.end()) {
+			HashMap<VkDescriptorPool, uint32_t>::Iterator curr_pair = curr_pool->value.begin();
+			while (curr_pair != curr_pool->value.end()) {
+				vkResetDescriptorPool(vk_device, curr_pair->key, 0);
+				curr_pair->value = 0;
+				++curr_pair;
+			}
+			++curr_pool;
+		}
+	}
+}
+
 // ----- COMMANDS -----
 
 void RenderingDeviceDriverVulkan::command_uniform_set_prepare_for_use(CommandBufferID p_cmd_buffer, UniformSetID p_uniform_set, ShaderID p_shader, uint32_t p_set_index) {
@@ -4168,6 +4291,16 @@ void RenderingDeviceDriverVulkan::command_copy_texture(CommandBufferID p_cmd_buf
 
 	const TextureInfo *src_tex_info = (const TextureInfo *)p_src_texture.id;
 	const TextureInfo *dst_tex_info = (const TextureInfo *)p_dst_texture.id;
+
+#ifdef DEBUG_ENABLED
+	if (src_tex_info->transient) {
+		ERR_PRINT("TEXTURE_USAGE_TRANSIENT_BIT p_src_texture must not be used in command_copy_texture.");
+	}
+	if (dst_tex_info->transient) {
+		ERR_PRINT("TEXTURE_USAGE_TRANSIENT_BIT p_dst_texture must not be used in command_copy_texture.");
+	}
+#endif
+
 	vkCmdCopyImage((VkCommandBuffer)p_cmd_buffer.id, src_tex_info->vk_view_create_info.image, RD_TO_VK_LAYOUT[p_src_texture_layout], dst_tex_info->vk_view_create_info.image, RD_TO_VK_LAYOUT[p_dst_texture_layout], p_regions.size(), vk_copy_regions);
 }
 
@@ -4188,6 +4321,15 @@ void RenderingDeviceDriverVulkan::command_resolve_texture(CommandBufferID p_cmd_
 	vk_resolve.extent.height = MAX(1u, src_tex_info->vk_create_info.extent.height >> p_src_mipmap);
 	vk_resolve.extent.depth = MAX(1u, src_tex_info->vk_create_info.extent.depth >> p_src_mipmap);
 
+#ifdef DEBUG_ENABLED
+	if (src_tex_info->transient) {
+		ERR_PRINT("TEXTURE_USAGE_TRANSIENT_BIT p_src_texture must not be used in command_resolve_texture. Use a resolve store action pass instead.");
+	}
+	if (dst_tex_info->transient) {
+		ERR_PRINT("TEXTURE_USAGE_TRANSIENT_BIT p_dst_texture must not be used in command_resolve_texture.");
+	}
+#endif
+
 	vkCmdResolveImage((VkCommandBuffer)p_cmd_buffer.id, src_tex_info->vk_view_create_info.image, RD_TO_VK_LAYOUT[p_src_texture_layout], dst_tex_info->vk_view_create_info.image, RD_TO_VK_LAYOUT[p_dst_texture_layout], 1, &vk_resolve);
 }
 
@@ -4199,6 +4341,11 @@ void RenderingDeviceDriverVulkan::command_clear_color_texture(CommandBufferID p_
 	_texture_subresource_range_to_vk(p_subresources, &vk_subresources);
 
 	const TextureInfo *tex_info = (const TextureInfo *)p_texture.id;
+#ifdef DEBUG_ENABLED
+	if (tex_info->transient) {
+		ERR_PRINT("TEXTURE_USAGE_TRANSIENT_BIT p_texture must not be used in command_clear_color_texture. Use a clear store action pass instead.");
+	}
+#endif
 	vkCmdClearColorImage((VkCommandBuffer)p_cmd_buffer.id, tex_info->vk_view_create_info.image, RD_TO_VK_LAYOUT[p_texture_layout], &vk_color, 1, &vk_subresources);
 }
 
@@ -4210,6 +4357,11 @@ void RenderingDeviceDriverVulkan::command_copy_buffer_to_texture(CommandBufferID
 
 	const BufferInfo *buf_info = (const BufferInfo *)p_src_buffer.id;
 	const TextureInfo *tex_info = (const TextureInfo *)p_dst_texture.id;
+#ifdef DEBUG_ENABLED
+	if (tex_info->transient) {
+		ERR_PRINT("TEXTURE_USAGE_TRANSIENT_BIT p_dst_texture must not be used in command_copy_buffer_to_texture.");
+	}
+#endif
 	vkCmdCopyBufferToImage((VkCommandBuffer)p_cmd_buffer.id, buf_info->vk_buffer, tex_info->vk_view_create_info.image, RD_TO_VK_LAYOUT[p_dst_texture_layout], p_regions.size(), vk_copy_regions);
 }
 
@@ -4221,6 +4373,11 @@ void RenderingDeviceDriverVulkan::command_copy_texture_to_buffer(CommandBufferID
 
 	const TextureInfo *tex_info = (const TextureInfo *)p_src_texture.id;
 	const BufferInfo *buf_info = (const BufferInfo *)p_dst_buffer.id;
+#ifdef DEBUG_ENABLED
+	if (tex_info->transient) {
+		ERR_PRINT("TEXTURE_USAGE_TRANSIENT_BIT p_src_texture must not be used in command_copy_texture_to_buffer.");
+	}
+#endif
 	vkCmdCopyImageToBuffer((VkCommandBuffer)p_cmd_buffer.id, tex_info->vk_view_create_info.image, RD_TO_VK_LAYOUT[p_src_texture_layout], buf_info->vk_buffer, p_regions.size(), vk_copy_regions);
 }
 
@@ -4602,6 +4759,23 @@ void RenderingDeviceDriverVulkan::command_bind_render_uniform_set(CommandBufferI
 	vkCmdBindDescriptorSets((VkCommandBuffer)p_cmd_buffer.id, VK_PIPELINE_BIND_POINT_GRAPHICS, shader_info->vk_pipeline_layout, p_set_index, 1, &usi->vk_descriptor_set, 0, nullptr);
 }
 
+void RenderingDeviceDriverVulkan::command_bind_render_uniform_sets(CommandBufferID p_cmd_buffer, VectorView<UniformSetID> p_uniform_sets, ShaderID p_shader, uint32_t p_first_set_index, uint32_t p_set_count) {
+	if (p_set_count == 0) {
+		return;
+	}
+
+	thread_local LocalVector<VkDescriptorSet> sets;
+	sets.clear();
+	sets.resize(p_set_count);
+
+	for (uint32_t i = 0; i < p_set_count; i++) {
+		sets[i] = ((const UniformSetInfo *)p_uniform_sets[i].id)->vk_descriptor_set;
+	}
+
+	const ShaderInfo *shader_info = (const ShaderInfo *)p_shader.id;
+	vkCmdBindDescriptorSets((VkCommandBuffer)p_cmd_buffer.id, VK_PIPELINE_BIND_POINT_GRAPHICS, shader_info->vk_pipeline_layout, p_first_set_index, p_set_count, &sets[0], 0, nullptr);
+}
+
 void RenderingDeviceDriverVulkan::command_render_draw(CommandBufferID p_cmd_buffer, uint32_t p_vertex_count, uint32_t p_instance_count, uint32_t p_base_vertex, uint32_t p_first_instance) {
 	vkCmdDraw((VkCommandBuffer)p_cmd_buffer.id, p_vertex_count, p_instance_count, p_base_vertex, p_first_instance);
 }
@@ -5017,6 +5191,23 @@ void RenderingDeviceDriverVulkan::command_bind_compute_uniform_set(CommandBuffer
 	vkCmdBindDescriptorSets((VkCommandBuffer)p_cmd_buffer.id, VK_PIPELINE_BIND_POINT_COMPUTE, shader_info->vk_pipeline_layout, p_set_index, 1, &usi->vk_descriptor_set, 0, nullptr);
 }
 
+void RenderingDeviceDriverVulkan::command_bind_compute_uniform_sets(CommandBufferID p_cmd_buffer, VectorView<UniformSetID> p_uniform_sets, ShaderID p_shader, uint32_t p_first_set_index, uint32_t p_set_count) {
+	if (p_set_count == 0) {
+		return;
+	}
+
+	thread_local LocalVector<VkDescriptorSet> sets;
+	sets.clear();
+	sets.resize(p_set_count);
+
+	for (uint32_t i = 0; i < p_set_count; i++) {
+		sets[i] = ((const UniformSetInfo *)p_uniform_sets[i].id)->vk_descriptor_set;
+	}
+
+	const ShaderInfo *shader_info = (const ShaderInfo *)p_shader.id;
+	vkCmdBindDescriptorSets((VkCommandBuffer)p_cmd_buffer.id, VK_PIPELINE_BIND_POINT_COMPUTE, shader_info->vk_pipeline_layout, p_first_set_index, p_set_count, &sets[0], 0, nullptr);
+}
+
 void RenderingDeviceDriverVulkan::command_compute_dispatch(CommandBufferID p_cmd_buffer, uint32_t p_x_groups, uint32_t p_y_groups, uint32_t p_z_groups) {
 	vkCmdDispatch((VkCommandBuffer)p_cmd_buffer.id, p_x_groups, p_y_groups, p_z_groups);
 }
@@ -5557,6 +5748,10 @@ uint64_t RenderingDeviceDriverVulkan::get_total_memory_used() {
 	return stats.total.statistics.allocationBytes;
 }
 
+uint64_t RenderingDeviceDriverVulkan::get_lazily_memory_used() {
+	return vmaCalculateLazilyAllocatedBytes(allocator);
+}
+
 uint64_t RenderingDeviceDriverVulkan::limit_get(Limit p_limit) {
 	const VkPhysicalDeviceLimits &limits = physical_device_properties.limits;
 	switch (p_limit) {
@@ -5730,6 +5925,15 @@ RenderingDeviceDriverVulkan::~RenderingDeviceDriverVulkan() {
 	}
 	vmaDestroyAllocator(allocator);
 
+	// Destroy linearly allocated descriptor pools
+	for (KeyValue<int, DescriptorSetPools> &pool_map : linear_descriptor_set_pools) {
+		for (KeyValue<DescriptorSetPoolKey, HashMap<VkDescriptorPool, uint32_t>> pools : pool_map.value) {
+			for (KeyValue<VkDescriptorPool, uint32_t> descriptor_pool : pools.value) {
+				vkDestroyDescriptorPool(vk_device, descriptor_pool.key, VKC::get_allocation_callbacks(VK_OBJECT_TYPE_DESCRIPTOR_POOL));
+			}
+		}
+	}
+
 	if (vk_device != VK_NULL_HANDLE) {
 		vkDestroyDevice(vk_device, VKC::get_allocation_callbacks(VK_OBJECT_TYPE_DEVICE));
 	}
diff --git a/drivers/vulkan/rendering_device_driver_vulkan.h b/drivers/vulkan/rendering_device_driver_vulkan.h
index 06cd2a31be..ba47848345 100644
--- a/drivers/vulkan/rendering_device_driver_vulkan.h
+++ b/drivers/vulkan/rendering_device_driver_vulkan.h
@@ -221,6 +221,7 @@ public:
 		} allocation; // All 0/null if just a view.
 #ifdef DEBUG_ENABLED
 		bool created_from_extension = false;
+		bool transient = false;
 #endif
 	};
 
@@ -333,6 +334,7 @@ private:
 
 public:
 	virtual CommandPoolID command_pool_create(CommandQueueFamilyID p_cmd_queue_family, CommandBufferType p_cmd_buffer_type) override final;
+	virtual bool command_pool_reset(CommandPoolID p_cmd_pool) override final;
 	virtual void command_pool_free(CommandPoolID p_cmd_pool) override final;
 
 	// ----- BUFFER -----
@@ -444,7 +446,7 @@ private:
 public:
 	virtual String shader_get_binary_cache_key() override final;
 	virtual Vector<uint8_t> shader_compile_binary_from_spirv(VectorView<ShaderStageSPIRVData> p_spirv, const String &p_shader_name) override final;
-	virtual ShaderID shader_create_from_bytecode(const Vector<uint8_t> &p_shader_binary, ShaderDescription &r_shader_desc, String &r_name) override final;
+	virtual ShaderID shader_create_from_bytecode(const Vector<uint8_t> &p_shader_binary, ShaderDescription &r_shader_desc, String &r_name, const Vector<ImmutableSampler> &p_immutable_samplers) override final;
 	virtual void shader_free(ShaderID p_shader) override final;
 
 	virtual void shader_destroy_modules(ShaderID p_shader) override final;
@@ -482,18 +484,27 @@ private:
 	DescriptorSetPools descriptor_set_pools;
 	uint32_t max_descriptor_sets_per_pool = 0;
 
-	VkDescriptorPool _descriptor_set_pool_find_or_create(const DescriptorSetPoolKey &p_key, DescriptorSetPools::Iterator *r_pool_sets_it);
-	void _descriptor_set_pool_unreference(DescriptorSetPools::Iterator p_pool_sets_it, VkDescriptorPool p_vk_descriptor_pool);
+	HashMap<int, DescriptorSetPools> linear_descriptor_set_pools;
+	bool linear_descriptor_pools_enabled = true;
+	VkDescriptorPool _descriptor_set_pool_find_or_create(const DescriptorSetPoolKey &p_key, DescriptorSetPools::Iterator *r_pool_sets_it, int p_linear_pool_index);
+	void _descriptor_set_pool_unreference(DescriptorSetPools::Iterator p_pool_sets_it, VkDescriptorPool p_vk_descriptor_pool, int p_linear_pool_index);
+
+	// Global flag to toggle usage of immutable sampler when creating pipeline layouts.
+	// It cannot change after creating the PSOs, since we need to skipping samplers when creating uniform sets.
+	bool immutable_samplers_enabled = true;
 
 	struct UniformSetInfo {
 		VkDescriptorSet vk_descriptor_set = VK_NULL_HANDLE;
 		VkDescriptorPool vk_descriptor_pool = VK_NULL_HANDLE;
+		VkDescriptorPool vk_linear_descriptor_pool = VK_NULL_HANDLE;
 		DescriptorSetPools::Iterator pool_sets_it;
 	};
 
 public:
-	virtual UniformSetID uniform_set_create(VectorView<BoundUniform> p_uniforms, ShaderID p_shader, uint32_t p_set_index) override final;
+	virtual UniformSetID uniform_set_create(VectorView<BoundUniform> p_uniforms, ShaderID p_shader, uint32_t p_set_index, int p_linear_pool_index) override final;
+	virtual void linear_uniform_set_pools_reset(int p_linear_pool_index) override final;
 	virtual void uniform_set_free(UniformSetID p_uniform_set) override final;
+	virtual bool uniform_sets_have_linear_pools() const override final;
 
 	// ----- COMMANDS -----
 
@@ -575,6 +586,7 @@ public:
 	// Binding.
 	virtual void command_bind_render_pipeline(CommandBufferID p_cmd_buffer, PipelineID p_pipeline) override final;
 	virtual void command_bind_render_uniform_set(CommandBufferID p_cmd_buffer, UniformSetID p_uniform_set, ShaderID p_shader, uint32_t p_set_index) override final;
+	virtual void command_bind_render_uniform_sets(CommandBufferID p_cmd_buffer, VectorView<UniformSetID> p_uniform_sets, ShaderID p_shader, uint32_t p_first_set_index, uint32_t p_set_count) override final;
 
 	// Drawing.
 	virtual void command_render_draw(CommandBufferID p_cmd_buffer, uint32_t p_vertex_count, uint32_t p_instance_count, uint32_t p_base_vertex, uint32_t p_first_instance) override final;
@@ -617,6 +629,7 @@ public:
 	// Binding.
 	virtual void command_bind_compute_pipeline(CommandBufferID p_cmd_buffer, PipelineID p_pipeline) override final;
 	virtual void command_bind_compute_uniform_set(CommandBufferID p_cmd_buffer, UniformSetID p_uniform_set, ShaderID p_shader, uint32_t p_set_index) override final;
+	virtual void command_bind_compute_uniform_sets(CommandBufferID p_cmd_buffer, VectorView<UniformSetID> p_uniform_sets, ShaderID p_shader, uint32_t p_first_set_index, uint32_t p_set_count) override final;
 
 	// Dispatching.
 	virtual void command_compute_dispatch(CommandBufferID p_cmd_buffer, uint32_t p_x_groups, uint32_t p_y_groups, uint32_t p_z_groups) override final;
@@ -671,7 +684,7 @@ public:
 	virtual void set_object_name(ObjectType p_type, ID p_driver_id, const String &p_name) override final;
 	virtual uint64_t get_resource_native_handle(DriverResource p_type, ID p_driver_id) override final;
 	virtual uint64_t get_total_memory_used() override final;
-
+	virtual uint64_t get_lazily_memory_used() override final;
 	virtual uint64_t limit_get(Limit p_limit) override final;
 	virtual uint64_t api_trait_get(ApiTrait p_trait) override final;
 	virtual bool has_feature(Features p_feature) override final;
diff --git a/servers/rendering/renderer_rd/forward_mobile/render_forward_mobile.cpp b/servers/rendering/renderer_rd/forward_mobile/render_forward_mobile.cpp
index e2d6796566..a1d05bd2f9 100644
--- a/servers/rendering/renderer_rd/forward_mobile/render_forward_mobile.cpp
+++ b/servers/rendering/renderer_rd/forward_mobile/render_forward_mobile.cpp
@@ -614,8 +614,7 @@ RID RenderForwardMobile::_setup_render_pass_uniform_set(RenderListType p_render_
 	if (render_pass_uniform_sets[p_index].is_valid() && RD::get_singleton()->uniform_set_is_valid(render_pass_uniform_sets[p_index])) {
 		RD::get_singleton()->free(render_pass_uniform_sets[p_index]);
 	}
-
-	render_pass_uniform_sets[p_index] = RD::get_singleton()->uniform_set_create(uniforms, scene_shader.default_shader_rd, RENDER_PASS_UNIFORM_SET);
+	render_pass_uniform_sets[p_index] = RD::get_singleton()->uniform_set_create(uniforms, scene_shader.default_shader_rd, RENDER_PASS_UNIFORM_SET, true);
 	return render_pass_uniform_sets[p_index];
 }
 
@@ -1664,7 +1663,9 @@ void RenderForwardMobile::base_uniforms_changed() {
 void RenderForwardMobile::_update_render_base_uniform_set() {
 	RendererRD::LightStorage *light_storage = RendererRD::LightStorage::get_singleton();
 
-	if (render_base_uniform_set.is_null() || !RD::get_singleton()->uniform_set_is_valid(render_base_uniform_set) || (lightmap_texture_array_version != light_storage->lightmap_array_get_version())) {
+	// We must always recreate the uniform set every frame if we're using linear pools (since we requested it on creation).
+	// This pays off as long as we often get inside the if() block (i.e. the settings end up changing often).
+	if (RD::get_singleton()->uniform_sets_have_linear_pools() || render_base_uniform_set.is_null() || !RD::get_singleton()->uniform_set_is_valid(render_base_uniform_set) || (lightmap_texture_array_version != light_storage->lightmap_array_get_version())) {
 		if (render_base_uniform_set.is_valid() && RD::get_singleton()->uniform_set_is_valid(render_base_uniform_set)) {
 			RD::get_singleton()->free(render_base_uniform_set);
 		}
@@ -1678,6 +1679,7 @@ void RenderForwardMobile::_update_render_base_uniform_set() {
 			u.binding = 2;
 			u.uniform_type = RD::UNIFORM_TYPE_SAMPLER;
 			u.append_id(scene_shader.shadow_sampler);
+			u.immutable_sampler = true;
 			uniforms.push_back(u);
 		}
 
@@ -1764,7 +1766,7 @@ void RenderForwardMobile::_update_render_base_uniform_set() {
 			uniforms.push_back(u);
 		}
 
-		render_base_uniform_set = RD::get_singleton()->uniform_set_create(uniforms, scene_shader.default_shader_rd, SCENE_UNIFORM_SET);
+		render_base_uniform_set = RD::get_singleton()->uniform_set_create(uniforms, scene_shader.default_shader_rd, SCENE_UNIFORM_SET, true);
 	}
 }
 
diff --git a/servers/rendering/renderer_rd/forward_mobile/scene_shader_forward_mobile.cpp b/servers/rendering/renderer_rd/forward_mobile/scene_shader_forward_mobile.cpp
index 6274dd7644..f9bd29e1c4 100644
--- a/servers/rendering/renderer_rd/forward_mobile/scene_shader_forward_mobile.cpp
+++ b/servers/rendering/renderer_rd/forward_mobile/scene_shader_forward_mobile.cpp
@@ -462,6 +462,16 @@ SceneShaderForwardMobile::SceneShaderForwardMobile() {
 void SceneShaderForwardMobile::init(const String p_defines) {
 	RendererRD::MaterialStorage *material_storage = RendererRD::MaterialStorage::get_singleton();
 
+	// Immutable samplers : create the shadow sampler to be passed when creating the pipeline.
+	{
+		RD::SamplerState sampler;
+		sampler.mag_filter = RD::SAMPLER_FILTER_LINEAR;
+		sampler.min_filter = RD::SAMPLER_FILTER_LINEAR;
+		sampler.enable_compare = true;
+		sampler.compare_op = RD::COMPARE_OP_GREATER;
+		shadow_sampler = RD::get_singleton()->sampler_create(sampler);
+	}
+
 	/* SCENE SHADER */
 
 	{
@@ -480,8 +490,13 @@ void SceneShaderForwardMobile::init(const String p_defines) {
 			shader_versions.push_back(base_define + "\n#define USE_MULTIVIEW\n#define MODE_RENDER_DEPTH\n"); // SHADER_VERSION_SHADOW_PASS_MULTIVIEW
 		}
 
-		shader.initialize(shader_versions, p_defines);
-
+		Vector<RD::PipelineImmutableSampler> immutable_samplers;
+		RD::PipelineImmutableSampler immutable_shadow_sampler;
+		immutable_shadow_sampler.binding = 2;
+		immutable_shadow_sampler.append_id(shadow_sampler);
+		immutable_shadow_sampler.uniform_type = RenderingDeviceCommons::UNIFORM_TYPE_SAMPLER;
+		immutable_samplers.push_back(immutable_shadow_sampler);
+		shader.initialize(shader_versions, p_defines, immutable_samplers);
 		if (!RendererCompositorRD::get_singleton()->is_xr_enabled()) {
 			for (uint32_t ubershader = 0; ubershader < 2; ubershader++) {
 				uint32_t base_variant = ubershader ? SHADER_VERSION_MAX : 0;
@@ -782,14 +797,6 @@ void fragment() {
 
 		default_vec4_xform_uniform_set = RD::get_singleton()->uniform_set_create(uniforms, default_shader_rd, RenderForwardMobile::TRANSFORMS_UNIFORM_SET);
 	}
-	{
-		RD::SamplerState sampler;
-		sampler.mag_filter = RD::SAMPLER_FILTER_LINEAR;
-		sampler.min_filter = RD::SAMPLER_FILTER_LINEAR;
-		sampler.enable_compare = true;
-		sampler.compare_op = RD::COMPARE_OP_GREATER;
-		shadow_sampler = RD::get_singleton()->sampler_create(sampler);
-	}
 }
 
 void SceneShaderForwardMobile::set_default_specialization(const ShaderSpecialization &p_specialization) {
diff --git a/servers/rendering/renderer_rd/renderer_canvas_render_rd.cpp b/servers/rendering/renderer_rd/renderer_canvas_render_rd.cpp
index 8367f6e29d..57559b99be 100644
--- a/servers/rendering/renderer_rd/renderer_canvas_render_rd.cpp
+++ b/servers/rendering/renderer_rd/renderer_canvas_render_rd.cpp
@@ -1846,6 +1846,10 @@ RendererCanvasRenderRD::RendererCanvasRenderRD() {
 		for (int i = 0; i < 2; i++) {
 			shadow_render.sdf_render_pipelines[i] = RD::get_singleton()->render_pipeline_create(shadow_render.shader.version_get_shader(shadow_render.shader_version, SHADOW_RENDER_MODE_SDF), shadow_render.sdf_framebuffer_format, shadow_render.sdf_vertex_format, i == 0 ? RD::RENDER_PRIMITIVE_TRIANGLES : RD::RENDER_PRIMITIVE_LINES, RD::PipelineRasterizationState(), RD::PipelineMultisampleState(), RD::PipelineDepthStencilState(), RD::PipelineColorBlendState::create_disabled(), 0);
 		}
+
+		// Unload shader modules to save memory.
+		RD::get_singleton()->shader_destroy_modules(shadow_render.shader.version_get_shader(shadow_render.shader_version, SHADOW_RENDER_MODE_SHADOW));
+		RD::get_singleton()->shader_destroy_modules(shadow_render.shader.version_get_shader(shadow_render.shader_version, SHADOW_RENDER_MODE_SDF));
 	}
 
 	{ //bindings
diff --git a/servers/rendering/renderer_rd/renderer_compositor_rd.cpp b/servers/rendering/renderer_rd/renderer_compositor_rd.cpp
index ba47508700..dbaa6ac7b2 100644
--- a/servers/rendering/renderer_rd/renderer_compositor_rd.cpp
+++ b/servers/rendering/renderer_rd/renderer_compositor_rd.cpp
@@ -133,6 +133,9 @@ void RendererCompositorRD::initialize() {
 
 		for (int i = 0; i < BLIT_MODE_MAX; i++) {
 			blit.pipelines[i] = RD::get_singleton()->render_pipeline_create(blit.shader.version_get_shader(blit.shader_version, i), RD::get_singleton()->screen_get_framebuffer_format(DisplayServer::MAIN_WINDOW_ID), RD::INVALID_ID, RD::RENDER_PRIMITIVE_TRIANGLES, RD::PipelineRasterizationState(), RD::PipelineMultisampleState(), RD::PipelineDepthStencilState(), i == BLIT_MODE_NORMAL_ALPHA ? RenderingDevice::PipelineColorBlendState::create_blend() : RenderingDevice::PipelineColorBlendState::create_disabled(), 0);
+
+			// Unload shader modules to save memory.
+			RD::get_singleton()->shader_destroy_modules(blit.shader.version_get_shader(blit.shader_version, i));
 		}
 
 		//create index array for copy shader
diff --git a/servers/rendering/renderer_rd/shader_rd.cpp b/servers/rendering/renderer_rd/shader_rd.cpp
index fb89c11a1b..28418168f9 100644
--- a/servers/rendering/renderer_rd/shader_rd.cpp
+++ b/servers/rendering/renderer_rd/shader_rd.cpp
@@ -349,7 +349,7 @@ void ShaderRD::_compile_variant(uint32_t p_variant, CompileData p_data) {
 	{
 		MutexLock lock(variant_set_mutex);
 
-		p_data.version->variants.write[variant] = RD::get_singleton()->shader_create_from_bytecode(shader_data, p_data.version->variants[variant]);
+		p_data.version->variants.write[variant] = RD::get_singleton()->shader_create_from_bytecode_with_samplers(shader_data, p_data.version->variants[variant], immutable_samplers);
 		p_data.version->variant_data.write[variant] = shader_data;
 	}
 }
@@ -491,7 +491,7 @@ bool ShaderRD::_load_from_cache(Version *p_version, int p_group) {
 		}
 		{
 			MutexLock lock(variant_set_mutex);
-			RID shader = RD::get_singleton()->shader_create_from_bytecode(p_version->variant_data[variant_id], p_version->variants[variant_id]);
+			RID shader = RD::get_singleton()->shader_create_from_bytecode_with_samplers(p_version->variant_data[variant_id], p_version->variants[variant_id], immutable_samplers);
 			if (shader.is_null()) {
 				for (uint32_t j = 0; j < i; j++) {
 					int variant_free_id = group_to_variant_map[p_group][j];
@@ -769,7 +769,8 @@ ShaderRD::ShaderRD() {
 	base_compute_defines = base_compute_define_text.ascii();
 }
 
-void ShaderRD::initialize(const Vector<String> &p_variant_defines, const String &p_general_defines) {
+void ShaderRD::initialize(const Vector<String> &p_variant_defines, const String &p_general_defines, const Vector<RD::PipelineImmutableSampler> &r_immutable_samplers) {
+	immutable_samplers = r_immutable_samplers;
 	ERR_FAIL_COND(variant_defines.size());
 	ERR_FAIL_COND(p_variant_defines.is_empty());
 
diff --git a/servers/rendering/renderer_rd/shader_rd.h b/servers/rendering/renderer_rd/shader_rd.h
index 90e41947b9..899f03db64 100644
--- a/servers/rendering/renderer_rd/shader_rd.h
+++ b/servers/rendering/renderer_rd/shader_rd.h
@@ -63,6 +63,8 @@ private:
 	HashMap<int, LocalVector<int>> group_to_variant_map;
 	Vector<bool> group_enabled;
 
+	Vector<RD::PipelineImmutableSampler> immutable_samplers;
+
 	struct Version {
 		CharString uniforms;
 		CharString vertex_globals;
@@ -211,7 +213,7 @@ public:
 
 	RS::ShaderNativeSourceCode version_get_native_source_code(RID p_version);
 
-	void initialize(const Vector<String> &p_variant_defines, const String &p_general_defines = "");
+	void initialize(const Vector<String> &p_variant_defines, const String &p_general_defines = "", const Vector<RD::PipelineImmutableSampler> &r_immutable_samplers = Vector<RD::PipelineImmutableSampler>());
 	void initialize(const Vector<VariantDefine> &p_variant_defines, const String &p_general_defines = "");
 
 	virtual ~ShaderRD();
diff --git a/servers/rendering/renderer_rd/storage_rd/render_scene_buffers_rd.cpp b/servers/rendering/renderer_rd/storage_rd/render_scene_buffers_rd.cpp
index 6e1259eac1..c140498f84 100644
--- a/servers/rendering/renderer_rd/storage_rd/render_scene_buffers_rd.cpp
+++ b/servers/rendering/renderer_rd/storage_rd/render_scene_buffers_rd.cpp
@@ -171,6 +171,9 @@ void RenderSceneBuffersRD::configure(const RenderSceneBuffersConfiguration *p_co
 	const bool resolve_target = msaa_3d != RS::VIEWPORT_MSAA_DISABLED;
 	create_texture(RB_SCOPE_BUFFERS, RB_TEX_COLOR, base_data_format, get_color_usage_bits(resolve_target, false, can_be_storage));
 
+	// TODO: Detect when it is safe to use RD::TEXTURE_USAGE_TRANSIENT_BIT for RB_TEX_DEPTH, RB_TEX_COLOR_MSAA and/or RB_TEX_DEPTH_MSAA.
+	// (it means we cannot sample from it, we cannot copy from/to it) to save VRAM (and maybe performance too).
+
 	// Create our depth buffer.
 	create_texture(RB_SCOPE_BUFFERS, RB_TEX_DEPTH, get_depth_format(resolve_target, false, can_be_storage), get_depth_usage_bits(resolve_target, false, can_be_storage));
 
diff --git a/servers/rendering/rendering_device.cpp b/servers/rendering/rendering_device.cpp
index 72984e5abf..2fbafcbda3 100644
--- a/servers/rendering/rendering_device.cpp
+++ b/servers/rendering/rendering_device.cpp
@@ -566,9 +566,12 @@ String RenderingDevice::get_perf_report() const {
 }
 
 void RenderingDevice::update_perf_report() {
-	perf_report_text = " gpu:" + String::num_int64(gpu_copy_count);
+	perf_report_text = "";
+	perf_report_text += " gpu:" + String::num_int64(gpu_copy_count);
 	perf_report_text += " bytes:" + String::num_int64(copy_bytes_count);
 
+	perf_report_text += " lazily alloc:" + String::num_int64(driver->get_lazily_memory_used());
+
 	gpu_copy_count = 0;
 	copy_bytes_count = 0;
 }
@@ -2639,6 +2642,15 @@ RenderingDevice::FramebufferFormatID RenderingDevice::framebuffer_get_format(RID
 	return framebuffer->format_id;
 }
 
+Size2 RenderingDevice::framebuffer_get_size(RID p_framebuffer) {
+	_THREAD_SAFE_METHOD_
+
+	Framebuffer *framebuffer = framebuffer_owner.get_or_null(p_framebuffer);
+	ERR_FAIL_NULL_V(framebuffer, Size2(0, 0));
+
+	return framebuffer->size;
+}
+
 bool RenderingDevice::framebuffer_is_valid(RID p_framebuffer) const {
 	_THREAD_SAFE_METHOD_
 
@@ -2954,11 +2966,33 @@ Vector<uint8_t> RenderingDevice::shader_compile_binary_from_spirv(const Vector<S
 }
 
 RID RenderingDevice::shader_create_from_bytecode(const Vector<uint8_t> &p_shader_binary, RID p_placeholder) {
+	// Immutable samplers :
+	// Expanding api when creating shader to allow passing optionally a set of immutable samplers
+	// keeping existing api but extending it by sending an empty set.
+	Vector<PipelineImmutableSampler> immutable_samplers;
+	return shader_create_from_bytecode_with_samplers(p_shader_binary, p_placeholder, immutable_samplers);
+}
+
+RID RenderingDevice::shader_create_from_bytecode_with_samplers(const Vector<uint8_t> &p_shader_binary, RID p_placeholder, const Vector<PipelineImmutableSampler> &p_immutable_samplers) {
 	_THREAD_SAFE_METHOD_
 
 	ShaderDescription shader_desc;
 	String name;
-	RDD::ShaderID shader_id = driver->shader_create_from_bytecode(p_shader_binary, shader_desc, name);
+
+	Vector<RDD::ImmutableSampler> driver_immutable_samplers;
+	for (const PipelineImmutableSampler &source_sampler : p_immutable_samplers) {
+		RDD::ImmutableSampler driver_sampler;
+		driver_sampler.type = source_sampler.uniform_type;
+		driver_sampler.binding = source_sampler.binding;
+
+		for (uint32_t j = 0; j < source_sampler.get_id_count(); j++) {
+			RDD::SamplerID *sampler_driver_id = sampler_owner.get_or_null(source_sampler.get_id(j));
+			driver_sampler.ids.push_back(*sampler_driver_id);
+		}
+
+		driver_immutable_samplers.append(driver_sampler);
+	}
+	RDD::ShaderID shader_id = driver->shader_create_from_bytecode(p_shader_binary, shader_desc, name, driver_immutable_samplers);
 	ERR_FAIL_COND_V(!shader_id, RID());
 
 	// All good, let's create modules.
@@ -3029,6 +3063,12 @@ RID RenderingDevice::shader_create_from_bytecode(const Vector<uint8_t> &p_shader
 	return id;
 }
 
+void RenderingDevice::shader_destroy_modules(RID p_shader) {
+	Shader *shader = shader_owner.get_or_null(p_shader);
+	ERR_FAIL_NULL(shader);
+	driver->shader_destroy_modules(shader->driver_id);
+}
+
 RID RenderingDevice::shader_create_placeholder() {
 	_THREAD_SAFE_METHOD_
 
@@ -3086,12 +3126,12 @@ void RenderingDevice::_uniform_set_update_shared(UniformSet *p_uniform_set) {
 	}
 }
 
-template RID RenderingDevice::uniform_set_create(const LocalVector<RD::Uniform> &p_uniforms, RID p_shader, uint32_t p_shader_set);
+template RID RenderingDevice::uniform_set_create(const LocalVector<RD::Uniform> &p_uniforms, RID p_shader, uint32_t p_shader_set, bool p_linear_pool);
 
-template RID RenderingDevice::uniform_set_create(const Vector<RD::Uniform> &p_uniforms, RID p_shader, uint32_t p_shader_set);
+template RID RenderingDevice::uniform_set_create(const Vector<RD::Uniform> &p_uniforms, RID p_shader, uint32_t p_shader_set, bool p_linear_pool);
 
 template <typename Collection>
-RID RenderingDevice::uniform_set_create(const Collection &p_uniforms, RID p_shader, uint32_t p_shader_set) {
+RID RenderingDevice::uniform_set_create(const Collection &p_uniforms, RID p_shader, uint32_t p_shader_set, bool p_linear_pool) {
 	_THREAD_SAFE_METHOD_
 
 	ERR_FAIL_COND_V(p_uniforms.is_empty(), RID());
@@ -3142,6 +3182,9 @@ RID RenderingDevice::uniform_set_create(const Collection &p_uniforms, RID p_shad
 		driver_uniform.type = uniform.uniform_type;
 		driver_uniform.binding = uniform.binding;
 
+		// Mark immutable samplers to be skipped when creating uniform set.
+		driver_uniform.immutable_sampler = uniform.immutable_sampler;
+
 		switch (uniform.uniform_type) {
 			case UNIFORM_TYPE_SAMPLER: {
 				if (uniform.get_id_count() != (uint32_t)set_uniform.length) {
@@ -3457,7 +3500,7 @@ RID RenderingDevice::uniform_set_create(const Collection &p_uniforms, RID p_shad
 		}
 	}
 
-	RDD::UniformSetID driver_uniform_set = driver->uniform_set_create(driver_uniforms, shader->driver_id, p_shader_set);
+	RDD::UniformSetID driver_uniform_set = driver->uniform_set_create(driver_uniforms, shader->driver_id, p_shader_set, p_linear_pool ? frame : -1);
 	ERR_FAIL_COND_V(!driver_uniform_set, RID());
 
 	UniformSet uniform_set;
@@ -3503,6 +3546,10 @@ void RenderingDevice::uniform_set_set_invalidation_callback(RID p_uniform_set, I
 	us->invalidated_callback_userdata = p_userdata;
 }
 
+bool RenderingDevice::uniform_sets_have_linear_pools() const {
+	return driver->uniform_sets_have_linear_pools();
+}
+
 /*******************/
 /**** PIPELINES ****/
 /*******************/
@@ -3782,6 +3829,7 @@ Error RenderingDevice::screen_create(DisplayServer::WindowID p_screen) {
 Error RenderingDevice::screen_prepare_for_drawing(DisplayServer::WindowID p_screen) {
 	_THREAD_SAFE_METHOD_
 
+	// After submitting work, acquire the swapchain image(s)
 	HashMap<DisplayServer::WindowID, RDD::SwapChainID>::ConstIterator it = screen_swap_chains.find(p_screen);
 	ERR_FAIL_COND_V_MSG(it == screen_swap_chains.end(), ERR_CANT_CREATE, "A swap chain was not created for the screen.");
 
@@ -3918,7 +3966,7 @@ RenderingDevice::DrawListID RenderingDevice::draw_list_begin_for_screen(DisplayS
 	clear_value.color = p_clear_color;
 
 	RDD::RenderPassID render_pass = driver->swap_chain_get_render_pass(sc_it->value);
-	draw_graph.add_draw_list_begin(render_pass, fb_it->value, viewport, RDG::ATTACHMENT_OPERATION_CLEAR, clear_value, true, false, RDD::BreadcrumbMarker::BLIT_PASS);
+	draw_graph.add_draw_list_begin(render_pass, fb_it->value, viewport, RDG::ATTACHMENT_OPERATION_CLEAR, clear_value, true, false, RDD::BreadcrumbMarker::BLIT_PASS, split_swapchain_into_its_own_cmd_buffer);
 
 	draw_graph.add_draw_list_set_viewport(viewport);
 	draw_graph.add_draw_list_set_scissor(viewport);
@@ -4354,37 +4402,69 @@ void RenderingDevice::draw_list_draw(DrawListID p_list, bool p_use_indices, uint
 		}
 	}
 #endif
+	thread_local LocalVector<RDD::UniformSetID> valid_descriptor_ids;
+	valid_descriptor_ids.clear();
+	valid_descriptor_ids.resize(dl->state.set_count);
+	uint32_t valid_set_count = 0;
+	uint32_t first_set_index = 0;
+	uint32_t last_set_index = 0;
+	bool found_first_set = false;
 
-	// Prepare descriptor sets if the API doesn't use pipeline barriers.
-	if (!driver->api_trait_get(RDD::API_TRAIT_HONORS_PIPELINE_BARRIERS)) {
-		for (uint32_t i = 0; i < dl->state.set_count; i++) {
-			if (dl->state.sets[i].pipeline_expected_format == 0) {
-				// Nothing expected by this pipeline.
-				continue;
-			}
+	for (uint32_t i = 0; i < dl->state.set_count; i++) {
+		if (dl->state.sets[i].pipeline_expected_format == 0) {
+			continue; // Nothing expected by this pipeline.
+		}
 
+		if (!dl->state.sets[i].bound && !found_first_set) {
+			first_set_index = i;
+			found_first_set = true;
+		}
+		// Prepare descriptor sets if the API doesn't use pipeline barriers.
+		if (!driver->api_trait_get(RDD::API_TRAIT_HONORS_PIPELINE_BARRIERS)) {
 			draw_graph.add_draw_list_uniform_set_prepare_for_use(dl->state.pipeline_shader_driver_id, dl->state.sets[i].uniform_set_driver_id, i);
 		}
 	}
 
 	// Bind descriptor sets.
-	for (uint32_t i = 0; i < dl->state.set_count; i++) {
+	for (uint32_t i = first_set_index; i < dl->state.set_count; i++) {
 		if (dl->state.sets[i].pipeline_expected_format == 0) {
 			continue; // Nothing expected by this pipeline.
 		}
+
 		if (!dl->state.sets[i].bound) {
-			// All good, see if this requires re-binding.
-			draw_graph.add_draw_list_bind_uniform_set(dl->state.pipeline_shader_driver_id, dl->state.sets[i].uniform_set_driver_id, i);
+			// Batch contiguous descriptor sets in a single call
+			if (descriptor_set_batching) {
+				// All good, see if this requires re-binding.
+				if (i - last_set_index > 1) {
+					// If the descriptor sets are not contiguous, bind the previous ones and start a new batch
+					draw_graph.add_draw_list_bind_uniform_sets(dl->state.pipeline_shader_driver_id, valid_descriptor_ids, first_set_index, valid_set_count);
 
-			UniformSet *uniform_set = uniform_set_owner.get_or_null(dl->state.sets[i].uniform_set);
-			_uniform_set_update_shared(uniform_set);
+					first_set_index = i;
+					valid_set_count = 1;
+					valid_descriptor_ids[0] = dl->state.sets[i].uniform_set_driver_id;
+				} else {
+					// Otherwise, keep storing in the current batch
+					valid_descriptor_ids[valid_set_count] = dl->state.sets[i].uniform_set_driver_id;
+					valid_set_count++;
+				}
 
-			draw_graph.add_draw_list_usages(uniform_set->draw_trackers, uniform_set->draw_trackers_usage);
+				UniformSet *uniform_set = uniform_set_owner.get_or_null(dl->state.sets[i].uniform_set);
+				_uniform_set_update_shared(uniform_set);
+				draw_graph.add_draw_list_usages(uniform_set->draw_trackers, uniform_set->draw_trackers_usage);
+				dl->state.sets[i].bound = true;
 
-			dl->state.sets[i].bound = true;
+				last_set_index = i;
+			} else {
+				draw_graph.add_draw_list_bind_uniform_set(dl->state.pipeline_shader_driver_id, dl->state.sets[i].uniform_set_driver_id, i);
+			}
 		}
 	}
 
+	// Bind the remaining batch
+	if (descriptor_set_batching && valid_set_count > 0) {
+		draw_graph.add_draw_list_bind_uniform_sets(dl->state.pipeline_shader_driver_id, valid_descriptor_ids, first_set_index, valid_set_count);
+	}
+
 	if (p_use_indices) {
 #ifdef DEBUG_ENABLED
 		ERR_FAIL_COND_MSG(p_procedural_vertices > 0,
@@ -4549,6 +4629,22 @@ void RenderingDevice::draw_list_draw_indirect(DrawListID p_list, bool p_use_indi
 	_check_transfer_worker_buffer(buffer);
 }
 
+void RenderingDevice::draw_list_set_viewport(DrawListID p_list, const Rect2 &p_rect) {
+	DrawList *dl = _get_draw_list_ptr(p_list);
+
+	ERR_FAIL_NULL(dl);
+#ifdef DEBUG_ENABLED
+	ERR_FAIL_COND_MSG(!dl->validation.active, "Submitted Draw Lists can no longer be modified.");
+#endif
+
+	if (p_rect.get_area() == 0) {
+		return;
+	}
+
+	dl->viewport = p_rect;
+	draw_graph.add_draw_list_set_viewport(p_rect);
+}
+
 void RenderingDevice::draw_list_enable_scissor(DrawListID p_list, const Rect2 &p_rect) {
 	ERR_RENDER_THREAD_GUARD();
 
@@ -4873,37 +4969,70 @@ void RenderingDevice::compute_list_dispatch(ComputeListID p_list, uint32_t p_x_g
 		}
 	}
 #endif
+	thread_local LocalVector<RDD::UniformSetID> valid_descriptor_ids;
+	valid_descriptor_ids.clear();
+	valid_descriptor_ids.resize(cl->state.set_count);
 
-	// Prepare descriptor sets if the API doesn't use pipeline barriers.
-	if (!driver->api_trait_get(RDD::API_TRAIT_HONORS_PIPELINE_BARRIERS)) {
-		for (uint32_t i = 0; i < cl->state.set_count; i++) {
-			if (cl->state.sets[i].pipeline_expected_format == 0) {
-				// Nothing expected by this pipeline.
-				continue;
-			}
+	uint32_t valid_set_count = 0;
+	uint32_t first_set_index = 0;
+	uint32_t last_set_index = 0;
+	bool found_first_set = false;
 
+	for (uint32_t i = 0; i < cl->state.set_count; i++) {
+		if (cl->state.sets[i].pipeline_expected_format == 0) {
+			// Nothing expected by this pipeline.
+			continue;
+		}
+
+		if (!cl->state.sets[i].bound && !found_first_set) {
+			first_set_index = i;
+			found_first_set = true;
+		}
+		// Prepare descriptor sets if the API doesn't use pipeline barriers.
+		if (!driver->api_trait_get(RDD::API_TRAIT_HONORS_PIPELINE_BARRIERS)) {
 			draw_graph.add_compute_list_uniform_set_prepare_for_use(cl->state.pipeline_shader_driver_id, cl->state.sets[i].uniform_set_driver_id, i);
 		}
 	}
 
 	// Bind descriptor sets.
-	for (uint32_t i = 0; i < cl->state.set_count; i++) {
+	for (uint32_t i = first_set_index; i < cl->state.set_count; i++) {
 		if (cl->state.sets[i].pipeline_expected_format == 0) {
 			continue; // Nothing expected by this pipeline.
 		}
-		if (!cl->state.sets[i].bound) {
-			// All good, see if this requires re-binding.
-			draw_graph.add_compute_list_bind_uniform_set(cl->state.pipeline_shader_driver_id, cl->state.sets[i].uniform_set_driver_id, i);
 
+		if (!cl->state.sets[i].bound) {
+			// Descriptor set batching
+			if (descriptor_set_batching) {
+				// All good, see if this requires re-binding.
+				if (i - last_set_index > 1) {
+					// If the descriptor sets are not contiguous, bind the previous ones and start a new batch
+					draw_graph.add_compute_list_bind_uniform_sets(cl->state.pipeline_shader_driver_id, valid_descriptor_ids, first_set_index, valid_set_count);
+
+					first_set_index = i;
+					valid_set_count = 1;
+					valid_descriptor_ids[0] = cl->state.sets[i].uniform_set_driver_id;
+				} else {
+					// Otherwise, keep storing in the current batch
+					valid_descriptor_ids[valid_set_count] = cl->state.sets[i].uniform_set_driver_id;
+					valid_set_count++;
+				}
+
+				last_set_index = i;
+			} else {
+				draw_graph.add_compute_list_bind_uniform_set(cl->state.pipeline_shader_driver_id, cl->state.sets[i].uniform_set_driver_id, i);
+			}
 			UniformSet *uniform_set = uniform_set_owner.get_or_null(cl->state.sets[i].uniform_set);
 			_uniform_set_update_shared(uniform_set);
 
 			draw_graph.add_compute_list_usages(uniform_set->draw_trackers, uniform_set->draw_trackers_usage);
-
 			cl->state.sets[i].bound = true;
 		}
 	}
 
+	// Bind the remaining batch
+	if (valid_set_count > 0) {
+		draw_graph.add_compute_list_bind_uniform_sets(cl->state.pipeline_shader_driver_id, valid_descriptor_ids, first_set_index, valid_set_count);
+	}
 	draw_graph.add_compute_list_dispatch(p_x_groups, p_y_groups, p_z_groups);
 	cl->state.dispatch_count++;
 }
@@ -4986,37 +5115,68 @@ void RenderingDevice::compute_list_dispatch_indirect(ComputeListID p_list, RID p
 		}
 	}
 #endif
+	thread_local LocalVector<RDD::UniformSetID> valid_descriptor_ids;
+	valid_descriptor_ids.clear();
+	valid_descriptor_ids.resize(cl->state.set_count);
 
-	// Prepare descriptor sets if the API doesn't use pipeline barriers.
-	if (!driver->api_trait_get(RDD::API_TRAIT_HONORS_PIPELINE_BARRIERS)) {
-		for (uint32_t i = 0; i < cl->state.set_count; i++) {
-			if (cl->state.sets[i].pipeline_expected_format == 0) {
-				// Nothing expected by this pipeline.
-				continue;
-			}
+	uint32_t valid_set_count = 0;
+	uint32_t first_set_index = 0;
+	uint32_t last_set_index = 0;
+	bool found_first_set = false;
 
+	for (uint32_t i = 0; i < cl->state.set_count; i++) {
+		if (cl->state.sets[i].pipeline_expected_format == 0) {
+			// Nothing expected by this pipeline.
+			continue;
+		}
+
+		if (!cl->state.sets[i].bound && !found_first_set) {
+			first_set_index = i;
+			found_first_set = true;
+		}
+
+		// Prepare descriptor sets if the API doesn't use pipeline barriers.
+		if (!driver->api_trait_get(RDD::API_TRAIT_HONORS_PIPELINE_BARRIERS)) {
 			draw_graph.add_compute_list_uniform_set_prepare_for_use(cl->state.pipeline_shader_driver_id, cl->state.sets[i].uniform_set_driver_id, i);
 		}
 	}
 
 	// Bind descriptor sets.
-	for (uint32_t i = 0; i < cl->state.set_count; i++) {
+	for (uint32_t i = first_set_index; i < cl->state.set_count; i++) {
 		if (cl->state.sets[i].pipeline_expected_format == 0) {
 			continue; // Nothing expected by this pipeline.
 		}
+
 		if (!cl->state.sets[i].bound) {
 			// All good, see if this requires re-binding.
-			draw_graph.add_compute_list_bind_uniform_set(cl->state.pipeline_shader_driver_id, cl->state.sets[i].uniform_set_driver_id, i);
+			if (i - last_set_index > 1) {
+				// If the descriptor sets are not contiguous, bind the previous ones and start a new batch
+				draw_graph.add_compute_list_bind_uniform_sets(cl->state.pipeline_shader_driver_id, valid_descriptor_ids, first_set_index, valid_set_count);
+
+				first_set_index = i;
+				valid_set_count = 1;
+				valid_descriptor_ids[0] = cl->state.sets[i].uniform_set_driver_id;
+			} else {
+				// Otherwise, keep storing in the current batch
+				valid_descriptor_ids[valid_set_count] = cl->state.sets[i].uniform_set_driver_id;
+				valid_set_count++;
+			}
+
+			last_set_index = i;
 
 			UniformSet *uniform_set = uniform_set_owner.get_or_null(cl->state.sets[i].uniform_set);
 			_uniform_set_update_shared(uniform_set);
 
 			draw_graph.add_compute_list_usages(uniform_set->draw_trackers, uniform_set->draw_trackers_usage);
-
 			cl->state.sets[i].bound = true;
 		}
 	}
 
+	// Bind the remaining batch
+	if (valid_set_count > 0) {
+		draw_graph.add_compute_list_bind_uniform_sets(cl->state.pipeline_shader_driver_id, valid_descriptor_ids, first_set_index, valid_set_count);
+	}
+
 	draw_graph.add_compute_list_dispatch_indirect(buffer->driver_id, p_offset);
 	cl->state.dispatch_count++;
 
@@ -5253,6 +5413,7 @@ void RenderingDevice::_submit_transfer_worker(TransferWorker *p_transfer_worker,
 
 void RenderingDevice::_wait_for_transfer_worker(TransferWorker *p_transfer_worker) {
 	driver->fence_wait(p_transfer_worker->command_fence);
+	driver->command_pool_reset(p_transfer_worker->command_pool);
 	p_transfer_worker->staging_buffer_size_in_use = 0;
 	p_transfer_worker->submitted = false;
 
@@ -5770,7 +5931,8 @@ void RenderingDevice::swap_buffers() {
 
 	// Advance to the next frame and begin recording again.
 	frame = (frame + 1) % frames.size();
-	_begin_frame();
+
+	_begin_frame(true);
 }
 
 void RenderingDevice::submit() {
@@ -5788,7 +5950,7 @@ void RenderingDevice::sync() {
 	ERR_FAIL_COND_MSG(is_main_instance, "Only local devices can submit and sync.");
 	ERR_FAIL_COND_MSG(!local_device_processing, "sync can only be called after a submit");
 
-	_begin_frame();
+	_begin_frame(true);
 	local_device_processing = false;
 }
 
@@ -5892,14 +6054,22 @@ uint64_t RenderingDevice::get_memory_usage(MemoryType p_type) const {
 	}
 }
 
-void RenderingDevice::_begin_frame() {
+void RenderingDevice::_begin_frame(bool p_presented) {
 	// Before beginning this frame, wait on the fence if it was signaled to make sure its work is finished.
 	if (frames[frame].fence_signaled) {
 		driver->fence_wait(frames[frame].fence);
 		frames[frame].fence_signaled = false;
 	}
 
-	update_perf_report();
+	if (command_pool_reset_enabled) {
+		bool reset = driver->command_pool_reset(frames[frame].command_pool);
+		ERR_FAIL_COND(!reset);
+	}
+
+	if (p_presented) {
+		update_perf_report();
+		driver->linear_uniform_set_pools_reset(frame);
+	}
 
 	// Begin recording on the frame's command buffers.
 	driver->begin_segment(frame, frames_drawn++);
@@ -5948,15 +6118,11 @@ void RenderingDevice::_end_frame() {
 	driver->end_segment();
 }
 
-void RenderingDevice::_execute_frame(bool p_present) {
-	// Check whether this frame should present the swap chains and in which queue.
-	const bool frame_can_present = p_present && !frames[frame].swap_chains_to_present.is_empty();
-	const bool separate_present_queue = main_queue != present_queue;
-	thread_local LocalVector<RDD::SwapChainID> swap_chains;
-	swap_chains.clear();
-
-	// Execute command buffers and use semaphores to wait on the execution of the previous one. Normally there's only one command buffer,
-	// but driver workarounds can force situations where there'll be more.
+void RenderingDevice::execute_chained_cmds(bool p_present_swap_chain, RenderingDeviceDriver::FenceID p_draw_fence,
+		RenderingDeviceDriver::SemaphoreID p_dst_draw_semaphore_to_signal) {
+	// Execute command buffers and use semaphores to wait on the execution of the previous one.
+	// Normally there's only one command buffer, but driver workarounds can force situations where
+	// there'll be more.
 	uint32_t command_buffer_count = 1;
 	RDG::CommandBufferPool &buffer_pool = frames[frame].command_buffer_pool;
 	if (buffer_pool.buffers_used > 0) {
@@ -5964,6 +6130,12 @@ void RenderingDevice::_execute_frame(bool p_present) {
 		buffer_pool.buffers_used = 0;
 	}
 
+	thread_local LocalVector<RDD::SwapChainID> swap_chains;
+	swap_chains.clear();
+
+	// Instead of having just one command; we have potentially many (which had to be split due to an
+	// Adreno workaround on mobile, only if the workaround is active). Thus we must execute all of them
+	// and chain them together via semaphores as dependent executions.
 	thread_local LocalVector<RDD::SemaphoreID> wait_semaphores;
 	wait_semaphores = frames[frame].semaphores_to_wait_on;
 
@@ -5973,45 +6145,57 @@ void RenderingDevice::_execute_frame(bool p_present) {
 		RDD::FenceID signal_fence;
 		if (i > 0) {
 			command_buffer = buffer_pool.buffers[i - 1];
-			signal_semaphore = buffer_pool.semaphores[i - 1];
 		} else {
 			command_buffer = frames[frame].command_buffer;
-			signal_semaphore = frames[frame].semaphore;
 		}
 
-		bool signal_semaphore_valid;
 		if (i == (command_buffer_count - 1)) {
-			// This is the last command buffer, it should signal the fence.
-			signal_fence = frames[frame].fence;
-			signal_semaphore_valid = false;
+			// This is the last command buffer, it should signal the semaphore & fence.
+			signal_semaphore = p_dst_draw_semaphore_to_signal;
+			signal_fence = p_draw_fence;
 
-			if (frame_can_present && separate_present_queue) {
-				// The semaphore is required if the frame can be presented and a separate present queue is used.
-				signal_semaphore_valid = true;
-			} else if (frame_can_present) {
+			if (p_present_swap_chain) {
 				// Just present the swap chains as part of the last command execution.
 				swap_chains = frames[frame].swap_chains_to_present;
 			}
 		} else {
+			signal_semaphore = buffer_pool.semaphores[i];
 			// Semaphores always need to be signaled if it's not the last command buffer.
-			signal_semaphore_valid = true;
 		}
 
-		driver->command_queue_execute_and_present(main_queue, wait_semaphores, command_buffer, signal_semaphore_valid ? signal_semaphore : VectorView<RDD::SemaphoreID>(), signal_fence, swap_chains);
+		driver->command_queue_execute_and_present(main_queue, wait_semaphores, command_buffer,
+				signal_semaphore ? signal_semaphore : VectorView<RDD::SemaphoreID>(), signal_fence,
+				swap_chains);
 
 		// Make the next command buffer wait on the semaphore signaled by this one.
 		wait_semaphores.resize(1);
 		wait_semaphores[0] = signal_semaphore;
 	}
 
-	// Indicate the fence has been signaled so the next time the frame's contents need to be used, the CPU needs to wait on the work to be completed.
 	frames[frame].semaphores_to_wait_on.clear();
+}
+
+void RenderingDevice::_execute_frame(bool p_present) {
+	// Check whether this frame should present the swap chains and in which queue.
+	const bool frame_can_present = p_present && !frames[frame].swap_chains_to_present.is_empty();
+	const bool separate_present_queue = main_queue != present_queue;
+
+	// The semaphore is required if the frame can be presented and a separate present queue is used;
+	// since the separate queue will wait for that semaphore before presenting.
+	const RDD::SemaphoreID semaphore = (frame_can_present && separate_present_queue)
+			? frames[frame].semaphore
+			: RDD::SemaphoreID(nullptr);
+	const bool present_swap_chain = frame_can_present && !separate_present_queue;
+
+	execute_chained_cmds(present_swap_chain, frames[frame].fence, semaphore);
+	// Indicate the fence has been signaled so the next time the frame's contents need to be
+	// used, the CPU needs to wait on the work to be completed.
 	frames[frame].fence_signaled = true;
 
 	if (frame_can_present) {
 		if (separate_present_queue) {
 			// Issue the presentation separately if the presentation queue is different from the main queue.
-			driver->command_queue_execute_and_present(present_queue, wait_semaphores, {}, {}, {}, frames[frame].swap_chains_to_present);
+			driver->command_queue_execute_and_present(present_queue, frames[frame].semaphore, {}, {}, {}, frames[frame].swap_chains_to_present);
 		}
 
 		frames[frame].swap_chains_to_present.clear();
diff --git a/servers/rendering/rendering_device.h b/servers/rendering/rendering_device.h
index 6b19d05fca..92045bd0d8 100644
--- a/servers/rendering/rendering_device.h
+++ b/servers/rendering/rendering_device.h
@@ -191,7 +191,12 @@ private:
 	Error _buffer_initialize(Buffer *p_buffer, const uint8_t *p_data, size_t p_data_size, uint32_t p_required_align = 32);
 
 	void update_perf_report();
-
+	// flag for batching descriptor sets
+	bool descriptor_set_batching = true;
+	// When true, the final draw call that copies our offscreen result into the Swapchain is put into its
+	// own cmd buffer, so that the whole rendering can start early instead of having to wait for the
+	// swapchain semaphore to be signaled (which causes bubbles).
+	bool split_swapchain_into_its_own_cmd_buffer = true;
 	uint32_t gpu_copy_count = 0;
 	uint32_t copy_bytes_count = 0;
 	String perf_report_text;
@@ -543,6 +548,7 @@ public:
 	void framebuffer_set_invalidation_callback(RID p_framebuffer, InvalidationCallback p_callback, void *p_userdata);
 
 	FramebufferFormatID framebuffer_get_format(RID p_framebuffer);
+	Size2 framebuffer_get_size(RID p_framebuffer);
 
 	/*****************/
 	/**** SAMPLER ****/
@@ -843,6 +849,7 @@ public:
 	RID shader_create_from_spirv(const Vector<ShaderStageSPIRVData> &p_spirv, const String &p_shader_name = "");
 	RID shader_create_from_bytecode(const Vector<uint8_t> &p_shader_binary, RID p_placeholder = RID());
 	RID shader_create_placeholder();
+	void shader_destroy_modules(RID p_shader);
 
 	uint64_t shader_get_vertex_input_attribute_mask(RID p_shader);
 
@@ -855,13 +862,20 @@ public:
 		STORAGE_BUFFER_USAGE_DISPATCH_INDIRECT = 1,
 	};
 
+	/*****************/
+	/**** BUFFERS ****/
+	/*****************/
+
 	RID uniform_buffer_create(uint32_t p_size_bytes, const Vector<uint8_t> &p_data = Vector<uint8_t>());
 	RID storage_buffer_create(uint32_t p_size, const Vector<uint8_t> &p_data = Vector<uint8_t>(), BitField<StorageBufferUsage> p_usage = 0);
+
 	RID texture_buffer_create(uint32_t p_size_elements, DataFormat p_format, const Vector<uint8_t> &p_data = Vector<uint8_t>());
 
 	struct Uniform {
 		UniformType uniform_type = UNIFORM_TYPE_IMAGE;
 		uint32_t binding = 0; // Binding index as specified in shader.
+		// This flag specifies that this is an immutable sampler to be set when creating pipeline layout.
+		bool immutable_sampler = false;
 
 	private:
 		// In most cases only one ID is provided per binding, so avoid allocating memory unnecessarily for performance.
@@ -922,6 +936,9 @@ public:
 		_FORCE_INLINE_ Uniform() = default;
 	};
 
+	typedef Uniform PipelineImmutableSampler;
+	RID shader_create_from_bytecode_with_samplers(const Vector<uint8_t> &p_shader_binary, RID p_placeholder = RID(), const Vector<PipelineImmutableSampler> &p_immutable_samplers = Vector<PipelineImmutableSampler>());
+
 private:
 	static const uint32_t MAX_UNIFORM_SETS = 16;
 	static const uint32_t MAX_PUSH_CONSTANT_SIZE = 128;
@@ -963,11 +980,23 @@ private:
 	void _uniform_set_update_shared(UniformSet *p_uniform_set);
 
 public:
+	/** Bake a set of uniforms that can be bound at runtime with the given shader.
+	 * @remark				Setting p_linear_pool = true while keeping the RID around for longer than the current frame will result in undefined behavior.
+	 * @param p_uniforms	The uniforms to bake into a set.
+	 * @param p_shader		The shader you intend to bind these uniforms with.
+	 * @param p_set_index	The set. Should be in range [0; 4)
+	 *						The value 4 comes from physical_device_properties.limits.maxBoundDescriptorSets. Vulkan only guarantees maxBoundDescriptorSets >= 4 (== 4 is very common on Mobile).
+	 * @param p_linear_pool	If you call this function every frame (and free the returned RID within the same frame!), set it to true for better performance.
+	 *						If you plan on keeping the return value around for more than one frame (e.g. Sets that are created once and reused forever) you MUST set it to false.
+	 * @return				Baked descriptor set.
+	 */
 	template <typename Collection>
-	RID uniform_set_create(const Collection &p_uniforms, RID p_shader, uint32_t p_shader_set);
+	RID uniform_set_create(const Collection &p_uniforms, RID p_shader, uint32_t p_shader_set, bool p_linear_pool = false);
 	bool uniform_set_is_valid(RID p_uniform_set);
 	void uniform_set_set_invalidation_callback(RID p_uniform_set, InvalidationCallback p_callback, void *p_userdata);
 
+	bool uniform_sets_have_linear_pools() const;
+
 	/*******************/
 	/**** PIPELINES ****/
 	/*******************/
@@ -1181,6 +1210,7 @@ public:
 	void draw_list_draw(DrawListID p_list, bool p_use_indices, uint32_t p_instances = 1, uint32_t p_procedural_vertices = 0);
 	void draw_list_draw_indirect(DrawListID p_list, bool p_use_indices, RID p_buffer, uint32_t p_offset = 0, uint32_t p_draw_count = 1, uint32_t p_stride = 0);
 
+	void draw_list_set_viewport(DrawListID p_list, const Rect2 &p_rect);
 	void draw_list_enable_scissor(DrawListID p_list, const Rect2 &p_rect);
 	void draw_list_disable_scissor(DrawListID p_list);
 
@@ -1374,7 +1404,8 @@ private:
 		// This must have the same size of the transfer worker pool.
 		TightLocalVector<RDD::SemaphoreID> transfer_worker_semaphores;
 
-		// Extra command buffer pool used for driver workarounds.
+		// Extra command buffer pool used for driver workarounds or to reduce GPU bubbles by
+		// splitting the final render pass to the swapchain into its own cmd buffer.
 		RDG::CommandBufferPool command_buffer_pool;
 
 		struct Timestamp {
@@ -1405,8 +1436,14 @@ private:
 	uint64_t texture_memory = 0;
 	uint64_t buffer_memory = 0;
 
+protected:
+	void execute_chained_cmds(bool p_present_swap_chain,
+			RenderingDeviceDriver::FenceID p_draw_fence,
+			RenderingDeviceDriver::SemaphoreID p_dst_draw_semaphore_to_signal);
+
+public:
 	void _free_internal(RID p_id);
-	void _begin_frame();
+	void _begin_frame(bool p_presented = false);
 	void _end_frame();
 	void _execute_frame(bool p_present);
 	void _stall_for_previous_frames();
diff --git a/servers/rendering/rendering_device_commons.h b/servers/rendering/rendering_device_commons.h
index 9d01b69550..284337fe6b 100644
--- a/servers/rendering/rendering_device_commons.h
+++ b/servers/rendering/rendering_device_commons.h
@@ -43,6 +43,8 @@ class RenderingDeviceCommons : public Object {
 	// with RenderingDeviceDriver.
 	////////////////////////////////////////////
 public:
+	static const bool command_pool_reset_enabled = true;
+
 	/*****************/
 	/**** GENERIC ****/
 	/*****************/
@@ -359,6 +361,22 @@ public:
 		TEXTURE_USAGE_CAN_COPY_TO_BIT = (1 << 8),
 		TEXTURE_USAGE_INPUT_ATTACHMENT_BIT = (1 << 9),
 		TEXTURE_USAGE_VRS_ATTACHMENT_BIT = (1 << 10),
+		// When set, the texture is not backed by actual memory. It only ever lives in the cache.
+		// This is particularly useful for:
+		//	1. Depth/stencil buffers that are not needed after producing the colour output.
+		//	2. MSAA surfaces that are immediately resolved (i.e. its raw content isn't needed).
+		//
+		// This flag heavily improves performance & saves memory on TBDR GPUs (e.g. mobile).
+		// On Desktop this flag won't save memory but it still instructs the render graph that data will
+		// be discarded aggressively which may still improve some performance.
+		//
+		// It is not valid to perform copies from/to this texture, since it doesn't occupy actual RAM.
+		// It is also not valid to sample from this texture except using subpasses or via read/write
+		// pixel shader extensions (e.g. VK_EXT_rasterization_order_attachment_access).
+		//
+		// Try to set this bit as much as possible. If you set it, validation doesn't complain
+		// and it works fine on mobile, then go ahead.
+		TEXTURE_USAGE_TRANSIENT_BIT = (1 << 11),
 	};
 
 	struct TextureFormat {
diff --git a/servers/rendering/rendering_device_driver.h b/servers/rendering/rendering_device_driver.h
index d2d14676db..9c0a060d54 100644
--- a/servers/rendering/rendering_device_driver.h
+++ b/servers/rendering/rendering_device_driver.h
@@ -426,6 +426,7 @@ public:
 	};
 
 	virtual CommandPoolID command_pool_create(CommandQueueFamilyID p_cmd_queue_family, CommandBufferType p_cmd_buffer_type) = 0;
+	virtual bool command_pool_reset(CommandPoolID p_cmd_pool) = 0;
 	virtual void command_pool_free(CommandPoolID p_cmd_pool) = 0;
 
 	// ----- BUFFER -----
@@ -478,7 +479,21 @@ public:
 
 	virtual String shader_get_binary_cache_key() = 0;
 	virtual Vector<uint8_t> shader_compile_binary_from_spirv(VectorView<ShaderStageSPIRVData> p_spirv, const String &p_shader_name) = 0;
-	virtual ShaderID shader_create_from_bytecode(const Vector<uint8_t> &p_shader_binary, ShaderDescription &r_shader_desc, String &r_name) = 0;
+
+	struct ImmutableSampler {
+		UniformType type = UNIFORM_TYPE_MAX;
+		uint32_t binding = 0xffffffff; // Binding index as specified in shader.
+		LocalVector<ID> ids;
+	};
+	/** Creates a Pipeline State Object (PSO) out of the shader and all the input data it needs.
+	@param p_shader_binary		Shader binary bytecode (e.g. SPIR-V).
+	@param r_shader_desc		TBD.
+	@param r_name				TBD.
+	@param p_immutable_samplers	Immutable samplers can be embedded when creating the pipeline layout on the condition they
+								remain valid and unchanged, so they don't need to be specified when creating uniform sets.
+	@return						PSO resource for binding.
+	*/
+	virtual ShaderID shader_create_from_bytecode(const Vector<uint8_t> &p_shader_binary, ShaderDescription &r_shader_desc, String &r_name, const Vector<ImmutableSampler> &p_immutable_samplers) = 0;
 	// Only meaningful if API_TRAIT_SHADER_CHANGE_INVALIDATION is SHADER_CHANGE_INVALIDATION_ALL_OR_NONE_ACCORDING_TO_LAYOUT_HASH.
 	virtual uint32_t shader_get_layout_hash(ShaderID p_shader) { return 0; }
 	virtual void shader_free(ShaderID p_shader) = 0;
@@ -497,10 +512,15 @@ public:
 		UniformType type = UNIFORM_TYPE_MAX;
 		uint32_t binding = 0xffffffff; // Binding index as specified in shader.
 		LocalVector<ID> ids;
+		// Flag to indicate  that this is an immutable sampler so it is skipped when creating uniform
+		// sets, as it would be set previously when creating the pipeline layout.
+		bool immutable_sampler = false;
 	};
 
-	virtual UniformSetID uniform_set_create(VectorView<BoundUniform> p_uniforms, ShaderID p_shader, uint32_t p_set_index) = 0;
+	virtual UniformSetID uniform_set_create(VectorView<BoundUniform> p_uniforms, ShaderID p_shader, uint32_t p_set_index, int p_linear_pool_index) = 0;
+	virtual void linear_uniform_set_pools_reset(int p_linear_pool_index) {}
 	virtual void uniform_set_free(UniformSetID p_uniform_set) = 0;
+	virtual bool uniform_sets_have_linear_pools() const { return false; }
 
 	// ----- COMMANDS -----
 
@@ -642,6 +662,7 @@ public:
 	// Binding.
 	virtual void command_bind_render_pipeline(CommandBufferID p_cmd_buffer, PipelineID p_pipeline) = 0;
 	virtual void command_bind_render_uniform_set(CommandBufferID p_cmd_buffer, UniformSetID p_uniform_set, ShaderID p_shader, uint32_t p_set_index) = 0;
+	virtual void command_bind_render_uniform_sets(CommandBufferID p_cmd_buffer, VectorView<UniformSetID> p_uniform_sets, ShaderID p_shader, uint32_t p_first_set_index, uint32_t p_set_count) = 0;
 
 	// Drawing.
 	virtual void command_render_draw(CommandBufferID p_cmd_buffer, uint32_t p_vertex_count, uint32_t p_instance_count, uint32_t p_base_vertex, uint32_t p_first_instance) = 0;
@@ -684,6 +705,7 @@ public:
 	// Binding.
 	virtual void command_bind_compute_pipeline(CommandBufferID p_cmd_buffer, PipelineID p_pipeline) = 0;
 	virtual void command_bind_compute_uniform_set(CommandBufferID p_cmd_buffer, UniformSetID p_uniform_set, ShaderID p_shader, uint32_t p_set_index) = 0;
+	virtual void command_bind_compute_uniform_sets(CommandBufferID p_cmd_buffer, VectorView<UniformSetID> p_uniform_sets, ShaderID p_shader, uint32_t p_first_set_index, uint32_t p_set_count) = 0;
 
 	// Dispatching.
 	virtual void command_compute_dispatch(CommandBufferID p_cmd_buffer, uint32_t p_x_groups, uint32_t p_y_groups, uint32_t p_z_groups) = 0;
@@ -785,6 +807,7 @@ public:
 	virtual void set_object_name(ObjectType p_type, ID p_driver_id, const String &p_name) = 0;
 	virtual uint64_t get_resource_native_handle(DriverResource p_type, ID p_driver_id) = 0;
 	virtual uint64_t get_total_memory_used() = 0;
+	virtual uint64_t get_lazily_memory_used() = 0;
 	virtual uint64_t limit_get(Limit p_limit) = 0;
 	virtual uint64_t api_trait_get(ApiTrait p_trait);
 	virtual bool has_feature(Features p_feature) = 0;
diff --git a/servers/rendering/rendering_device_graph.cpp b/servers/rendering/rendering_device_graph.cpp
index b2779af620..8cc9026659 100644
--- a/servers/rendering/rendering_device_graph.cpp
+++ b/servers/rendering/rendering_device_graph.cpp
@@ -701,10 +701,10 @@ void RenderingDeviceGraph::_run_compute_list_command(RDD::CommandBufferID p_comm
 				driver->command_bind_compute_pipeline(p_command_buffer, bind_pipeline_instruction->pipeline);
 				instruction_data_cursor += sizeof(ComputeListBindPipelineInstruction);
 			} break;
-			case ComputeListInstruction::TYPE_BIND_UNIFORM_SET: {
-				const ComputeListBindUniformSetInstruction *bind_uniform_set_instruction = reinterpret_cast<const ComputeListBindUniformSetInstruction *>(instruction);
-				driver->command_bind_compute_uniform_set(p_command_buffer, bind_uniform_set_instruction->uniform_set, bind_uniform_set_instruction->shader, bind_uniform_set_instruction->set_index);
-				instruction_data_cursor += sizeof(ComputeListBindUniformSetInstruction);
+			case ComputeListInstruction::TYPE_BIND_UNIFORM_SETS: {
+				const ComputeListBindUniformSetsInstruction *bind_uniform_sets_instruction = reinterpret_cast<const ComputeListBindUniformSetsInstruction *>(instruction);
+				driver->command_bind_compute_uniform_sets(p_command_buffer, VectorView<RDD::UniformSetID>(bind_uniform_sets_instruction->uniform_set_ids(), bind_uniform_sets_instruction->set_count), bind_uniform_sets_instruction->shader, bind_uniform_sets_instruction->first_set_index, bind_uniform_sets_instruction->set_count);
+				instruction_data_cursor += sizeof(ComputeListBindUniformSetsInstruction) + sizeof(RDD::UniformSetID) * bind_uniform_sets_instruction->set_count;
 			} break;
 			case ComputeListInstruction::TYPE_DISPATCH: {
 				const ComputeListDispatchInstruction *dispatch_instruction = reinterpret_cast<const ComputeListDispatchInstruction *>(instruction);
@@ -784,10 +784,10 @@ void RenderingDeviceGraph::_run_draw_list_command(RDD::CommandBufferID p_command
 				driver->command_bind_render_pipeline(p_command_buffer, bind_pipeline_instruction->pipeline);
 				instruction_data_cursor += sizeof(DrawListBindPipelineInstruction);
 			} break;
-			case DrawListInstruction::TYPE_BIND_UNIFORM_SET: {
-				const DrawListBindUniformSetInstruction *bind_uniform_set_instruction = reinterpret_cast<const DrawListBindUniformSetInstruction *>(instruction);
-				driver->command_bind_render_uniform_set(p_command_buffer, bind_uniform_set_instruction->uniform_set, bind_uniform_set_instruction->shader, bind_uniform_set_instruction->set_index);
-				instruction_data_cursor += sizeof(DrawListBindUniformSetInstruction);
+			case DrawListInstruction::TYPE_BIND_UNIFORM_SETS: {
+				const DrawListBindUniformSetsInstruction *bind_uniform_sets_instruction = reinterpret_cast<const DrawListBindUniformSetsInstruction *>(instruction);
+				driver->command_bind_render_uniform_sets(p_command_buffer, VectorView<RDD::UniformSetID>(bind_uniform_sets_instruction->uniform_set_ids(), bind_uniform_sets_instruction->set_count), bind_uniform_sets_instruction->shader, bind_uniform_sets_instruction->first_set_index, bind_uniform_sets_instruction->set_count);
+				instruction_data_cursor += sizeof(DrawListBindUniformSetsInstruction) + sizeof(RDD::UniformSetID) * bind_uniform_sets_instruction->set_count;
 			} break;
 			case DrawListInstruction::TYPE_BIND_VERTEX_BUFFERS: {
 				const DrawListBindVertexBuffersInstruction *bind_vertex_buffers_instruction = reinterpret_cast<const DrawListBindVertexBuffersInstruction *>(instruction);
@@ -874,7 +874,7 @@ void RenderingDeviceGraph::_run_draw_list_command(RDD::CommandBufferID p_command
 	}
 }
 
-void RenderingDeviceGraph::_add_draw_list_begin(FramebufferCache *p_framebuffer_cache, RDD::RenderPassID p_render_pass, RDD::FramebufferID p_framebuffer, Rect2i p_region, VectorView<AttachmentOperation> p_attachment_operations, VectorView<RDD::RenderPassClearValue> p_attachment_clear_values, bool p_uses_color, bool p_uses_depth, uint32_t p_breadcrumb) {
+void RenderingDeviceGraph::_add_draw_list_begin(FramebufferCache *p_framebuffer_cache, RDD::RenderPassID p_render_pass, RDD::FramebufferID p_framebuffer, Rect2i p_region, VectorView<AttachmentOperation> p_attachment_operations, VectorView<RDD::RenderPassClearValue> p_attachment_clear_values, bool p_uses_color, bool p_uses_depth, uint32_t p_breadcrumb, bool p_split_cmd_buffer) {
 	DEV_ASSERT(p_attachment_operations.size() == p_attachment_clear_values.size());
 
 	draw_instruction_list.clear();
@@ -900,6 +900,8 @@ void RenderingDeviceGraph::_add_draw_list_begin(FramebufferCache *p_framebuffer_
 		draw_instruction_list.stages.set_flag(RDD::PIPELINE_STAGE_LATE_FRAGMENT_TESTS_BIT);
 	}
 
+	draw_instruction_list.split_cmd_buffer = p_split_cmd_buffer;
+
 #if defined(DEBUG_ENABLED) || defined(DEV_ENABLED)
 	draw_instruction_list.breadcrumb = p_breadcrumb;
 #endif
@@ -979,6 +981,24 @@ void RenderingDeviceGraph::_run_render_commands(int32_t p_level, const RecordedC
 				}
 
 				const RecordedDrawListCommand *draw_list_command = reinterpret_cast<const RecordedDrawListCommand *>(command);
+
+				if (draw_list_command->split_cmd_buffer) {
+					// Create or reuse a command buffer and finish recording the current one.
+					driver->command_buffer_end(r_command_buffer);
+
+					while (r_command_buffer_pool.buffers_used >= r_command_buffer_pool.buffers.size()) {
+						RDD::CommandBufferID command_buffer = driver->command_buffer_create(r_command_buffer_pool.pool);
+						RDD::SemaphoreID command_semaphore = driver->semaphore_create();
+						r_command_buffer_pool.buffers.push_back(command_buffer);
+						r_command_buffer_pool.semaphores.push_back(command_semaphore);
+					}
+
+					// Start recording on the next usable command buffer from the pool.
+					uint32_t command_buffer_index = r_command_buffer_pool.buffers_used++;
+					r_command_buffer = r_command_buffer_pool.buffers[command_buffer_index];
+					driver->command_buffer_begin(r_command_buffer);
+				}
+
 				const VectorView clear_values(draw_list_command->clear_values(), draw_list_command->clear_values_count);
 #if defined(DEBUG_ENABLED) || defined(DEV_ENABLED)
 				driver->command_insert_breadcrumb(r_command_buffer, draw_list_command->breadcrumb);
@@ -1319,10 +1339,13 @@ void RenderingDeviceGraph::_print_draw_list(const uint8_t *p_instruction_data, u
 				print_line("\tBIND PIPELINE ID", itos(bind_pipeline_instruction->pipeline.id));
 				instruction_data_cursor += sizeof(DrawListBindPipelineInstruction);
 			} break;
-			case DrawListInstruction::TYPE_BIND_UNIFORM_SET: {
-				const DrawListBindUniformSetInstruction *bind_uniform_set_instruction = reinterpret_cast<const DrawListBindUniformSetInstruction *>(instruction);
-				print_line("\tBIND UNIFORM SET ID", itos(bind_uniform_set_instruction->uniform_set.id), "SET INDEX", bind_uniform_set_instruction->set_index);
-				instruction_data_cursor += sizeof(DrawListBindUniformSetInstruction);
+			case DrawListInstruction::TYPE_BIND_UNIFORM_SETS: {
+				const DrawListBindUniformSetsInstruction *bind_uniform_sets_instruction = reinterpret_cast<const DrawListBindUniformSetsInstruction *>(instruction);
+				print_line("\tBIND UNIFORM SETS COUNT", bind_uniform_sets_instruction->set_count);
+				for (uint32_t i = 0; i < bind_uniform_sets_instruction->set_count; i++) {
+					print_line("\tBIND UNIFORM SET ID", itos(bind_uniform_sets_instruction->uniform_set_ids()[i].id), "START INDEX", bind_uniform_sets_instruction->first_set_index);
+				}
+				instruction_data_cursor += sizeof(DrawListBindUniformSetsInstruction) + sizeof(RDD::UniformSetID) * bind_uniform_sets_instruction->set_count;
 			} break;
 			case DrawListInstruction::TYPE_BIND_VERTEX_BUFFERS: {
 				const DrawListBindVertexBuffersInstruction *bind_vertex_buffers_instruction = reinterpret_cast<const DrawListBindVertexBuffersInstruction *>(instruction);
@@ -1416,10 +1439,13 @@ void RenderingDeviceGraph::_print_compute_list(const uint8_t *p_instruction_data
 				print_line("\tBIND PIPELINE ID", itos(bind_pipeline_instruction->pipeline.id));
 				instruction_data_cursor += sizeof(ComputeListBindPipelineInstruction);
 			} break;
-			case ComputeListInstruction::TYPE_BIND_UNIFORM_SET: {
-				const ComputeListBindUniformSetInstruction *bind_uniform_set_instruction = reinterpret_cast<const ComputeListBindUniformSetInstruction *>(instruction);
-				print_line("\tBIND UNIFORM SET ID", itos(bind_uniform_set_instruction->uniform_set.id), "SHADER ID", itos(bind_uniform_set_instruction->shader.id));
-				instruction_data_cursor += sizeof(ComputeListBindUniformSetInstruction);
+			case ComputeListInstruction::TYPE_BIND_UNIFORM_SETS: {
+				const ComputeListBindUniformSetsInstruction *bind_uniform_sets_instruction = reinterpret_cast<const ComputeListBindUniformSetsInstruction *>(instruction);
+				print_line("\tBIND UNIFORM SETS COUNT", bind_uniform_sets_instruction->set_count);
+				for (uint32_t i = 0; i < bind_uniform_sets_instruction->set_count; i++) {
+					print_line("\tBIND UNIFORM SET ID", itos(bind_uniform_sets_instruction->uniform_set_ids()[i].id), "START INDEX", bind_uniform_sets_instruction->first_set_index);
+				}
+				instruction_data_cursor += sizeof(ComputeListBindUniformSetsInstruction) + sizeof(RDD::UniformSetID) * bind_uniform_sets_instruction->set_count;
 			} break;
 			case ComputeListInstruction::TYPE_DISPATCH: {
 				const ComputeListDispatchInstruction *dispatch_instruction = reinterpret_cast<const ComputeListDispatchInstruction *>(instruction);
@@ -1604,11 +1630,23 @@ void RenderingDeviceGraph::add_compute_list_bind_pipeline(RDD::PipelineID p_pipe
 }
 
 void RenderingDeviceGraph::add_compute_list_bind_uniform_set(RDD::ShaderID p_shader, RDD::UniformSetID p_uniform_set, uint32_t set_index) {
-	ComputeListBindUniformSetInstruction *instruction = reinterpret_cast<ComputeListBindUniformSetInstruction *>(_allocate_compute_list_instruction(sizeof(ComputeListBindUniformSetInstruction)));
-	instruction->type = ComputeListInstruction::TYPE_BIND_UNIFORM_SET;
+	add_compute_list_bind_uniform_sets(p_shader, VectorView(&p_uniform_set, 1), set_index, 1);
+}
+
+void RenderingDeviceGraph::add_compute_list_bind_uniform_sets(RDD::ShaderID p_shader, VectorView<RDD::UniformSetID> p_uniform_sets, uint32_t p_first_set_index, uint32_t p_set_count) {
+	DEV_ASSERT(p_uniform_sets.size() >= p_set_count);
+
+	uint32_t instruction_size = sizeof(ComputeListBindUniformSetsInstruction) + sizeof(RDD::UniformSetID) * p_set_count;
+	ComputeListBindUniformSetsInstruction *instruction = reinterpret_cast<ComputeListBindUniformSetsInstruction *>(_allocate_compute_list_instruction(instruction_size));
+	instruction->type = ComputeListInstruction::TYPE_BIND_UNIFORM_SETS;
 	instruction->shader = p_shader;
-	instruction->uniform_set = p_uniform_set;
-	instruction->set_index = set_index;
+	instruction->first_set_index = p_first_set_index;
+	instruction->set_count = p_set_count;
+
+	RDD::UniformSetID *ids = instruction->uniform_set_ids();
+	for (uint32_t i = 0; i < p_set_count; i++) {
+		ids[i] = p_uniform_sets[i];
+	}
 }
 
 void RenderingDeviceGraph::add_compute_list_dispatch(uint32_t p_x_groups, uint32_t p_y_groups, uint32_t p_z_groups) {
@@ -1682,12 +1720,12 @@ void RenderingDeviceGraph::add_compute_list_end() {
 	_add_command_to_graph(compute_instruction_list.command_trackers.ptr(), compute_instruction_list.command_tracker_usages.ptr(), compute_instruction_list.command_trackers.size(), command_index, command);
 }
 
-void RenderingDeviceGraph::add_draw_list_begin(FramebufferCache *p_framebuffer_cache, Rect2i p_region, VectorView<AttachmentOperation> p_attachment_operations, VectorView<RDD::RenderPassClearValue> p_attachment_clear_values, bool p_uses_color, bool p_uses_depth, uint32_t p_breadcrumb) {
-	_add_draw_list_begin(p_framebuffer_cache, RDD::RenderPassID(), RDD::FramebufferID(), p_region, p_attachment_operations, p_attachment_clear_values, p_uses_color, p_uses_depth, p_breadcrumb);
+void RenderingDeviceGraph::add_draw_list_begin(FramebufferCache *p_framebuffer_cache, Rect2i p_region, VectorView<AttachmentOperation> p_attachment_operations, VectorView<RDD::RenderPassClearValue> p_attachment_clear_values, bool p_uses_color, bool p_uses_depth, uint32_t p_breadcrumb, bool p_split_cmd_buffer) {
+	_add_draw_list_begin(p_framebuffer_cache, RDD::RenderPassID(), RDD::FramebufferID(), p_region, p_attachment_operations, p_attachment_clear_values, p_uses_color, p_uses_depth, p_breadcrumb, p_split_cmd_buffer);
 }
 
-void RenderingDeviceGraph::add_draw_list_begin(RDD::RenderPassID p_render_pass, RDD::FramebufferID p_framebuffer, Rect2i p_region, VectorView<AttachmentOperation> p_attachment_operations, VectorView<RDD::RenderPassClearValue> p_attachment_clear_values, bool p_uses_color, bool p_uses_depth, uint32_t p_breadcrumb) {
-	_add_draw_list_begin(nullptr, p_render_pass, p_framebuffer, p_region, p_attachment_operations, p_attachment_clear_values, p_uses_color, p_uses_depth, p_breadcrumb);
+void RenderingDeviceGraph::add_draw_list_begin(RDD::RenderPassID p_render_pass, RDD::FramebufferID p_framebuffer, Rect2i p_region, VectorView<AttachmentOperation> p_attachment_operations, VectorView<RDD::RenderPassClearValue> p_attachment_clear_values, bool p_uses_color, bool p_uses_depth, uint32_t p_breadcrumb, bool p_split_cmd_buffer) {
+	_add_draw_list_begin(nullptr, p_render_pass, p_framebuffer, p_region, p_attachment_operations, p_attachment_clear_values, p_uses_color, p_uses_depth, p_breadcrumb, p_split_cmd_buffer);
 }
 
 void RenderingDeviceGraph::add_draw_list_bind_index_buffer(RDD::BufferID p_buffer, RDD::IndexBufferFormat p_format, uint32_t p_offset) {
@@ -1710,11 +1748,22 @@ void RenderingDeviceGraph::add_draw_list_bind_pipeline(RDD::PipelineID p_pipelin
 }
 
 void RenderingDeviceGraph::add_draw_list_bind_uniform_set(RDD::ShaderID p_shader, RDD::UniformSetID p_uniform_set, uint32_t set_index) {
-	DrawListBindUniformSetInstruction *instruction = reinterpret_cast<DrawListBindUniformSetInstruction *>(_allocate_draw_list_instruction(sizeof(DrawListBindUniformSetInstruction)));
-	instruction->type = DrawListInstruction::TYPE_BIND_UNIFORM_SET;
+	add_draw_list_bind_uniform_sets(p_shader, VectorView(&p_uniform_set, 1), set_index, 1);
+}
+
+void RenderingDeviceGraph::add_draw_list_bind_uniform_sets(RDD::ShaderID p_shader, VectorView<RDD::UniformSetID> p_uniform_sets, uint32_t p_first_index, uint32_t p_set_count) {
+	DEV_ASSERT(p_uniform_sets.size() >= p_set_count);
+
+	uint32_t instruction_size = sizeof(DrawListBindUniformSetsInstruction) + sizeof(RDD::UniformSetID) * p_set_count;
+	DrawListBindUniformSetsInstruction *instruction = reinterpret_cast<DrawListBindUniformSetsInstruction *>(_allocate_draw_list_instruction(instruction_size));
+	instruction->type = DrawListInstruction::TYPE_BIND_UNIFORM_SETS;
 	instruction->shader = p_shader;
-	instruction->uniform_set = p_uniform_set;
-	instruction->set_index = set_index;
+	instruction->first_set_index = p_first_index;
+	instruction->set_count = p_set_count;
+
+	for (uint32_t i = 0; i < p_set_count; i++) {
+		instruction->uniform_set_ids()[i] = p_uniform_sets[i];
+	}
 }
 
 void RenderingDeviceGraph::add_draw_list_bind_vertex_buffers(VectorView<RDD::BufferID> p_vertex_buffers, VectorView<uint64_t> p_vertex_buffer_offsets) {
@@ -1887,6 +1936,7 @@ void RenderingDeviceGraph::add_draw_list_end() {
 #if defined(DEBUG_ENABLED) || defined(DEV_ENABLED)
 	command->breadcrumb = draw_instruction_list.breadcrumb;
 #endif
+	command->split_cmd_buffer = draw_instruction_list.split_cmd_buffer;
 	command->clear_values_count = draw_instruction_list.attachment_clear_values.size();
 	command->trackers_count = trackers_count;
 
diff --git a/servers/rendering/rendering_device_graph.h b/servers/rendering/rendering_device_graph.h
index adfbb47e84..15431e41c0 100644
--- a/servers/rendering/rendering_device_graph.h
+++ b/servers/rendering/rendering_device_graph.h
@@ -49,7 +49,7 @@ public:
 		enum Type {
 			TYPE_NONE,
 			TYPE_BIND_PIPELINE,
-			TYPE_BIND_UNIFORM_SET,
+			TYPE_BIND_UNIFORM_SETS,
 			TYPE_DISPATCH,
 			TYPE_DISPATCH_INDIRECT,
 			TYPE_SET_PUSH_CONSTANT,
@@ -64,7 +64,7 @@ public:
 			TYPE_NONE,
 			TYPE_BIND_INDEX_BUFFER,
 			TYPE_BIND_PIPELINE,
-			TYPE_BIND_UNIFORM_SET,
+			TYPE_BIND_UNIFORM_SETS,
 			TYPE_BIND_VERTEX_BUFFERS,
 			TYPE_CLEAR_ATTACHMENTS,
 			TYPE_DRAW,
@@ -266,6 +266,7 @@ private:
 #if defined(DEBUG_ENABLED) || defined(DEV_ENABLED)
 		uint32_t breadcrumb;
 #endif
+		bool split_cmd_buffer = false;
 	};
 
 	struct RecordedCommandSort {
@@ -361,6 +362,7 @@ private:
 #if defined(DEBUG_ENABLED) || defined(DEV_ENABLED)
 		uint32_t breadcrumb = 0;
 #endif
+		bool split_cmd_buffer = false;
 
 		_FORCE_INLINE_ RDD::RenderPassClearValue *clear_values() {
 			return reinterpret_cast<RDD::RenderPassClearValue *>(&this[1]);
@@ -474,10 +476,18 @@ private:
 		RDD::PipelineID pipeline;
 	};
 
-	struct DrawListBindUniformSetInstruction : DrawListInstruction {
-		RDD::UniformSetID uniform_set;
+	struct DrawListBindUniformSetsInstruction : DrawListInstruction {
 		RDD::ShaderID shader;
-		uint32_t set_index = 0;
+		uint32_t first_set_index = 0;
+		uint32_t set_count = 0;
+
+		_FORCE_INLINE_ RDD::UniformSetID *uniform_set_ids() {
+			return reinterpret_cast<RDD::UniformSetID *>(&this[1]);
+		}
+
+		_FORCE_INLINE_ const RDD::UniformSetID *uniform_set_ids() const {
+			return reinterpret_cast<const RDD::UniformSetID *>(&this[1]);
+		}
 	};
 
 	struct DrawListBindVertexBuffersInstruction : DrawListInstruction {
@@ -597,10 +607,18 @@ private:
 		RDD::PipelineID pipeline;
 	};
 
-	struct ComputeListBindUniformSetInstruction : ComputeListInstruction {
-		RDD::UniformSetID uniform_set;
+	struct ComputeListBindUniformSetsInstruction : ComputeListInstruction {
 		RDD::ShaderID shader;
-		uint32_t set_index = 0;
+		uint32_t first_set_index = 0;
+		uint32_t set_count = 0;
+
+		_FORCE_INLINE_ RDD::UniformSetID *uniform_set_ids() {
+			return reinterpret_cast<RDD::UniformSetID *>(&this[1]);
+		}
+
+		_FORCE_INLINE_ const RDD::UniformSetID *uniform_set_ids() const {
+			return reinterpret_cast<const RDD::UniformSetID *>(&this[1]);
+		}
 	};
 
 	struct ComputeListDispatchInstruction : ComputeListInstruction {
@@ -726,7 +744,7 @@ private:
 	void _run_compute_list_command(RDD::CommandBufferID p_command_buffer, const uint8_t *p_instruction_data, uint32_t p_instruction_data_size);
 	void _get_draw_list_render_pass_and_framebuffer(const RecordedDrawListCommand *p_draw_list_command, RDD::RenderPassID &r_render_pass, RDD::FramebufferID &r_framebuffer);
 	void _run_draw_list_command(RDD::CommandBufferID p_command_buffer, const uint8_t *p_instruction_data, uint32_t p_instruction_data_size);
-	void _add_draw_list_begin(FramebufferCache *p_framebuffer_cache, RDD::RenderPassID p_render_pass, RDD::FramebufferID p_framebuffer, Rect2i p_region, VectorView<AttachmentOperation> p_attachment_operations, VectorView<RDD::RenderPassClearValue> p_attachment_clear_values, bool p_uses_color, bool p_uses_depth, uint32_t p_breadcrumb);
+	void _add_draw_list_begin(FramebufferCache *p_framebuffer_cache, RDD::RenderPassID p_render_pass, RDD::FramebufferID p_framebuffer, Rect2i p_region, VectorView<AttachmentOperation> p_attachment_operations, VectorView<RDD::RenderPassClearValue> p_attachment_clear_values, bool p_uses_color, bool p_uses_depth, uint32_t p_breadcrumb, bool p_split_cmd_buffer);
 	void _run_secondary_command_buffer_task(const SecondaryCommandBuffer *p_secondary);
 	void _wait_for_secondary_command_buffer_tasks();
 	void _run_render_commands(int32_t p_level, const RecordedCommandSort *p_sorted_commands, uint32_t p_sorted_commands_count, RDD::CommandBufferID &r_command_buffer, CommandBufferPool &r_command_buffer_pool, int32_t &r_current_label_index, int32_t &r_current_label_level);
@@ -750,6 +768,7 @@ public:
 	void add_compute_list_begin(RDD::BreadcrumbMarker p_phase = RDD::BreadcrumbMarker::NONE, uint32_t p_breadcrumb_data = 0);
 	void add_compute_list_bind_pipeline(RDD::PipelineID p_pipeline);
 	void add_compute_list_bind_uniform_set(RDD::ShaderID p_shader, RDD::UniformSetID p_uniform_set, uint32_t set_index);
+	void add_compute_list_bind_uniform_sets(RDD::ShaderID p_shader, VectorView<RDD::UniformSetID> p_uniform_set, uint32_t p_first_set_index, uint32_t p_set_count);
 	void add_compute_list_dispatch(uint32_t p_x_groups, uint32_t p_y_groups, uint32_t p_z_groups);
 	void add_compute_list_dispatch_indirect(RDD::BufferID p_buffer, uint32_t p_offset);
 	void add_compute_list_set_push_constant(RDD::ShaderID p_shader, const void *p_data, uint32_t p_data_size);
@@ -757,11 +776,12 @@ public:
 	void add_compute_list_usage(ResourceTracker *p_tracker, ResourceUsage p_usage);
 	void add_compute_list_usages(VectorView<ResourceTracker *> p_trackers, VectorView<ResourceUsage> p_usages);
 	void add_compute_list_end();
-	void add_draw_list_begin(FramebufferCache *p_framebuffer_cache, Rect2i p_region, VectorView<AttachmentOperation> p_attachment_operations, VectorView<RDD::RenderPassClearValue> p_attachment_clear_values, bool p_uses_color, bool p_uses_depth, uint32_t p_breadcrumb = 0);
-	void add_draw_list_begin(RDD::RenderPassID p_render_pass, RDD::FramebufferID p_framebuffer, Rect2i p_region, VectorView<AttachmentOperation> p_attachment_operations, VectorView<RDD::RenderPassClearValue> p_attachment_clear_values, bool p_uses_color, bool p_uses_depth, uint32_t p_breadcrumb = 0);
+	void add_draw_list_begin(FramebufferCache *p_framebuffer_cache, Rect2i p_region, VectorView<AttachmentOperation> p_attachment_operations, VectorView<RDD::RenderPassClearValue> p_attachment_clear_values, bool p_uses_color, bool p_uses_depth, uint32_t p_breadcrumb = 0, bool p_split_cmd_buffer = false);
+	void add_draw_list_begin(RDD::RenderPassID p_render_pass, RDD::FramebufferID p_framebuffer, Rect2i p_region, VectorView<AttachmentOperation> p_attachment_operations, VectorView<RDD::RenderPassClearValue> p_attachment_clear_values, bool p_uses_color, bool p_uses_depth, uint32_t p_breadcrumb = 0, bool p_split_cmd_buffer = false);
 	void add_draw_list_bind_index_buffer(RDD::BufferID p_buffer, RDD::IndexBufferFormat p_format, uint32_t p_offset);
 	void add_draw_list_bind_pipeline(RDD::PipelineID p_pipeline, BitField<RDD::PipelineStageBits> p_pipeline_stage_bits);
 	void add_draw_list_bind_uniform_set(RDD::ShaderID p_shader, RDD::UniformSetID p_uniform_set, uint32_t set_index);
+	void add_draw_list_bind_uniform_sets(RDD::ShaderID p_shader, VectorView<RDD::UniformSetID> p_uniform_set, uint32_t p_first_index, uint32_t p_set_count);
 	void add_draw_list_bind_vertex_buffers(VectorView<RDD::BufferID> p_vertex_buffers, VectorView<uint64_t> p_vertex_buffer_offsets);
 	void add_draw_list_clear_attachments(VectorView<RDD::AttachmentClear> p_attachments_clear, VectorView<Rect2i> p_attachments_clear_rect);
 	void add_draw_list_draw(uint32_t p_vertex_count, uint32_t p_instance_count);
diff --git a/thirdparty/vulkan/patches/0003-VMA-add-vmaCalculateLazilyAllocatedBytes.patch b/thirdparty/vulkan/patches/0003-VMA-add-vmaCalculateLazilyAllocatedBytes.patch
new file mode 100644
index 0000000000..4c0d909352
--- /dev/null
+++ b/thirdparty/vulkan/patches/0003-VMA-add-vmaCalculateLazilyAllocatedBytes.patch
@@ -0,0 +1,55 @@
+diff --git a/thirdparty/vulkan/vk_mem_alloc.h b/thirdparty/vulkan/vk_mem_alloc.h
+index ecb84094b9..50ff4ea1c2 100644
+--- a/thirdparty/vulkan/vk_mem_alloc.h
++++ b/thirdparty/vulkan/vk_mem_alloc.h
+@@ -1713,6 +1713,21 @@ VMA_CALL_PRE void VMA_CALL_POST vmaCalculateStatistics(
+     VmaAllocator VMA_NOT_NULL allocator,
+     VmaTotalStatistics* VMA_NOT_NULL pStats);
+ 
++// -- GODOT begin --
++/** \brief Retrieves lazily allocated bytes
++
++This function is called "calculate" not "get" because it has to traverse all
++internal data structures, so it may be quite slow. Use it for debugging purposes.
++For faster but more brief statistics suitable to be called every frame or every allocation,
++use vmaGetHeapBudgets().
++
++Note that when using allocator from multiple threads, returned information may immediately
++become outdated.
++*/
++VMA_CALL_PRE uint64_t VMA_CALL_POST vmaCalculateLazilyAllocatedBytes(
++    VmaAllocator VMA_NOT_NULL allocator);
++// -- GODOT end --
++
+ /** \brief Retrieves information about current memory usage and budget for all memory heaps.
+ 
+ \param allocator
+@@ -14912,6 +14927,28 @@ VMA_CALL_PRE void VMA_CALL_POST vmaCalculateStatistics(
+     allocator->CalculateStatistics(pStats);
+ }
+ 
++// -- GODOT begin --
++VMA_CALL_PRE uint64_t VMA_CALL_POST vmaCalculateLazilyAllocatedBytes(
++    VmaAllocator allocator)
++{
++    VMA_ASSERT(allocator);
++    VMA_DEBUG_GLOBAL_MUTEX_LOCK
++	VmaTotalStatistics stats;
++    allocator->CalculateStatistics(&stats);
++	uint64_t total_lazilily_allocated_bytes = 0;
++	for (uint32_t heapIndex = 0; heapIndex < allocator->GetMemoryHeapCount(); ++heapIndex) {
++		for (uint32_t typeIndex = 0; typeIndex < allocator->GetMemoryTypeCount(); ++typeIndex) {
++			if (allocator->MemoryTypeIndexToHeapIndex(typeIndex) == heapIndex) {
++				VkMemoryPropertyFlags flags = allocator->m_MemProps.memoryTypes[typeIndex].propertyFlags;
++				if (flags & VK_MEMORY_PROPERTY_LAZILY_ALLOCATED_BIT)
++					total_lazilily_allocated_bytes += stats.memoryType[typeIndex].statistics.allocationBytes;
++			}
++		}
++	}
++	return total_lazilily_allocated_bytes;
++}
++// -- GODOT end --
++
+ VMA_CALL_PRE void VMA_CALL_POST vmaGetHeapBudgets(
+     VmaAllocator allocator,
+     VmaBudget* pBudgets)
diff --git a/thirdparty/vulkan/vk_mem_alloc.h b/thirdparty/vulkan/vk_mem_alloc.h
index ecb84094b9..50ff4ea1c2 100644
--- a/thirdparty/vulkan/vk_mem_alloc.h
+++ b/thirdparty/vulkan/vk_mem_alloc.h
@@ -1713,6 +1713,21 @@ VMA_CALL_PRE void VMA_CALL_POST vmaCalculateStatistics(
     VmaAllocator VMA_NOT_NULL allocator,
     VmaTotalStatistics* VMA_NOT_NULL pStats);
 
+// -- GODOT begin --
+/** \brief Retrieves lazily allocated bytes
+
+This function is called "calculate" not "get" because it has to traverse all
+internal data structures, so it may be quite slow. Use it for debugging purposes.
+For faster but more brief statistics suitable to be called every frame or every allocation,
+use vmaGetHeapBudgets().
+
+Note that when using allocator from multiple threads, returned information may immediately
+become outdated.
+*/
+VMA_CALL_PRE uint64_t VMA_CALL_POST vmaCalculateLazilyAllocatedBytes(
+    VmaAllocator VMA_NOT_NULL allocator);
+// -- GODOT end --
+
 /** \brief Retrieves information about current memory usage and budget for all memory heaps.
 
 \param allocator
@@ -14912,6 +14927,28 @@ VMA_CALL_PRE void VMA_CALL_POST vmaCalculateStatistics(
     allocator->CalculateStatistics(pStats);
 }
 
+// -- GODOT begin --
+VMA_CALL_PRE uint64_t VMA_CALL_POST vmaCalculateLazilyAllocatedBytes(
+    VmaAllocator allocator)
+{
+    VMA_ASSERT(allocator);
+    VMA_DEBUG_GLOBAL_MUTEX_LOCK
+	VmaTotalStatistics stats;
+    allocator->CalculateStatistics(&stats);
+	uint64_t total_lazilily_allocated_bytes = 0;
+	for (uint32_t heapIndex = 0; heapIndex < allocator->GetMemoryHeapCount(); ++heapIndex) {
+		for (uint32_t typeIndex = 0; typeIndex < allocator->GetMemoryTypeCount(); ++typeIndex) {
+			if (allocator->MemoryTypeIndexToHeapIndex(typeIndex) == heapIndex) {
+				VkMemoryPropertyFlags flags = allocator->m_MemProps.memoryTypes[typeIndex].propertyFlags;
+				if (flags & VK_MEMORY_PROPERTY_LAZILY_ALLOCATED_BIT)
+					total_lazilily_allocated_bytes += stats.memoryType[typeIndex].statistics.allocationBytes;
+			}
+		}
+	}
+	return total_lazilily_allocated_bytes;
+}
+// -- GODOT end --
+
 VMA_CALL_PRE void VMA_CALL_POST vmaGetHeapBudgets(
     VmaAllocator allocator,
     VmaBudget* pBudgets)

From 3aebcaf4d343055c61ab7dfaeb79d6a8c0090aab Mon Sep 17 00:00:00 2001
From: kobewi <kobewi4e@gmail.com>
Date: Mon, 9 Dec 2024 15:30:53 +0100
Subject: [PATCH 043/124] Clarify limit_length() for infinite vectors

---
 doc/classes/Vector2.xml | 2 +-
 doc/classes/Vector3.xml | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/doc/classes/Vector2.xml b/doc/classes/Vector2.xml
index c03262bb33..57c351ce4a 100644
--- a/doc/classes/Vector2.xml
+++ b/doc/classes/Vector2.xml
@@ -270,7 +270,7 @@
 			<return type="Vector2" />
 			<param index="0" name="length" type="float" default="1.0" />
 			<description>
-				Returns the vector with a maximum length by limiting its length to [param length].
+				Returns the vector with a maximum length by limiting its length to [param length]. If the vector is non-finite, the result is undefined.
 			</description>
 		</method>
 		<method name="max" qualifiers="const">
diff --git a/doc/classes/Vector3.xml b/doc/classes/Vector3.xml
index 4ab3140eb6..6483d96937 100644
--- a/doc/classes/Vector3.xml
+++ b/doc/classes/Vector3.xml
@@ -239,7 +239,7 @@
 			<return type="Vector3" />
 			<param index="0" name="length" type="float" default="1.0" />
 			<description>
-				Returns the vector with a maximum length by limiting its length to [param length].
+				Returns the vector with a maximum length by limiting its length to [param length]. If the vector is non-finite, the result is undefined.
 			</description>
 		</method>
 		<method name="max" qualifiers="const">

From 1e749f628e6eda211ff29c81fae9e7a71b030bd9 Mon Sep 17 00:00:00 2001
From: Robin Ward <robin.ward@gmail.com>
Date: Mon, 9 Dec 2024 11:37:37 -0500
Subject: [PATCH 044/124] Fix user selection on top of `bgcolor` areas in a
 `RichTextLabel`

---
 scene/gui/rich_text_label.cpp | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/scene/gui/rich_text_label.cpp b/scene/gui/rich_text_label.cpp
index ea0b8cd808..be54ef7c94 100644
--- a/scene/gui/rich_text_label.cpp
+++ b/scene/gui/rich_text_label.cpp
@@ -1370,6 +1370,13 @@ int RichTextLabel::_draw_line(ItemFrame *p_frame, int p_line, const Vector2 &p_o
 				}
 			}
 			// Finish lines and boxes.
+			if (step == DRAW_STEP_BACKGROUND || step == DRAW_STEP_FOREGROUND) {
+				if (last_color.a > 0.0) {
+					Vector2 rect_off = p_ofs + Vector2(box_start - theme_cache.text_highlight_h_padding, off_step.y - l_ascent - theme_cache.text_highlight_v_padding);
+					Vector2 rect_size = Vector2(off_step.x - box_start + 2 * theme_cache.text_highlight_h_padding, l_size.y + 2 * theme_cache.text_highlight_v_padding);
+					RenderingServer::get_singleton()->canvas_item_add_rect(ci, Rect2(rect_off, rect_size), last_color);
+				}
+			}
 			if (step == DRAW_STEP_BACKGROUND) {
 				if (sel_start != -1) {
 					Color selection_bg = theme_cache.selection_color;
@@ -1380,13 +1387,6 @@ int RichTextLabel::_draw_line(ItemFrame *p_frame, int p_line, const Vector2 &p_o
 					}
 				}
 			}
-			if (step == DRAW_STEP_BACKGROUND || step == DRAW_STEP_FOREGROUND) {
-				if (last_color.a > 0.0) {
-					Vector2 rect_off = p_ofs + Vector2(box_start - theme_cache.text_highlight_h_padding, off_step.y - l_ascent - theme_cache.text_highlight_v_padding);
-					Vector2 rect_size = Vector2(off_step.x - box_start + 2 * theme_cache.text_highlight_h_padding, l_size.y + 2 * theme_cache.text_highlight_v_padding);
-					RenderingServer::get_singleton()->canvas_item_add_rect(ci, Rect2(rect_off, rect_size), last_color);
-				}
-			}
 			if (step == DRAW_STEP_TEXT) {
 				if (ul_started) {
 					ul_started = false;

From 404383e161a431452b0fcf7002e571d3ed7aea74 Mon Sep 17 00:00:00 2001
From: bruvzg <7645683+bruvzg@users.noreply.github.com>
Date: Mon, 9 Dec 2024 18:27:30 +0200
Subject: [PATCH 045/124] [Windows] Fix MinGW debug symbols.

---
 SConstruct                                        | 9 ++++++---
 platform/windows/crash_handler_windows_signal.cpp | 3 +++
 2 files changed, 9 insertions(+), 3 deletions(-)

diff --git a/SConstruct b/SConstruct
index fa324b2aa0..572ab0b99a 100644
--- a/SConstruct
+++ b/SConstruct
@@ -731,9 +731,12 @@ if env.msvc:
         env.Append(CCFLAGS=["/Od"])
 else:
     if env["debug_symbols"]:
-        # Adding dwarf-4 explicitly makes stacktraces work with clang builds,
-        # otherwise addr2line doesn't understand them
-        env.Append(CCFLAGS=["-gdwarf-4"])
+        if env["platform"] == "windows":
+            env.Append(CCFLAGS=["-gdwarf-5"])
+        else:
+            # Adding dwarf-4 explicitly makes stacktraces work with clang builds,
+            # otherwise addr2line doesn't understand them
+            env.Append(CCFLAGS=["-gdwarf-4"])
         if methods.using_emcc(env):
             # Emscripten only produces dwarf symbols when using "-g3".
             env.Append(CCFLAGS=["-g3"])
diff --git a/platform/windows/crash_handler_windows_signal.cpp b/platform/windows/crash_handler_windows_signal.cpp
index c3a0d08ad6..ddfb9b65de 100644
--- a/platform/windows/crash_handler_windows_signal.cpp
+++ b/platform/windows/crash_handler_windows_signal.cpp
@@ -164,6 +164,9 @@ extern void CrashHandlerException(int signal) {
 	int64_t image_file_base = get_image_base(_execpath);
 	data.offset = image_mem_base - image_file_base;
 
+	if (FileAccess::exists(_execpath + ".debugsymbols")) {
+		_execpath = _execpath + ".debugsymbols";
+	}
 	data.state = backtrace_create_state(_execpath.utf8().get_data(), 0, &error_callback, reinterpret_cast<void *>(&data));
 	if (data.state != nullptr) {
 		data.index = 1;

From 2211b1bf91b080280c3f2d2c3a0eb38cfec090d0 Mon Sep 17 00:00:00 2001
From: Zae <zaevi@live.com>
Date: Tue, 10 Dec 2024 01:35:25 +0800
Subject: [PATCH 046/124] Fix popup shows behind always_on_top parent.

---
 platform/macos/display_server_macos.h       |  4 ++-
 platform/macos/display_server_macos.mm      | 31 ++++++++++++++-------
 platform/macos/godot_window_delegate.mm     |  2 +-
 platform/windows/display_server_windows.cpp | 19 +++++++++++--
 platform/windows/display_server_windows.h   |  2 ++
 5 files changed, 44 insertions(+), 14 deletions(-)

diff --git a/platform/macos/display_server_macos.h b/platform/macos/display_server_macos.h
index 1cb4abc205..39b84adf1a 100644
--- a/platform/macos/display_server_macos.h
+++ b/platform/macos/display_server_macos.h
@@ -215,7 +215,7 @@ private:
 	Callable system_theme_changed;
 
 	WindowID _create_window(WindowMode p_mode, VSyncMode p_vsync_mode, const Rect2i &p_rect);
-	void _update_window_style(WindowData p_wd);
+	void _update_window_style(WindowData p_wd, WindowID p_window);
 
 	void _update_displays_arrangement() const;
 	Point2i _get_native_screen_position(int p_screen) const;
@@ -265,6 +265,8 @@ public:
 	void mouse_exit_window(WindowID p_window);
 	void update_presentation_mode();
 
+	bool is_always_on_top_recursive(WindowID p_window) const;
+
 	void window_destroy(WindowID p_window);
 	void window_resize(WindowID p_window, int p_width, int p_height);
 	void window_set_custom_window_buttons(WindowData &p_wd, bool p_enabled);
diff --git a/platform/macos/display_server_macos.mm b/platform/macos/display_server_macos.mm
index 476e508a55..5b625ef590 100644
--- a/platform/macos/display_server_macos.mm
+++ b/platform/macos/display_server_macos.mm
@@ -211,7 +211,7 @@ DisplayServerMacOS::WindowID DisplayServerMacOS::_create_window(WindowMode p_mod
 	return id;
 }
 
-void DisplayServerMacOS::_update_window_style(WindowData p_wd) {
+void DisplayServerMacOS::_update_window_style(WindowData p_wd, WindowID p_window) {
 	bool borderless_full = false;
 
 	if (p_wd.borderless) {
@@ -231,7 +231,7 @@ void DisplayServerMacOS::_update_window_style(WindowData p_wd) {
 		[(NSWindow *)p_wd.window_object setHidesOnDeactivate:YES];
 	} else {
 		// Reset these when our window is not a borderless window that covers up the screen.
-		if (p_wd.on_top && !p_wd.fullscreen) {
+		if (is_always_on_top_recursive(p_window) && !p_wd.fullscreen) {
 			[(NSWindow *)p_wd.window_object setLevel:NSFloatingWindowLevel];
 		} else {
 			[(NSWindow *)p_wd.window_object setLevel:NSNormalWindowLevel];
@@ -240,6 +240,21 @@ void DisplayServerMacOS::_update_window_style(WindowData p_wd) {
 	}
 }
 
+bool DisplayServerMacOS::is_always_on_top_recursive(WindowID p_window) const {
+	ERR_FAIL_COND_V(!windows.has(p_window), false);
+
+	const WindowData &wd = windows[p_window];
+	if (wd.on_top) {
+		return true;
+	}
+
+	if (wd.transient_parent != INVALID_WINDOW_ID) {
+		return is_always_on_top_recursive(wd.transient_parent);
+	}
+
+	return false;
+}
+
 void DisplayServerMacOS::set_window_per_pixel_transparency_enabled(bool p_enabled, WindowID p_window) {
 	ERR_FAIL_COND(!windows.has(p_window));
 	WindowData &wd = windows[p_window];
@@ -2049,7 +2064,7 @@ void DisplayServerMacOS::window_set_position(const Point2i &p_position, WindowID
 
 	[wd.window_object setFrameTopLeftPoint:NSMakePoint(position.x - offset.x, position.y - offset.y)];
 
-	_update_window_style(wd);
+	_update_window_style(wd, p_window);
 	update_mouse_pos(wd, [wd.window_object mouseLocationOutsideOfEventStream]);
 }
 
@@ -2187,7 +2202,7 @@ void DisplayServerMacOS::window_set_size(const Size2i p_size, WindowID p_window)
 
 	[wd.window_object setFrame:new_frame display:YES];
 
-	_update_window_style(wd);
+	_update_window_style(wd, p_window);
 }
 
 Size2i DisplayServerMacOS::window_get_size(WindowID p_window) const {
@@ -2484,7 +2499,7 @@ void DisplayServerMacOS::window_set_flag(WindowFlags p_flag, bool p_enabled, Win
 				[wd.window_object setFrame:NSMakeRect(frameRect.origin.x, frameRect.origin.y, frameRect.size.width + 1, frameRect.size.height) display:NO];
 				[wd.window_object setFrame:frameRect display:NO];
 			}
-			_update_window_style(wd);
+			_update_window_style(wd, p_window);
 			if (was_visible || [wd.window_object isVisible]) {
 				if ([wd.window_object isMiniaturized]) {
 					return;
@@ -2561,11 +2576,7 @@ bool DisplayServerMacOS::window_get_flag(WindowFlags p_flag, WindowID p_window)
 			return [wd.window_object styleMask] == NSWindowStyleMaskBorderless;
 		} break;
 		case WINDOW_FLAG_ALWAYS_ON_TOP: {
-			if (wd.fullscreen) {
-				return wd.on_top;
-			} else {
-				return [(NSWindow *)wd.window_object level] == NSFloatingWindowLevel;
-			}
+			return wd.on_top;
 		} break;
 		case WINDOW_FLAG_TRANSPARENT: {
 			return wd.layered_window;
diff --git a/platform/macos/godot_window_delegate.mm b/platform/macos/godot_window_delegate.mm
index 7749debfd6..35c053dca5 100644
--- a/platform/macos/godot_window_delegate.mm
+++ b/platform/macos/godot_window_delegate.mm
@@ -194,7 +194,7 @@
 	}
 
 	// Restore on-top state.
-	if (wd.on_top) {
+	if (ds->is_always_on_top_recursive(window_id)) {
 		[wd.window_object setLevel:NSFloatingWindowLevel];
 	}
 
diff --git a/platform/windows/display_server_windows.cpp b/platform/windows/display_server_windows.cpp
index 271a93b122..1ca1b9d194 100644
--- a/platform/windows/display_server_windows.cpp
+++ b/platform/windows/display_server_windows.cpp
@@ -1542,6 +1542,21 @@ DisplayServer::WindowID DisplayServerWindows::create_sub_window(WindowMode p_mod
 	return window_id;
 }
 
+bool DisplayServerWindows::_is_always_on_top_recursive(WindowID p_window) const {
+	ERR_FAIL_COND_V(!windows.has(p_window), false);
+
+	const WindowData &wd = windows[p_window];
+	if (wd.always_on_top) {
+		return true;
+	}
+
+	if (wd.transient_parent != INVALID_WINDOW_ID) {
+		return _is_always_on_top_recursive(wd.transient_parent);
+	}
+
+	return false;
+}
+
 void DisplayServerWindows::show_window(WindowID p_id) {
 	ERR_FAIL_COND(!windows.has(p_id));
 
@@ -1569,7 +1584,7 @@ void DisplayServerWindows::show_window(WindowID p_id) {
 		SetForegroundWindow(wd.hWnd); // Slightly higher priority.
 		SetFocus(wd.hWnd); // Set keyboard focus.
 	}
-	if (wd.always_on_top) {
+	if (_is_always_on_top_recursive(p_id)) {
 		SetWindowPos(wd.hWnd, HWND_TOPMOST, 0, 0, 0, 0, SWP_FRAMECHANGED | SWP_NOMOVE | SWP_NOSIZE | ((wd.no_focus || wd.is_popup) ? SWP_NOACTIVATE : 0));
 	}
 }
@@ -2192,7 +2207,7 @@ void DisplayServerWindows::_update_window_style(WindowID p_window, bool p_repain
 		set_icon(icon);
 	}
 
-	SetWindowPos(wd.hWnd, wd.always_on_top ? HWND_TOPMOST : HWND_NOTOPMOST, 0, 0, 0, 0, SWP_FRAMECHANGED | SWP_NOMOVE | SWP_NOSIZE | ((wd.no_focus || wd.is_popup) ? SWP_NOACTIVATE : 0));
+	SetWindowPos(wd.hWnd, _is_always_on_top_recursive(p_window) ? HWND_TOPMOST : HWND_NOTOPMOST, 0, 0, 0, 0, SWP_FRAMECHANGED | SWP_NOMOVE | SWP_NOSIZE | ((wd.no_focus || wd.is_popup) ? SWP_NOACTIVATE : 0));
 
 	if (p_repaint) {
 		RECT rect;
diff --git a/platform/windows/display_server_windows.h b/platform/windows/display_server_windows.h
index c6b8693a9d..b5d14233dc 100644
--- a/platform/windows/display_server_windows.h
+++ b/platform/windows/display_server_windows.h
@@ -628,6 +628,8 @@ class DisplayServerWindows : public DisplayServer {
 	void _drag_event(WindowID p_window, float p_x, float p_y, int idx);
 	void _touch_event(WindowID p_window, bool p_pressed, float p_x, float p_y, int idx);
 
+	bool _is_always_on_top_recursive(WindowID p_window) const;
+
 	void _update_window_style(WindowID p_window, bool p_repaint = true);
 	void _update_window_mouse_passthrough(WindowID p_window);
 

From 070aeb5688a7343d406ce3e6ce846fedd06b1364 Mon Sep 17 00:00:00 2001
From: Thaddeus Crews <repiteo@outlook.com>
Date: Sun, 8 Dec 2024 15:58:14 -0600
Subject: [PATCH 047/124] SCons: Fix MSVC bypassing disabled warnings

---
 SConstruct                                    | 51 +++++++++----------
 drivers/d3d12/d3d12ma.cpp                     |  2 +-
 methods.py                                    |  7 +--
 modules/csg/SCsub                             | 21 +++++---
 .../gdextension_build/methods.py              | 16 +++---
 .../gdextension_build/methods.py              | 16 +++---
 modules/theora/video_stream_theora.cpp        |  9 ----
 7 files changed, 55 insertions(+), 67 deletions(-)

diff --git a/SConstruct b/SConstruct
index fa324b2aa0..79c341171a 100644
--- a/SConstruct
+++ b/SConstruct
@@ -815,37 +815,36 @@ elif env.msvc:
 
 # Configure compiler warnings
 if env.msvc and not methods.using_clang(env):  # MSVC
-    if env["warnings"] == "no":
-        env.Append(CCFLAGS=["/w"])
-    else:
-        if env["warnings"] == "extra":
-            env.Append(CCFLAGS=["/W4"])
-        elif env["warnings"] == "all":
-            # C4458 is like -Wshadow. Part of /W4 but let's apply it for the default /W3 too.
-            env.Append(CCFLAGS=["/W3", "/w34458"])
-        elif env["warnings"] == "moderate":
-            env.Append(CCFLAGS=["/W2"])
-        # Disable warnings which we don't plan to fix.
+    # Disable warnings which we don't plan to fix.
+    disabled_warnings = [
+        "/wd4100",  # C4100 (unreferenced formal parameter): Doesn't play nice with polymorphism.
+        "/wd4127",  # C4127 (conditional expression is constant)
+        "/wd4201",  # C4201 (non-standard nameless struct/union): Only relevant for C89.
+        "/wd4244",  # C4244 C4245 C4267 (narrowing conversions): Unavoidable at this scale.
+        "/wd4245",
+        "/wd4267",
+        "/wd4305",  # C4305 (truncation): double to float or real_t, too hard to avoid.
+        "/wd4324",  # C4820 (structure was padded due to alignment specifier)
+        "/wd4514",  # C4514 (unreferenced inline function has been removed)
+        "/wd4714",  # C4714 (function marked as __forceinline not inlined)
+        "/wd4820",  # C4820 (padding added after construct)
+    ]
 
-        env.Append(
-            CCFLAGS=[
-                "/wd4100",  # C4100 (unreferenced formal parameter): Doesn't play nice with polymorphism.
-                "/wd4127",  # C4127 (conditional expression is constant)
-                "/wd4201",  # C4201 (non-standard nameless struct/union): Only relevant for C89.
-                "/wd4244",  # C4244 C4245 C4267 (narrowing conversions): Unavoidable at this scale.
-                "/wd4245",
-                "/wd4267",
-                "/wd4305",  # C4305 (truncation): double to float or real_t, too hard to avoid.
-                "/wd4324",  # C4820 (structure was padded due to alignment specifier)
-                "/wd4514",  # C4514 (unreferenced inline function has been removed)
-                "/wd4714",  # C4714 (function marked as __forceinline not inlined)
-                "/wd4820",  # C4820 (padding added after construct)
-            ]
-        )
+    if env["warnings"] == "extra":
+        env.Append(CCFLAGS=["/W4"] + disabled_warnings)
+    elif env["warnings"] == "all":
+        # C4458 is like -Wshadow. Part of /W4 but let's apply it for the default /W3 too.
+        env.Append(CCFLAGS=["/W3", "/w34458"] + disabled_warnings)
+    elif env["warnings"] == "moderate":
+        env.Append(CCFLAGS=["/W2"] + disabled_warnings)
+    else:  # 'no'
+        # C4267 is particularly finicky & needs to be explicitly disabled.
+        env.Append(CCFLAGS=["/w", "/wd4267"])
 
     if env["werror"]:
         env.Append(CCFLAGS=["/WX"])
         env.Append(LINKFLAGS=["/WX"])
+
 else:  # GCC, Clang
     common_warnings = []
 
diff --git a/drivers/d3d12/d3d12ma.cpp b/drivers/d3d12/d3d12ma.cpp
index b7c9eb7ec0..fdec19f92a 100644
--- a/drivers/d3d12/d3d12ma.cpp
+++ b/drivers/d3d12/d3d12ma.cpp
@@ -58,7 +58,7 @@
 #endif
 
 #if defined(_MSC_VER)
-#pragma warning(disable : 4189 4324 4505)
+#pragma warning(disable : 4189 4505)
 #endif
 
 #include "thirdparty/d3d12ma/D3D12MemAlloc.cpp"
diff --git a/methods.py b/methods.py
index be290f8128..6db6b9580c 100644
--- a/methods.py
+++ b/methods.py
@@ -130,9 +130,10 @@ def disable_warnings(self):
     if self.msvc and not using_clang(self):
         # We have to remove existing warning level defines before appending /w,
         # otherwise we get: "warning D9025 : overriding '/W3' with '/w'"
-        self["CCFLAGS"] = [x for x in self["CCFLAGS"] if not (x.startswith("/W") or x.startswith("/w"))]
-        self["CFLAGS"] = [x for x in self["CFLAGS"] if not (x.startswith("/W") or x.startswith("/w"))]
-        self["CXXFLAGS"] = [x for x in self["CXXFLAGS"] if not (x.startswith("/W") or x.startswith("/w"))]
+        WARN_FLAGS = ["/Wall", "/W4", "/W3", "/W2", "/W1", "/W0"]
+        self["CCFLAGS"] = [x for x in self["CCFLAGS"] if x not in WARN_FLAGS]
+        self["CFLAGS"] = [x for x in self["CFLAGS"] if x not in WARN_FLAGS]
+        self["CXXFLAGS"] = [x for x in self["CXXFLAGS"] if x not in WARN_FLAGS]
         self.AppendUnique(CCFLAGS=["/w"])
     else:
         self.AppendUnique(CCFLAGS=["-w"])
diff --git a/modules/csg/SCsub b/modules/csg/SCsub
index a14e999fb8..5b880f155e 100644
--- a/modules/csg/SCsub
+++ b/modules/csg/SCsub
@@ -31,17 +31,22 @@ thirdparty_sources = [
 ]
 
 thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources]
-env_csg.Prepend(
-    CPPPATH=[
-        thirdparty_dir + "include",
-    ]
-)
+env_csg.Prepend(CPPPATH=[thirdparty_dir + "include"])
 env_thirdparty = env_csg.Clone()
 env_thirdparty.disable_warnings()
 env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources)
 env.modules_sources += thirdparty_obj
 
-# Godot's own source files
-env_csg.add_source_files(env.modules_sources, "*.cpp")
+# Godot source files
+
+module_obj = []
+
+env_csg.add_source_files(module_obj, "*.cpp")
+
 if env.editor_build:
-    env_csg.add_source_files(env.modules_sources, "editor/*.cpp")
+    env_csg.add_source_files(module_obj, "editor/*.cpp")
+
+env.modules_sources += module_obj
+
+# Needed to force rebuilding the module files when the thirdparty library is updated.
+env.Depends(module_obj, thirdparty_obj)
diff --git a/modules/text_server_adv/gdextension_build/methods.py b/modules/text_server_adv/gdextension_build/methods.py
index 43bf56abfe..d57acee071 100644
--- a/modules/text_server_adv/gdextension_build/methods.py
+++ b/modules/text_server_adv/gdextension_build/methods.py
@@ -77,17 +77,13 @@ def disable_warnings(self):
     if self["platform"] == "windows" and not self["use_mingw"]:
         # We have to remove existing warning level defines before appending /w,
         # otherwise we get: "warning D9025 : overriding '/W3' with '/w'"
-        warn_flags = ["/Wall", "/W4", "/W3", "/W2", "/W1", "/WX"]
-        self.Append(CCFLAGS=["/w"])
-        self.Append(CFLAGS=["/w"])
-        self.Append(CXXFLAGS=["/w"])
-        self["CCFLAGS"] = [x for x in self["CCFLAGS"] if x not in warn_flags]
-        self["CFLAGS"] = [x for x in self["CFLAGS"] if x not in warn_flags]
-        self["CXXFLAGS"] = [x for x in self["CXXFLAGS"] if x not in warn_flags]
+        WARN_FLAGS = ["/Wall", "/W4", "/W3", "/W2", "/W1", "/W0"]
+        self["CCFLAGS"] = [x for x in self["CCFLAGS"] if x not in WARN_FLAGS]
+        self["CFLAGS"] = [x for x in self["CFLAGS"] if x not in WARN_FLAGS]
+        self["CXXFLAGS"] = [x for x in self["CXXFLAGS"] if x not in WARN_FLAGS]
+        self.AppendUnique(CCFLAGS=["/w"])
     else:
-        self.Append(CCFLAGS=["-w"])
-        self.Append(CFLAGS=["-w"])
-        self.Append(CXXFLAGS=["-w"])
+        self.AppendUnique(CCFLAGS=["-w"])
 
 
 def make_icu_data(target, source, env):
diff --git a/modules/text_server_fb/gdextension_build/methods.py b/modules/text_server_fb/gdextension_build/methods.py
index 43bf56abfe..d57acee071 100644
--- a/modules/text_server_fb/gdextension_build/methods.py
+++ b/modules/text_server_fb/gdextension_build/methods.py
@@ -77,17 +77,13 @@ def disable_warnings(self):
     if self["platform"] == "windows" and not self["use_mingw"]:
         # We have to remove existing warning level defines before appending /w,
         # otherwise we get: "warning D9025 : overriding '/W3' with '/w'"
-        warn_flags = ["/Wall", "/W4", "/W3", "/W2", "/W1", "/WX"]
-        self.Append(CCFLAGS=["/w"])
-        self.Append(CFLAGS=["/w"])
-        self.Append(CXXFLAGS=["/w"])
-        self["CCFLAGS"] = [x for x in self["CCFLAGS"] if x not in warn_flags]
-        self["CFLAGS"] = [x for x in self["CFLAGS"] if x not in warn_flags]
-        self["CXXFLAGS"] = [x for x in self["CXXFLAGS"] if x not in warn_flags]
+        WARN_FLAGS = ["/Wall", "/W4", "/W3", "/W2", "/W1", "/W0"]
+        self["CCFLAGS"] = [x for x in self["CCFLAGS"] if x not in WARN_FLAGS]
+        self["CFLAGS"] = [x for x in self["CFLAGS"] if x not in WARN_FLAGS]
+        self["CXXFLAGS"] = [x for x in self["CXXFLAGS"] if x not in WARN_FLAGS]
+        self.AppendUnique(CCFLAGS=["/w"])
     else:
-        self.Append(CCFLAGS=["-w"])
-        self.Append(CFLAGS=["-w"])
-        self.Append(CXXFLAGS=["-w"])
+        self.AppendUnique(CCFLAGS=["-w"])
 
 
 def make_icu_data(target, source, env):
diff --git a/modules/theora/video_stream_theora.cpp b/modules/theora/video_stream_theora.cpp
index 372885b0b4..11492d189f 100644
--- a/modules/theora/video_stream_theora.cpp
+++ b/modules/theora/video_stream_theora.cpp
@@ -35,17 +35,8 @@
 #include "core/os/os.h"
 #include "scene/resources/image_texture.h"
 
-#ifdef _MSC_VER
-#pragma warning(push)
-#pragma warning(disable : 4127)
-#endif
-
 #include "thirdparty/misc/yuv2rgb.h"
 
-#ifdef _MSC_VER
-#pragma warning(pop)
-#endif
-
 int VideoStreamPlaybackTheora::buffer_data() {
 	char *buffer = ogg_sync_buffer(&oy, 4096);
 

From be1dce1071dac9b4f1fc9279a154773940f0a2ef Mon Sep 17 00:00:00 2001
From: Dario <dariosamo@gmail.com>
Date: Mon, 9 Dec 2024 15:07:52 -0300
Subject: [PATCH 048/124] Improve thread-safety of pipeline hash map.

---
 .../render_forward_clustered.cpp              |  4 +-
 .../forward_mobile/render_forward_mobile.cpp  |  4 +-
 .../renderer_rd/pipeline_hash_map_rd.h        | 49 ++++++++++---------
 3 files changed, 30 insertions(+), 27 deletions(-)

diff --git a/servers/rendering/renderer_rd/forward_clustered/render_forward_clustered.cpp b/servers/rendering/renderer_rd/forward_clustered/render_forward_clustered.cpp
index d569ce1101..ef48fdfa6d 100644
--- a/servers/rendering/renderer_rd/forward_clustered/render_forward_clustered.cpp
+++ b/servers/rendering/renderer_rd/forward_clustered/render_forward_clustered.cpp
@@ -4675,10 +4675,10 @@ void RenderForwardClustered::mesh_generate_pipelines(RID p_mesh, bool p_backgrou
 		_mesh_compile_pipelines_for_surface(surface, global_pipeline_data_required, RS::PIPELINE_SOURCE_MESH, &pipeline_pairs);
 	}
 
-	// Try to retrieve all the pipeline pairs that were compiled. This will force the loader to wait on all ubershader pipelines to be ready.
+	// Wait for all the pipelines that were compiled. This will force the loader to wait on all ubershader pipelines to be ready.
 	if (!p_background_compilation && !pipeline_pairs.is_empty()) {
 		for (ShaderPipelinePair pair : pipeline_pairs) {
-			pair.first->pipeline_hash_map.get_pipeline(pair.second, pair.second.hash(), true, RS::PIPELINE_SOURCE_MESH);
+			pair.first->pipeline_hash_map.wait_for_pipeline(pair.second.hash());
 		}
 	}
 }
diff --git a/servers/rendering/renderer_rd/forward_mobile/render_forward_mobile.cpp b/servers/rendering/renderer_rd/forward_mobile/render_forward_mobile.cpp
index e2d6796566..bb85e9c9be 100644
--- a/servers/rendering/renderer_rd/forward_mobile/render_forward_mobile.cpp
+++ b/servers/rendering/renderer_rd/forward_mobile/render_forward_mobile.cpp
@@ -327,10 +327,10 @@ void RenderForwardMobile::mesh_generate_pipelines(RID p_mesh, bool p_background_
 		_mesh_compile_pipelines_for_surface(surface, global_pipeline_data_required, RS::PIPELINE_SOURCE_MESH, &pipeline_pairs);
 	}
 
-	// Try to retrieve all the pipeline pairs that were compiled. This will force the loader to wait on all ubershader pipelines to be ready.
+	// Try to wait for all the pipelines that were compiled. This will force the loader to wait on all ubershader pipelines to be ready.
 	if (!p_background_compilation && !pipeline_pairs.is_empty()) {
 		for (ShaderPipelinePair pair : pipeline_pairs) {
-			pair.first->pipeline_hash_map.get_pipeline(pair.second, pair.second.hash(), true, RS::PIPELINE_SOURCE_MESH);
+			pair.first->pipeline_hash_map.wait_for_pipeline(pair.second.hash());
 		}
 	}
 }
diff --git a/servers/rendering/renderer_rd/pipeline_hash_map_rd.h b/servers/rendering/renderer_rd/pipeline_hash_map_rd.h
index b76f8ae5e6..cdd06accf2 100644
--- a/servers/rendering/renderer_rd/pipeline_hash_map_rd.h
+++ b/servers/rendering/renderer_rd/pipeline_hash_map_rd.h
@@ -46,6 +46,7 @@ private:
 	RBMap<uint32_t, RID> hash_map;
 	LocalVector<Pair<uint32_t, RID>> compiled_queue;
 	Mutex compiled_queue_mutex;
+	RBSet<uint32_t> compilation_set;
 	HashMap<uint32_t, WorkerThreadPool::TaskID> compilation_tasks;
 	Mutex local_mutex;
 
@@ -76,7 +77,7 @@ private:
 		return !hashes_added.is_empty();
 	}
 
-	void _wait_for_compilation() {
+	void _wait_for_all_pipelines() {
 		MutexLock local_lock(local_mutex);
 		for (KeyValue<uint32_t, WorkerThreadPool::TaskID> key_value : compilation_tasks) {
 			WorkerThreadPool::get_singleton()->wait_for_task_completion(key_value.value);
@@ -91,22 +92,17 @@ public:
 	}
 
 	// Start compilation of a pipeline ahead of time in the background. Returns true if the compilation was started, false if it wasn't required. Source is only used for collecting statistics.
-	bool compile_pipeline(const Key &p_key, uint32_t p_key_hash, RS::PipelineSource p_source) {
+	void compile_pipeline(const Key &p_key, uint32_t p_key_hash, RS::PipelineSource p_source) {
 		DEV_ASSERT((creation_object != nullptr) && (creation_function != nullptr) && "Creation object and function was not set before attempting to compile a pipeline.");
 
-		// Check if the pipeline was already compiled.
-		compiled_queue_mutex.lock();
-		bool already_exists = hash_map.has(p_key_hash);
-		compiled_queue_mutex.unlock();
-		if (already_exists) {
-			return false;
+		MutexLock local_lock(local_mutex);
+		if (compilation_set.has(p_key_hash)) {
+			// Check if the pipeline was already submitted.
+			return;
 		}
 
-		// Check if the pipeline is currently being compiled.
-		MutexLock local_lock(local_mutex);
-		if (compilation_tasks.has(p_key_hash)) {
-			return false;
-		}
+		// Record the pipeline as submitted, a task can't be started for it again.
+		compilation_set.insert(p_key_hash);
 
 		if (compilations_mutex != nullptr) {
 			MutexLock compilations_lock(*compilations_mutex);
@@ -139,8 +135,21 @@ public:
 		// Queue a background compilation task.
 		WorkerThreadPool::TaskID task_id = WorkerThreadPool::get_singleton()->add_template_task(creation_object, creation_function, p_key, false, "PipelineCompilation");
 		compilation_tasks.insert(p_key_hash, task_id);
+	}
 
-		return true;
+	void wait_for_pipeline(uint32_t p_key_hash) {
+		MutexLock local_lock(local_mutex);
+		if (!compilation_set.has(p_key_hash)) {
+			// The pipeline was never submitted, we can't wait for it.
+			return;
+		}
+
+		HashMap<uint32_t, WorkerThreadPool::TaskID>::Iterator task_it = compilation_tasks.find(p_key_hash);
+		if (task_it != compilation_tasks.end()) {
+			// Wait for and remove the compilation task if it exists.
+			WorkerThreadPool::get_singleton()->wait_for_task_completion(task_it->value);
+			compilation_tasks.remove(task_it);
+		}
 	}
 
 	// Retrieve a pipeline. It'll return an empty pipeline if it's not available yet, but it'll be guaranteed to succeed if 'wait for compilation' is true and stall as necessary. Source is just an optional number to aid debugging.
@@ -155,18 +164,11 @@ public:
 		}
 
 		if (e == nullptr) {
-			// Lock access to the compilation maps.
-			MutexLock local_lock(local_mutex);
-
 			// Request compilation. The method will ignore the request if it's already being compiled.
 			compile_pipeline(p_key, p_key_hash, p_source);
 
 			if (p_wait_for_compilation) {
-				if (compilation_tasks.has(p_key_hash)) {
-					// If a background compilation task was used, wait for it.
-					WorkerThreadPool::get_singleton()->wait_for_task_completion(compilation_tasks[p_key_hash]);
-				}
-
+				wait_for_pipeline(p_key_hash);
 				_add_new_pipelines_to_map();
 
 				e = hash_map.find(p_key_hash);
@@ -187,7 +189,7 @@ public:
 
 	// Delete all cached pipelines. Can stall if background compilation is in progress.
 	void clear_pipelines() {
-		_wait_for_compilation();
+		_wait_for_all_pipelines();
 		_add_new_pipelines_to_map();
 
 		for (KeyValue<uint32_t, RID> entry : hash_map) {
@@ -195,6 +197,7 @@ public:
 		}
 
 		hash_map.clear();
+		compilation_set.clear();
 	}
 
 	// Set the external pipeline compilations array to increase the counters on every time a pipeline is compiled.

From b397add4566d6172010be39a102376c985acda3f Mon Sep 17 00:00:00 2001
From: Michael Alexsander <michaelalexsander@protonmail.com>
Date: Wed, 30 Oct 2024 12:40:15 -0300
Subject: [PATCH 049/124] Fix certain editor plugins not showing when they
 should

---
 editor/editor_node.cpp | 11 ++++++++++-
 1 file changed, 10 insertions(+), 1 deletion(-)

diff --git a/editor/editor_node.cpp b/editor/editor_node.cpp
index 0dd05d4e33..a051b87169 100644
--- a/editor/editor_node.cpp
+++ b/editor/editor_node.cpp
@@ -2318,16 +2318,20 @@ void EditorNode::edit_item(Object *p_object, Object *p_editing_owner) {
 		active_plugins[owner_id].erase(plugin);
 	}
 
+	LocalVector<EditorPlugin *> to_over_edit;
+
 	// Send the edited object to the plugins.
 	for (EditorPlugin *plugin : available_plugins) {
 		if (active_plugins[owner_id].has(plugin)) {
-			// Plugin was already active, just change the object.
+			// Plugin was already active, just change the object and ensure it's visible.
+			plugin->make_visible(true);
 			plugin->edit(p_object);
 			continue;
 		}
 
 		if (active_plugins.has(plugin->get_instance_id())) {
 			// Plugin is already active, but as self-owning, so it needs a separate check.
+			plugin->make_visible(true);
 			plugin->edit(p_object);
 			continue;
 		}
@@ -2346,6 +2350,11 @@ void EditorNode::edit_item(Object *p_object, Object *p_editing_owner) {
 
 		// Activate previously inactive plugin and edit the object.
 		active_plugins[owner_id].insert(plugin);
+		// TODO: Call the function directly once a proper priority system is implemented.
+		to_over_edit.push_back(plugin);
+	}
+
+	for (EditorPlugin *plugin : to_over_edit) {
 		_plugin_over_edit(plugin, p_object);
 	}
 }

From a3f48f70473194ce5d04537a18ef06a63094d798 Mon Sep 17 00:00:00 2001
From: Lukas Tenbrink <lukas.tenbrink@gmail.com>
Date: Sat, 7 Dec 2024 02:28:43 +0100
Subject: [PATCH 050/124] Optimize String construction from statically known
 strings allowing `strlen` to be evaluated at compile time, where possible.

---
 core/string/ustring.cpp | 158 +++-----------------------------
 core/string/ustring.h   | 198 +++++++++++++++++++++++++++++++++-------
 2 files changed, 177 insertions(+), 179 deletions(-)

diff --git a/core/string/ustring.cpp b/core/string/ustring.cpp
index 805ddd4bc3..e4d2cb0cba 100644
--- a/core/string/ustring.cpp
+++ b/core/string/ustring.cpp
@@ -65,15 +65,6 @@ const char16_t Char16String::_null = 0;
 const char32_t String::_null = 0;
 const char32_t String::_replacement_char = 0xfffd;
 
-// strlen equivalent function for char32_t * arguments.
-_FORCE_INLINE_ size_t strlen(const char32_t *p_str) {
-	const char32_t *ptr = p_str;
-	while (*ptr != 0) {
-		++ptr;
-	}
-	return ptr - p_str;
-}
-
 bool select_word(const String &p_s, int p_col, int &r_beg, int &r_end) {
 	const String &s = p_s;
 	int beg = CLAMP(p_col, 0, s.length());
@@ -313,80 +304,32 @@ Error String::parse_url(String &r_scheme, String &r_host, int &r_port, String &r
 	return OK;
 }
 
-void String::copy_from(const char *p_cstr) {
-	// copy Latin-1 encoded c-string directly
-	if (!p_cstr) {
+void String::copy_from(const StrRange<char> &p_cstr) {
+	if (p_cstr.len == 0) {
 		resize(0);
 		return;
 	}
 
-	const size_t len = strlen(p_cstr);
+	resize(p_cstr.len + 1); // include 0
 
-	if (len == 0) {
-		resize(0);
-		return;
-	}
-
-	resize(len + 1); // include 0
-
-	const char *end = p_cstr + len;
+	const char *src = p_cstr.c_str;
+	const char *end = src + p_cstr.len;
 	char32_t *dst = ptrw();
 
-	for (; p_cstr < end; ++p_cstr, ++dst) {
+	for (; src < end; ++src, ++dst) {
 		// If char is int8_t, a set sign bit will be reinterpreted as 256 - val implicitly.
-		*dst = static_cast<uint8_t>(*p_cstr);
+		*dst = static_cast<uint8_t>(*src);
 	}
 	*dst = 0;
 }
 
-void String::copy_from(const char *p_cstr, const int p_clip_to) {
-	// copy Latin-1 encoded c-string directly
-	if (!p_cstr) {
+void String::copy_from(const StrRange<char32_t> &p_cstr) {
+	if (p_cstr.len == 0) {
 		resize(0);
 		return;
 	}
 
-	int len = 0;
-	const char *ptr = p_cstr;
-	while ((p_clip_to < 0 || len < p_clip_to) && *(ptr++) != 0) {
-		len++;
-	}
-
-	if (len == 0) {
-		resize(0);
-		return;
-	}
-
-	resize(len + 1); // include 0
-
-	const char *end = p_cstr + len;
-	char32_t *dst = ptrw();
-
-	for (; p_cstr < end; ++p_cstr, ++dst) {
-		// If char is int8_t, a set sign bit will be reinterpreted as 256 - val implicitly.
-		*dst = static_cast<uint8_t>(*p_cstr);
-	}
-	*dst = 0;
-}
-
-void String::copy_from(const wchar_t *p_cstr) {
-#ifdef WINDOWS_ENABLED
-	// wchar_t is 16-bit, parse as UTF-16
-	parse_utf16((const char16_t *)p_cstr);
-#else
-	// wchar_t is 32-bit, copy directly
-	copy_from((const char32_t *)p_cstr);
-#endif
-}
-
-void String::copy_from(const wchar_t *p_cstr, const int p_clip_to) {
-#ifdef WINDOWS_ENABLED
-	// wchar_t is 16-bit, parse as UTF-16
-	parse_utf16((const char16_t *)p_cstr, p_clip_to);
-#else
-	// wchar_t is 32-bit, copy directly
-	copy_from((const char32_t *)p_cstr, p_clip_to);
-#endif
+	copy_from_unchecked(p_cstr.c_str, p_cstr.len);
 }
 
 void String::copy_from(const char32_t &p_char) {
@@ -412,42 +355,6 @@ void String::copy_from(const char32_t &p_char) {
 	dst[1] = 0;
 }
 
-void String::copy_from(const char32_t *p_cstr) {
-	if (!p_cstr) {
-		resize(0);
-		return;
-	}
-
-	const int len = strlen(p_cstr);
-
-	if (len == 0) {
-		resize(0);
-		return;
-	}
-
-	copy_from_unchecked(p_cstr, len);
-}
-
-void String::copy_from(const char32_t *p_cstr, const int p_clip_to) {
-	if (!p_cstr) {
-		resize(0);
-		return;
-	}
-
-	int len = 0;
-	const char32_t *ptr = p_cstr;
-	while ((p_clip_to < 0 || len < p_clip_to) && *(ptr++) != 0) {
-		len++;
-	}
-
-	if (len == 0) {
-		resize(0);
-		return;
-	}
-
-	copy_from_unchecked(p_cstr, len);
-}
-
 // assumes the following have already been validated:
 // p_char != nullptr
 // p_length > 0
@@ -475,18 +382,6 @@ void String::copy_from_unchecked(const char32_t *p_char, const int p_length) {
 	*dst = 0;
 }
 
-void String::operator=(const char *p_str) {
-	copy_from(p_str);
-}
-
-void String::operator=(const char32_t *p_str) {
-	copy_from(p_str);
-}
-
-void String::operator=(const wchar_t *p_str) {
-	copy_from(p_str);
-}
-
 String String::operator+(const String &p_str) const {
 	String res = *this;
 	res += p_str;
@@ -697,7 +592,7 @@ bool String::operator==(const String &p_str) const {
 	return true;
 }
 
-bool String::operator==(const StrRange &p_str_range) const {
+bool String::operator==(const StrRange<char32_t> &p_str_range) const {
 	int len = p_str_range.len;
 
 	if (length() != len) {
@@ -2512,37 +2407,6 @@ Char16String String::utf16() const {
 	return utf16s;
 }
 
-String::String(const char *p_str) {
-	copy_from(p_str);
-}
-
-String::String(const wchar_t *p_str) {
-	copy_from(p_str);
-}
-
-String::String(const char32_t *p_str) {
-	copy_from(p_str);
-}
-
-String::String(const char *p_str, int p_clip_to_len) {
-	copy_from(p_str, p_clip_to_len);
-}
-
-String::String(const wchar_t *p_str, int p_clip_to_len) {
-	copy_from(p_str, p_clip_to_len);
-}
-
-String::String(const char32_t *p_str, int p_clip_to_len) {
-	copy_from(p_str, p_clip_to_len);
-}
-
-String::String(const StrRange &p_range) {
-	if (!p_range.c_str) {
-		return;
-	}
-	copy_from(p_range.c_str, p_range.len);
-}
-
 int64_t String::hex_to_int() const {
 	int len = length();
 	if (len == 0) {
diff --git a/core/string/ustring.h b/core/string/ustring.h
index 3849bf3ce3..ac4ace2249 100644
--- a/core/string/ustring.h
+++ b/core/string/ustring.h
@@ -39,6 +39,89 @@
 #include "core/typedefs.h"
 #include "core/variant/array.h"
 
+/*************************************************************************/
+/*  Utility Functions                                                    */
+/*************************************************************************/
+
+// Not defined by std.
+// strlen equivalent function for char16_t * arguments.
+constexpr size_t strlen(const char16_t *p_str) {
+	const char16_t *ptr = p_str;
+	while (*ptr != 0) {
+		++ptr;
+	}
+	return ptr - p_str;
+}
+
+// strlen equivalent function for char32_t * arguments.
+constexpr size_t strlen(const char32_t *p_str) {
+	const char32_t *ptr = p_str;
+	while (*ptr != 0) {
+		++ptr;
+	}
+	return ptr - p_str;
+}
+
+// strlen equivalent function for wchar_t * arguments; depends on the platform.
+constexpr size_t strlen(const wchar_t *str) {
+	// Use static_cast twice because reinterpret_cast is not allowed in constexpr
+#ifdef WINDOWS_ENABLED
+	// wchar_t is 16-bit
+	return strlen(static_cast<const char16_t *>(static_cast<const void *>(str)));
+#else
+	// wchar_t is 32-bit
+	return strlen(static_cast<const char32_t *>(static_cast<const void *>(str)));
+#endif
+}
+
+constexpr size_t _strlen_clipped(const char *p_str, int p_clip_to_len) {
+	if (p_clip_to_len < 0) {
+		return strlen(p_str);
+	}
+
+	int len = 0;
+	while (len < p_clip_to_len && *(p_str++) != 0) {
+		len++;
+	}
+	return len;
+}
+
+constexpr size_t _strlen_clipped(const char32_t *p_str, int p_clip_to_len) {
+	if (p_clip_to_len < 0) {
+		return strlen(p_str);
+	}
+
+	int len = 0;
+	while (len < p_clip_to_len && *(p_str++) != 0) {
+		len++;
+	}
+	return len;
+}
+
+/*************************************************************************/
+/*  StrRange                                                             */
+/*************************************************************************/
+
+template <typename Element>
+struct StrRange {
+	const Element *c_str;
+	size_t len;
+
+	explicit StrRange(const std::nullptr_t p_cstring) :
+			c_str(nullptr), len(0) {}
+
+	explicit StrRange(const Element *p_cstring, const size_t p_len) :
+			c_str(p_cstring), len(p_len) {}
+
+	template <size_t len>
+	explicit StrRange(const Element (&p_cstring)[len]) :
+			c_str(p_cstring), len(strlen(p_cstring)) {}
+
+	static StrRange from_c_str(const Element *p_cstring) {
+		return StrRange(p_cstring, p_cstring ? strlen(p_cstring) : 0);
+	}
+};
+
 /*************************************************************************/
 /*  CharProxy                                                            */
 /*************************************************************************/
@@ -119,6 +202,7 @@ public:
 	int length() const { return size() ? size() - 1 : 0; }
 	const char16_t *get_data() const;
 	operator const char16_t *() const { return get_data(); }
+	explicit operator StrRange<char16_t>() const { return StrRange(get_data(), length()); }
 
 protected:
 	void copy_from(const char16_t *p_cstr);
@@ -161,6 +245,7 @@ public:
 	int length() const { return size() ? size() - 1 : 0; }
 	const char *get_data() const;
 	operator const char *() const { return get_data(); }
+	explicit operator StrRange<char>() const { return StrRange(get_data(), length()); }
 
 protected:
 	void copy_from(const char *p_cstr);
@@ -170,31 +255,59 @@ protected:
 /*  String                                                               */
 /*************************************************************************/
 
-struct StrRange {
-	const char32_t *c_str;
-	int len;
-
-	StrRange(const char32_t *p_c_str = nullptr, int p_len = 0) {
-		c_str = p_c_str;
-		len = p_len;
-	}
-};
-
 class String {
 	CowData<char32_t> _cowdata;
 	static const char32_t _null;
 	static const char32_t _replacement_char;
 
-	void copy_from(const char *p_cstr);
-	void copy_from(const char *p_cstr, const int p_clip_to);
-	void copy_from(const wchar_t *p_cstr);
-	void copy_from(const wchar_t *p_cstr, const int p_clip_to);
-	void copy_from(const char32_t *p_cstr);
-	void copy_from(const char32_t *p_cstr, const int p_clip_to);
-
+	// Known-length copy.
+	void copy_from(const StrRange<char> &p_cstr);
+	void copy_from(const StrRange<char32_t> &p_cstr);
 	void copy_from(const char32_t &p_char);
+	void copy_from_unchecked(const char32_t *p_char, int p_length);
 
-	void copy_from_unchecked(const char32_t *p_char, const int p_length);
+	// NULL-terminated c string copy - automatically parse the string to find the length.
+	void copy_from(const char *p_cstr) {
+		copy_from(StrRange<char>::from_c_str(p_cstr));
+	}
+	void copy_from(const char *p_cstr, int p_clip_to) {
+		copy_from(StrRange(p_cstr, p_cstr ? _strlen_clipped(p_cstr, p_clip_to) : 0));
+	}
+	void copy_from(const char32_t *p_cstr) {
+		copy_from(StrRange<char32_t>::from_c_str(p_cstr));
+	}
+	void copy_from(const char32_t *p_cstr, int p_clip_to) {
+		copy_from(StrRange(p_cstr, p_cstr ? _strlen_clipped(p_cstr, p_clip_to) : 0));
+	}
+
+	// wchar_t copy_from depends on the platform.
+	void copy_from(const StrRange<wchar_t> &p_cstr) {
+#ifdef WINDOWS_ENABLED
+		// wchar_t is 16-bit, parse as UTF-16
+		parse_utf16((const char16_t *)p_cstr.c_str, p_cstr.len);
+#else
+		// wchar_t is 32-bit, copy directly
+		copy_from((StrRange<char32_t> &)p_cstr);
+#endif
+	}
+	void copy_from(const wchar_t *p_cstr) {
+#ifdef WINDOWS_ENABLED
+		// wchar_t is 16-bit, parse as UTF-16
+		parse_utf16((const char16_t *)p_cstr);
+#else
+		// wchar_t is 32-bit, copy directly
+		copy_from((const char32_t *)p_cstr);
+#endif
+	}
+	void copy_from(const wchar_t *p_cstr, int p_clip_to) {
+#ifdef WINDOWS_ENABLED
+		// wchar_t is 16-bit, parse as UTF-16
+		parse_utf16((const char16_t *)p_cstr, p_clip_to);
+#else
+		// wchar_t is 32-bit, copy directly
+		copy_from((const char32_t *)p_cstr, p_clip_to);
+#endif
+	}
 
 	bool _base_is_subsequence_of(const String &p_string, bool case_insensitive) const;
 	int _count(const String &p_string, int p_from, int p_to, bool p_case_insensitive) const;
@@ -227,6 +340,8 @@ public:
 	}
 	_FORCE_INLINE_ CharProxy<char32_t> operator[](int p_index) { return CharProxy<char32_t>(p_index, _cowdata); }
 
+	/* Compatibility Operators */
+
 	bool operator==(const String &p_str) const;
 	bool operator!=(const String &p_str) const;
 	String operator+(const String &p_str) const;
@@ -238,16 +353,10 @@ public:
 	String &operator+=(const wchar_t *p_str);
 	String &operator+=(const char32_t *p_str);
 
-	/* Compatibility Operators */
-
-	void operator=(const char *p_str);
-	void operator=(const wchar_t *p_str);
-	void operator=(const char32_t *p_str);
-
 	bool operator==(const char *p_str) const;
 	bool operator==(const wchar_t *p_str) const;
 	bool operator==(const char32_t *p_str) const;
-	bool operator==(const StrRange &p_str_range) const;
+	bool operator==(const StrRange<char32_t> &p_str_range) const;
 
 	bool operator!=(const char *p_str) const;
 	bool operator!=(const wchar_t *p_str) const;
@@ -493,13 +602,38 @@ public:
 	Vector<uint8_t> to_utf32_buffer() const;
 	Vector<uint8_t> to_wchar_buffer() const;
 
-	String(const char *p_str);
-	String(const wchar_t *p_str);
-	String(const char32_t *p_str);
-	String(const char *p_str, int p_clip_to_len);
-	String(const wchar_t *p_str, int p_clip_to_len);
-	String(const char32_t *p_str, int p_clip_to_len);
-	String(const StrRange &p_range);
+	// Constructors for NULL terminated C strings.
+	String(const char *p_cstr) {
+		copy_from(p_cstr);
+	}
+	String(const wchar_t *p_cstr) {
+		copy_from(p_cstr);
+	}
+	String(const char32_t *p_cstr) {
+		copy_from(p_cstr);
+	}
+	String(const char *p_cstr, int p_clip_to_len) {
+		copy_from(p_cstr, p_clip_to_len);
+	}
+	String(const wchar_t *p_cstr, int p_clip_to_len) {
+		copy_from(p_cstr, p_clip_to_len);
+	}
+	String(const char32_t *p_cstr, int p_clip_to_len) {
+		copy_from(p_cstr, p_clip_to_len);
+	}
+
+	// Copy assignment for NULL terminated C strings.
+	void operator=(const char *p_cstr) {
+		copy_from(p_cstr);
+	}
+	void operator=(const wchar_t *p_cstr) {
+		copy_from(p_cstr);
+	}
+	void operator=(const char32_t *p_cstr) {
+		copy_from(p_cstr);
+	}
+
+	explicit operator StrRange<char32_t>() const { return StrRange(get_data(), length()); }
 };
 
 bool operator==(const char *p_chr, const String &p_str);

From 6095a37d64f0cc85ebf0c5c3d30be0cc4411639a Mon Sep 17 00:00:00 2001
From: Hugo Locurcio <hugo.locurcio@hugo.pro>
Date: Sun, 8 Dec 2024 15:34:01 +0100
Subject: [PATCH 051/124] Tweak unknown annotation GDScript error for
 `@deprecated`/`@experimental`/`@tutorial`

These annotations don't exist at a source level, so the error messages
point to the documentation comment syntax.
---
 modules/gdscript/gdscript_parser.cpp                   | 10 +++++++++-
 .../scripts/parser/errors/annotation_deprecated.gd     |  5 +++++
 .../scripts/parser/errors/annotation_deprecated.out    |  2 ++
 .../scripts/parser/errors/annotation_experimental.gd   |  6 ++++++
 .../scripts/parser/errors/annotation_experimental.out  |  2 ++
 .../tests/scripts/parser/errors/annotation_tutorial.gd |  5 +++++
 .../scripts/parser/errors/annotation_tutorial.out      |  2 ++
 7 files changed, 31 insertions(+), 1 deletion(-)
 create mode 100644 modules/gdscript/tests/scripts/parser/errors/annotation_deprecated.gd
 create mode 100644 modules/gdscript/tests/scripts/parser/errors/annotation_deprecated.out
 create mode 100644 modules/gdscript/tests/scripts/parser/errors/annotation_experimental.gd
 create mode 100644 modules/gdscript/tests/scripts/parser/errors/annotation_experimental.out
 create mode 100644 modules/gdscript/tests/scripts/parser/errors/annotation_tutorial.gd
 create mode 100644 modules/gdscript/tests/scripts/parser/errors/annotation_tutorial.out

diff --git a/modules/gdscript/gdscript_parser.cpp b/modules/gdscript/gdscript_parser.cpp
index 12e71004db..b636dd0828 100644
--- a/modules/gdscript/gdscript_parser.cpp
+++ b/modules/gdscript/gdscript_parser.cpp
@@ -1624,7 +1624,15 @@ GDScriptParser::AnnotationNode *GDScriptParser::parse_annotation(uint32_t p_vali
 	bool valid = true;
 
 	if (!valid_annotations.has(annotation->name)) {
-		push_error(vformat(R"(Unrecognized annotation: "%s".)", annotation->name));
+		if (annotation->name == "@deprecated") {
+			push_error(R"("@deprecated" annotation does not exist. Use "## @deprecated: Reason here." instead.)");
+		} else if (annotation->name == "@experimental") {
+			push_error(R"("@experimental" annotation does not exist. Use "## @experimental: Reason here." instead.)");
+		} else if (annotation->name == "@tutorial") {
+			push_error(R"("@tutorial" annotation does not exist. Use "## @tutorial(Title): https://example.com" instead.)");
+		} else {
+			push_error(vformat(R"(Unrecognized annotation: "%s".)", annotation->name));
+		}
 		valid = false;
 	}
 
diff --git a/modules/gdscript/tests/scripts/parser/errors/annotation_deprecated.gd b/modules/gdscript/tests/scripts/parser/errors/annotation_deprecated.gd
new file mode 100644
index 0000000000..95569c48de
--- /dev/null
+++ b/modules/gdscript/tests/scripts/parser/errors/annotation_deprecated.gd
@@ -0,0 +1,5 @@
+# This annotation should be used within a documentation comment instead:
+# ## @deprecated: Reason here.
+
+@deprecated
+var some_variable = "value"
diff --git a/modules/gdscript/tests/scripts/parser/errors/annotation_deprecated.out b/modules/gdscript/tests/scripts/parser/errors/annotation_deprecated.out
new file mode 100644
index 0000000000..a996088902
--- /dev/null
+++ b/modules/gdscript/tests/scripts/parser/errors/annotation_deprecated.out
@@ -0,0 +1,2 @@
+GDTEST_PARSER_ERROR
+"@deprecated" annotation does not exist. Use "## @deprecated: Reason here." instead.
diff --git a/modules/gdscript/tests/scripts/parser/errors/annotation_experimental.gd b/modules/gdscript/tests/scripts/parser/errors/annotation_experimental.gd
new file mode 100644
index 0000000000..ce00639de6
--- /dev/null
+++ b/modules/gdscript/tests/scripts/parser/errors/annotation_experimental.gd
@@ -0,0 +1,6 @@
+# This annotation should be used within a documentation comment instead:
+# ## @experimental: Reason here.
+
+@experimental("This function isn't implemented yet.")
+func say_hello():
+    pass
diff --git a/modules/gdscript/tests/scripts/parser/errors/annotation_experimental.out b/modules/gdscript/tests/scripts/parser/errors/annotation_experimental.out
new file mode 100644
index 0000000000..d7e1c2eb2f
--- /dev/null
+++ b/modules/gdscript/tests/scripts/parser/errors/annotation_experimental.out
@@ -0,0 +1,2 @@
+GDTEST_PARSER_ERROR
+"@experimental" annotation does not exist. Use "## @experimental: Reason here." instead.
diff --git a/modules/gdscript/tests/scripts/parser/errors/annotation_tutorial.gd b/modules/gdscript/tests/scripts/parser/errors/annotation_tutorial.gd
new file mode 100644
index 0000000000..ccd995bb50
--- /dev/null
+++ b/modules/gdscript/tests/scripts/parser/errors/annotation_tutorial.gd
@@ -0,0 +1,5 @@
+# This annotation should be used within a documentation comment instead:
+# ## @tutorial(Title): https://example.com
+
+@tutorial("https://example.com")
+const SOME_CONSTANT = "value"
diff --git a/modules/gdscript/tests/scripts/parser/errors/annotation_tutorial.out b/modules/gdscript/tests/scripts/parser/errors/annotation_tutorial.out
new file mode 100644
index 0000000000..7d8be3567b
--- /dev/null
+++ b/modules/gdscript/tests/scripts/parser/errors/annotation_tutorial.out
@@ -0,0 +1,2 @@
+GDTEST_PARSER_ERROR
+"@tutorial" annotation does not exist. Use "## @tutorial(Title): https://example.com" instead.

From 56645f466cc227764ba9d3ef421aeb8df99f66a8 Mon Sep 17 00:00:00 2001
From: Dario <dariosamo@gmail.com>
Date: Mon, 9 Dec 2024 19:29:59 -0300
Subject: [PATCH 052/124] Fix incorrect return value on partial coverage
 detection.

---
 servers/rendering/rendering_device_graph.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/servers/rendering/rendering_device_graph.cpp b/servers/rendering/rendering_device_graph.cpp
index b2779af620..4221768f6e 100644
--- a/servers/rendering/rendering_device_graph.cpp
+++ b/servers/rendering/rendering_device_graph.cpp
@@ -162,7 +162,7 @@ bool RenderingDeviceGraph::_check_command_intersection(ResourceTracker *p_resour
 bool RenderingDeviceGraph::_check_command_partial_coverage(ResourceTracker *p_resource_tracker, int32_t p_command_index) const {
 	if (p_resource_tracker->usage != RESOURCE_USAGE_ATTACHMENT_COLOR_READ_WRITE && p_resource_tracker->usage != RESOURCE_USAGE_ATTACHMENT_DEPTH_STENCIL_READ_WRITE) {
 		// We don't check for partial coverage in usages that aren't attachment writes.
-		return true;
+		return false;
 	}
 
 	const uint32_t command_data_offset = command_data_offsets[p_command_index];

From 65c28ed31d02eb2ab5a0773aa58b5f7a80240bbe Mon Sep 17 00:00:00 2001
From: Riteo <riteo@posteo.net>
Date: Mon, 2 Dec 2024 20:08:01 +0100
Subject: [PATCH 053/124] Wayland: Add support for xdg-foreign-unstable-v2

The v1 version is deprecated and bound to be removed in the future from
all compositors. This patch adds a v1/v2 designator to everything
related to the protocol and prefers the v2 protocol if both are
available.

Additionally, renames the event handler to follow the Wayland interface
name, for consistency with the rest of the codebase.
---
 platform/linuxbsd/wayland/SCsub               |  17 +-
 platform/linuxbsd/wayland/wayland_thread.cpp  |  60 ++++--
 platform/linuxbsd/wayland/wayland_thread.h    |  33 ++-
 .../xdg-foreign/xdg-foreign-unstable-v2.xml   | 200 ++++++++++++++++++
 4 files changed, 287 insertions(+), 23 deletions(-)
 create mode 100644 thirdparty/wayland-protocols/unstable/xdg-foreign/xdg-foreign-unstable-v2.xml

diff --git a/platform/linuxbsd/wayland/SCsub b/platform/linuxbsd/wayland/SCsub
index f3a554a58d..c5d8c00c6d 100644
--- a/platform/linuxbsd/wayland/SCsub
+++ b/platform/linuxbsd/wayland/SCsub
@@ -163,15 +163,25 @@ env.WAYLAND_API_CODE(
 )
 
 env.WAYLAND_API_HEADER(
-    target="protocol/xdg_foreign.gen.h",
+    target="protocol/xdg_foreign_v1.gen.h",
     source="#thirdparty/wayland-protocols/unstable/xdg-foreign/xdg-foreign-unstable-v1.xml",
 )
 
 env.WAYLAND_API_CODE(
-    target="protocol/xdg_foreign.gen.c",
+    target="protocol/xdg_foreign_v1.gen.c",
     source="#thirdparty/wayland-protocols/unstable/xdg-foreign/xdg-foreign-unstable-v1.xml",
 )
 
+env.WAYLAND_API_HEADER(
+    target="protocol/xdg_foreign_v2.gen.h",
+    source="#thirdparty/wayland-protocols/unstable/xdg-foreign/xdg-foreign-unstable-v2.xml",
+)
+
+env.WAYLAND_API_CODE(
+    target="protocol/xdg_foreign_v2.gen.c",
+    source="#thirdparty/wayland-protocols/unstable/xdg-foreign/xdg-foreign-unstable-v2.xml",
+)
+
 env.WAYLAND_API_HEADER(
     target="protocol/xdg_system_bell.gen.h",
     source="#thirdparty/wayland-protocols/staging/xdg-system-bell/xdg-system-bell-v1.xml",
@@ -188,7 +198,8 @@ source_files = [
     "protocol/fractional_scale.gen.c",
     "protocol/xdg_shell.gen.c",
     "protocol/xdg_system_bell.gen.c",
-    "protocol/xdg_foreign.gen.c",
+    "protocol/xdg_foreign_v1.gen.c",
+    "protocol/xdg_foreign_v2.gen.c",
     "protocol/xdg_decoration.gen.c",
     "protocol/xdg_activation.gen.c",
     "protocol/relative_pointer.gen.c",
diff --git a/platform/linuxbsd/wayland/wayland_thread.cpp b/platform/linuxbsd/wayland/wayland_thread.cpp
index 8e3dc75581..baba4d6f78 100644
--- a/platform/linuxbsd/wayland/wayland_thread.cpp
+++ b/platform/linuxbsd/wayland/wayland_thread.cpp
@@ -378,9 +378,16 @@ void WaylandThread::_wl_registry_on_global(void *data, struct wl_registry *wl_re
 		return;
 	}
 
+	// NOTE: Deprecated.
 	if (strcmp(interface, zxdg_exporter_v1_interface.name) == 0) {
-		registry->xdg_exporter = (struct zxdg_exporter_v1 *)wl_registry_bind(wl_registry, name, &zxdg_exporter_v1_interface, 1);
-		registry->xdg_exporter_name = name;
+		registry->xdg_exporter_v1 = (struct zxdg_exporter_v1 *)wl_registry_bind(wl_registry, name, &zxdg_exporter_v1_interface, 1);
+		registry->xdg_exporter_v1_name = name;
+		return;
+	}
+
+	if (strcmp(interface, zxdg_exporter_v2_interface.name) == 0) {
+		registry->xdg_exporter_v2 = (struct zxdg_exporter_v2 *)wl_registry_bind(wl_registry, name, &zxdg_exporter_v2_interface, 1);
+		registry->xdg_exporter_v2_name = name;
 		return;
 	}
 
@@ -600,13 +607,25 @@ void WaylandThread::_wl_registry_on_global_remove(void *data, struct wl_registry
 		return;
 	}
 
-	if (name == registry->xdg_exporter_name) {
-		if (registry->xdg_exporter) {
-			zxdg_exporter_v1_destroy(registry->xdg_exporter);
-			registry->xdg_exporter = nullptr;
+	// NOTE: Deprecated.
+	if (name == registry->xdg_exporter_v1_name) {
+		if (registry->xdg_exporter_v1) {
+			zxdg_exporter_v1_destroy(registry->xdg_exporter_v1);
+			registry->xdg_exporter_v1 = nullptr;
 		}
 
-		registry->xdg_exporter_name = 0;
+		registry->xdg_exporter_v1_name = 0;
+
+		return;
+	}
+
+	if (name == registry->xdg_exporter_v2_name) {
+		if (registry->xdg_exporter_v2) {
+			zxdg_exporter_v2_destroy(registry->xdg_exporter_v2);
+			registry->xdg_exporter_v2 = nullptr;
+		}
+
+		registry->xdg_exporter_v2_name = 0;
 
 		return;
 	}
@@ -1187,7 +1206,15 @@ void WaylandThread::_xdg_toplevel_on_wm_capabilities(void *data, struct xdg_topl
 	}
 }
 
-void WaylandThread::_xdg_exported_on_exported(void *data, zxdg_exported_v1 *exported, const char *handle) {
+// NOTE: Deprecated.
+void WaylandThread::_xdg_exported_v1_on_handle(void *data, zxdg_exported_v1 *exported, const char *handle) {
+	WindowState *ws = (WindowState *)data;
+	ERR_FAIL_NULL(ws);
+
+	ws->exported_handle = vformat("wayland:%s", String::utf8(handle));
+}
+
+void WaylandThread::_xdg_exported_v2_on_handle(void *data, zxdg_exported_v2 *exported, const char *handle) {
 	WindowState *ws = (WindowState *)data;
 	ERR_FAIL_NULL(ws);
 
@@ -3285,9 +3312,12 @@ void WaylandThread::window_create(DisplayServer::WindowID p_window_id, int p_wid
 	ws.frame_callback = wl_surface_frame(ws.wl_surface);
 	wl_callback_add_listener(ws.frame_callback, &frame_wl_callback_listener, &ws);
 
-	if (registry.xdg_exporter) {
-		ws.xdg_exported = zxdg_exporter_v1_export(registry.xdg_exporter, ws.wl_surface);
-		zxdg_exported_v1_add_listener(ws.xdg_exported, &xdg_exported_listener, &ws);
+	if (registry.xdg_exporter_v2) {
+		ws.xdg_exported_v2 = zxdg_exporter_v2_export_toplevel(registry.xdg_exporter_v2, ws.wl_surface);
+		zxdg_exported_v2_add_listener(ws.xdg_exported_v2, &xdg_exported_v2_listener, &ws);
+	} else if (registry.xdg_exporter_v1) {
+		ws.xdg_exported_v1 = zxdg_exporter_v1_export(registry.xdg_exporter_v1, ws.wl_surface);
+		zxdg_exported_v1_add_listener(ws.xdg_exported_v1, &xdg_exported_v1_listener, &ws);
 	}
 
 	wl_surface_commit(ws.wl_surface);
@@ -4411,10 +4441,14 @@ void WaylandThread::destroy() {
 		xdg_wm_base_destroy(registry.xdg_wm_base);
 	}
 
-	if (registry.xdg_exporter) {
-		zxdg_exporter_v1_destroy(registry.xdg_exporter);
+	// NOTE: Deprecated.
+	if (registry.xdg_exporter_v1) {
+		zxdg_exporter_v1_destroy(registry.xdg_exporter_v1);
 	}
 
+	if (registry.xdg_exporter_v2) {
+		zxdg_exporter_v2_destroy(registry.xdg_exporter_v2);
+	}
 	if (registry.wl_shm) {
 		wl_shm_destroy(registry.wl_shm);
 	}
diff --git a/platform/linuxbsd/wayland/wayland_thread.h b/platform/linuxbsd/wayland/wayland_thread.h
index c07694b52a..8f1605b820 100644
--- a/platform/linuxbsd/wayland/wayland_thread.h
+++ b/platform/linuxbsd/wayland/wayland_thread.h
@@ -67,10 +67,13 @@
 #include "wayland/protocol/wayland.gen.h"
 #include "wayland/protocol/xdg_activation.gen.h"
 #include "wayland/protocol/xdg_decoration.gen.h"
-#include "wayland/protocol/xdg_foreign.gen.h"
+#include "wayland/protocol/xdg_foreign_v2.gen.h"
 #include "wayland/protocol/xdg_shell.gen.h"
 #include "wayland/protocol/xdg_system_bell.gen.h"
 
+// NOTE: Deprecated.
+#include "wayland/protocol/xdg_foreign_v1.gen.h"
+
 #ifdef LIBDECOR_ENABLED
 #ifdef SOWRAP_ENABLED
 #include "dynwrappers/libdecor-so_wrap.h"
@@ -149,8 +152,12 @@ public:
 		struct xdg_wm_base *xdg_wm_base = nullptr;
 		uint32_t xdg_wm_base_name = 0;
 
-		struct zxdg_exporter_v1 *xdg_exporter = nullptr;
-		uint32_t xdg_exporter_name = 0;
+		// NOTE: Deprecated.
+		struct zxdg_exporter_v1 *xdg_exporter_v1 = nullptr;
+		uint32_t xdg_exporter_v1_name = 0;
+
+		uint32_t xdg_exporter_v2_name = 0;
+		struct zxdg_exporter_v2 *xdg_exporter_v2 = nullptr;
 
 		// wayland-protocols globals.
 
@@ -224,7 +231,11 @@ public:
 
 		struct wp_viewport *wp_viewport = nullptr;
 		struct wp_fractional_scale_v1 *wp_fractional_scale = nullptr;
-		struct zxdg_exported_v1 *xdg_exported = nullptr;
+
+		// NOTE: Deprecated.
+		struct zxdg_exported_v1 *xdg_exported_v1 = nullptr;
+
+		struct zxdg_exported_v2 *xdg_exported_v2 = nullptr;
 
 		String exported_handle;
 
@@ -652,7 +663,10 @@ private:
 
 	static void _xdg_toplevel_decoration_on_configure(void *data, struct zxdg_toplevel_decoration_v1 *xdg_toplevel_decoration, uint32_t mode);
 
-	static void _xdg_exported_on_exported(void *data, zxdg_exported_v1 *exported, const char *handle);
+	// NOTE: Deprecated.
+	static void _xdg_exported_v1_on_handle(void *data, zxdg_exported_v1 *exported, const char *handle);
+
+	static void _xdg_exported_v2_on_handle(void *data, zxdg_exported_v2 *exported, const char *handle);
 
 	static void _xdg_activation_token_on_done(void *data, struct xdg_activation_token_v1 *xdg_activation_token, const char *token);
 
@@ -820,8 +834,13 @@ private:
 		.done = _wp_text_input_on_done,
 	};
 
-	static constexpr struct zxdg_exported_v1_listener xdg_exported_listener = {
-		.handle = _xdg_exported_on_exported
+	// NOTE: Deprecated.
+	static constexpr struct zxdg_exported_v1_listener xdg_exported_v1_listener = {
+		.handle = _xdg_exported_v1_on_handle,
+	};
+
+	static constexpr struct zxdg_exported_v2_listener xdg_exported_v2_listener = {
+		.handle = _xdg_exported_v2_on_handle,
 	};
 
 	static constexpr struct zxdg_toplevel_decoration_v1_listener xdg_toplevel_decoration_listener = {
diff --git a/thirdparty/wayland-protocols/unstable/xdg-foreign/xdg-foreign-unstable-v2.xml b/thirdparty/wayland-protocols/unstable/xdg-foreign/xdg-foreign-unstable-v2.xml
new file mode 100644
index 0000000000..cc3271dca4
--- /dev/null
+++ b/thirdparty/wayland-protocols/unstable/xdg-foreign/xdg-foreign-unstable-v2.xml
@@ -0,0 +1,200 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<protocol name="xdg_foreign_unstable_v2">
+
+  <copyright>
+    Copyright © 2015-2016 Red Hat Inc.
+
+    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 (including the next
+    paragraph) 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.
+  </copyright>
+
+  <description summary="Protocol for exporting xdg surface handles">
+    This protocol specifies a way for making it possible to reference a surface
+    of a different client. With such a reference, a client can, by using the
+    interfaces provided by this protocol, manipulate the relationship between
+    its own surfaces and the surface of some other client. For example, stack
+    some of its own surface above the other clients surface.
+
+    In order for a client A to get a reference of a surface of client B, client
+    B must first export its surface using xdg_exporter.export_toplevel. Upon
+    doing this, client B will receive a handle (a unique string) that it may
+    share with client A in some way (for example D-Bus). After client A has
+    received the handle from client B, it may use xdg_importer.import_toplevel
+    to create a reference to the surface client B just exported. See the
+    corresponding requests for details.
+
+    A possible use case for this is out-of-process dialogs. For example when a
+    sandboxed client without file system access needs the user to select a file
+    on the file system, given sandbox environment support, it can export its
+    surface, passing the exported surface handle to an unsandboxed process that
+    can show a file browser dialog and stack it above the sandboxed client's
+    surface.
+
+    Warning! The protocol described in this file is experimental and backward
+    incompatible changes may be made. Backward compatible changes may be added
+    together with the corresponding interface version bump. Backward
+    incompatible changes are done by bumping the version number in the protocol
+    and interface names and resetting the interface version. Once the protocol
+    is to be declared stable, the 'z' prefix and the version number in the
+    protocol and interface names are removed and the interface version number is
+    reset.
+  </description>
+
+  <interface name="zxdg_exporter_v2" version="1">
+    <description summary="interface for exporting surfaces">
+      A global interface used for exporting surfaces that can later be imported
+      using xdg_importer.
+    </description>
+
+    <request name="destroy" type="destructor">
+      <description summary="destroy the xdg_exporter object">
+	Notify the compositor that the xdg_exporter object will no longer be
+	used.
+      </description>
+    </request>
+
+    <enum name="error">
+      <description summary="error values">
+        These errors can be emitted in response to invalid xdg_exporter
+        requests.
+      </description>
+      <entry name="invalid_surface" value="0" summary="surface is not an xdg_toplevel"/>
+    </enum>
+
+    <request name="export_toplevel">
+      <description summary="export a toplevel surface">
+	The export_toplevel request exports the passed surface so that it can later be
+	imported via xdg_importer. When called, a new xdg_exported object will
+	be created and xdg_exported.handle will be sent immediately. See the
+	corresponding interface and event for details.
+
+	A surface may be exported multiple times, and each exported handle may
+	be used to create an xdg_imported multiple times. Only xdg_toplevel
+        equivalent surfaces may be exported, otherwise an invalid_surface
+        protocol error is sent.
+      </description>
+      <arg name="id" type="new_id" interface="zxdg_exported_v2"
+	   summary="the new xdg_exported object"/>
+      <arg name="surface" type="object" interface="wl_surface"
+	   summary="the surface to export"/>
+    </request>
+  </interface>
+
+  <interface name="zxdg_importer_v2" version="1">
+    <description summary="interface for importing surfaces">
+      A global interface used for importing surfaces exported by xdg_exporter.
+      With this interface, a client can create a reference to a surface of
+      another client.
+    </description>
+
+    <request name="destroy" type="destructor">
+      <description summary="destroy the xdg_importer object">
+	Notify the compositor that the xdg_importer object will no longer be
+	used.
+      </description>
+    </request>
+
+    <request name="import_toplevel">
+      <description summary="import a toplevel surface">
+	The import_toplevel request imports a surface from any client given a handle
+	retrieved by exporting said surface using xdg_exporter.export_toplevel.
+	When called, a new xdg_imported object will be created. This new object
+	represents the imported surface, and the importing client can
+	manipulate its relationship using it. See xdg_imported for details.
+      </description>
+      <arg name="id" type="new_id" interface="zxdg_imported_v2"
+	   summary="the new xdg_imported object"/>
+      <arg name="handle" type="string"
+	   summary="the exported surface handle"/>
+    </request>
+  </interface>
+
+  <interface name="zxdg_exported_v2" version="1">
+    <description summary="an exported surface handle">
+      An xdg_exported object represents an exported reference to a surface. The
+      exported surface may be referenced as long as the xdg_exported object not
+      destroyed. Destroying the xdg_exported invalidates any relationship the
+      importer may have established using xdg_imported.
+    </description>
+
+    <request name="destroy" type="destructor">
+      <description summary="unexport the exported surface">
+	Revoke the previously exported surface. This invalidates any
+	relationship the importer may have set up using the xdg_imported created
+	given the handle sent via xdg_exported.handle.
+      </description>
+    </request>
+
+    <event name="handle">
+      <description summary="the exported surface handle">
+	The handle event contains the unique handle of this exported surface
+	reference. It may be shared with any client, which then can use it to
+	import the surface by calling xdg_importer.import_toplevel. A handle
+	may be used to import the surface multiple times.
+      </description>
+      <arg name="handle" type="string" summary="the exported surface handle"/>
+    </event>
+  </interface>
+
+  <interface name="zxdg_imported_v2" version="1">
+    <description summary="an imported surface handle">
+      An xdg_imported object represents an imported reference to surface exported
+      by some client. A client can use this interface to manipulate
+      relationships between its own surfaces and the imported surface.
+    </description>
+
+    <enum name="error">
+      <description summary="error values">
+        These errors can be emitted in response to invalid xdg_imported
+        requests.
+      </description>
+      <entry name="invalid_surface" value="0" summary="surface is not an xdg_toplevel"/>
+    </enum>
+
+    <request name="destroy" type="destructor">
+      <description summary="destroy the xdg_imported object">
+	Notify the compositor that it will no longer use the xdg_imported
+	object. Any relationship that may have been set up will at this point
+	be invalidated.
+      </description>
+    </request>
+
+    <request name="set_parent_of">
+      <description summary="set as the parent of some surface">
+        Set the imported surface as the parent of some surface of the client.
+        The passed surface must be an xdg_toplevel equivalent, otherwise an
+        invalid_surface protocol error is sent. Calling this function sets up
+        a surface to surface relation with the same stacking and positioning
+        semantics as xdg_toplevel.set_parent.
+      </description>
+      <arg name="surface" type="object" interface="wl_surface"
+	   summary="the child surface"/>
+    </request>
+
+    <event name="destroyed">
+      <description summary="the imported surface handle has been destroyed">
+	The imported surface handle has been destroyed and any relationship set
+	up has been invalidated. This may happen for various reasons, for
+	example if the exported surface or the exported surface handle has been
+	destroyed, if the handle used for importing was invalid.
+      </description>
+    </event>
+  </interface>
+
+</protocol>

From 66b7d5f1b527f9059745477ecec9671ceb191e86 Mon Sep 17 00:00:00 2001
From: Sai Nane <esainane+git@gmail.com>
Date: Sat, 7 Dec 2024 16:22:33 +0000
Subject: [PATCH 054/124] Remove apparent contradiction in vector.h header

3205a92ad8 was a major commit which removed `PoolVector`, and replaced
most references to `PoolVector` with `Vector` instead. In most cases,
this was appropriate, given that `PoolVector` was being replaced with
`Vector`, as an effective generalist in 64-bit address space layouts.

However, vector.h itself was left with an artifact advising the reader
to use `Vector` instead of `Vector` for large arrays. While this led
to a fascinating deep dive, and hopefully improved some of the
documentation along the way, it's probably best to clean this up for
the next person.
---
 core/templates/vector.h | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/core/templates/vector.h b/core/templates/vector.h
index 52c10eea68..0fcca47bae 100644
--- a/core/templates/vector.h
+++ b/core/templates/vector.h
@@ -33,7 +33,10 @@
 
 /**
  * @class Vector
- * Vector container. Regular Vector Container. Use with care and for smaller arrays when possible. Use Vector for large arrays.
+ * Vector container. Simple copy-on-write container.
+ *
+ * LocalVector is an alternative available for internal use when COW is not
+ * required.
  */
 
 #include "core/error/error_macros.h"

From 04fff3fe0d590c55914770dcc7549467b2fdabdd Mon Sep 17 00:00:00 2001
From: HP van Braam <hp@tmm.cx>
Date: Mon, 9 Dec 2024 17:56:51 +0100
Subject: [PATCH 055/124] Unconditionally use env.Decider("MD5-timestamp")

It seems that we are only enabling this option together with unsafe
build determinators. However it seems that MD5-timestamp by itself is
not unsafe.

MD5-Timestamp works by first checking the timestamp of a file, comparing
it to the scons database and if it does not match it will do an md5sum
to determine if the file needs rebuilding.

Without this option SCons will always create md5sums of every file. In
the case of a null build this is 50% of the time.
---
 SConstruct | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/SConstruct b/SConstruct
index fa324b2aa0..591dc89362 100644
--- a/SConstruct
+++ b/SConstruct
@@ -501,14 +501,19 @@ else:
     # Disable assert() for production targets (only used in thirdparty code).
     env.Append(CPPDEFINES=["NDEBUG"])
 
+# This is not part of fast_unsafe because the only downside it has compared to
+# the default is that SCons won't mark files that were changed in the last second
+# as different. This is unlikely to be a problem in any real situation as just booting
+# up scons takes more than that time.
+# Renamed to `content-timestamp` in SCons >= 4.2, keeping MD5 for compat.
+env.Decider("MD5-timestamp")
+
 # SCons speed optimization controlled by the `fast_unsafe` option, which provide
 # more than 10 s speed up for incremental rebuilds.
 # Unsafe as they reduce the certainty of rebuilding all changed files, so it's
 # enabled by default for `debug` builds, and can be overridden from command line.
 # Ref: https://github.com/SCons/scons/wiki/GoFastButton
 if methods.get_cmdline_bool("fast_unsafe", env.dev_build):
-    # Renamed to `content-timestamp` in SCons >= 4.2, keeping MD5 for compat.
-    env.Decider("MD5-timestamp")
     env.SetOption("implicit_cache", 1)
     env.SetOption("max_drift", 60)
 

From 13fcb05e7b2cc30c9e5870ba65f4a116d95e75a8 Mon Sep 17 00:00:00 2001
From: JackErb <ankhenac@gmail.com>
Date: Sat, 30 Nov 2024 14:29:30 -0800
Subject: [PATCH 056/124] Improve GDScript "unexpected token in class body"
 parser error

This parser error was misleading.

Fixes:
1. Now points at correct line
2. For identifiers, prints out `Identifier "%s"`
---
 modules/gdscript/gdscript_parser.cpp                     | 2 +-
 modules/gdscript/gdscript_tokenizer.cpp                  | 9 +++++++++
 modules/gdscript/gdscript_tokenizer.h                    | 1 +
 .../parser/errors/unexpected_token_in_class_body.gd      | 7 +++++++
 .../parser/errors/unexpected_token_in_class_body.out     | 2 ++
 5 files changed, 20 insertions(+), 1 deletion(-)
 create mode 100644 modules/gdscript/tests/scripts/parser/errors/unexpected_token_in_class_body.gd
 create mode 100644 modules/gdscript/tests/scripts/parser/errors/unexpected_token_in_class_body.out

diff --git a/modules/gdscript/gdscript_parser.cpp b/modules/gdscript/gdscript_parser.cpp
index 12e71004db..d5112fa308 100644
--- a/modules/gdscript/gdscript_parser.cpp
+++ b/modules/gdscript/gdscript_parser.cpp
@@ -1060,8 +1060,8 @@ void GDScriptParser::parse_class_body(bool p_is_multiline) {
 			default:
 				// Display a completion with identifiers.
 				make_completion_context(COMPLETION_IDENTIFIER, nullptr);
-				push_error(vformat(R"(Unexpected "%s" in class body.)", current.get_name()));
 				advance();
+				push_error(vformat(R"(Unexpected %s in class body.)", previous.get_debug_name()));
 				break;
 		}
 		if (token.type != GDScriptTokenizer::Token::STATIC) {
diff --git a/modules/gdscript/gdscript_tokenizer.cpp b/modules/gdscript/gdscript_tokenizer.cpp
index 404b61fb40..dd936cbe7e 100644
--- a/modules/gdscript/gdscript_tokenizer.cpp
+++ b/modules/gdscript/gdscript_tokenizer.cpp
@@ -164,6 +164,15 @@ const char *GDScriptTokenizer::Token::get_name() const {
 	return token_names[type];
 }
 
+String GDScriptTokenizer::Token::get_debug_name() const {
+	switch (type) {
+		case IDENTIFIER:
+			return vformat(R"(identifier "%s")", source);
+		default:
+			return vformat(R"("%s")", get_name());
+	}
+}
+
 bool GDScriptTokenizer::Token::can_precede_bin_op() const {
 	switch (type) {
 		case IDENTIFIER:
diff --git a/modules/gdscript/gdscript_tokenizer.h b/modules/gdscript/gdscript_tokenizer.h
index 5d76375173..08e7417922 100644
--- a/modules/gdscript/gdscript_tokenizer.h
+++ b/modules/gdscript/gdscript_tokenizer.h
@@ -178,6 +178,7 @@ public:
 		String source;
 
 		const char *get_name() const;
+		String get_debug_name() const;
 		bool can_precede_bin_op() const;
 		bool is_identifier() const;
 		bool is_node_name() const;
diff --git a/modules/gdscript/tests/scripts/parser/errors/unexpected_token_in_class_body.gd b/modules/gdscript/tests/scripts/parser/errors/unexpected_token_in_class_body.gd
new file mode 100644
index 0000000000..b036799d91
--- /dev/null
+++ b/modules/gdscript/tests/scripts/parser/errors/unexpected_token_in_class_body.gd
@@ -0,0 +1,7 @@
+# GH-96792
+
+var error
+error = true
+
+func test():
+	pass
diff --git a/modules/gdscript/tests/scripts/parser/errors/unexpected_token_in_class_body.out b/modules/gdscript/tests/scripts/parser/errors/unexpected_token_in_class_body.out
new file mode 100644
index 0000000000..f6eed886d1
--- /dev/null
+++ b/modules/gdscript/tests/scripts/parser/errors/unexpected_token_in_class_body.out
@@ -0,0 +1,2 @@
+GDTEST_PARSER_ERROR
+Unexpected identifier "error" in class body.

From ba39b078f50887c5f4c3b3300fe9dec2fcda26ca Mon Sep 17 00:00:00 2001
From: Chaosus <chaosus89@gmail.com>
Date: Thu, 5 Dec 2024 17:13:20 +0300
Subject: [PATCH 057/124] [Scene] Add `SceneStringName::FlatButton`

---
 editor/debugger/script_editor_debugger.cpp    | 16 +++----
 editor/editor_audio_buses.cpp                 |  6 +--
 editor/editor_help_search.cpp                 |  4 +-
 editor/editor_log.cpp                         |  8 ++--
 editor/gui/editor_file_dialog.cpp             | 24 +++++-----
 editor/node_dock.cpp                          |  4 +-
 editor/plugins/abstract_polygon_2d_editor.cpp |  6 +--
 .../animation_blend_space_1d_editor.cpp       | 10 ++--
 .../animation_blend_space_2d_editor.cpp       | 14 +++---
 .../animation_player_editor_plugin.cpp        | 16 +++----
 .../animation_state_machine_editor.cpp        | 10 ++--
 editor/plugins/canvas_item_editor_plugin.cpp  | 38 +++++++--------
 editor/plugins/control_editor_plugin.cpp      |  4 +-
 editor/plugins/game_view_plugin.cpp           | 18 +++----
 ..._particles_collision_sdf_editor_plugin.cpp |  2 +-
 editor/plugins/lightmap_gi_editor_plugin.cpp  |  2 +-
 .../navigation_obstacle_3d_editor_plugin.cpp  | 10 ++--
 editor/plugins/node_3d_editor_plugin.cpp      | 28 +++++------
 .../occluder_instance_3d_editor_plugin.cpp    |  2 +-
 editor/plugins/path_2d_editor_plugin.cpp      | 12 ++---
 editor/plugins/path_3d_editor_plugin.cpp      | 14 +++---
 .../physical_bone_3d_editor_plugin.cpp        |  2 +-
 editor/plugins/polygon_2d_editor_plugin.cpp   |  8 ++--
 editor/plugins/polygon_3d_editor_plugin.cpp   |  4 +-
 editor/plugins/skeleton_3d_editor_plugin.cpp  | 12 ++---
 .../plugins/sprite_frames_editor_plugin.cpp   | 44 ++++++++---------
 editor/plugins/theme_editor_preview.cpp       |  2 +-
 editor/plugins/tiles/tile_data_editors.cpp    | 14 +++---
 .../plugins/tiles/tile_map_layer_editor.cpp   | 48 +++++++++----------
 .../tiles/tile_set_atlas_source_editor.cpp    |  8 ++--
 editor/plugins/tiles/tile_set_editor.cpp      |  8 ++--
 ...le_set_scenes_collection_source_editor.cpp |  4 +-
 .../plugins/version_control_editor_plugin.cpp | 14 +++---
 .../plugins/visual_shader_editor_plugin.cpp   |  4 +-
 editor/plugins/voxel_gi_editor_plugin.cpp     |  2 +-
 editor/scene_tree_dock.cpp                    |  4 +-
 editor/themes/editor_theme_manager.cpp        |  8 ++--
 .../gridmap/editor/grid_map_editor_plugin.cpp | 30 ++++++------
 .../multiplayer/editor/replication_editor.cpp |  2 +-
 .../editor/navigation_mesh_editor_plugin.cpp  |  4 +-
 scene/gui/file_dialog.cpp                     | 14 +++---
 scene/gui/graph_edit.cpp                      | 14 +++---
 scene/scene_string_names.h                    |  2 +
 scene/theme/default_theme.cpp                 | 10 ++--
 44 files changed, 256 insertions(+), 254 deletions(-)

diff --git a/editor/debugger/script_editor_debugger.cpp b/editor/debugger/script_editor_debugger.cpp
index e50b6273bc..56b75b0b2f 100644
--- a/editor/debugger/script_editor_debugger.cpp
+++ b/editor/debugger/script_editor_debugger.cpp
@@ -1853,7 +1853,7 @@ ScriptEditorDebugger::ScriptEditorDebugger() {
 		hbc->add_child(memnew(VSeparator));
 
 		skip_breakpoints = memnew(Button);
-		skip_breakpoints->set_theme_type_variation("FlatButton");
+		skip_breakpoints->set_theme_type_variation(SceneStringName(FlatButton));
 		hbc->add_child(skip_breakpoints);
 		skip_breakpoints->set_tooltip_text(TTR("Skip Breakpoints"));
 		skip_breakpoints->connect(SceneStringName(pressed), callable_mp(this, &ScriptEditorDebugger::debug_skip_breakpoints));
@@ -1861,7 +1861,7 @@ ScriptEditorDebugger::ScriptEditorDebugger() {
 		hbc->add_child(memnew(VSeparator));
 
 		copy = memnew(Button);
-		copy->set_theme_type_variation("FlatButton");
+		copy->set_theme_type_variation(SceneStringName(FlatButton));
 		hbc->add_child(copy);
 		copy->set_tooltip_text(TTR("Copy Error"));
 		copy->connect(SceneStringName(pressed), callable_mp(this, &ScriptEditorDebugger::debug_copy));
@@ -1869,14 +1869,14 @@ ScriptEditorDebugger::ScriptEditorDebugger() {
 		hbc->add_child(memnew(VSeparator));
 
 		step = memnew(Button);
-		step->set_theme_type_variation("FlatButton");
+		step->set_theme_type_variation(SceneStringName(FlatButton));
 		hbc->add_child(step);
 		step->set_tooltip_text(TTR("Step Into"));
 		step->set_shortcut(ED_GET_SHORTCUT("debugger/step_into"));
 		step->connect(SceneStringName(pressed), callable_mp(this, &ScriptEditorDebugger::debug_step));
 
 		next = memnew(Button);
-		next->set_theme_type_variation("FlatButton");
+		next->set_theme_type_variation(SceneStringName(FlatButton));
 		hbc->add_child(next);
 		next->set_tooltip_text(TTR("Step Over"));
 		next->set_shortcut(ED_GET_SHORTCUT("debugger/step_over"));
@@ -1885,14 +1885,14 @@ ScriptEditorDebugger::ScriptEditorDebugger() {
 		hbc->add_child(memnew(VSeparator));
 
 		dobreak = memnew(Button);
-		dobreak->set_theme_type_variation("FlatButton");
+		dobreak->set_theme_type_variation(SceneStringName(FlatButton));
 		hbc->add_child(dobreak);
 		dobreak->set_tooltip_text(TTR("Break"));
 		dobreak->set_shortcut(ED_GET_SHORTCUT("debugger/break"));
 		dobreak->connect(SceneStringName(pressed), callable_mp(this, &ScriptEditorDebugger::debug_break));
 
 		docontinue = memnew(Button);
-		docontinue->set_theme_type_variation("FlatButton");
+		docontinue->set_theme_type_variation(SceneStringName(FlatButton));
 		hbc->add_child(docontinue);
 		docontinue->set_tooltip_text(TTR("Continue"));
 		docontinue->set_shortcut(ED_GET_SHORTCUT("debugger/continue"));
@@ -2079,10 +2079,10 @@ ScriptEditorDebugger::ScriptEditorDebugger() {
 		vmem_total->set_custom_minimum_size(Size2(100, 0) * EDSCALE);
 		vmem_hb->add_child(vmem_total);
 		vmem_refresh = memnew(Button);
-		vmem_refresh->set_theme_type_variation("FlatButton");
+		vmem_refresh->set_theme_type_variation(SceneStringName(FlatButton));
 		vmem_hb->add_child(vmem_refresh);
 		vmem_export = memnew(Button);
-		vmem_export->set_theme_type_variation("FlatButton");
+		vmem_export->set_theme_type_variation(SceneStringName(FlatButton));
 		vmem_export->set_tooltip_text(TTR("Export list to a CSV file"));
 		vmem_hb->add_child(vmem_export);
 		vmem_vb->add_child(vmem_hb);
diff --git a/editor/editor_audio_buses.cpp b/editor/editor_audio_buses.cpp
index 11a1fbf452..d429427ab3 100644
--- a/editor/editor_audio_buses.cpp
+++ b/editor/editor_audio_buses.cpp
@@ -822,21 +822,21 @@ EditorAudioBus::EditorAudioBus(EditorAudioBuses *p_buses, bool p_is_master) {
 	HBoxContainer *hbc = memnew(HBoxContainer);
 	vb->add_child(hbc);
 	solo = memnew(Button);
-	solo->set_theme_type_variation("FlatButton");
+	solo->set_theme_type_variation(SceneStringName(FlatButton));
 	solo->set_toggle_mode(true);
 	solo->set_tooltip_text(TTR("Solo"));
 	solo->set_focus_mode(FOCUS_NONE);
 	solo->connect(SceneStringName(pressed), callable_mp(this, &EditorAudioBus::_solo_toggled));
 	hbc->add_child(solo);
 	mute = memnew(Button);
-	mute->set_theme_type_variation("FlatButton");
+	mute->set_theme_type_variation(SceneStringName(FlatButton));
 	mute->set_toggle_mode(true);
 	mute->set_tooltip_text(TTR("Mute"));
 	mute->set_focus_mode(FOCUS_NONE);
 	mute->connect(SceneStringName(pressed), callable_mp(this, &EditorAudioBus::_mute_toggled));
 	hbc->add_child(mute);
 	bypass = memnew(Button);
-	bypass->set_theme_type_variation("FlatButton");
+	bypass->set_theme_type_variation(SceneStringName(FlatButton));
 	bypass->set_toggle_mode(true);
 	bypass->set_tooltip_text(TTR("Bypass"));
 	bypass->set_focus_mode(FOCUS_NONE);
diff --git a/editor/editor_help_search.cpp b/editor/editor_help_search.cpp
index 9f4bb3ab2f..d8a2af5577 100644
--- a/editor/editor_help_search.cpp
+++ b/editor/editor_help_search.cpp
@@ -338,7 +338,7 @@ EditorHelpSearch::EditorHelpSearch() {
 	hbox->add_child(search_box);
 
 	case_sensitive_button = memnew(Button);
-	case_sensitive_button->set_theme_type_variation("FlatButton");
+	case_sensitive_button->set_theme_type_variation(SceneStringName(FlatButton));
 	case_sensitive_button->set_tooltip_text(TTR("Case Sensitive"));
 	case_sensitive_button->connect(SceneStringName(pressed), callable_mp(this, &EditorHelpSearch::_update_results));
 	case_sensitive_button->set_toggle_mode(true);
@@ -346,7 +346,7 @@ EditorHelpSearch::EditorHelpSearch() {
 	hbox->add_child(case_sensitive_button);
 
 	hierarchy_button = memnew(Button);
-	hierarchy_button->set_theme_type_variation("FlatButton");
+	hierarchy_button->set_theme_type_variation(SceneStringName(FlatButton));
 	hierarchy_button->set_tooltip_text(TTR("Show Hierarchy"));
 	hierarchy_button->connect(SceneStringName(pressed), callable_mp(this, &EditorHelpSearch::_update_results));
 	hierarchy_button->set_toggle_mode(true);
diff --git a/editor/editor_log.cpp b/editor/editor_log.cpp
index db26a75cb8..76a1c9b04d 100644
--- a/editor/editor_log.cpp
+++ b/editor/editor_log.cpp
@@ -480,7 +480,7 @@ EditorLog::EditorLog() {
 
 	// Clear.
 	clear_button = memnew(Button);
-	clear_button->set_theme_type_variation("FlatButton");
+	clear_button->set_theme_type_variation(SceneStringName(FlatButton));
 	clear_button->set_focus_mode(FOCUS_NONE);
 	clear_button->set_shortcut(ED_SHORTCUT("editor/clear_output", TTR("Clear Output"), KeyModifierMask::CMD_OR_CTRL | KeyModifierMask::ALT | Key::K));
 	clear_button->connect(SceneStringName(pressed), callable_mp(this, &EditorLog::_clear_request));
@@ -488,7 +488,7 @@ EditorLog::EditorLog() {
 
 	// Copy.
 	copy_button = memnew(Button);
-	copy_button->set_theme_type_variation("FlatButton");
+	copy_button->set_theme_type_variation(SceneStringName(FlatButton));
 	copy_button->set_focus_mode(FOCUS_NONE);
 	copy_button->set_shortcut(ED_SHORTCUT("editor/copy_output", TTR("Copy Selection"), KeyModifierMask::CMD_OR_CTRL | Key::C));
 	copy_button->set_shortcut_context(this);
@@ -505,7 +505,7 @@ EditorLog::EditorLog() {
 
 	// Collapse.
 	collapse_button = memnew(Button);
-	collapse_button->set_theme_type_variation("FlatButton");
+	collapse_button->set_theme_type_variation(SceneStringName(FlatButton));
 	collapse_button->set_focus_mode(FOCUS_NONE);
 	collapse_button->set_tooltip_text(TTR("Collapse duplicate messages into one log entry. Shows number of occurrences."));
 	collapse_button->set_toggle_mode(true);
@@ -515,7 +515,7 @@ EditorLog::EditorLog() {
 
 	// Show Search.
 	show_search_button = memnew(Button);
-	show_search_button->set_theme_type_variation("FlatButton");
+	show_search_button->set_theme_type_variation(SceneStringName(FlatButton));
 	show_search_button->set_focus_mode(FOCUS_NONE);
 	show_search_button->set_toggle_mode(true);
 	show_search_button->set_pressed(true);
diff --git a/editor/gui/editor_file_dialog.cpp b/editor/gui/editor_file_dialog.cpp
index 8ece6f5dfe..7ef9823537 100644
--- a/editor/gui/editor_file_dialog.cpp
+++ b/editor/gui/editor_file_dialog.cpp
@@ -2309,13 +2309,13 @@ EditorFileDialog::EditorFileDialog() {
 	vbc->add_child(pathhb);
 
 	dir_prev = memnew(Button);
-	dir_prev->set_theme_type_variation("FlatButton");
+	dir_prev->set_theme_type_variation(SceneStringName(FlatButton));
 	dir_prev->set_tooltip_text(TTR("Go to previous folder."));
 	dir_next = memnew(Button);
-	dir_next->set_theme_type_variation("FlatButton");
+	dir_next->set_theme_type_variation(SceneStringName(FlatButton));
 	dir_next->set_tooltip_text(TTR("Go to next folder."));
 	dir_up = memnew(Button);
-	dir_up->set_theme_type_variation("FlatButton");
+	dir_up->set_theme_type_variation(SceneStringName(FlatButton));
 	dir_up->set_tooltip_text(TTR("Go to parent folder."));
 
 	pathhb->add_child(dir_prev);
@@ -2338,13 +2338,13 @@ EditorFileDialog::EditorFileDialog() {
 	pathhb->add_child(dir);
 
 	refresh = memnew(Button);
-	refresh->set_theme_type_variation("FlatButton");
+	refresh->set_theme_type_variation(SceneStringName(FlatButton));
 	refresh->set_tooltip_text(TTR("Refresh files."));
 	refresh->connect(SceneStringName(pressed), callable_mp(this, &EditorFileDialog::update_file_list));
 	pathhb->add_child(refresh);
 
 	favorite = memnew(Button);
-	favorite->set_theme_type_variation("FlatButton");
+	favorite->set_theme_type_variation(SceneStringName(FlatButton));
 	favorite->set_toggle_mode(true);
 	favorite->set_tooltip_text(TTR("(Un)favorite current folder."));
 	favorite->connect(SceneStringName(pressed), callable_mp(this, &EditorFileDialog::_favorite_pressed));
@@ -2361,7 +2361,7 @@ EditorFileDialog::EditorFileDialog() {
 	pathhb->add_child(makedir_sep);
 
 	makedir = memnew(Button);
-	makedir->set_theme_type_variation("FlatButton");
+	makedir->set_theme_type_variation(SceneStringName(FlatButton));
 	makedir->set_tooltip_text(TTR("Create a new folder."));
 	makedir->connect(SceneStringName(pressed), callable_mp(this, &EditorFileDialog::_make_dir));
 	pathhb->add_child(makedir);
@@ -2397,11 +2397,11 @@ EditorFileDialog::EditorFileDialog() {
 
 	fav_hb->add_spacer();
 	fav_up = memnew(Button);
-	fav_up->set_theme_type_variation("FlatButton");
+	fav_up->set_theme_type_variation(SceneStringName(FlatButton));
 	fav_hb->add_child(fav_up);
 	fav_up->connect(SceneStringName(pressed), callable_mp(this, &EditorFileDialog::_favorite_move_up));
 	fav_down = memnew(Button);
-	fav_down->set_theme_type_variation("FlatButton");
+	fav_down->set_theme_type_variation(SceneStringName(FlatButton));
 	fav_hb->add_child(fav_down);
 	fav_down->connect(SceneStringName(pressed), callable_mp(this, &EditorFileDialog::_favorite_move_down));
 
@@ -2444,7 +2444,7 @@ EditorFileDialog::EditorFileDialog() {
 	lower_hb->add_child(l);
 
 	show_hidden = memnew(Button);
-	show_hidden->set_theme_type_variation("FlatButton");
+	show_hidden->set_theme_type_variation(SceneStringName(FlatButton));
 	show_hidden->set_toggle_mode(true);
 	show_hidden->set_pressed(is_showing_hidden_files());
 	show_hidden->set_tooltip_text(TTR("Toggle the visibility of hidden files."));
@@ -2457,7 +2457,7 @@ EditorFileDialog::EditorFileDialog() {
 	view_mode_group.instantiate();
 
 	mode_thumbnails = memnew(Button);
-	mode_thumbnails->set_theme_type_variation("FlatButton");
+	mode_thumbnails->set_theme_type_variation(SceneStringName(FlatButton));
 	mode_thumbnails->connect(SceneStringName(pressed), callable_mp(this, &EditorFileDialog::set_display_mode).bind(DISPLAY_THUMBNAILS));
 	mode_thumbnails->set_toggle_mode(true);
 	mode_thumbnails->set_pressed(display_mode == DISPLAY_THUMBNAILS);
@@ -2466,7 +2466,7 @@ EditorFileDialog::EditorFileDialog() {
 	lower_hb->add_child(mode_thumbnails);
 
 	mode_list = memnew(Button);
-	mode_list->set_theme_type_variation("FlatButton");
+	mode_list->set_theme_type_variation(SceneStringName(FlatButton));
 	mode_list->connect(SceneStringName(pressed), callable_mp(this, &EditorFileDialog::set_display_mode).bind(DISPLAY_LIST));
 	mode_list->set_toggle_mode(true);
 	mode_list->set_pressed(display_mode == DISPLAY_LIST);
@@ -2482,7 +2482,7 @@ EditorFileDialog::EditorFileDialog() {
 	file_sort_button->set_tooltip_text(TTR("Sort files"));
 
 	show_search_filter_button = memnew(Button);
-	show_search_filter_button->set_theme_type_variation("FlatButton");
+	show_search_filter_button->set_theme_type_variation(SceneStringName(FlatButton));
 	show_search_filter_button->set_toggle_mode(true);
 	show_search_filter_button->set_pressed(false);
 	show_search_filter_button->set_tooltip_text(TTR("Toggle the visibility of the filter for file names."));
diff --git a/editor/node_dock.cpp b/editor/node_dock.cpp
index 985a319c84..6b711349da 100644
--- a/editor/node_dock.cpp
+++ b/editor/node_dock.cpp
@@ -94,7 +94,7 @@ NodeDock::NodeDock() {
 	mode_hb->hide();
 
 	connections_button = memnew(Button);
-	connections_button->set_theme_type_variation("FlatButton");
+	connections_button->set_theme_type_variation(SceneStringName(FlatButton));
 	connections_button->set_text(TTR("Signals"));
 	connections_button->set_toggle_mode(true);
 	connections_button->set_pressed(true);
@@ -104,7 +104,7 @@ NodeDock::NodeDock() {
 	connections_button->connect(SceneStringName(pressed), callable_mp(this, &NodeDock::show_connections));
 
 	groups_button = memnew(Button);
-	groups_button->set_theme_type_variation("FlatButton");
+	groups_button->set_theme_type_variation(SceneStringName(FlatButton));
 	groups_button->set_text(TTR("Groups"));
 	groups_button->set_toggle_mode(true);
 	groups_button->set_pressed(false);
diff --git a/editor/plugins/abstract_polygon_2d_editor.cpp b/editor/plugins/abstract_polygon_2d_editor.cpp
index 01154ee6b0..a2668bb825 100644
--- a/editor/plugins/abstract_polygon_2d_editor.cpp
+++ b/editor/plugins/abstract_polygon_2d_editor.cpp
@@ -727,19 +727,19 @@ AbstractPolygon2DEditor::AbstractPolygon2DEditor(bool p_wip_destructive) {
 	edge_point = PosVertex();
 
 	button_create = memnew(Button);
-	button_create->set_theme_type_variation("FlatButton");
+	button_create->set_theme_type_variation(SceneStringName(FlatButton));
 	add_child(button_create);
 	button_create->connect(SceneStringName(pressed), callable_mp(this, &AbstractPolygon2DEditor::_menu_option).bind(MODE_CREATE));
 	button_create->set_toggle_mode(true);
 
 	button_edit = memnew(Button);
-	button_edit->set_theme_type_variation("FlatButton");
+	button_edit->set_theme_type_variation(SceneStringName(FlatButton));
 	add_child(button_edit);
 	button_edit->connect(SceneStringName(pressed), callable_mp(this, &AbstractPolygon2DEditor::_menu_option).bind(MODE_EDIT));
 	button_edit->set_toggle_mode(true);
 
 	button_delete = memnew(Button);
-	button_delete->set_theme_type_variation("FlatButton");
+	button_delete->set_theme_type_variation(SceneStringName(FlatButton));
 	add_child(button_delete);
 	button_delete->connect(SceneStringName(pressed), callable_mp(this, &AbstractPolygon2DEditor::_menu_option).bind(MODE_DELETE));
 	button_delete->set_toggle_mode(true);
diff --git a/editor/plugins/animation_blend_space_1d_editor.cpp b/editor/plugins/animation_blend_space_1d_editor.cpp
index 3f534eebc5..79b603bbdf 100644
--- a/editor/plugins/animation_blend_space_1d_editor.cpp
+++ b/editor/plugins/animation_blend_space_1d_editor.cpp
@@ -661,7 +661,7 @@ AnimationNodeBlendSpace1DEditor::AnimationNodeBlendSpace1DEditor() {
 	bg.instantiate();
 
 	tool_blend = memnew(Button);
-	tool_blend->set_theme_type_variation("FlatButton");
+	tool_blend->set_theme_type_variation(SceneStringName(FlatButton));
 	tool_blend->set_toggle_mode(true);
 	tool_blend->set_button_group(bg);
 	top_hb->add_child(tool_blend);
@@ -670,7 +670,7 @@ AnimationNodeBlendSpace1DEditor::AnimationNodeBlendSpace1DEditor() {
 	tool_blend->connect(SceneStringName(pressed), callable_mp(this, &AnimationNodeBlendSpace1DEditor::_tool_switch).bind(3));
 
 	tool_select = memnew(Button);
-	tool_select->set_theme_type_variation("FlatButton");
+	tool_select->set_theme_type_variation(SceneStringName(FlatButton));
 	tool_select->set_toggle_mode(true);
 	tool_select->set_button_group(bg);
 	top_hb->add_child(tool_select);
@@ -678,7 +678,7 @@ AnimationNodeBlendSpace1DEditor::AnimationNodeBlendSpace1DEditor() {
 	tool_select->connect(SceneStringName(pressed), callable_mp(this, &AnimationNodeBlendSpace1DEditor::_tool_switch).bind(0));
 
 	tool_create = memnew(Button);
-	tool_create->set_theme_type_variation("FlatButton");
+	tool_create->set_theme_type_variation(SceneStringName(FlatButton));
 	tool_create->set_toggle_mode(true);
 	tool_create->set_button_group(bg);
 	top_hb->add_child(tool_create);
@@ -688,7 +688,7 @@ AnimationNodeBlendSpace1DEditor::AnimationNodeBlendSpace1DEditor() {
 	tool_erase_sep = memnew(VSeparator);
 	top_hb->add_child(tool_erase_sep);
 	tool_erase = memnew(Button);
-	tool_erase->set_theme_type_variation("FlatButton");
+	tool_erase->set_theme_type_variation(SceneStringName(FlatButton));
 	top_hb->add_child(tool_erase);
 	tool_erase->set_tooltip_text(TTR("Erase points."));
 	tool_erase->connect(SceneStringName(pressed), callable_mp(this, &AnimationNodeBlendSpace1DEditor::_erase_selected));
@@ -696,7 +696,7 @@ AnimationNodeBlendSpace1DEditor::AnimationNodeBlendSpace1DEditor() {
 	top_hb->add_child(memnew(VSeparator));
 
 	snap = memnew(Button);
-	snap->set_theme_type_variation("FlatButton");
+	snap->set_theme_type_variation(SceneStringName(FlatButton));
 	snap->set_toggle_mode(true);
 	top_hb->add_child(snap);
 	snap->set_pressed(true);
diff --git a/editor/plugins/animation_blend_space_2d_editor.cpp b/editor/plugins/animation_blend_space_2d_editor.cpp
index ba2dad5880..a43fc494d8 100644
--- a/editor/plugins/animation_blend_space_2d_editor.cpp
+++ b/editor/plugins/animation_blend_space_2d_editor.cpp
@@ -882,7 +882,7 @@ AnimationNodeBlendSpace2DEditor::AnimationNodeBlendSpace2DEditor() {
 	bg.instantiate();
 
 	tool_blend = memnew(Button);
-	tool_blend->set_theme_type_variation("FlatButton");
+	tool_blend->set_theme_type_variation(SceneStringName(FlatButton));
 	tool_blend->set_toggle_mode(true);
 	tool_blend->set_button_group(bg);
 	top_hb->add_child(tool_blend);
@@ -891,7 +891,7 @@ AnimationNodeBlendSpace2DEditor::AnimationNodeBlendSpace2DEditor() {
 	tool_blend->connect(SceneStringName(pressed), callable_mp(this, &AnimationNodeBlendSpace2DEditor::_tool_switch).bind(3));
 
 	tool_select = memnew(Button);
-	tool_select->set_theme_type_variation("FlatButton");
+	tool_select->set_theme_type_variation(SceneStringName(FlatButton));
 	tool_select->set_toggle_mode(true);
 	tool_select->set_button_group(bg);
 	top_hb->add_child(tool_select);
@@ -899,7 +899,7 @@ AnimationNodeBlendSpace2DEditor::AnimationNodeBlendSpace2DEditor() {
 	tool_select->connect(SceneStringName(pressed), callable_mp(this, &AnimationNodeBlendSpace2DEditor::_tool_switch).bind(0));
 
 	tool_create = memnew(Button);
-	tool_create->set_theme_type_variation("FlatButton");
+	tool_create->set_theme_type_variation(SceneStringName(FlatButton));
 	tool_create->set_toggle_mode(true);
 	tool_create->set_button_group(bg);
 	top_hb->add_child(tool_create);
@@ -907,7 +907,7 @@ AnimationNodeBlendSpace2DEditor::AnimationNodeBlendSpace2DEditor() {
 	tool_create->connect(SceneStringName(pressed), callable_mp(this, &AnimationNodeBlendSpace2DEditor::_tool_switch).bind(1));
 
 	tool_triangle = memnew(Button);
-	tool_triangle->set_theme_type_variation("FlatButton");
+	tool_triangle->set_theme_type_variation(SceneStringName(FlatButton));
 	tool_triangle->set_toggle_mode(true);
 	tool_triangle->set_button_group(bg);
 	top_hb->add_child(tool_triangle);
@@ -917,7 +917,7 @@ AnimationNodeBlendSpace2DEditor::AnimationNodeBlendSpace2DEditor() {
 	tool_erase_sep = memnew(VSeparator);
 	top_hb->add_child(tool_erase_sep);
 	tool_erase = memnew(Button);
-	tool_erase->set_theme_type_variation("FlatButton");
+	tool_erase->set_theme_type_variation(SceneStringName(FlatButton));
 	top_hb->add_child(tool_erase);
 	tool_erase->set_tooltip_text(TTR("Erase points and triangles."));
 	tool_erase->connect(SceneStringName(pressed), callable_mp(this, &AnimationNodeBlendSpace2DEditor::_erase_selected));
@@ -926,7 +926,7 @@ AnimationNodeBlendSpace2DEditor::AnimationNodeBlendSpace2DEditor() {
 	top_hb->add_child(memnew(VSeparator));
 
 	auto_triangles = memnew(Button);
-	auto_triangles->set_theme_type_variation("FlatButton");
+	auto_triangles->set_theme_type_variation(SceneStringName(FlatButton));
 	top_hb->add_child(auto_triangles);
 	auto_triangles->connect(SceneStringName(pressed), callable_mp(this, &AnimationNodeBlendSpace2DEditor::_auto_triangles_toggled));
 	auto_triangles->set_toggle_mode(true);
@@ -935,7 +935,7 @@ AnimationNodeBlendSpace2DEditor::AnimationNodeBlendSpace2DEditor() {
 	top_hb->add_child(memnew(VSeparator));
 
 	snap = memnew(Button);
-	snap->set_theme_type_variation("FlatButton");
+	snap->set_theme_type_variation(SceneStringName(FlatButton));
 	snap->set_toggle_mode(true);
 	top_hb->add_child(snap);
 	snap->set_pressed(true);
diff --git a/editor/plugins/animation_player_editor_plugin.cpp b/editor/plugins/animation_player_editor_plugin.cpp
index 108b0b1661..eb11b033e9 100644
--- a/editor/plugins/animation_player_editor_plugin.cpp
+++ b/editor/plugins/animation_player_editor_plugin.cpp
@@ -2013,27 +2013,27 @@ AnimationPlayerEditor::AnimationPlayerEditor(AnimationPlayerEditorPlugin *p_plug
 	hb->add_child(playback_container);
 
 	play_bw_from = memnew(Button);
-	play_bw_from->set_theme_type_variation("FlatButton");
+	play_bw_from->set_theme_type_variation(SceneStringName(FlatButton));
 	play_bw_from->set_tooltip_text(TTR("Play Animation Backwards"));
 	playback_container->add_child(play_bw_from);
 
 	play_bw = memnew(Button);
-	play_bw->set_theme_type_variation("FlatButton");
+	play_bw->set_theme_type_variation(SceneStringName(FlatButton));
 	play_bw->set_tooltip_text(TTR("Play Animation Backwards from End"));
 	playback_container->add_child(play_bw);
 
 	stop = memnew(Button);
-	stop->set_theme_type_variation("FlatButton");
+	stop->set_theme_type_variation(SceneStringName(FlatButton));
 	stop->set_tooltip_text(TTR("Pause/Stop Animation"));
 	playback_container->add_child(stop);
 
 	play = memnew(Button);
-	play->set_theme_type_variation("FlatButton");
+	play->set_theme_type_variation(SceneStringName(FlatButton));
 	play->set_tooltip_text(TTR("Play Animation from Start"));
 	playback_container->add_child(play);
 
 	play_from = memnew(Button);
-	play_from->set_theme_type_variation("FlatButton");
+	play_from->set_theme_type_variation(SceneStringName(FlatButton));
 	play_from->set_tooltip_text(TTR("Play Animation"));
 	playback_container->add_child(play_from);
 
@@ -2084,7 +2084,7 @@ AnimationPlayerEditor::AnimationPlayerEditor(AnimationPlayerEditorPlugin *p_plug
 	animation->set_auto_translate_mode(AUTO_TRANSLATE_MODE_DISABLED);
 
 	autoplay = memnew(Button);
-	autoplay->set_theme_type_variation("FlatButton");
+	autoplay->set_theme_type_variation(SceneStringName(FlatButton));
 	hb->add_child(autoplay);
 	autoplay->set_tooltip_text(TTR("Autoplay on Load"));
 
@@ -2096,7 +2096,7 @@ AnimationPlayerEditor::AnimationPlayerEditor(AnimationPlayerEditorPlugin *p_plug
 	hb->add_child(memnew(VSeparator));
 
 	onion_toggle = memnew(Button);
-	onion_toggle->set_theme_type_variation("FlatButton");
+	onion_toggle->set_theme_type_variation(SceneStringName(FlatButton));
 	onion_toggle->set_toggle_mode(true);
 	onion_toggle->set_tooltip_text(TTR("Enable Onion Skinning"));
 	onion_toggle->connect(SceneStringName(pressed), callable_mp(this, &AnimationPlayerEditor::_onion_skinning_menu).bind(ONION_SKINNING_ENABLE));
@@ -2126,7 +2126,7 @@ AnimationPlayerEditor::AnimationPlayerEditor(AnimationPlayerEditorPlugin *p_plug
 	hb->add_child(memnew(VSeparator));
 
 	pin = memnew(Button);
-	pin->set_theme_type_variation("FlatButton");
+	pin->set_theme_type_variation(SceneStringName(FlatButton));
 	pin->set_toggle_mode(true);
 	pin->set_tooltip_text(TTR("Pin AnimationPlayer"));
 	hb->add_child(pin);
diff --git a/editor/plugins/animation_state_machine_editor.cpp b/editor/plugins/animation_state_machine_editor.cpp
index f75030b1ac..17dc158a04 100644
--- a/editor/plugins/animation_state_machine_editor.cpp
+++ b/editor/plugins/animation_state_machine_editor.cpp
@@ -1821,7 +1821,7 @@ AnimationNodeStateMachineEditor::AnimationNodeStateMachineEditor() {
 	bg.instantiate();
 
 	tool_select = memnew(Button);
-	tool_select->set_theme_type_variation("FlatButton");
+	tool_select->set_theme_type_variation(SceneStringName(FlatButton));
 	top_hb->add_child(tool_select);
 	tool_select->set_toggle_mode(true);
 	tool_select->set_button_group(bg);
@@ -1830,7 +1830,7 @@ AnimationNodeStateMachineEditor::AnimationNodeStateMachineEditor() {
 	tool_select->connect(SceneStringName(pressed), callable_mp(this, &AnimationNodeStateMachineEditor::_update_mode), CONNECT_DEFERRED);
 
 	tool_create = memnew(Button);
-	tool_create->set_theme_type_variation("FlatButton");
+	tool_create->set_theme_type_variation(SceneStringName(FlatButton));
 	top_hb->add_child(tool_create);
 	tool_create->set_toggle_mode(true);
 	tool_create->set_button_group(bg);
@@ -1838,7 +1838,7 @@ AnimationNodeStateMachineEditor::AnimationNodeStateMachineEditor() {
 	tool_create->connect(SceneStringName(pressed), callable_mp(this, &AnimationNodeStateMachineEditor::_update_mode), CONNECT_DEFERRED);
 
 	tool_connect = memnew(Button);
-	tool_connect->set_theme_type_variation("FlatButton");
+	tool_connect->set_theme_type_variation(SceneStringName(FlatButton));
 	top_hb->add_child(tool_connect);
 	tool_connect->set_toggle_mode(true);
 	tool_connect->set_button_group(bg);
@@ -1851,7 +1851,7 @@ AnimationNodeStateMachineEditor::AnimationNodeStateMachineEditor() {
 	selection_tools_hb->add_child(memnew(VSeparator));
 
 	tool_erase = memnew(Button);
-	tool_erase->set_theme_type_variation("FlatButton");
+	tool_erase->set_theme_type_variation(SceneStringName(FlatButton));
 	tool_erase->set_tooltip_text(TTR("Remove selected node or transition."));
 	tool_erase->connect(SceneStringName(pressed), callable_mp(this, &AnimationNodeStateMachineEditor::_erase_selected).bind(false));
 	tool_erase->set_disabled(true);
@@ -1866,7 +1866,7 @@ AnimationNodeStateMachineEditor::AnimationNodeStateMachineEditor() {
 	transition_tools_hb->add_child(switch_mode);
 
 	auto_advance = memnew(Button);
-	auto_advance->set_theme_type_variation("FlatButton");
+	auto_advance->set_theme_type_variation(SceneStringName(FlatButton));
 	auto_advance->set_tooltip_text(TTR("New Transitions Should Auto Advance"));
 	auto_advance->set_toggle_mode(true);
 	auto_advance->set_pressed(true);
diff --git a/editor/plugins/canvas_item_editor_plugin.cpp b/editor/plugins/canvas_item_editor_plugin.cpp
index 5b948d9b7c..094b14722e 100644
--- a/editor/plugins/canvas_item_editor_plugin.cpp
+++ b/editor/plugins/canvas_item_editor_plugin.cpp
@@ -5369,7 +5369,7 @@ CanvasItemEditor::CanvasItemEditor() {
 	viewport->add_child(controls_vb);
 
 	select_button = memnew(Button);
-	select_button->set_theme_type_variation("FlatButton");
+	select_button->set_theme_type_variation(SceneStringName(FlatButton));
 	main_menu_hbox->add_child(select_button);
 	select_button->set_toggle_mode(true);
 	select_button->connect(SceneStringName(pressed), callable_mp(this, &CanvasItemEditor::_button_tool_select).bind(TOOL_SELECT));
@@ -5381,7 +5381,7 @@ CanvasItemEditor::CanvasItemEditor() {
 	main_menu_hbox->add_child(memnew(VSeparator));
 
 	move_button = memnew(Button);
-	move_button->set_theme_type_variation("FlatButton");
+	move_button->set_theme_type_variation(SceneStringName(FlatButton));
 	main_menu_hbox->add_child(move_button);
 	move_button->set_toggle_mode(true);
 	move_button->connect(SceneStringName(pressed), callable_mp(this, &CanvasItemEditor::_button_tool_select).bind(TOOL_MOVE));
@@ -5390,7 +5390,7 @@ CanvasItemEditor::CanvasItemEditor() {
 	move_button->set_tooltip_text(TTR("Move Mode"));
 
 	rotate_button = memnew(Button);
-	rotate_button->set_theme_type_variation("FlatButton");
+	rotate_button->set_theme_type_variation(SceneStringName(FlatButton));
 	main_menu_hbox->add_child(rotate_button);
 	rotate_button->set_toggle_mode(true);
 	rotate_button->connect(SceneStringName(pressed), callable_mp(this, &CanvasItemEditor::_button_tool_select).bind(TOOL_ROTATE));
@@ -5399,7 +5399,7 @@ CanvasItemEditor::CanvasItemEditor() {
 	rotate_button->set_tooltip_text(TTR("Rotate Mode"));
 
 	scale_button = memnew(Button);
-	scale_button->set_theme_type_variation("FlatButton");
+	scale_button->set_theme_type_variation(SceneStringName(FlatButton));
 	main_menu_hbox->add_child(scale_button);
 	scale_button->set_toggle_mode(true);
 	scale_button->connect(SceneStringName(pressed), callable_mp(this, &CanvasItemEditor::_button_tool_select).bind(TOOL_SCALE));
@@ -5410,21 +5410,21 @@ CanvasItemEditor::CanvasItemEditor() {
 	main_menu_hbox->add_child(memnew(VSeparator));
 
 	list_select_button = memnew(Button);
-	list_select_button->set_theme_type_variation("FlatButton");
+	list_select_button->set_theme_type_variation(SceneStringName(FlatButton));
 	main_menu_hbox->add_child(list_select_button);
 	list_select_button->set_toggle_mode(true);
 	list_select_button->connect(SceneStringName(pressed), callable_mp(this, &CanvasItemEditor::_button_tool_select).bind(TOOL_LIST_SELECT));
 	list_select_button->set_tooltip_text(TTR("Show list of selectable nodes at position clicked."));
 
 	pivot_button = memnew(Button);
-	pivot_button->set_theme_type_variation("FlatButton");
+	pivot_button->set_theme_type_variation(SceneStringName(FlatButton));
 	main_menu_hbox->add_child(pivot_button);
 	pivot_button->set_toggle_mode(true);
 	pivot_button->connect(SceneStringName(pressed), callable_mp(this, &CanvasItemEditor::_button_tool_select).bind(TOOL_EDIT_PIVOT));
 	pivot_button->set_tooltip_text(TTR("Click to change object's pivot.") + "\n" + TTR("Shift: Set temporary pivot.") + "\n" + TTR("Click this button while holding Shift to put the temporary pivot in the center of the selected nodes."));
 
 	pan_button = memnew(Button);
-	pan_button->set_theme_type_variation("FlatButton");
+	pan_button->set_theme_type_variation(SceneStringName(FlatButton));
 	main_menu_hbox->add_child(pan_button);
 	pan_button->set_toggle_mode(true);
 	pan_button->connect(SceneStringName(pressed), callable_mp(this, &CanvasItemEditor::_button_tool_select).bind(TOOL_PAN));
@@ -5433,7 +5433,7 @@ CanvasItemEditor::CanvasItemEditor() {
 	pan_button->set_tooltip_text(TTR("You can also use Pan View shortcut (Space by default) to pan in any mode."));
 
 	ruler_button = memnew(Button);
-	ruler_button->set_theme_type_variation("FlatButton");
+	ruler_button->set_theme_type_variation(SceneStringName(FlatButton));
 	main_menu_hbox->add_child(ruler_button);
 	ruler_button->set_toggle_mode(true);
 	ruler_button->connect(SceneStringName(pressed), callable_mp(this, &CanvasItemEditor::_button_tool_select).bind(TOOL_RULER));
@@ -5444,7 +5444,7 @@ CanvasItemEditor::CanvasItemEditor() {
 	main_menu_hbox->add_child(memnew(VSeparator));
 
 	smart_snap_button = memnew(Button);
-	smart_snap_button->set_theme_type_variation("FlatButton");
+	smart_snap_button->set_theme_type_variation(SceneStringName(FlatButton));
 	main_menu_hbox->add_child(smart_snap_button);
 	smart_snap_button->set_toggle_mode(true);
 	smart_snap_button->connect(SceneStringName(toggled), callable_mp(this, &CanvasItemEditor::_button_toggle_smart_snap));
@@ -5453,7 +5453,7 @@ CanvasItemEditor::CanvasItemEditor() {
 	smart_snap_button->set_shortcut_context(this);
 
 	grid_snap_button = memnew(Button);
-	grid_snap_button->set_theme_type_variation("FlatButton");
+	grid_snap_button->set_theme_type_variation(SceneStringName(FlatButton));
 	main_menu_hbox->add_child(grid_snap_button);
 	grid_snap_button->set_toggle_mode(true);
 	grid_snap_button->connect(SceneStringName(toggled), callable_mp(this, &CanvasItemEditor::_button_toggle_grid_snap));
@@ -5495,7 +5495,7 @@ CanvasItemEditor::CanvasItemEditor() {
 	main_menu_hbox->add_child(memnew(VSeparator));
 
 	lock_button = memnew(Button);
-	lock_button->set_theme_type_variation("FlatButton");
+	lock_button->set_theme_type_variation(SceneStringName(FlatButton));
 	main_menu_hbox->add_child(lock_button);
 
 	lock_button->connect(SceneStringName(pressed), callable_mp(this, &CanvasItemEditor::_popup_callback).bind(LOCK_SELECTED));
@@ -5504,7 +5504,7 @@ CanvasItemEditor::CanvasItemEditor() {
 	lock_button->set_shortcut(ED_GET_SHORTCUT("editor/lock_selected_nodes"));
 
 	unlock_button = memnew(Button);
-	unlock_button->set_theme_type_variation("FlatButton");
+	unlock_button->set_theme_type_variation(SceneStringName(FlatButton));
 	main_menu_hbox->add_child(unlock_button);
 	unlock_button->connect(SceneStringName(pressed), callable_mp(this, &CanvasItemEditor::_popup_callback).bind(UNLOCK_SELECTED));
 	unlock_button->set_tooltip_text(TTR("Unlock selected node, allowing selection and movement."));
@@ -5512,7 +5512,7 @@ CanvasItemEditor::CanvasItemEditor() {
 	unlock_button->set_shortcut(ED_GET_SHORTCUT("editor/unlock_selected_nodes"));
 
 	group_button = memnew(Button);
-	group_button->set_theme_type_variation("FlatButton");
+	group_button->set_theme_type_variation(SceneStringName(FlatButton));
 	main_menu_hbox->add_child(group_button);
 	group_button->connect(SceneStringName(pressed), callable_mp(this, &CanvasItemEditor::_popup_callback).bind(GROUP_SELECTED));
 	group_button->set_tooltip_text(TTR("Groups the selected node with its children. This causes the parent to be selected when any child node is clicked in 2D and 3D view."));
@@ -5520,7 +5520,7 @@ CanvasItemEditor::CanvasItemEditor() {
 	group_button->set_shortcut(ED_GET_SHORTCUT("editor/group_selected_nodes"));
 
 	ungroup_button = memnew(Button);
-	ungroup_button->set_theme_type_variation("FlatButton");
+	ungroup_button->set_theme_type_variation(SceneStringName(FlatButton));
 	main_menu_hbox->add_child(ungroup_button);
 	ungroup_button->connect(SceneStringName(pressed), callable_mp(this, &CanvasItemEditor::_popup_callback).bind(UNGROUP_SELECTED));
 	ungroup_button->set_tooltip_text(TTR("Ungroups the selected node from its children. Child nodes will be individual items in 2D and 3D view."));
@@ -5621,7 +5621,7 @@ CanvasItemEditor::CanvasItemEditor() {
 	animation_hb->hide();
 
 	key_loc_button = memnew(Button);
-	key_loc_button->set_theme_type_variation("FlatButton");
+	key_loc_button->set_theme_type_variation(SceneStringName(FlatButton));
 	key_loc_button->set_toggle_mode(true);
 	key_loc_button->set_pressed(true);
 	key_loc_button->set_focus_mode(FOCUS_NONE);
@@ -5630,7 +5630,7 @@ CanvasItemEditor::CanvasItemEditor() {
 	animation_hb->add_child(key_loc_button);
 
 	key_rot_button = memnew(Button);
-	key_rot_button->set_theme_type_variation("FlatButton");
+	key_rot_button->set_theme_type_variation(SceneStringName(FlatButton));
 	key_rot_button->set_toggle_mode(true);
 	key_rot_button->set_pressed(true);
 	key_rot_button->set_focus_mode(FOCUS_NONE);
@@ -5639,7 +5639,7 @@ CanvasItemEditor::CanvasItemEditor() {
 	animation_hb->add_child(key_rot_button);
 
 	key_scale_button = memnew(Button);
-	key_scale_button->set_theme_type_variation("FlatButton");
+	key_scale_button->set_theme_type_variation(SceneStringName(FlatButton));
 	key_scale_button->set_toggle_mode(true);
 	key_scale_button->set_focus_mode(FOCUS_NONE);
 	key_scale_button->connect(SceneStringName(pressed), callable_mp(this, &CanvasItemEditor::_popup_callback).bind(ANIM_INSERT_SCALE));
@@ -5647,7 +5647,7 @@ CanvasItemEditor::CanvasItemEditor() {
 	animation_hb->add_child(key_scale_button);
 
 	key_insert_button = memnew(Button);
-	key_insert_button->set_theme_type_variation("FlatButton");
+	key_insert_button->set_theme_type_variation(SceneStringName(FlatButton));
 	key_insert_button->set_focus_mode(FOCUS_NONE);
 	key_insert_button->connect(SceneStringName(pressed), callable_mp(this, &CanvasItemEditor::_popup_callback).bind(ANIM_INSERT_KEY));
 	key_insert_button->set_tooltip_text(TTR("Insert keys (based on mask)."));
@@ -5656,7 +5656,7 @@ CanvasItemEditor::CanvasItemEditor() {
 	animation_hb->add_child(key_insert_button);
 
 	key_auto_insert_button = memnew(Button);
-	key_auto_insert_button->set_theme_type_variation("FlatButton");
+	key_auto_insert_button->set_theme_type_variation(SceneStringName(FlatButton));
 	key_auto_insert_button->set_toggle_mode(true);
 	key_auto_insert_button->set_focus_mode(FOCUS_NONE);
 	key_auto_insert_button->set_tooltip_text(TTR("Auto insert keys when objects are translated, rotated or scaled (based on mask).\nKeys are only added to existing tracks, no new tracks will be created.\nKeys must be inserted manually for the first time."));
diff --git a/editor/plugins/control_editor_plugin.cpp b/editor/plugins/control_editor_plugin.cpp
index a52d949819..e3ac775185 100644
--- a/editor/plugins/control_editor_plugin.cpp
+++ b/editor/plugins/control_editor_plugin.cpp
@@ -531,7 +531,7 @@ void ControlEditorPopupButton::_notification(int p_what) {
 }
 
 ControlEditorPopupButton::ControlEditorPopupButton() {
-	set_theme_type_variation("FlatButton");
+	set_theme_type_variation(SceneStringName(FlatButton));
 	set_toggle_mode(true);
 	set_focus_mode(FOCUS_NONE);
 
@@ -1081,7 +1081,7 @@ ControlEditorToolbar::ControlEditorToolbar() {
 	keep_ratio_button->connect(SceneStringName(pressed), callable_mp(this, &ControlEditorToolbar::_anchors_to_current_ratio));
 
 	anchor_mode_button = memnew(Button);
-	anchor_mode_button->set_theme_type_variation("FlatButton");
+	anchor_mode_button->set_theme_type_variation(SceneStringName(FlatButton));
 	anchor_mode_button->set_toggle_mode(true);
 	anchor_mode_button->set_tooltip_text(TTR("When active, moving Control nodes changes their anchors instead of their offsets."));
 	add_child(anchor_mode_button);
diff --git a/editor/plugins/game_view_plugin.cpp b/editor/plugins/game_view_plugin.cpp
index 5c1f81ee94..b94b4a41e8 100644
--- a/editor/plugins/game_view_plugin.cpp
+++ b/editor/plugins/game_view_plugin.cpp
@@ -346,13 +346,13 @@ GameView::GameView(Ref<GameViewDebugger> p_debugger) {
 	suspend_button = memnew(Button);
 	main_menu_hbox->add_child(suspend_button);
 	suspend_button->set_toggle_mode(true);
-	suspend_button->set_theme_type_variation("FlatButton");
+	suspend_button->set_theme_type_variation(SceneStringName(FlatButton));
 	suspend_button->connect(SceneStringName(toggled), callable_mp(this, &GameView::_suspend_button_toggled));
 	suspend_button->set_tooltip_text(TTR("Suspend"));
 
 	next_frame_button = memnew(Button);
 	main_menu_hbox->add_child(next_frame_button);
-	next_frame_button->set_theme_type_variation("FlatButton");
+	next_frame_button->set_theme_type_variation(SceneStringName(FlatButton));
 	next_frame_button->connect(SceneStringName(pressed), callable_mp(*debugger, &GameViewDebugger::next_frame));
 	next_frame_button->set_tooltip_text(TTR("Next Frame"));
 
@@ -363,7 +363,7 @@ GameView::GameView(Ref<GameViewDebugger> p_debugger) {
 	node_type_button[RuntimeNodeSelect::NODE_TYPE_NONE]->set_text(TTR("Input"));
 	node_type_button[RuntimeNodeSelect::NODE_TYPE_NONE]->set_toggle_mode(true);
 	node_type_button[RuntimeNodeSelect::NODE_TYPE_NONE]->set_pressed(true);
-	node_type_button[RuntimeNodeSelect::NODE_TYPE_NONE]->set_theme_type_variation("FlatButton");
+	node_type_button[RuntimeNodeSelect::NODE_TYPE_NONE]->set_theme_type_variation(SceneStringName(FlatButton));
 	node_type_button[RuntimeNodeSelect::NODE_TYPE_NONE]->connect(SceneStringName(pressed), callable_mp(this, &GameView::_node_type_pressed).bind(RuntimeNodeSelect::NODE_TYPE_NONE));
 	node_type_button[RuntimeNodeSelect::NODE_TYPE_NONE]->set_tooltip_text(TTR("Allow game input."));
 
@@ -371,7 +371,7 @@ GameView::GameView(Ref<GameViewDebugger> p_debugger) {
 	main_menu_hbox->add_child(node_type_button[RuntimeNodeSelect::NODE_TYPE_2D]);
 	node_type_button[RuntimeNodeSelect::NODE_TYPE_2D]->set_text(TTR("2D"));
 	node_type_button[RuntimeNodeSelect::NODE_TYPE_2D]->set_toggle_mode(true);
-	node_type_button[RuntimeNodeSelect::NODE_TYPE_2D]->set_theme_type_variation("FlatButton");
+	node_type_button[RuntimeNodeSelect::NODE_TYPE_2D]->set_theme_type_variation(SceneStringName(FlatButton));
 	node_type_button[RuntimeNodeSelect::NODE_TYPE_2D]->connect(SceneStringName(pressed), callable_mp(this, &GameView::_node_type_pressed).bind(RuntimeNodeSelect::NODE_TYPE_2D));
 	node_type_button[RuntimeNodeSelect::NODE_TYPE_2D]->set_tooltip_text(TTR("Disable game input and allow to select Node2Ds, Controls, and manipulate the 2D camera."));
 
@@ -380,7 +380,7 @@ GameView::GameView(Ref<GameViewDebugger> p_debugger) {
 	main_menu_hbox->add_child(node_type_button[RuntimeNodeSelect::NODE_TYPE_3D]);
 	node_type_button[RuntimeNodeSelect::NODE_TYPE_3D]->set_text(TTR("3D"));
 	node_type_button[RuntimeNodeSelect::NODE_TYPE_3D]->set_toggle_mode(true);
-	node_type_button[RuntimeNodeSelect::NODE_TYPE_3D]->set_theme_type_variation("FlatButton");
+	node_type_button[RuntimeNodeSelect::NODE_TYPE_3D]->set_theme_type_variation(SceneStringName(FlatButton));
 	node_type_button[RuntimeNodeSelect::NODE_TYPE_3D]->connect(SceneStringName(pressed), callable_mp(this, &GameView::_node_type_pressed).bind(RuntimeNodeSelect::NODE_TYPE_3D));
 	node_type_button[RuntimeNodeSelect::NODE_TYPE_3D]->set_tooltip_text(TTR("Disable game input and allow to select Node3Ds and manipulate the 3D camera."));
 #endif // _3D_DISABLED
@@ -390,7 +390,7 @@ GameView::GameView(Ref<GameViewDebugger> p_debugger) {
 	hide_selection = memnew(Button);
 	main_menu_hbox->add_child(hide_selection);
 	hide_selection->set_toggle_mode(true);
-	hide_selection->set_theme_type_variation("FlatButton");
+	hide_selection->set_theme_type_variation(SceneStringName(FlatButton));
 	hide_selection->connect(SceneStringName(toggled), callable_mp(this, &GameView::_hide_selection_toggled));
 	hide_selection->set_tooltip_text(TTR("Toggle Selection Visibility"));
 
@@ -400,7 +400,7 @@ GameView::GameView(Ref<GameViewDebugger> p_debugger) {
 	main_menu_hbox->add_child(select_mode_button[RuntimeNodeSelect::SELECT_MODE_SINGLE]);
 	select_mode_button[RuntimeNodeSelect::SELECT_MODE_SINGLE]->set_toggle_mode(true);
 	select_mode_button[RuntimeNodeSelect::SELECT_MODE_SINGLE]->set_pressed(true);
-	select_mode_button[RuntimeNodeSelect::SELECT_MODE_SINGLE]->set_theme_type_variation("FlatButton");
+	select_mode_button[RuntimeNodeSelect::SELECT_MODE_SINGLE]->set_theme_type_variation(SceneStringName(FlatButton));
 	select_mode_button[RuntimeNodeSelect::SELECT_MODE_SINGLE]->connect(SceneStringName(pressed), callable_mp(this, &GameView::_select_mode_pressed).bind(RuntimeNodeSelect::SELECT_MODE_SINGLE));
 	select_mode_button[RuntimeNodeSelect::SELECT_MODE_SINGLE]->set_shortcut(ED_SHORTCUT("spatial_editor/tool_select", TTR("Select Mode"), Key::Q));
 	select_mode_button[RuntimeNodeSelect::SELECT_MODE_SINGLE]->set_shortcut_context(this);
@@ -409,7 +409,7 @@ GameView::GameView(Ref<GameViewDebugger> p_debugger) {
 	select_mode_button[RuntimeNodeSelect::SELECT_MODE_LIST] = memnew(Button);
 	main_menu_hbox->add_child(select_mode_button[RuntimeNodeSelect::SELECT_MODE_LIST]);
 	select_mode_button[RuntimeNodeSelect::SELECT_MODE_LIST]->set_toggle_mode(true);
-	select_mode_button[RuntimeNodeSelect::SELECT_MODE_LIST]->set_theme_type_variation("FlatButton");
+	select_mode_button[RuntimeNodeSelect::SELECT_MODE_LIST]->set_theme_type_variation(SceneStringName(FlatButton));
 	select_mode_button[RuntimeNodeSelect::SELECT_MODE_LIST]->connect(SceneStringName(pressed), callable_mp(this, &GameView::_select_mode_pressed).bind(RuntimeNodeSelect::SELECT_MODE_LIST));
 	select_mode_button[RuntimeNodeSelect::SELECT_MODE_LIST]->set_tooltip_text(TTR("Show list of selectable nodes at position clicked."));
 
@@ -418,7 +418,7 @@ GameView::GameView(Ref<GameViewDebugger> p_debugger) {
 	camera_override_button = memnew(Button);
 	main_menu_hbox->add_child(camera_override_button);
 	camera_override_button->set_toggle_mode(true);
-	camera_override_button->set_theme_type_variation("FlatButton");
+	camera_override_button->set_theme_type_variation(SceneStringName(FlatButton));
 	camera_override_button->connect(SceneStringName(toggled), callable_mp(this, &GameView::_camera_override_button_toggled));
 	camera_override_button->set_tooltip_text(TTR("Override the in-game camera."));
 
diff --git a/editor/plugins/gpu_particles_collision_sdf_editor_plugin.cpp b/editor/plugins/gpu_particles_collision_sdf_editor_plugin.cpp
index 4c2a3b60f8..1fa9f672c5 100644
--- a/editor/plugins/gpu_particles_collision_sdf_editor_plugin.cpp
+++ b/editor/plugins/gpu_particles_collision_sdf_editor_plugin.cpp
@@ -180,7 +180,7 @@ GPUParticlesCollisionSDF3DEditorPlugin::GPUParticlesCollisionSDF3DEditorPlugin()
 	bake_hb->set_h_size_flags(Control::SIZE_EXPAND_FILL);
 	bake_hb->hide();
 	bake = memnew(Button);
-	bake->set_theme_type_variation("FlatButton");
+	bake->set_theme_type_variation(SceneStringName(FlatButton));
 	bake->set_button_icon(EditorNode::get_singleton()->get_editor_theme()->get_icon(SNAME("Bake"), EditorStringName(EditorIcons)));
 	bake->set_text(TTR("Bake SDF"));
 	bake->connect(SceneStringName(pressed), callable_mp(this, &GPUParticlesCollisionSDF3DEditorPlugin::_bake));
diff --git a/editor/plugins/lightmap_gi_editor_plugin.cpp b/editor/plugins/lightmap_gi_editor_plugin.cpp
index 3f21d5d11c..024a496707 100644
--- a/editor/plugins/lightmap_gi_editor_plugin.cpp
+++ b/editor/plugins/lightmap_gi_editor_plugin.cpp
@@ -178,7 +178,7 @@ void LightmapGIEditorPlugin::_bind_methods() {
 
 LightmapGIEditorPlugin::LightmapGIEditorPlugin() {
 	bake = memnew(Button);
-	bake->set_theme_type_variation("FlatButton");
+	bake->set_theme_type_variation(SceneStringName(FlatButton));
 	// TODO: Rework this as a dedicated toolbar control so we can hook into theme changes and update it
 	// when the editor theme updates.
 	bake->set_button_icon(EditorNode::get_singleton()->get_editor_theme()->get_icon(SNAME("Bake"), EditorStringName(EditorIcons)));
diff --git a/editor/plugins/navigation_obstacle_3d_editor_plugin.cpp b/editor/plugins/navigation_obstacle_3d_editor_plugin.cpp
index efa98d8e4d..ee385b6dce 100644
--- a/editor/plugins/navigation_obstacle_3d_editor_plugin.cpp
+++ b/editor/plugins/navigation_obstacle_3d_editor_plugin.cpp
@@ -840,7 +840,7 @@ NavigationObstacle3DEditorPlugin::NavigationObstacle3DEditorPlugin() {
 	bg.instantiate();
 
 	button_create = memnew(Button);
-	button_create->set_theme_type_variation("FlatButton");
+	button_create->set_theme_type_variation(SceneStringName(FlatButton));
 	obstacle_editor->add_child(button_create);
 	button_create->set_tooltip_text(TTR("Add Vertex"));
 	button_create->connect(SceneStringName(pressed), callable_mp(this, &NavigationObstacle3DEditorPlugin::set_mode).bind(NavigationObstacle3DEditorPlugin::MODE_CREATE));
@@ -848,27 +848,27 @@ NavigationObstacle3DEditorPlugin::NavigationObstacle3DEditorPlugin() {
 	button_create->set_button_group(bg);
 
 	button_edit = memnew(Button);
-	button_edit->set_theme_type_variation("FlatButton");
+	button_edit->set_theme_type_variation(SceneStringName(FlatButton));
 	obstacle_editor->add_child(button_edit);
 	button_edit->connect(SceneStringName(pressed), callable_mp(this, &NavigationObstacle3DEditorPlugin::set_mode).bind(NavigationObstacle3DEditorPlugin::MODE_EDIT));
 	button_edit->set_toggle_mode(true);
 	button_edit->set_button_group(bg);
 
 	button_delete = memnew(Button);
-	button_delete->set_theme_type_variation("FlatButton");
+	button_delete->set_theme_type_variation(SceneStringName(FlatButton));
 	obstacle_editor->add_child(button_delete);
 	button_delete->connect(SceneStringName(pressed), callable_mp(this, &NavigationObstacle3DEditorPlugin::set_mode).bind(NavigationObstacle3DEditorPlugin::MODE_DELETE));
 	button_delete->set_toggle_mode(true);
 	button_delete->set_button_group(bg);
 
 	button_flip = memnew(Button);
-	button_flip->set_theme_type_variation("FlatButton");
+	button_flip->set_theme_type_variation(SceneStringName(FlatButton));
 	obstacle_editor->add_child(button_flip);
 	button_flip->connect(SceneStringName(pressed), callable_mp(this, &NavigationObstacle3DEditorPlugin::set_mode).bind(NavigationObstacle3DEditorPlugin::ACTION_FLIP));
 	button_flip->set_toggle_mode(true);
 
 	button_clear = memnew(Button);
-	button_clear->set_theme_type_variation("FlatButton");
+	button_clear->set_theme_type_variation(SceneStringName(FlatButton));
 	obstacle_editor->add_child(button_clear);
 	button_clear->connect(SceneStringName(pressed), callable_mp(this, &NavigationObstacle3DEditorPlugin::set_mode).bind(NavigationObstacle3DEditorPlugin::ACTION_CLEAR));
 	button_clear->set_toggle_mode(true);
diff --git a/editor/plugins/node_3d_editor_plugin.cpp b/editor/plugins/node_3d_editor_plugin.cpp
index e5cffbd41e..708eac3fbb 100644
--- a/editor/plugins/node_3d_editor_plugin.cpp
+++ b/editor/plugins/node_3d_editor_plugin.cpp
@@ -8696,7 +8696,7 @@ Node3DEditor::Node3DEditor() {
 	tool_button[TOOL_MODE_SELECT] = memnew(Button);
 	main_menu_hbox->add_child(tool_button[TOOL_MODE_SELECT]);
 	tool_button[TOOL_MODE_SELECT]->set_toggle_mode(true);
-	tool_button[TOOL_MODE_SELECT]->set_theme_type_variation("FlatButton");
+	tool_button[TOOL_MODE_SELECT]->set_theme_type_variation(SceneStringName(FlatButton));
 	tool_button[TOOL_MODE_SELECT]->set_pressed(true);
 	tool_button[TOOL_MODE_SELECT]->connect(SceneStringName(pressed), callable_mp(this, &Node3DEditor::_menu_item_pressed).bind(MENU_TOOL_SELECT));
 	tool_button[TOOL_MODE_SELECT]->set_shortcut(ED_SHORTCUT("spatial_editor/tool_select", TTR("Select Mode"), Key::Q));
@@ -8707,7 +8707,7 @@ Node3DEditor::Node3DEditor() {
 	tool_button[TOOL_MODE_MOVE] = memnew(Button);
 	main_menu_hbox->add_child(tool_button[TOOL_MODE_MOVE]);
 	tool_button[TOOL_MODE_MOVE]->set_toggle_mode(true);
-	tool_button[TOOL_MODE_MOVE]->set_theme_type_variation("FlatButton");
+	tool_button[TOOL_MODE_MOVE]->set_theme_type_variation(SceneStringName(FlatButton));
 
 	tool_button[TOOL_MODE_MOVE]->connect(SceneStringName(pressed), callable_mp(this, &Node3DEditor::_menu_item_pressed).bind(MENU_TOOL_MOVE));
 	tool_button[TOOL_MODE_MOVE]->set_shortcut(ED_SHORTCUT("spatial_editor/tool_move", TTR("Move Mode"), Key::W));
@@ -8717,7 +8717,7 @@ Node3DEditor::Node3DEditor() {
 	tool_button[TOOL_MODE_ROTATE] = memnew(Button);
 	main_menu_hbox->add_child(tool_button[TOOL_MODE_ROTATE]);
 	tool_button[TOOL_MODE_ROTATE]->set_toggle_mode(true);
-	tool_button[TOOL_MODE_ROTATE]->set_theme_type_variation("FlatButton");
+	tool_button[TOOL_MODE_ROTATE]->set_theme_type_variation(SceneStringName(FlatButton));
 	tool_button[TOOL_MODE_ROTATE]->connect(SceneStringName(pressed), callable_mp(this, &Node3DEditor::_menu_item_pressed).bind(MENU_TOOL_ROTATE));
 	tool_button[TOOL_MODE_ROTATE]->set_shortcut(ED_SHORTCUT("spatial_editor/tool_rotate", TTR("Rotate Mode"), Key::E));
 	tool_button[TOOL_MODE_ROTATE]->set_shortcut_context(this);
@@ -8726,7 +8726,7 @@ Node3DEditor::Node3DEditor() {
 	tool_button[TOOL_MODE_SCALE] = memnew(Button);
 	main_menu_hbox->add_child(tool_button[TOOL_MODE_SCALE]);
 	tool_button[TOOL_MODE_SCALE]->set_toggle_mode(true);
-	tool_button[TOOL_MODE_SCALE]->set_theme_type_variation("FlatButton");
+	tool_button[TOOL_MODE_SCALE]->set_theme_type_variation(SceneStringName(FlatButton));
 	tool_button[TOOL_MODE_SCALE]->connect(SceneStringName(pressed), callable_mp(this, &Node3DEditor::_menu_item_pressed).bind(MENU_TOOL_SCALE));
 	tool_button[TOOL_MODE_SCALE]->set_shortcut(ED_SHORTCUT("spatial_editor/tool_scale", TTR("Scale Mode"), Key::R));
 	tool_button[TOOL_MODE_SCALE]->set_shortcut_context(this);
@@ -8737,13 +8737,13 @@ Node3DEditor::Node3DEditor() {
 	tool_button[TOOL_MODE_LIST_SELECT] = memnew(Button);
 	main_menu_hbox->add_child(tool_button[TOOL_MODE_LIST_SELECT]);
 	tool_button[TOOL_MODE_LIST_SELECT]->set_toggle_mode(true);
-	tool_button[TOOL_MODE_LIST_SELECT]->set_theme_type_variation("FlatButton");
+	tool_button[TOOL_MODE_LIST_SELECT]->set_theme_type_variation(SceneStringName(FlatButton));
 	tool_button[TOOL_MODE_LIST_SELECT]->connect(SceneStringName(pressed), callable_mp(this, &Node3DEditor::_menu_item_pressed).bind(MENU_TOOL_LIST_SELECT));
 	tool_button[TOOL_MODE_LIST_SELECT]->set_tooltip_text(TTR("Show list of selectable nodes at position clicked."));
 
 	tool_button[TOOL_LOCK_SELECTED] = memnew(Button);
 	main_menu_hbox->add_child(tool_button[TOOL_LOCK_SELECTED]);
-	tool_button[TOOL_LOCK_SELECTED]->set_theme_type_variation("FlatButton");
+	tool_button[TOOL_LOCK_SELECTED]->set_theme_type_variation(SceneStringName(FlatButton));
 	tool_button[TOOL_LOCK_SELECTED]->connect(SceneStringName(pressed), callable_mp(this, &Node3DEditor::_menu_item_pressed).bind(MENU_LOCK_SELECTED));
 	tool_button[TOOL_LOCK_SELECTED]->set_tooltip_text(TTR("Lock selected node, preventing selection and movement."));
 	// Define the shortcut globally (without a context) so that it works if the Scene tree dock is currently focused.
@@ -8751,7 +8751,7 @@ Node3DEditor::Node3DEditor() {
 
 	tool_button[TOOL_UNLOCK_SELECTED] = memnew(Button);
 	main_menu_hbox->add_child(tool_button[TOOL_UNLOCK_SELECTED]);
-	tool_button[TOOL_UNLOCK_SELECTED]->set_theme_type_variation("FlatButton");
+	tool_button[TOOL_UNLOCK_SELECTED]->set_theme_type_variation(SceneStringName(FlatButton));
 	tool_button[TOOL_UNLOCK_SELECTED]->connect(SceneStringName(pressed), callable_mp(this, &Node3DEditor::_menu_item_pressed).bind(MENU_UNLOCK_SELECTED));
 	tool_button[TOOL_UNLOCK_SELECTED]->set_tooltip_text(TTR("Unlock selected node, allowing selection and movement."));
 	// Define the shortcut globally (without a context) so that it works if the Scene tree dock is currently focused.
@@ -8759,7 +8759,7 @@ Node3DEditor::Node3DEditor() {
 
 	tool_button[TOOL_GROUP_SELECTED] = memnew(Button);
 	main_menu_hbox->add_child(tool_button[TOOL_GROUP_SELECTED]);
-	tool_button[TOOL_GROUP_SELECTED]->set_theme_type_variation("FlatButton");
+	tool_button[TOOL_GROUP_SELECTED]->set_theme_type_variation(SceneStringName(FlatButton));
 	tool_button[TOOL_GROUP_SELECTED]->connect(SceneStringName(pressed), callable_mp(this, &Node3DEditor::_menu_item_pressed).bind(MENU_GROUP_SELECTED));
 	tool_button[TOOL_GROUP_SELECTED]->set_tooltip_text(TTR("Groups the selected node with its children. This selects the parent when any child node is clicked in 2D and 3D view."));
 	// Define the shortcut globally (without a context) so that it works if the Scene tree dock is currently focused.
@@ -8767,7 +8767,7 @@ Node3DEditor::Node3DEditor() {
 
 	tool_button[TOOL_UNGROUP_SELECTED] = memnew(Button);
 	main_menu_hbox->add_child(tool_button[TOOL_UNGROUP_SELECTED]);
-	tool_button[TOOL_UNGROUP_SELECTED]->set_theme_type_variation("FlatButton");
+	tool_button[TOOL_UNGROUP_SELECTED]->set_theme_type_variation(SceneStringName(FlatButton));
 	tool_button[TOOL_UNGROUP_SELECTED]->connect(SceneStringName(pressed), callable_mp(this, &Node3DEditor::_menu_item_pressed).bind(MENU_UNGROUP_SELECTED));
 	tool_button[TOOL_UNGROUP_SELECTED]->set_tooltip_text(TTR("Ungroups the selected node from its children. Child nodes will be individual items in 2D and 3D view."));
 	// Define the shortcut globally (without a context) so that it works if the Scene tree dock is currently focused.
@@ -8778,7 +8778,7 @@ Node3DEditor::Node3DEditor() {
 	tool_option_button[TOOL_OPT_LOCAL_COORDS] = memnew(Button);
 	main_menu_hbox->add_child(tool_option_button[TOOL_OPT_LOCAL_COORDS]);
 	tool_option_button[TOOL_OPT_LOCAL_COORDS]->set_toggle_mode(true);
-	tool_option_button[TOOL_OPT_LOCAL_COORDS]->set_theme_type_variation("FlatButton");
+	tool_option_button[TOOL_OPT_LOCAL_COORDS]->set_theme_type_variation(SceneStringName(FlatButton));
 	tool_option_button[TOOL_OPT_LOCAL_COORDS]->connect(SceneStringName(toggled), callable_mp(this, &Node3DEditor::_menu_item_toggled).bind(MENU_TOOL_LOCAL_COORDS));
 	tool_option_button[TOOL_OPT_LOCAL_COORDS]->set_shortcut(ED_SHORTCUT("spatial_editor/local_coords", TTR("Use Local Space"), Key::T));
 	tool_option_button[TOOL_OPT_LOCAL_COORDS]->set_shortcut_context(this);
@@ -8786,7 +8786,7 @@ Node3DEditor::Node3DEditor() {
 	tool_option_button[TOOL_OPT_USE_SNAP] = memnew(Button);
 	main_menu_hbox->add_child(tool_option_button[TOOL_OPT_USE_SNAP]);
 	tool_option_button[TOOL_OPT_USE_SNAP]->set_toggle_mode(true);
-	tool_option_button[TOOL_OPT_USE_SNAP]->set_theme_type_variation("FlatButton");
+	tool_option_button[TOOL_OPT_USE_SNAP]->set_theme_type_variation(SceneStringName(FlatButton));
 	tool_option_button[TOOL_OPT_USE_SNAP]->connect(SceneStringName(toggled), callable_mp(this, &Node3DEditor::_menu_item_toggled).bind(MENU_TOOL_USE_SNAP));
 	tool_option_button[TOOL_OPT_USE_SNAP]->set_shortcut(ED_SHORTCUT("spatial_editor/snap", TTR("Use Snap"), Key::Y));
 	tool_option_button[TOOL_OPT_USE_SNAP]->set_shortcut_context(this);
@@ -8795,7 +8795,7 @@ Node3DEditor::Node3DEditor() {
 	sun_button = memnew(Button);
 	sun_button->set_tooltip_text(TTR("Toggle preview sunlight.\nIf a DirectionalLight3D node is added to the scene, preview sunlight is disabled."));
 	sun_button->set_toggle_mode(true);
-	sun_button->set_theme_type_variation("FlatButton");
+	sun_button->set_theme_type_variation(SceneStringName(FlatButton));
 	sun_button->connect(SceneStringName(pressed), callable_mp(this, &Node3DEditor::_update_preview_environment), CONNECT_DEFERRED);
 	// Preview is enabled by default - ensure this applies on editor startup when there is no state yet.
 	sun_button->set_pressed(true);
@@ -8805,7 +8805,7 @@ Node3DEditor::Node3DEditor() {
 	environ_button = memnew(Button);
 	environ_button->set_tooltip_text(TTR("Toggle preview environment.\nIf a WorldEnvironment node is added to the scene, preview environment is disabled."));
 	environ_button->set_toggle_mode(true);
-	environ_button->set_theme_type_variation("FlatButton");
+	environ_button->set_theme_type_variation(SceneStringName(FlatButton));
 	environ_button->connect(SceneStringName(pressed), callable_mp(this, &Node3DEditor::_update_preview_environment), CONNECT_DEFERRED);
 	// Preview is enabled by default - ensure this applies on editor startup when there is no state yet.
 	environ_button->set_pressed(true);
@@ -8814,7 +8814,7 @@ Node3DEditor::Node3DEditor() {
 
 	sun_environ_settings = memnew(Button);
 	sun_environ_settings->set_tooltip_text(TTR("Edit Sun and Environment settings."));
-	sun_environ_settings->set_theme_type_variation("FlatButton");
+	sun_environ_settings->set_theme_type_variation(SceneStringName(FlatButton));
 	sun_environ_settings->connect(SceneStringName(pressed), callable_mp(this, &Node3DEditor::_sun_environ_settings_pressed));
 
 	main_menu_hbox->add_child(sun_environ_settings);
diff --git a/editor/plugins/occluder_instance_3d_editor_plugin.cpp b/editor/plugins/occluder_instance_3d_editor_plugin.cpp
index b9c884178f..35e8cd0005 100644
--- a/editor/plugins/occluder_instance_3d_editor_plugin.cpp
+++ b/editor/plugins/occluder_instance_3d_editor_plugin.cpp
@@ -104,7 +104,7 @@ void OccluderInstance3DEditorPlugin::_bind_methods() {
 
 OccluderInstance3DEditorPlugin::OccluderInstance3DEditorPlugin() {
 	bake = memnew(Button);
-	bake->set_theme_type_variation("FlatButton");
+	bake->set_theme_type_variation(SceneStringName(FlatButton));
 	bake->set_button_icon(EditorNode::get_singleton()->get_editor_theme()->get_icon(SNAME("Bake"), EditorStringName(EditorIcons)));
 	bake->set_text(TTR("Bake Occluders"));
 	bake->hide();
diff --git a/editor/plugins/path_2d_editor_plugin.cpp b/editor/plugins/path_2d_editor_plugin.cpp
index 524f3d86c2..51eca8fe5d 100644
--- a/editor/plugins/path_2d_editor_plugin.cpp
+++ b/editor/plugins/path_2d_editor_plugin.cpp
@@ -682,7 +682,7 @@ Path2DEditor::Path2DEditor() {
 	toolbar = memnew(HBoxContainer);
 
 	curve_edit = memnew(Button);
-	curve_edit->set_theme_type_variation("FlatButton");
+	curve_edit->set_theme_type_variation(SceneStringName(FlatButton));
 	curve_edit->set_toggle_mode(true);
 	curve_edit->set_pressed(true);
 	curve_edit->set_focus_mode(Control::FOCUS_NONE);
@@ -691,7 +691,7 @@ Path2DEditor::Path2DEditor() {
 	toolbar->add_child(curve_edit);
 
 	curve_edit_curve = memnew(Button);
-	curve_edit_curve->set_theme_type_variation("FlatButton");
+	curve_edit_curve->set_theme_type_variation(SceneStringName(FlatButton));
 	curve_edit_curve->set_toggle_mode(true);
 	curve_edit_curve->set_focus_mode(Control::FOCUS_NONE);
 	curve_edit_curve->set_tooltip_text(TTR("Select Control Points (Shift+Drag)"));
@@ -699,7 +699,7 @@ Path2DEditor::Path2DEditor() {
 	toolbar->add_child(curve_edit_curve);
 
 	curve_create = memnew(Button);
-	curve_create->set_theme_type_variation("FlatButton");
+	curve_create->set_theme_type_variation(SceneStringName(FlatButton));
 	curve_create->set_toggle_mode(true);
 	curve_create->set_focus_mode(Control::FOCUS_NONE);
 	curve_create->set_tooltip_text(TTR("Add Point (in empty space)") + "\n" + TTR("Right Click: Delete Point"));
@@ -707,7 +707,7 @@ Path2DEditor::Path2DEditor() {
 	toolbar->add_child(curve_create);
 
 	curve_del = memnew(Button);
-	curve_del->set_theme_type_variation("FlatButton");
+	curve_del->set_theme_type_variation(SceneStringName(FlatButton));
 	curve_del->set_toggle_mode(true);
 	curve_del->set_focus_mode(Control::FOCUS_NONE);
 	curve_del->set_tooltip_text(TTR("Delete Point"));
@@ -715,14 +715,14 @@ Path2DEditor::Path2DEditor() {
 	toolbar->add_child(curve_del);
 
 	curve_close = memnew(Button);
-	curve_close->set_theme_type_variation("FlatButton");
+	curve_close->set_theme_type_variation(SceneStringName(FlatButton));
 	curve_close->set_focus_mode(Control::FOCUS_NONE);
 	curve_close->set_tooltip_text(TTR("Close Curve"));
 	curve_close->connect(SceneStringName(pressed), callable_mp(this, &Path2DEditor::_mode_selected).bind(MODE_CLOSE));
 	toolbar->add_child(curve_close);
 
 	curve_clear_points = memnew(Button);
-	curve_clear_points->set_theme_type_variation("FlatButton");
+	curve_clear_points->set_theme_type_variation(SceneStringName(FlatButton));
 	curve_clear_points->set_focus_mode(Control::FOCUS_NONE);
 	curve_clear_points->set_tooltip_text(TTR("Clear Points"));
 	curve_clear_points->connect(SceneStringName(pressed), callable_mp(this, &Path2DEditor::_confirm_clear_points));
diff --git a/editor/plugins/path_3d_editor_plugin.cpp b/editor/plugins/path_3d_editor_plugin.cpp
index de4ab828bc..0874ff6d84 100644
--- a/editor/plugins/path_3d_editor_plugin.cpp
+++ b/editor/plugins/path_3d_editor_plugin.cpp
@@ -873,7 +873,7 @@ Path3DEditorPlugin::Path3DEditorPlugin() {
 	topmenu_bar->add_child(toolbar);
 
 	curve_edit = memnew(Button);
-	curve_edit->set_theme_type_variation("FlatButton");
+	curve_edit->set_theme_type_variation(SceneStringName(FlatButton));
 	curve_edit->set_toggle_mode(true);
 	curve_edit->set_focus_mode(Control::FOCUS_NONE);
 	curve_edit->set_tooltip_text(TTR("Select Points") + "\n" + TTR("Shift+Click: Select multiple Points") + "\n" + keycode_get_string((Key)KeyModifierMask::CMD_OR_CTRL) + TTR("Click: Add Point") + "\n" + TTR("Right Click: Delete Point"));
@@ -881,7 +881,7 @@ Path3DEditorPlugin::Path3DEditorPlugin() {
 	curve_edit->connect(SceneStringName(pressed), callable_mp(this, &Path3DEditorPlugin::_mode_changed).bind(MODE_EDIT));
 
 	curve_edit_curve = memnew(Button);
-	curve_edit_curve->set_theme_type_variation("FlatButton");
+	curve_edit_curve->set_theme_type_variation(SceneStringName(FlatButton));
 	curve_edit_curve->set_toggle_mode(true);
 	curve_edit_curve->set_focus_mode(Control::FOCUS_NONE);
 	curve_edit_curve->set_tooltip_text(TTR("Select Control Points") + "\n" + TTR("Shift+Click: Drag out Control Points"));
@@ -889,7 +889,7 @@ Path3DEditorPlugin::Path3DEditorPlugin() {
 	curve_edit_curve->connect(SceneStringName(pressed), callable_mp(this, &Path3DEditorPlugin::_mode_changed).bind(MODE_EDIT_CURVE));
 
 	curve_edit_tilt = memnew(Button);
-	curve_edit_tilt->set_theme_type_variation("FlatButton");
+	curve_edit_tilt->set_theme_type_variation(SceneStringName(FlatButton));
 	curve_edit_tilt->set_toggle_mode(true);
 	curve_edit_tilt->set_focus_mode(Control::FOCUS_NONE);
 	curve_edit_tilt->set_tooltip_text(TTR("Select Tilt Handles"));
@@ -897,7 +897,7 @@ Path3DEditorPlugin::Path3DEditorPlugin() {
 	curve_edit_tilt->connect(SceneStringName(pressed), callable_mp(this, &Path3DEditorPlugin::_mode_changed).bind(MODE_EDIT_TILT));
 
 	curve_create = memnew(Button);
-	curve_create->set_theme_type_variation("FlatButton");
+	curve_create->set_theme_type_variation(SceneStringName(FlatButton));
 	curve_create->set_toggle_mode(true);
 	curve_create->set_focus_mode(Control::FOCUS_NONE);
 	curve_create->set_tooltip_text(TTR("Add Point (in empty space)") + "\n" + TTR("Split Segment (in curve)"));
@@ -905,7 +905,7 @@ Path3DEditorPlugin::Path3DEditorPlugin() {
 	curve_create->connect(SceneStringName(pressed), callable_mp(this, &Path3DEditorPlugin::_mode_changed).bind(MODE_CREATE));
 
 	curve_del = memnew(Button);
-	curve_del->set_theme_type_variation("FlatButton");
+	curve_del->set_theme_type_variation(SceneStringName(FlatButton));
 	curve_del->set_toggle_mode(true);
 	curve_del->set_focus_mode(Control::FOCUS_NONE);
 	curve_del->set_tooltip_text(TTR("Delete Point"));
@@ -913,14 +913,14 @@ Path3DEditorPlugin::Path3DEditorPlugin() {
 	curve_del->connect(SceneStringName(pressed), callable_mp(this, &Path3DEditorPlugin::_mode_changed).bind(MODE_DELETE));
 
 	curve_closed = memnew(Button);
-	curve_closed->set_theme_type_variation("FlatButton");
+	curve_closed->set_theme_type_variation(SceneStringName(FlatButton));
 	curve_closed->set_focus_mode(Control::FOCUS_NONE);
 	curve_closed->set_tooltip_text(TTR("Close Curve"));
 	toolbar->add_child(curve_closed);
 	curve_closed->connect(SceneStringName(pressed), callable_mp(this, &Path3DEditorPlugin::_toggle_closed_curve));
 
 	curve_clear_points = memnew(Button);
-	curve_clear_points->set_theme_type_variation("FlatButton");
+	curve_clear_points->set_theme_type_variation(SceneStringName(FlatButton));
 	curve_clear_points->set_focus_mode(Control::FOCUS_NONE);
 	curve_clear_points->set_tooltip_text(TTR("Clear Points"));
 	curve_clear_points->connect(SceneStringName(pressed), callable_mp(this, &Path3DEditorPlugin::_confirm_clear_points));
diff --git a/editor/plugins/physical_bone_3d_editor_plugin.cpp b/editor/plugins/physical_bone_3d_editor_plugin.cpp
index 15a957d5c4..45cdc41208 100644
--- a/editor/plugins/physical_bone_3d_editor_plugin.cpp
+++ b/editor/plugins/physical_bone_3d_editor_plugin.cpp
@@ -54,7 +54,7 @@ PhysicalBone3DEditor::PhysicalBone3DEditor() {
 	Node3DEditor::get_singleton()->add_control_to_menu_panel(spatial_editor_hb);
 
 	button_transform_joint = memnew(Button);
-	button_transform_joint->set_theme_type_variation("FlatButton");
+	button_transform_joint->set_theme_type_variation(SceneStringName(FlatButton));
 	spatial_editor_hb->add_child(button_transform_joint);
 
 	button_transform_joint->set_text(TTR("Move Joint"));
diff --git a/editor/plugins/polygon_2d_editor_plugin.cpp b/editor/plugins/polygon_2d_editor_plugin.cpp
index 8ab08ff28f..6fc4bef71e 100644
--- a/editor/plugins/polygon_2d_editor_plugin.cpp
+++ b/editor/plugins/polygon_2d_editor_plugin.cpp
@@ -1343,7 +1343,7 @@ Polygon2DEditor::Polygon2DEditor() {
 	snap_show_grid = EditorSettings::get_singleton()->get_project_metadata("polygon_2d_uv_editor", "show_grid", false);
 
 	button_uv = memnew(Button);
-	button_uv->set_theme_type_variation("FlatButton");
+	button_uv->set_theme_type_variation(SceneStringName(FlatButton));
 	add_child(button_uv);
 	button_uv->set_tooltip_text(TTR("Open Polygon 2D UV editor."));
 	button_uv->connect(SceneStringName(pressed), callable_mp(this, &Polygon2DEditor::_menu_option).bind(MODE_EDIT_UV));
@@ -1397,7 +1397,7 @@ Polygon2DEditor::Polygon2DEditor() {
 	uv_main_vb->add_child(uv_mode_hb);
 	for (int i = 0; i < UV_MODE_MAX; i++) {
 		uv_button[i] = memnew(Button);
-		uv_button[i]->set_theme_type_variation("FlatButton");
+		uv_button[i]->set_theme_type_variation(SceneStringName(FlatButton));
 		uv_button[i]->set_toggle_mode(true);
 		uv_mode_hb->add_child(uv_button[i]);
 		uv_button[i]->connect(SceneStringName(pressed), callable_mp(this, &Polygon2DEditor::_uv_mode).bind(i));
@@ -1486,7 +1486,7 @@ Polygon2DEditor::Polygon2DEditor() {
 	uv_mode_hb->add_child(memnew(VSeparator));
 
 	b_snap_enable = memnew(Button);
-	b_snap_enable->set_theme_type_variation("FlatButton");
+	b_snap_enable->set_theme_type_variation(SceneStringName(FlatButton));
 	uv_mode_hb->add_child(b_snap_enable);
 	b_snap_enable->set_text(TTR("Snap"));
 	b_snap_enable->set_focus_mode(FOCUS_NONE);
@@ -1496,7 +1496,7 @@ Polygon2DEditor::Polygon2DEditor() {
 	b_snap_enable->connect(SceneStringName(toggled), callable_mp(this, &Polygon2DEditor::_set_use_snap));
 
 	b_snap_grid = memnew(Button);
-	b_snap_grid->set_theme_type_variation("FlatButton");
+	b_snap_grid->set_theme_type_variation(SceneStringName(FlatButton));
 	uv_mode_hb->add_child(b_snap_grid);
 	b_snap_grid->set_text(TTR("Grid"));
 	b_snap_grid->set_focus_mode(FOCUS_NONE);
diff --git a/editor/plugins/polygon_3d_editor_plugin.cpp b/editor/plugins/polygon_3d_editor_plugin.cpp
index 017504f0d6..a1636d55e4 100644
--- a/editor/plugins/polygon_3d_editor_plugin.cpp
+++ b/editor/plugins/polygon_3d_editor_plugin.cpp
@@ -536,13 +536,13 @@ Polygon3DEditor::Polygon3DEditor() {
 	node = nullptr;
 
 	button_create = memnew(Button);
-	button_create->set_theme_type_variation("FlatButton");
+	button_create->set_theme_type_variation(SceneStringName(FlatButton));
 	add_child(button_create);
 	button_create->connect(SceneStringName(pressed), callable_mp(this, &Polygon3DEditor::_menu_option).bind(MODE_CREATE));
 	button_create->set_toggle_mode(true);
 
 	button_edit = memnew(Button);
-	button_edit->set_theme_type_variation("FlatButton");
+	button_edit->set_theme_type_variation(SceneStringName(FlatButton));
 	add_child(button_edit);
 	button_edit->connect(SceneStringName(pressed), callable_mp(this, &Polygon3DEditor::_menu_option).bind(MODE_EDIT));
 	button_edit->set_toggle_mode(true);
diff --git a/editor/plugins/skeleton_3d_editor_plugin.cpp b/editor/plugins/skeleton_3d_editor_plugin.cpp
index 1a7c42534a..c4abc5b20c 100644
--- a/editor/plugins/skeleton_3d_editor_plugin.cpp
+++ b/editor/plugins/skeleton_3d_editor_plugin.cpp
@@ -1024,7 +1024,7 @@ void Skeleton3DEditor::create_editors() {
 
 	edit_mode_button = memnew(Button);
 	topmenu_bar->add_child(edit_mode_button);
-	edit_mode_button->set_theme_type_variation("FlatButton");
+	edit_mode_button->set_theme_type_variation(SceneStringName(FlatButton));
 	edit_mode_button->set_toggle_mode(true);
 	edit_mode_button->set_focus_mode(FOCUS_NONE);
 	edit_mode_button->set_tooltip_text(TTR("Edit Mode\nShow buttons on joints."));
@@ -1044,7 +1044,7 @@ void Skeleton3DEditor::create_editors() {
 	animation_hb->hide();
 
 	key_loc_button = memnew(Button);
-	key_loc_button->set_theme_type_variation("FlatButton");
+	key_loc_button->set_theme_type_variation(SceneStringName(FlatButton));
 	key_loc_button->set_toggle_mode(true);
 	key_loc_button->set_pressed(false);
 	key_loc_button->set_focus_mode(FOCUS_NONE);
@@ -1052,7 +1052,7 @@ void Skeleton3DEditor::create_editors() {
 	animation_hb->add_child(key_loc_button);
 
 	key_rot_button = memnew(Button);
-	key_rot_button->set_theme_type_variation("FlatButton");
+	key_rot_button->set_theme_type_variation(SceneStringName(FlatButton));
 	key_rot_button->set_toggle_mode(true);
 	key_rot_button->set_pressed(true);
 	key_rot_button->set_focus_mode(FOCUS_NONE);
@@ -1060,7 +1060,7 @@ void Skeleton3DEditor::create_editors() {
 	animation_hb->add_child(key_rot_button);
 
 	key_scale_button = memnew(Button);
-	key_scale_button->set_theme_type_variation("FlatButton");
+	key_scale_button->set_theme_type_variation(SceneStringName(FlatButton));
 	key_scale_button->set_toggle_mode(true);
 	key_scale_button->set_pressed(false);
 	key_scale_button->set_focus_mode(FOCUS_NONE);
@@ -1068,7 +1068,7 @@ void Skeleton3DEditor::create_editors() {
 	animation_hb->add_child(key_scale_button);
 
 	key_insert_button = memnew(Button);
-	key_insert_button->set_theme_type_variation("FlatButton");
+	key_insert_button->set_theme_type_variation(SceneStringName(FlatButton));
 	key_insert_button->set_focus_mode(FOCUS_NONE);
 	key_insert_button->connect(SceneStringName(pressed), callable_mp(this, &Skeleton3DEditor::insert_keys).bind(false));
 	key_insert_button->set_tooltip_text(TTR("Insert key (based on mask) for bones with an existing track."));
@@ -1076,7 +1076,7 @@ void Skeleton3DEditor::create_editors() {
 	animation_hb->add_child(key_insert_button);
 
 	key_insert_all_button = memnew(Button);
-	key_insert_all_button->set_theme_type_variation("FlatButton");
+	key_insert_all_button->set_theme_type_variation(SceneStringName(FlatButton));
 	key_insert_all_button->set_focus_mode(FOCUS_NONE);
 	key_insert_all_button->connect(SceneStringName(pressed), callable_mp(this, &Skeleton3DEditor::insert_keys).bind(true));
 	key_insert_all_button->set_tooltip_text(TTR("Insert key (based on mask) for all bones."));
diff --git a/editor/plugins/sprite_frames_editor_plugin.cpp b/editor/plugins/sprite_frames_editor_plugin.cpp
index 0c50fc457b..e516387efb 100644
--- a/editor/plugins/sprite_frames_editor_plugin.cpp
+++ b/editor/plugins/sprite_frames_editor_plugin.cpp
@@ -1898,7 +1898,7 @@ SpriteFramesEditor::SpriteFramesEditor() {
 	sub_vb->add_child(hbc_animlist);
 
 	add_anim = memnew(Button);
-	add_anim->set_theme_type_variation("FlatButton");
+	add_anim->set_theme_type_variation(SceneStringName(FlatButton));
 	hbc_animlist->add_child(add_anim);
 	add_anim->connect(SceneStringName(pressed), callable_mp(this, &SpriteFramesEditor::_animation_add));
 
@@ -1908,7 +1908,7 @@ SpriteFramesEditor::SpriteFramesEditor() {
 	duplicate_anim->connect(SceneStringName(pressed), callable_mp(this, &SpriteFramesEditor::_animation_duplicate));
 
 	delete_anim = memnew(Button);
-	delete_anim->set_theme_type_variation("FlatButton");
+	delete_anim->set_theme_type_variation(SceneStringName(FlatButton));
 	hbc_animlist->add_child(delete_anim);
 	delete_anim->set_disabled(true);
 	delete_anim->connect(SceneStringName(pressed), callable_mp(this, &SpriteFramesEditor::_animation_remove));
@@ -1919,7 +1919,7 @@ SpriteFramesEditor::SpriteFramesEditor() {
 	autoplay_container->add_child(memnew(VSeparator));
 
 	autoplay = memnew(Button);
-	autoplay->set_theme_type_variation("FlatButton");
+	autoplay->set_theme_type_variation(SceneStringName(FlatButton));
 	autoplay->set_tooltip_text(TTR("Autoplay on Load"));
 	autoplay_container->add_child(autoplay);
 
@@ -1927,7 +1927,7 @@ SpriteFramesEditor::SpriteFramesEditor() {
 
 	anim_loop = memnew(Button);
 	anim_loop->set_toggle_mode(true);
-	anim_loop->set_theme_type_variation("FlatButton");
+	anim_loop->set_theme_type_variation(SceneStringName(FlatButton));
 	anim_loop->set_tooltip_text(TTR("Animation Looping"));
 	anim_loop->connect(SceneStringName(pressed), callable_mp(this, &SpriteFramesEditor::_animation_loop_changed));
 	hbc_animlist->add_child(anim_loop);
@@ -1991,27 +1991,27 @@ SpriteFramesEditor::SpriteFramesEditor() {
 	hfc->add_child(playback_container);
 
 	play_bw_from = memnew(Button);
-	play_bw_from->set_theme_type_variation("FlatButton");
+	play_bw_from->set_theme_type_variation(SceneStringName(FlatButton));
 	play_bw_from->set_tooltip_text(TTR("Play selected animation backwards from current pos. (A)"));
 	playback_container->add_child(play_bw_from);
 
 	play_bw = memnew(Button);
-	play_bw->set_theme_type_variation("FlatButton");
+	play_bw->set_theme_type_variation(SceneStringName(FlatButton));
 	play_bw->set_tooltip_text(TTR("Play selected animation backwards from end. (Shift+A)"));
 	playback_container->add_child(play_bw);
 
 	stop = memnew(Button);
-	stop->set_theme_type_variation("FlatButton");
+	stop->set_theme_type_variation(SceneStringName(FlatButton));
 	stop->set_tooltip_text(TTR("Pause/stop animation playback. (S)"));
 	playback_container->add_child(stop);
 
 	play = memnew(Button);
-	play->set_theme_type_variation("FlatButton");
+	play->set_theme_type_variation(SceneStringName(FlatButton));
 	play->set_tooltip_text(TTR("Play selected animation from start. (Shift+D)"));
 	playback_container->add_child(play);
 
 	play_from = memnew(Button);
-	play_from->set_theme_type_variation("FlatButton");
+	play_from->set_theme_type_variation(SceneStringName(FlatButton));
 	play_from->set_tooltip_text(TTR("Play selected animation from current pos. (D)"));
 	playback_container->add_child(play_from);
 
@@ -2029,45 +2029,45 @@ SpriteFramesEditor::SpriteFramesEditor() {
 	hfc->add_child(hbc_actions);
 
 	load = memnew(Button);
-	load->set_theme_type_variation("FlatButton");
+	load->set_theme_type_variation(SceneStringName(FlatButton));
 	hbc_actions->add_child(load);
 
 	load_sheet = memnew(Button);
-	load_sheet->set_theme_type_variation("FlatButton");
+	load_sheet->set_theme_type_variation(SceneStringName(FlatButton));
 	hbc_actions->add_child(load_sheet);
 
 	hbc_actions->add_child(memnew(VSeparator));
 
 	copy = memnew(Button);
-	copy->set_theme_type_variation("FlatButton");
+	copy->set_theme_type_variation(SceneStringName(FlatButton));
 	hbc_actions->add_child(copy);
 
 	paste = memnew(Button);
-	paste->set_theme_type_variation("FlatButton");
+	paste->set_theme_type_variation(SceneStringName(FlatButton));
 	hbc_actions->add_child(paste);
 
 	hbc_actions->add_child(memnew(VSeparator));
 
 	empty_before = memnew(Button);
-	empty_before->set_theme_type_variation("FlatButton");
+	empty_before->set_theme_type_variation(SceneStringName(FlatButton));
 	hbc_actions->add_child(empty_before);
 
 	empty_after = memnew(Button);
-	empty_after->set_theme_type_variation("FlatButton");
+	empty_after->set_theme_type_variation(SceneStringName(FlatButton));
 	hbc_actions->add_child(empty_after);
 
 	hbc_actions->add_child(memnew(VSeparator));
 
 	move_up = memnew(Button);
-	move_up->set_theme_type_variation("FlatButton");
+	move_up->set_theme_type_variation(SceneStringName(FlatButton));
 	hbc_actions->add_child(move_up);
 
 	move_down = memnew(Button);
-	move_down->set_theme_type_variation("FlatButton");
+	move_down->set_theme_type_variation(SceneStringName(FlatButton));
 	hbc_actions->add_child(move_down);
 
 	delete_frame = memnew(Button);
-	delete_frame->set_theme_type_variation("FlatButton");
+	delete_frame->set_theme_type_variation(SceneStringName(FlatButton));
 	hbc_actions->add_child(delete_frame);
 
 	hbc_actions->add_child(memnew(VSeparator));
@@ -2235,7 +2235,7 @@ SpriteFramesEditor::SpriteFramesEditor() {
 
 	toggle_settings_button = memnew(Button);
 	toggle_settings_button->set_h_size_flags(SIZE_SHRINK_END);
-	toggle_settings_button->set_theme_type_variation("FlatButton");
+	toggle_settings_button->set_theme_type_variation(SceneStringName(FlatButton));
 	toggle_settings_button->connect(SceneStringName(pressed), callable_mp(this, &SpriteFramesEditor::_toggle_show_settings));
 	toggle_settings_button->set_tooltip_text(TTR("Toggle Settings Panel"));
 	split_sheet_menu_hb->add_child(toggle_settings_button);
@@ -2273,21 +2273,21 @@ SpriteFramesEditor::SpriteFramesEditor() {
 	split_sheet_zoom_margin->add_child(split_sheet_zoom_hb);
 
 	split_sheet_zoom_out = memnew(Button);
-	split_sheet_zoom_out->set_theme_type_variation("FlatButton");
+	split_sheet_zoom_out->set_theme_type_variation(SceneStringName(FlatButton));
 	split_sheet_zoom_out->set_focus_mode(FOCUS_NONE);
 	split_sheet_zoom_out->set_tooltip_text(TTR("Zoom Out"));
 	split_sheet_zoom_out->connect(SceneStringName(pressed), callable_mp(this, &SpriteFramesEditor::_sheet_zoom_out));
 	split_sheet_zoom_hb->add_child(split_sheet_zoom_out);
 
 	split_sheet_zoom_reset = memnew(Button);
-	split_sheet_zoom_reset->set_theme_type_variation("FlatButton");
+	split_sheet_zoom_reset->set_theme_type_variation(SceneStringName(FlatButton));
 	split_sheet_zoom_reset->set_focus_mode(FOCUS_NONE);
 	split_sheet_zoom_reset->set_tooltip_text(TTR("Zoom Reset"));
 	split_sheet_zoom_reset->connect(SceneStringName(pressed), callable_mp(this, &SpriteFramesEditor::_sheet_zoom_reset));
 	split_sheet_zoom_hb->add_child(split_sheet_zoom_reset);
 
 	split_sheet_zoom_in = memnew(Button);
-	split_sheet_zoom_in->set_theme_type_variation("FlatButton");
+	split_sheet_zoom_in->set_theme_type_variation(SceneStringName(FlatButton));
 	split_sheet_zoom_in->set_focus_mode(FOCUS_NONE);
 	split_sheet_zoom_in->set_tooltip_text(TTR("Zoom In"));
 	split_sheet_zoom_in->connect(SceneStringName(pressed), callable_mp(this, &SpriteFramesEditor::_sheet_zoom_in));
diff --git a/editor/plugins/theme_editor_preview.cpp b/editor/plugins/theme_editor_preview.cpp
index 3790b23f76..681b0f046c 100644
--- a/editor/plugins/theme_editor_preview.cpp
+++ b/editor/plugins/theme_editor_preview.cpp
@@ -246,7 +246,7 @@ ThemeEditorPreview::ThemeEditorPreview() {
 
 	picker_button = memnew(Button);
 	preview_toolbar->add_child(picker_button);
-	picker_button->set_theme_type_variation("FlatButton");
+	picker_button->set_theme_type_variation(SceneStringName(FlatButton));
 	picker_button->set_toggle_mode(true);
 	picker_button->set_tooltip_text(TTR("Toggle the control picker, allowing to visually select control types for edit."));
 	picker_button->connect(SceneStringName(pressed), callable_mp(this, &ThemeEditorPreview::_picker_button_cbk));
diff --git a/editor/plugins/tiles/tile_data_editors.cpp b/editor/plugins/tiles/tile_data_editors.cpp
index bf53a9dfba..3b63beeb24 100644
--- a/editor/plugins/tiles/tile_data_editors.cpp
+++ b/editor/plugins/tiles/tile_data_editors.cpp
@@ -918,7 +918,7 @@ GenericTilePolygonEditor::GenericTilePolygonEditor() {
 	tools_button_group.instantiate();
 
 	button_expand = memnew(Button);
-	button_expand->set_theme_type_variation("FlatButton");
+	button_expand->set_theme_type_variation(SceneStringName(FlatButton));
 	button_expand->set_toggle_mode(true);
 	button_expand->set_pressed(false);
 	button_expand->set_tooltip_text(TTR("Expand editor"));
@@ -928,7 +928,7 @@ GenericTilePolygonEditor::GenericTilePolygonEditor() {
 	toolbar->add_child(memnew(VSeparator));
 
 	button_create = memnew(Button);
-	button_create->set_theme_type_variation("FlatButton");
+	button_create->set_theme_type_variation(SceneStringName(FlatButton));
 	button_create->set_toggle_mode(true);
 	button_create->set_button_group(tools_button_group);
 	button_create->set_pressed(true);
@@ -936,14 +936,14 @@ GenericTilePolygonEditor::GenericTilePolygonEditor() {
 	toolbar->add_child(button_create);
 
 	button_edit = memnew(Button);
-	button_edit->set_theme_type_variation("FlatButton");
+	button_edit->set_theme_type_variation(SceneStringName(FlatButton));
 	button_edit->set_toggle_mode(true);
 	button_edit->set_button_group(tools_button_group);
 	button_edit->set_tooltip_text(TTR("Edit points tool"));
 	toolbar->add_child(button_edit);
 
 	button_delete = memnew(Button);
-	button_delete->set_theme_type_variation("FlatButton");
+	button_delete->set_theme_type_variation(SceneStringName(FlatButton));
 	button_delete->set_toggle_mode(true);
 	button_delete->set_button_group(tools_button_group);
 	button_delete->set_tooltip_text(TTR("Delete points tool"));
@@ -1015,7 +1015,7 @@ GenericTilePolygonEditor::GenericTilePolygonEditor() {
 	button_center_view->set_anchors_and_offsets_preset(Control::PRESET_TOP_RIGHT, Control::PRESET_MODE_MINSIZE, 5);
 	button_center_view->set_grow_direction_preset(Control::PRESET_TOP_RIGHT);
 	button_center_view->connect(SceneStringName(pressed), callable_mp(this, &GenericTilePolygonEditor::_center_view));
-	button_center_view->set_theme_type_variation("FlatButton");
+	button_center_view->set_theme_type_variation(SceneStringName(FlatButton));
 	button_center_view->set_tooltip_text(TTR("Center View"));
 	button_center_view->set_disabled(true);
 	root->add_child(button_center_view);
@@ -1379,7 +1379,7 @@ TileDataDefaultEditor::TileDataDefaultEditor() {
 	add_child(label);
 
 	picker_button = memnew(Button);
-	picker_button->set_theme_type_variation("FlatButton");
+	picker_button->set_theme_type_variation(SceneStringName(FlatButton));
 	picker_button->set_toggle_mode(true);
 	picker_button->set_shortcut(ED_GET_SHORTCUT("tiles_editor/picker"));
 	toolbar->add_child(picker_button);
@@ -2880,7 +2880,7 @@ TileDataTerrainsEditor::TileDataTerrainsEditor() {
 
 	// Toolbar
 	picker_button = memnew(Button);
-	picker_button->set_theme_type_variation("FlatButton");
+	picker_button->set_theme_type_variation(SceneStringName(FlatButton));
 	picker_button->set_toggle_mode(true);
 	picker_button->set_shortcut(ED_GET_SHORTCUT("tiles_editor/picker"));
 	toolbar->add_child(picker_button);
diff --git a/editor/plugins/tiles/tile_map_layer_editor.cpp b/editor/plugins/tiles/tile_map_layer_editor.cpp
index 52e3784eb4..2a67598241 100644
--- a/editor/plugins/tiles/tile_map_layer_editor.cpp
+++ b/editor/plugins/tiles/tile_map_layer_editor.cpp
@@ -2208,7 +2208,7 @@ TileMapLayerEditorTilesPlugin::TileMapLayerEditorTilesPlugin() {
 	tool_buttons_group.instantiate();
 
 	select_tool_button = memnew(Button);
-	select_tool_button->set_theme_type_variation("FlatButton");
+	select_tool_button->set_theme_type_variation(SceneStringName(FlatButton));
 	select_tool_button->set_toggle_mode(true);
 	select_tool_button->set_button_group(tool_buttons_group);
 	select_tool_button->set_shortcut(ED_SHORTCUT("tiles_editor/selection_tool", TTR("Selection"), Key::S));
@@ -2217,7 +2217,7 @@ TileMapLayerEditorTilesPlugin::TileMapLayerEditorTilesPlugin() {
 	viewport_shortcut_buttons.push_back(select_tool_button);
 
 	paint_tool_button = memnew(Button);
-	paint_tool_button->set_theme_type_variation("FlatButton");
+	paint_tool_button->set_theme_type_variation(SceneStringName(FlatButton));
 	paint_tool_button->set_toggle_mode(true);
 	paint_tool_button->set_button_group(tool_buttons_group);
 	paint_tool_button->set_shortcut(ED_GET_SHORTCUT("tiles_editor/paint_tool"));
@@ -2227,7 +2227,7 @@ TileMapLayerEditorTilesPlugin::TileMapLayerEditorTilesPlugin() {
 	viewport_shortcut_buttons.push_back(paint_tool_button);
 
 	line_tool_button = memnew(Button);
-	line_tool_button->set_theme_type_variation("FlatButton");
+	line_tool_button->set_theme_type_variation(SceneStringName(FlatButton));
 	line_tool_button->set_toggle_mode(true);
 	line_tool_button->set_button_group(tool_buttons_group);
 	// TRANSLATORS: This refers to the line tool in the tilemap editor.
@@ -2237,7 +2237,7 @@ TileMapLayerEditorTilesPlugin::TileMapLayerEditorTilesPlugin() {
 	viewport_shortcut_buttons.push_back(line_tool_button);
 
 	rect_tool_button = memnew(Button);
-	rect_tool_button->set_theme_type_variation("FlatButton");
+	rect_tool_button->set_theme_type_variation(SceneStringName(FlatButton));
 	rect_tool_button->set_toggle_mode(true);
 	rect_tool_button->set_button_group(tool_buttons_group);
 	rect_tool_button->set_shortcut(ED_GET_SHORTCUT("tiles_editor/rect_tool"));
@@ -2246,7 +2246,7 @@ TileMapLayerEditorTilesPlugin::TileMapLayerEditorTilesPlugin() {
 	viewport_shortcut_buttons.push_back(rect_tool_button);
 
 	bucket_tool_button = memnew(Button);
-	bucket_tool_button->set_theme_type_variation("FlatButton");
+	bucket_tool_button->set_theme_type_variation(SceneStringName(FlatButton));
 	bucket_tool_button->set_toggle_mode(true);
 	bucket_tool_button->set_button_group(tool_buttons_group);
 	bucket_tool_button->set_shortcut(ED_GET_SHORTCUT("tiles_editor/bucket_tool"));
@@ -2264,7 +2264,7 @@ TileMapLayerEditorTilesPlugin::TileMapLayerEditorTilesPlugin() {
 
 	// Picker
 	picker_button = memnew(Button);
-	picker_button->set_theme_type_variation("FlatButton");
+	picker_button->set_theme_type_variation(SceneStringName(FlatButton));
 	picker_button->set_toggle_mode(true);
 	picker_button->set_shortcut(ED_GET_SHORTCUT("tiles_editor/picker"));
 	Key key = (OS::get_singleton()->has_feature("macos") || OS::get_singleton()->has_feature("web_macos") || OS::get_singleton()->has_feature("web_ios")) ? Key::META : Key::CTRL;
@@ -2275,7 +2275,7 @@ TileMapLayerEditorTilesPlugin::TileMapLayerEditorTilesPlugin() {
 
 	// Erase button.
 	erase_button = memnew(Button);
-	erase_button->set_theme_type_variation("FlatButton");
+	erase_button->set_theme_type_variation(SceneStringName(FlatButton));
 	erase_button->set_toggle_mode(true);
 	erase_button->set_shortcut(ED_GET_SHORTCUT("tiles_editor/eraser"));
 	erase_button->set_tooltip_text(TTR("Alternatively use RMB to erase tiles."));
@@ -2289,28 +2289,28 @@ TileMapLayerEditorTilesPlugin::TileMapLayerEditorTilesPlugin() {
 	transform_toolbar->add_child(memnew(VSeparator));
 
 	transform_button_rotate_left = memnew(Button);
-	transform_button_rotate_left->set_theme_type_variation("FlatButton");
+	transform_button_rotate_left->set_theme_type_variation(SceneStringName(FlatButton));
 	transform_button_rotate_left->set_shortcut(ED_SHORTCUT("tiles_editor/rotate_tile_left", TTR("Rotate Tile Left"), Key::Z));
 	transform_toolbar->add_child(transform_button_rotate_left);
 	transform_button_rotate_left->connect(SceneStringName(pressed), callable_mp(this, &TileMapLayerEditorTilesPlugin::_apply_transform).bind(TRANSFORM_ROTATE_LEFT));
 	viewport_shortcut_buttons.push_back(transform_button_rotate_left);
 
 	transform_button_rotate_right = memnew(Button);
-	transform_button_rotate_right->set_theme_type_variation("FlatButton");
+	transform_button_rotate_right->set_theme_type_variation(SceneStringName(FlatButton));
 	transform_button_rotate_right->set_shortcut(ED_SHORTCUT("tiles_editor/rotate_tile_right", TTR("Rotate Tile Right"), Key::X));
 	transform_toolbar->add_child(transform_button_rotate_right);
 	transform_button_rotate_right->connect(SceneStringName(pressed), callable_mp(this, &TileMapLayerEditorTilesPlugin::_apply_transform).bind(TRANSFORM_ROTATE_RIGHT));
 	viewport_shortcut_buttons.push_back(transform_button_rotate_right);
 
 	transform_button_flip_h = memnew(Button);
-	transform_button_flip_h->set_theme_type_variation("FlatButton");
+	transform_button_flip_h->set_theme_type_variation(SceneStringName(FlatButton));
 	transform_button_flip_h->set_shortcut(ED_SHORTCUT("tiles_editor/flip_tile_horizontal", TTR("Flip Tile Horizontally"), Key::C));
 	transform_toolbar->add_child(transform_button_flip_h);
 	transform_button_flip_h->connect(SceneStringName(pressed), callable_mp(this, &TileMapLayerEditorTilesPlugin::_apply_transform).bind(TRANSFORM_FLIP_H));
 	viewport_shortcut_buttons.push_back(transform_button_flip_h);
 
 	transform_button_flip_v = memnew(Button);
-	transform_button_flip_v->set_theme_type_variation("FlatButton");
+	transform_button_flip_v->set_theme_type_variation(SceneStringName(FlatButton));
 	transform_button_flip_v->set_shortcut(ED_SHORTCUT("tiles_editor/flip_tile_vertical", TTR("Flip Tile Vertically"), Key::V));
 	transform_toolbar->add_child(transform_button_flip_v);
 	transform_button_flip_v->connect(SceneStringName(pressed), callable_mp(this, &TileMapLayerEditorTilesPlugin::_apply_transform).bind(TRANSFORM_FLIP_V));
@@ -2329,7 +2329,7 @@ TileMapLayerEditorTilesPlugin::TileMapLayerEditorTilesPlugin() {
 
 	// Random tile checkbox.
 	random_tile_toggle = memnew(Button);
-	random_tile_toggle->set_theme_type_variation("FlatButton");
+	random_tile_toggle->set_theme_type_variation(SceneStringName(FlatButton));
 	random_tile_toggle->set_toggle_mode(true);
 	random_tile_toggle->set_tooltip_text(TTR("Place Random Tile"));
 	random_tile_toggle->connect(SceneStringName(toggled), callable_mp(this, &TileMapLayerEditorTilesPlugin::_on_random_tile_checkbox_toggled));
@@ -3553,7 +3553,7 @@ TileMapLayerEditorTerrainsPlugin::TileMapLayerEditorTerrainsPlugin() {
 	tool_buttons_group.instantiate();
 
 	paint_tool_button = memnew(Button);
-	paint_tool_button->set_theme_type_variation("FlatButton");
+	paint_tool_button->set_theme_type_variation(SceneStringName(FlatButton));
 	paint_tool_button->set_toggle_mode(true);
 	paint_tool_button->set_button_group(tool_buttons_group);
 	paint_tool_button->set_pressed(true);
@@ -3563,7 +3563,7 @@ TileMapLayerEditorTerrainsPlugin::TileMapLayerEditorTerrainsPlugin() {
 	viewport_shortcut_buttons.push_back(paint_tool_button);
 
 	line_tool_button = memnew(Button);
-	line_tool_button->set_theme_type_variation("FlatButton");
+	line_tool_button->set_theme_type_variation(SceneStringName(FlatButton));
 	line_tool_button->set_toggle_mode(true);
 	line_tool_button->set_button_group(tool_buttons_group);
 	line_tool_button->set_shortcut(ED_GET_SHORTCUT("tiles_editor/line_tool"));
@@ -3572,7 +3572,7 @@ TileMapLayerEditorTerrainsPlugin::TileMapLayerEditorTerrainsPlugin() {
 	viewport_shortcut_buttons.push_back(line_tool_button);
 
 	rect_tool_button = memnew(Button);
-	rect_tool_button->set_theme_type_variation("FlatButton");
+	rect_tool_button->set_theme_type_variation(SceneStringName(FlatButton));
 	rect_tool_button->set_toggle_mode(true);
 	rect_tool_button->set_button_group(tool_buttons_group);
 	rect_tool_button->set_shortcut(ED_GET_SHORTCUT("tiles_editor/rect_tool"));
@@ -3581,7 +3581,7 @@ TileMapLayerEditorTerrainsPlugin::TileMapLayerEditorTerrainsPlugin() {
 	viewport_shortcut_buttons.push_back(rect_tool_button);
 
 	bucket_tool_button = memnew(Button);
-	bucket_tool_button->set_theme_type_variation("FlatButton");
+	bucket_tool_button->set_theme_type_variation(SceneStringName(FlatButton));
 	bucket_tool_button->set_toggle_mode(true);
 	bucket_tool_button->set_button_group(tool_buttons_group);
 	bucket_tool_button->set_shortcut(ED_GET_SHORTCUT("tiles_editor/bucket_tool"));
@@ -3600,7 +3600,7 @@ TileMapLayerEditorTerrainsPlugin::TileMapLayerEditorTerrainsPlugin() {
 
 	// Picker
 	picker_button = memnew(Button);
-	picker_button->set_theme_type_variation("FlatButton");
+	picker_button->set_theme_type_variation(SceneStringName(FlatButton));
 	picker_button->set_toggle_mode(true);
 	picker_button->set_shortcut(ED_GET_SHORTCUT("tiles_editor/picker"));
 	picker_button->connect(SceneStringName(pressed), callable_mp(CanvasItemEditor::get_singleton(), &CanvasItemEditor::update_viewport));
@@ -3609,7 +3609,7 @@ TileMapLayerEditorTerrainsPlugin::TileMapLayerEditorTerrainsPlugin() {
 
 	// Erase button.
 	erase_button = memnew(Button);
-	erase_button->set_theme_type_variation("FlatButton");
+	erase_button->set_theme_type_variation(SceneStringName(FlatButton));
 	erase_button->set_toggle_mode(true);
 	erase_button->set_shortcut(ED_GET_SHORTCUT("tiles_editor/eraser"));
 	erase_button->connect(SceneStringName(pressed), callable_mp(CanvasItemEditor::get_singleton(), &CanvasItemEditor::update_viewport));
@@ -4467,19 +4467,19 @@ TileMapLayerEditor::TileMapLayerEditor() {
 	layer_selection_hbox->add_child(layers_selection_button);
 
 	select_previous_layer = memnew(Button);
-	select_previous_layer->set_theme_type_variation("FlatButton");
+	select_previous_layer->set_theme_type_variation(SceneStringName(FlatButton));
 	select_previous_layer->set_tooltip_text(TTR("Select previous layer"));
 	select_previous_layer->connect(SceneStringName(pressed), callable_mp(this, &TileMapLayerEditor::_select_previous_layer_pressed));
 	layer_selection_hbox->add_child(select_previous_layer);
 
 	select_next_layer = memnew(Button);
-	select_next_layer->set_theme_type_variation("FlatButton");
+	select_next_layer->set_theme_type_variation(SceneStringName(FlatButton));
 	select_next_layer->set_tooltip_text(TTR("Select next layer"));
 	select_next_layer->connect(SceneStringName(pressed), callable_mp(this, &TileMapLayerEditor::_select_next_layer_pressed));
 	layer_selection_hbox->add_child(select_next_layer);
 
 	select_all_layers = memnew(Button);
-	select_all_layers->set_theme_type_variation("FlatButton");
+	select_all_layers->set_theme_type_variation(SceneStringName(FlatButton));
 	select_all_layers->set_text(TTR("Select all layers"));
 	select_all_layers->connect(SceneStringName(pressed), callable_mp(this, &TileMapLayerEditor::_select_all_layers_pressed));
 	select_all_layers->set_tooltip_text(TTR("Select all TileMapLayers in scene"));
@@ -4487,7 +4487,7 @@ TileMapLayerEditor::TileMapLayerEditor() {
 
 	// Highlighting selected layer.
 	toggle_highlight_selected_layer_button = memnew(Button);
-	toggle_highlight_selected_layer_button->set_theme_type_variation("FlatButton");
+	toggle_highlight_selected_layer_button->set_theme_type_variation(SceneStringName(FlatButton));
 	toggle_highlight_selected_layer_button->set_toggle_mode(true);
 	toggle_highlight_selected_layer_button->set_pressed(true);
 	toggle_highlight_selected_layer_button->connect(SceneStringName(toggled), callable_mp(this, &TileMapLayerEditor::_highlight_selected_layer_button_toggled));
@@ -4498,7 +4498,7 @@ TileMapLayerEditor::TileMapLayerEditor() {
 
 	// Grid toggle.
 	toggle_grid_button = memnew(Button);
-	toggle_grid_button->set_theme_type_variation("FlatButton");
+	toggle_grid_button->set_theme_type_variation(SceneStringName(FlatButton));
 	toggle_grid_button->set_toggle_mode(true);
 	toggle_grid_button->set_tooltip_text(TTR("Toggle grid visibility."));
 	toggle_grid_button->connect(SceneStringName(toggled), callable_mp(this, &TileMapLayerEditor::_on_grid_toggled));
@@ -4507,7 +4507,7 @@ TileMapLayerEditor::TileMapLayerEditor() {
 	// Advanced settings menu button.
 	advanced_menu_button = memnew(MenuButton);
 	advanced_menu_button->set_flat(false);
-	advanced_menu_button->set_theme_type_variation("FlatButton");
+	advanced_menu_button->set_theme_type_variation(SceneStringName(FlatButton));
 	advanced_menu_button->get_popup()->add_item(TTR("Automatically Replace Tiles with Proxies"), ADVANCED_MENU_REPLACE_WITH_PROXIES);
 	advanced_menu_button->get_popup()->add_item(TTR("Extract TileMap layers as individual TileMapLayer nodes"), ADVANCED_MENU_EXTRACT_TILE_MAP_LAYERS);
 	advanced_menu_button->get_popup()->connect(SceneStringName(id_pressed), callable_mp(this, &TileMapLayerEditor::_advanced_menu_button_id_pressed));
diff --git a/editor/plugins/tiles/tile_set_atlas_source_editor.cpp b/editor/plugins/tiles/tile_set_atlas_source_editor.cpp
index 941d44c85e..85db510451 100644
--- a/editor/plugins/tiles/tile_set_atlas_source_editor.cpp
+++ b/editor/plugins/tiles/tile_set_atlas_source_editor.cpp
@@ -2527,7 +2527,7 @@ TileSetAtlasSourceEditor::TileSetAtlasSourceEditor() {
 
 	tool_setup_atlas_source_button = memnew(Button);
 	tool_setup_atlas_source_button->set_text(TTR("Setup"));
-	tool_setup_atlas_source_button->set_theme_type_variation("FlatButton");
+	tool_setup_atlas_source_button->set_theme_type_variation(SceneStringName(FlatButton));
 	tool_setup_atlas_source_button->set_toggle_mode(true);
 	tool_setup_atlas_source_button->set_pressed(true);
 	tool_setup_atlas_source_button->set_button_group(tools_button_group);
@@ -2536,7 +2536,7 @@ TileSetAtlasSourceEditor::TileSetAtlasSourceEditor() {
 
 	tool_select_button = memnew(Button);
 	tool_select_button->set_text(TTR("Select"));
-	tool_select_button->set_theme_type_variation("FlatButton");
+	tool_select_button->set_theme_type_variation(SceneStringName(FlatButton));
 	tool_select_button->set_toggle_mode(true);
 	tool_select_button->set_pressed(false);
 	tool_select_button->set_button_group(tools_button_group);
@@ -2545,7 +2545,7 @@ TileSetAtlasSourceEditor::TileSetAtlasSourceEditor() {
 
 	tool_paint_button = memnew(Button);
 	tool_paint_button->set_text(TTR("Paint"));
-	tool_paint_button->set_theme_type_variation("FlatButton");
+	tool_paint_button->set_theme_type_variation(SceneStringName(FlatButton));
 	tool_paint_button->set_toggle_mode(true);
 	tool_paint_button->set_button_group(tools_button_group);
 	toolbox->add_child(tool_paint_button);
@@ -2626,7 +2626,7 @@ TileSetAtlasSourceEditor::TileSetAtlasSourceEditor() {
 	tool_settings->add_child(tool_settings_tile_data_toolbar_container);
 
 	tools_settings_erase_button = memnew(Button);
-	tools_settings_erase_button->set_theme_type_variation("FlatButton");
+	tools_settings_erase_button->set_theme_type_variation(SceneStringName(FlatButton));
 	tools_settings_erase_button->set_toggle_mode(true);
 	tools_settings_erase_button->set_shortcut(ED_GET_SHORTCUT("tiles_editor/eraser"));
 	tools_settings_erase_button->set_shortcut_context(this);
diff --git a/editor/plugins/tiles/tile_set_editor.cpp b/editor/plugins/tiles/tile_set_editor.cpp
index dd64e38370..37b1161463 100644
--- a/editor/plugins/tiles/tile_set_editor.cpp
+++ b/editor/plugins/tiles/tile_set_editor.cpp
@@ -845,7 +845,7 @@ TileSetEditor::TileSetEditor() {
 
 	source_sort_button = memnew(MenuButton);
 	source_sort_button->set_flat(false);
-	source_sort_button->set_theme_type_variation("FlatButton");
+	source_sort_button->set_theme_type_variation(SceneStringName(FlatButton));
 	source_sort_button->set_tooltip_text(TTR("Sort Sources"));
 
 	PopupMenu *p = source_sort_button->get_popup();
@@ -876,14 +876,14 @@ TileSetEditor::TileSetEditor() {
 	split_container_left_side->add_child(sources_bottom_actions);
 
 	sources_delete_button = memnew(Button);
-	sources_delete_button->set_theme_type_variation("FlatButton");
+	sources_delete_button->set_theme_type_variation(SceneStringName(FlatButton));
 	sources_delete_button->set_disabled(true);
 	sources_delete_button->connect(SceneStringName(pressed), callable_mp(this, &TileSetEditor::_source_delete_pressed));
 	sources_bottom_actions->add_child(sources_delete_button);
 
 	sources_add_button = memnew(MenuButton);
 	sources_add_button->set_flat(false);
-	sources_add_button->set_theme_type_variation("FlatButton");
+	sources_add_button->set_theme_type_variation(SceneStringName(FlatButton));
 	sources_add_button->get_popup()->add_item(TTR("Atlas"));
 	sources_add_button->get_popup()->set_item_tooltip(-1, TTR("A palette of tiles made from a texture."));
 	sources_add_button->get_popup()->add_item(TTR("Scenes Collection"));
@@ -893,7 +893,7 @@ TileSetEditor::TileSetEditor() {
 
 	sources_advanced_menu_button = memnew(MenuButton);
 	sources_advanced_menu_button->set_flat(false);
-	sources_advanced_menu_button->set_theme_type_variation("FlatButton");
+	sources_advanced_menu_button->set_theme_type_variation(SceneStringName(FlatButton));
 	sources_advanced_menu_button->get_popup()->add_item(TTR("Open Atlas Merging Tool"));
 	sources_advanced_menu_button->get_popup()->add_item(TTR("Manage Tile Proxies"));
 	sources_advanced_menu_button->get_popup()->connect(SceneStringName(id_pressed), callable_mp(this, &TileSetEditor::_sources_advanced_menu_id_pressed));
diff --git a/editor/plugins/tiles/tile_set_scenes_collection_source_editor.cpp b/editor/plugins/tiles/tile_set_scenes_collection_source_editor.cpp
index e2b3c451b0..a4147be54a 100644
--- a/editor/plugins/tiles/tile_set_scenes_collection_source_editor.cpp
+++ b/editor/plugins/tiles/tile_set_scenes_collection_source_editor.cpp
@@ -571,12 +571,12 @@ TileSetScenesCollectionSourceEditor::TileSetScenesCollectionSourceEditor() {
 	right_vbox_container->add_child(scenes_bottom_actions);
 
 	scene_tile_add_button = memnew(Button);
-	scene_tile_add_button->set_theme_type_variation("FlatButton");
+	scene_tile_add_button->set_theme_type_variation(SceneStringName(FlatButton));
 	scene_tile_add_button->connect(SceneStringName(pressed), callable_mp(this, &TileSetScenesCollectionSourceEditor::_source_add_pressed));
 	scenes_bottom_actions->add_child(scene_tile_add_button);
 
 	scene_tile_delete_button = memnew(Button);
-	scene_tile_delete_button->set_theme_type_variation("FlatButton");
+	scene_tile_delete_button->set_theme_type_variation(SceneStringName(FlatButton));
 	scene_tile_delete_button->set_disabled(true);
 	scene_tile_delete_button->connect(SceneStringName(pressed), callable_mp(this, &TileSetScenesCollectionSourceEditor::_source_delete_pressed));
 	scenes_bottom_actions->add_child(scene_tile_delete_button);
diff --git a/editor/plugins/version_control_editor_plugin.cpp b/editor/plugins/version_control_editor_plugin.cpp
index 3a3d25ba9f..e5004de65d 100644
--- a/editor/plugins/version_control_editor_plugin.cpp
+++ b/editor/plugins/version_control_editor_plugin.cpp
@@ -1158,7 +1158,7 @@ VersionControlEditorPlugin::VersionControlEditorPlugin() {
 
 	refresh_button = memnew(Button);
 	refresh_button->set_tooltip_text(TTR("Detect new changes"));
-	refresh_button->set_theme_type_variation("FlatButton");
+	refresh_button->set_theme_type_variation(SceneStringName(FlatButton));
 	refresh_button->set_button_icon(EditorNode::get_singleton()->get_editor_theme()->get_icon(SNAME("Reload"), EditorStringName(EditorIcons)));
 	refresh_button->connect(SceneStringName(pressed), callable_mp(this, &VersionControlEditorPlugin::_refresh_stage_area));
 	refresh_button->connect(SceneStringName(pressed), callable_mp(this, &VersionControlEditorPlugin::_refresh_commit_list));
@@ -1181,11 +1181,11 @@ VersionControlEditorPlugin::VersionControlEditorPlugin() {
 	discard_all_button->set_tooltip_text(TTR("Discard all changes"));
 	discard_all_button->set_button_icon(EditorNode::get_singleton()->get_editor_theme()->get_icon(SNAME("Close"), EditorStringName(EditorIcons)));
 	discard_all_button->connect(SceneStringName(pressed), callable_mp(this, &VersionControlEditorPlugin::_confirm_discard_all));
-	discard_all_button->set_theme_type_variation("FlatButton");
+	discard_all_button->set_theme_type_variation(SceneStringName(FlatButton));
 	unstage_title->add_child(discard_all_button);
 
 	stage_all_button = memnew(Button);
-	stage_all_button->set_theme_type_variation("FlatButton");
+	stage_all_button->set_theme_type_variation(SceneStringName(FlatButton));
 	stage_all_button->set_button_icon(EditorNode::get_singleton()->get_editor_theme()->get_icon(SNAME("MoveDown"), EditorStringName(EditorIcons)));
 	stage_all_button->set_tooltip_text(TTR("Stage all changes"));
 	unstage_title->add_child(stage_all_button);
@@ -1215,7 +1215,7 @@ VersionControlEditorPlugin::VersionControlEditorPlugin() {
 	stage_title->add_child(stage_label);
 
 	unstage_all_button = memnew(Button);
-	unstage_all_button->set_theme_type_variation("FlatButton");
+	unstage_all_button->set_theme_type_variation(SceneStringName(FlatButton));
 	unstage_all_button->set_button_icon(EditorNode::get_singleton()->get_editor_theme()->get_icon(SNAME("MoveUp"), EditorStringName(EditorIcons)));
 	unstage_all_button->set_tooltip_text(TTR("Unstage all changes"));
 	stage_title->add_child(unstage_all_button);
@@ -1410,21 +1410,21 @@ VersionControlEditorPlugin::VersionControlEditorPlugin() {
 	remote_create_hbc->add_child(remote_create_url_input);
 
 	fetch_button = memnew(Button);
-	fetch_button->set_theme_type_variation("FlatButton");
+	fetch_button->set_theme_type_variation(SceneStringName(FlatButton));
 	fetch_button->set_tooltip_text(TTR("Fetch"));
 	fetch_button->set_button_icon(EditorNode::get_singleton()->get_editor_theme()->get_icon(SNAME("Reload"), EditorStringName(EditorIcons)));
 	fetch_button->connect(SceneStringName(pressed), callable_mp(this, &VersionControlEditorPlugin::_fetch));
 	menu_bar->add_child(fetch_button);
 
 	pull_button = memnew(Button);
-	pull_button->set_theme_type_variation("FlatButton");
+	pull_button->set_theme_type_variation(SceneStringName(FlatButton));
 	pull_button->set_tooltip_text(TTR("Pull"));
 	pull_button->set_button_icon(EditorNode::get_singleton()->get_editor_theme()->get_icon(SNAME("MoveDown"), EditorStringName(EditorIcons)));
 	pull_button->connect(SceneStringName(pressed), callable_mp(this, &VersionControlEditorPlugin::_pull));
 	menu_bar->add_child(pull_button);
 
 	push_button = memnew(Button);
-	push_button->set_theme_type_variation("FlatButton");
+	push_button->set_theme_type_variation(SceneStringName(FlatButton));
 	push_button->set_tooltip_text(TTR("Push"));
 	push_button->set_button_icon(EditorNode::get_singleton()->get_editor_theme()->get_icon(SNAME("MoveUp"), EditorStringName(EditorIcons)));
 	push_button->connect(SceneStringName(pressed), callable_mp(this, &VersionControlEditorPlugin::_push));
diff --git a/editor/plugins/visual_shader_editor_plugin.cpp b/editor/plugins/visual_shader_editor_plugin.cpp
index 9e1ffaed92..1162943b4a 100644
--- a/editor/plugins/visual_shader_editor_plugin.cpp
+++ b/editor/plugins/visual_shader_editor_plugin.cpp
@@ -6554,14 +6554,14 @@ VisualShaderEditor::VisualShaderEditor() {
 	varying_menu->connect(SceneStringName(id_pressed), callable_mp(this, &VisualShaderEditor::_varying_menu_id_pressed));
 
 	code_preview_button = memnew(Button);
-	code_preview_button->set_theme_type_variation("FlatButton");
+	code_preview_button->set_theme_type_variation(SceneStringName(FlatButton));
 	code_preview_button->set_toggle_mode(true);
 	code_preview_button->set_tooltip_text(TTR("Show generated shader code."));
 	toolbar->add_child(code_preview_button);
 	code_preview_button->connect(SceneStringName(pressed), callable_mp(this, &VisualShaderEditor::_show_preview_text));
 
 	shader_preview_button = memnew(Button);
-	shader_preview_button->set_theme_type_variation("FlatButton");
+	shader_preview_button->set_theme_type_variation(SceneStringName(FlatButton));
 	shader_preview_button->set_toggle_mode(true);
 	shader_preview_button->set_tooltip_text(TTR("Toggle shader preview."));
 	shader_preview_button->set_pressed(true);
diff --git a/editor/plugins/voxel_gi_editor_plugin.cpp b/editor/plugins/voxel_gi_editor_plugin.cpp
index 527138e060..245ef22bad 100644
--- a/editor/plugins/voxel_gi_editor_plugin.cpp
+++ b/editor/plugins/voxel_gi_editor_plugin.cpp
@@ -182,7 +182,7 @@ VoxelGIEditorPlugin::VoxelGIEditorPlugin() {
 	bake_hb->set_h_size_flags(Control::SIZE_EXPAND_FILL);
 	bake_hb->hide();
 	bake = memnew(Button);
-	bake->set_theme_type_variation("FlatButton");
+	bake->set_theme_type_variation(SceneStringName(FlatButton));
 	// TODO: Rework this as a dedicated toolbar control so we can hook into theme changes and update it
 	// when the editor theme updates.
 	bake->set_button_icon(EditorNode::get_singleton()->get_editor_theme()->get_icon(SNAME("Bake"), EditorStringName(EditorIcons)));
diff --git a/editor/scene_tree_dock.cpp b/editor/scene_tree_dock.cpp
index 87ba2e6875..20a7faadc4 100644
--- a/editor/scene_tree_dock.cpp
+++ b/editor/scene_tree_dock.cpp
@@ -4692,7 +4692,7 @@ SceneTreeDock::SceneTreeDock(Node *p_scene_root, EditorSelection *p_editor_selec
 	vbc->add_child(button_hb);
 
 	edit_remote = memnew(Button);
-	edit_remote->set_theme_type_variation("FlatButton");
+	edit_remote->set_theme_type_variation(SceneStringName(FlatButton));
 	edit_remote->set_h_size_flags(SIZE_EXPAND_FILL);
 	edit_remote->set_text(TTR("Remote"));
 	edit_remote->set_toggle_mode(true);
@@ -4701,7 +4701,7 @@ SceneTreeDock::SceneTreeDock(Node *p_scene_root, EditorSelection *p_editor_selec
 	edit_remote->connect(SceneStringName(pressed), callable_mp(this, &SceneTreeDock::_remote_tree_selected));
 
 	edit_local = memnew(Button);
-	edit_local->set_theme_type_variation("FlatButton");
+	edit_local->set_theme_type_variation(SceneStringName(FlatButton));
 	edit_local->set_h_size_flags(SIZE_EXPAND_FILL);
 	edit_local->set_text(TTR("Local"));
 	edit_local->set_toggle_mode(true);
diff --git a/editor/themes/editor_theme_manager.cpp b/editor/themes/editor_theme_manager.cpp
index 7f3adbc1b6..36b5f3d5c3 100644
--- a/editor/themes/editor_theme_manager.cpp
+++ b/editor/themes/editor_theme_manager.cpp
@@ -1990,10 +1990,10 @@ void EditorThemeManager::_populate_editor_styles(const Ref<EditorTheme> &p_theme
 			}
 			style_flat_button_pressed->set_bg_color(flat_pressed_color);
 
-			p_theme->set_stylebox(CoreStringName(normal), "FlatButton", style_flat_button);
-			p_theme->set_stylebox("hover", "FlatButton", style_flat_button_hover);
-			p_theme->set_stylebox(SceneStringName(pressed), "FlatButton", style_flat_button_pressed);
-			p_theme->set_stylebox("disabled", "FlatButton", style_flat_button);
+			p_theme->set_stylebox(CoreStringName(normal), SceneStringName(FlatButton), style_flat_button);
+			p_theme->set_stylebox("hover", SceneStringName(FlatButton), style_flat_button_hover);
+			p_theme->set_stylebox(SceneStringName(pressed), SceneStringName(FlatButton), style_flat_button_pressed);
+			p_theme->set_stylebox("disabled", SceneStringName(FlatButton), style_flat_button);
 
 			p_theme->set_stylebox(CoreStringName(normal), "FlatMenuButton", style_flat_button);
 			p_theme->set_stylebox("hover", "FlatMenuButton", style_flat_button_hover);
diff --git a/modules/gridmap/editor/grid_map_editor_plugin.cpp b/modules/gridmap/editor/grid_map_editor_plugin.cpp
index 26473b28e9..905067b69b 100644
--- a/modules/gridmap/editor/grid_map_editor_plugin.cpp
+++ b/modules/gridmap/editor/grid_map_editor_plugin.cpp
@@ -1296,7 +1296,7 @@ GridMapEditor::GridMapEditor() {
 	ED_SHORTCUT("grid_map/clear_rotation", TTR("Clear Rotation"));
 
 	options = memnew(MenuButton);
-	options->set_theme_type_variation("FlatButton");
+	options->set_theme_type_variation(SceneStringName(FlatButton));
 	options->get_popup()->add_separator();
 	options->get_popup()->add_radio_check_shortcut(ED_GET_SHORTCUT("grid_map/edit_x_axis"), MENU_OPTION_X_AXIS);
 	options->get_popup()->add_radio_check_shortcut(ED_GET_SHORTCUT("grid_map/edit_y_axis"), MENU_OPTION_Y_AXIS);
@@ -1335,7 +1335,7 @@ GridMapEditor::GridMapEditor() {
 	mode_buttons_group.instantiate();
 
 	transform_mode_button = memnew(Button);
-	transform_mode_button->set_theme_type_variation("FlatButton");
+	transform_mode_button->set_theme_type_variation(SceneStringName(FlatButton));
 	transform_mode_button->set_toggle_mode(true);
 	transform_mode_button->set_button_group(mode_buttons_group);
 	transform_mode_button->set_shortcut(ED_SHORTCUT("grid_map/transform_tool", TTR("Transform"), Key::T, true));
@@ -1347,7 +1347,7 @@ GridMapEditor::GridMapEditor() {
 	mode_buttons->add_child(vsep);
 
 	select_mode_button = memnew(Button);
-	select_mode_button->set_theme_type_variation("FlatButton");
+	select_mode_button->set_theme_type_variation(SceneStringName(FlatButton));
 	select_mode_button->set_toggle_mode(true);
 	select_mode_button->set_button_group(mode_buttons_group);
 	select_mode_button->set_shortcut(ED_SHORTCUT("grid_map/selection_tool", TTR("Selection"), Key::Q, true));
@@ -1357,7 +1357,7 @@ GridMapEditor::GridMapEditor() {
 	viewport_shortcut_buttons.push_back(select_mode_button);
 
 	erase_mode_button = memnew(Button);
-	erase_mode_button->set_theme_type_variation("FlatButton");
+	erase_mode_button->set_theme_type_variation(SceneStringName(FlatButton));
 	erase_mode_button->set_toggle_mode(true);
 	erase_mode_button->set_button_group(mode_buttons_group);
 	erase_mode_button->set_shortcut(ED_SHORTCUT("grid_map/erase_tool", TTR("Erase"), Key::W, true));
@@ -1367,7 +1367,7 @@ GridMapEditor::GridMapEditor() {
 	viewport_shortcut_buttons.push_back(erase_mode_button);
 
 	paint_mode_button = memnew(Button);
-	paint_mode_button->set_theme_type_variation("FlatButton");
+	paint_mode_button->set_theme_type_variation(SceneStringName(FlatButton));
 	paint_mode_button->set_toggle_mode(true);
 	paint_mode_button->set_button_group(mode_buttons_group);
 	paint_mode_button->set_shortcut(ED_SHORTCUT("grid_map/paint_tool", TTR("Paint"), Key::E, true));
@@ -1377,7 +1377,7 @@ GridMapEditor::GridMapEditor() {
 	viewport_shortcut_buttons.push_back(paint_mode_button);
 
 	pick_mode_button = memnew(Button);
-	pick_mode_button->set_theme_type_variation("FlatButton");
+	pick_mode_button->set_theme_type_variation(SceneStringName(FlatButton));
 	pick_mode_button->set_toggle_mode(true);
 	pick_mode_button->set_button_group(mode_buttons_group);
 	pick_mode_button->set_shortcut(ED_SHORTCUT("grid_map/pick_tool", TTR("Pick"), Key::R, true));
@@ -1393,7 +1393,7 @@ GridMapEditor::GridMapEditor() {
 	toolbar->add_child(action_buttons);
 
 	fill_action_button = memnew(Button);
-	fill_action_button->set_theme_type_variation("FlatButton");
+	fill_action_button->set_theme_type_variation(SceneStringName(FlatButton));
 	fill_action_button->set_shortcut(ED_SHORTCUT("grid_map/fill_tool", TTR("Fill"), Key::Z, true));
 	fill_action_button->connect(SceneStringName(pressed),
 			callable_mp(this, &GridMapEditor::_menu_option).bind(MENU_OPTION_SELECTION_FILL));
@@ -1401,7 +1401,7 @@ GridMapEditor::GridMapEditor() {
 	viewport_shortcut_buttons.push_back(fill_action_button);
 
 	move_action_button = memnew(Button);
-	move_action_button->set_theme_type_variation("FlatButton");
+	move_action_button->set_theme_type_variation(SceneStringName(FlatButton));
 	move_action_button->set_shortcut(ED_SHORTCUT("grid_map/move_tool", TTR("Move"), Key::X, true));
 	move_action_button->connect(SceneStringName(pressed),
 			callable_mp(this, &GridMapEditor::_menu_option).bind(MENU_OPTION_SELECTION_CUT));
@@ -1409,7 +1409,7 @@ GridMapEditor::GridMapEditor() {
 	viewport_shortcut_buttons.push_back(move_action_button);
 
 	duplicate_action_button = memnew(Button);
-	duplicate_action_button->set_theme_type_variation("FlatButton");
+	duplicate_action_button->set_theme_type_variation(SceneStringName(FlatButton));
 	duplicate_action_button->set_shortcut(ED_SHORTCUT("grid_map/duplicate_tool", TTR("Duplicate"), Key::C, true));
 	duplicate_action_button->connect(SceneStringName(pressed),
 			callable_mp(this, &GridMapEditor::_menu_option).bind(MENU_OPTION_SELECTION_DUPLICATE));
@@ -1417,7 +1417,7 @@ GridMapEditor::GridMapEditor() {
 	viewport_shortcut_buttons.push_back(duplicate_action_button);
 
 	delete_action_button = memnew(Button);
-	delete_action_button->set_theme_type_variation("FlatButton");
+	delete_action_button->set_theme_type_variation(SceneStringName(FlatButton));
 	delete_action_button->set_shortcut(ED_SHORTCUT("grid_map/delete_tool", TTR("Delete"), Key::V, true));
 	delete_action_button->connect(SceneStringName(pressed),
 			callable_mp(this, &GridMapEditor::_menu_option).bind(MENU_OPTION_SELECTION_CLEAR));
@@ -1431,7 +1431,7 @@ GridMapEditor::GridMapEditor() {
 	toolbar->add_child(rotation_buttons);
 
 	rotate_x_button = memnew(Button);
-	rotate_x_button->set_theme_type_variation("FlatButton");
+	rotate_x_button->set_theme_type_variation(SceneStringName(FlatButton));
 	rotate_x_button->set_shortcut(ED_SHORTCUT("grid_map/cursor_rotate_x", TTR("Cursor Rotate X"), Key::A, true));
 	rotate_x_button->connect(SceneStringName(pressed),
 			callable_mp(this, &GridMapEditor::_menu_option).bind(MENU_OPTION_CURSOR_ROTATE_X));
@@ -1439,7 +1439,7 @@ GridMapEditor::GridMapEditor() {
 	viewport_shortcut_buttons.push_back(rotate_x_button);
 
 	rotate_y_button = memnew(Button);
-	rotate_y_button->set_theme_type_variation("FlatButton");
+	rotate_y_button->set_theme_type_variation(SceneStringName(FlatButton));
 	rotate_y_button->set_shortcut(ED_SHORTCUT("grid_map/cursor_rotate_y", TTR("Cursor Rotate Y"), Key::S, true));
 	rotate_y_button->connect(SceneStringName(pressed),
 			callable_mp(this, &GridMapEditor::_menu_option).bind(MENU_OPTION_CURSOR_ROTATE_Y));
@@ -1447,7 +1447,7 @@ GridMapEditor::GridMapEditor() {
 	viewport_shortcut_buttons.push_back(rotate_y_button);
 
 	rotate_z_button = memnew(Button);
-	rotate_z_button->set_theme_type_variation("FlatButton");
+	rotate_z_button->set_theme_type_variation(SceneStringName(FlatButton));
 	rotate_z_button->set_shortcut(ED_SHORTCUT("grid_map/cursor_rotate_z", TTR("Cursor Rotate Z"), Key::D, true));
 	rotate_z_button->connect(SceneStringName(pressed),
 			callable_mp(this, &GridMapEditor::_menu_option).bind(MENU_OPTION_CURSOR_ROTATE_Z));
@@ -1492,14 +1492,14 @@ GridMapEditor::GridMapEditor() {
 	zoom_widget->set_shortcut_context(this);
 
 	mode_thumbnail = memnew(Button);
-	mode_thumbnail->set_theme_type_variation("FlatButton");
+	mode_thumbnail->set_theme_type_variation(SceneStringName(FlatButton));
 	mode_thumbnail->set_toggle_mode(true);
 	mode_thumbnail->set_pressed(true);
 	toolbar->add_child(mode_thumbnail);
 	mode_thumbnail->connect(SceneStringName(pressed), callable_mp(this, &GridMapEditor::_set_display_mode).bind(DISPLAY_THUMBNAIL));
 
 	mode_list = memnew(Button);
-	mode_list->set_theme_type_variation("FlatButton");
+	mode_list->set_theme_type_variation(SceneStringName(FlatButton));
 	mode_list->set_toggle_mode(true);
 	mode_list->set_pressed(false);
 	toolbar->add_child(mode_list);
diff --git a/modules/multiplayer/editor/replication_editor.cpp b/modules/multiplayer/editor/replication_editor.cpp
index b582d01921..191369d1de 100644
--- a/modules/multiplayer/editor/replication_editor.cpp
+++ b/modules/multiplayer/editor/replication_editor.cpp
@@ -249,7 +249,7 @@ ReplicationEditor::ReplicationEditor() {
 	hb->add_child(vs);
 
 	pin = memnew(Button);
-	pin->set_theme_type_variation("FlatButton");
+	pin->set_theme_type_variation(SceneStringName(FlatButton));
 	pin->set_toggle_mode(true);
 	pin->set_tooltip_text(TTR("Pin replication editor"));
 	hb->add_child(pin);
diff --git a/modules/navigation/editor/navigation_mesh_editor_plugin.cpp b/modules/navigation/editor/navigation_mesh_editor_plugin.cpp
index ed0cdeb287..b78093b590 100644
--- a/modules/navigation/editor/navigation_mesh_editor_plugin.cpp
+++ b/modules/navigation/editor/navigation_mesh_editor_plugin.cpp
@@ -130,7 +130,7 @@ NavigationMeshEditor::NavigationMeshEditor() {
 	bake_hbox = memnew(HBoxContainer);
 
 	button_bake = memnew(Button);
-	button_bake->set_theme_type_variation("FlatButton");
+	button_bake->set_theme_type_variation(SceneStringName(FlatButton));
 	bake_hbox->add_child(button_bake);
 	button_bake->set_toggle_mode(true);
 	button_bake->set_text(TTR("Bake NavigationMesh"));
@@ -138,7 +138,7 @@ NavigationMeshEditor::NavigationMeshEditor() {
 	button_bake->connect(SceneStringName(pressed), callable_mp(this, &NavigationMeshEditor::_bake_pressed));
 
 	button_reset = memnew(Button);
-	button_reset->set_theme_type_variation("FlatButton");
+	button_reset->set_theme_type_variation(SceneStringName(FlatButton));
 	bake_hbox->add_child(button_reset);
 	button_reset->set_text(TTR("Clear NavigationMesh"));
 	button_reset->set_tooltip_text(TTR("Clears the internal NavigationMesh vertices and polygons."));
diff --git a/scene/gui/file_dialog.cpp b/scene/gui/file_dialog.cpp
index 8fa14f5169..5512e74c04 100644
--- a/scene/gui/file_dialog.cpp
+++ b/scene/gui/file_dialog.cpp
@@ -1639,13 +1639,13 @@ FileDialog::FileDialog() {
 	HBoxContainer *hbc = memnew(HBoxContainer);
 
 	dir_prev = memnew(Button);
-	dir_prev->set_theme_type_variation("FlatButton");
+	dir_prev->set_theme_type_variation(SceneStringName(FlatButton));
 	dir_prev->set_tooltip_text(ETR("Go to previous folder."));
 	dir_next = memnew(Button);
-	dir_next->set_theme_type_variation("FlatButton");
+	dir_next->set_theme_type_variation(SceneStringName(FlatButton));
 	dir_next->set_tooltip_text(ETR("Go to next folder."));
 	dir_up = memnew(Button);
-	dir_up->set_theme_type_variation("FlatButton");
+	dir_up->set_theme_type_variation(SceneStringName(FlatButton));
 	dir_up->set_tooltip_text(ETR("Go to parent folder."));
 	hbc->add_child(dir_prev);
 	hbc->add_child(dir_next);
@@ -1669,13 +1669,13 @@ FileDialog::FileDialog() {
 	dir->set_h_size_flags(Control::SIZE_EXPAND_FILL);
 
 	refresh = memnew(Button);
-	refresh->set_theme_type_variation("FlatButton");
+	refresh->set_theme_type_variation(SceneStringName(FlatButton));
 	refresh->set_tooltip_text(ETR("Refresh files."));
 	refresh->connect(SceneStringName(pressed), callable_mp(this, &FileDialog::update_file_list));
 	hbc->add_child(refresh);
 
 	show_hidden = memnew(Button);
-	show_hidden->set_theme_type_variation("FlatButton");
+	show_hidden->set_theme_type_variation(SceneStringName(FlatButton));
 	show_hidden->set_toggle_mode(true);
 	show_hidden->set_pressed(is_showing_hidden_files());
 	show_hidden->set_tooltip_text(ETR("Toggle the visibility of hidden files."));
@@ -1683,7 +1683,7 @@ FileDialog::FileDialog() {
 	hbc->add_child(show_hidden);
 
 	show_filename_filter_button = memnew(Button);
-	show_filename_filter_button->set_theme_type_variation("FlatButton");
+	show_filename_filter_button->set_theme_type_variation(SceneStringName(FlatButton));
 	show_filename_filter_button->set_toggle_mode(true);
 	show_filename_filter_button->set_pressed(false);
 	show_filename_filter_button->set_tooltip_text(RTR("Toggle the visibility of the filter for file names."));
@@ -1694,7 +1694,7 @@ FileDialog::FileDialog() {
 	hbc->add_child(shortcuts_container);
 
 	makedir = memnew(Button);
-	makedir->set_theme_type_variation("FlatButton");
+	makedir->set_theme_type_variation(SceneStringName(FlatButton));
 	makedir->set_tooltip_text(ETR("Create a new folder."));
 	makedir->connect(SceneStringName(pressed), callable_mp(this, &FileDialog::_make_dir));
 	hbc->add_child(makedir);
diff --git a/scene/gui/graph_edit.cpp b/scene/gui/graph_edit.cpp
index 7d01f24298..bcf0f5bdc8 100644
--- a/scene/gui/graph_edit.cpp
+++ b/scene/gui/graph_edit.cpp
@@ -2916,7 +2916,7 @@ GraphEdit::GraphEdit() {
 	_update_zoom_label();
 
 	zoom_minus_button = memnew(Button);
-	zoom_minus_button->set_theme_type_variation("FlatButton");
+	zoom_minus_button->set_theme_type_variation(SceneStringName(FlatButton));
 	zoom_minus_button->set_visible(show_zoom_buttons);
 	zoom_minus_button->set_tooltip_text(ETR("Zoom Out"));
 	zoom_minus_button->set_focus_mode(FOCUS_NONE);
@@ -2924,7 +2924,7 @@ GraphEdit::GraphEdit() {
 	zoom_minus_button->connect(SceneStringName(pressed), callable_mp(this, &GraphEdit::_zoom_minus));
 
 	zoom_reset_button = memnew(Button);
-	zoom_reset_button->set_theme_type_variation("FlatButton");
+	zoom_reset_button->set_theme_type_variation(SceneStringName(FlatButton));
 	zoom_reset_button->set_visible(show_zoom_buttons);
 	zoom_reset_button->set_tooltip_text(ETR("Zoom Reset"));
 	zoom_reset_button->set_focus_mode(FOCUS_NONE);
@@ -2932,7 +2932,7 @@ GraphEdit::GraphEdit() {
 	zoom_reset_button->connect(SceneStringName(pressed), callable_mp(this, &GraphEdit::_zoom_reset));
 
 	zoom_plus_button = memnew(Button);
-	zoom_plus_button->set_theme_type_variation("FlatButton");
+	zoom_plus_button->set_theme_type_variation(SceneStringName(FlatButton));
 	zoom_plus_button->set_visible(show_zoom_buttons);
 	zoom_plus_button->set_tooltip_text(ETR("Zoom In"));
 	zoom_plus_button->set_focus_mode(FOCUS_NONE);
@@ -2942,7 +2942,7 @@ GraphEdit::GraphEdit() {
 	// Grid controls.
 
 	toggle_grid_button = memnew(Button);
-	toggle_grid_button->set_theme_type_variation("FlatButton");
+	toggle_grid_button->set_theme_type_variation(SceneStringName(FlatButton));
 	toggle_grid_button->set_visible(show_grid_buttons);
 	toggle_grid_button->set_toggle_mode(true);
 	toggle_grid_button->set_pressed(true);
@@ -2952,7 +2952,7 @@ GraphEdit::GraphEdit() {
 	toggle_grid_button->connect(SceneStringName(pressed), callable_mp(this, &GraphEdit::_show_grid_toggled));
 
 	toggle_snapping_button = memnew(Button);
-	toggle_snapping_button->set_theme_type_variation("FlatButton");
+	toggle_snapping_button->set_theme_type_variation(SceneStringName(FlatButton));
 	toggle_snapping_button->set_visible(show_grid_buttons);
 	toggle_snapping_button->set_toggle_mode(true);
 	toggle_snapping_button->set_tooltip_text(ETR("Toggle snapping to the grid."));
@@ -2974,7 +2974,7 @@ GraphEdit::GraphEdit() {
 	// Extra controls.
 
 	minimap_button = memnew(Button);
-	minimap_button->set_theme_type_variation("FlatButton");
+	minimap_button->set_theme_type_variation(SceneStringName(FlatButton));
 	minimap_button->set_visible(show_minimap_button);
 	minimap_button->set_toggle_mode(true);
 	minimap_button->set_tooltip_text(ETR("Toggle the graph minimap."));
@@ -2984,7 +2984,7 @@ GraphEdit::GraphEdit() {
 	minimap_button->connect(SceneStringName(pressed), callable_mp(this, &GraphEdit::_minimap_toggled));
 
 	arrange_button = memnew(Button);
-	arrange_button->set_theme_type_variation("FlatButton");
+	arrange_button->set_theme_type_variation(SceneStringName(FlatButton));
 	arrange_button->set_visible(show_arrange_button);
 	arrange_button->connect(SceneStringName(pressed), callable_mp(this, &GraphEdit::arrange_nodes));
 	arrange_button->set_focus_mode(FOCUS_NONE);
diff --git a/scene/scene_string_names.h b/scene/scene_string_names.h
index 6287bce6d3..47eaaa3c86 100644
--- a/scene/scene_string_names.h
+++ b/scene/scene_string_names.h
@@ -150,6 +150,8 @@ public:
 	const StringName text_changed = StaticCString::create("text_changed");
 	const StringName text_submitted = StaticCString::create("text_submitted");
 	const StringName value_changed = StaticCString::create("value_changed");
+
+	const StringName FlatButton = StaticCString::create("FlatButton");
 };
 
 #define SceneStringName(m_name) SceneStringNames::get_singleton()->m_name
diff --git a/scene/theme/default_theme.cpp b/scene/theme/default_theme.cpp
index 5195ea4d4b..b6de733cad 100644
--- a/scene/theme/default_theme.cpp
+++ b/scene/theme/default_theme.cpp
@@ -365,7 +365,7 @@ void fill_default_theme(Ref<Theme> &theme, const Ref<Font> &default_font, const
 
 	// Button variations
 
-	theme->set_type_variation("FlatButton", "Button");
+	theme->set_type_variation(SceneStringName(FlatButton), "Button");
 	theme->set_type_variation("FlatMenuButton", "MenuButton");
 
 	Ref<StyleBoxEmpty> flat_button_normal = make_empty_stylebox();
@@ -375,10 +375,10 @@ void fill_default_theme(Ref<Theme> &theme, const Ref<Font> &default_font, const
 	Ref<StyleBoxFlat> flat_button_pressed = button_pressed->duplicate();
 	flat_button_pressed->set_bg_color(style_pressed_color * Color(1, 1, 1, 0.85));
 
-	theme->set_stylebox(CoreStringName(normal), "FlatButton", flat_button_normal);
-	theme->set_stylebox("hover", "FlatButton", flat_button_normal);
-	theme->set_stylebox(SceneStringName(pressed), "FlatButton", flat_button_pressed);
-	theme->set_stylebox("disabled", "FlatButton", flat_button_normal);
+	theme->set_stylebox(CoreStringName(normal), SceneStringName(FlatButton), flat_button_normal);
+	theme->set_stylebox("hover", SceneStringName(FlatButton), flat_button_normal);
+	theme->set_stylebox(SceneStringName(pressed), SceneStringName(FlatButton), flat_button_pressed);
+	theme->set_stylebox("disabled", SceneStringName(FlatButton), flat_button_normal);
 
 	theme->set_stylebox(CoreStringName(normal), "FlatMenuButton", flat_button_normal);
 	theme->set_stylebox("hover", "FlatMenuButton", flat_button_normal);

From 9fece033ab11b97bb1c3b1ddeca7b5a21bab0771 Mon Sep 17 00:00:00 2001
From: bruvzg <7645683+bruvzg@users.noreply.github.com>
Date: Tue, 27 Feb 2024 22:19:47 +0200
Subject: [PATCH 058/124] [macOS, Windows] Add support for excluding windows
 from a screenshot.

---
 doc/classes/DisplayServer.xml               |  19 +-
 doc/classes/Window.xml                      |  10 +-
 platform/macos/display_server_macos.h       |   2 +
 platform/macos/display_server_macos.mm      | 188 ++++++++++++++------
 platform/windows/display_server_windows.cpp |  80 ++++++++-
 platform/windows/display_server_windows.h   |   8 +
 scene/gui/color_picker.cpp                  | 107 +++++++++--
 scene/gui/color_picker.h                    |   9 +-
 scene/main/window.cpp                       |   2 +
 scene/main/window.h                         |   2 +
 servers/display_server.cpp                  |   3 +
 servers/display_server.h                    |   4 +
 12 files changed, 357 insertions(+), 77 deletions(-)

diff --git a/doc/classes/DisplayServer.xml b/doc/classes/DisplayServer.xml
index fea38aad55..e84143e47b 100644
--- a/doc/classes/DisplayServer.xml
+++ b/doc/classes/DisplayServer.xml
@@ -1079,6 +1079,15 @@
 				[b]Note:[/b] On macOS, this method requires "Screen Recording" permission, if permission is not granted it will return desktop wallpaper color.
 			</description>
 		</method>
+		<method name="screen_get_image_rect" qualifiers="const">
+			<return type="Image" />
+			<param index="0" name="rect" type="Rect2i" />
+			<description>
+				Returns screenshot of the screen [param rect].
+				[b]Note:[/b] This method is implemented on macOS and Windows.
+				[b]Note:[/b] On macOS, this method requires "Screen Recording" permission, if permission is not granted it will return desktop wallpaper color.
+			</description>
+		</method>
 		<method name="screen_get_max_scale" qualifiers="const">
 			<return type="float" />
 			<description>
@@ -1913,6 +1922,9 @@
 		<constant name="FEATURE_WINDOW_DRAG" value="27" enum="Feature">
 			The display server supports initiating window drag operation on demand. See [method window_start_drag].
 		</constant>
+		<constant name="FEATURE_SCREEN_EXCLUDE_FROM_CAPTURE" value="28" enum="Feature">
+			Display server supports [constant WINDOW_FLAG_EXCLUDE_FROM_CAPTURE] window flag.
+		</constant>
 		<constant name="MOUSE_MODE_VISIBLE" value="0" enum="MouseMode">
 			Makes the mouse cursor visible if it is hidden.
 		</constant>
@@ -2128,7 +2140,12 @@
 			Window style is overridden, forcing sharp corners.
 			[b]Note:[/b] This flag is implemented only on Windows (11).
 		</constant>
-		<constant name="WINDOW_FLAG_MAX" value="9" enum="WindowFlags">
+		<constant name="WINDOW_FLAG_EXCLUDE_FROM_CAPTURE" value="9" enum="WindowFlags">
+			Windows is excluded from screenshots taken by [method screen_get_image], [method screen_get_image_rect], and [method screen_get_pixel].
+			[b]Note:[/b] This flag is implemented on macOS and Windows.
+			[b]Note:[/b] Setting this flag will [b]NOT[/b] prevent other apps from capturing an image, it should not be used as a security measure.
+		</constant>
+		<constant name="WINDOW_FLAG_MAX" value="10" enum="WindowFlags">
 			Max value of the [enum WindowFlags].
 		</constant>
 		<constant name="WINDOW_EVENT_MOUSE_ENTER" value="0" enum="WindowEvent">
diff --git a/doc/classes/Window.xml b/doc/classes/Window.xml
index 6d3294cc27..f779da8af9 100644
--- a/doc/classes/Window.xml
+++ b/doc/classes/Window.xml
@@ -586,6 +586,9 @@
 		<member name="current_screen" type="int" setter="set_current_screen" getter="get_current_screen">
 			The screen the window is currently on.
 		</member>
+		<member name="exclude_from_capture" type="bool" setter="set_flag" getter="get_flag" default="false">
+			Windows is excluded from screenshots taken by [method DisplayServer.screen_get_image], [method DisplayServer.screen_get_image_rect], and [method DisplayServer.screen_get_pixel].
+		</member>
 		<member name="exclusive" type="bool" setter="set_exclusive" getter="is_exclusive" default="false">
 			If [code]true[/code], the [Window] will be in exclusive mode. Exclusive windows are always on top of their parent and will block all input going to the parent [Window].
 			Needs [member transient] enabled to work.
@@ -853,7 +856,12 @@
 			[b]Note:[/b] This flag has no effect in embedded windows.
 			[b]Note:[/b] This flag is implemented only on Windows (11).
 		</constant>
-		<constant name="FLAG_MAX" value="9" enum="Flags">
+		<constant name="FLAG_EXCLUDE_FROM_CAPTURE" value="9" enum="Flags">
+			Windows is excluded from screenshots taken by [method DisplayServer.screen_get_image], [method DisplayServer.screen_get_image_rect], and [method DisplayServer.screen_get_pixel].
+			[b]Note:[/b] This flag is implemented on macOS and Windows.
+			[b]Note:[/b] Setting this flag will [b]NOT[/b] prevent other apps from capturing an image, it should not be used as a security measure.
+		</constant>
+		<constant name="FLAG_MAX" value="10" enum="Flags">
 			Max value of the [enum Flags].
 		</constant>
 		<constant name="CONTENT_SCALE_MODE_DISABLED" value="0" enum="ContentScaleMode">
diff --git a/platform/macos/display_server_macos.h b/platform/macos/display_server_macos.h
index 1cb4abc205..9f1f7d21e2 100644
--- a/platform/macos/display_server_macos.h
+++ b/platform/macos/display_server_macos.h
@@ -129,6 +129,7 @@ public:
 		bool focused = false;
 		bool is_visible = true;
 		bool extend_to_title = false;
+		bool hide_from_capture = false;
 
 		Rect2i parent_safe_rect;
 	};
@@ -326,6 +327,7 @@ public:
 	virtual float screen_get_refresh_rate(int p_screen = SCREEN_OF_MAIN_WINDOW) const override;
 	virtual Color screen_get_pixel(const Point2i &p_position) const override;
 	virtual Ref<Image> screen_get_image(int p_screen = SCREEN_OF_MAIN_WINDOW) const override;
+	virtual Ref<Image> screen_get_image_rect(const Rect2i &p_rect) const override;
 	virtual void screen_set_keep_on(bool p_enable) override;
 	virtual bool screen_is_kept_on() const override;
 
diff --git a/platform/macos/display_server_macos.mm b/platform/macos/display_server_macos.mm
index 476e508a55..472d54ae62 100644
--- a/platform/macos/display_server_macos.mm
+++ b/platform/macos/display_server_macos.mm
@@ -50,6 +50,8 @@
 #include "main/main.h"
 #include "scene/resources/image_texture.h"
 
+#include <AppKit/AppKit.h>
+
 #if defined(GLES3_ENABLED)
 #include "drivers/gles3/rasterizer_gles3.h"
 #endif
@@ -1609,35 +1611,41 @@ Rect2i DisplayServerMacOS::screen_get_usable_rect(int p_screen) const {
 }
 
 Color DisplayServerMacOS::screen_get_pixel(const Point2i &p_position) const {
-	Point2i position = p_position;
-	// macOS native y-coordinate relative to _get_screens_origin() is negative,
-	// Godot passes a positive value.
-	position.y *= -1;
-	position += _get_screens_origin();
+	HashSet<CGWindowID> exclude_windows;
+	for (HashMap<WindowID, WindowData>::ConstIterator E = windows.begin(); E; ++E) {
+		if (E->value.hide_from_capture) {
+			exclude_windows.insert([E->value.window_object windowNumber]);
+		}
+	}
+
+	CFArrayRef on_screen_windows = CGWindowListCreate(kCGWindowListOptionOnScreenOnly, kCGNullWindowID);
+	CFMutableArrayRef capture_windows = CFArrayCreateMutableCopy(nullptr, 0, on_screen_windows);
+	for (long i = CFArrayGetCount(on_screen_windows) - 1; i >= 0; i--) {
+		CGWindowID window = (CGWindowID)(uintptr_t)CFArrayGetValueAtIndex(capture_windows, i);
+		if (exclude_windows.has(window)) {
+			CFArrayRemoveValueAtIndex(capture_windows, i);
+		}
+	}
+
+	Point2i position = p_position - Vector2i(1, 1);
+	position -= screen_get_position(0); // Note: coordinates where the screen origin is in the upper-left corner of the main display and y-axis values increase downward.
 	position /= screen_get_max_scale();
 
 	Color color;
-	for (NSScreen *screen in [NSScreen screens]) {
-		NSRect frame = [screen frame];
-		if (NSMouseInRect(NSMakePoint(position.x, position.y), frame, NO)) {
-			NSDictionary *screenDescription = [screen deviceDescription];
-			CGDirectDisplayID display_id = [[screenDescription objectForKey:@"NSScreenNumber"] unsignedIntValue];
-			CGImageRef image = CGDisplayCreateImageForRect(display_id, CGRectMake(position.x - frame.origin.x, frame.size.height - (position.y - frame.origin.y), 1, 1));
-			if (image) {
-				CGColorSpaceRef color_space = CGColorSpaceCreateDeviceRGB();
-				if (color_space) {
-					uint8_t img_data[4];
-					CGContextRef context = CGBitmapContextCreate(img_data, 1, 1, 8, 4, color_space, kCGImageAlphaPremultipliedLast | kCGBitmapByteOrder32Big);
-					if (context) {
-						CGContextDrawImage(context, CGRectMake(0, 0, 1, 1), image);
-						color = Color(img_data[0] / 255.0f, img_data[1] / 255.0f, img_data[2] / 255.0f, img_data[3] / 255.0f);
-						CGContextRelease(context);
-					}
-					CGColorSpaceRelease(color_space);
-				}
-				CGImageRelease(image);
+	CGImageRef image = CGWindowListCreateImageFromArray(CGRectMake(position.x, position.y, 1, 1), capture_windows, kCGWindowListOptionAll);
+	if (image) {
+		CGColorSpaceRef color_space = CGColorSpaceCreateDeviceRGB();
+		if (color_space) {
+			uint8_t img_data[4];
+			CGContextRef context = CGBitmapContextCreate(img_data, 1, 1, 8, 4, color_space, kCGImageAlphaPremultipliedLast | kCGBitmapByteOrder32Big);
+			if (context) {
+				CGContextDrawImage(context, CGRectMake(0, 0, 1, 1), image);
+				color = Color(img_data[0] / 255.0f, img_data[1] / 255.0f, img_data[2] / 255.0f, img_data[3] / 255.0f);
+				CGContextRelease(context);
 			}
+			CGColorSpaceRelease(color_space);
 		}
+		CGImageRelease(image);
 	}
 	return color;
 }
@@ -1645,43 +1653,98 @@ Color DisplayServerMacOS::screen_get_pixel(const Point2i &p_position) const {
 Ref<Image> DisplayServerMacOS::screen_get_image(int p_screen) const {
 	ERR_FAIL_INDEX_V(p_screen, get_screen_count(), Ref<Image>());
 
-	switch (p_screen) {
-		case SCREEN_PRIMARY: {
-			p_screen = get_primary_screen();
-		} break;
-		case SCREEN_OF_MAIN_WINDOW: {
-			p_screen = window_get_current_screen(MAIN_WINDOW_ID);
-		} break;
-		default:
-			break;
+	HashSet<CGWindowID> exclude_windows;
+	for (HashMap<WindowID, WindowData>::ConstIterator E = windows.begin(); E; ++E) {
+		if (E->value.hide_from_capture) {
+			exclude_windows.insert([E->value.window_object windowNumber]);
+		}
 	}
 
-	Ref<Image> img;
-	NSArray *screenArray = [NSScreen screens];
-	if ((NSUInteger)p_screen < [screenArray count]) {
-		NSRect nsrect = [[screenArray objectAtIndex:p_screen] frame];
-		NSDictionary *screenDescription = [[screenArray objectAtIndex:p_screen] deviceDescription];
-		CGDirectDisplayID display_id = [[screenDescription objectForKey:@"NSScreenNumber"] unsignedIntValue];
-		CGImageRef image = CGDisplayCreateImageForRect(display_id, CGRectMake(0, 0, nsrect.size.width, nsrect.size.height));
-		if (image) {
-			CGColorSpaceRef color_space = CGColorSpaceCreateDeviceRGB();
-			if (color_space) {
-				NSUInteger width = CGImageGetWidth(image);
-				NSUInteger height = CGImageGetHeight(image);
-
-				Vector<uint8_t> img_data;
-				img_data.resize(height * width * 4);
-				CGContextRef context = CGBitmapContextCreate(img_data.ptrw(), width, height, 8, 4 * width, color_space, kCGImageAlphaPremultipliedLast | kCGBitmapByteOrder32Big);
-				if (context) {
-					CGContextDrawImage(context, CGRectMake(0, 0, width, height), image);
-					img = Image::create_from_data(width, height, false, Image::FORMAT_RGBA8, img_data);
-					CGContextRelease(context);
-				}
-				CGColorSpaceRelease(color_space);
-			}
-			CGImageRelease(image);
+	CFArrayRef on_screen_windows = CGWindowListCreate(kCGWindowListOptionOnScreenOnly, kCGNullWindowID);
+	CFMutableArrayRef capture_windows = CFArrayCreateMutableCopy(nullptr, 0, on_screen_windows);
+	for (long i = CFArrayGetCount(on_screen_windows) - 1; i >= 0; i--) {
+		CGWindowID window = (CGWindowID)(uintptr_t)CFArrayGetValueAtIndex(capture_windows, i);
+		if (exclude_windows.has(window)) {
+			CFArrayRemoveValueAtIndex(capture_windows, i);
 		}
 	}
+
+	Point2i position = screen_get_position(p_screen);
+	position -= screen_get_position(0); // Note: coordinates where the screen origin is in the upper-left corner of the main display and y-axis values increase downward.
+	position /= screen_get_max_scale();
+
+	Size2i size = screen_get_size(p_screen);
+	size /= screen_get_max_scale();
+
+	Ref<Image> img;
+	CGImageRef image = CGWindowListCreateImageFromArray(CGRectMake(position.x, position.y, size.width, size.height), capture_windows, kCGWindowListOptionAll);
+	if (image) {
+		CGColorSpaceRef color_space = CGColorSpaceCreateDeviceRGB();
+		if (color_space) {
+			NSUInteger width = CGImageGetWidth(image);
+			NSUInteger height = CGImageGetHeight(image);
+
+			Vector<uint8_t> img_data;
+			img_data.resize(height * width * 4);
+			CGContextRef context = CGBitmapContextCreate(img_data.ptrw(), width, height, 8, 4 * width, color_space, kCGImageAlphaPremultipliedLast | kCGBitmapByteOrder32Big);
+			if (context) {
+				CGContextDrawImage(context, CGRectMake(0, 0, width, height), image);
+				img = Image::create_from_data(width, height, false, Image::FORMAT_RGBA8, img_data);
+				CGContextRelease(context);
+				img->resize(screen_get_size(p_screen).x, screen_get_size(p_screen).y, Image::INTERPOLATE_NEAREST);
+			}
+			CGColorSpaceRelease(color_space);
+		}
+		CGImageRelease(image);
+	}
+	return img;
+}
+
+Ref<Image> DisplayServerMacOS::screen_get_image_rect(const Rect2i &p_rect) const {
+	HashSet<CGWindowID> exclude_windows;
+	for (HashMap<WindowID, WindowData>::ConstIterator E = windows.begin(); E; ++E) {
+		if (E->value.hide_from_capture) {
+			exclude_windows.insert([E->value.window_object windowNumber]);
+		}
+	}
+
+	CFArrayRef on_screen_windows = CGWindowListCreate(kCGWindowListOptionOnScreenOnly, kCGNullWindowID);
+	CFMutableArrayRef capture_windows = CFArrayCreateMutableCopy(nullptr, 0, on_screen_windows);
+	for (long i = CFArrayGetCount(on_screen_windows) - 1; i >= 0; i--) {
+		CGWindowID window = (CGWindowID)(uintptr_t)CFArrayGetValueAtIndex(capture_windows, i);
+		if (exclude_windows.has(window)) {
+			CFArrayRemoveValueAtIndex(capture_windows, i);
+		}
+	}
+
+	Point2i position = p_rect.position;
+	position -= screen_get_position(0); // Note: coordinates where the screen origin is in the upper-left corner of the main display and y-axis values increase downward.
+	position /= screen_get_max_scale();
+
+	Size2i size = p_rect.size;
+	size /= screen_get_max_scale();
+
+	Ref<Image> img;
+	CGImageRef image = CGWindowListCreateImageFromArray(CGRectMake(position.x, position.y, size.width, size.height), capture_windows, kCGWindowListOptionAll);
+	if (image) {
+		CGColorSpaceRef color_space = CGColorSpaceCreateDeviceRGB();
+		if (color_space) {
+			NSUInteger width = CGImageGetWidth(image);
+			NSUInteger height = CGImageGetHeight(image);
+
+			Vector<uint8_t> img_data;
+			img_data.resize_zeroed(height * width * 4);
+			CGContextRef context = CGBitmapContextCreate(img_data.ptrw(), width, height, 8, 4 * width, color_space, kCGImageAlphaPremultipliedLast | kCGBitmapByteOrder32Big);
+			if (context) {
+				CGContextDrawImage(context, CGRectMake(0, 0, width, height), image);
+				img = Image::create_from_data(width, height, false, Image::FORMAT_RGBA8, img_data);
+				CGContextRelease(context);
+				img->resize(p_rect.size.x, p_rect.size.y, Image::INTERPOLATE_NEAREST);
+			}
+			CGColorSpaceRelease(color_space);
+		}
+		CGImageRelease(image);
+	}
 	return img;
 }
 
@@ -2528,6 +2591,14 @@ void DisplayServerMacOS::window_set_flag(WindowFlags p_flag, bool p_enabled, Win
 			NSWindow *w = wd.window_object;
 			w.excludedFromWindowsMenu = wd.is_popup || wd.no_focus;
 		} break;
+		case WINDOW_FLAG_EXCLUDE_FROM_CAPTURE: {
+			if (p_enabled) {
+				[wd.window_object setSharingType:NSWindowSharingNone];
+			} else {
+				[wd.window_object setSharingType:NSWindowSharingReadWrite];
+			}
+			wd.hide_from_capture = p_enabled;
+		} break;
 		case WINDOW_FLAG_MOUSE_PASSTHROUGH: {
 			wd.mpass = p_enabled;
 		} break;
@@ -2573,6 +2644,9 @@ bool DisplayServerMacOS::window_get_flag(WindowFlags p_flag, WindowID p_window)
 		case WINDOW_FLAG_NO_FOCUS: {
 			return wd.no_focus;
 		} break;
+		case WINDOW_FLAG_EXCLUDE_FROM_CAPTURE: {
+			return wd.hide_from_capture;
+		} break;
 		case WINDOW_FLAG_MOUSE_PASSTHROUGH: {
 			return wd.mpass;
 		} break;
diff --git a/platform/windows/display_server_windows.cpp b/platform/windows/display_server_windows.cpp
index 271a93b122..0769603d8e 100644
--- a/platform/windows/display_server_windows.cpp
+++ b/platform/windows/display_server_windows.cpp
@@ -1395,6 +1395,62 @@ Ref<Image> DisplayServerWindows::screen_get_image(int p_screen) const {
 	return img;
 }
 
+Ref<Image> DisplayServerWindows::screen_get_image_rect(const Rect2i &p_rect) const {
+	Point2i pos = p_rect.position + _get_screens_origin();
+	Size2i size = p_rect.size;
+
+	POINT p1;
+	p1.x = pos.x;
+	p1.y = pos.y;
+
+	POINT p2;
+	p2.x = pos.x + size.x;
+	p2.y = pos.y + size.y;
+	if (win81p_LogicalToPhysicalPointForPerMonitorDPI) {
+		win81p_LogicalToPhysicalPointForPerMonitorDPI(0, &p1);
+		win81p_LogicalToPhysicalPointForPerMonitorDPI(0, &p2);
+	}
+
+	Ref<Image> img;
+	HDC dc = GetDC(0);
+	if (dc) {
+		HDC hdc = CreateCompatibleDC(dc);
+		int width = p2.x - p1.x;
+		int height = p2.y - p1.y;
+		if (hdc) {
+			HBITMAP hbm = CreateCompatibleBitmap(dc, width, height);
+			if (hbm) {
+				SelectObject(hdc, hbm);
+				BitBlt(hdc, 0, 0, width, height, dc, p1.x, p1.y, SRCCOPY);
+
+				BITMAPINFO bmp_info = {};
+				bmp_info.bmiHeader.biSize = sizeof(bmp_info.bmiHeader);
+				bmp_info.bmiHeader.biWidth = width;
+				bmp_info.bmiHeader.biHeight = -height;
+				bmp_info.bmiHeader.biPlanes = 1;
+				bmp_info.bmiHeader.biBitCount = 32;
+				bmp_info.bmiHeader.biCompression = BI_RGB;
+
+				Vector<uint8_t> img_data;
+				img_data.resize(width * height * 4);
+				GetDIBits(hdc, hbm, 0, height, img_data.ptrw(), &bmp_info, DIB_RGB_COLORS);
+
+				uint8_t *wr = (uint8_t *)img_data.ptrw();
+				for (int i = 0; i < width * height; i++) {
+					SWAP(wr[i * 4 + 0], wr[i * 4 + 2]); // Swap B and R.
+				}
+				img = Image::create_from_data(width, height, false, Image::FORMAT_RGBA8, img_data);
+
+				DeleteObject(hbm);
+			}
+			DeleteDC(hdc);
+		}
+		ReleaseDC(NULL, dc);
+	}
+
+	return img;
+}
+
 float DisplayServerWindows::screen_get_refresh_rate(int p_screen) const {
 	_THREAD_SAFE_METHOD_
 
@@ -1507,6 +1563,14 @@ DisplayServer::WindowID DisplayServerWindows::create_sub_window(WindowMode p_mod
 	if (p_flags & WINDOW_FLAG_MOUSE_PASSTHROUGH_BIT) {
 		wd.mpass = true;
 	}
+	if (p_flags & WINDOW_FLAG_EXCLUDE_FROM_CAPTURE_BIT) {
+		wd.hide_from_capture = true;
+		if (os_ver.dwBuildNumber >= 19041) {
+			SetWindowDisplayAffinity(wd.hWnd, WDA_EXCLUDEFROMCAPTURE);
+		} else {
+			SetWindowDisplayAffinity(wd.hWnd, WDA_MONITOR);
+		}
+	}
 	if (p_flags & WINDOW_FLAG_POPUP_BIT) {
 		wd.is_popup = true;
 	}
@@ -2395,6 +2459,18 @@ void DisplayServerWindows::window_set_flag(WindowFlags p_flag, bool p_enabled, W
 			wd.mpass = p_enabled;
 			_update_window_mouse_passthrough(p_window);
 		} break;
+		case WINDOW_FLAG_EXCLUDE_FROM_CAPTURE: {
+			wd.hide_from_capture = p_enabled;
+			if (p_enabled) {
+				if (os_ver.dwBuildNumber >= 19041) {
+					SetWindowDisplayAffinity(wd.hWnd, WDA_EXCLUDEFROMCAPTURE);
+				} else {
+					SetWindowDisplayAffinity(wd.hWnd, WDA_MONITOR);
+				}
+			} else {
+				SetWindowDisplayAffinity(wd.hWnd, WDA_NONE);
+			}
+		} break;
 		case WINDOW_FLAG_POPUP: {
 			ERR_FAIL_COND_MSG(p_window == MAIN_WINDOW_ID, "Main window can't be popup.");
 			ERR_FAIL_COND_MSG(IsWindowVisible(wd.hWnd) && (wd.is_popup != p_enabled), "Popup flag can't changed while window is opened.");
@@ -2432,6 +2508,9 @@ bool DisplayServerWindows::window_get_flag(WindowFlags p_flag, WindowID p_window
 		case WINDOW_FLAG_MOUSE_PASSTHROUGH: {
 			return wd.mpass;
 		} break;
+		case WINDOW_FLAG_EXCLUDE_FROM_CAPTURE: {
+			return wd.hide_from_capture;
+		} break;
 		case WINDOW_FLAG_POPUP: {
 			return wd.is_popup;
 		} break;
@@ -6083,7 +6162,6 @@ DisplayServerWindows::DisplayServerWindows(const String &p_rendering_driver, Win
 	screen_set_keep_on(GLOBAL_GET("display/window/energy_saving/keep_screen_on"));
 
 	// Load Windows version info.
-	OSVERSIONINFOW os_ver;
 	ZeroMemory(&os_ver, sizeof(OSVERSIONINFOW));
 	os_ver.dwOSVersionInfoSize = sizeof(OSVERSIONINFOW);
 
diff --git a/platform/windows/display_server_windows.h b/platform/windows/display_server_windows.h
index c6b8693a9d..af5243a149 100644
--- a/platform/windows/display_server_windows.h
+++ b/platform/windows/display_server_windows.h
@@ -362,6 +362,10 @@ typedef enum _SHC_PROCESS_DPI_AWARENESS {
 
 class DropTargetWindows;
 
+#ifndef WDA_EXCLUDEFROMCAPTURE
+#define WDA_EXCLUDEFROMCAPTURE 0x00000011
+#endif
+
 class DisplayServerWindows : public DisplayServer {
 	// No need to register with GDCLASS, it's platform-specific and nothing is added.
 
@@ -415,6 +419,8 @@ class DisplayServerWindows : public DisplayServer {
 		TIMER_ID_WINDOW_ACTIVATION = 2,
 	};
 
+	OSVERSIONINFOW os_ver;
+
 	enum {
 		KEY_EVENT_BUFFER_SIZE = 512
 	};
@@ -483,6 +489,7 @@ class DisplayServerWindows : public DisplayServer {
 		bool context_created = false;
 		bool mpass = false;
 		bool sharp_corners = false;
+		bool hide_from_capture = false;
 
 		// Used to transfer data between events using timer.
 		WPARAM saved_wparam;
@@ -714,6 +721,7 @@ public:
 	virtual float screen_get_refresh_rate(int p_screen = SCREEN_OF_MAIN_WINDOW) const override;
 	virtual Color screen_get_pixel(const Point2i &p_position) const override;
 	virtual Ref<Image> screen_get_image(int p_screen = SCREEN_OF_MAIN_WINDOW) const override;
+	virtual Ref<Image> screen_get_image_rect(const Rect2i &p_rect) const override;
 
 	virtual void screen_set_keep_on(bool p_enable) override; //disable screensaver
 	virtual bool screen_is_kept_on() const override;
diff --git a/scene/gui/color_picker.cpp b/scene/gui/color_picker.cpp
index d12caad2f6..e04ad55fd2 100644
--- a/scene/gui/color_picker.cpp
+++ b/scene/gui/color_picker.cpp
@@ -40,10 +40,12 @@
 #include "scene/gui/margin_container.h"
 #include "scene/gui/menu_button.h"
 #include "scene/gui/option_button.h"
+#include "scene/gui/panel.h"
 #include "scene/gui/popup_menu.h"
 #include "scene/gui/slider.h"
 #include "scene/gui/spin_box.h"
 #include "scene/gui/texture_rect.h"
+#include "scene/resources/atlas_texture.h"
 #include "scene/resources/color_palette.h"
 #include "scene/resources/image_texture.h"
 #include "scene/resources/style_box_flat.h"
@@ -155,7 +157,19 @@ void ColorPicker::_notification(int p_what) {
 			if (!is_picking_color) {
 				return;
 			}
-			set_pick_color(DisplayServer::get_singleton()->screen_get_pixel(DisplayServer::get_singleton()->mouse_get_position()));
+			DisplayServer *ds = DisplayServer::get_singleton();
+			Vector2 ofs = ds->mouse_get_position();
+			picker_window->set_position(ofs - Vector2(28, 28));
+
+			Color c = DisplayServer::get_singleton()->screen_get_pixel(DisplayServer::get_singleton()->mouse_get_position());
+
+			picker_preview_style_box_color->set_bg_color(c);
+			picker_preview_style_box->set_bg_color(c.get_luminance() < 0.5 ? Color(1.0f, 1.0f, 1.0f) : Color(0.0f, 0.0f, 0.0f));
+
+			Ref<Image> zoom_preview_img = ds->screen_get_image_rect(Rect2i(ofs.x - 8, ofs.y - 8, 17, 17));
+			picker_texture_zoom->set_texture(ImageTexture::create_from_image(zoom_preview_img));
+
+			set_pick_color(c);
 		}
 	}
 }
@@ -1685,17 +1699,54 @@ void ColorPicker::_add_preset_pressed() {
 
 void ColorPicker::_pick_button_pressed() {
 	is_picking_color = true;
-	set_process_internal(true);
 
 	if (!picker_window) {
 		picker_window = memnew(Popup);
-		picker_window->set_size(Vector2i(1, 1));
+		picker_window->set_size(Vector2i(55, 72));
 		picker_window->connect(SceneStringName(visibility_changed), callable_mp(this, &ColorPicker::_pick_finished));
+		picker_window->connect(SceneStringName(window_input), callable_mp(this, &ColorPicker::_target_gui_input));
+
+		picker_window->set_flag(Window::FLAG_EXCLUDE_FROM_CAPTURE, true);
+
+		picker_preview = memnew(Panel);
+		picker_preview->set_mouse_filter(MOUSE_FILTER_IGNORE);
+		picker_preview->set_size(Vector2i(55, 72));
+		picker_window->add_child(picker_preview);
+
+		picker_preview_color = memnew(Panel);
+		picker_preview_color->set_mouse_filter(MOUSE_FILTER_IGNORE);
+		picker_preview_color->set_size(Vector2i(51, 15));
+		picker_preview_color->set_position(Vector2i(2, 55));
+		picker_preview->add_child(picker_preview_color);
+
+		picker_texture_zoom = memnew(TextureRect);
+		picker_texture_zoom->set_mouse_filter(MOUSE_FILTER_IGNORE);
+		picker_texture_zoom->set_custom_minimum_size(Vector2i(51, 51));
+		picker_texture_zoom->set_position(Vector2i(2, 2));
+		picker_texture_zoom->set_texture_filter(CanvasItem::TEXTURE_FILTER_NEAREST);
+		picker_preview->add_child(picker_texture_zoom);
+
+		picker_preview_style_box.instantiate();
+		picker_preview->add_theme_style_override(SceneStringName(panel), picker_preview_style_box);
+
+		picker_preview_style_box_color.instantiate();
+		picker_preview_color->add_theme_style_override(SceneStringName(panel), picker_preview_style_box_color);
+
 		add_child(picker_window, false, INTERNAL_MODE_FRONT);
 	}
+	set_process_internal(true);
+
 	picker_window->popup();
 }
 
+void ColorPicker::_target_gui_input(const Ref<InputEvent> &p_event) {
+	const Ref<InputEventMouseButton> mouse_event = p_event;
+	if (mouse_event.is_valid() && mouse_event->is_pressed()) {
+		picker_window->hide();
+		_pick_finished();
+	}
+}
+
 void ColorPicker::_pick_finished() {
 	if (picker_window->is_visible()) {
 		return;
@@ -1825,27 +1876,46 @@ void ColorPicker::_pick_button_pressed_legacy() {
 		picker_texture_rect = memnew(TextureRect);
 		picker_texture_rect->set_anchors_preset(Control::PRESET_FULL_RECT);
 		picker_texture_rect->set_expand_mode(TextureRect::EXPAND_IGNORE_SIZE);
+		picker_texture_rect->set_default_cursor_shape(Control::CURSOR_CROSS);
 		picker_window->add_child(picker_texture_rect);
-		picker_texture_rect->set_default_cursor_shape(CURSOR_POINTING_HAND);
 		picker_texture_rect->connect(SceneStringName(gui_input), callable_mp(this, &ColorPicker::_picker_texture_input));
 
-		picker_preview_label = memnew(Label);
-		picker_preview_label->set_anchors_preset(Control::PRESET_CENTER_TOP);
-		picker_preview_label->set_text(ETR("Color Picking active"));
+		picker_preview = memnew(Panel);
+		picker_preview->set_mouse_filter(MOUSE_FILTER_IGNORE);
+		picker_preview->set_size(Vector2i(55, 72));
+		picker_window->add_child(picker_preview);
+
+		picker_preview_color = memnew(Panel);
+		picker_preview_color->set_mouse_filter(MOUSE_FILTER_IGNORE);
+		picker_preview_color->set_size(Vector2i(51, 15));
+		picker_preview_color->set_position(Vector2i(2, 55));
+		picker_preview->add_child(picker_preview_color);
+
+		picker_texture_zoom = memnew(TextureRect);
+		picker_texture_zoom->set_mouse_filter(MOUSE_FILTER_IGNORE);
+		picker_texture_zoom->set_custom_minimum_size(Vector2i(51, 51));
+		picker_texture_zoom->set_position(Vector2i(2, 2));
+		picker_texture_zoom->set_texture_filter(CanvasItem::TEXTURE_FILTER_NEAREST);
+		picker_preview->add_child(picker_texture_zoom);
 
 		picker_preview_style_box.instantiate();
-		picker_preview_style_box->set_bg_color(Color(1.0, 1.0, 1.0));
-		picker_preview_style_box->set_content_margin_all(4.0);
-		picker_preview_label->add_theme_style_override(CoreStringName(normal), picker_preview_style_box);
+		picker_preview->add_theme_style_override(SceneStringName(panel), picker_preview_style_box);
 
-		picker_window->add_child(picker_preview_label);
+		picker_preview_style_box_color.instantiate();
+		picker_preview_color->add_theme_style_override(SceneStringName(panel), picker_preview_style_box_color);
 	}
 
 	Rect2i screen_rect;
 	if (picker_window->is_embedded()) {
+		Ref<ImageTexture> tx = ImageTexture::create_from_image(picker_window->get_embedder()->get_texture()->get_image());
 		screen_rect = picker_window->get_embedder()->get_visible_rect();
 		picker_window->set_position(Point2i());
-		picker_texture_rect->set_texture(ImageTexture::create_from_image(picker_window->get_embedder()->get_texture()->get_image()));
+		picker_texture_rect->set_texture(tx);
+
+		Ref<AtlasTexture> atlas;
+		atlas.instantiate();
+		atlas->set_atlas(tx);
+		picker_texture_zoom->set_texture(atlas);
 	} else {
 		screen_rect = picker_window->get_parent_rect();
 		picker_window->set_position(screen_rect.position);
@@ -1874,7 +1944,6 @@ void ColorPicker::_pick_button_pressed_legacy() {
 	}
 
 	picker_window->set_size(screen_rect.size);
-	picker_preview_label->set_custom_minimum_size(screen_rect.size / 10); // 10% of size in each axis.
 	picker_window->popup();
 }
 
@@ -1895,9 +1964,17 @@ void ColorPicker::_picker_texture_input(const Ref<InputEvent> &p_event) {
 		Ref<Image> img = picker_texture_rect->get_texture()->get_image();
 		if (img.is_valid() && !img->is_empty()) {
 			Vector2 ofs = mev->get_position();
+			picker_preview->set_position(ofs - Vector2(28, 28));
+			Vector2 scale = picker_texture_rect->get_size() / img->get_size();
+			ofs /= scale;
 			picker_color = img->get_pixel(ofs.x, ofs.y);
-			picker_preview_style_box->set_bg_color(picker_color);
-			picker_preview_label->add_theme_color_override(SceneStringName(font_color), picker_color.get_luminance() < 0.5 ? Color(1.0f, 1.0f, 1.0f) : Color(0.0f, 0.0f, 0.0f));
+			picker_preview_style_box_color->set_bg_color(picker_color);
+			picker_preview_style_box->set_bg_color(picker_color.get_luminance() < 0.5 ? Color(1.0f, 1.0f, 1.0f) : Color(0.0f, 0.0f, 0.0f));
+
+			Ref<AtlasTexture> atlas = picker_texture_zoom->get_texture();
+			if (atlas.is_valid()) {
+				atlas->set_region(Rect2i(ofs.x - 8, ofs.y - 8, 17, 17));
+			}
 		}
 	}
 }
diff --git a/scene/gui/color_picker.h b/scene/gui/color_picker.h
index 183bbae072..829263fbf5 100644
--- a/scene/gui/color_picker.h
+++ b/scene/gui/color_picker.h
@@ -133,10 +133,14 @@ private:
 	Vector<ColorMode *> modes;
 
 	Popup *picker_window = nullptr;
+	TextureRect *picker_texture_zoom = nullptr;
+	Panel *picker_preview = nullptr;
+	Panel *picker_preview_color = nullptr;
+	Ref<StyleBoxFlat> picker_preview_style_box;
+	Ref<StyleBoxFlat> picker_preview_style_box_color;
+
 	// Legacy color picking.
 	TextureRect *picker_texture_rect = nullptr;
-	Label *picker_preview_label = nullptr;
-	Ref<StyleBoxFlat> picker_preview_style_box;
 	Color picker_color;
 	FileDialog *file_dialog = nullptr;
 	Button *menu_btn = nullptr;
@@ -297,6 +301,7 @@ private:
 	void _add_preset_pressed();
 	void _html_focus_exit();
 	void _pick_button_pressed();
+	void _target_gui_input(const Ref<InputEvent> &p_event);
 	void _pick_finished();
 	void _update_menu_items();
 	void _update_menu();
diff --git a/scene/main/window.cpp b/scene/main/window.cpp
index 33e8b81991..71fac3df99 100644
--- a/scene/main/window.cpp
+++ b/scene/main/window.cpp
@@ -3008,6 +3008,7 @@ void Window::_bind_methods() {
 	ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "extend_to_title"), "set_flag", "get_flag", FLAG_EXTEND_TO_TITLE);
 	ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "mouse_passthrough"), "set_flag", "get_flag", FLAG_MOUSE_PASSTHROUGH);
 	ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "sharp_corners"), "set_flag", "get_flag", FLAG_SHARP_CORNERS);
+	ADD_PROPERTYI(PropertyInfo(Variant::BOOL, "exclude_from_capture"), "set_flag", "get_flag", FLAG_EXCLUDE_FROM_CAPTURE);
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "force_native"), "set_force_native", "get_force_native");
 
 	ADD_GROUP("Limits", "");
@@ -3062,6 +3063,7 @@ void Window::_bind_methods() {
 	BIND_ENUM_CONSTANT(FLAG_EXTEND_TO_TITLE);
 	BIND_ENUM_CONSTANT(FLAG_MOUSE_PASSTHROUGH);
 	BIND_ENUM_CONSTANT(FLAG_SHARP_CORNERS);
+	BIND_ENUM_CONSTANT(FLAG_EXCLUDE_FROM_CAPTURE);
 	BIND_ENUM_CONSTANT(FLAG_MAX);
 
 	BIND_ENUM_CONSTANT(CONTENT_SCALE_MODE_DISABLED);
diff --git a/scene/main/window.h b/scene/main/window.h
index 6c82efcc3f..86b1d7c71a 100644
--- a/scene/main/window.h
+++ b/scene/main/window.h
@@ -33,6 +33,7 @@
 
 #include "scene/main/viewport.h"
 #include "scene/resources/theme.h"
+#include "servers/display_server.h"
 
 class Font;
 class Shortcut;
@@ -63,6 +64,7 @@ public:
 		FLAG_EXTEND_TO_TITLE = DisplayServer::WINDOW_FLAG_EXTEND_TO_TITLE,
 		FLAG_MOUSE_PASSTHROUGH = DisplayServer::WINDOW_FLAG_MOUSE_PASSTHROUGH,
 		FLAG_SHARP_CORNERS = DisplayServer::WINDOW_FLAG_SHARP_CORNERS,
+		FLAG_EXCLUDE_FROM_CAPTURE = DisplayServer::WINDOW_FLAG_EXCLUDE_FROM_CAPTURE,
 		FLAG_MAX = DisplayServer::WINDOW_FLAG_MAX,
 	};
 
diff --git a/servers/display_server.cpp b/servers/display_server.cpp
index ff9a07f397..ff274a2f69 100644
--- a/servers/display_server.cpp
+++ b/servers/display_server.cpp
@@ -903,6 +903,7 @@ void DisplayServer::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("screen_get_refresh_rate", "screen"), &DisplayServer::screen_get_refresh_rate, DEFVAL(SCREEN_OF_MAIN_WINDOW));
 	ClassDB::bind_method(D_METHOD("screen_get_pixel", "position"), &DisplayServer::screen_get_pixel);
 	ClassDB::bind_method(D_METHOD("screen_get_image", "screen"), &DisplayServer::screen_get_image, DEFVAL(SCREEN_OF_MAIN_WINDOW));
+	ClassDB::bind_method(D_METHOD("screen_get_image_rect", "rect"), &DisplayServer::screen_get_image_rect);
 
 	ClassDB::bind_method(D_METHOD("screen_set_orientation", "orientation", "screen"), &DisplayServer::screen_set_orientation, DEFVAL(SCREEN_OF_MAIN_WINDOW));
 	ClassDB::bind_method(D_METHOD("screen_get_orientation", "screen"), &DisplayServer::screen_get_orientation, DEFVAL(SCREEN_OF_MAIN_WINDOW));
@@ -1065,6 +1066,7 @@ void DisplayServer::_bind_methods() {
 	BIND_ENUM_CONSTANT(FEATURE_NATIVE_DIALOG_FILE);
 	BIND_ENUM_CONSTANT(FEATURE_NATIVE_DIALOG_FILE_EXTRA);
 	BIND_ENUM_CONSTANT(FEATURE_WINDOW_DRAG);
+	BIND_ENUM_CONSTANT(FEATURE_SCREEN_EXCLUDE_FROM_CAPTURE);
 
 	BIND_ENUM_CONSTANT(MOUSE_MODE_VISIBLE);
 	BIND_ENUM_CONSTANT(MOUSE_MODE_HIDDEN);
@@ -1138,6 +1140,7 @@ void DisplayServer::_bind_methods() {
 	BIND_ENUM_CONSTANT(WINDOW_FLAG_EXTEND_TO_TITLE);
 	BIND_ENUM_CONSTANT(WINDOW_FLAG_MOUSE_PASSTHROUGH);
 	BIND_ENUM_CONSTANT(WINDOW_FLAG_SHARP_CORNERS);
+	BIND_ENUM_CONSTANT(WINDOW_FLAG_EXCLUDE_FROM_CAPTURE);
 	BIND_ENUM_CONSTANT(WINDOW_FLAG_MAX);
 
 	BIND_ENUM_CONSTANT(WINDOW_EVENT_MOUSE_ENTER);
diff --git a/servers/display_server.h b/servers/display_server.h
index 805d2374ab..93d03f5554 100644
--- a/servers/display_server.h
+++ b/servers/display_server.h
@@ -154,6 +154,7 @@ public:
 		FEATURE_NATIVE_DIALOG_FILE,
 		FEATURE_NATIVE_DIALOG_FILE_EXTRA,
 		FEATURE_WINDOW_DRAG,
+		FEATURE_SCREEN_EXCLUDE_FROM_CAPTURE,
 	};
 
 	virtual bool has_feature(Feature p_feature) const = 0;
@@ -346,6 +347,7 @@ public:
 	virtual float screen_get_refresh_rate(int p_screen = SCREEN_OF_MAIN_WINDOW) const = 0;
 	virtual Color screen_get_pixel(const Point2i &p_position) const { return Color(); }
 	virtual Ref<Image> screen_get_image(int p_screen = SCREEN_OF_MAIN_WINDOW) const { return Ref<Image>(); }
+	virtual Ref<Image> screen_get_image_rect(const Rect2i &p_rect) const { return Ref<Image>(); }
 	virtual bool is_touchscreen_available() const;
 
 	// Keep the ScreenOrientation enum values in sync with the `display/window/handheld/orientation`
@@ -387,6 +389,7 @@ public:
 		WINDOW_FLAG_EXTEND_TO_TITLE,
 		WINDOW_FLAG_MOUSE_PASSTHROUGH,
 		WINDOW_FLAG_SHARP_CORNERS,
+		WINDOW_FLAG_EXCLUDE_FROM_CAPTURE,
 		WINDOW_FLAG_MAX,
 	};
 
@@ -401,6 +404,7 @@ public:
 		WINDOW_FLAG_EXTEND_TO_TITLE_BIT = (1 << WINDOW_FLAG_EXTEND_TO_TITLE),
 		WINDOW_FLAG_MOUSE_PASSTHROUGH_BIT = (1 << WINDOW_FLAG_MOUSE_PASSTHROUGH),
 		WINDOW_FLAG_SHARP_CORNERS_BIT = (1 << WINDOW_FLAG_SHARP_CORNERS),
+		WINDOW_FLAG_EXCLUDE_FROM_CAPTURE_BIT = (1 << WINDOW_FLAG_EXCLUDE_FROM_CAPTURE),
 	};
 
 	virtual WindowID create_sub_window(WindowMode p_mode, VSyncMode p_vsync_mode, uint32_t p_flags, const Rect2i &p_rect = Rect2i(), bool p_exclusive = false, WindowID p_transient_parent = INVALID_WINDOW_ID);

From 2153a60425729b322a9062a9cc705d94bb870c29 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?R=C3=A9mi=20Verschelde?= <rverschelde@gmail.com>
Date: Tue, 10 Dec 2024 10:09:58 +0100
Subject: [PATCH 059/124] Revert "Avoid duplicating signals from scene
 instances into packed scenes"

This partially reverts commit 8a42e3d3eff8f8ef44d663cbe735c2bb05170cba.

Comment improvements and the test case were kept, with one part commented out.
---
 core/object/object.h             | 2 +-
 scene/resources/packed_scene.cpp | 6 ------
 tests/scene/test_packed_scene.h  | 3 +++
 3 files changed, 4 insertions(+), 7 deletions(-)

diff --git a/core/object/object.h b/core/object/object.h
index da8e8d1b86..11b94a7fbf 100644
--- a/core/object/object.h
+++ b/core/object/object.h
@@ -572,7 +572,7 @@ public:
 		CONNECT_PERSIST = 2, // hint for scene to save this connection
 		CONNECT_ONE_SHOT = 4,
 		CONNECT_REFERENCE_COUNTED = 8,
-		CONNECT_INHERITED = 16, // Whether or not the connection is in an instance of a scene.
+		CONNECT_INHERITED = 16, // Used in editor builds.
 	};
 
 	struct Connection {
diff --git a/scene/resources/packed_scene.cpp b/scene/resources/packed_scene.cpp
index 8b1cbdf488..d6748ed2e6 100644
--- a/scene/resources/packed_scene.cpp
+++ b/scene/resources/packed_scene.cpp
@@ -1060,12 +1060,6 @@ Error SceneState::_parse_connections(Node *p_owner, Node *p_node, HashMap<String
 				continue;
 			}
 
-			// Don't include signals that are from scene instances
-			// (they are already saved in the scenes themselves).
-			if (c.flags & CONNECT_INHERITED) {
-				continue;
-			}
-
 			// only connections that originate or end into main saved scene are saved
 			// everything else is discarded
 
diff --git a/tests/scene/test_packed_scene.h b/tests/scene/test_packed_scene.h
index 3698b00c6b..df23c251c0 100644
--- a/tests/scene/test_packed_scene.h
+++ b/tests/scene/test_packed_scene.h
@@ -96,6 +96,8 @@ TEST_CASE("[PackedScene] Signals Preserved when Packing Scene") {
 		CHECK_EQ(state->get_connection_count(), 3);
 	}
 
+	/*
+	// FIXME: This subcase requires GH-48064 to be fixed.
 	SUBCASE("Signals that should not be saved") {
 		int subscene_flags = Object::CONNECT_PERSIST | Object::CONNECT_INHERITED;
 		// subscene node to itself
@@ -115,6 +117,7 @@ TEST_CASE("[PackedScene] Signals Preserved when Packing Scene") {
 		Ref<SceneState> state = packed_scene->get_state();
 		CHECK_EQ(state->get_connection_count(), 0);
 	}
+	*/
 
 	memdelete(main_scene_root);
 }

From a8caad3437617786aaf3e9d6e7ece9a444741d75 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?R=C3=A9mi=20Verschelde?= <rverschelde@gmail.com>
Date: Mon, 9 Dec 2024 21:19:46 +0100
Subject: [PATCH 060/124] [Debugger] Fix scene debugger crash when editor
 shortcuts are invalid

---
 scene/debugger/scene_debugger.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/scene/debugger/scene_debugger.cpp b/scene/debugger/scene_debugger.cpp
index 993a825582..f369aec569 100644
--- a/scene/debugger/scene_debugger.cpp
+++ b/scene/debugger/scene_debugger.cpp
@@ -96,7 +96,7 @@ void SceneDebugger::deinitialize() {
 #ifdef DEBUG_ENABLED
 void SceneDebugger::_handle_input(const Ref<InputEvent> &p_event, const Ref<Shortcut> &p_shortcut) {
 	Ref<InputEventKey> k = p_event;
-	if (k.is_valid() && k->is_pressed() && !k->is_echo() && p_shortcut->matches_event(k)) {
+	if (p_shortcut.is_valid() && k.is_valid() && k->is_pressed() && !k->is_echo() && p_shortcut->matches_event(k)) {
 		EngineDebugger::get_singleton()->send_message("request_quit", Array());
 	}
 }

From 12f682bdc292a6400803eeddef35473e1eddd3b2 Mon Sep 17 00:00:00 2001
From: Gustavo Jaruga Cruz <gustavojarugacruz@gmail.com>
Date: Mon, 19 Aug 2024 02:51:07 -0300
Subject: [PATCH 061/124] Exposes interactive audio playback current playing
 clip.

---
 .../interactive_music/audio_stream_interactive.cpp   |  5 +++++
 modules/interactive_music/audio_stream_interactive.h |  1 +
 .../doc_classes/AudioStreamPlaybackInteractive.xml   | 12 ++++++++++++
 3 files changed, 18 insertions(+)

diff --git a/modules/interactive_music/audio_stream_interactive.cpp b/modules/interactive_music/audio_stream_interactive.cpp
index 8472c4e352..fb67b1a782 100644
--- a/modules/interactive_music/audio_stream_interactive.cpp
+++ b/modules/interactive_music/audio_stream_interactive.cpp
@@ -1011,6 +1011,10 @@ void AudioStreamPlaybackInteractive::switch_to_clip(int p_index) {
 	switch_request = p_index;
 }
 
+int AudioStreamPlaybackInteractive::get_current_clip_index() const {
+	return playback_current;
+}
+
 int AudioStreamPlaybackInteractive::get_loop_count() const {
 	return 0; // Looping not supported
 }
@@ -1026,4 +1030,5 @@ bool AudioStreamPlaybackInteractive::is_playing() const {
 void AudioStreamPlaybackInteractive::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("switch_to_clip_by_name", "clip_name"), &AudioStreamPlaybackInteractive::switch_to_clip_by_name);
 	ClassDB::bind_method(D_METHOD("switch_to_clip", "clip_index"), &AudioStreamPlaybackInteractive::switch_to_clip);
+	ClassDB::bind_method(D_METHOD("get_current_clip_index"), &AudioStreamPlaybackInteractive::get_current_clip_index);
 }
diff --git a/modules/interactive_music/audio_stream_interactive.h b/modules/interactive_music/audio_stream_interactive.h
index 12d3ce8aad..fd9521f3b2 100644
--- a/modules/interactive_music/audio_stream_interactive.h
+++ b/modules/interactive_music/audio_stream_interactive.h
@@ -259,6 +259,7 @@ public:
 
 	void switch_to_clip_by_name(const StringName &p_name);
 	void switch_to_clip(int p_index);
+	int get_current_clip_index() const;
 
 	virtual void set_parameter(const StringName &p_name, const Variant &p_value) override;
 	virtual Variant get_parameter(const StringName &p_name) const override;
diff --git a/modules/interactive_music/doc_classes/AudioStreamPlaybackInteractive.xml b/modules/interactive_music/doc_classes/AudioStreamPlaybackInteractive.xml
index c87d7c8fcb..86b608b02f 100644
--- a/modules/interactive_music/doc_classes/AudioStreamPlaybackInteractive.xml
+++ b/modules/interactive_music/doc_classes/AudioStreamPlaybackInteractive.xml
@@ -9,6 +9,18 @@
 	<tutorials>
 	</tutorials>
 	<methods>
+		<method name="get_current_clip_index" qualifiers="const">
+			<return type="int" />
+			<description>
+				Return the index of the currently playing clip. You can use this to get the name of the currently playing clip with [method AudioStreamInteractive.get_clip_name].
+				[b]Example:[/b] Get the currently playing clip name from inside an [AudioStreamPlayer] node.
+				[codeblocks]
+				[gdscript]
+				var playing_clip_name = stream.get_clip_name(get_stream_playback().get_current_clip_index())
+				[/gdscript]
+				[/codeblocks]
+			</description>
+		</method>
 		<method name="switch_to_clip">
 			<return type="void" />
 			<param index="0" name="clip_index" type="int" />

From ab0a2517b11a662c4932ec59cdfa71dcc967cc37 Mon Sep 17 00:00:00 2001
From: BlueCube3310 <53150244+BlueCube3310@users.noreply.github.com>
Date: Tue, 10 Dec 2024 14:57:10 +0100
Subject: [PATCH 062/124] Consider all texture types for resource thumbnail
 generation

---
 editor/plugins/editor_preview_plugins.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/editor/plugins/editor_preview_plugins.cpp b/editor/plugins/editor_preview_plugins.cpp
index 3618c0e6d3..321f9c55e7 100644
--- a/editor/plugins/editor_preview_plugins.cpp
+++ b/editor/plugins/editor_preview_plugins.cpp
@@ -77,7 +77,7 @@ void post_process_preview(Ref<Image> p_image) {
 }
 
 bool EditorTexturePreviewPlugin::handles(const String &p_type) const {
-	return ClassDB::is_parent_class(p_type, "Texture2D") || ClassDB::is_parent_class(p_type, "Texture3D") || ClassDB::is_parent_class(p_type, "TextureLayered");
+	return ClassDB::is_parent_class(p_type, "Texture");
 }
 
 bool EditorTexturePreviewPlugin::generate_small_preview_automatically() const {

From e2ed286d2d47f6539fadd9d1107fa359e3b0c6f0 Mon Sep 17 00:00:00 2001
From: Robert Yevdokimov <105675984+ryevdokimov@users.noreply.github.com>
Date: Sat, 7 Dec 2024 21:21:17 -0500
Subject: [PATCH 063/124] Add measure mode to 3D

---
 editor/plugins/node_3d_editor_plugin.cpp | 139 +++++++++++++++++++++--
 editor/plugins/node_3d_editor_plugin.h   |  16 ++-
 2 files changed, 145 insertions(+), 10 deletions(-)

diff --git a/editor/plugins/node_3d_editor_plugin.cpp b/editor/plugins/node_3d_editor_plugin.cpp
index 5d11cc7bc5..bf65aff5cf 100644
--- a/editor/plugins/node_3d_editor_plugin.cpp
+++ b/editor/plugins/node_3d_editor_plugin.cpp
@@ -1817,6 +1817,12 @@ void Node3DEditorViewport::_sinput(const Ref<InputEvent> &p_event) {
 						break;
 					}
 
+					if (spatial_editor->get_tool_mode() == Node3DEditor::TOOL_RULER) {
+						EditorNode::get_singleton()->get_scene_root()->add_child(ruler);
+						collision_reposition = true;
+						break;
+					}
+
 					if (spatial_editor->get_tool_mode() == Node3DEditor::TOOL_MODE_LIST_SELECT) {
 						_list_select(b);
 						break;
@@ -1953,6 +1959,15 @@ void Node3DEditorViewport::_sinput(const Ref<InputEvent> &p_event) {
 
 					surface->queue_redraw();
 				} else {
+					if (spatial_editor->get_tool_mode() == Node3DEditor::TOOL_RULER) {
+						EditorNode::get_singleton()->get_scene_root()->remove_child(ruler);
+						ruler_start_point->set_visible(false);
+						ruler_end_point->set_visible(false);
+						ruler_label->set_visible(false);
+						collision_reposition = false;
+						break;
+					}
+
 					if (_edit.gizmo.is_valid()) {
 						_edit.gizmo->commit_handle(_edit.gizmo_handle, _edit.gizmo_handle_secondary, _edit.gizmo_initial_value, false);
 						spatial_editor->get_single_selected_node()->update_gizmos();
@@ -2881,6 +2896,24 @@ void Node3DEditorViewport::_notification(int p_what) {
 		} break;
 
 		case NOTIFICATION_PROCESS: {
+			if (ruler->is_inside_tree()) {
+				Vector3 start_pos = ruler_start_point->get_global_position();
+				Vector3 end_pos = ruler_end_point->get_global_position();
+
+				geometry->clear_surfaces();
+				geometry->surface_begin(Mesh::PRIMITIVE_LINES);
+				geometry->surface_add_vertex(start_pos);
+				geometry->surface_add_vertex(end_pos);
+				geometry->surface_end();
+
+				float distance = start_pos.distance_to(end_pos);
+				ruler_label->set_text(TS->format_number(vformat("%.3f m", distance)));
+
+				Vector3 center = (start_pos + end_pos) / 2;
+				Vector2 screen_position = camera->unproject_position(center) - (ruler_label->get_custom_minimum_size() / 2);
+				ruler_label->set_position(screen_position);
+			}
+
 			real_t delta = get_process_delta_time();
 
 			if (zoom_indicator_delay > 0) {
@@ -3093,15 +3126,37 @@ void Node3DEditorViewport::_notification(int p_what) {
 
 		case NOTIFICATION_PHYSICS_PROCESS: {
 			if (collision_reposition) {
-				List<Node *> &selection = editor_selection->get_selected_node_list();
+				Node3D *selected_node = nullptr;
 
-				if (selection.size() == 1) {
-					Node3D *first_selected_node = Object::cast_to<Node3D>(selection.front()->get());
-					double snap = EDITOR_GET("interface/inspector/default_float_step");
-					int snap_step_decimals = Math::range_step_decimals(snap);
-					set_message(TTR("Translating:") + " (" + String::num(first_selected_node->get_global_position().x, snap_step_decimals) + ", " +
-							String::num(first_selected_node->get_global_position().y, snap_step_decimals) + ", " + String::num(first_selected_node->get_global_position().z, snap_step_decimals) + ")");
-					first_selected_node->set_global_position(spatial_editor->snap_point(_get_instance_position(_edit.mouse_pos, first_selected_node)));
+				if (ruler->is_inside_tree()) {
+					if (ruler_start_point->is_visible()) {
+						selected_node = ruler_end_point;
+					} else {
+						selected_node = ruler_start_point;
+					}
+				} else {
+					List<Node *> &selection = editor_selection->get_selected_node_list();
+					if (selection.size() == 1) {
+						selected_node = Object::cast_to<Node3D>(selection.front()->get());
+					}
+				}
+
+				if (selected_node) {
+					if (!ruler->is_inside_tree()) {
+						double snap = EDITOR_GET("interface/inspector/default_float_step");
+						int snap_step_decimals = Math::range_step_decimals(snap);
+						set_message(TTR("Translating:") + " (" + String::num(selected_node->get_global_position().x, snap_step_decimals) + ", " +
+								String::num(selected_node->get_global_position().y, snap_step_decimals) + ", " + String::num(selected_node->get_global_position().z, snap_step_decimals) + ")");
+					}
+
+					selected_node->set_global_position(spatial_editor->snap_point(_get_instance_position(_edit.mouse_pos, selected_node)));
+
+					if (ruler->is_inside_tree() && !ruler_start_point->is_visible()) {
+						ruler_end_point->set_global_position(ruler_start_point->get_global_position());
+						ruler_start_point->set_visible(true);
+						ruler_end_point->set_visible(true);
+						ruler_label->set_visible(true);
+					}
 				}
 			}
 
@@ -4310,7 +4365,7 @@ Vector3 Node3DEditorViewport::_get_instance_position(const Point2 &p_pos, Node3D
 
 	HashSet<RID> rids;
 
-	if (!preview_node->is_inside_tree()) {
+	if (!preview_node->is_inside_tree() && !ruler->is_inside_tree()) {
 		List<Node *> &selection = editor_selection->get_selected_node_list();
 
 		Node3D *first_selected_node = Object::cast_to<Node3D>(selection.front()->get());
@@ -5738,6 +5793,53 @@ Node3DEditorViewport::Node3DEditorViewport(Node3DEditor *p_spatial_editor, int p
 		viewport->set_as_audio_listener_3d(true);
 	}
 
+	ruler = memnew(Node);
+
+	ruler_start_point = memnew(Node3D);
+	ruler_start_point->set_visible(false);
+
+	ruler_end_point = memnew(Node3D);
+	ruler_end_point->set_visible(false);
+
+	ruler_material.instantiate();
+	ruler_material->set_albedo(Color(1.0, 0.9, 0.0, 1.0));
+	ruler_material->set_flag(BaseMaterial3D::FLAG_DISABLE_FOG, true);
+	ruler_material->set_shading_mode(BaseMaterial3D::SHADING_MODE_UNSHADED);
+	ruler_material->set_depth_draw_mode(BaseMaterial3D::DEPTH_DRAW_DISABLED);
+
+	ruler_material_xray.instantiate();
+	ruler_material_xray->set_albedo(Color(1.0, 0.9, 0.0, 0.15));
+	ruler_material_xray->set_flag(BaseMaterial3D::FLAG_DISABLE_FOG, true);
+	ruler_material_xray->set_shading_mode(BaseMaterial3D::SHADING_MODE_UNSHADED);
+	ruler_material_xray->set_flag(BaseMaterial3D::FLAG_DISABLE_DEPTH_TEST, true);
+	ruler_material_xray->set_transparency(BaseMaterial3D::TRANSPARENCY_ALPHA);
+	ruler_material_xray->set_render_priority(BaseMaterial3D::RENDER_PRIORITY_MAX);
+
+	geometry.instantiate();
+
+	ruler_line = memnew(MeshInstance3D);
+	ruler_line->set_mesh(geometry);
+	ruler_line->set_material_override(ruler_material);
+
+	ruler_line_xray = memnew(MeshInstance3D);
+	ruler_line_xray->set_mesh(geometry);
+	ruler_line_xray->set_material_override(ruler_material_xray);
+
+	ruler_label = memnew(Label);
+	ruler_label->add_theme_color_override(SceneStringName(font_color), Color(1.0, 0.9, 0.0, 1.0));
+	ruler_label->add_theme_color_override("font_outline_color", Color(0.0, 0.0, 0.0, 1.0));
+	ruler_label->add_theme_constant_override("outline_size", 4 * EDSCALE);
+	ruler_label->add_theme_font_size_override(SceneStringName(font_size), 15 * EDSCALE);
+	ruler_label->add_theme_font_override(SceneStringName(font), get_theme_font(SNAME("bold"), EditorStringName(EditorFonts)));
+	ruler_label->set_visible(false);
+
+	ruler->add_child(ruler_start_point);
+	ruler->add_child(ruler_end_point);
+	ruler->add_child(ruler_line);
+	ruler->add_child(ruler_line_xray);
+
+	viewport->add_child(ruler_label);
+
 	view_type = VIEW_TYPE_USER;
 	_update_name();
 
@@ -5745,6 +5847,7 @@ Node3DEditorViewport::Node3DEditorViewport(Node3DEditor *p_spatial_editor, int p
 }
 
 Node3DEditorViewport::~Node3DEditorViewport() {
+	memdelete(ruler);
 	memdelete(frame_time_gradient);
 }
 
@@ -6859,6 +6962,14 @@ void Node3DEditor::_menu_item_pressed(int p_option) {
 			undo_redo->add_undo_method(this, "_refresh_menu_icons");
 			undo_redo->commit_action();
 		} break;
+		case MENU_RULER: {
+			for (int i = 0; i < TOOL_MAX; i++) {
+				tool_button[i]->set_pressed(i == p_option);
+			}
+			tool_button[TOOL_RULER]->set_pressed(true);
+			tool_mode = ToolMode::TOOL_RULER;
+			update_transform_gizmo();
+		} break;
 	}
 }
 
@@ -8023,6 +8134,7 @@ void Node3DEditor::_update_theme() {
 	tool_button[TOOL_UNLOCK_SELECTED]->set_button_icon(get_editor_theme_icon(SNAME("Unlock")));
 	tool_button[TOOL_GROUP_SELECTED]->set_button_icon(get_editor_theme_icon(SNAME("Group")));
 	tool_button[TOOL_UNGROUP_SELECTED]->set_button_icon(get_editor_theme_icon(SNAME("Ungroup")));
+	tool_button[TOOL_RULER]->set_button_icon(get_editor_theme_icon(SNAME("Ruler")));
 
 	tool_option_button[TOOL_OPT_LOCAL_COORDS]->set_button_icon(get_editor_theme_icon(SNAME("Object")));
 	tool_option_button[TOOL_OPT_USE_SNAP]->set_button_icon(get_editor_theme_icon(SNAME("Snap")));
@@ -8772,6 +8884,15 @@ Node3DEditor::Node3DEditor() {
 	// Define the shortcut globally (without a context) so that it works if the Scene tree dock is currently focused.
 	tool_button[TOOL_UNGROUP_SELECTED]->set_shortcut(ED_GET_SHORTCUT("editor/ungroup_selected_nodes"));
 
+	tool_button[TOOL_RULER] = memnew(Button);
+	main_menu_hbox->add_child(tool_button[TOOL_RULER]);
+	tool_button[TOOL_RULER]->set_toggle_mode(true);
+	tool_button[TOOL_RULER]->set_theme_type_variation("FlatButton");
+	tool_button[TOOL_RULER]->connect(SceneStringName(pressed), callable_mp(this, &Node3DEditor::_menu_item_pressed).bind(MENU_RULER));
+	tool_button[TOOL_RULER]->set_tooltip_text(TTR("LMB+Drag: Measure the distance between two points in 3D space."));
+	// Define the shortcut globally (without a context) so that it works if the Scene tree dock is currently focused.
+	tool_button[TOOL_RULER]->set_shortcut(ED_SHORTCUT("spatial_editor/measure", TTR("Ruler Mode"), Key::M));
+
 	main_menu_hbox->add_child(memnew(VSeparator));
 
 	tool_option_button[TOOL_OPT_LOCAL_COORDS] = memnew(Button);
diff --git a/editor/plugins/node_3d_editor_plugin.h b/editor/plugins/node_3d_editor_plugin.h
index 243a712c3a..54620562f9 100644
--- a/editor/plugins/node_3d_editor_plugin.h
+++ b/editor/plugins/node_3d_editor_plugin.h
@@ -38,6 +38,7 @@
 #include "scene/gui/box_container.h"
 #include "scene/gui/button.h"
 #include "scene/gui/spin_box.h"
+#include "scene/resources/immediate_mesh.h"
 
 class AcceptDialog;
 class CheckBox;
@@ -61,6 +62,7 @@ class VSeparator;
 class VSplitContainer;
 class ViewportNavigationControl;
 class WorldEnvironment;
+class MeshInstance3D;
 
 class ViewportRotationControl : public Control {
 	GDCLASS(ViewportRotationControl, Control);
@@ -214,6 +216,16 @@ private:
 	double gpu_time_history[FRAME_TIME_HISTORY];
 	int gpu_time_history_index;
 
+	Node *ruler = nullptr;
+	Node3D *ruler_start_point = nullptr;
+	Node3D *ruler_end_point = nullptr;
+	Ref<ImmediateMesh> geometry;
+	MeshInstance3D *ruler_line = nullptr;
+	MeshInstance3D *ruler_line_xray = nullptr;
+	Label *ruler_label = nullptr;
+	Ref<StandardMaterial3D> ruler_material;
+	Ref<StandardMaterial3D> ruler_material_xray;
+
 	int index;
 	ViewType view_type;
 	void _menu_option(int p_option);
@@ -621,6 +633,7 @@ public:
 		TOOL_UNLOCK_SELECTED,
 		TOOL_GROUP_SELECTED,
 		TOOL_UNGROUP_SELECTED,
+		TOOL_RULER,
 		TOOL_MAX
 	};
 
@@ -726,7 +739,8 @@ private:
 		MENU_UNLOCK_SELECTED,
 		MENU_GROUP_SELECTED,
 		MENU_UNGROUP_SELECTED,
-		MENU_SNAP_TO_FLOOR
+		MENU_SNAP_TO_FLOOR,
+		MENU_RULER,
 	};
 
 	Button *tool_button[TOOL_MAX];

From bdf24924e6a1fdc052fd52c46d4c60119ecff4a7 Mon Sep 17 00:00:00 2001
From: Yufeng Ying <nbyyf2002@mail.ustc.edu.cn>
Date: Wed, 11 Dec 2024 00:48:00 +0800
Subject: [PATCH 064/124] [Codestyle] Merge identical code piece.

---
 core/math/convex_hull.cpp | 14 ++++----------
 1 file changed, 4 insertions(+), 10 deletions(-)

diff --git a/core/math/convex_hull.cpp b/core/math/convex_hull.cpp
index 620a7541e4..22aabc8ecf 100644
--- a/core/math/convex_hull.cpp
+++ b/core/math/convex_hull.cpp
@@ -2152,10 +2152,11 @@ static int32_t get_vertex_copy(ConvexHullInternal::Vertex *p_vertex, LocalVector
 }
 
 real_t ConvexHullComputer::compute(const Vector3 *p_coords, int32_t p_count, real_t p_shrink, real_t p_shrink_clamp) {
+	vertices.clear();
+	edges.clear();
+	faces.clear();
+
 	if (p_count <= 0) {
-		vertices.clear();
-		edges.clear();
-		faces.clear();
 		return 0;
 	}
 
@@ -2164,16 +2165,9 @@ real_t ConvexHullComputer::compute(const Vector3 *p_coords, int32_t p_count, rea
 
 	real_t shift = 0;
 	if ((p_shrink > 0) && ((shift = hull.shrink(p_shrink, p_shrink_clamp)) < 0)) {
-		vertices.clear();
-		edges.clear();
-		faces.clear();
 		return shift;
 	}
 
-	vertices.clear();
-	edges.clear();
-	faces.clear();
-
 	LocalVector<ConvexHullInternal::Vertex *> old_vertices;
 	get_vertex_copy(hull.vertex_list, old_vertices);
 	int32_t copied = 0;

From 9d329f54c0e5dea2ca7864918ab53baf3a42835d Mon Sep 17 00:00:00 2001
From: Fabio Alessandrelli <fabio.alessandrelli@gmail.com>
Date: Tue, 10 Dec 2024 14:32:07 +0100
Subject: [PATCH 065/124] [Web] Restore ScriptProcessorNode audio driver
 fallback

Godot has a ScriptProcessorNode audio driver implementation for the
(deprecated) Web API.

As reported by some users, this fallback was not properly re-added
during the Godot 4 transition, and was left as "dead code".

While the API is deprecated, it is still supported by most browsers, and
some WebView may not implement AudioWorklet correctly (the new
recommended API).

This commit re-adds the ScriptProcessorNode implementation as a fallback
if the AudioWorklet driver fails to initialized (and can be forced if
desired via project settings as usual).
---
 platform/web/audio_driver_web.cpp           | 4 ++--
 platform/web/audio_driver_web.h             | 5 ++---
 platform/web/js/libs/library_godot_audio.js | 2 +-
 platform/web/os_web.cpp                     | 1 +
 4 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/platform/web/audio_driver_web.cpp b/platform/web/audio_driver_web.cpp
index 0108f40726..f3d5b5cd1a 100644
--- a/platform/web/audio_driver_web.cpp
+++ b/platform/web/audio_driver_web.cpp
@@ -479,6 +479,8 @@ void AudioDriverWorklet::_capture_callback(int p_pos, int p_samples) {
 	driver->_audio_driver_capture(p_pos, p_samples);
 }
 
+#endif // THREADS_ENABLED
+
 /// ScriptProcessorNode implementation
 AudioDriverScriptProcessor *AudioDriverScriptProcessor::singleton = nullptr;
 
@@ -497,5 +499,3 @@ Error AudioDriverScriptProcessor::create(int &p_buffer_samples, int p_channels)
 void AudioDriverScriptProcessor::start(float *p_out_buf, int p_out_buf_size, float *p_in_buf, int p_in_buf_size) {
 	godot_audio_script_start(p_in_buf, p_in_buf_size, p_out_buf, p_out_buf_size, &_process_callback);
 }
-
-#endif // THREADS_ENABLED
diff --git a/platform/web/audio_driver_web.h b/platform/web/audio_driver_web.h
index d352fa4692..c317acfff0 100644
--- a/platform/web/audio_driver_web.h
+++ b/platform/web/audio_driver_web.h
@@ -169,6 +169,8 @@ public:
 	AudioDriverWorklet() { singleton = this; }
 };
 
+#endif // THREADS_ENABLED
+
 class AudioDriverScriptProcessor : public AudioDriverWeb {
 private:
 	static void _process_callback();
@@ -178,7 +180,6 @@ private:
 protected:
 	virtual Error create(int &p_buffer_size, int p_output_channels) override;
 	virtual void start(float *p_out_buf, int p_out_buf_size, float *p_in_buf, int p_in_buf_size) override;
-	virtual void finish_driver() override;
 
 public:
 	virtual const char *get_name() const override { return "ScriptProcessor"; }
@@ -191,6 +192,4 @@ public:
 	AudioDriverScriptProcessor() { singleton = this; }
 };
 
-#endif // THREADS_ENABLED
-
 #endif // AUDIO_DRIVER_WEB_H
diff --git a/platform/web/js/libs/library_godot_audio.js b/platform/web/js/libs/library_godot_audio.js
index aaf986b966..f566268e53 100644
--- a/platform/web/js/libs/library_godot_audio.js
+++ b/platform/web/js/libs/library_godot_audio.js
@@ -2160,7 +2160,7 @@ autoAddDeps(GodotAudioWorklet, '$GodotAudioWorklet');
 mergeInto(LibraryManager.library, GodotAudioWorklet);
 
 /*
- * The ScriptProcessorNode API, used when threads are disabled.
+ * The ScriptProcessorNode API, used as a fallback if AudioWorklet is not available.
  */
 const GodotAudioScript = {
 	$GodotAudioScript__deps: ['$GodotAudio'],
diff --git a/platform/web/os_web.cpp b/platform/web/os_web.cpp
index 51facbaa84..c7ca613f4e 100644
--- a/platform/web/os_web.cpp
+++ b/platform/web/os_web.cpp
@@ -275,6 +275,7 @@ OS_Web::OS_Web() {
 
 	if (AudioDriverWeb::is_available()) {
 		audio_drivers.push_back(memnew(AudioDriverWorklet));
+		audio_drivers.push_back(memnew(AudioDriverScriptProcessor));
 	}
 	for (AudioDriverWeb *audio_driver : audio_drivers) {
 		AudioDriverManager::add_driver(audio_driver);

From 57d08dbec3e1effabddb8738bced67da99a02fd4 Mon Sep 17 00:00:00 2001
From: Thaddeus Crews <repiteo@outlook.com>
Date: Sun, 1 Dec 2024 12:01:15 -0600
Subject: [PATCH 066/124] C#: Fix warnings caught by new problem-matchers
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

• Restore MSVC problem matcher for Linux builds
---
 .github/workflows/linux_builds.yml            |   3 -
 .../ScriptSignalsGenerator.cs                 |   2 +-
 modules/mono/editor/bindings_generator.cpp    | 178 ++++++++++++------
 modules/mono/editor/bindings_generator.h      |  10 +-
 .../glue/GodotSharp/GodotSharp/Core/Array.cs  |   2 +
 .../GodotSharp/GodotSharp/Core/Dictionary.cs  |   2 +
 .../GodotSharp/Core/ReflectionUtils.cs        |   2 +-
 7 files changed, 132 insertions(+), 67 deletions(-)

diff --git a/.github/workflows/linux_builds.yml b/.github/workflows/linux_builds.yml
index f7dbeb6a06..d795ab718a 100644
--- a/.github/workflows/linux_builds.yml
+++ b/.github/workflows/linux_builds.yml
@@ -155,9 +155,6 @@ jobs:
       - name: Build .NET solutions
         if: matrix.build-mono
         run: |
-          # FIXME: C# warnings should be properly handled eventually, but we don't want to clutter
-          # the GitHub Actions annotations, so remove the associated problem matcher for now.
-          echo "::remove-matcher owner=msvc::"
           ./modules/mono/build_scripts/build_assemblies.py --godot-output-dir=./bin --godot-platform=linuxbsd
 
       - name: Prepare artifact
diff --git a/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators/ScriptSignalsGenerator.cs b/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators/ScriptSignalsGenerator.cs
index c7a7415851..2c1b48a858 100644
--- a/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators/ScriptSignalsGenerator.cs
+++ b/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators/ScriptSignalsGenerator.cs
@@ -309,7 +309,7 @@ namespace Godot.SourceGenerators
                     // Enums must be converted to the underlying type before they can be implicitly converted to Variant
                     if (paramSymbol.Type.TypeKind == TypeKind.Enum)
                     {
-                        var underlyingType = ((INamedTypeSymbol)paramSymbol.Type).EnumUnderlyingType;
+                        var underlyingType = ((INamedTypeSymbol)paramSymbol.Type).EnumUnderlyingType!;
                         source.Append($", ({underlyingType.FullQualifiedNameIncludeGlobal()})@{paramSymbol.Name}");
                         continue;
                     }
diff --git a/modules/mono/editor/bindings_generator.cpp b/modules/mono/editor/bindings_generator.cpp
index df773324b5..00e0a52de0 100644
--- a/modules/mono/editor/bindings_generator.cpp
+++ b/modules/mono/editor/bindings_generator.cpp
@@ -141,6 +141,11 @@ const Vector<String> prop_allowed_inherited_member_hiding = {
 	"MenuBar.TextDirection",
 	"RichTextLabel.TextDirection",
 	"TextEdit.TextDirection",
+	"VisualShaderNodeReroute.PortType",
+	// The following instances are uniquely egregious violations, hiding `GetType()` from `object`.
+	// Included for the sake of CI, with the understanding that they *deserve* warnings.
+	"GltfAccessor.GetType",
+	"GltfAccessor.MethodName.GetType",
 };
 
 void BindingsGenerator::TypeInterface::postsetup_enum_type(BindingsGenerator::TypeInterface &r_enum_itype) {
@@ -583,7 +588,7 @@ String BindingsGenerator::bbcode_to_xml(const String &p_bbcode, const TypeInterf
 			}
 
 			if (link_tag == "method") {
-				_append_xml_method(xml_output, target_itype, target_cname, link_target, link_target_parts);
+				_append_xml_method(xml_output, target_itype, target_cname, link_target, link_target_parts, p_itype);
 			} else if (link_tag == "constructor") {
 				// TODO: Support constructors?
 				_append_xml_undeclared(xml_output, link_target);
@@ -591,11 +596,11 @@ String BindingsGenerator::bbcode_to_xml(const String &p_bbcode, const TypeInterf
 				// TODO: Support operators?
 				_append_xml_undeclared(xml_output, link_target);
 			} else if (link_tag == "member") {
-				_append_xml_member(xml_output, target_itype, target_cname, link_target, link_target_parts);
+				_append_xml_member(xml_output, target_itype, target_cname, link_target, link_target_parts, p_itype);
 			} else if (link_tag == "signal") {
-				_append_xml_signal(xml_output, target_itype, target_cname, link_target, link_target_parts);
+				_append_xml_signal(xml_output, target_itype, target_cname, link_target, link_target_parts, p_itype);
 			} else if (link_tag == "enum") {
-				_append_xml_enum(xml_output, target_itype, target_cname, link_target, link_target_parts);
+				_append_xml_enum(xml_output, target_itype, target_cname, link_target, link_target_parts, p_itype);
 			} else if (link_tag == "constant") {
 				_append_xml_constant(xml_output, target_itype, target_cname, link_target, link_target_parts);
 			} else if (link_tag == "param") {
@@ -662,14 +667,8 @@ String BindingsGenerator::bbcode_to_xml(const String &p_bbcode, const TypeInterf
 				}
 
 				if (target_itype) {
-					if ((!p_itype || p_itype->api_type == ClassDB::API_CORE) && target_itype->api_type == ClassDB::API_EDITOR) {
-						// Editor references in core documentation cannot be resolved,
-						// handle as standard codeblock.
-						_log("Cannot reference editor type '%s' in documentation for core type '%s'\n",
-								target_itype->proxy_name.utf8().get_data(), p_itype ? p_itype->proxy_name.utf8().get_data() : "@GlobalScope");
-						xml_output.append("<c>");
-						xml_output.append(target_itype->proxy_name);
-						xml_output.append("</c>");
+					if (!_validate_api_type(target_itype, p_itype)) {
+						_append_xml_undeclared(xml_output, target_itype->proxy_name);
 					} else {
 						xml_output.append("<see cref=\"" BINDINGS_NAMESPACE ".");
 						xml_output.append(target_itype->proxy_name);
@@ -1086,7 +1085,7 @@ void BindingsGenerator::_append_text_undeclared(StringBuilder &p_output, const S
 	p_output.append("'" + p_link_target + "'");
 }
 
-void BindingsGenerator::_append_xml_method(StringBuilder &p_xml_output, const TypeInterface *p_target_itype, const StringName &p_target_cname, const String &p_link_target, const Vector<String> &p_link_target_parts) {
+void BindingsGenerator::_append_xml_method(StringBuilder &p_xml_output, const TypeInterface *p_target_itype, const StringName &p_target_cname, const String &p_link_target, const Vector<String> &p_link_target_parts, const TypeInterface *p_source_itype) {
 	if (p_link_target_parts[0] == name_cache.type_at_GlobalScope) {
 		if (OS::get_singleton()->is_stdout_verbose()) {
 			OS::get_singleton()->print("Cannot resolve @GlobalScope method reference in documentation: %s\n", p_link_target.utf8().get_data());
@@ -1117,35 +1116,38 @@ void BindingsGenerator::_append_xml_method(StringBuilder &p_xml_output, const Ty
 			const MethodInterface *target_imethod = p_target_itype->find_method_by_name(p_target_cname);
 
 			if (target_imethod) {
-				p_xml_output.append("<see cref=\"" BINDINGS_NAMESPACE ".");
-				p_xml_output.append(p_target_itype->proxy_name);
-				p_xml_output.append(".");
-				p_xml_output.append(target_imethod->proxy_name);
-				p_xml_output.append("(");
-				bool first_key = true;
-				for (const ArgumentInterface &iarg : target_imethod->arguments) {
-					const TypeInterface *arg_type = _get_type_or_null(iarg.type);
+				const String method_name = p_target_itype->proxy_name + "." + target_imethod->proxy_name;
+				if (!_validate_api_type(p_target_itype, p_source_itype)) {
+					_append_xml_undeclared(p_xml_output, method_name);
+				} else {
+					p_xml_output.append("<see cref=\"" BINDINGS_NAMESPACE ".");
+					p_xml_output.append(method_name);
+					p_xml_output.append("(");
+					bool first_key = true;
+					for (const ArgumentInterface &iarg : target_imethod->arguments) {
+						const TypeInterface *arg_type = _get_type_or_null(iarg.type);
 
-					if (first_key) {
-						first_key = false;
-					} else {
-						p_xml_output.append(", ");
-					}
-					if (!arg_type) {
-						ERR_PRINT("Cannot resolve argument type in documentation: '" + p_link_target + "'.");
-						p_xml_output.append(iarg.type.cname);
-						continue;
-					}
-					if (iarg.def_param_mode == ArgumentInterface::NULLABLE_VAL) {
-						p_xml_output.append("Nullable{");
-					}
-					String arg_cs_type = arg_type->cs_type + _get_generic_type_parameters(*arg_type, iarg.type.generic_type_parameters);
-					p_xml_output.append(arg_cs_type.replacen("<", "{").replacen(">", "}").replacen("params ", ""));
-					if (iarg.def_param_mode == ArgumentInterface::NULLABLE_VAL) {
-						p_xml_output.append("}");
+						if (first_key) {
+							first_key = false;
+						} else {
+							p_xml_output.append(", ");
+						}
+						if (!arg_type) {
+							ERR_PRINT("Cannot resolve argument type in documentation: '" + p_link_target + "'.");
+							p_xml_output.append(iarg.type.cname);
+							continue;
+						}
+						if (iarg.def_param_mode == ArgumentInterface::NULLABLE_VAL) {
+							p_xml_output.append("Nullable{");
+						}
+						String arg_cs_type = arg_type->cs_type + _get_generic_type_parameters(*arg_type, iarg.type.generic_type_parameters);
+						p_xml_output.append(arg_cs_type.replacen("<", "{").replacen(">", "}").replacen("params ", ""));
+						if (iarg.def_param_mode == ArgumentInterface::NULLABLE_VAL) {
+							p_xml_output.append("}");
+						}
 					}
+					p_xml_output.append(")\"/>");
 				}
-				p_xml_output.append(")\"/>");
 			} else {
 				if (!p_target_itype->is_intentionally_ignored(p_link_target)) {
 					ERR_PRINT("Cannot resolve method reference in documentation: '" + p_link_target + "'.");
@@ -1157,7 +1159,7 @@ void BindingsGenerator::_append_xml_method(StringBuilder &p_xml_output, const Ty
 	}
 }
 
-void BindingsGenerator::_append_xml_member(StringBuilder &p_xml_output, const TypeInterface *p_target_itype, const StringName &p_target_cname, const String &p_link_target, const Vector<String> &p_link_target_parts) {
+void BindingsGenerator::_append_xml_member(StringBuilder &p_xml_output, const TypeInterface *p_target_itype, const StringName &p_target_cname, const String &p_link_target, const Vector<String> &p_link_target_parts, const TypeInterface *p_source_itype) {
 	if (p_link_target.contains_char('/')) {
 		// Properties with '/' (slash) in the name are not declared in C#, so there is nothing to reference.
 		_append_xml_undeclared(p_xml_output, p_link_target);
@@ -1184,11 +1186,14 @@ void BindingsGenerator::_append_xml_member(StringBuilder &p_xml_output, const Ty
 		}
 
 		if (target_iprop) {
-			p_xml_output.append("<see cref=\"" BINDINGS_NAMESPACE ".");
-			p_xml_output.append(current_itype->proxy_name);
-			p_xml_output.append(".");
-			p_xml_output.append(target_iprop->proxy_name);
-			p_xml_output.append("\"/>");
+			const String member_name = current_itype->proxy_name + "." + target_iprop->proxy_name;
+			if (!_validate_api_type(p_target_itype, p_source_itype)) {
+				_append_xml_undeclared(p_xml_output, member_name);
+			} else {
+				p_xml_output.append("<see cref=\"" BINDINGS_NAMESPACE ".");
+				p_xml_output.append(member_name);
+				p_xml_output.append("\"/>");
+			}
 		} else {
 			if (!p_target_itype->is_intentionally_ignored(p_link_target)) {
 				ERR_PRINT("Cannot resolve member reference in documentation: '" + p_link_target + "'.");
@@ -1199,7 +1204,7 @@ void BindingsGenerator::_append_xml_member(StringBuilder &p_xml_output, const Ty
 	}
 }
 
-void BindingsGenerator::_append_xml_signal(StringBuilder &p_xml_output, const TypeInterface *p_target_itype, const StringName &p_target_cname, const String &p_link_target, const Vector<String> &p_link_target_parts) {
+void BindingsGenerator::_append_xml_signal(StringBuilder &p_xml_output, const TypeInterface *p_target_itype, const StringName &p_target_cname, const String &p_link_target, const Vector<String> &p_link_target_parts, const TypeInterface *p_source_itype) {
 	if (!p_target_itype || !p_target_itype->is_object_type) {
 		if (OS::get_singleton()->is_stdout_verbose()) {
 			if (p_target_itype) {
@@ -1215,11 +1220,14 @@ void BindingsGenerator::_append_xml_signal(StringBuilder &p_xml_output, const Ty
 		const SignalInterface *target_isignal = p_target_itype->find_signal_by_name(p_target_cname);
 
 		if (target_isignal) {
-			p_xml_output.append("<see cref=\"" BINDINGS_NAMESPACE ".");
-			p_xml_output.append(p_target_itype->proxy_name);
-			p_xml_output.append(".");
-			p_xml_output.append(target_isignal->proxy_name);
-			p_xml_output.append("\"/>");
+			const String signal_name = p_target_itype->proxy_name + "." + target_isignal->proxy_name;
+			if (!_validate_api_type(p_target_itype, p_source_itype)) {
+				_append_xml_undeclared(p_xml_output, signal_name);
+			} else {
+				p_xml_output.append("<see cref=\"" BINDINGS_NAMESPACE ".");
+				p_xml_output.append(signal_name);
+				p_xml_output.append("\"/>");
+			}
 		} else {
 			if (!p_target_itype->is_intentionally_ignored(p_link_target)) {
 				ERR_PRINT("Cannot resolve signal reference in documentation: '" + p_link_target + "'.");
@@ -1230,7 +1238,7 @@ void BindingsGenerator::_append_xml_signal(StringBuilder &p_xml_output, const Ty
 	}
 }
 
-void BindingsGenerator::_append_xml_enum(StringBuilder &p_xml_output, const TypeInterface *p_target_itype, const StringName &p_target_cname, const String &p_link_target, const Vector<String> &p_link_target_parts) {
+void BindingsGenerator::_append_xml_enum(StringBuilder &p_xml_output, const TypeInterface *p_target_itype, const StringName &p_target_cname, const String &p_link_target, const Vector<String> &p_link_target_parts, const TypeInterface *p_source_itype) {
 	const StringName search_cname = !p_target_itype ? p_target_cname : StringName(p_target_itype->name + "." + (String)p_target_cname);
 
 	HashMap<StringName, TypeInterface>::ConstIterator enum_match = enum_types.find(search_cname);
@@ -1242,9 +1250,13 @@ void BindingsGenerator::_append_xml_enum(StringBuilder &p_xml_output, const Type
 	if (enum_match) {
 		const TypeInterface &target_enum_itype = enum_match->value;
 
-		p_xml_output.append("<see cref=\"" BINDINGS_NAMESPACE ".");
-		p_xml_output.append(target_enum_itype.proxy_name); // Includes nesting class if any
-		p_xml_output.append("\"/>");
+		if (!_validate_api_type(p_target_itype, p_source_itype)) {
+			_append_xml_undeclared(p_xml_output, target_enum_itype.proxy_name);
+		} else {
+			p_xml_output.append("<see cref=\"" BINDINGS_NAMESPACE ".");
+			p_xml_output.append(target_enum_itype.proxy_name); // Includes nesting class if any
+			p_xml_output.append("\"/>");
+		}
 	} else {
 		if (!p_target_itype->is_intentionally_ignored(p_link_target)) {
 			ERR_PRINT("Cannot resolve enum reference in documentation: '" + p_link_target + "'.");
@@ -1379,6 +1391,46 @@ void BindingsGenerator::_append_xml_undeclared(StringBuilder &p_xml_output, cons
 	p_xml_output.append("</c>");
 }
 
+bool BindingsGenerator::_validate_api_type(const TypeInterface *p_target_itype, const TypeInterface *p_source_itype) {
+	static constexpr const char *api_types[5] = {
+		"Core",
+		"Editor",
+		"Extension",
+		"Editor Extension",
+		"None",
+	};
+
+	const ClassDB::APIType target_api = p_target_itype ? p_target_itype->api_type : ClassDB::API_NONE;
+	ERR_FAIL_INDEX_V((int)target_api, 5, false);
+	const ClassDB::APIType source_api = p_source_itype ? p_source_itype->api_type : ClassDB::API_NONE;
+	ERR_FAIL_INDEX_V((int)source_api, 5, false);
+	bool validate = false;
+
+	switch (target_api) {
+		case ClassDB::API_NONE:
+		case ClassDB::API_CORE:
+		default:
+			validate = true;
+			break;
+		case ClassDB::API_EDITOR:
+			validate = source_api == ClassDB::API_EDITOR || source_api == ClassDB::API_EDITOR_EXTENSION;
+			break;
+		case ClassDB::API_EXTENSION:
+			validate = source_api == ClassDB::API_EXTENSION || source_api == ClassDB::API_EDITOR_EXTENSION;
+			break;
+		case ClassDB::API_EDITOR_EXTENSION:
+			validate = source_api == ClassDB::API_EDITOR_EXTENSION;
+			break;
+	}
+	if (!validate) {
+		const String target_name = p_target_itype ? p_target_itype->proxy_name : "@GlobalScope";
+		const String source_name = p_source_itype ? p_source_itype->proxy_name : "@GlobalScope";
+		WARN_PRINT(vformat("Type '%s' has API level '%s'; it cannot be referenced by type '%s' with API level '%s'.",
+				target_name, api_types[target_api], source_name, api_types[source_api]));
+	}
+	return validate;
+}
+
 int BindingsGenerator::_determine_enum_prefix(const EnumInterface &p_ienum) {
 	CRASH_COND(p_ienum.constants.is_empty());
 
@@ -2585,7 +2637,9 @@ Error BindingsGenerator::_generate_cs_type(const TypeInterface &itype, const Str
 		output << INDENT2 "/// <summary>\n"
 			   << INDENT2 "/// Cached name for the '" << iprop.cname << "' property.\n"
 			   << INDENT2 "/// </summary>\n"
-			   << INDENT2 "public static readonly StringName " << iprop.proxy_name << " = \"" << iprop.cname << "\";\n";
+			   << INDENT2 "public static "
+			   << (prop_allowed_inherited_member_hiding.has(itype.proxy_name + ".PropertyName." + iprop.proxy_name) ? "new " : "")
+			   << "readonly StringName " << iprop.proxy_name << " = \"" << iprop.cname << "\";\n";
 	}
 	output << INDENT1 "}\n";
 	//MethodName
@@ -2609,7 +2663,9 @@ Error BindingsGenerator::_generate_cs_type(const TypeInterface &itype, const Str
 		output << INDENT2 "/// <summary>\n"
 			   << INDENT2 "/// Cached name for the '" << imethod.cname << "' method.\n"
 			   << INDENT2 "/// </summary>\n"
-			   << INDENT2 "public static readonly StringName " << imethod.proxy_name << " = \"" << imethod.cname << "\";\n";
+			   << INDENT2 "public static "
+			   << (prop_allowed_inherited_member_hiding.has(itype.proxy_name + ".MethodName." + imethod.proxy_name) ? "new " : "")
+			   << "readonly StringName " << imethod.proxy_name << " = \"" << imethod.cname << "\";\n";
 	}
 	output << INDENT1 "}\n";
 	//SignalName
@@ -2627,7 +2683,9 @@ Error BindingsGenerator::_generate_cs_type(const TypeInterface &itype, const Str
 		output << INDENT2 "/// <summary>\n"
 			   << INDENT2 "/// Cached name for the '" << isignal.cname << "' signal.\n"
 			   << INDENT2 "/// </summary>\n"
-			   << INDENT2 "public static readonly StringName " << isignal.proxy_name << " = \"" << isignal.cname << "\";\n";
+			   << INDENT2 "public static "
+			   << (prop_allowed_inherited_member_hiding.has(itype.proxy_name + ".SignalName." + isignal.proxy_name) ? "new " : "")
+			   << "readonly StringName " << isignal.proxy_name << " = \"" << isignal.cname << "\";\n";
 	}
 	output << INDENT1 "}\n";
 
@@ -3030,6 +3088,10 @@ Error BindingsGenerator::_generate_cs_method(const BindingsGenerator::TypeInterf
 		p_output.append(MEMBER_BEGIN);
 		p_output.append(p_imethod.is_internal ? "internal " : "public ");
 
+		if (prop_allowed_inherited_member_hiding.has(p_itype.proxy_name + "." + p_imethod.proxy_name)) {
+			p_output.append("new ");
+		}
+
 		if (p_itype.is_singleton || p_imethod.is_static) {
 			p_output.append("static ");
 		} else if (p_imethod.is_virtual) {
diff --git a/modules/mono/editor/bindings_generator.h b/modules/mono/editor/bindings_generator.h
index 1670aca4b3..30a5ba7ab0 100644
--- a/modules/mono/editor/bindings_generator.h
+++ b/modules/mono/editor/bindings_generator.h
@@ -804,15 +804,17 @@ class BindingsGenerator {
 	void _append_text_param(StringBuilder &p_output, const String &p_link_target);
 	void _append_text_undeclared(StringBuilder &p_output, const String &p_link_target);
 
-	void _append_xml_method(StringBuilder &p_xml_output, const TypeInterface *p_target_itype, const StringName &p_target_cname, const String &p_link_target, const Vector<String> &p_link_target_parts);
-	void _append_xml_member(StringBuilder &p_xml_output, const TypeInterface *p_target_itype, const StringName &p_target_cname, const String &p_link_target, const Vector<String> &p_link_target_parts);
-	void _append_xml_signal(StringBuilder &p_xml_output, const TypeInterface *p_target_itype, const StringName &p_target_cname, const String &p_link_target, const Vector<String> &p_link_target_parts);
-	void _append_xml_enum(StringBuilder &p_xml_output, const TypeInterface *p_target_itype, const StringName &p_target_cname, const String &p_link_target, const Vector<String> &p_link_target_parts);
+	void _append_xml_method(StringBuilder &p_xml_output, const TypeInterface *p_target_itype, const StringName &p_target_cname, const String &p_link_target, const Vector<String> &p_link_target_parts, const TypeInterface *p_source_itype);
+	void _append_xml_member(StringBuilder &p_xml_output, const TypeInterface *p_target_itype, const StringName &p_target_cname, const String &p_link_target, const Vector<String> &p_link_target_parts, const TypeInterface *p_source_itype);
+	void _append_xml_signal(StringBuilder &p_xml_output, const TypeInterface *p_target_itype, const StringName &p_target_cname, const String &p_link_target, const Vector<String> &p_link_target_parts, const TypeInterface *p_source_itype);
+	void _append_xml_enum(StringBuilder &p_xml_output, const TypeInterface *p_target_itype, const StringName &p_target_cname, const String &p_link_target, const Vector<String> &p_link_target_parts, const TypeInterface *p_source_itype);
 	void _append_xml_constant(StringBuilder &p_xml_output, const TypeInterface *p_target_itype, const StringName &p_target_cname, const String &p_link_target, const Vector<String> &p_link_target_parts);
 	void _append_xml_constant_in_global_scope(StringBuilder &p_xml_output, const String &p_target_cname, const String &p_link_target);
 	void _append_xml_param(StringBuilder &p_xml_output, const String &p_link_target, bool p_is_signal);
 	void _append_xml_undeclared(StringBuilder &p_xml_output, const String &p_link_target);
 
+	bool _validate_api_type(const TypeInterface *p_target_itype, const TypeInterface *p_source_itype);
+
 	int _determine_enum_prefix(const EnumInterface &p_ienum);
 	void _apply_prefix_to_enum_constants(EnumInterface &p_ienum, int p_prefix_length);
 
diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Array.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Array.cs
index 6d3724b11e..c4bfed8542 100644
--- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Array.cs
+++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Array.cs
@@ -1046,6 +1046,8 @@ namespace Godot.Collections
     [DebuggerTypeProxy(typeof(ArrayDebugView<>))]
     [DebuggerDisplay("Count = {Count}")]
     [SuppressMessage("ReSharper", "RedundantExtendsListEntry")]
+    [SuppressMessage("Design", "CA1001", MessageId = "Types that own disposable fields should be disposable",
+            Justification = "Known issue. Requires explicit refcount management to not dispose untyped collections.")]
     [SuppressMessage("Naming", "CA1710", MessageId = "Identifiers should have correct suffix")]
     public sealed class Array<[MustBeVariant] T> :
         IList<T>,
diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Dictionary.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Dictionary.cs
index 8c3dec1cbf..ad313dbbf0 100644
--- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Dictionary.cs
+++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Dictionary.cs
@@ -485,6 +485,8 @@ namespace Godot.Collections
     /// <typeparam name="TValue">The type of the dictionary's values.</typeparam>
     [DebuggerTypeProxy(typeof(DictionaryDebugView<,>))]
     [DebuggerDisplay("Count = {Count}")]
+    [SuppressMessage("Design", "CA1001", MessageId = "Types that own disposable fields should be disposable",
+            Justification = "Known issue. Requires explicit refcount management to not dispose untyped collections.")]
     public class Dictionary<[MustBeVariant] TKey, [MustBeVariant] TValue> :
         IDictionary<TKey, TValue>,
         IReadOnlyDictionary<TKey, TValue>,
diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/ReflectionUtils.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/ReflectionUtils.cs
index e88155edb8..cbcac5bc95 100644
--- a/modules/mono/glue/GodotSharp/GodotSharp/Core/ReflectionUtils.cs
+++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/ReflectionUtils.cs
@@ -109,7 +109,7 @@ internal class ReflectionUtils
             // Append brackets.
             AppendArrayBrackets(sb, type);
 
-            static void AppendArrayBrackets(StringBuilder sb, Type type)
+            static void AppendArrayBrackets(StringBuilder sb, Type? type)
             {
                 while (type != null && type.IsArray)
                 {

From 3deb5884d750d9ebdf45b15f09d80aa2f23b4c33 Mon Sep 17 00:00:00 2001
From: devloglogan <devloglogan@gmail.com>
Date: Tue, 10 Dec 2024 13:40:47 -0600
Subject: [PATCH 067/124] Renderer agnostic motion vector rendering/OpenXR
 changes

---
 drivers/gles3/storage/texture_storage.cpp     |   2 +-
 drivers/gles3/storage/texture_storage.h       |   6 +-
 .../openxr/doc_classes/OpenXRAPIExtension.xml | 103 +++++++++++++++++
 .../OpenXRExtensionWrapperExtension.xml       |   8 ++
 .../openxr_composition_layer_extension.cpp    |   4 +-
 .../extensions/openxr_extension_wrapper.h     |   1 +
 .../openxr_extension_wrapper_extension.cpp    |  11 ++
 .../openxr_extension_wrapper_extension.h      |   2 +
 modules/openxr/openxr_api.cpp                 |  57 ++++++++++
 modules/openxr/openxr_api.h                   |  24 +++-
 modules/openxr/openxr_api_extension.cpp       | 107 ++++++++++++++++++
 modules/openxr/openxr_api_extension.h         |  19 ++++
 modules/openxr/openxr_interface.cpp           |  24 ++++
 modules/openxr/openxr_interface.h             |   3 +
 scene/main/viewport.cpp                       |   2 +-
 .../rendering/dummy/storage/texture_storage.h |   6 +-
 .../storage_rd/texture_storage.cpp            |   2 +-
 .../renderer_rd/storage_rd/texture_storage.h  |   6 +-
 servers/rendering/renderer_viewport.cpp       |  32 +++++-
 servers/rendering/renderer_viewport.h         |   2 +
 servers/rendering/rendering_server_default.h  |   1 +
 servers/rendering/storage/texture_storage.h   |   7 +-
 servers/rendering_server.h                    |   2 +
 servers/xr/xr_interface.cpp                   |   8 ++
 servers/xr/xr_interface.h                     |   2 +
 25 files changed, 429 insertions(+), 12 deletions(-)

diff --git a/drivers/gles3/storage/texture_storage.cpp b/drivers/gles3/storage/texture_storage.cpp
index 14bbe635a4..28eeb2ebb3 100644
--- a/drivers/gles3/storage/texture_storage.cpp
+++ b/drivers/gles3/storage/texture_storage.cpp
@@ -2481,7 +2481,7 @@ Size2i TextureStorage::render_target_get_size(RID p_render_target) const {
 	return rt->size;
 }
 
-void TextureStorage::render_target_set_override(RID p_render_target, RID p_color_texture, RID p_depth_texture, RID p_velocity_texture) {
+void TextureStorage::render_target_set_override(RID p_render_target, RID p_color_texture, RID p_depth_texture, RID p_velocity_texture, RID p_velocity_depth_texture) {
 	RenderTarget *rt = render_target_owner.get_or_null(p_render_target);
 	ERR_FAIL_NULL(rt);
 	ERR_FAIL_COND(rt->direct_to_screen);
diff --git a/drivers/gles3/storage/texture_storage.h b/drivers/gles3/storage/texture_storage.h
index 916662dcd6..1fecb2e5bb 100644
--- a/drivers/gles3/storage/texture_storage.h
+++ b/drivers/gles3/storage/texture_storage.h
@@ -685,13 +685,17 @@ public:
 	virtual void render_target_set_vrs_texture(RID p_render_target, RID p_texture) override {}
 	virtual RID render_target_get_vrs_texture(RID p_render_target) const override { return RID(); }
 
-	virtual void render_target_set_override(RID p_render_target, RID p_color_texture, RID p_depth_texture, RID p_velocity_texture) override;
+	virtual void render_target_set_override(RID p_render_target, RID p_color_texture, RID p_depth_texture, RID p_velocity_texture, RID p_velocity_depth_texture) override;
 	virtual RID render_target_get_override_color(RID p_render_target) const override;
 	virtual RID render_target_get_override_depth(RID p_render_target) const override;
 	virtual RID render_target_get_override_velocity(RID p_render_target) const override;
+	virtual RID render_target_get_override_velocity_depth(RID p_render_target) const override { return RID(); }
 
 	virtual RID render_target_get_texture(RID p_render_target) override;
 
+	virtual void render_target_set_velocity_target_size(RID p_render_target, const Size2i &p_target_size) override {}
+	virtual Size2i render_target_get_velocity_target_size(RID p_render_target) const override { return Size2i(); }
+
 	void bind_framebuffer(GLuint framebuffer) {
 		glBindFramebuffer(GL_FRAMEBUFFER, framebuffer);
 	}
diff --git a/modules/openxr/doc_classes/OpenXRAPIExtension.xml b/modules/openxr/doc_classes/OpenXRAPIExtension.xml
index 432b331eec..d244b4219d 100644
--- a/modules/openxr/doc_classes/OpenXRAPIExtension.xml
+++ b/modules/openxr/doc_classes/OpenXRAPIExtension.xml
@@ -82,12 +82,32 @@
 				Returns the predicted display timing for the current frame.
 			</description>
 		</method>
+		<method name="get_render_state_z_far">
+			<return type="float" />
+			<description>
+				Returns the far boundary value of the camera frustum.
+				[b]Note:[/b] This is only accessible in the render thread.
+			</description>
+		</method>
+		<method name="get_render_state_z_near">
+			<return type="float" />
+			<description>
+				Returns the near boundary value of the camera frustum.
+				[b]Note:[/b] This is only accessible in the render thread.
+			</description>
+		</method>
 		<method name="get_session">
 			<return type="int" />
 			<description>
 				Returns the OpenXR session, which is an [url=https://registry.khronos.org/OpenXR/specs/1.0/man/html/XrSession.html]XrSession[/url] cast to an integer.
 			</description>
 		</method>
+		<method name="get_supported_swapchain_formats">
+			<return type="PackedInt64Array" />
+			<description>
+				Returns an array of supported swapchain formats.
+			</description>
+		</method>
 		<method name="get_swapchain_format_name">
 			<return type="String" />
 			<param index="0" name="swapchain_format" type="int" />
@@ -133,6 +153,54 @@
 				Returns [code]true[/code] if OpenXR is enabled.
 			</description>
 		</method>
+		<method name="openxr_swapchain_acquire">
+			<return type="void" />
+			<param index="0" name="swapchain" type="int" />
+			<description>
+				Acquires the image of the provided swapchain.
+			</description>
+		</method>
+		<method name="openxr_swapchain_create">
+			<return type="int" />
+			<param index="0" name="create_flags" type="int" />
+			<param index="1" name="usage_flags" type="int" />
+			<param index="2" name="swapchain_format" type="int" />
+			<param index="3" name="width" type="int" />
+			<param index="4" name="height" type="int" />
+			<param index="5" name="sample_count" type="int" />
+			<param index="6" name="array_size" type="int" />
+			<description>
+				Returns a pointer to a new swapchain created using the provided parameters.
+			</description>
+		</method>
+		<method name="openxr_swapchain_free">
+			<return type="void" />
+			<param index="0" name="swapchain" type="int" />
+			<description>
+				Destroys the provided swapchain and frees it from memory.
+			</description>
+		</method>
+		<method name="openxr_swapchain_get_image">
+			<return type="RID" />
+			<param index="0" name="swapchain" type="int" />
+			<description>
+				Returns the RID of the provided swapchain's image.
+			</description>
+		</method>
+		<method name="openxr_swapchain_get_swapchain">
+			<return type="int" />
+			<param index="0" name="swapchain" type="int" />
+			<description>
+				Returns the [code]XrSwapchain[/code] handle of the provided swapchain.
+			</description>
+		</method>
+		<method name="openxr_swapchain_release">
+			<return type="void" />
+			<param index="0" name="swapchain" type="int" />
+			<description>
+				Releases the image of the provided swapchain.
+			</description>
+		</method>
 		<method name="register_composition_layer_provider">
 			<return type="void" />
 			<param index="0" name="extension" type="OpenXRExtensionWrapperExtension" />
@@ -140,6 +208,13 @@
 				Registers the given extension as a composition layer provider.
 			</description>
 		</method>
+		<method name="register_projection_views_extension">
+			<return type="void" />
+			<param index="0" name="extension" type="OpenXRExtensionWrapperExtension" />
+			<description>
+				Registers the given extension as a provider of additional data structures to projections views.
+			</description>
+		</method>
 		<method name="set_emulate_environment_blend_mode_alpha_blend">
 			<return type="void" />
 			<param index="0" name="enabled" type="bool" />
@@ -156,6 +231,27 @@
 				Set the object name of an OpenXR object, used for debug output. [param object_type] must be a valid OpenXR [code]XrObjectType[/code] enum and [param object_handle] must be a valid OpenXR object handle.
 			</description>
 		</method>
+		<method name="set_velocity_depth_texture">
+			<return type="void" />
+			<param index="0" name="render_target" type="RID" />
+			<description>
+				Sets the render target of the velocity depth texture.
+			</description>
+		</method>
+		<method name="set_velocity_target_size">
+			<return type="void" />
+			<param index="0" name="target_size" type="Vector2i" />
+			<description>
+				Sets the target size of the velocity and velocity depth textures.
+			</description>
+		</method>
+		<method name="set_velocity_texture">
+			<return type="void" />
+			<param index="0" name="render_target" type="RID" />
+			<description>
+				Sets the render target of the velocity texture.
+			</description>
+		</method>
 		<method name="transform_from_pose">
 			<return type="Transform3D" />
 			<param index="0" name="pose" type="const void*" />
@@ -170,6 +266,13 @@
 				Unregisters the given extension as a composition layer provider.
 			</description>
 		</method>
+		<method name="unregister_projection_views_extension">
+			<return type="void" />
+			<param index="0" name="extension" type="OpenXRExtensionWrapperExtension" />
+			<description>
+				Unregisters the given extension as a provider of additional data structures to projections views.
+			</description>
+		</method>
 		<method name="xr_result">
 			<return type="bool" />
 			<param index="0" name="result" type="int" />
diff --git a/modules/openxr/doc_classes/OpenXRExtensionWrapperExtension.xml b/modules/openxr/doc_classes/OpenXRExtensionWrapperExtension.xml
index c174ee4d69..cf6cd37c30 100644
--- a/modules/openxr/doc_classes/OpenXRExtensionWrapperExtension.xml
+++ b/modules/openxr/doc_classes/OpenXRExtensionWrapperExtension.xml
@@ -217,6 +217,14 @@
 				Adds additional data structures when the OpenXR instance is created.
 			</description>
 		</method>
+		<method name="_set_projection_views_and_get_next_pointer" qualifiers="virtual">
+			<return type="int" />
+			<param index="0" name="view_index" type="int" />
+			<param index="1" name="next_pointer" type="void*" />
+			<description>
+				Adds additional data structures to the projection view of the given [param view_index].
+			</description>
+		</method>
 		<method name="_set_session_create_and_get_next_pointer" qualifiers="virtual">
 			<return type="int" />
 			<param index="0" name="next_pointer" type="void*" />
diff --git a/modules/openxr/extensions/openxr_composition_layer_extension.cpp b/modules/openxr/extensions/openxr_composition_layer_extension.cpp
index 1e3490d1ed..f627028891 100644
--- a/modules/openxr/extensions/openxr_composition_layer_extension.cpp
+++ b/modules/openxr/extensions/openxr_composition_layer_extension.cpp
@@ -210,7 +210,7 @@ void OpenXRViewportCompositionLayerProvider::set_viewport(RID p_viewport, Size2i
 	if (subviewport.viewport != p_viewport) {
 		if (subviewport.viewport.is_valid()) {
 			RID rt = rs->viewport_get_render_target(subviewport.viewport);
-			RSG::texture_storage->render_target_set_override(rt, RID(), RID(), RID());
+			RSG::texture_storage->render_target_set_override(rt, RID(), RID(), RID(), RID());
 		}
 
 		subviewport.viewport = p_viewport;
@@ -323,7 +323,7 @@ void OpenXRViewportCompositionLayerProvider::on_pre_render() {
 			if (update_and_acquire_swapchain(update_mode == RS::VIEWPORT_UPDATE_ONCE)) {
 				// Render to our XR swapchain image.
 				RID rt = rs->viewport_get_render_target(subviewport.viewport);
-				RSG::texture_storage->render_target_set_override(rt, get_current_swapchain_texture(), RID(), RID());
+				RSG::texture_storage->render_target_set_override(rt, get_current_swapchain_texture(), RID(), RID(), RID());
 			}
 		}
 	}
diff --git a/modules/openxr/extensions/openxr_extension_wrapper.h b/modules/openxr/extensions/openxr_extension_wrapper.h
index 95b537d1b4..9e3a7136f5 100644
--- a/modules/openxr/extensions/openxr_extension_wrapper.h
+++ b/modules/openxr/extensions/openxr_extension_wrapper.h
@@ -62,6 +62,7 @@ public:
 	virtual void *set_session_create_and_get_next_pointer(void *p_next_pointer) { return p_next_pointer; } // Add additional data structures when we create our OpenXR session.
 	virtual void *set_swapchain_create_info_and_get_next_pointer(void *p_next_pointer) { return p_next_pointer; } // Add additional data structures when creating OpenXR swap chains.
 	virtual void *set_hand_joint_locations_and_get_next_pointer(int p_hand_index, void *p_next_pointer) { return p_next_pointer; }
+	virtual void *set_projection_views_and_get_next_pointer(int p_view_index, void *p_next_pointer) { return p_next_pointer; }
 
 	virtual PackedStringArray get_suggested_tracker_names() { return PackedStringArray(); }
 
diff --git a/modules/openxr/extensions/openxr_extension_wrapper_extension.cpp b/modules/openxr/extensions/openxr_extension_wrapper_extension.cpp
index fb8d1b9d69..e6a4c2618c 100644
--- a/modules/openxr/extensions/openxr_extension_wrapper_extension.cpp
+++ b/modules/openxr/extensions/openxr_extension_wrapper_extension.cpp
@@ -39,6 +39,7 @@ void OpenXRExtensionWrapperExtension::_bind_methods() {
 	GDVIRTUAL_BIND(_set_session_create_and_get_next_pointer, "next_pointer");
 	GDVIRTUAL_BIND(_set_swapchain_create_info_and_get_next_pointer, "next_pointer");
 	GDVIRTUAL_BIND(_set_hand_joint_locations_and_get_next_pointer, "hand_index", "next_pointer");
+	GDVIRTUAL_BIND(_set_projection_views_and_get_next_pointer, "view_index", "next_pointer");
 	GDVIRTUAL_BIND(_get_composition_layer_count);
 	GDVIRTUAL_BIND(_get_composition_layer, "index");
 	GDVIRTUAL_BIND(_get_composition_layer_order, "index");
@@ -140,6 +141,16 @@ void *OpenXRExtensionWrapperExtension::set_hand_joint_locations_and_get_next_poi
 	return nullptr;
 }
 
+void *OpenXRExtensionWrapperExtension::set_projection_views_and_get_next_pointer(int p_view_index, void *p_next_pointer) {
+	uint64_t pointer = 0;
+
+	if (GDVIRTUAL_CALL(_set_projection_views_and_get_next_pointer, p_view_index, GDExtensionPtr<void>(p_next_pointer), pointer)) {
+		return reinterpret_cast<void *>(pointer);
+	}
+
+	return nullptr;
+}
+
 PackedStringArray OpenXRExtensionWrapperExtension::get_suggested_tracker_names() {
 	PackedStringArray ret;
 
diff --git a/modules/openxr/extensions/openxr_extension_wrapper_extension.h b/modules/openxr/extensions/openxr_extension_wrapper_extension.h
index 8fc6511277..350bcb9830 100644
--- a/modules/openxr/extensions/openxr_extension_wrapper_extension.h
+++ b/modules/openxr/extensions/openxr_extension_wrapper_extension.h
@@ -60,6 +60,7 @@ public:
 	virtual void *set_session_create_and_get_next_pointer(void *p_next_pointer) override;
 	virtual void *set_swapchain_create_info_and_get_next_pointer(void *p_next_pointer) override;
 	virtual void *set_hand_joint_locations_and_get_next_pointer(int p_hand_index, void *p_next_pointer) override;
+	virtual void *set_projection_views_and_get_next_pointer(int p_view_index, void *p_next_pointer) override;
 
 	virtual int get_composition_layer_count() override;
 	virtual XrCompositionLayerBaseHeader *get_composition_layer(int p_index) override;
@@ -71,6 +72,7 @@ public:
 	GDVIRTUAL1R(uint64_t, _set_session_create_and_get_next_pointer, GDExtensionPtr<void>);
 	GDVIRTUAL1R(uint64_t, _set_swapchain_create_info_and_get_next_pointer, GDExtensionPtr<void>);
 	GDVIRTUAL2R(uint64_t, _set_hand_joint_locations_and_get_next_pointer, int, GDExtensionPtr<void>);
+	GDVIRTUAL2R(uint64_t, _set_projection_views_and_get_next_pointer, int, GDExtensionPtr<void>);
 	GDVIRTUAL0R(int, _get_composition_layer_count);
 	GDVIRTUAL1R(uint64_t, _get_composition_layer, int);
 	GDVIRTUAL1R(int, _get_composition_layer_order, int);
diff --git a/modules/openxr/openxr_api.cpp b/modules/openxr/openxr_api.cpp
index 1775541757..ae2632564e 100644
--- a/modules/openxr/openxr_api.cpp
+++ b/modules/openxr/openxr_api.cpp
@@ -1916,6 +1916,9 @@ bool OpenXRAPI::get_view_projection(uint32_t p_view, double p_z_near, double p_z
 		}
 	}
 
+	render_state.z_near = p_z_near;
+	render_state.z_far = p_z_far;
+
 	// now update our projection
 	return graphics_extension->create_projection_fov(render_state.views[p_view].fov, p_z_near, p_z_far, p_camera_matrix);
 }
@@ -2343,6 +2346,30 @@ RID OpenXRAPI::get_depth_texture() {
 	}
 }
 
+void OpenXRAPI::set_velocity_texture(RID p_render_target) {
+	velocity_texture = p_render_target;
+}
+
+RID OpenXRAPI::get_velocity_texture() {
+	return velocity_texture;
+}
+
+void OpenXRAPI::set_velocity_depth_texture(RID p_render_target) {
+	velocity_depth_texture = p_render_target;
+}
+
+RID OpenXRAPI::get_velocity_depth_texture() {
+	return velocity_depth_texture;
+}
+
+void OpenXRAPI::set_velocity_target_size(const Size2i &p_target_size) {
+	velocity_target_size = p_target_size;
+}
+
+Size2i OpenXRAPI::get_velocity_target_size() {
+	return velocity_target_size;
+}
+
 void OpenXRAPI::post_draw_viewport(RID p_render_target) {
 	// Must be called from rendering thread!
 	ERR_NOT_ON_RENDER_THREAD;
@@ -2448,6 +2475,20 @@ void OpenXRAPI::end_frame() {
 		render_state.view_count, // viewCount
 		render_state.projection_views, // views
 	};
+
+	if (projection_views_extensions.size() > 0) {
+		for (uint32_t v = 0; v < render_state.view_count; v++) {
+			void *next_pointer = nullptr;
+			for (OpenXRExtensionWrapper *wrapper : projection_views_extensions) {
+				void *np = wrapper->set_projection_views_and_get_next_pointer(v, next_pointer);
+				if (np != nullptr) {
+					next_pointer = np;
+				}
+			}
+			render_state.projection_views[v].next = next_pointer;
+		}
+	}
+
 	ordered_layers_list.push_back({ (const XrCompositionLayerBaseHeader *)&projection_layer, 0 });
 
 	// Sort our layers.
@@ -2578,6 +2619,14 @@ Size2 OpenXRAPI::get_play_space_bounds() const {
 	return ret;
 }
 
+PackedInt64Array OpenXRAPI::get_supported_swapchain_formats() {
+	PackedInt64Array supported_swapchain_list;
+	for (uint32_t i = 0; i < num_swapchain_formats; i++) {
+		supported_swapchain_list.push_back(supported_swapchain_formats[i]);
+	}
+	return supported_swapchain_list;
+}
+
 OpenXRAPI::OpenXRAPI() {
 	// OpenXRAPI is only constructed if OpenXR is enabled.
 	singleton = this;
@@ -3499,6 +3548,14 @@ void OpenXRAPI::unregister_composition_layer_provider(OpenXRCompositionLayerProv
 	composition_layer_providers.erase(provider);
 }
 
+void OpenXRAPI::register_projection_views_extension(OpenXRExtensionWrapper *p_extension) {
+	projection_views_extensions.append(p_extension);
+}
+
+void OpenXRAPI::unregister_projection_views_extension(OpenXRExtensionWrapper *p_extension) {
+	projection_views_extensions.erase(p_extension);
+}
+
 const XrEnvironmentBlendMode *OpenXRAPI::get_supported_environment_blend_modes(uint32_t &count) {
 	count = num_supported_environment_blend_modes;
 	return supported_environment_blend_modes;
diff --git a/modules/openxr/openxr_api.h b/modules/openxr/openxr_api.h
index ecffce1816..21e18fc5f8 100644
--- a/modules/openxr/openxr_api.h
+++ b/modules/openxr/openxr_api.h
@@ -102,6 +102,9 @@ private:
 	// composition layer providers
 	Vector<OpenXRCompositionLayerProvider *> composition_layer_providers;
 
+	// projection views extensions
+	Vector<OpenXRExtensionWrapper *> projection_views_extensions;
+
 	// view configuration
 	uint32_t num_view_configuration_types = 0;
 	XrViewConfigurationType *supported_view_configuration_types = nullptr;
@@ -153,7 +156,6 @@ private:
 	enum OpenXRSwapChainTypes {
 		OPENXR_SWAPCHAIN_COLOR,
 		OPENXR_SWAPCHAIN_DEPTH,
-		// OPENXR_SWAPCHAIN_VELOCITY,
 		OPENXR_SWAPCHAIN_MAX
 	};
 
@@ -165,6 +167,10 @@ private:
 	XrSpace view_space = XR_NULL_HANDLE;
 	XRPose::TrackingConfidence head_pose_confidence = XRPose::XR_TRACKING_CONFIDENCE_NONE;
 
+	RID velocity_texture;
+	RID velocity_depth_texture;
+	Size2i velocity_target_size;
+
 	// When LOCAL_FLOOR isn't supported, we use an approach based on the example code in the
 	// OpenXR spec in order to emulate it.
 	// See: https://registry.khronos.org/OpenXR/specs/1.0/html/xrspec.html#XR_EXT_local_floor
@@ -345,6 +351,9 @@ private:
 		bool submit_depth_buffer = false; // if set to true we submit depth buffers to OpenXR if a suitable extension is enabled.
 		bool view_pose_valid = false;
 
+		double z_near = 0.0;
+		double z_far = 0.0;
+
 		Size2i main_swapchain_size;
 		OpenXRSwapChainInfo main_swapchains[OPENXR_SWAPCHAIN_MAX];
 	} render_state;
@@ -477,6 +486,12 @@ public:
 	XrSwapchain get_color_swapchain();
 	RID get_color_texture();
 	RID get_depth_texture();
+	void set_velocity_texture(RID p_render_target);
+	RID get_velocity_texture();
+	void set_velocity_depth_texture(RID p_render_target);
+	RID get_velocity_depth_texture();
+	void set_velocity_target_size(const Size2i &p_target_size);
+	Size2i get_velocity_target_size();
 	void post_draw_viewport(RID p_render_target);
 	void end_frame();
 
@@ -502,9 +517,13 @@ public:
 	Size2 get_play_space_bounds() const;
 
 	// swapchains
+	PackedInt64Array get_supported_swapchain_formats();
 	int64_t get_color_swapchain_format() const { return color_swapchain_format; }
 	int64_t get_depth_swapchain_format() const { return depth_swapchain_format; }
 
+	double get_render_state_z_near() const { return render_state.z_near; }
+	double get_render_state_z_far() const { return render_state.z_far; }
+
 	// action map
 	String get_default_action_map_resource_name();
 
@@ -542,6 +561,9 @@ public:
 	void register_composition_layer_provider(OpenXRCompositionLayerProvider *provider);
 	void unregister_composition_layer_provider(OpenXRCompositionLayerProvider *provider);
 
+	void register_projection_views_extension(OpenXRExtensionWrapper *p_extension);
+	void unregister_projection_views_extension(OpenXRExtensionWrapper *p_extension);
+
 	const XrEnvironmentBlendMode *get_supported_environment_blend_modes(uint32_t &count);
 	bool is_environment_blend_mode_supported(XrEnvironmentBlendMode p_blend_mode) const;
 	bool set_environment_blend_mode(XrEnvironmentBlendMode p_blend_mode);
diff --git a/modules/openxr/openxr_api_extension.cpp b/modules/openxr/openxr_api_extension.cpp
index f3bc178d3a..506e1fc64d 100644
--- a/modules/openxr/openxr_api_extension.cpp
+++ b/modules/openxr/openxr_api_extension.cpp
@@ -61,6 +61,25 @@ void OpenXRAPIExtension::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("register_composition_layer_provider", "extension"), &OpenXRAPIExtension::register_composition_layer_provider);
 	ClassDB::bind_method(D_METHOD("unregister_composition_layer_provider", "extension"), &OpenXRAPIExtension::unregister_composition_layer_provider);
 
+	ClassDB::bind_method(D_METHOD("register_projection_views_extension", "extension"), &OpenXRAPIExtension::register_projection_views_extension);
+	ClassDB::bind_method(D_METHOD("unregister_projection_views_extension", "extension"), &OpenXRAPIExtension::unregister_projection_views_extension);
+
+	ClassDB::bind_method(D_METHOD("get_render_state_z_near"), &OpenXRAPIExtension::get_render_state_z_near);
+	ClassDB::bind_method(D_METHOD("get_render_state_z_far"), &OpenXRAPIExtension::get_render_state_z_far);
+
+	ClassDB::bind_method(D_METHOD("set_velocity_texture", "render_target"), &OpenXRAPIExtension::set_velocity_texture);
+	ClassDB::bind_method(D_METHOD("set_velocity_depth_texture", "render_target"), &OpenXRAPIExtension::set_velocity_depth_texture);
+	ClassDB::bind_method(D_METHOD("set_velocity_target_size", "target_size"), &OpenXRAPIExtension::set_velocity_target_size);
+
+	ClassDB::bind_method(D_METHOD("get_supported_swapchain_formats"), &OpenXRAPIExtension::get_supported_swapchain_formats);
+
+	ClassDB::bind_method(D_METHOD("openxr_swapchain_create", "create_flags", "usage_flags", "swapchain_format", "width", "height", "sample_count", "array_size"), &OpenXRAPIExtension::openxr_swapchain_create);
+	ClassDB::bind_method(D_METHOD("openxr_swapchain_free", "swapchain"), &OpenXRAPIExtension::openxr_swapchain_free);
+	ClassDB::bind_method(D_METHOD("openxr_swapchain_get_swapchain", "swapchain"), &OpenXRAPIExtension::openxr_swapchain_get_swapchain);
+	ClassDB::bind_method(D_METHOD("openxr_swapchain_acquire", "swapchain"), &OpenXRAPIExtension::openxr_swapchain_acquire);
+	ClassDB::bind_method(D_METHOD("openxr_swapchain_get_image", "swapchain"), &OpenXRAPIExtension::openxr_swapchain_get_image);
+	ClassDB::bind_method(D_METHOD("openxr_swapchain_release", "swapchain"), &OpenXRAPIExtension::openxr_swapchain_release);
+
 	ClassDB::bind_method(D_METHOD("set_emulate_environment_blend_mode_alpha_blend", "enabled"), &OpenXRAPIExtension::set_emulate_environment_blend_mode_alpha_blend);
 	ClassDB::bind_method(D_METHOD("is_environment_blend_mode_alpha_supported"), &OpenXRAPIExtension::is_environment_blend_mode_alpha_blend_supported);
 
@@ -193,6 +212,94 @@ void OpenXRAPIExtension::unregister_composition_layer_provider(OpenXRExtensionWr
 	OpenXRAPI::get_singleton()->unregister_composition_layer_provider(p_extension);
 }
 
+void OpenXRAPIExtension::register_projection_views_extension(OpenXRExtensionWrapperExtension *p_extension) {
+	ERR_FAIL_NULL(OpenXRAPI::get_singleton());
+	OpenXRAPI::get_singleton()->register_projection_views_extension(p_extension);
+}
+
+void OpenXRAPIExtension::unregister_projection_views_extension(OpenXRExtensionWrapperExtension *p_extension) {
+	ERR_FAIL_NULL(OpenXRAPI::get_singleton());
+	OpenXRAPI::get_singleton()->unregister_projection_views_extension(p_extension);
+}
+
+double OpenXRAPIExtension::get_render_state_z_near() {
+	ERR_NOT_ON_RENDER_THREAD_V(0.0);
+	ERR_FAIL_NULL_V(OpenXRAPI::get_singleton(), 0.0);
+	return OpenXRAPI::get_singleton()->get_render_state_z_near();
+}
+
+double OpenXRAPIExtension::get_render_state_z_far() {
+	ERR_NOT_ON_RENDER_THREAD_V(0.0);
+	ERR_FAIL_NULL_V(OpenXRAPI::get_singleton(), 0.0);
+	return OpenXRAPI::get_singleton()->get_render_state_z_far();
+}
+
+void OpenXRAPIExtension::set_velocity_texture(RID p_render_target) {
+	ERR_FAIL_NULL(OpenXRAPI::get_singleton());
+	OpenXRAPI::get_singleton()->set_velocity_texture(p_render_target);
+}
+
+void OpenXRAPIExtension::set_velocity_depth_texture(RID p_render_target) {
+	ERR_FAIL_NULL(OpenXRAPI::get_singleton());
+	OpenXRAPI::get_singleton()->set_velocity_depth_texture(p_render_target);
+}
+
+void OpenXRAPIExtension::set_velocity_target_size(const Size2i &p_target_size) {
+	ERR_FAIL_NULL(OpenXRAPI::get_singleton());
+	OpenXRAPI::get_singleton()->set_velocity_target_size(p_target_size);
+}
+
+PackedInt64Array OpenXRAPIExtension::get_supported_swapchain_formats() {
+	ERR_FAIL_NULL_V(OpenXRAPI::get_singleton(), PackedInt64Array());
+	return OpenXRAPI::get_singleton()->get_supported_swapchain_formats();
+}
+
+uint64_t OpenXRAPIExtension::openxr_swapchain_create(XrSwapchainCreateFlags p_create_flags, XrSwapchainUsageFlags p_usage_flags, int64_t p_swapchain_format, uint32_t p_width, uint32_t p_height, uint32_t p_sample_count, uint32_t p_array_size) {
+	ERR_FAIL_NULL_V(OpenXRAPI::get_singleton(), 0);
+
+	OpenXRAPI::OpenXRSwapChainInfo *new_swapchain_info = memnew(OpenXRAPI::OpenXRSwapChainInfo);
+	new_swapchain_info->create(p_create_flags, p_usage_flags, p_swapchain_format, p_width, p_height, p_sample_count, p_array_size);
+	return reinterpret_cast<uint64_t>(new_swapchain_info);
+}
+
+void OpenXRAPIExtension::openxr_swapchain_free(uint64_t p_swapchain_info) {
+	ERR_FAIL_NULL(OpenXRAPI::get_singleton());
+
+	OpenXRAPI::OpenXRSwapChainInfo *swapchain_info = reinterpret_cast<OpenXRAPI::OpenXRSwapChainInfo *>(p_swapchain_info);
+	swapchain_info->free();
+	memfree(swapchain_info);
+}
+
+uint64_t OpenXRAPIExtension::openxr_swapchain_get_swapchain(uint64_t p_swapchain_info) {
+	ERR_FAIL_NULL_V(OpenXRAPI::get_singleton(), 0);
+
+	OpenXRAPI::OpenXRSwapChainInfo *swapchain_info = reinterpret_cast<OpenXRAPI::OpenXRSwapChainInfo *>(p_swapchain_info);
+	XrSwapchain swapchain = swapchain_info->get_swapchain();
+	return reinterpret_cast<uint64_t>(swapchain);
+}
+
+void OpenXRAPIExtension::openxr_swapchain_acquire(uint64_t p_swapchain_info) {
+	ERR_FAIL_NULL(OpenXRAPI::get_singleton());
+
+	OpenXRAPI::OpenXRSwapChainInfo *swapchain_info = reinterpret_cast<OpenXRAPI::OpenXRSwapChainInfo *>(p_swapchain_info);
+	bool should_render = true; // Can ignore should_render.
+	swapchain_info->acquire(should_render);
+}
+
+RID OpenXRAPIExtension::openxr_swapchain_get_image(uint64_t p_swapchain_info) {
+	ERR_FAIL_NULL_V(OpenXRAPI::get_singleton(), RID());
+
+	OpenXRAPI::OpenXRSwapChainInfo *swapchain_info = reinterpret_cast<OpenXRAPI::OpenXRSwapChainInfo *>(p_swapchain_info);
+	return swapchain_info->get_image();
+}
+
+void OpenXRAPIExtension::openxr_swapchain_release(uint64_t p_swapchain_info) {
+	ERR_FAIL_NULL(OpenXRAPI::get_singleton());
+
+	OpenXRAPI::OpenXRSwapChainInfo *swapchain_info = reinterpret_cast<OpenXRAPI::OpenXRSwapChainInfo *>(p_swapchain_info);
+	swapchain_info->release();
+}
+
 void OpenXRAPIExtension::set_emulate_environment_blend_mode_alpha_blend(bool p_enabled) {
 	ERR_FAIL_NULL(OpenXRAPI::get_singleton());
 	OpenXRAPI::get_singleton()->set_emulate_environment_blend_mode_alpha_blend(p_enabled);
diff --git a/modules/openxr/openxr_api_extension.h b/modules/openxr/openxr_api_extension.h
index 1b88b418f6..6519aea7f1 100644
--- a/modules/openxr/openxr_api_extension.h
+++ b/modules/openxr/openxr_api_extension.h
@@ -82,6 +82,25 @@ public:
 	void register_composition_layer_provider(OpenXRExtensionWrapperExtension *p_extension);
 	void unregister_composition_layer_provider(OpenXRExtensionWrapperExtension *p_extension);
 
+	void register_projection_views_extension(OpenXRExtensionWrapperExtension *p_extension);
+	void unregister_projection_views_extension(OpenXRExtensionWrapperExtension *p_extension);
+
+	double get_render_state_z_near();
+	double get_render_state_z_far();
+
+	void set_velocity_texture(RID p_render_target);
+	void set_velocity_depth_texture(RID p_render_target);
+	void set_velocity_target_size(const Size2i &p_target_size);
+
+	PackedInt64Array get_supported_swapchain_formats();
+
+	uint64_t openxr_swapchain_create(XrSwapchainCreateFlags p_create_flags, XrSwapchainUsageFlags p_usage_flags, int64_t p_swapchain_format, uint32_t p_width, uint32_t p_height, uint32_t p_sample_count, uint32_t p_array_size);
+	void openxr_swapchain_free(uint64_t p_swapchain_info);
+	uint64_t openxr_swapchain_get_swapchain(uint64_t p_swapchain_info);
+	void openxr_swapchain_acquire(uint64_t p_swapchain_info);
+	RID openxr_swapchain_get_image(uint64_t p_swapchain_info);
+	void openxr_swapchain_release(uint64_t p_swapchain_info);
+
 	enum OpenXRAlphaBlendModeSupport {
 		OPENXR_ALPHA_BLEND_MODE_SUPPORT_NONE = 0,
 		OPENXR_ALPHA_BLEND_MODE_SUPPORT_REAL = 1,
diff --git a/modules/openxr/openxr_interface.cpp b/modules/openxr/openxr_interface.cpp
index 68e04694e3..f7f47f36e8 100644
--- a/modules/openxr/openxr_interface.cpp
+++ b/modules/openxr/openxr_interface.cpp
@@ -1053,6 +1053,30 @@ RID OpenXRInterface::get_depth_texture() {
 	}
 }
 
+RID OpenXRInterface::get_velocity_texture() {
+	if (openxr_api) {
+		return openxr_api->get_velocity_texture();
+	} else {
+		return RID();
+	}
+}
+
+RID OpenXRInterface::get_velocity_depth_texture() {
+	if (openxr_api) {
+		return openxr_api->get_velocity_depth_texture();
+	} else {
+		return RID();
+	}
+}
+
+Size2i OpenXRInterface::get_velocity_target_size() {
+	if (openxr_api) {
+		return openxr_api->get_velocity_target_size();
+	} else {
+		return Size2i();
+	}
+}
+
 void OpenXRInterface::handle_hand_tracking(const String &p_path, OpenXRHandTrackingExtension::HandTrackedHands p_hand) {
 	OpenXRHandTrackingExtension *hand_tracking_ext = OpenXRHandTrackingExtension::get_singleton();
 	if (hand_tracking_ext && hand_tracking_ext->get_active()) {
diff --git a/modules/openxr/openxr_interface.h b/modules/openxr/openxr_interface.h
index d1bf2aaf78..b37c2eda29 100644
--- a/modules/openxr/openxr_interface.h
+++ b/modules/openxr/openxr_interface.h
@@ -185,6 +185,9 @@ public:
 
 	virtual RID get_color_texture() override;
 	virtual RID get_depth_texture() override;
+	virtual RID get_velocity_texture() override;
+	virtual RID get_velocity_depth_texture() override;
+	virtual Size2i get_velocity_target_size() override;
 
 	virtual void process() override;
 	virtual void pre_render() override;
diff --git a/scene/main/viewport.cpp b/scene/main/viewport.cpp
index 1a047dfa27..68085edbb1 100644
--- a/scene/main/viewport.cpp
+++ b/scene/main/viewport.cpp
@@ -4527,7 +4527,7 @@ void Viewport::set_use_xr(bool p_use_xr) {
 
 			// Reset render target override textures.
 			RID rt = RS::get_singleton()->viewport_get_render_target(viewport);
-			RSG::texture_storage->render_target_set_override(rt, RID(), RID(), RID());
+			RSG::texture_storage->render_target_set_override(rt, RID(), RID(), RID(), RID());
 		}
 	}
 }
diff --git a/servers/rendering/dummy/storage/texture_storage.h b/servers/rendering/dummy/storage/texture_storage.h
index 63eb441915..35135ce860 100644
--- a/servers/rendering/dummy/storage/texture_storage.h
+++ b/servers/rendering/dummy/storage/texture_storage.h
@@ -200,12 +200,16 @@ public:
 	virtual void render_target_set_vrs_texture(RID p_render_target, RID p_texture) override {}
 	virtual RID render_target_get_vrs_texture(RID p_render_target) const override { return RID(); }
 
-	virtual void render_target_set_override(RID p_render_target, RID p_color_texture, RID p_depth_texture, RID p_velocity_texture) override {}
+	virtual void render_target_set_override(RID p_render_target, RID p_color_texture, RID p_depth_texture, RID p_velocity_texture, RID p_velocity_depth_texture) override {}
 	virtual RID render_target_get_override_color(RID p_render_target) const override { return RID(); }
 	virtual RID render_target_get_override_depth(RID p_render_target) const override { return RID(); }
 	virtual RID render_target_get_override_velocity(RID p_render_target) const override { return RID(); }
+	virtual RID render_target_get_override_velocity_depth(RID p_render_target) const override { return RID(); }
 
 	virtual RID render_target_get_texture(RID p_render_target) override { return RID(); }
+
+	virtual void render_target_set_velocity_target_size(RID p_render_target, const Size2i &p_target_size) override {}
+	virtual Size2i render_target_get_velocity_target_size(RID p_render_target) const override { return Size2i(0, 0); }
 };
 
 } // namespace RendererDummy
diff --git a/servers/rendering/renderer_rd/storage_rd/texture_storage.cpp b/servers/rendering/renderer_rd/storage_rd/texture_storage.cpp
index 697d949015..52eacb1609 100644
--- a/servers/rendering/renderer_rd/storage_rd/texture_storage.cpp
+++ b/servers/rendering/renderer_rd/storage_rd/texture_storage.cpp
@@ -3436,7 +3436,7 @@ RID TextureStorage::render_target_get_texture(RID p_render_target) {
 	return rt->texture;
 }
 
-void TextureStorage::render_target_set_override(RID p_render_target, RID p_color_texture, RID p_depth_texture, RID p_velocity_texture) {
+void TextureStorage::render_target_set_override(RID p_render_target, RID p_color_texture, RID p_depth_texture, RID p_velocity_texture, RID p_velocity_depth_texture) {
 	RenderTarget *rt = render_target_owner.get_or_null(p_render_target);
 	ERR_FAIL_NULL(rt);
 
diff --git a/servers/rendering/renderer_rd/storage_rd/texture_storage.h b/servers/rendering/renderer_rd/storage_rd/texture_storage.h
index 3d281cf98a..cfd85b2816 100644
--- a/servers/rendering/renderer_rd/storage_rd/texture_storage.h
+++ b/servers/rendering/renderer_rd/storage_rd/texture_storage.h
@@ -779,15 +779,19 @@ public:
 	virtual void render_target_set_vrs_texture(RID p_render_target, RID p_texture) override;
 	virtual RID render_target_get_vrs_texture(RID p_render_target) const override;
 
-	virtual void render_target_set_override(RID p_render_target, RID p_color_texture, RID p_depth_texture, RID p_velocity_texture) override;
+	virtual void render_target_set_override(RID p_render_target, RID p_color_texture, RID p_depth_texture, RID p_velocity_texture, RID p_velocity_depth_texture) override;
 	virtual RID render_target_get_override_color(RID p_render_target) const override;
 	virtual RID render_target_get_override_depth(RID p_render_target) const override;
 	RID render_target_get_override_depth_slice(RID p_render_target, const uint32_t p_layer) const;
 	virtual RID render_target_get_override_velocity(RID p_render_target) const override;
 	RID render_target_get_override_velocity_slice(RID p_render_target, const uint32_t p_layer) const;
+	virtual RID render_target_get_override_velocity_depth(RID p_render_target) const override { return RID(); }
 
 	virtual RID render_target_get_texture(RID p_render_target) override;
 
+	virtual void render_target_set_velocity_target_size(RID p_render_target, const Size2i &p_target_size) override {}
+	virtual Size2i render_target_get_velocity_target_size(RID p_render_target) const override { return Size2i(0, 0); }
+
 	RID render_target_get_rd_framebuffer(RID p_render_target);
 	RID render_target_get_rd_texture(RID p_render_target);
 	RID render_target_get_rd_texture_slice(RID p_render_target, uint32_t p_layer);
diff --git a/servers/rendering/renderer_viewport.cpp b/servers/rendering/renderer_viewport.cpp
index 4d6435f48a..cdb055f1f0 100644
--- a/servers/rendering/renderer_viewport.cpp
+++ b/servers/rendering/renderer_viewport.cpp
@@ -789,7 +789,16 @@ void RendererViewport::draw_viewports(bool p_swap_buffers) {
 				RSG::texture_storage->render_target_set_override(vp->render_target,
 						xr_interface->get_color_texture(),
 						xr_interface->get_depth_texture(),
-						xr_interface->get_velocity_texture());
+						xr_interface->get_velocity_texture(),
+						xr_interface->get_velocity_depth_texture());
+
+				RSG::texture_storage->render_target_set_velocity_target_size(vp->render_target, xr_interface->get_velocity_target_size());
+
+				if (xr_interface->get_velocity_texture().is_valid()) {
+					viewport_set_force_motion_vectors(vp->self, true);
+				} else {
+					viewport_set_force_motion_vectors(vp->self, false);
+				}
 
 				// render...
 				RSG::scene->set_debug_draw_mode(vp->debug_draw);
@@ -990,7 +999,7 @@ void RendererViewport::_viewport_set_size(Viewport *p_viewport, int p_width, int
 }
 
 bool RendererViewport::_viewport_requires_motion_vectors(Viewport *p_viewport) {
-	return p_viewport->use_taa || p_viewport->scaling_3d_mode == RenderingServer::VIEWPORT_SCALING_3D_MODE_FSR2 || p_viewport->debug_draw == RenderingServer::VIEWPORT_DEBUG_DRAW_MOTION_VECTORS;
+	return p_viewport->use_taa || p_viewport->scaling_3d_mode == RenderingServer::VIEWPORT_SCALING_3D_MODE_FSR2 || p_viewport->debug_draw == RenderingServer::VIEWPORT_DEBUG_DRAW_MOTION_VECTORS || p_viewport->force_motion_vectors;
 }
 
 void RendererViewport::viewport_set_active(RID p_viewport, bool p_active) {
@@ -1338,6 +1347,25 @@ void RendererViewport::viewport_set_use_debanding(RID p_viewport, bool p_use_deb
 	_configure_3d_render_buffers(viewport);
 }
 
+void RendererViewport::viewport_set_force_motion_vectors(RID p_viewport, bool p_force_motion_vectors) {
+	Viewport *viewport = viewport_owner.get_or_null(p_viewport);
+	ERR_FAIL_NULL(viewport);
+
+	if (viewport->force_motion_vectors == p_force_motion_vectors) {
+		return;
+	}
+
+	bool motion_vectors_before = _viewport_requires_motion_vectors(viewport);
+	viewport->force_motion_vectors = p_force_motion_vectors;
+
+	bool motion_vectors_after = _viewport_requires_motion_vectors(viewport);
+	if (motion_vectors_before != motion_vectors_after) {
+		num_viewports_with_motion_vectors += motion_vectors_after ? 1 : -1;
+	}
+
+	_configure_3d_render_buffers(viewport);
+}
+
 void RendererViewport::viewport_set_use_occlusion_culling(RID p_viewport, bool p_use_occlusion_culling) {
 	Viewport *viewport = viewport_owner.get_or_null(p_viewport);
 	ERR_FAIL_NULL(viewport);
diff --git a/servers/rendering/renderer_viewport.h b/servers/rendering/renderer_viewport.h
index bf97905f86..21cc06d6e7 100644
--- a/servers/rendering/renderer_viewport.h
+++ b/servers/rendering/renderer_viewport.h
@@ -77,6 +77,7 @@ public:
 		RS::ViewportScreenSpaceAA screen_space_aa = RenderingServer::VIEWPORT_SCREEN_SPACE_AA_DISABLED;
 		bool use_taa = false;
 		bool use_debanding = false;
+		bool force_motion_vectors = false;
 
 		RendererSceneRender::CameraData prev_camera_data;
 		uint64_t prev_camera_data_frame = 0;
@@ -277,6 +278,7 @@ public:
 	void viewport_set_screen_space_aa(RID p_viewport, RS::ViewportScreenSpaceAA p_mode);
 	void viewport_set_use_taa(RID p_viewport, bool p_use_taa);
 	void viewport_set_use_debanding(RID p_viewport, bool p_use_debanding);
+	void viewport_set_force_motion_vectors(RID p_viewport, bool p_force_motion_vectors);
 	void viewport_set_use_occlusion_culling(RID p_viewport, bool p_use_occlusion_culling);
 	void viewport_set_occlusion_rays_per_thread(int p_rays_per_thread);
 	void viewport_set_occlusion_culling_build_quality(RS::ViewportOcclusionCullingBuildQuality p_quality);
diff --git a/servers/rendering/rendering_server_default.h b/servers/rendering/rendering_server_default.h
index 29b1e163c7..4cad8682e0 100644
--- a/servers/rendering/rendering_server_default.h
+++ b/servers/rendering/rendering_server_default.h
@@ -724,6 +724,7 @@ public:
 	FUNC2(viewport_set_screen_space_aa, RID, ViewportScreenSpaceAA)
 	FUNC2(viewport_set_use_taa, RID, bool)
 	FUNC2(viewport_set_use_debanding, RID, bool)
+	FUNC2(viewport_set_force_motion_vectors, RID, bool)
 	FUNC2(viewport_set_use_occlusion_culling, RID, bool)
 	FUNC1(viewport_set_occlusion_rays_per_thread, int)
 	FUNC1(viewport_set_occlusion_culling_build_quality, ViewportOcclusionCullingBuildQuality)
diff --git a/servers/rendering/storage/texture_storage.h b/servers/rendering/storage/texture_storage.h
index e528384891..45e21a7055 100644
--- a/servers/rendering/storage/texture_storage.h
+++ b/servers/rendering/storage/texture_storage.h
@@ -178,13 +178,18 @@ public:
 	virtual RID render_target_get_vrs_texture(RID p_render_target) const = 0;
 
 	// override color, depth and velocity buffers (depth and velocity only for 3D)
-	virtual void render_target_set_override(RID p_render_target, RID p_color_texture, RID p_depth_texture, RID p_velocity_texture) = 0;
+	virtual void render_target_set_override(RID p_render_target, RID p_color_texture, RID p_depth_texture, RID p_velocity_texture, RID p_velocity_depth_texture) = 0;
 	virtual RID render_target_get_override_color(RID p_render_target) const = 0;
 	virtual RID render_target_get_override_depth(RID p_render_target) const = 0;
 	virtual RID render_target_get_override_velocity(RID p_render_target) const = 0;
+	virtual RID render_target_get_override_velocity_depth(RID p_render_target) const = 0;
 
 	// get textures
 	virtual RID render_target_get_texture(RID p_render_target) = 0;
+
+	// Motion vectors
+	virtual void render_target_set_velocity_target_size(RID p_render_target, const Size2i &p_target_size) = 0;
+	virtual Size2i render_target_get_velocity_target_size(RID p_render_target) const = 0;
 };
 
 #endif // TEXTURE_STORAGE_H
diff --git a/servers/rendering_server.h b/servers/rendering_server.h
index 0917af73c6..9a379e12d1 100644
--- a/servers/rendering_server.h
+++ b/servers/rendering_server.h
@@ -1015,6 +1015,8 @@ public:
 
 	virtual void viewport_set_use_debanding(RID p_viewport, bool p_use_debanding) = 0;
 
+	virtual void viewport_set_force_motion_vectors(RID p_viewport, bool p_force_motion_vectors) = 0;
+
 	virtual void viewport_set_mesh_lod_threshold(RID p_viewport, float p_pixels) = 0;
 
 	virtual void viewport_set_use_occlusion_culling(RID p_viewport, bool p_use_occlusion_culling) = 0;
diff --git a/servers/xr/xr_interface.cpp b/servers/xr/xr_interface.cpp
index e9d2fc8520..c7fc017d78 100644
--- a/servers/xr/xr_interface.cpp
+++ b/servers/xr/xr_interface.cpp
@@ -187,6 +187,14 @@ RID XRInterface::get_velocity_texture() {
 	return RID();
 }
 
+RID XRInterface::get_velocity_depth_texture() {
+	return RID();
+}
+
+Size2i XRInterface::get_velocity_target_size() {
+	return Size2i();
+}
+
 PackedStringArray XRInterface::get_suggested_tracker_names() const {
 	PackedStringArray arr;
 
diff --git a/servers/xr/xr_interface.h b/servers/xr/xr_interface.h
index abd5476fc9..883eb9cbc5 100644
--- a/servers/xr/xr_interface.h
+++ b/servers/xr/xr_interface.h
@@ -137,6 +137,8 @@ public:
 	virtual RID get_color_texture(); /* obtain color output texture (if applicable) */
 	virtual RID get_depth_texture(); /* obtain depth output texture (if applicable, used for reprojection) */
 	virtual RID get_velocity_texture(); /* obtain velocity output texture (if applicable, used for spacewarp) */
+	virtual RID get_velocity_depth_texture();
+	virtual Size2i get_velocity_target_size();
 	virtual void pre_render() {}
 	virtual bool pre_draw_viewport(RID p_render_target) { return true; } /* inform XR interface we are about to start our viewport draw process */
 	virtual Vector<BlitToScreen> post_draw_viewport(RID p_render_target, const Rect2 &p_screen_rect) = 0; /* inform XR interface we finished our viewport draw process */

From 4d6a6b21e2f4b8e2a6a69c27524fc04a4ebe18ca Mon Sep 17 00:00:00 2001
From: Markus Sauermann <6299227+Sauermann@users.noreply.github.com>
Date: Tue, 10 Dec 2024 23:50:11 +0100
Subject: [PATCH 068/124] Allow canceling drag-and-drop with right mouse button

This is a small usability enhancement, that allows users to cancel
drag-and-drop without the need to press the escape key on the keyboard.
---
 scene/main/viewport.cpp     |  5 +++++
 tests/scene/test_viewport.h | 13 +++++++++++--
 2 files changed, 16 insertions(+), 2 deletions(-)

diff --git a/scene/main/viewport.cpp b/scene/main/viewport.cpp
index 1a047dfa27..06724b461f 100644
--- a/scene/main/viewport.cpp
+++ b/scene/main/viewport.cpp
@@ -1742,6 +1742,11 @@ void Viewport::_gui_input_event(Ref<InputEvent> p_event) {
 	if (mb.is_valid()) {
 		Point2 mpos = mb->get_position();
 		if (mb->is_pressed()) {
+			if (gui.dragging && mb->get_button_index() == MouseButton::RIGHT) {
+				_perform_drop();
+				set_input_as_handled();
+				return;
+			}
 			MouseButtonMask button_mask = mouse_button_to_mask(mb->get_button_index());
 			if (!gui.mouse_focus_mask.is_empty() && !gui.mouse_focus_mask.has_flag(button_mask)) {
 				// Do not steal mouse focus and stuff while a focus mask without the current mouse button exists.
diff --git a/tests/scene/test_viewport.h b/tests/scene/test_viewport.h
index 06a38f234b..dbf348ba96 100644
--- a/tests/scene/test_viewport.h
+++ b/tests/scene/test_viewport.h
@@ -1341,8 +1341,8 @@ TEST_CASE("[SceneTree][Viewport] Controls and InputEvent handling") {
 				SEND_GUI_MOUSE_MOTION_EVENT(on_d, MouseButtonMask::NONE, Key::NONE);
 
 				// Force Drop doesn't get triggered by mouse Buttons other than LMB.
-				SEND_GUI_MOUSE_BUTTON_EVENT(on_d, MouseButton::RIGHT, MouseButtonMask::RIGHT, Key::NONE);
-				SEND_GUI_MOUSE_BUTTON_RELEASED_EVENT(on_a, MouseButton::RIGHT, MouseButtonMask::NONE, Key::NONE);
+				SEND_GUI_MOUSE_BUTTON_EVENT(on_d, MouseButton::MIDDLE, MouseButtonMask::MIDDLE, Key::NONE);
+				SEND_GUI_MOUSE_BUTTON_RELEASED_EVENT(on_a, MouseButton::MIDDLE, MouseButtonMask::NONE, Key::NONE);
 				CHECK(root->gui_is_dragging());
 
 				// Force Drop with LMB-Down.
@@ -1351,6 +1351,15 @@ TEST_CASE("[SceneTree][Viewport] Controls and InputEvent handling") {
 				CHECK(root->gui_is_drag_successful());
 
 				SEND_GUI_MOUSE_BUTTON_RELEASED_EVENT(on_d, MouseButton::LEFT, MouseButtonMask::NONE, Key::NONE);
+
+				node_a->force_drag(SNAME("Drag Data"), nullptr);
+				CHECK(root->gui_is_dragging());
+
+				// Cancel with RMB.
+				SEND_GUI_MOUSE_BUTTON_EVENT(on_d, MouseButton::RIGHT, MouseButtonMask::RIGHT, Key::NONE);
+				CHECK_FALSE(root->gui_is_dragging());
+				CHECK_FALSE(root->gui_is_drag_successful());
+				SEND_GUI_MOUSE_BUTTON_RELEASED_EVENT(on_a, MouseButton::RIGHT, MouseButtonMask::NONE, Key::NONE);
 			}
 		}
 

From 7c84eba6630cf73ac40de1c0747d274c8c202c2d Mon Sep 17 00:00:00 2001
From: smix8 <52464204+smix8@users.noreply.github.com>
Date: Mon, 9 Dec 2024 00:37:38 +0100
Subject: [PATCH 069/124] Add basic editor editing for ConvexPolygonShape2D and
 ConcavePolygonShape2D

Adds basic editor tooling to move the existing points of a ConvexPolygonShape2D or move the  segments of a ConcavePolygonShape2D while a CollisionShape2D node is selected.
---
 .../collision_shape_2d_editor_plugin.cpp      | 72 ++++++++++++++++++-
 scene/2d/physics/collision_shape_2d.cpp       |  2 +-
 2 files changed, 71 insertions(+), 3 deletions(-)

diff --git a/editor/plugins/collision_shape_2d_editor_plugin.cpp b/editor/plugins/collision_shape_2d_editor_plugin.cpp
index 40c13ea3c4..7c4c3e25d2 100644
--- a/editor/plugins/collision_shape_2d_editor_plugin.cpp
+++ b/editor/plugins/collision_shape_2d_editor_plugin.cpp
@@ -73,9 +73,17 @@ Variant CollisionShape2DEditor::get_handle_value(int idx) const {
 		} break;
 
 		case CONCAVE_POLYGON_SHAPE: {
+			Ref<ConcavePolygonShape2D> shape = node->get_shape();
+			const Vector<Vector2> &segments = shape->get_segments();
+			return segments[idx];
+
 		} break;
 
 		case CONVEX_POLYGON_SHAPE: {
+			Ref<ConvexPolygonShape2D> shape = node->get_shape();
+			const Vector<Vector2> &points = shape->get_points();
+			return points[idx];
+
 		} break;
 
 		case WORLD_BOUNDARY_SHAPE: {
@@ -145,9 +153,27 @@ void CollisionShape2DEditor::set_handle(int idx, Point2 &p_point) {
 		} break;
 
 		case CONCAVE_POLYGON_SHAPE: {
+			Ref<ConcavePolygonShape2D> concave_shape = node->get_shape();
+
+			Vector<Vector2> segments = concave_shape->get_segments();
+
+			ERR_FAIL_INDEX(idx, segments.size());
+			segments.write[idx] = p_point;
+
+			concave_shape->set_segments(segments);
+
 		} break;
 
 		case CONVEX_POLYGON_SHAPE: {
+			Ref<ConvexPolygonShape2D> convex_shape = node->get_shape();
+
+			Vector<Vector2> points = convex_shape->get_points();
+
+			ERR_FAIL_INDEX(idx, points.size());
+			points.write[idx] = p_point;
+
+			convex_shape->set_points(points);
+
 		} break;
 
 		case WORLD_BOUNDARY_SHAPE: {
@@ -237,11 +263,33 @@ void CollisionShape2DEditor::commit_handle(int idx, Variant &p_org) {
 		} break;
 
 		case CONCAVE_POLYGON_SHAPE: {
-			// Cannot be edited directly, use CollisionPolygon2D instead.
+			Ref<ConcavePolygonShape2D> concave_shape = node->get_shape();
+
+			Vector2 values = p_org;
+
+			Vector<Vector2> undo_segments = concave_shape->get_segments();
+
+			ERR_FAIL_INDEX(idx, undo_segments.size());
+			undo_segments.write[idx] = values;
+
+			undo_redo->add_do_method(concave_shape.ptr(), "set_segments", concave_shape->get_segments());
+			undo_redo->add_undo_method(concave_shape.ptr(), "set_segments", undo_segments);
+
 		} break;
 
 		case CONVEX_POLYGON_SHAPE: {
-			// Cannot be edited directly, use CollisionPolygon2D instead.
+			Ref<ConvexPolygonShape2D> convex_shape = node->get_shape();
+
+			Vector2 values = p_org;
+
+			Vector<Vector2> undo_points = convex_shape->get_points();
+
+			ERR_FAIL_INDEX(idx, undo_points.size());
+			undo_points.write[idx] = values;
+
+			undo_redo->add_do_method(convex_shape.ptr(), "set_points", convex_shape->get_points());
+			undo_redo->add_undo_method(convex_shape.ptr(), "set_points", undo_points);
+
 		} break;
 
 		case WORLD_BOUNDARY_SHAPE: {
@@ -471,9 +519,29 @@ void CollisionShape2DEditor::forward_canvas_draw_over_viewport(Control *p_overla
 		} break;
 
 		case CONCAVE_POLYGON_SHAPE: {
+			Ref<ConcavePolygonShape2D> shape = current_shape;
+
+			const Vector<Vector2> &segments = shape->get_segments();
+
+			handles.resize(segments.size());
+			for (int i = 0; i < handles.size(); i++) {
+				handles.write[i] = segments[i];
+				p_overlay->draw_texture(h, gt.xform(handles[i]) - size);
+			}
+
 		} break;
 
 		case CONVEX_POLYGON_SHAPE: {
+			Ref<ConvexPolygonShape2D> shape = current_shape;
+
+			const Vector<Vector2> &points = shape->get_points();
+
+			handles.resize(points.size());
+			for (int i = 0; i < handles.size(); i++) {
+				handles.write[i] = points[i];
+				p_overlay->draw_texture(h, gt.xform(handles[i]) - size);
+			}
+
 		} break;
 
 		case WORLD_BOUNDARY_SHAPE: {
diff --git a/scene/2d/physics/collision_shape_2d.cpp b/scene/2d/physics/collision_shape_2d.cpp
index bdd0d06b5e..a6ddff4563 100644
--- a/scene/2d/physics/collision_shape_2d.cpp
+++ b/scene/2d/physics/collision_shape_2d.cpp
@@ -190,7 +190,7 @@ PackedStringArray CollisionShape2D::get_configuration_warnings() const {
 	Ref<ConvexPolygonShape2D> convex = shape;
 	Ref<ConcavePolygonShape2D> concave = shape;
 	if (convex.is_valid() || concave.is_valid()) {
-		warnings.push_back(RTR("Polygon-based shapes are not meant be used nor edited directly through the CollisionShape2D node. Please use the CollisionPolygon2D node instead."));
+		warnings.push_back(RTR("The CollisionShape2D node has limited editing options for polygon-based shapes. Consider using a CollisionPolygon2D node instead."));
 	}
 
 	return warnings;

From 8b853b7db766acacb2b49ad18de470fc126b984c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Pa=CC=84vels=20Nadtoc=CC=8Cajevs?=
 <7645683+bruvzg@users.noreply.github.com>
Date: Wed, 11 Dec 2024 09:21:25 +0200
Subject: [PATCH 070/124] [macOS] Print full source paths in the trace.

---
 platform/macos/crash_handler_macos.mm | 1 +
 1 file changed, 1 insertion(+)

diff --git a/platform/macos/crash_handler_macos.mm b/platform/macos/crash_handler_macos.mm
index c370422bfa..bc83c077b1 100644
--- a/platform/macos/crash_handler_macos.mm
+++ b/platform/macos/crash_handler_macos.mm
@@ -149,6 +149,7 @@ static void handle_crash(int sig) {
 				args.push_back("-arch");
 				args.push_back("arm64");
 #endif
+				args.push_back("--fullPath");
 				args.push_back("-l");
 				snprintf(str, 1024, "%p", load_addr);
 				args.push_back(str);

From 7adb986e15c46e41e1e77a1fc8371ebcafb2a956 Mon Sep 17 00:00:00 2001
From: Chaosus <chaosus89@gmail.com>
Date: Mon, 2 Dec 2024 21:21:05 +0300
Subject: [PATCH 071/124] Fix orphan strings in `AnimationNodeStateMachine`

---
 .../animation_state_machine_editor.cpp        | 12 ++---
 .../animation_node_state_machine.cpp          | 48 ++++++++-----------
 .../animation/animation_node_state_machine.h  |  3 --
 scene/scene_string_names.h                    |  3 ++
 4 files changed, 30 insertions(+), 36 deletions(-)

diff --git a/editor/plugins/animation_state_machine_editor.cpp b/editor/plugins/animation_state_machine_editor.cpp
index 17dc158a04..428e0b14a9 100644
--- a/editor/plugins/animation_state_machine_editor.cpp
+++ b/editor/plugins/animation_state_machine_editor.cpp
@@ -691,7 +691,7 @@ bool AnimationNodeStateMachineEditor::_create_submenu(PopupMenu *p_menu, Ref<Ani
 
 			if (ansm == state_machine) {
 				end_menu->add_item(E, nodes_to_connect.size());
-				nodes_to_connect.push_back(AnimationNodeStateMachine::END_NODE);
+				nodes_to_connect.push_back(SceneStringName(End));
 				continue;
 			}
 
@@ -1241,11 +1241,11 @@ void AnimationNodeStateMachineEditor::_state_machine_draw() {
 
 		state_machine_draw->draw_style_box(node_style, nr.node);
 
-		if (!is_selected && AnimationNodeStateMachine::START_NODE == name) {
+		if (!is_selected && SceneStringName(Start) == name) {
 			Ref<StyleBox> start_style = _adjust_stylebox_opacity(theme_cache.node_frame_start, opacity);
 			state_machine_draw->draw_style_box(start_style, nr.node);
 		}
-		if (!is_selected && AnimationNodeStateMachine::END_NODE == name) {
+		if (!is_selected && SceneStringName(End) == name) {
 			Ref<StyleBox> end_style = _adjust_stylebox_opacity(theme_cache.node_frame_end, opacity);
 			state_machine_draw->draw_style_box(end_style, nr.node);
 		}
@@ -1341,7 +1341,7 @@ void AnimationNodeStateMachineEditor::_state_machine_pos_draw_individual(const S
 		return;
 	}
 
-	if (p_name == AnimationNodeStateMachine::START_NODE || p_name == AnimationNodeStateMachine::END_NODE || p_name.is_empty()) {
+	if (p_name == SceneStringName(Start) || p_name == SceneStringName(End) || p_name.is_empty()) {
 		return;
 	}
 
@@ -1676,7 +1676,7 @@ void AnimationNodeStateMachineEditor::_erase_selected(const bool p_nested_action
 		undo_redo->create_action(TTR("Node Removed"));
 
 		for (int i = 0; i < node_rects.size(); i++) {
-			if (node_rects[i].node_name == AnimationNodeStateMachine::START_NODE || node_rects[i].node_name == AnimationNodeStateMachine::END_NODE) {
+			if (node_rects[i].node_name == SceneStringName(Start) || node_rects[i].node_name == SceneStringName(End)) {
 				continue;
 			}
 
@@ -1736,7 +1736,7 @@ void AnimationNodeStateMachineEditor::_update_mode() {
 	if (tool_select->is_pressed()) {
 		selection_tools_hb->show();
 		bool nothing_selected = selected_nodes.is_empty() && selected_transition_from == StringName() && selected_transition_to == StringName();
-		bool start_end_selected = selected_nodes.size() == 1 && (*selected_nodes.begin() == AnimationNodeStateMachine::START_NODE || *selected_nodes.begin() == AnimationNodeStateMachine::END_NODE);
+		bool start_end_selected = selected_nodes.size() == 1 && (*selected_nodes.begin() == SceneStringName(Start) || *selected_nodes.begin() == SceneStringName(End));
 		tool_erase->set_disabled(nothing_selected || start_end_selected || read_only);
 	} else {
 		selection_tools_hb->hide();
diff --git a/scene/animation/animation_node_state_machine.cpp b/scene/animation/animation_node_state_machine.cpp
index 420caaad7f..844cc9d622 100644
--- a/scene/animation/animation_node_state_machine.cpp
+++ b/scene/animation/animation_node_state_machine.cpp
@@ -31,9 +31,6 @@
 #include "animation_node_state_machine.h"
 #include "scene/main/window.h"
 
-StringName AnimationNodeStateMachine::START_NODE;
-StringName AnimationNodeStateMachine::END_NODE;
-
 /////////////////////////////////////////////////
 
 void AnimationNodeStateMachineTransition::set_switch_mode(SwitchMode p_mode) {
@@ -229,9 +226,9 @@ void AnimationNodeStateMachinePlayback::_set_current(AnimationNodeStateMachine *
 
 	// Validation.
 	if (anodesm->get_state_machine_type() == AnimationNodeStateMachine::STATE_MACHINE_TYPE_GROUPED) {
-		indices = anodesm->find_transition_from(AnimationNodeStateMachine::START_NODE);
+		indices = anodesm->find_transition_from(SceneStringName(Start));
 		int anodesm_start_size = indices.size();
-		indices = anodesm->find_transition_to(AnimationNodeStateMachine::END_NODE);
+		indices = anodesm->find_transition_to(SceneStringName(End));
 		int anodesm_end_size = indices.size();
 		if (group_start_size > 1) {
 			WARN_PRINT_ED("There are two or more transitions to the Grouped AnimationNodeStateMachine in AnimationNodeStateMachine: " + base_path + ", which may result in unintended transitions.");
@@ -307,7 +304,7 @@ bool AnimationNodeStateMachinePlayback::is_playing() const {
 }
 
 bool AnimationNodeStateMachinePlayback::is_end() const {
-	return current == AnimationNodeStateMachine::END_NODE && fading_from == StringName();
+	return current == SceneStringName(End) && fading_from == StringName();
 }
 
 StringName AnimationNodeStateMachinePlayback::get_current_node() const {
@@ -447,7 +444,7 @@ bool AnimationNodeStateMachinePlayback::_travel_children(AnimationTree *p_tree,
 					if (p_test_only) {
 						child_playback = child_playback->duplicate();
 					}
-					child_playback->_travel_main(AnimationNodeStateMachine::END_NODE);
+					child_playback->_travel_main(SceneStringName(End));
 					child_found_route &= child_playback->_travel(p_tree, child_anodesm.ptr(), false, p_test_only);
 					child_path += "/" + child_playback->get_current_node();
 				}
@@ -490,7 +487,7 @@ bool AnimationNodeStateMachinePlayback::_travel_children(AnimationTree *p_tree,
 
 void AnimationNodeStateMachinePlayback::_start(AnimationNodeStateMachine *p_state_machine) {
 	playing = true;
-	_set_current(p_state_machine, start_request != StringName() ? start_request : AnimationNodeStateMachine::START_NODE);
+	_set_current(p_state_machine, start_request != StringName() ? start_request : SceneStringName(Start));
 	teleport_request = true;
 	stop_request = false;
 	start_request = StringName();
@@ -507,7 +504,7 @@ String AnimationNodeStateMachinePlayback::_validate_path(AnimationNodeStateMachi
 	String target = p_path;
 	Ref<AnimationNodeStateMachine> anodesm = p_state_machine->find_node_by_path(target);
 	while (anodesm.is_valid() && anodesm->get_state_machine_type() == AnimationNodeStateMachine::STATE_MACHINE_TYPE_GROUPED) {
-		Vector<int> indices = anodesm->find_transition_from(AnimationNodeStateMachine::START_NODE);
+		Vector<int> indices = anodesm->find_transition_from(SceneStringName(Start));
 		if (indices.size()) {
 			target = target + "/" + anodesm->get_transition_to(indices[0]); // Find next state of Start.
 		} else {
@@ -660,7 +657,7 @@ bool AnimationNodeStateMachinePlayback::_make_travel_path(AnimationTree *p_tree,
 				if (i >= new_path.size()) {
 					break; // Tracing has been finished, needs to break.
 				}
-				playback->_travel_main(AnimationNodeStateMachine::END_NODE);
+				playback->_travel_main(SceneStringName(End));
 				if (!playback->_travel(p_tree, anodesm.ptr(), false, p_test_only)) {
 					found_route = false;
 					break;
@@ -818,8 +815,8 @@ AnimationNode::NodeTimeInfo AnimationNodeStateMachinePlayback::_process(const St
 	bool is_start_of_group = false;
 	bool is_end_of_group = false;
 	if (!p_state_machine->are_ends_reset() || p_state_machine->get_state_machine_type() == AnimationNodeStateMachine::STATE_MACHINE_TYPE_GROUPED) {
-		is_start_of_group = fading_from == AnimationNodeStateMachine::START_NODE;
-		is_end_of_group = current == AnimationNodeStateMachine::END_NODE;
+		is_start_of_group = fading_from == SceneStringName(Start);
+		is_end_of_group = current == SceneStringName(End);
 	}
 
 	// Calc blend amount by cross-fade.
@@ -881,7 +878,7 @@ AnimationNode::NodeTimeInfo AnimationNodeStateMachinePlayback::_process(const St
 	}
 
 	// Find next and see when to transition.
-	bool will_end = _transition_to_next_recursive(tree, p_state_machine, p_delta, p_test_only) || current == AnimationNodeStateMachine::END_NODE;
+	bool will_end = _transition_to_next_recursive(tree, p_state_machine, p_delta, p_test_only) || current == SceneStringName(End);
 
 	// Predict remaining time.
 	if (will_end || ((p_state_machine->get_state_machine_type() == AnimationNodeStateMachine::STATE_MACHINE_TYPE_NESTED) && !p_state_machine->has_transition_from(current))) {
@@ -951,7 +948,7 @@ bool AnimationNodeStateMachinePlayback::_transition_to_next_recursive(AnimationT
 		_set_current(p_state_machine, next.node);
 		current_curve = next.curve;
 
-		if (current == AnimationNodeStateMachine::END_NODE) {
+		if (current == SceneStringName(End)) {
 			break;
 		}
 
@@ -981,7 +978,7 @@ bool AnimationNodeStateMachinePlayback::_transition_to_next_recursive(AnimationT
 		}
 	}
 
-	return next.node == AnimationNodeStateMachine::END_NODE;
+	return next.node == SceneStringName(End);
 }
 
 bool AnimationNodeStateMachinePlayback::_can_transition_to_next(AnimationTree *p_tree, AnimationNodeStateMachine *p_state_machine, NextInfo p_next, bool p_test_only) {
@@ -1015,7 +1012,7 @@ bool AnimationNodeStateMachinePlayback::_can_transition_to_next(AnimationTree *p
 		return false;
 	}
 
-	if (current != AnimationNodeStateMachine::START_NODE && p_next.switch_mode == AnimationNodeStateMachineTransition::SWITCH_MODE_AT_END) {
+	if (current != SceneStringName(Start) && p_next.switch_mode == AnimationNodeStateMachineTransition::SWITCH_MODE_AT_END) {
 		return Animation::is_less_or_equal_approx(current_nti.get_remain(p_next.break_loop_at_end), p_next.xfade);
 	}
 	return true;
@@ -1025,13 +1022,13 @@ Ref<AnimationNodeStateMachineTransition> AnimationNodeStateMachinePlayback::_che
 	Ref<AnimationNodeStateMachineTransition> temp_transition;
 	Ref<AnimationNodeStateMachinePlayback> parent_playback;
 	if (r_state_machine->get_state_machine_type() == AnimationNodeStateMachine::STATE_MACHINE_TYPE_GROUPED) {
-		if (p_transition.from == AnimationNodeStateMachine::START_NODE) {
+		if (p_transition.from == SceneStringName(Start)) {
 			parent_playback = _get_parent_playback(p_tree);
 			if (parent_playback.is_valid()) {
 				r_bypass = true;
 				temp_transition = parent_playback->_get_group_start_transition();
 			}
-		} else if (p_transition.to == AnimationNodeStateMachine::END_NODE) {
+		} else if (p_transition.to == SceneStringName(End)) {
 			parent_playback = _get_parent_playback(p_tree);
 			if (parent_playback.is_valid()) {
 				temp_transition = parent_playback->_get_group_end_transition();
@@ -1518,7 +1515,7 @@ void AnimationNodeStateMachine::add_transition(const StringName &p_from, const S
 		return;
 	}
 
-	ERR_FAIL_COND(p_from == END_NODE || p_to == START_NODE);
+	ERR_FAIL_COND(p_from == SceneStringName(End) || p_to == SceneStringName(Start));
 	ERR_FAIL_COND(p_from == p_to);
 	ERR_FAIL_COND(!_can_connect(p_from));
 	ERR_FAIL_COND(!_can_connect(p_to));
@@ -1560,7 +1557,7 @@ StringName AnimationNodeStateMachine::get_transition_to(int p_transition) const
 bool AnimationNodeStateMachine::is_transition_across_group(int p_transition) const {
 	ERR_FAIL_INDEX_V(p_transition, transitions.size(), false);
 	if (get_state_machine_type() == AnimationNodeStateMachine::STATE_MACHINE_TYPE_GROUPED) {
-		if (transitions[p_transition].from == START_NODE || transitions[p_transition].to == END_NODE) {
+		if (transitions[p_transition].from == SceneStringName(Start) || transitions[p_transition].to == SceneStringName(End)) {
 			return true;
 		}
 	}
@@ -1745,14 +1742,14 @@ void AnimationNodeStateMachine::reset_state() {
 	State start;
 	start.node = s;
 	start.position = Vector2(200, 100);
-	states[START_NODE] = start;
+	states[SceneStringName(Start)] = start;
 
 	Ref<AnimationNodeEndState> e;
 	e.instantiate();
 	State end;
 	end.node = e;
 	end.position = Vector2(900, 100);
-	states[END_NODE] = end;
+	states[SceneStringName(End)] = end;
 
 	emit_changed();
 	emit_signal(SNAME("tree_changed"));
@@ -1862,20 +1859,17 @@ Vector<StringName> AnimationNodeStateMachine::get_nodes_with_transitions_to(cons
 }
 
 AnimationNodeStateMachine::AnimationNodeStateMachine() {
-	START_NODE = "Start";
-	END_NODE = "End";
-
 	Ref<AnimationNodeStartState> s;
 	s.instantiate();
 	State start;
 	start.node = s;
 	start.position = Vector2(200, 100);
-	states[START_NODE] = start;
+	states[SceneStringName(Start)] = start;
 
 	Ref<AnimationNodeEndState> e;
 	e.instantiate();
 	State end;
 	end.node = e;
 	end.position = Vector2(900, 100);
-	states[END_NODE] = end;
+	states[SceneStringName(End)] = end;
 }
diff --git a/scene/animation/animation_node_state_machine.h b/scene/animation/animation_node_state_machine.h
index 286c77aa04..ae28c126b3 100644
--- a/scene/animation/animation_node_state_machine.h
+++ b/scene/animation/animation_node_state_machine.h
@@ -110,9 +110,6 @@ class AnimationNodeStateMachine : public AnimationRootNode {
 	GDCLASS(AnimationNodeStateMachine, AnimationRootNode);
 
 public:
-	static StringName START_NODE;
-	static StringName END_NODE;
-
 	enum StateMachineType {
 		STATE_MACHINE_TYPE_ROOT,
 		STATE_MACHINE_TYPE_NESTED,
diff --git a/scene/scene_string_names.h b/scene/scene_string_names.h
index 47eaaa3c86..9b18394792 100644
--- a/scene/scene_string_names.h
+++ b/scene/scene_string_names.h
@@ -151,6 +151,9 @@ public:
 	const StringName text_submitted = StaticCString::create("text_submitted");
 	const StringName value_changed = StaticCString::create("value_changed");
 
+	const StringName Start = StaticCString::create("Start");
+	const StringName End = StaticCString::create("End");
+
 	const StringName FlatButton = StaticCString::create("FlatButton");
 };
 

From 9a8d6628de9b3ef5f5d1e3aaf67b365e2fc5c78f Mon Sep 17 00:00:00 2001
From: Chaosus <chaosus89@gmail.com>
Date: Thu, 5 Dec 2024 11:24:52 +0300
Subject: [PATCH 072/124] [Scene] Add `SceneStringName::hover`

---
 editor/animation_track_editor.cpp             |  2 +-
 editor/editor_audio_buses.cpp                 |  2 +-
 editor/gui/editor_quick_open_dialog.cpp       |  2 +-
 editor/gui/editor_zoom_widget.cpp             |  2 +-
 editor/import/3d/scene_import_settings.cpp    |  2 +-
 editor/plugins/node_3d_editor_plugin.cpp      |  4 +--
 .../tiles/tile_set_atlas_source_editor.cpp    |  2 +-
 editor/themes/editor_theme_manager.cpp        | 36 +++++++++----------
 scene/gui/color_picker.cpp                    |  2 +-
 scene/scene_string_names.h                    |  1 +
 scene/theme/default_theme.cpp                 | 20 +++++------
 11 files changed, 38 insertions(+), 37 deletions(-)

diff --git a/editor/animation_track_editor.cpp b/editor/animation_track_editor.cpp
index 17191197ff..ae2b63dead 100644
--- a/editor/animation_track_editor.cpp
+++ b/editor/animation_track_editor.cpp
@@ -2061,7 +2061,7 @@ void AnimationTrackEdit::_notification(int p_what) {
 
 			const Ref<StyleBox> &stylebox_odd = get_theme_stylebox(SNAME("odd"), SNAME("AnimationTrackEdit"));
 			const Ref<StyleBox> &stylebox_focus = get_theme_stylebox(SNAME("focus"), SNAME("AnimationTrackEdit"));
-			const Ref<StyleBox> &stylebox_hover = get_theme_stylebox(SNAME("hover"), SNAME("AnimationTrackEdit"));
+			const Ref<StyleBox> &stylebox_hover = get_theme_stylebox(SceneStringName(hover), SNAME("AnimationTrackEdit"));
 
 			const Color h_line_color = get_theme_color(SNAME("h_line_color"), SNAME("AnimationTrackEdit"));
 			const int h_separation = get_theme_constant(SNAME("h_separation"), SNAME("AnimationTrackEdit"));
diff --git a/editor/editor_audio_buses.cpp b/editor/editor_audio_buses.cpp
index d429427ab3..54fac1a299 100644
--- a/editor/editor_audio_buses.cpp
+++ b/editor/editor_audio_buses.cpp
@@ -849,7 +849,7 @@ EditorAudioBus::EditorAudioBus(EditorAudioBuses *p_buses, bool p_is_master) {
 		Control *child = Object::cast_to<Control>(hbc->get_child(i));
 		child->begin_bulk_theme_override();
 		child->add_theme_style_override(CoreStringName(normal), sbempty);
-		child->add_theme_style_override("hover", sbempty);
+		child->add_theme_style_override(SceneStringName(hover), sbempty);
 		child->add_theme_style_override("hover_mirrored", sbempty);
 		child->add_theme_style_override("focus", sbempty);
 		child->add_theme_style_override("focus_mirrored", sbempty);
diff --git a/editor/gui/editor_quick_open_dialog.cpp b/editor/gui/editor_quick_open_dialog.cpp
index 44e7b3e483..86fa92f862 100644
--- a/editor/gui/editor_quick_open_dialog.cpp
+++ b/editor/gui/editor_quick_open_dialog.cpp
@@ -816,7 +816,7 @@ void QuickOpenResultItem::_notification(int p_what) {
 		} break;
 		case NOTIFICATION_THEME_CHANGED: {
 			selected_stylebox = get_theme_stylebox("selected", "Tree");
-			hovering_stylebox = get_theme_stylebox("hover", "Tree");
+			hovering_stylebox = get_theme_stylebox(SceneStringName(hover), "Tree");
 			highlighted_font_color = get_theme_color("font_focus_color", EditorStringName(Editor));
 		} break;
 		case NOTIFICATION_DRAW: {
diff --git a/editor/gui/editor_zoom_widget.cpp b/editor/gui/editor_zoom_widget.cpp
index ff232a854f..a5f66cd405 100644
--- a/editor/gui/editor_zoom_widget.cpp
+++ b/editor/gui/editor_zoom_widget.cpp
@@ -199,7 +199,7 @@ EditorZoomWidget::EditorZoomWidget() {
 
 	Ref<StyleBoxEmpty> empty_stylebox = memnew(StyleBoxEmpty);
 	zoom_reset->add_theme_style_override(CoreStringName(normal), empty_stylebox);
-	zoom_reset->add_theme_style_override("hover", empty_stylebox);
+	zoom_reset->add_theme_style_override(SceneStringName(hover), empty_stylebox);
 	zoom_reset->add_theme_style_override("focus", empty_stylebox);
 	zoom_reset->add_theme_style_override(SceneStringName(pressed), empty_stylebox);
 	zoom_reset->add_theme_constant_override("outline_size", Math::ceil(2 * EDSCALE));
diff --git a/editor/import/3d/scene_import_settings.cpp b/editor/import/3d/scene_import_settings.cpp
index 945c1811d7..7bb68349b2 100644
--- a/editor/import/3d/scene_import_settings.cpp
+++ b/editor/import/3d/scene_import_settings.cpp
@@ -1314,7 +1314,7 @@ void SceneImportSettingsDialog::_notification(int p_what) {
 		case NOTIFICATION_THEME_CHANGED: {
 			action_menu->begin_bulk_theme_override();
 			action_menu->add_theme_style_override(CoreStringName(normal), get_theme_stylebox(CoreStringName(normal), "Button"));
-			action_menu->add_theme_style_override("hover", get_theme_stylebox("hover", "Button"));
+			action_menu->add_theme_style_override(SceneStringName(hover), get_theme_stylebox(SceneStringName(hover), "Button"));
 			action_menu->add_theme_style_override(SceneStringName(pressed), get_theme_stylebox(SceneStringName(pressed), "Button"));
 			action_menu->end_bulk_theme_override();
 
diff --git a/editor/plugins/node_3d_editor_plugin.cpp b/editor/plugins/node_3d_editor_plugin.cpp
index 708eac3fbb..470e44d8eb 100644
--- a/editor/plugins/node_3d_editor_plugin.cpp
+++ b/editor/plugins/node_3d_editor_plugin.cpp
@@ -3156,7 +3156,7 @@ void Node3DEditorViewport::_notification(int p_what) {
 			view_menu->begin_bulk_theme_override();
 			view_menu->add_theme_style_override(CoreStringName(normal), information_3d_stylebox);
 			view_menu->add_theme_style_override("normal_mirrored", information_3d_stylebox);
-			view_menu->add_theme_style_override("hover", information_3d_stylebox);
+			view_menu->add_theme_style_override(SceneStringName(hover), information_3d_stylebox);
 			view_menu->add_theme_style_override("hover_mirrored", information_3d_stylebox);
 			view_menu->add_theme_style_override("hover_pressed", information_3d_stylebox);
 			view_menu->add_theme_style_override("hover_pressed_mirrored", information_3d_stylebox);
@@ -3171,7 +3171,7 @@ void Node3DEditorViewport::_notification(int p_what) {
 			preview_camera->begin_bulk_theme_override();
 			preview_camera->add_theme_style_override(CoreStringName(normal), information_3d_stylebox);
 			preview_camera->add_theme_style_override("normal_mirrored", information_3d_stylebox);
-			preview_camera->add_theme_style_override("hover", information_3d_stylebox);
+			preview_camera->add_theme_style_override(SceneStringName(hover), information_3d_stylebox);
 			preview_camera->add_theme_style_override("hover_mirrored", information_3d_stylebox);
 			preview_camera->add_theme_style_override("hover_pressed", information_3d_stylebox);
 			preview_camera->add_theme_style_override("hover_pressed_mirrored", information_3d_stylebox);
diff --git a/editor/plugins/tiles/tile_set_atlas_source_editor.cpp b/editor/plugins/tiles/tile_set_atlas_source_editor.cpp
index 85db510451..78e386d9e0 100644
--- a/editor/plugins/tiles/tile_set_atlas_source_editor.cpp
+++ b/editor/plugins/tiles/tile_set_atlas_source_editor.cpp
@@ -998,7 +998,7 @@ void TileSetAtlasSourceEditor::_update_atlas_view() {
 			button->set_flat(true);
 			button->set_button_icon(get_editor_theme_icon(SNAME("Add")));
 			button->add_theme_style_override(CoreStringName(normal), memnew(StyleBoxEmpty));
-			button->add_theme_style_override("hover", memnew(StyleBoxEmpty));
+			button->add_theme_style_override(SceneStringName(hover), memnew(StyleBoxEmpty));
 			button->add_theme_style_override("focus", memnew(StyleBoxEmpty));
 			button->add_theme_style_override(SceneStringName(pressed), memnew(StyleBoxEmpty));
 			button->connect(SceneStringName(pressed), callable_mp(tile_set_atlas_source, &TileSetAtlasSource::create_alternative_tile).bind(tile_id, TileSetSource::INVALID_TILE_ALTERNATIVE));
diff --git a/editor/themes/editor_theme_manager.cpp b/editor/themes/editor_theme_manager.cpp
index 36b5f3d5c3..f4e4f85b5e 100644
--- a/editor/themes/editor_theme_manager.cpp
+++ b/editor/themes/editor_theme_manager.cpp
@@ -725,7 +725,7 @@ void EditorThemeManager::_populate_standard_styles(const Ref<EditorTheme> &p_the
 		// Button.
 
 		p_theme->set_stylebox(CoreStringName(normal), "Button", p_config.button_style);
-		p_theme->set_stylebox("hover", "Button", p_config.button_style_hover);
+		p_theme->set_stylebox(SceneStringName(hover), "Button", p_config.button_style_hover);
 		p_theme->set_stylebox(SceneStringName(pressed), "Button", p_config.button_style_pressed);
 		p_theme->set_stylebox("focus", "Button", p_config.button_style_focus);
 		p_theme->set_stylebox("disabled", "Button", p_config.button_style_disabled);
@@ -753,7 +753,7 @@ void EditorThemeManager::_populate_standard_styles(const Ref<EditorTheme> &p_the
 		// MenuButton.
 
 		p_theme->set_stylebox(CoreStringName(normal), "MenuButton", p_config.panel_container_style);
-		p_theme->set_stylebox("hover", "MenuButton", p_config.button_style_hover);
+		p_theme->set_stylebox(SceneStringName(hover), "MenuButton", p_config.button_style_hover);
 		p_theme->set_stylebox(SceneStringName(pressed), "MenuButton", p_config.panel_container_style);
 		p_theme->set_stylebox("focus", "MenuButton", p_config.panel_container_style);
 		p_theme->set_stylebox("disabled", "MenuButton", p_config.panel_container_style);
@@ -769,7 +769,7 @@ void EditorThemeManager::_populate_standard_styles(const Ref<EditorTheme> &p_the
 		// MenuBar.
 
 		p_theme->set_stylebox(CoreStringName(normal), "MenuBar", p_config.button_style);
-		p_theme->set_stylebox("hover", "MenuBar", p_config.button_style_hover);
+		p_theme->set_stylebox(SceneStringName(hover), "MenuBar", p_config.button_style_hover);
 		p_theme->set_stylebox(SceneStringName(pressed), "MenuBar", p_config.button_style_pressed);
 		p_theme->set_stylebox("disabled", "MenuBar", p_config.button_style_disabled);
 
@@ -806,7 +806,7 @@ void EditorThemeManager::_populate_standard_styles(const Ref<EditorTheme> &p_the
 
 			p_theme->set_stylebox("focus", "OptionButton", option_button_focus_style);
 			p_theme->set_stylebox(CoreStringName(normal), "OptionButton", p_config.button_style);
-			p_theme->set_stylebox("hover", "OptionButton", p_config.button_style_hover);
+			p_theme->set_stylebox(SceneStringName(hover), "OptionButton", p_config.button_style_hover);
 			p_theme->set_stylebox(SceneStringName(pressed), "OptionButton", p_config.button_style_pressed);
 			p_theme->set_stylebox("disabled", "OptionButton", p_config.button_style_disabled);
 
@@ -841,7 +841,7 @@ void EditorThemeManager::_populate_standard_styles(const Ref<EditorTheme> &p_the
 		p_theme->set_stylebox(CoreStringName(normal), "CheckButton", p_config.panel_container_style);
 		p_theme->set_stylebox(SceneStringName(pressed), "CheckButton", p_config.panel_container_style);
 		p_theme->set_stylebox("disabled", "CheckButton", p_config.panel_container_style);
-		p_theme->set_stylebox("hover", "CheckButton", p_config.panel_container_style);
+		p_theme->set_stylebox(SceneStringName(hover), "CheckButton", p_config.panel_container_style);
 		p_theme->set_stylebox("hover_pressed", "CheckButton", p_config.panel_container_style);
 
 		p_theme->set_icon("checked", "CheckButton", p_theme->get_icon(SNAME("GuiToggleOn"), EditorStringName(EditorIcons)));
@@ -879,7 +879,7 @@ void EditorThemeManager::_populate_standard_styles(const Ref<EditorTheme> &p_the
 			p_theme->set_stylebox(CoreStringName(normal), "CheckBox", checkbox_style);
 			p_theme->set_stylebox(SceneStringName(pressed), "CheckBox", checkbox_style);
 			p_theme->set_stylebox("disabled", "CheckBox", checkbox_style);
-			p_theme->set_stylebox("hover", "CheckBox", checkbox_style);
+			p_theme->set_stylebox(SceneStringName(hover), "CheckBox", checkbox_style);
 			p_theme->set_stylebox("hover_pressed", "CheckBox", checkbox_style);
 			p_theme->set_icon("checked", "CheckBox", p_theme->get_icon(SNAME("GuiChecked"), EditorStringName(EditorIcons)));
 			p_theme->set_icon("unchecked", "CheckBox", p_theme->get_icon(SNAME("GuiUnchecked"), EditorStringName(EditorIcons)));
@@ -1334,7 +1334,7 @@ void EditorThemeManager::_populate_standard_styles(const Ref<EditorTheme> &p_the
 			Ref<StyleBoxFlat> style_menu_hover = p_config.button_style_hover->duplicate();
 			// Don't use rounded corners for hover highlights since the StyleBox touches the PopupMenu's edges.
 			style_menu_hover->set_corner_radius_all(0);
-			p_theme->set_stylebox("hover", "PopupMenu", style_menu_hover);
+			p_theme->set_stylebox(SceneStringName(hover), "PopupMenu", style_menu_hover);
 
 			Ref<StyleBoxLine> style_popup_separator(memnew(StyleBoxLine));
 			style_popup_separator->set_color(p_config.separator_color);
@@ -1796,7 +1796,7 @@ void EditorThemeManager::_populate_editor_styles(const Ref<EditorTheme> &p_theme
 			tag->set_corner_radius(CORNER_BOTTOM_LEFT, 0);
 			tag->set_corner_radius(CORNER_TOP_RIGHT, 4);
 			tag->set_corner_radius(CORNER_BOTTOM_RIGHT, 4);
-			p_theme->set_stylebox("hover", "ProjectTag", tag);
+			p_theme->set_stylebox(SceneStringName(hover), "ProjectTag", tag);
 
 			tag = p_config.button_style_pressed->duplicate();
 			tag->set_corner_radius(CORNER_TOP_LEFT, 0);
@@ -1883,7 +1883,7 @@ void EditorThemeManager::_populate_editor_styles(const Ref<EditorTheme> &p_theme
 		p_theme->set_stylebox("normal_mirrored", "MainScreenButton", menu_transparent_style);
 		p_theme->set_stylebox(SceneStringName(pressed), "MainScreenButton", menu_transparent_style);
 		p_theme->set_stylebox("pressed_mirrored", "MainScreenButton", menu_transparent_style);
-		p_theme->set_stylebox("hover", "MainScreenButton", main_screen_button_hover);
+		p_theme->set_stylebox(SceneStringName(hover), "MainScreenButton", main_screen_button_hover);
 		p_theme->set_stylebox("hover_mirrored", "MainScreenButton", main_screen_button_hover);
 		p_theme->set_stylebox("hover_pressed", "MainScreenButton", main_screen_button_hover);
 		p_theme->set_stylebox("hover_pressed_mirrored", "MainScreenButton", main_screen_button_hover);
@@ -1891,7 +1891,7 @@ void EditorThemeManager::_populate_editor_styles(const Ref<EditorTheme> &p_theme
 		p_theme->set_type_variation("MainMenuBar", "FlatMenuButton");
 		p_theme->set_stylebox(CoreStringName(normal), "MainMenuBar", menu_transparent_style);
 		p_theme->set_stylebox(SceneStringName(pressed), "MainMenuBar", main_screen_button_hover);
-		p_theme->set_stylebox("hover", "MainMenuBar", main_screen_button_hover);
+		p_theme->set_stylebox(SceneStringName(hover), "MainMenuBar", main_screen_button_hover);
 		p_theme->set_stylebox("hover_pressed", "MainMenuBar", main_screen_button_hover);
 
 		// Run bar.
@@ -1907,7 +1907,7 @@ void EditorThemeManager::_populate_editor_styles(const Ref<EditorTheme> &p_theme
 		p_theme->set_stylebox(CoreStringName(normal), "BottomPanelButton", menu_transparent_style);
 		p_theme->set_stylebox(SceneStringName(pressed), "BottomPanelButton", menu_transparent_style);
 		p_theme->set_stylebox("hover_pressed", "BottomPanelButton", main_screen_button_hover);
-		p_theme->set_stylebox("hover", "BottomPanelButton", main_screen_button_hover);
+		p_theme->set_stylebox(SceneStringName(hover), "BottomPanelButton", main_screen_button_hover);
 		// Don't tint the icon even when in "pressed" state.
 		p_theme->set_color("icon_pressed_color", "BottomPanelButton", Color(1, 1, 1, 1));
 		Color icon_hover_color = p_config.icon_normal_color * (p_config.dark_theme ? 1.15 : 1.0);
@@ -1991,12 +1991,12 @@ void EditorThemeManager::_populate_editor_styles(const Ref<EditorTheme> &p_theme
 			style_flat_button_pressed->set_bg_color(flat_pressed_color);
 
 			p_theme->set_stylebox(CoreStringName(normal), SceneStringName(FlatButton), style_flat_button);
-			p_theme->set_stylebox("hover", SceneStringName(FlatButton), style_flat_button_hover);
+			p_theme->set_stylebox(SceneStringName(hover), SceneStringName(FlatButton), style_flat_button_hover);
 			p_theme->set_stylebox(SceneStringName(pressed), SceneStringName(FlatButton), style_flat_button_pressed);
 			p_theme->set_stylebox("disabled", SceneStringName(FlatButton), style_flat_button);
 
 			p_theme->set_stylebox(CoreStringName(normal), "FlatMenuButton", style_flat_button);
-			p_theme->set_stylebox("hover", "FlatMenuButton", style_flat_button_hover);
+			p_theme->set_stylebox(SceneStringName(hover), "FlatMenuButton", style_flat_button_hover);
 			p_theme->set_stylebox(SceneStringName(pressed), "FlatMenuButton", style_flat_button_pressed);
 			p_theme->set_stylebox("disabled", "FlatMenuButton", style_flat_button);
 
@@ -2022,7 +2022,7 @@ void EditorThemeManager::_populate_editor_styles(const Ref<EditorTheme> &p_theme
 				editor_log_button_pressed->set_bg_color(flat_pressed_color.lightened(0.5));
 			}
 			p_theme->set_stylebox(CoreStringName(normal), "EditorLogFilterButton", style_flat_button);
-			p_theme->set_stylebox("hover", "EditorLogFilterButton", style_flat_button_hover);
+			p_theme->set_stylebox(SceneStringName(hover), "EditorLogFilterButton", style_flat_button_hover);
 			p_theme->set_stylebox(SceneStringName(pressed), "EditorLogFilterButton", editor_log_button_pressed);
 		}
 
@@ -2043,7 +2043,7 @@ void EditorThemeManager::_populate_editor_styles(const Ref<EditorTheme> &p_theme
 			panel_button_style_disabled->set_bg_color(p_config.disabled_bg_color);
 
 			p_theme->set_stylebox(CoreStringName(normal), "PanelBackgroundButton", panel_button_style);
-			p_theme->set_stylebox("hover", "PanelBackgroundButton", panel_button_style_hover);
+			p_theme->set_stylebox(SceneStringName(hover), "PanelBackgroundButton", panel_button_style_hover);
 			p_theme->set_stylebox(SceneStringName(pressed), "PanelBackgroundButton", panel_button_style_pressed);
 			p_theme->set_stylebox("disabled", "PanelBackgroundButton", panel_button_style_disabled);
 		}
@@ -2086,7 +2086,7 @@ void EditorThemeManager::_populate_editor_styles(const Ref<EditorTheme> &p_theme
 
 			style_inspector_action = p_config.button_style_hover->duplicate();
 			style_inspector_action->set_content_margin(SIDE_RIGHT, action_extra_margin);
-			p_theme->set_stylebox("hover", "InspectorActionButton", style_inspector_action);
+			p_theme->set_stylebox(SceneStringName(hover), "InspectorActionButton", style_inspector_action);
 
 			style_inspector_action = p_config.button_style_hover->duplicate();
 			style_inspector_action->set_content_margin(SIDE_LEFT, action_extra_margin);
@@ -2125,7 +2125,7 @@ void EditorThemeManager::_populate_editor_styles(const Ref<EditorTheme> &p_theme
 			p_theme->set_color("icon_hover_color", "PreviewLightButton", dim_light_highlighted_color);
 
 			p_theme->set_stylebox(CoreStringName(normal), "PreviewLightButton", sb_empty_borderless);
-			p_theme->set_stylebox("hover", "PreviewLightButton", sb_empty_borderless);
+			p_theme->set_stylebox(SceneStringName(hover), "PreviewLightButton", sb_empty_borderless);
 			p_theme->set_stylebox("focus", "PreviewLightButton", sb_empty_borderless);
 			p_theme->set_stylebox(SceneStringName(pressed), "PreviewLightButton", sb_empty_borderless);
 		}
@@ -2330,7 +2330,7 @@ void EditorThemeManager::_populate_editor_styles(const Ref<EditorTheme> &p_theme
 		Ref<StyleBoxFlat> style_animation_track_hover = make_flat_stylebox(Color(0.5, 0.5, 0.5, 0.1), 0, 0, 0, 0, p_config.corner_radius);
 
 		p_theme->set_stylebox("odd", "AnimationTrackEdit", style_animation_track_odd);
-		p_theme->set_stylebox("hover", "AnimationTrackEdit", style_animation_track_hover);
+		p_theme->set_stylebox(SceneStringName(hover), "AnimationTrackEdit", style_animation_track_hover);
 		p_theme->set_stylebox("focus", "AnimationTrackEdit", p_config.button_style_focus);
 
 		p_theme->set_color("h_line_color", "AnimationTrackEdit", p_config.font_color * Color(1, 1, 1, 0.2));
diff --git a/scene/gui/color_picker.cpp b/scene/gui/color_picker.cpp
index e04ad55fd2..373411e668 100644
--- a/scene/gui/color_picker.cpp
+++ b/scene/gui/color_picker.cpp
@@ -115,7 +115,7 @@ void ColorPicker::_notification(int p_what) {
 				mode_btns[i]->begin_bulk_theme_override();
 				mode_btns[i]->add_theme_style_override(SceneStringName(pressed), theme_cache.mode_button_pressed);
 				mode_btns[i]->add_theme_style_override(CoreStringName(normal), theme_cache.mode_button_normal);
-				mode_btns[i]->add_theme_style_override(SNAME("hover"), theme_cache.mode_button_hover);
+				mode_btns[i]->add_theme_style_override(SceneStringName(hover), theme_cache.mode_button_hover);
 				mode_btns[i]->end_bulk_theme_override();
 			}
 
diff --git a/scene/scene_string_names.h b/scene/scene_string_names.h
index 47eaaa3c86..9a49cfe946 100644
--- a/scene/scene_string_names.h
+++ b/scene/scene_string_names.h
@@ -142,6 +142,7 @@ public:
 	const StringName pressed = StaticCString::create("pressed");
 	const StringName id_pressed = StaticCString::create("id_pressed");
 	const StringName toggled = StaticCString::create("toggled");
+	const StringName hover = StaticCString::create("hover");
 
 	const StringName panel = StaticCString::create("panel");
 	const StringName item_selected = StaticCString::create("item_selected");
diff --git a/scene/theme/default_theme.cpp b/scene/theme/default_theme.cpp
index b6de733cad..f7ba440faf 100644
--- a/scene/theme/default_theme.cpp
+++ b/scene/theme/default_theme.cpp
@@ -161,7 +161,7 @@ void fill_default_theme(Ref<Theme> &theme, const Ref<Font> &default_font, const
 	focus->set_expand_margin_all(Math::round(2 * scale));
 
 	theme->set_stylebox(CoreStringName(normal), "Button", button_normal);
-	theme->set_stylebox("hover", "Button", button_hover);
+	theme->set_stylebox(SceneStringName(hover), "Button", button_hover);
 	theme->set_stylebox(SceneStringName(pressed), "Button", button_pressed);
 	theme->set_stylebox("disabled", "Button", button_disabled);
 	theme->set_stylebox("focus", "Button", focus);
@@ -192,7 +192,7 @@ void fill_default_theme(Ref<Theme> &theme, const Ref<Font> &default_font, const
 
 	// MenuBar
 	theme->set_stylebox(CoreStringName(normal), "MenuBar", button_normal);
-	theme->set_stylebox("hover", "MenuBar", button_hover);
+	theme->set_stylebox(SceneStringName(hover), "MenuBar", button_hover);
 	theme->set_stylebox(SceneStringName(pressed), "MenuBar", button_pressed);
 	theme->set_stylebox("disabled", "MenuBar", button_disabled);
 
@@ -235,7 +235,7 @@ void fill_default_theme(Ref<Theme> &theme, const Ref<Font> &default_font, const
 	Ref<StyleBox> sb_optbutton_disabled = make_flat_stylebox(style_disabled_color, 2 * default_margin, default_margin, 2 * default_margin, default_margin);
 
 	theme->set_stylebox(CoreStringName(normal), "OptionButton", sb_optbutton_normal);
-	theme->set_stylebox("hover", "OptionButton", sb_optbutton_hover);
+	theme->set_stylebox(SceneStringName(hover), "OptionButton", sb_optbutton_hover);
 	theme->set_stylebox(SceneStringName(pressed), "OptionButton", sb_optbutton_pressed);
 	theme->set_stylebox("disabled", "OptionButton", sb_optbutton_disabled);
 
@@ -271,7 +271,7 @@ void fill_default_theme(Ref<Theme> &theme, const Ref<Font> &default_font, const
 
 	theme->set_stylebox(CoreStringName(normal), "MenuButton", button_normal);
 	theme->set_stylebox(SceneStringName(pressed), "MenuButton", button_pressed);
-	theme->set_stylebox("hover", "MenuButton", button_hover);
+	theme->set_stylebox(SceneStringName(hover), "MenuButton", button_hover);
 	theme->set_stylebox("disabled", "MenuButton", button_disabled);
 	theme->set_stylebox("focus", "MenuButton", focus);
 
@@ -298,7 +298,7 @@ void fill_default_theme(Ref<Theme> &theme, const Ref<Font> &default_font, const
 	theme->set_stylebox(CoreStringName(normal), "CheckBox", cbx_empty);
 	theme->set_stylebox(SceneStringName(pressed), "CheckBox", cbx_empty);
 	theme->set_stylebox("disabled", "CheckBox", cbx_empty);
-	theme->set_stylebox("hover", "CheckBox", cbx_empty);
+	theme->set_stylebox(SceneStringName(hover), "CheckBox", cbx_empty);
 	theme->set_stylebox("hover_pressed", "CheckBox", cbx_empty);
 	theme->set_stylebox("focus", "CheckBox", cbx_focus);
 
@@ -334,7 +334,7 @@ void fill_default_theme(Ref<Theme> &theme, const Ref<Font> &default_font, const
 	theme->set_stylebox(CoreStringName(normal), "CheckButton", cb_empty);
 	theme->set_stylebox(SceneStringName(pressed), "CheckButton", cb_empty);
 	theme->set_stylebox("disabled", "CheckButton", cb_empty);
-	theme->set_stylebox("hover", "CheckButton", cb_empty);
+	theme->set_stylebox(SceneStringName(hover), "CheckButton", cb_empty);
 	theme->set_stylebox("hover_pressed", "CheckButton", cb_empty);
 	theme->set_stylebox("focus", "CheckButton", focus);
 
@@ -376,12 +376,12 @@ void fill_default_theme(Ref<Theme> &theme, const Ref<Font> &default_font, const
 	flat_button_pressed->set_bg_color(style_pressed_color * Color(1, 1, 1, 0.85));
 
 	theme->set_stylebox(CoreStringName(normal), SceneStringName(FlatButton), flat_button_normal);
-	theme->set_stylebox("hover", SceneStringName(FlatButton), flat_button_normal);
+	theme->set_stylebox(SceneStringName(hover), SceneStringName(FlatButton), flat_button_normal);
 	theme->set_stylebox(SceneStringName(pressed), SceneStringName(FlatButton), flat_button_pressed);
 	theme->set_stylebox("disabled", SceneStringName(FlatButton), flat_button_normal);
 
 	theme->set_stylebox(CoreStringName(normal), "FlatMenuButton", flat_button_normal);
-	theme->set_stylebox("hover", "FlatMenuButton", flat_button_normal);
+	theme->set_stylebox(SceneStringName(hover), "FlatMenuButton", flat_button_normal);
 	theme->set_stylebox(SceneStringName(pressed), "FlatMenuButton", flat_button_pressed);
 	theme->set_stylebox("disabled", "FlatMenuButton", flat_button_normal);
 
@@ -733,7 +733,7 @@ void fill_default_theme(Ref<Theme> &theme, const Ref<Font> &default_font, const
 	style_popup_panel->set_border_color(style_popup_border_color);
 
 	theme->set_stylebox(SceneStringName(panel), "PopupMenu", style_popup_panel);
-	theme->set_stylebox("hover", "PopupMenu", make_flat_stylebox(style_popup_hover_color));
+	theme->set_stylebox(SceneStringName(hover), "PopupMenu", make_flat_stylebox(style_popup_hover_color));
 	theme->set_stylebox("separator", "PopupMenu", separator_horizontal);
 	theme->set_stylebox("labeled_separator_left", "PopupMenu", separator_horizontal);
 	theme->set_stylebox("labeled_separator_right", "PopupMenu", separator_horizontal);
@@ -1110,7 +1110,7 @@ void fill_default_theme(Ref<Theme> &theme, const Ref<Font> &default_font, const
 	theme->set_icon("bg", "ColorPickerButton", icons["mini_checkerboard"]);
 	theme->set_stylebox(CoreStringName(normal), "ColorPickerButton", button_normal);
 	theme->set_stylebox(SceneStringName(pressed), "ColorPickerButton", button_pressed);
-	theme->set_stylebox("hover", "ColorPickerButton", button_hover);
+	theme->set_stylebox(SceneStringName(hover), "ColorPickerButton", button_hover);
 	theme->set_stylebox("disabled", "ColorPickerButton", button_disabled);
 	theme->set_stylebox("focus", "ColorPickerButton", focus);
 

From 800076a6c258fcd8fabd64879d9bab667bb4e62f Mon Sep 17 00:00:00 2001
From: bruvzg <7645683+bruvzg@users.noreply.github.com>
Date: Wed, 11 Dec 2024 13:50:36 +0200
Subject: [PATCH 073/124] [Windows] Use `dwarf-4` for clang builds on Windows.

---
 SConstruct | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/SConstruct b/SConstruct
index 931ebfd3cf..f49ef9596c 100644
--- a/SConstruct
+++ b/SConstruct
@@ -737,7 +737,10 @@ if env.msvc:
 else:
     if env["debug_symbols"]:
         if env["platform"] == "windows":
-            env.Append(CCFLAGS=["-gdwarf-5"])
+            if methods.using_clang(env):
+                env.Append(CCFLAGS=["-gdwarf-4"])  # clang dwarf-5 symbols are broken on Windows.
+            else:
+                env.Append(CCFLAGS=["-gdwarf-5"])  # For gcc, only dwarf-5 symbols seem usable by libbacktrace.
         else:
             # Adding dwarf-4 explicitly makes stacktraces work with clang builds,
             # otherwise addr2line doesn't understand them

From d470c2ac6ac529a3e79f4f62d677129b121c085e Mon Sep 17 00:00:00 2001
From: Mikael Hermansson <mikael@hermansson.io>
Date: Wed, 11 Dec 2024 13:57:25 +0100
Subject: [PATCH 074/124] Add Jolt Physics as an alternative 3D physics engine

Co-authored-by: Jorrit Rouwe <jrouwe@gmail.com>
---
 COPYRIGHT.txt                                 |   12 +
 core/os/memory.h                              |    8 +-
 doc/classes/ProjectSettings.xml               |  131 +
 modules/jolt_physics/SCsub                    |  178 ++
 modules/jolt_physics/config.py                |    6 +
 .../joints/jolt_cone_twist_joint_3d.cpp       |  384 +++
 .../joints/jolt_cone_twist_joint_3d.h         |   97 +
 .../joints/jolt_generic_6dof_joint_3d.cpp     |  694 +++++
 .../joints/jolt_generic_6dof_joint_3d.h       |  127 +
 .../joints/jolt_hinge_joint_3d.cpp            |  429 +++
 .../jolt_physics/joints/jolt_hinge_joint_3d.h |  100 +
 modules/jolt_physics/joints/jolt_joint_3d.cpp |  235 ++
 modules/jolt_physics/joints/jolt_joint_3d.h   |  107 +
 .../jolt_physics/joints/jolt_pin_joint_3d.cpp |  169 +
 .../jolt_physics/joints/jolt_pin_joint_3d.h   |   64 +
 .../joints/jolt_slider_joint_3d.cpp           |  542 ++++
 .../joints/jolt_slider_joint_3d.h             |   96 +
 modules/jolt_physics/jolt_globals.cpp         |  121 +
 modules/jolt_physics/jolt_globals.h           |   37 +
 .../jolt_physics/jolt_physics_server_3d.cpp   | 1977 ++++++++++++
 modules/jolt_physics/jolt_physics_server_3d.h |  502 +++
 .../jolt_physics/jolt_project_settings.cpp    |  227 ++
 modules/jolt_physics/jolt_project_settings.h  |   81 +
 .../jolt_physics/misc/jolt_stream_wrappers.h  |   81 +
 .../jolt_physics/misc/jolt_type_conversions.h |  147 +
 modules/jolt_physics/objects/jolt_area_3d.cpp |  665 ++++
 modules/jolt_physics/objects/jolt_area_3d.h   |  226 ++
 modules/jolt_physics/objects/jolt_body_3d.cpp | 1435 +++++++++
 modules/jolt_physics/objects/jolt_body_3d.h   |  304 ++
 .../objects/jolt_group_filter.cpp             |   60 +
 .../jolt_physics/objects/jolt_group_filter.h  |   51 +
 .../jolt_physics/objects/jolt_object_3d.cpp   |  148 +
 modules/jolt_physics/objects/jolt_object_3d.h |  157 +
 .../jolt_physics_direct_body_state_3d.cpp     |  245 ++
 .../jolt_physics_direct_body_state_3d.h       |  116 +
 .../objects/jolt_shaped_object_3d.cpp         |  431 +++
 .../objects/jolt_shaped_object_3d.h           |  123 +
 .../objects/jolt_soft_body_3d.cpp             |  734 +++++
 .../jolt_physics/objects/jolt_soft_body_3d.h  |  174 ++
 modules/jolt_physics/register_types.cpp       |   79 +
 modules/jolt_physics/register_types.h         |   39 +
 .../jolt_physics/shapes/jolt_box_shape_3d.cpp |   83 +
 .../jolt_physics/shapes/jolt_box_shape_3d.h   |   57 +
 .../shapes/jolt_capsule_shape_3d.cpp          |   90 +
 .../shapes/jolt_capsule_shape_3d.h            |   57 +
 .../shapes/jolt_concave_polygon_shape_3d.cpp  |  125 +
 .../shapes/jolt_concave_polygon_shape_3d.h    |   60 +
 .../shapes/jolt_convex_polygon_shape_3d.cpp   |  107 +
 .../shapes/jolt_convex_polygon_shape_3d.h     |   60 +
 .../shapes/jolt_custom_decorated_shape.h      |   95 +
 .../shapes/jolt_custom_double_sided_shape.cpp |  113 +
 .../shapes/jolt_custom_double_sided_shape.h   |   74 +
 .../shapes/jolt_custom_motion_shape.cpp       |   78 +
 .../shapes/jolt_custom_motion_shape.h         |  117 +
 .../shapes/jolt_custom_ray_shape.cpp          |  232 ++
 .../shapes/jolt_custom_ray_shape.h            |  107 +
 .../shapes/jolt_custom_shape_type.h           |   47 +
 .../shapes/jolt_custom_user_data_shape.cpp    |   99 +
 .../shapes/jolt_custom_user_data_shape.h      |   61 +
 .../shapes/jolt_cylinder_shape_3d.cpp         |   98 +
 .../shapes/jolt_cylinder_shape_3d.h           |   58 +
 .../shapes/jolt_height_map_shape_3d.cpp       |  233 ++
 .../shapes/jolt_height_map_shape_3d.h         |   69 +
 .../shapes/jolt_separation_ray_shape_3d.cpp   |   85 +
 .../shapes/jolt_separation_ray_shape_3d.h     |   57 +
 modules/jolt_physics/shapes/jolt_shape_3d.cpp |  278 ++
 modules/jolt_physics/shapes/jolt_shape_3d.h   |  136 +
 .../shapes/jolt_shape_instance_3d.cpp         |  106 +
 .../shapes/jolt_shape_instance_3d.h           |  103 +
 .../shapes/jolt_sphere_shape_3d.cpp           |   71 +
 .../shapes/jolt_sphere_shape_3d.h             |   56 +
 .../shapes/jolt_world_boundary_shape_3d.cpp   |   75 +
 .../shapes/jolt_world_boundary_shape_3d.h     |   56 +
 .../spaces/jolt_body_accessor_3d.cpp          |  196 ++
 .../spaces/jolt_body_accessor_3d.h            |  214 ++
 .../spaces/jolt_broad_phase_layer.h           |   52 +
 .../spaces/jolt_contact_listener_3d.cpp       |  548 ++++
 .../spaces/jolt_contact_listener_3d.h         |  147 +
 .../jolt_physics/spaces/jolt_job_system.cpp   |  201 ++
 modules/jolt_physics/spaces/jolt_job_system.h |  107 +
 modules/jolt_physics/spaces/jolt_layers.cpp   |  223 ++
 modules/jolt_physics/spaces/jolt_layers.h     |   71 +
 .../spaces/jolt_motion_filter_3d.cpp          |  114 +
 .../spaces/jolt_motion_filter_3d.h            |   72 +
 .../jolt_physics_direct_space_state_3d.cpp    |  936 ++++++
 .../jolt_physics_direct_space_state_3d.h      |   87 +
 .../spaces/jolt_query_collectors.h            |  238 ++
 .../spaces/jolt_query_filter_3d.cpp           |   83 +
 .../spaces/jolt_query_filter_3d.h             |   67 +
 modules/jolt_physics/spaces/jolt_space_3d.cpp |  514 +++
 modules/jolt_physics/spaces/jolt_space_3d.h   |  149 +
 .../spaces/jolt_temp_allocator.cpp            |  120 +
 .../jolt_physics/spaces/jolt_temp_allocator.h |   53 +
 pyproject.toml                                |    1 +
 servers/physics_server_2d.cpp                 |    1 +
 servers/physics_server_3d.cpp                 |    1 +
 thirdparty/README.md                          |   12 +
 .../Jolt/AABBTree/AABBTreeBuilder.cpp         |  242 ++
 .../Jolt/AABBTree/AABBTreeBuilder.h           |  118 +
 .../Jolt/AABBTree/AABBTreeToBuffer.h          |  296 ++
 .../NodeCodec/NodeCodecQuadTreeHalfFloat.h    |  313 ++
 .../TriangleCodecIndexed8BitPackSOA4Flags.h   |  549 ++++
 .../jolt_physics/Jolt/ConfigurationString.h   |   94 +
 thirdparty/jolt_physics/Jolt/Core/ARMNeon.h   |   94 +
 thirdparty/jolt_physics/Jolt/Core/Array.h     |  604 ++++
 thirdparty/jolt_physics/Jolt/Core/Atomics.h   |   44 +
 .../jolt_physics/Jolt/Core/BinaryHeap.h       |   96 +
 .../jolt_physics/Jolt/Core/ByteBuffer.h       |   74 +
 thirdparty/jolt_physics/Jolt/Core/Color.cpp   |   38 +
 thirdparty/jolt_physics/Jolt/Core/Color.h     |   84 +
 thirdparty/jolt_physics/Jolt/Core/Core.h      |  593 ++++
 .../jolt_physics/Jolt/Core/FPControlWord.h    |  135 +
 .../jolt_physics/Jolt/Core/FPException.h      |   74 +
 .../jolt_physics/Jolt/Core/FPFlushDenormals.h |   41 +
 thirdparty/jolt_physics/Jolt/Core/Factory.cpp |   92 +
 thirdparty/jolt_physics/Jolt/Core/Factory.h   |   54 +
 .../Jolt/Core/FixedSizeFreeList.h             |  122 +
 .../Jolt/Core/FixedSizeFreeList.inl           |  215 ++
 .../jolt_physics/Jolt/Core/HashCombine.h      |  212 ++
 thirdparty/jolt_physics/Jolt/Core/HashTable.h |  854 +++++
 .../jolt_physics/Jolt/Core/InsertionSort.h    |   58 +
 .../jolt_physics/Jolt/Core/IssueReporting.cpp |   27 +
 .../jolt_physics/Jolt/Core/IssueReporting.h   |   38 +
 thirdparty/jolt_physics/Jolt/Core/JobSystem.h |  311 ++
 .../jolt_physics/Jolt/Core/JobSystem.inl      |   56 +
 .../Jolt/Core/JobSystemSingleThreaded.cpp     |   65 +
 .../Jolt/Core/JobSystemSingleThreaded.h       |   62 +
 .../Jolt/Core/JobSystemThreadPool.cpp         |  364 +++
 .../Jolt/Core/JobSystemThreadPool.h           |  101 +
 .../Jolt/Core/JobSystemWithBarrier.cpp        |  230 ++
 .../Jolt/Core/JobSystemWithBarrier.h          |   85 +
 .../jolt_physics/Jolt/Core/LinearCurve.cpp    |   51 +
 .../jolt_physics/Jolt/Core/LinearCurve.h      |   67 +
 .../jolt_physics/Jolt/Core/LockFreeHashMap.h  |  182 ++
 .../Jolt/Core/LockFreeHashMap.inl             |  351 +++
 thirdparty/jolt_physics/Jolt/Core/Memory.cpp  |   85 +
 thirdparty/jolt_physics/Jolt/Core/Memory.h    |   58 +
 thirdparty/jolt_physics/Jolt/Core/Mutex.h     |  223 ++
 .../jolt_physics/Jolt/Core/MutexArray.h       |   98 +
 .../jolt_physics/Jolt/Core/NonCopyable.h      |   18 +
 .../jolt_physics/Jolt/Core/Profiler.cpp       |  354 +++
 thirdparty/jolt_physics/Jolt/Core/Profiler.h  |  301 ++
 .../jolt_physics/Jolt/Core/Profiler.inl       |   90 +
 thirdparty/jolt_physics/Jolt/Core/QuickSort.h |  137 +
 thirdparty/jolt_physics/Jolt/Core/RTTI.cpp    |  149 +
 thirdparty/jolt_physics/Jolt/Core/RTTI.h      |  436 +++
 thirdparty/jolt_physics/Jolt/Core/Reference.h |  244 ++
 thirdparty/jolt_physics/Jolt/Core/Result.h    |  174 ++
 .../Jolt/Core/STLAlignedAllocator.h           |   72 +
 .../jolt_physics/Jolt/Core/STLAllocator.h     |  127 +
 .../jolt_physics/Jolt/Core/STLTempAllocator.h |   80 +
 thirdparty/jolt_physics/Jolt/Core/ScopeExit.h |   49 +
 .../jolt_physics/Jolt/Core/Semaphore.cpp      |   82 +
 thirdparty/jolt_physics/Jolt/Core/Semaphore.h |   51 +
 .../jolt_physics/Jolt/Core/StaticArray.h      |  329 ++
 thirdparty/jolt_physics/Jolt/Core/StreamIn.h  |  119 +
 thirdparty/jolt_physics/Jolt/Core/StreamOut.h |   97 +
 .../jolt_physics/Jolt/Core/StreamUtils.h      |  168 +
 .../jolt_physics/Jolt/Core/StreamWrapper.h    |   53 +
 .../jolt_physics/Jolt/Core/StridedPtr.h       |   63 +
 .../jolt_physics/Jolt/Core/StringTools.cpp    |  101 +
 .../jolt_physics/Jolt/Core/StringTools.h      |   38 +
 .../jolt_physics/Jolt/Core/TempAllocator.h    |  188 ++
 .../jolt_physics/Jolt/Core/TickCounter.cpp    |   36 +
 .../jolt_physics/Jolt/Core/TickCounter.h      |   49 +
 .../jolt_physics/Jolt/Core/UnorderedMap.h     |   80 +
 .../jolt_physics/Jolt/Core/UnorderedSet.h     |   32 +
 thirdparty/jolt_physics/Jolt/Geometry/AABox.h |  312 ++
 .../jolt_physics/Jolt/Geometry/AABox4.h       |  224 ++
 .../jolt_physics/Jolt/Geometry/ClipPoly.h     |  200 ++
 .../jolt_physics/Jolt/Geometry/ClosestPoint.h |  498 +++
 .../Jolt/Geometry/ConvexHullBuilder.cpp       | 1467 +++++++++
 .../Jolt/Geometry/ConvexHullBuilder.h         |  276 ++
 .../Jolt/Geometry/ConvexHullBuilder2D.cpp     |  335 ++
 .../Jolt/Geometry/ConvexHullBuilder2D.h       |  105 +
 .../Jolt/Geometry/ConvexSupport.h             |  188 ++
 .../Jolt/Geometry/EPAConvexHullBuilder.h      |  845 +++++
 .../Jolt/Geometry/EPAPenetrationDepth.h       |  559 ++++
 .../jolt_physics/Jolt/Geometry/Ellipse.h      |   77 +
 .../Jolt/Geometry/GJKClosestPoint.h           |  946 ++++++
 .../Jolt/Geometry/IndexedTriangle.h           |  116 +
 .../jolt_physics/Jolt/Geometry/Indexify.cpp   |  222 ++
 .../jolt_physics/Jolt/Geometry/Indexify.h     |   19 +
 .../jolt_physics/Jolt/Geometry/MortonCode.h   |   40 +
 .../Jolt/Geometry/OrientedBox.cpp             |  178 ++
 .../jolt_physics/Jolt/Geometry/OrientedBox.h  |   39 +
 thirdparty/jolt_physics/Jolt/Geometry/Plane.h |  101 +
 .../jolt_physics/Jolt/Geometry/RayAABox.h     |  241 ++
 .../jolt_physics/Jolt/Geometry/RayCapsule.h   |   37 +
 .../jolt_physics/Jolt/Geometry/RayCylinder.h  |  101 +
 .../jolt_physics/Jolt/Geometry/RaySphere.h    |   96 +
 .../jolt_physics/Jolt/Geometry/RayTriangle.h  |  158 +
 .../jolt_physics/Jolt/Geometry/Sphere.h       |   72 +
 .../jolt_physics/Jolt/Geometry/Triangle.h     |   34 +
 thirdparty/jolt_physics/Jolt/Jolt.h           |   16 +
 thirdparty/jolt_physics/Jolt/Jolt.natvis      |  116 +
 thirdparty/jolt_physics/Jolt/Math/BVec16.h    |   99 +
 thirdparty/jolt_physics/Jolt/Math/BVec16.inl  |  177 ++
 thirdparty/jolt_physics/Jolt/Math/DMat44.h    |  158 +
 thirdparty/jolt_physics/Jolt/Math/DMat44.inl  |  310 ++
 thirdparty/jolt_physics/Jolt/Math/DVec3.h     |  288 ++
 thirdparty/jolt_physics/Jolt/Math/DVec3.inl   |  936 ++++++
 thirdparty/jolt_physics/Jolt/Math/Double3.h   |   48 +
 thirdparty/jolt_physics/Jolt/Math/DynMatrix.h |   31 +
 .../Jolt/Math/EigenValueSymmetric.h           |  177 ++
 thirdparty/jolt_physics/Jolt/Math/FindRoot.h  |   42 +
 thirdparty/jolt_physics/Jolt/Math/Float2.h    |   36 +
 thirdparty/jolt_physics/Jolt/Math/Float3.h    |   50 +
 thirdparty/jolt_physics/Jolt/Math/Float4.h    |   33 +
 .../Jolt/Math/GaussianElimination.h           |  102 +
 thirdparty/jolt_physics/Jolt/Math/HalfFloat.h |  204 ++
 thirdparty/jolt_physics/Jolt/Math/Mat44.h     |  243 ++
 thirdparty/jolt_physics/Jolt/Math/Mat44.inl   |  952 ++++++
 thirdparty/jolt_physics/Jolt/Math/Math.h      |  205 ++
 thirdparty/jolt_physics/Jolt/Math/MathTypes.h |   32 +
 thirdparty/jolt_physics/Jolt/Math/Matrix.h    |  259 ++
 thirdparty/jolt_physics/Jolt/Math/Quat.h      |  255 ++
 thirdparty/jolt_physics/Jolt/Math/Quat.inl    |  328 ++
 thirdparty/jolt_physics/Jolt/Math/Real.h      |   44 +
 thirdparty/jolt_physics/Jolt/Math/Swizzle.h   |   19 +
 .../jolt_physics/Jolt/Math/Trigonometry.h     |   79 +
 thirdparty/jolt_physics/Jolt/Math/UVec4.h     |  220 ++
 thirdparty/jolt_physics/Jolt/Math/UVec4.inl   |  581 ++++
 thirdparty/jolt_physics/Jolt/Math/Vec3.cpp    |   71 +
 thirdparty/jolt_physics/Jolt/Math/Vec3.h      |  295 ++
 thirdparty/jolt_physics/Jolt/Math/Vec3.inl    |  859 +++++
 thirdparty/jolt_physics/Jolt/Math/Vec4.h      |  283 ++
 thirdparty/jolt_physics/Jolt/Math/Vec4.inl    |  981 ++++++
 thirdparty/jolt_physics/Jolt/Math/Vector.h    |  211 ++
 .../Jolt/ObjectStream/ObjectStream.h          |  333 ++
 .../Jolt/ObjectStream/SerializableAttribute.h |  111 +
 .../ObjectStream/SerializableAttributeEnum.h  |   67 +
 .../ObjectStream/SerializableAttributeTyped.h |   60 +
 .../Jolt/ObjectStream/SerializableObject.cpp  |   15 +
 .../Jolt/ObjectStream/SerializableObject.h    |  164 +
 .../Jolt/ObjectStream/TypeDeclarations.h      |   42 +
 .../Jolt/Physics/Body/AllowedDOFs.h           |   68 +
 .../jolt_physics/Jolt/Physics/Body/Body.cpp   |  423 +++
 .../jolt_physics/Jolt/Physics/Body/Body.h     |  429 +++
 .../jolt_physics/Jolt/Physics/Body/Body.inl   |  197 ++
 .../Jolt/Physics/Body/BodyAccess.h            |   68 +
 .../Physics/Body/BodyActivationListener.h     |   28 +
 .../Physics/Body/BodyCreationSettings.cpp     |  234 ++
 .../Jolt/Physics/Body/BodyCreationSettings.h  |  124 +
 .../Jolt/Physics/Body/BodyFilter.h            |  130 +
 .../jolt_physics/Jolt/Physics/Body/BodyID.h   |  100 +
 .../Jolt/Physics/Body/BodyInterface.cpp       | 1034 +++++++
 .../Jolt/Physics/Body/BodyInterface.h         |  298 ++
 .../jolt_physics/Jolt/Physics/Body/BodyLock.h |  111 +
 .../Jolt/Physics/Body/BodyLockInterface.h     |  134 +
 .../Jolt/Physics/Body/BodyLockMulti.h         |  104 +
 .../Jolt/Physics/Body/BodyManager.cpp         | 1156 +++++++
 .../Jolt/Physics/Body/BodyManager.h           |  377 +++
 .../jolt_physics/Jolt/Physics/Body/BodyPair.h |   36 +
 .../jolt_physics/Jolt/Physics/Body/BodyType.h |   19 +
 .../Jolt/Physics/Body/MassProperties.cpp      |  185 ++
 .../Jolt/Physics/Body/MassProperties.h        |   58 +
 .../Jolt/Physics/Body/MotionProperties.cpp    |   92 +
 .../Jolt/Physics/Body/MotionProperties.h      |  282 ++
 .../Jolt/Physics/Body/MotionProperties.inl    |  178 ++
 .../Jolt/Physics/Body/MotionQuality.h         |   31 +
 .../Jolt/Physics/Body/MotionType.h            |   17 +
 .../Jolt/Physics/Character/Character.cpp      |  323 ++
 .../Jolt/Physics/Character/Character.h        |  147 +
 .../Jolt/Physics/Character/CharacterBase.cpp  |   59 +
 .../Jolt/Physics/Character/CharacterBase.h    |  157 +
 .../Physics/Character/CharacterVirtual.cpp    | 1734 +++++++++++
 .../Jolt/Physics/Character/CharacterVirtual.h |  642 ++++
 .../Jolt/Physics/Collision/AABoxCast.h        |   20 +
 .../Jolt/Physics/Collision/ActiveEdgeMode.h   |   17 +
 .../Jolt/Physics/Collision/ActiveEdges.h      |  114 +
 .../Jolt/Physics/Collision/BackFaceMode.h     |   16 +
 .../Collision/BroadPhase/BroadPhase.cpp       |   16 +
 .../Physics/Collision/BroadPhase/BroadPhase.h |  112 +
 .../BroadPhase/BroadPhaseBruteForce.cpp       |  313 ++
 .../BroadPhase/BroadPhaseBruteForce.h         |   38 +
 .../Collision/BroadPhase/BroadPhaseLayer.h    |  148 +
 .../BroadPhase/BroadPhaseLayerInterfaceMask.h |   92 +
 .../BroadPhaseLayerInterfaceTable.h           |   64 +
 .../BroadPhase/BroadPhaseQuadTree.cpp         |  609 ++++
 .../Collision/BroadPhase/BroadPhaseQuadTree.h |  108 +
 .../Collision/BroadPhase/BroadPhaseQuery.h    |   53 +
 .../ObjectVsBroadPhaseLayerFilterMask.h       |   35 +
 .../ObjectVsBroadPhaseLayerFilterTable.h      |   66 +
 .../Physics/Collision/BroadPhase/QuadTree.cpp | 1692 ++++++++++
 .../Physics/Collision/BroadPhase/QuadTree.h   |  390 +++
 .../Collision/CastConvexVsTriangles.cpp       |  109 +
 .../Physics/Collision/CastConvexVsTriangles.h |   46 +
 .../Jolt/Physics/Collision/CastResult.h       |   37 +
 .../Collision/CastSphereVsTriangles.cpp       |  223 ++
 .../Physics/Collision/CastSphereVsTriangles.h |   49 +
 .../Jolt/Physics/Collision/CollectFacesMode.h |   16 +
 .../Collision/CollideConvexVsTriangles.cpp    |  150 +
 .../Collision/CollideConvexVsTriangles.h      |   56 +
 .../Physics/Collision/CollidePointResult.h    |   25 +
 .../Jolt/Physics/Collision/CollideShape.h     |  105 +
 .../Collision/CollideSoftBodyVertexIterator.h |  110 +
 .../CollideSoftBodyVerticesVsTriangles.h      |   90 +
 .../Collision/CollideSphereVsTriangles.cpp    |  123 +
 .../Collision/CollideSphereVsTriangles.h      |   50 +
 .../Physics/Collision/CollisionCollector.h    |  105 +
 .../Collision/CollisionCollectorImpl.h        |  134 +
 .../Physics/Collision/CollisionDispatch.cpp   |  107 +
 .../Physics/Collision/CollisionDispatch.h     |   97 +
 .../Jolt/Physics/Collision/CollisionGroup.cpp |   33 +
 .../Jolt/Physics/Collision/CollisionGroup.h   |   94 +
 .../Jolt/Physics/Collision/ContactListener.h  |  119 +
 .../Collision/EstimateCollisionResponse.cpp   |  213 ++
 .../Collision/EstimateCollisionResponse.h     |   48 +
 .../Jolt/Physics/Collision/GroupFilter.cpp    |   32 +
 .../Jolt/Physics/Collision/GroupFilter.h      |   41 +
 .../Physics/Collision/GroupFilterTable.cpp    |   38 +
 .../Jolt/Physics/Collision/GroupFilterTable.h |  130 +
 .../Collision/InternalEdgeRemovingCollector.h |  250 ++
 .../Collision/ManifoldBetweenTwoFaces.cpp     |  237 ++
 .../Collision/ManifoldBetweenTwoFaces.h       |   44 +
 .../Physics/Collision/NarrowPhaseQuery.cpp    |  493 +++
 .../Jolt/Physics/Collision/NarrowPhaseQuery.h |   77 +
 .../Physics/Collision/NarrowPhaseStats.cpp    |   62 +
 .../Jolt/Physics/Collision/NarrowPhaseStats.h |  110 +
 .../Jolt/Physics/Collision/ObjectLayer.h      |  111 +
 .../Collision/ObjectLayerPairFilterMask.h     |   52 +
 .../Collision/ObjectLayerPairFilterTable.h    |   78 +
 .../Physics/Collision/PhysicsMaterial.cpp     |   35 +
 .../Jolt/Physics/Collision/PhysicsMaterial.h  |   52 +
 .../Collision/PhysicsMaterialSimple.cpp       |   38 +
 .../Physics/Collision/PhysicsMaterialSimple.h |   37 +
 .../Jolt/Physics/Collision/RayCast.h          |   87 +
 .../Jolt/Physics/Collision/Shape/BoxShape.cpp |  312 ++
 .../Jolt/Physics/Collision/Shape/BoxShape.h   |  115 +
 .../Physics/Collision/Shape/CapsuleShape.cpp  |  438 +++
 .../Physics/Collision/Shape/CapsuleShape.h    |  129 +
 .../Physics/Collision/Shape/CompoundShape.cpp |  428 +++
 .../Physics/Collision/Shape/CompoundShape.h   |  350 +++
 .../Collision/Shape/CompoundShapeVisitors.h   |  460 +++
 .../Collision/Shape/ConvexHullShape.cpp       | 1308 ++++++++
 .../Physics/Collision/Shape/ConvexHullShape.h |  202 ++
 .../Physics/Collision/Shape/ConvexShape.cpp   |  559 ++++
 .../Physics/Collision/Shape/ConvexShape.h     |  150 +
 .../Physics/Collision/Shape/CylinderShape.cpp |  404 +++
 .../Physics/Collision/Shape/CylinderShape.h   |  126 +
 .../Collision/Shape/DecoratedShape.cpp        |   87 +
 .../Physics/Collision/Shape/DecoratedShape.h  |   80 +
 .../Physics/Collision/Shape/EmptyShape.cpp    |   65 +
 .../Jolt/Physics/Collision/Shape/EmptyShape.h |   75 +
 .../Collision/Shape/GetTrianglesContext.h     |  248 ++
 .../Collision/Shape/HeightFieldShape.cpp      | 2714 ++++++++++++++++
 .../Collision/Shape/HeightFieldShape.h        |  380 +++
 .../Physics/Collision/Shape/MeshShape.cpp     | 1266 ++++++++
 .../Jolt/Physics/Collision/Shape/MeshShape.h  |  217 ++
 .../Collision/Shape/MutableCompoundShape.cpp  |  589 ++++
 .../Collision/Shape/MutableCompoundShape.h    |  169 +
 .../Shape/OffsetCenterOfMassShape.cpp         |  217 ++
 .../Collision/Shape/OffsetCenterOfMassShape.h |  140 +
 .../Physics/Collision/Shape/PlaneShape.cpp    |  541 ++++
 .../Jolt/Physics/Collision/Shape/PlaneShape.h |  143 +
 .../PolyhedronSubmergedVolumeCalculator.h     |  319 ++
 .../Shape/RotatedTranslatedShape.cpp          |  333 ++
 .../Collision/Shape/RotatedTranslatedShape.h  |  161 +
 .../Physics/Collision/Shape/ScaleHelpers.h    |   83 +
 .../Physics/Collision/Shape/ScaledShape.cpp   |  238 ++
 .../Physics/Collision/Shape/ScaledShape.h     |  145 +
 .../Jolt/Physics/Collision/Shape/Shape.cpp    |  325 ++
 .../Jolt/Physics/Collision/Shape/Shape.h      |  466 +++
 .../Physics/Collision/Shape/SphereShape.cpp   |  347 +++
 .../Physics/Collision/Shape/SphereShape.h     |  125 +
 .../Collision/Shape/StaticCompoundShape.cpp   |  674 ++++
 .../Collision/Shape/StaticCompoundShape.h     |  139 +
 .../Jolt/Physics/Collision/Shape/SubShapeID.h |  138 +
 .../Physics/Collision/Shape/SubShapeIDPair.h  |   80 +
 .../Collision/Shape/TaperedCapsuleShape.cpp   |  453 +++
 .../Shape/TaperedCapsuleShape.gliffy          |    1 +
 .../Collision/Shape/TaperedCapsuleShape.h     |  135 +
 .../Collision/Shape/TaperedCylinderShape.cpp  |  687 ++++
 .../Collision/Shape/TaperedCylinderShape.h    |  132 +
 .../Physics/Collision/Shape/TriangleShape.cpp |  423 +++
 .../Physics/Collision/Shape/TriangleShape.h   |  143 +
 .../Jolt/Physics/Collision/ShapeCast.h        |  173 ++
 .../Jolt/Physics/Collision/ShapeFilter.h      |   73 +
 .../Jolt/Physics/Collision/SimShapeFilter.h   |   40 +
 .../Physics/Collision/SimShapeFilterWrapper.h |   81 +
 .../Physics/Collision/SortReverseAndStore.h   |   48 +
 .../Physics/Collision/TransformedShape.cpp    |  180 ++
 .../Jolt/Physics/Collision/TransformedShape.h |  194 ++
 .../Constraints/CalculateSolverSteps.h        |   66 +
 .../Physics/Constraints/ConeConstraint.cpp    |  246 ++
 .../Jolt/Physics/Constraints/ConeConstraint.h |  133 +
 .../Jolt/Physics/Constraints/Constraint.cpp   |   73 +
 .../Jolt/Physics/Constraints/Constraint.h     |  238 ++
 .../Physics/Constraints/ConstraintManager.cpp |  289 ++
 .../Physics/Constraints/ConstraintManager.h   |   97 +
 .../ConstraintPart/AngleConstraintPart.h      |  257 ++
 .../ConstraintPart/AxisConstraintPart.h       |  682 ++++
 .../ConstraintPart/DualAxisConstraintPart.h   |  276 ++
 .../ConstraintPart/GearConstraintPart.h       |  195 ++
 .../HingeRotationConstraintPart.h             |  222 ++
 .../IndependentAxisConstraintPart.h           |  246 ++
 .../ConstraintPart/PointConstraintPart.h      |  239 ++
 .../RackAndPinionConstraintPart.h             |  196 ++
 .../RotationEulerConstraintPart.h             |  270 ++
 .../RotationQuatConstraintPart.h              |  246 ++
 .../Constraints/ConstraintPart/SpringPart.h   |  169 +
 .../ConstraintPart/SwingTwistConstraintPart.h |  597 ++++
 .../Constraints/ContactConstraintManager.cpp  | 1783 +++++++++++
 .../Constraints/ContactConstraintManager.h    |  513 +++
 .../Constraints/DistanceConstraint.cpp        |  266 ++
 .../Physics/Constraints/DistanceConstraint.h  |  120 +
 .../Physics/Constraints/FixedConstraint.cpp   |  215 ++
 .../Physics/Constraints/FixedConstraint.h     |   96 +
 .../Physics/Constraints/GearConstraint.cpp    |  188 ++
 .../Jolt/Physics/Constraints/GearConstraint.h |  116 +
 .../Physics/Constraints/HingeConstraint.cpp   |  424 +++
 .../Physics/Constraints/HingeConstraint.h     |  200 ++
 .../Physics/Constraints/MotorSettings.cpp     |   43 +
 .../Jolt/Physics/Constraints/MotorSettings.h  |   66 +
 .../Physics/Constraints/PathConstraint.cpp    |  458 +++
 .../Jolt/Physics/Constraints/PathConstraint.h |  191 ++
 .../Constraints/PathConstraintPath.cpp        |   85 +
 .../Physics/Constraints/PathConstraintPath.h  |   71 +
 .../Constraints/PathConstraintPathHermite.cpp |  308 ++
 .../Constraints/PathConstraintPathHermite.h   |   54 +
 .../Physics/Constraints/PointConstraint.cpp   |  157 +
 .../Physics/Constraints/PointConstraint.h     |   94 +
 .../Physics/Constraints/PulleyConstraint.cpp  |  253 ++
 .../Physics/Constraints/PulleyConstraint.h    |  137 +
 .../Constraints/RackAndPinionConstraint.cpp   |  189 ++
 .../Constraints/RackAndPinionConstraint.h     |  118 +
 .../Physics/Constraints/SixDOFConstraint.cpp  |  900 ++++++
 .../Physics/Constraints/SixDOFConstraint.h    |  289 ++
 .../Physics/Constraints/SliderConstraint.cpp  |  501 +++
 .../Physics/Constraints/SliderConstraint.h    |  198 ++
 .../Physics/Constraints/SpringSettings.cpp    |   35 +
 .../Jolt/Physics/Constraints/SpringSettings.h |   70 +
 .../Constraints/SwingTwistConstraint.cpp      |  524 ++++
 .../Constraints/SwingTwistConstraint.h        |  197 ++
 .../Physics/Constraints/TwoBodyConstraint.cpp |   56 +
 .../Physics/Constraints/TwoBodyConstraint.h   |   65 +
 .../Jolt/Physics/DeterminismLog.cpp           |   17 +
 .../Jolt/Physics/DeterminismLog.h             |  159 +
 .../jolt_physics/Jolt/Physics/EActivation.h   |   16 +
 .../Jolt/Physics/EPhysicsUpdateError.h        |   37 +
 .../Jolt/Physics/IslandBuilder.cpp            |  484 +++
 .../jolt_physics/Jolt/Physics/IslandBuilder.h |  125 +
 .../Jolt/Physics/LargeIslandSplitter.cpp      |  582 ++++
 .../Jolt/Physics/LargeIslandSplitter.h        |  185 ++
 .../jolt_physics/Jolt/Physics/PhysicsLock.h   |  169 +
 .../Jolt/Physics/PhysicsScene.cpp             |  261 ++
 .../jolt_physics/Jolt/Physics/PhysicsScene.h  |  104 +
 .../Jolt/Physics/PhysicsSettings.h            |  119 +
 .../Jolt/Physics/PhysicsStepListener.h        |   37 +
 .../Jolt/Physics/PhysicsSystem.cpp            | 2754 +++++++++++++++++
 .../jolt_physics/Jolt/Physics/PhysicsSystem.h |  338 ++
 .../Jolt/Physics/PhysicsUpdateContext.cpp     |   23 +
 .../Jolt/Physics/PhysicsUpdateContext.h       |  172 +
 .../Jolt/Physics/Ragdoll/Ragdoll.cpp          |  705 +++++
 .../Jolt/Physics/Ragdoll/Ragdoll.h            |  240 ++
 .../SoftBody/SoftBodyContactListener.h        |   55 +
 .../SoftBody/SoftBodyCreationSettings.cpp     |  122 +
 .../SoftBody/SoftBodyCreationSettings.h       |   73 +
 .../Jolt/Physics/SoftBody/SoftBodyManifold.h  |   74 +
 .../SoftBody/SoftBodyMotionProperties.cpp     | 1321 ++++++++
 .../SoftBody/SoftBodyMotionProperties.h       |  297 ++
 .../Jolt/Physics/SoftBody/SoftBodyShape.cpp   |  338 ++
 .../Jolt/Physics/SoftBody/SoftBodyShape.h     |   73 +
 .../SoftBody/SoftBodySharedSettings.cpp       | 1032 ++++++
 .../Physics/SoftBody/SoftBodySharedSettings.h |  335 ++
 .../Physics/SoftBody/SoftBodyUpdateContext.h  |   59 +
 .../Jolt/Physics/SoftBody/SoftBodyVertex.h    |   36 +
 .../jolt_physics/Jolt/Physics/StateRecorder.h |  136 +
 .../Jolt/Physics/StateRecorderImpl.cpp        |   90 +
 .../Jolt/Physics/StateRecorderImpl.h          |   50 +
 .../Physics/Vehicle/MotorcycleController.cpp  |  293 ++
 .../Physics/Vehicle/MotorcycleController.h    |  116 +
 .../Vehicle/TrackedVehicleController.cpp      |  531 ++++
 .../Vehicle/TrackedVehicleController.h        |  166 +
 .../Physics/Vehicle/VehicleAntiRollBar.cpp    |   33 +
 .../Jolt/Physics/Vehicle/VehicleAntiRollBar.h |   31 +
 .../Vehicle/VehicleCollisionTester.cpp        |  376 +++
 .../Physics/Vehicle/VehicleCollisionTester.h  |  146 +
 .../Physics/Vehicle/VehicleConstraint.cpp     |  697 +++++
 .../Jolt/Physics/Vehicle/VehicleConstraint.h  |  246 ++
 .../Physics/Vehicle/VehicleController.cpp     |   17 +
 .../Jolt/Physics/Vehicle/VehicleController.h  |   84 +
 .../Physics/Vehicle/VehicleDifferential.cpp   |   81 +
 .../Physics/Vehicle/VehicleDifferential.h     |   39 +
 .../Jolt/Physics/Vehicle/VehicleEngine.cpp    |  122 +
 .../Jolt/Physics/Vehicle/VehicleEngine.h      |   93 +
 .../Jolt/Physics/Vehicle/VehicleTrack.cpp     |   52 +
 .../Jolt/Physics/Vehicle/VehicleTrack.h       |   56 +
 .../Physics/Vehicle/VehicleTransmission.cpp   |  159 +
 .../Physics/Vehicle/VehicleTransmission.h     |   87 +
 .../Jolt/Physics/Vehicle/Wheel.cpp            |   93 +
 .../jolt_physics/Jolt/Physics/Vehicle/Wheel.h |  148 +
 .../Vehicle/WheeledVehicleController.cpp      |  845 +++++
 .../Vehicle/WheeledVehicleController.h        |  199 ++
 .../jolt_physics/Jolt/RegisterTypes.cpp       |  192 ++
 thirdparty/jolt_physics/Jolt/RegisterTypes.h  |   29 +
 .../Jolt/Renderer/DebugRenderer.cpp           | 1107 +++++++
 .../Jolt/Renderer/DebugRenderer.h             |  383 +++
 .../Jolt/Renderer/DebugRendererPlayback.cpp   |  168 +
 .../Jolt/Renderer/DebugRendererPlayback.h     |   48 +
 .../Jolt/Renderer/DebugRendererRecorder.cpp   |  158 +
 .../Jolt/Renderer/DebugRendererRecorder.h     |  130 +
 .../Jolt/Renderer/DebugRendererSimple.cpp     |   80 +
 .../Jolt/Renderer/DebugRendererSimple.h       |   88 +
 .../Jolt/Skeleton/SkeletalAnimation.cpp       |  110 +
 .../Jolt/Skeleton/SkeletalAnimation.h         |   77 +
 .../jolt_physics/Jolt/Skeleton/Skeleton.cpp   |   82 +
 .../jolt_physics/Jolt/Skeleton/Skeleton.h     |   72 +
 .../Jolt/Skeleton/SkeletonMapper.cpp          |  237 ++
 .../Jolt/Skeleton/SkeletonMapper.h            |  145 +
 .../Jolt/Skeleton/SkeletonPose.cpp            |   87 +
 .../jolt_physics/Jolt/Skeleton/SkeletonPose.h |   82 +
 .../Jolt/TriangleGrouper/TriangleGrouper.h    |   27 +
 .../TriangleGrouperClosestCentroid.cpp        |   95 +
 .../TriangleGrouperClosestCentroid.h          |   21 +
 .../TriangleGrouper/TriangleGrouperMorton.cpp |   49 +
 .../TriangleGrouper/TriangleGrouperMorton.h   |   20 +
 .../TriangleSplitter/TriangleSplitter.cpp     |   67 +
 .../Jolt/TriangleSplitter/TriangleSplitter.h  |   84 +
 .../TriangleSplitterBinning.cpp               |  136 +
 .../TriangleSplitterBinning.h                 |   52 +
 .../TriangleSplitterFixedLeafSize.cpp         |  170 +
 .../TriangleSplitterFixedLeafSize.h           |   55 +
 .../TriangleSplitterLongestAxis.cpp           |   31 +
 .../TriangleSplitterLongestAxis.h             |   28 +
 .../TriangleSplitter/TriangleSplitterMean.cpp |   40 +
 .../TriangleSplitter/TriangleSplitterMean.h   |   28 +
 .../TriangleSplitterMorton.cpp                |   63 +
 .../TriangleSplitter/TriangleSplitterMorton.h |   32 +
 thirdparty/jolt_physics/LICENSE               |    7 +
 531 files changed, 115764 insertions(+), 4 deletions(-)
 create mode 100644 modules/jolt_physics/SCsub
 create mode 100644 modules/jolt_physics/config.py
 create mode 100644 modules/jolt_physics/joints/jolt_cone_twist_joint_3d.cpp
 create mode 100644 modules/jolt_physics/joints/jolt_cone_twist_joint_3d.h
 create mode 100644 modules/jolt_physics/joints/jolt_generic_6dof_joint_3d.cpp
 create mode 100644 modules/jolt_physics/joints/jolt_generic_6dof_joint_3d.h
 create mode 100644 modules/jolt_physics/joints/jolt_hinge_joint_3d.cpp
 create mode 100644 modules/jolt_physics/joints/jolt_hinge_joint_3d.h
 create mode 100644 modules/jolt_physics/joints/jolt_joint_3d.cpp
 create mode 100644 modules/jolt_physics/joints/jolt_joint_3d.h
 create mode 100644 modules/jolt_physics/joints/jolt_pin_joint_3d.cpp
 create mode 100644 modules/jolt_physics/joints/jolt_pin_joint_3d.h
 create mode 100644 modules/jolt_physics/joints/jolt_slider_joint_3d.cpp
 create mode 100644 modules/jolt_physics/joints/jolt_slider_joint_3d.h
 create mode 100644 modules/jolt_physics/jolt_globals.cpp
 create mode 100644 modules/jolt_physics/jolt_globals.h
 create mode 100644 modules/jolt_physics/jolt_physics_server_3d.cpp
 create mode 100644 modules/jolt_physics/jolt_physics_server_3d.h
 create mode 100644 modules/jolt_physics/jolt_project_settings.cpp
 create mode 100644 modules/jolt_physics/jolt_project_settings.h
 create mode 100644 modules/jolt_physics/misc/jolt_stream_wrappers.h
 create mode 100644 modules/jolt_physics/misc/jolt_type_conversions.h
 create mode 100644 modules/jolt_physics/objects/jolt_area_3d.cpp
 create mode 100644 modules/jolt_physics/objects/jolt_area_3d.h
 create mode 100644 modules/jolt_physics/objects/jolt_body_3d.cpp
 create mode 100644 modules/jolt_physics/objects/jolt_body_3d.h
 create mode 100644 modules/jolt_physics/objects/jolt_group_filter.cpp
 create mode 100644 modules/jolt_physics/objects/jolt_group_filter.h
 create mode 100644 modules/jolt_physics/objects/jolt_object_3d.cpp
 create mode 100644 modules/jolt_physics/objects/jolt_object_3d.h
 create mode 100644 modules/jolt_physics/objects/jolt_physics_direct_body_state_3d.cpp
 create mode 100644 modules/jolt_physics/objects/jolt_physics_direct_body_state_3d.h
 create mode 100644 modules/jolt_physics/objects/jolt_shaped_object_3d.cpp
 create mode 100644 modules/jolt_physics/objects/jolt_shaped_object_3d.h
 create mode 100644 modules/jolt_physics/objects/jolt_soft_body_3d.cpp
 create mode 100644 modules/jolt_physics/objects/jolt_soft_body_3d.h
 create mode 100644 modules/jolt_physics/register_types.cpp
 create mode 100644 modules/jolt_physics/register_types.h
 create mode 100644 modules/jolt_physics/shapes/jolt_box_shape_3d.cpp
 create mode 100644 modules/jolt_physics/shapes/jolt_box_shape_3d.h
 create mode 100644 modules/jolt_physics/shapes/jolt_capsule_shape_3d.cpp
 create mode 100644 modules/jolt_physics/shapes/jolt_capsule_shape_3d.h
 create mode 100644 modules/jolt_physics/shapes/jolt_concave_polygon_shape_3d.cpp
 create mode 100644 modules/jolt_physics/shapes/jolt_concave_polygon_shape_3d.h
 create mode 100644 modules/jolt_physics/shapes/jolt_convex_polygon_shape_3d.cpp
 create mode 100644 modules/jolt_physics/shapes/jolt_convex_polygon_shape_3d.h
 create mode 100644 modules/jolt_physics/shapes/jolt_custom_decorated_shape.h
 create mode 100644 modules/jolt_physics/shapes/jolt_custom_double_sided_shape.cpp
 create mode 100644 modules/jolt_physics/shapes/jolt_custom_double_sided_shape.h
 create mode 100644 modules/jolt_physics/shapes/jolt_custom_motion_shape.cpp
 create mode 100644 modules/jolt_physics/shapes/jolt_custom_motion_shape.h
 create mode 100644 modules/jolt_physics/shapes/jolt_custom_ray_shape.cpp
 create mode 100644 modules/jolt_physics/shapes/jolt_custom_ray_shape.h
 create mode 100644 modules/jolt_physics/shapes/jolt_custom_shape_type.h
 create mode 100644 modules/jolt_physics/shapes/jolt_custom_user_data_shape.cpp
 create mode 100644 modules/jolt_physics/shapes/jolt_custom_user_data_shape.h
 create mode 100644 modules/jolt_physics/shapes/jolt_cylinder_shape_3d.cpp
 create mode 100644 modules/jolt_physics/shapes/jolt_cylinder_shape_3d.h
 create mode 100644 modules/jolt_physics/shapes/jolt_height_map_shape_3d.cpp
 create mode 100644 modules/jolt_physics/shapes/jolt_height_map_shape_3d.h
 create mode 100644 modules/jolt_physics/shapes/jolt_separation_ray_shape_3d.cpp
 create mode 100644 modules/jolt_physics/shapes/jolt_separation_ray_shape_3d.h
 create mode 100644 modules/jolt_physics/shapes/jolt_shape_3d.cpp
 create mode 100644 modules/jolt_physics/shapes/jolt_shape_3d.h
 create mode 100644 modules/jolt_physics/shapes/jolt_shape_instance_3d.cpp
 create mode 100644 modules/jolt_physics/shapes/jolt_shape_instance_3d.h
 create mode 100644 modules/jolt_physics/shapes/jolt_sphere_shape_3d.cpp
 create mode 100644 modules/jolt_physics/shapes/jolt_sphere_shape_3d.h
 create mode 100644 modules/jolt_physics/shapes/jolt_world_boundary_shape_3d.cpp
 create mode 100644 modules/jolt_physics/shapes/jolt_world_boundary_shape_3d.h
 create mode 100644 modules/jolt_physics/spaces/jolt_body_accessor_3d.cpp
 create mode 100644 modules/jolt_physics/spaces/jolt_body_accessor_3d.h
 create mode 100644 modules/jolt_physics/spaces/jolt_broad_phase_layer.h
 create mode 100644 modules/jolt_physics/spaces/jolt_contact_listener_3d.cpp
 create mode 100644 modules/jolt_physics/spaces/jolt_contact_listener_3d.h
 create mode 100644 modules/jolt_physics/spaces/jolt_job_system.cpp
 create mode 100644 modules/jolt_physics/spaces/jolt_job_system.h
 create mode 100644 modules/jolt_physics/spaces/jolt_layers.cpp
 create mode 100644 modules/jolt_physics/spaces/jolt_layers.h
 create mode 100644 modules/jolt_physics/spaces/jolt_motion_filter_3d.cpp
 create mode 100644 modules/jolt_physics/spaces/jolt_motion_filter_3d.h
 create mode 100644 modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.cpp
 create mode 100644 modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.h
 create mode 100644 modules/jolt_physics/spaces/jolt_query_collectors.h
 create mode 100644 modules/jolt_physics/spaces/jolt_query_filter_3d.cpp
 create mode 100644 modules/jolt_physics/spaces/jolt_query_filter_3d.h
 create mode 100644 modules/jolt_physics/spaces/jolt_space_3d.cpp
 create mode 100644 modules/jolt_physics/spaces/jolt_space_3d.h
 create mode 100644 modules/jolt_physics/spaces/jolt_temp_allocator.cpp
 create mode 100644 modules/jolt_physics/spaces/jolt_temp_allocator.h
 create mode 100644 thirdparty/jolt_physics/Jolt/AABBTree/AABBTreeBuilder.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/AABBTree/AABBTreeBuilder.h
 create mode 100644 thirdparty/jolt_physics/Jolt/AABBTree/AABBTreeToBuffer.h
 create mode 100644 thirdparty/jolt_physics/Jolt/AABBTree/NodeCodec/NodeCodecQuadTreeHalfFloat.h
 create mode 100644 thirdparty/jolt_physics/Jolt/AABBTree/TriangleCodec/TriangleCodecIndexed8BitPackSOA4Flags.h
 create mode 100644 thirdparty/jolt_physics/Jolt/ConfigurationString.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/ARMNeon.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/Array.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/Atomics.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/BinaryHeap.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/ByteBuffer.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/Color.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/Color.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/Core.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/FPControlWord.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/FPException.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/FPFlushDenormals.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/Factory.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/Factory.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/FixedSizeFreeList.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/FixedSizeFreeList.inl
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/HashCombine.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/HashTable.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/InsertionSort.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/IssueReporting.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/IssueReporting.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/JobSystem.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/JobSystem.inl
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/JobSystemSingleThreaded.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/JobSystemSingleThreaded.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/JobSystemThreadPool.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/JobSystemThreadPool.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/JobSystemWithBarrier.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/JobSystemWithBarrier.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/LinearCurve.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/LinearCurve.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/LockFreeHashMap.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/LockFreeHashMap.inl
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/Memory.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/Memory.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/Mutex.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/MutexArray.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/NonCopyable.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/Profiler.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/Profiler.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/Profiler.inl
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/QuickSort.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/RTTI.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/RTTI.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/Reference.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/Result.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/STLAlignedAllocator.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/STLAllocator.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/STLTempAllocator.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/ScopeExit.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/Semaphore.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/Semaphore.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/StaticArray.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/StreamIn.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/StreamOut.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/StreamUtils.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/StreamWrapper.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/StridedPtr.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/StringTools.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/StringTools.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/TempAllocator.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/TickCounter.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/TickCounter.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/UnorderedMap.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Core/UnorderedSet.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Geometry/AABox.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Geometry/AABox4.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Geometry/ClipPoly.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Geometry/ClosestPoint.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder2D.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder2D.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Geometry/ConvexSupport.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Geometry/EPAConvexHullBuilder.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Geometry/EPAPenetrationDepth.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Geometry/Ellipse.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Geometry/GJKClosestPoint.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Geometry/IndexedTriangle.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Geometry/Indexify.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Geometry/Indexify.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Geometry/MortonCode.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Geometry/OrientedBox.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Geometry/OrientedBox.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Geometry/Plane.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Geometry/RayAABox.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Geometry/RayCapsule.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Geometry/RayCylinder.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Geometry/RaySphere.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Geometry/RayTriangle.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Geometry/Sphere.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Geometry/Triangle.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Jolt.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Jolt.natvis
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/BVec16.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/BVec16.inl
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/DMat44.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/DMat44.inl
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/DVec3.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/DVec3.inl
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/Double3.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/DynMatrix.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/EigenValueSymmetric.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/FindRoot.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/Float2.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/Float3.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/Float4.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/GaussianElimination.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/HalfFloat.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/Mat44.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/Mat44.inl
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/Math.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/MathTypes.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/Matrix.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/Quat.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/Quat.inl
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/Real.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/Swizzle.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/Trigonometry.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/UVec4.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/UVec4.inl
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/Vec3.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/Vec3.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/Vec3.inl
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/Vec4.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/Vec4.inl
 create mode 100644 thirdparty/jolt_physics/Jolt/Math/Vector.h
 create mode 100644 thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStream.h
 create mode 100644 thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttribute.h
 create mode 100644 thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttributeEnum.h
 create mode 100644 thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttributeTyped.h
 create mode 100644 thirdparty/jolt_physics/Jolt/ObjectStream/SerializableObject.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/ObjectStream/SerializableObject.h
 create mode 100644 thirdparty/jolt_physics/Jolt/ObjectStream/TypeDeclarations.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Body/AllowedDOFs.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Body/Body.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Body/Body.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Body/Body.inl
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Body/BodyAccess.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Body/BodyActivationListener.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Body/BodyCreationSettings.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Body/BodyCreationSettings.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Body/BodyFilter.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Body/BodyID.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Body/BodyLock.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Body/BodyLockInterface.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Body/BodyLockMulti.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Body/BodyPair.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Body/BodyType.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Body/MassProperties.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Body/MassProperties.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.inl
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Body/MotionQuality.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Body/MotionType.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Character/Character.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Character/Character.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Character/CharacterBase.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Character/CharacterBase.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/AABoxCast.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/ActiveEdgeMode.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/ActiveEdges.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/BackFaceMode.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhase.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhase.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceMask.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceTable.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterMask.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterTable.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/CastResult.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/CastSphereVsTriangles.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/CastSphereVsTriangles.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/CollectFacesMode.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/CollidePointResult.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/CollideShape.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionCollector.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionCollectorImpl.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionDispatch.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionDispatch.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionGroup.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionGroup.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/ContactListener.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/EstimateCollisionResponse.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/EstimateCollisionResponse.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilter.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilter.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilterTable.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilterTable.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/InternalEdgeRemovingCollector.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseQuery.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseQuery.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseStats.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseStats.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayer.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayerPairFilterMask.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayerPairFilterTable.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterial.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterial.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterialSimple.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterialSimple.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/RayCast.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CapsuleShape.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CapsuleShape.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShape.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShape.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShapeVisitors.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexShape.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexShape.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CylinderShape.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CylinderShape.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/DecoratedShape.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/DecoratedShape.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/EmptyShape.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/EmptyShape.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/GetTrianglesContext.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/HeightFieldShape.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/HeightFieldShape.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MutableCompoundShape.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MutableCompoundShape.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PlaneShape.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PlaneShape.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PolyhedronSubmergedVolumeCalculator.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaleHelpers.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaledShape.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaledShape.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/Shape.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/Shape.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SphereShape.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SphereShape.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/StaticCompoundShape.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/StaticCompoundShape.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SubShapeID.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SubShapeIDPair.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.gliffy
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCylinderShape.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCylinderShape.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TriangleShape.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TriangleShape.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/ShapeCast.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/ShapeFilter.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/SimShapeFilter.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/SimShapeFilterWrapper.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/SortReverseAndStore.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/TransformedShape.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Collision/TransformedShape.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/CalculateSolverSteps.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/ConeConstraint.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/ConeConstraint.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintManager.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintManager.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/DualAxisConstraintPart.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/GearConstraintPart.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/HingeRotationConstraintPart.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/IndependentAxisConstraintPart.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RackAndPinionConstraintPart.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RotationQuatConstraintPart.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/SpringPart.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/SwingTwistConstraintPart.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/DistanceConstraint.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/DistanceConstraint.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/FixedConstraint.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/FixedConstraint.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/GearConstraint.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/GearConstraint.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/MotorSettings.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/MotorSettings.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraint.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraint.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPath.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPath.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPathHermite.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPathHermite.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/PointConstraint.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/PointConstraint.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/PulleyConstraint.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/PulleyConstraint.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/RackAndPinionConstraint.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/RackAndPinionConstraint.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/SixDOFConstraint.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/SixDOFConstraint.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/SliderConstraint.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/SliderConstraint.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/SpringSettings.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/SpringSettings.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/SwingTwistConstraint.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/SwingTwistConstraint.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/TwoBodyConstraint.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Constraints/TwoBodyConstraint.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/DeterminismLog.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/DeterminismLog.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/EActivation.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/EPhysicsUpdateError.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/LargeIslandSplitter.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/LargeIslandSplitter.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/PhysicsLock.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/PhysicsScene.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/PhysicsScene.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/PhysicsSettings.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/PhysicsStepListener.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/PhysicsUpdateContext.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/PhysicsUpdateContext.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyContactListener.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyManifold.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyShape.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyShape.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyUpdateContext.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyVertex.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/StateRecorder.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/StateRecorderImpl.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/StateRecorderImpl.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Vehicle/MotorcycleController.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Vehicle/MotorcycleController.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Vehicle/TrackedVehicleController.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Vehicle/TrackedVehicleController.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleAntiRollBar.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleAntiRollBar.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleCollisionTester.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleCollisionTester.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleController.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleController.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleDifferential.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleDifferential.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleEngine.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleEngine.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTrack.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTrack.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTransmission.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTransmission.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Vehicle/Wheel.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Vehicle/Wheel.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.h
 create mode 100644 thirdparty/jolt_physics/Jolt/RegisterTypes.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/RegisterTypes.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Renderer/DebugRenderer.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Renderer/DebugRenderer.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Renderer/DebugRendererPlayback.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Renderer/DebugRendererPlayback.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Renderer/DebugRendererRecorder.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Renderer/DebugRendererRecorder.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Renderer/DebugRendererSimple.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Renderer/DebugRendererSimple.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Skeleton/SkeletalAnimation.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Skeleton/SkeletalAnimation.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Skeleton/Skeleton.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Skeleton/Skeleton.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Skeleton/SkeletonMapper.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Skeleton/SkeletonMapper.h
 create mode 100644 thirdparty/jolt_physics/Jolt/Skeleton/SkeletonPose.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/Skeleton/SkeletonPose.h
 create mode 100644 thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouper.h
 create mode 100644 thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.h
 create mode 100644 thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperMorton.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperMorton.h
 create mode 100644 thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitter.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitter.h
 create mode 100644 thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterBinning.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterBinning.h
 create mode 100644 thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.h
 create mode 100644 thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterLongestAxis.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterLongestAxis.h
 create mode 100644 thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMean.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMean.h
 create mode 100644 thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMorton.cpp
 create mode 100644 thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMorton.h
 create mode 100644 thirdparty/jolt_physics/LICENSE

diff --git a/COPYRIGHT.txt b/COPYRIGHT.txt
index cff3642cb6..d2d9794b91 100644
--- a/COPYRIGHT.txt
+++ b/COPYRIGHT.txt
@@ -101,6 +101,13 @@ Copyright: 2007, Starbreeze Studios
  2007-2014, Juan Linietsky, Ariel Manzur
 License: Expat and Zlib
 
+Files: ./modules/jolt_physics/spaces/jolt_temp_allocator.cpp
+Comment: Jolt Physics
+Copyright: 2021, Jorrit Rouwe
+ 2014-present, Godot Engine contributors
+ 2007-2014, Juan Linietsky, Ariel Manzur
+License: Expat
+
 Files: ./modules/lightmapper_rd/lm_compute.glsl
 Comment: Joint Non-Local Means (JNLM) denoiser
 Copyright: 2020, Manuel Prandini
@@ -294,6 +301,11 @@ Comment: International Components for Unicode
 Copyright: 2016-2024, Unicode, Inc.
 License: Unicode
 
+Files: ./thirdparty/jolt_physics/
+Comment: Jolt Physics
+Copyright: 2021, Jorrit Rouwe
+License: Expat
+
 Files: ./thirdparty/jpeg-compressor/
 Comment: jpeg-compressor
 Copyright: 2012, Rich Geldreich
diff --git a/core/os/memory.h b/core/os/memory.h
index 033e417cb5..0071c3d514 100644
--- a/core/os/memory.h
+++ b/core/os/memory.h
@@ -122,10 +122,10 @@ _ALWAYS_INLINE_ T *_post_initialize(T *p_obj) {
 	return p_obj;
 }
 
-#define memnew(m_class) _post_initialize(new ("") m_class)
+#define memnew(m_class) _post_initialize(::new ("") m_class)
 
-#define memnew_allocator(m_class, m_allocator) _post_initialize(new (m_allocator::alloc) m_class)
-#define memnew_placement(m_placement, m_class) _post_initialize(new (m_placement) m_class)
+#define memnew_allocator(m_class, m_allocator) _post_initialize(::new (m_allocator::alloc) m_class)
+#define memnew_placement(m_placement, m_class) _post_initialize(::new (m_placement) m_class)
 
 _ALWAYS_INLINE_ bool predelete_handler(void *) {
 	return true;
@@ -189,7 +189,7 @@ T *memnew_arr_template(size_t p_elements) {
 
 		/* call operator new */
 		for (size_t i = 0; i < p_elements; i++) {
-			new (&elems[i]) T;
+			::new (&elems[i]) T;
 		}
 	}
 
diff --git a/doc/classes/ProjectSettings.xml b/doc/classes/ProjectSettings.xml
index 1da578167c..979994b4ac 100644
--- a/doc/classes/ProjectSettings.xml
+++ b/doc/classes/ProjectSettings.xml
@@ -2364,6 +2364,137 @@
 			[b]Note:[/b] This property is only read when the project starts. To change the physics FPS at runtime, set [member Engine.physics_ticks_per_second] instead.
 			[b]Note:[/b] Only [member physics/common/max_physics_steps_per_frame] physics ticks may be simulated per rendered frame at most. If more physics ticks have to be simulated per rendered frame to keep up with rendering, the project will appear to slow down (even if [code]delta[/code] is used consistently in physics calculations). Therefore, it is recommended to also increase [member physics/common/max_physics_steps_per_frame] if increasing [member physics/common/physics_ticks_per_second] significantly above its default value.
 		</member>
+		<member name="physics/jolt_physics_3d/collisions/active_edge_threshold" type="float" setter="" getter="" default="0.872665">
+			The maximum angle, in radians, between two adjacent triangles in a [ConcavePolygonShape3D] or [HeightMapShape3D] for which the edge between those triangles is considered inactive.
+			Collisions against an inactive edge will have its normal overridden to instead be the surface normal of the triangle. This can help alleviate ghost collisions.
+			[b]Note:[/b] Setting this too high can result in objects not depenetrating properly.
+			[b]Note:[/b] This applies to all shape queries, as well as physics bodies within the simulation.
+			[b]Note:[/b] This does not apply when enabling Jolt's enhanced internal edge removal, which supersedes this.
+			[b]Note:[/b] This setting will only be read once during the lifetime of the application.
+		</member>
+		<member name="physics/jolt_physics_3d/collisions/collision_margin_fraction" type="float" setter="" getter="" default="0.08">
+			The amount of collision margin to use for certain convex collision shapes, such as [BoxShape3D], [CylinderShape3D] and [ConvexPolygonShape3D], as a fraction of the shape's shortest axis, with [member Shape3D.margin] as the upper bound. This is mainly used to speed up collision detection with convex shapes.
+			[b]Note:[/b] Collision margins in Jolt do not add any extra size to the shape. Instead the shape is first shrunk by the margin and then expanded by the same amount, resulting in a shape with rounded corners.
+			[b]Note:[/b] Setting this value too close to [code]0.0[/code] may also negatively affect the accuracy of the collision detection with convex shapes.
+			[b]Note:[/b] This setting will only be read once during the lifetime of the application.
+		</member>
+		<member name="physics/jolt_physics_3d/joints/world_node" type="int" setter="" getter="" default="0">
+			Which of the two nodes bound by a joint should represent the world when one of the two is omitted, as either [member Joint3D.node_a] or [member Joint3D.node_b]. This can be thought of as having the omitted node be a [StaticBody3D] at the joint's position. Joint limits are more easily expressed when [member Joint3D.node_a] represents the world.
+			[b]Note:[/b] In Godot Physics, only [member Joint3D.node_b] can represent the world.
+		</member>
+		<member name="physics/jolt_physics_3d/limits/max_angular_velocity" type="float" setter="" getter="" default="47.1239">
+			The maximum angular velocity that a [RigidBody3D] can reach, in radians per second.
+			This is mainly used as a fail-safe, to prevent the simulation from exploding, as fast-moving objects colliding with complex physics structures can otherwise cause them to go out of control. Fast-moving objects can also cause a lot of stress on the collision detection system, which can slow down the simulation considerably.
+		</member>
+		<member name="physics/jolt_physics_3d/limits/max_bodies" type="int" setter="" getter="" default="10240">
+			The maximum number of [PhysicsBody3D] to support at the same time, awake or sleeping. When this limit is exceeded, an error is reported and anything past that point is undefined behavior.
+			[b]Note:[/b] This limit also applies within the editor.
+		</member>
+		<member name="physics/jolt_physics_3d/limits/max_body_pairs" type="int" setter="" getter="" default="65536">
+			The maximum number of body pairs to allow processing of. When this limit is exceeded, a warning is reported and collisions will randomly be ignored while bodies pass through each other.
+		</member>
+		<member name="physics/jolt_physics_3d/limits/max_contact_constraints" type="int" setter="" getter="" default="20480">
+			The maximum number of contact constraints to allow processing of. When this limit is exceeded, a warning is reported and collisions will randomly be ignored while bodies pass through each other.
+		</member>
+		<member name="physics/jolt_physics_3d/limits/max_linear_velocity" type="float" setter="" getter="" default="500.0">
+			The maximum linear velocity that a [RigidBody3D] can reach, in meters per second.
+			This is mainly used as a fail-safe, to prevent the simulation from exploding, as fast-moving objects colliding with complex physics structures can otherwise cause them to go out of control. Fast-moving objects can also cause a lot of stress on the collision detection system, which can slow down the simulation considerably.
+		</member>
+		<member name="physics/jolt_physics_3d/limits/temporary_memory_buffer_size" type="int" setter="" getter="" default="32">
+			The amount of memory to pre-allocate for the stack allocator used within Jolt, in MiB. This allocator is used within the physics step to store things that are only needed during it, like which bodies are in contact, how they form islands and the data needed to solve the contacts.
+		</member>
+		<member name="physics/jolt_physics_3d/limits/world_boundary_shape_size" type="float" setter="" getter="" default="2000.0">
+			The size of [WorldBoundaryShape3D] boundaries, for all three dimensions. The plane is effectively centered within a box of this size, and anything outside of the box will not collide with it. This is necessary as [WorldBoundaryShape3D] is not unbounded when using Jolt, in order to prevent precision issues.
+			[b]Note:[/b] Setting this value too high can make collision detection less accurate.
+			[b]Note:[/b] Collisions against the effective edges of a [WorldBoundaryShape3D] will be inconsistent.
+		</member>
+		<member name="physics/jolt_physics_3d/motion_queries/recovery_amount" type="float" setter="" getter="" default="0.4">
+			Fraction of the total penetration to depenetrate per iteration during motion queries.
+			[b]Note:[/b] This affects methods [method CharacterBody3D.move_and_slide], [method PhysicsBody3D.move_and_collide], [method PhysicsBody3D.test_move] and [method PhysicsServer3D.body_test_motion].
+			[b]Note:[/b] This setting will only be read once during the lifetime of the application.
+		</member>
+		<member name="physics/jolt_physics_3d/motion_queries/recovery_iterations" type="int" setter="" getter="" default="4">
+			The number of iterations to run when depenetrating during motion queries.
+			[b]Note:[/b] This affects methods [method CharacterBody3D.move_and_slide], [method PhysicsBody3D.move_and_collide], [method PhysicsBody3D.test_move] and [method PhysicsServer3D.body_test_motion].
+			[b]Note:[/b] This setting will only be read once during the lifetime of the application.
+		</member>
+		<member name="physics/jolt_physics_3d/motion_queries/use_enhanced_internal_edge_removal" type="bool" setter="" getter="" default="true">
+			If [code]true[/code], enables Jolt's enhanced internal edge removal during motion queries. This can help alleviate ghost collisions, but only with edges within a single body, meaning edges between separate bodies can still cause ghost collisions.
+			[b]Note:[/b] This affects methods [method CharacterBody3D.move_and_slide], [method PhysicsBody3D.move_and_collide], [method PhysicsBody3D.test_move] and [method PhysicsServer3D.body_test_motion].
+			[b]Note:[/b] This setting will only be read once during the lifetime of the application.
+		</member>
+		<member name="physics/jolt_physics_3d/queries/enable_ray_cast_face_index" type="bool" setter="" getter="" default="false">
+			If [code]true[/code], populates the [code]face_index[/code] field in the results of [method PhysicsDirectSpaceState3D.intersect_ray], also accessed through [method RayCast3D.get_collision_face_index]. If [code]false[/code], the [code]face_index[/code] field will be left at its default value of [code]-1[/code].
+			[b]Note:[/b] Enabling this setting will increase Jolt's memory usage for [ConcavePolygonShape3D] by around 25%.
+			[b]Note:[/b] This setting will only be read once during the lifetime of the application.
+		</member>
+		<member name="physics/jolt_physics_3d/queries/use_enhanced_internal_edge_removal" type="bool" setter="" getter="" default="false">
+			If [code]true[/code], enables Jolt's enhanced internal edge removal during shape queries. This can help alleviate ghost collisions when using shape queries for things like character movement, but only with edges within a single body, meaning edges between separate bodies can still cause ghost collisions.
+			[b]Note:[/b] This affects methods [method PhysicsDirectSpaceState3D.cast_motion], [method PhysicsDirectSpaceState3D.collide_shape], [method PhysicsDirectSpaceState3D.get_rest_info] and [method PhysicsDirectSpaceState3D.intersect_shape].
+			[b]Note:[/b] Enabling this setting can cause certain shapes to be culled from the results entirely, but you will get at least one intersection per body.
+			[b]Note:[/b] This setting will only be read once during the lifetime of the application.
+		</member>
+		<member name="physics/jolt_physics_3d/simulation/allow_sleep" type="bool" setter="" getter="" default="true">
+			If [code]true[/code], [RigidBody3D] nodes are allowed to go to sleep if their velocity is below the threshold defined in [member physics/jolt_physics_3d/simulation/sleep_velocity_threshold] for the duration set in [member physics/jolt_physics_3d/simulation/sleep_time_threshold]. This can improve physics simulation performance when there are non-moving [RigidBody3D] nodes, at the cost of some nodes possibly failing to wake up in certain scenarios. Consider disabling this temporarily to troubleshoot [RigidBody3D] nodes not moving when they should.
+		</member>
+		<member name="physics/jolt_physics_3d/simulation/areas_detect_static_bodies" type="bool" setter="" getter="" default="false">
+			If [code]true[/code], [Area3D] nodes are able to detect overlaps with [StaticBody3D] nodes.
+			[b]Note:[/b] Enabling this setting can come at a heavy CPU and memory cost if you allow many/large [Area3D] to overlap with complex static geometry, such as [ConcavePolygonShape3D] or [HeightMapShape3D]. It is strongly recommended that you set up your collision layers and masks in such a way that only a few small [Area3D] nodes can detect [StaticBody3D] nodes.
+			[b]Note:[/b] This also applies to overlaps with a [RigidBody3D] frozen with [constant RigidBody3D.FREEZE_MODE_STATIC].
+			[b]Note:[/b] This is not needed to detect overlaps with [AnimatableBody3D], as it is a kinematic body, despite inheriting from [StaticBody3D].
+		</member>
+		<member name="physics/jolt_physics_3d/simulation/baumgarte_stabilization_factor" type="float" setter="" getter="" default="0.2">
+			How much of the position error of a [RigidBody3D] to fix during a physics step, where [code]0.0[/code] is none and [code]1.0[/code] is the full amount. This affects things like how quickly bodies depenetrate.
+			[b]Note:[/b] Setting this value too high can make [RigidBody3D] nodes unstable.
+		</member>
+		<member name="physics/jolt_physics_3d/simulation/body_pair_contact_cache_angle_threshold" type="float" setter="" getter="" default="0.0349066">
+			The maximum relative angle by which a body pair can move and still reuse the collision results from the previous physics step, in radians.
+		</member>
+		<member name="physics/jolt_physics_3d/simulation/body_pair_contact_cache_distance_threshold" type="float" setter="" getter="" default="0.001">
+			The maximum relative distance by which a body pair can move and still reuse the collision results from the previous physics step, in meters.
+		</member>
+		<member name="physics/jolt_physics_3d/simulation/body_pair_contact_cache_enabled" type="bool" setter="" getter="" default="true">
+			If [code]true[/code], enables the body pair contact cache, which removes the need for potentially expensive collision detection when the relative orientation between two bodies hasn't changed much.
+		</member>
+		<member name="physics/jolt_physics_3d/simulation/bounce_velocity_threshold" type="float" setter="" getter="" default="1.0">
+			The minimum velocity needed before a collision can be bouncy, in meters per second.
+		</member>
+		<member name="physics/jolt_physics_3d/simulation/continuous_cd_max_penetration" type="float" setter="" getter="" default="0.25">
+			Fraction of a body's inner radius that may penetrate another body while using continuous collision detection.
+		</member>
+		<member name="physics/jolt_physics_3d/simulation/continuous_cd_movement_threshold" type="float" setter="" getter="" default="0.75">
+			Fraction of a body's inner radius that the body must move per step to make use of continuous collision detection.
+		</member>
+		<member name="physics/jolt_physics_3d/simulation/generate_all_kinematic_contacts" type="bool" setter="" getter="" default="false">
+			If [code]true[/code], a [RigidBody3D] frozen with [constant RigidBody3D.FREEZE_MODE_KINEMATIC] is able to collide with other kinematic and static bodies, and therefore generate contacts for them.
+			[b]Note:[/b] This setting can come at a heavy CPU and memory cost if you allow many/large frozen kinematic bodies with a non-zero [member RigidBody3D.max_contacts_reported] to overlap with complex static geometry, such as [ConcavePolygonShape3D] or [HeightMapShape3D].
+		</member>
+		<member name="physics/jolt_physics_3d/simulation/penetration_slop" type="float" setter="" getter="" default="0.02">
+			How much bodies are allowed to penetrate each other, in meters.
+		</member>
+		<member name="physics/jolt_physics_3d/simulation/position_steps" type="int" setter="" getter="" default="2">
+			Number of solver position iterations. The greater the number of iterations, the more accurate the simulation will be, at the cost of CPU performance.
+		</member>
+		<member name="physics/jolt_physics_3d/simulation/sleep_time_threshold" type="float" setter="" getter="" default="0.5">
+			Time in seconds a [RigidBody3D] will spend below the sleep velocity threshold before going to sleep.
+		</member>
+		<member name="physics/jolt_physics_3d/simulation/sleep_velocity_threshold" type="float" setter="" getter="" default="0.03">
+			The linear velocity of specific points on the bounding box of a [RigidBody3D], below which it can be put to sleep, in meters per second. These points help capture both the linear and angular motion of a [RigidBody3D].
+		</member>
+		<member name="physics/jolt_physics_3d/simulation/soft_body_point_radius" type="float" setter="" getter="" default="0.01">
+			How big the points of a [SoftBody3D] are, in meters. A higher value can prevent behavior such as cloth laying perfectly flush against other surfaces and causing Z-fighting.
+		</member>
+		<member name="physics/jolt_physics_3d/simulation/speculative_contact_distance" type="float" setter="" getter="" default="0.02">
+			Radius around physics bodies, inside which speculative contact points will be detected, in meters. This is mainly used to prevent tunneling/penetration for [RigidBody3D] nodes during simulation.
+			[b]Note:[/b] Setting this too high may result in ghost collisions, as speculative contacts are based on the closest points during the collision detection step which may not be the actual closest points by the time the two bodies hit.
+		</member>
+		<member name="physics/jolt_physics_3d/simulation/use_enhanced_internal_edge_removal" type="bool" setter="" getter="" default="true">
+			If [code]true[/code], enables Jolt's enhanced internal edge removal for [RigidBody3D]. This can help alleviate ghost collisions when, for example, a [RigidBody3D] collides with the edges of two perfectly joined [BoxShape3D]. The removal only applies to edges internal to a single body, meaning edges between separate bodies can still cause ghost collisions.
+		</member>
+		<member name="physics/jolt_physics_3d/simulation/velocity_steps" type="int" setter="" getter="" default="10">
+			Number of solver velocity iterations. The greater the number of iterations, the more accurate the simulation will be, at the cost of CPU performance.
+			[b]Note:[/b] This needs to be at least [code]2[/code] in order for friction to work, as friction is applied using the non-penetration impulse from the previous iteration.
+		</member>
 		<member name="rendering/2d/batching/item_buffer_size" type="int" setter="" getter="" default="16384">
 			Maximum number of canvas item commands that can be batched into a single draw call.
 		</member>
diff --git a/modules/jolt_physics/SCsub b/modules/jolt_physics/SCsub
new file mode 100644
index 0000000000..d4b02ec41d
--- /dev/null
+++ b/modules/jolt_physics/SCsub
@@ -0,0 +1,178 @@
+#!/usr/bin/env python
+from misc.utility.scons_hints import *
+
+Import("env")
+Import("env_modules")
+
+env_jolt = env_modules.Clone()
+
+# Thirdparty source files
+
+thirdparty_dir = "#thirdparty/jolt_physics/"
+thirdparty_sources = [
+    "Jolt/RegisterTypes.cpp",
+    "Jolt/AABBTree/AABBTreeBuilder.cpp",
+    "Jolt/Core/Color.cpp",
+    "Jolt/Core/Factory.cpp",
+    "Jolt/Core/IssueReporting.cpp",
+    "Jolt/Core/JobSystemSingleThreaded.cpp",
+    "Jolt/Core/JobSystemThreadPool.cpp",
+    "Jolt/Core/JobSystemWithBarrier.cpp",
+    "Jolt/Core/LinearCurve.cpp",
+    "Jolt/Core/Memory.cpp",
+    "Jolt/Core/Profiler.cpp",
+    "Jolt/Core/RTTI.cpp",
+    "Jolt/Core/Semaphore.cpp",
+    "Jolt/Core/StringTools.cpp",
+    "Jolt/Core/TickCounter.cpp",
+    "Jolt/Geometry/ConvexHullBuilder.cpp",
+    "Jolt/Geometry/ConvexHullBuilder2D.cpp",
+    "Jolt/Geometry/Indexify.cpp",
+    "Jolt/Geometry/OrientedBox.cpp",
+    "Jolt/Math/Vec3.cpp",
+    "Jolt/ObjectStream/SerializableObject.cpp",
+    "Jolt/Physics/DeterminismLog.cpp",
+    "Jolt/Physics/IslandBuilder.cpp",
+    "Jolt/Physics/LargeIslandSplitter.cpp",
+    "Jolt/Physics/PhysicsScene.cpp",
+    "Jolt/Physics/PhysicsSystem.cpp",
+    "Jolt/Physics/PhysicsUpdateContext.cpp",
+    "Jolt/Physics/StateRecorderImpl.cpp",
+    "Jolt/Physics/Body/Body.cpp",
+    "Jolt/Physics/Body/BodyCreationSettings.cpp",
+    "Jolt/Physics/Body/BodyInterface.cpp",
+    "Jolt/Physics/Body/BodyManager.cpp",
+    "Jolt/Physics/Body/MassProperties.cpp",
+    "Jolt/Physics/Body/MotionProperties.cpp",
+    "Jolt/Physics/Character/Character.cpp",
+    "Jolt/Physics/Character/CharacterBase.cpp",
+    "Jolt/Physics/Character/CharacterVirtual.cpp",
+    "Jolt/Physics/Collision/CastConvexVsTriangles.cpp",
+    "Jolt/Physics/Collision/CastSphereVsTriangles.cpp",
+    "Jolt/Physics/Collision/CollideConvexVsTriangles.cpp",
+    "Jolt/Physics/Collision/CollideSphereVsTriangles.cpp",
+    "Jolt/Physics/Collision/CollisionDispatch.cpp",
+    "Jolt/Physics/Collision/CollisionGroup.cpp",
+    "Jolt/Physics/Collision/EstimateCollisionResponse.cpp",
+    "Jolt/Physics/Collision/GroupFilter.cpp",
+    "Jolt/Physics/Collision/GroupFilterTable.cpp",
+    "Jolt/Physics/Collision/ManifoldBetweenTwoFaces.cpp",
+    "Jolt/Physics/Collision/NarrowPhaseQuery.cpp",
+    "Jolt/Physics/Collision/NarrowPhaseStats.cpp",
+    "Jolt/Physics/Collision/PhysicsMaterial.cpp",
+    "Jolt/Physics/Collision/PhysicsMaterialSimple.cpp",
+    "Jolt/Physics/Collision/TransformedShape.cpp",
+    "Jolt/Physics/Collision/BroadPhase/BroadPhase.cpp",
+    "Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.cpp",
+    "Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.cpp",
+    "Jolt/Physics/Collision/BroadPhase/QuadTree.cpp",
+    "Jolt/Physics/Collision/Shape/BoxShape.cpp",
+    "Jolt/Physics/Collision/Shape/CapsuleShape.cpp",
+    "Jolt/Physics/Collision/Shape/CompoundShape.cpp",
+    "Jolt/Physics/Collision/Shape/ConvexHullShape.cpp",
+    "Jolt/Physics/Collision/Shape/ConvexShape.cpp",
+    "Jolt/Physics/Collision/Shape/CylinderShape.cpp",
+    "Jolt/Physics/Collision/Shape/DecoratedShape.cpp",
+    "Jolt/Physics/Collision/Shape/EmptyShape.cpp",
+    "Jolt/Physics/Collision/Shape/HeightFieldShape.cpp",
+    "Jolt/Physics/Collision/Shape/MeshShape.cpp",
+    "Jolt/Physics/Collision/Shape/MutableCompoundShape.cpp",
+    "Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.cpp",
+    "Jolt/Physics/Collision/Shape/PlaneShape.cpp",
+    "Jolt/Physics/Collision/Shape/RotatedTranslatedShape.cpp",
+    "Jolt/Physics/Collision/Shape/ScaledShape.cpp",
+    "Jolt/Physics/Collision/Shape/Shape.cpp",
+    "Jolt/Physics/Collision/Shape/SphereShape.cpp",
+    "Jolt/Physics/Collision/Shape/StaticCompoundShape.cpp",
+    "Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp",
+    "Jolt/Physics/Collision/Shape/TaperedCylinderShape.cpp",
+    "Jolt/Physics/Collision/Shape/TriangleShape.cpp",
+    "Jolt/Physics/Constraints/ConeConstraint.cpp",
+    "Jolt/Physics/Constraints/Constraint.cpp",
+    "Jolt/Physics/Constraints/ConstraintManager.cpp",
+    "Jolt/Physics/Constraints/ContactConstraintManager.cpp",
+    "Jolt/Physics/Constraints/DistanceConstraint.cpp",
+    "Jolt/Physics/Constraints/FixedConstraint.cpp",
+    "Jolt/Physics/Constraints/GearConstraint.cpp",
+    "Jolt/Physics/Constraints/HingeConstraint.cpp",
+    "Jolt/Physics/Constraints/MotorSettings.cpp",
+    "Jolt/Physics/Constraints/PathConstraint.cpp",
+    "Jolt/Physics/Constraints/PathConstraintPath.cpp",
+    "Jolt/Physics/Constraints/PathConstraintPathHermite.cpp",
+    "Jolt/Physics/Constraints/PointConstraint.cpp",
+    "Jolt/Physics/Constraints/PulleyConstraint.cpp",
+    "Jolt/Physics/Constraints/RackAndPinionConstraint.cpp",
+    "Jolt/Physics/Constraints/SixDOFConstraint.cpp",
+    "Jolt/Physics/Constraints/SliderConstraint.cpp",
+    "Jolt/Physics/Constraints/SpringSettings.cpp",
+    "Jolt/Physics/Constraints/SwingTwistConstraint.cpp",
+    "Jolt/Physics/Constraints/TwoBodyConstraint.cpp",
+    "Jolt/Physics/Ragdoll/Ragdoll.cpp",
+    "Jolt/Physics/SoftBody/SoftBodyCreationSettings.cpp",
+    "Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp",
+    "Jolt/Physics/SoftBody/SoftBodyShape.cpp",
+    "Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp",
+    "Jolt/Physics/Vehicle/MotorcycleController.cpp",
+    "Jolt/Physics/Vehicle/TrackedVehicleController.cpp",
+    "Jolt/Physics/Vehicle/VehicleAntiRollBar.cpp",
+    "Jolt/Physics/Vehicle/VehicleCollisionTester.cpp",
+    "Jolt/Physics/Vehicle/VehicleConstraint.cpp",
+    "Jolt/Physics/Vehicle/VehicleController.cpp",
+    "Jolt/Physics/Vehicle/VehicleDifferential.cpp",
+    "Jolt/Physics/Vehicle/VehicleEngine.cpp",
+    "Jolt/Physics/Vehicle/VehicleTrack.cpp",
+    "Jolt/Physics/Vehicle/VehicleTransmission.cpp",
+    "Jolt/Physics/Vehicle/Wheel.cpp",
+    "Jolt/Physics/Vehicle/WheeledVehicleController.cpp",
+    "Jolt/Renderer/DebugRenderer.cpp",
+    "Jolt/Renderer/DebugRendererPlayback.cpp",
+    "Jolt/Renderer/DebugRendererRecorder.cpp",
+    "Jolt/Renderer/DebugRendererSimple.cpp",
+    "Jolt/Skeleton/SkeletalAnimation.cpp",
+    "Jolt/Skeleton/Skeleton.cpp",
+    "Jolt/Skeleton/SkeletonMapper.cpp",
+    "Jolt/Skeleton/SkeletonPose.cpp",
+    "Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.cpp",
+    "Jolt/TriangleGrouper/TriangleGrouperMorton.cpp",
+    "Jolt/TriangleSplitter/TriangleSplitter.cpp",
+    "Jolt/TriangleSplitter/TriangleSplitterBinning.cpp",
+    "Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.cpp",
+    "Jolt/TriangleSplitter/TriangleSplitterLongestAxis.cpp",
+    "Jolt/TriangleSplitter/TriangleSplitterMean.cpp",
+    "Jolt/TriangleSplitter/TriangleSplitterMorton.cpp",
+]
+
+thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources]
+
+env_jolt.Prepend(CPPPATH=[thirdparty_dir])
+
+if env.dev_build:
+    env_jolt.Append(CPPDEFINES=["JPH_ENABLE_ASSERTS"])
+
+if env.editor_build:
+    env_jolt.Append(CPPDEFINES=["JPH_DEBUG_RENDERER"])
+
+if env["precision"] == "double":
+    env_jolt.Append(CPPDEFINES=["JPH_DOUBLE_PRECISION"])
+
+env_thirdparty = env_jolt.Clone()
+env_thirdparty.disable_warnings()
+
+thirdparty_obj = []
+env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources)
+env.modules_sources += thirdparty_obj
+
+# Godot source files
+
+module_obj = []
+
+env_jolt.add_source_files(module_obj, "*.cpp")
+env_jolt.add_source_files(module_obj, "joints/*.cpp")
+env_jolt.add_source_files(module_obj, "misc/*.cpp")
+env_jolt.add_source_files(module_obj, "objects/*.cpp")
+env_jolt.add_source_files(module_obj, "shapes/*.cpp")
+env_jolt.add_source_files(module_obj, "spaces/*.cpp")
+env.modules_sources += module_obj
+
+# Needed to force rebuilding the module files when the thirdparty library is updated.
+env.Depends(module_obj, thirdparty_obj)
diff --git a/modules/jolt_physics/config.py b/modules/jolt_physics/config.py
new file mode 100644
index 0000000000..a42f27fbe1
--- /dev/null
+++ b/modules/jolt_physics/config.py
@@ -0,0 +1,6 @@
+def can_build(env, platform):
+    return not env["disable_3d"]
+
+
+def configure(env):
+    pass
diff --git a/modules/jolt_physics/joints/jolt_cone_twist_joint_3d.cpp b/modules/jolt_physics/joints/jolt_cone_twist_joint_3d.cpp
new file mode 100644
index 0000000000..05b241eee2
--- /dev/null
+++ b/modules/jolt_physics/joints/jolt_cone_twist_joint_3d.cpp
@@ -0,0 +1,384 @@
+/**************************************************************************/
+/*  jolt_cone_twist_joint_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_cone_twist_joint_3d.h"
+
+#include "../misc/jolt_type_conversions.h"
+#include "../objects/jolt_body_3d.h"
+#include "../spaces/jolt_space_3d.h"
+
+#include "Jolt/Physics/Constraints/SwingTwistConstraint.h"
+
+namespace {
+
+constexpr double DEFAULT_BIAS = 0.3;
+constexpr double DEFAULT_SOFTNESS = 0.8;
+constexpr double DEFAULT_RELAXATION = 1.0;
+
+} // namespace
+
+JPH::Constraint *JoltConeTwistJoint3D::_build_swing_twist(JPH::Body *p_jolt_body_a, JPH::Body *p_jolt_body_b, const Transform3D &p_shifted_ref_a, const Transform3D &p_shifted_ref_b, float p_swing_limit_span, float p_twist_limit_span) const {
+	JPH::SwingTwistConstraintSettings constraint_settings;
+
+	const bool twist_span_valid = p_twist_limit_span >= 0 && p_twist_limit_span <= JPH::JPH_PI;
+	const bool swing_span_valid = p_swing_limit_span >= 0 && p_swing_limit_span <= JPH::JPH_PI;
+
+	if (twist_limit_enabled && twist_span_valid) {
+		constraint_settings.mTwistMinAngle = -p_twist_limit_span;
+		constraint_settings.mTwistMaxAngle = p_twist_limit_span;
+	} else {
+		constraint_settings.mTwistMinAngle = -JPH::JPH_PI;
+		constraint_settings.mTwistMaxAngle = JPH::JPH_PI;
+	}
+
+	if (swing_limit_enabled && swing_span_valid) {
+		constraint_settings.mNormalHalfConeAngle = p_swing_limit_span;
+		constraint_settings.mPlaneHalfConeAngle = p_swing_limit_span;
+	} else {
+		constraint_settings.mNormalHalfConeAngle = JPH::JPH_PI;
+		constraint_settings.mPlaneHalfConeAngle = JPH::JPH_PI;
+
+		if (!swing_span_valid) {
+			constraint_settings.mTwistMinAngle = -JPH::JPH_PI;
+			constraint_settings.mTwistMaxAngle = JPH::JPH_PI;
+		}
+	}
+
+	constraint_settings.mSpace = JPH::EConstraintSpace::LocalToBodyCOM;
+	constraint_settings.mPosition1 = to_jolt_r(p_shifted_ref_a.origin);
+	constraint_settings.mTwistAxis1 = to_jolt(p_shifted_ref_a.basis.get_column(Vector3::AXIS_X));
+	constraint_settings.mPlaneAxis1 = to_jolt(p_shifted_ref_a.basis.get_column(Vector3::AXIS_Z));
+	constraint_settings.mPosition2 = to_jolt_r(p_shifted_ref_b.origin);
+	constraint_settings.mTwistAxis2 = to_jolt(p_shifted_ref_b.basis.get_column(Vector3::AXIS_X));
+	constraint_settings.mPlaneAxis2 = to_jolt(p_shifted_ref_b.basis.get_column(Vector3::AXIS_Z));
+	constraint_settings.mSwingType = JPH::ESwingType::Pyramid;
+
+	if (p_jolt_body_a == nullptr) {
+		return constraint_settings.Create(JPH::Body::sFixedToWorld, *p_jolt_body_b);
+	} else if (p_jolt_body_b == nullptr) {
+		return constraint_settings.Create(*p_jolt_body_a, JPH::Body::sFixedToWorld);
+	} else {
+		return constraint_settings.Create(*p_jolt_body_a, *p_jolt_body_b);
+	}
+}
+
+void JoltConeTwistJoint3D::_update_swing_motor_state() {
+	if (JPH::SwingTwistConstraint *constraint = static_cast<JPH::SwingTwistConstraint *>(jolt_ref.GetPtr())) {
+		constraint->SetSwingMotorState(swing_motor_enabled ? JPH::EMotorState::Velocity : JPH::EMotorState::Off);
+	}
+}
+
+void JoltConeTwistJoint3D::_update_twist_motor_state() {
+	if (JPH::SwingTwistConstraint *constraint = static_cast<JPH::SwingTwistConstraint *>(jolt_ref.GetPtr())) {
+		constraint->SetTwistMotorState(twist_motor_enabled ? JPH::EMotorState::Velocity : JPH::EMotorState::Off);
+	}
+}
+
+void JoltConeTwistJoint3D::_update_motor_velocity() {
+	if (JPH::SwingTwistConstraint *constraint = static_cast<JPH::SwingTwistConstraint *>(jolt_ref.GetPtr())) {
+		// We flip the direction since Jolt is CCW but Godot is CW.
+		constraint->SetTargetAngularVelocityCS({ (float)-twist_motor_target_speed, (float)-swing_motor_target_speed_y, (float)-swing_motor_target_speed_z });
+	}
+}
+
+void JoltConeTwistJoint3D::_update_swing_motor_limit() {
+	if (JPH::SwingTwistConstraint *constraint = static_cast<JPH::SwingTwistConstraint *>(jolt_ref.GetPtr())) {
+		JPH::MotorSettings &motor_settings = constraint->GetSwingMotorSettings();
+		motor_settings.mMinTorqueLimit = (float)-swing_motor_max_torque;
+		motor_settings.mMaxTorqueLimit = (float)swing_motor_max_torque;
+	}
+}
+
+void JoltConeTwistJoint3D::_update_twist_motor_limit() {
+	if (JPH::SwingTwistConstraint *constraint = static_cast<JPH::SwingTwistConstraint *>(jolt_ref.GetPtr())) {
+		JPH::MotorSettings &motor_settings = constraint->GetTwistMotorSettings();
+		motor_settings.mMinTorqueLimit = (float)-twist_motor_max_torque;
+		motor_settings.mMaxTorqueLimit = (float)twist_motor_max_torque;
+	}
+}
+
+void JoltConeTwistJoint3D::_limits_changed() {
+	rebuild();
+	_wake_up_bodies();
+}
+
+void JoltConeTwistJoint3D::_swing_motor_state_changed() {
+	_update_swing_motor_state();
+	_wake_up_bodies();
+}
+
+void JoltConeTwistJoint3D::_twist_motor_state_changed() {
+	_update_twist_motor_state();
+	_wake_up_bodies();
+}
+
+void JoltConeTwistJoint3D::_motor_velocity_changed() {
+	_update_motor_velocity();
+	_wake_up_bodies();
+}
+
+void JoltConeTwistJoint3D::_swing_motor_limit_changed() {
+	_update_swing_motor_limit();
+	_wake_up_bodies();
+}
+
+void JoltConeTwistJoint3D::_twist_motor_limit_changed() {
+	_update_twist_motor_limit();
+	_wake_up_bodies();
+}
+
+JoltConeTwistJoint3D::JoltConeTwistJoint3D(const JoltJoint3D &p_old_joint, JoltBody3D *p_body_a, JoltBody3D *p_body_b, const Transform3D &p_local_ref_a, const Transform3D &p_local_ref_b) :
+		JoltJoint3D(p_old_joint, p_body_a, p_body_b, p_local_ref_a, p_local_ref_b) {
+	rebuild();
+}
+
+double JoltConeTwistJoint3D::get_param(PhysicsServer3D::ConeTwistJointParam p_param) const {
+	switch (p_param) {
+		case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN: {
+			return swing_limit_span;
+		}
+		case PhysicsServer3D::CONE_TWIST_JOINT_TWIST_SPAN: {
+			return twist_limit_span;
+		}
+		case PhysicsServer3D::CONE_TWIST_JOINT_BIAS: {
+			return DEFAULT_BIAS;
+		}
+		case PhysicsServer3D::CONE_TWIST_JOINT_SOFTNESS: {
+			return DEFAULT_SOFTNESS;
+		}
+		case PhysicsServer3D::CONE_TWIST_JOINT_RELAXATION: {
+			return DEFAULT_RELAXATION;
+		}
+		default: {
+			ERR_FAIL_V_MSG(0.0, vformat("Unhandled cone twist joint parameter: '%d'. This should not happen. Please report this.", p_param));
+		}
+	}
+}
+
+void JoltConeTwistJoint3D::set_param(PhysicsServer3D::ConeTwistJointParam p_param, double p_value) {
+	switch (p_param) {
+		case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN: {
+			swing_limit_span = p_value;
+			_limits_changed();
+		} break;
+		case PhysicsServer3D::CONE_TWIST_JOINT_TWIST_SPAN: {
+			twist_limit_span = p_value;
+			_limits_changed();
+		} break;
+		case PhysicsServer3D::CONE_TWIST_JOINT_BIAS: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_BIAS)) {
+				WARN_PRINT(vformat("Cone twist joint bias is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::CONE_TWIST_JOINT_SOFTNESS: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_SOFTNESS)) {
+				WARN_PRINT(vformat("Cone twist joint softness is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::CONE_TWIST_JOINT_RELAXATION: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_RELAXATION)) {
+				WARN_PRINT(vformat("Cone twist joint relaxation is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		default: {
+			ERR_FAIL_MSG(vformat("Unhandled cone twist joint parameter: '%d'. This should not happen. Please report this.", p_param));
+		} break;
+	}
+}
+
+double JoltConeTwistJoint3D::get_jolt_param(JoltParameter p_param) const {
+	switch (p_param) {
+		case JoltPhysicsServer3D::CONE_TWIST_JOINT_SWING_MOTOR_TARGET_VELOCITY_Y: {
+			return swing_motor_target_speed_y;
+		}
+		case JoltPhysicsServer3D::CONE_TWIST_JOINT_SWING_MOTOR_TARGET_VELOCITY_Z: {
+			return swing_motor_target_speed_z;
+		}
+		case JoltPhysicsServer3D::CONE_TWIST_JOINT_TWIST_MOTOR_TARGET_VELOCITY: {
+			return twist_motor_target_speed;
+		}
+		case JoltPhysicsServer3D::CONE_TWIST_JOINT_SWING_MOTOR_MAX_TORQUE: {
+			return swing_motor_max_torque;
+		}
+		case JoltPhysicsServer3D::CONE_TWIST_JOINT_TWIST_MOTOR_MAX_TORQUE: {
+			return twist_motor_max_torque;
+		}
+		default: {
+			ERR_FAIL_V_MSG(0.0, vformat("Unhandled parameter: '%d'. This should not happen. Please report this.", p_param));
+		}
+	}
+}
+
+void JoltConeTwistJoint3D::set_jolt_param(JoltParameter p_param, double p_value) {
+	switch (p_param) {
+		case JoltPhysicsServer3D::CONE_TWIST_JOINT_SWING_MOTOR_TARGET_VELOCITY_Y: {
+			swing_motor_target_speed_y = p_value;
+			_motor_velocity_changed();
+		} break;
+		case JoltPhysicsServer3D::CONE_TWIST_JOINT_SWING_MOTOR_TARGET_VELOCITY_Z: {
+			swing_motor_target_speed_z = p_value;
+			_motor_velocity_changed();
+		} break;
+		case JoltPhysicsServer3D::CONE_TWIST_JOINT_TWIST_MOTOR_TARGET_VELOCITY: {
+			twist_motor_target_speed = p_value;
+			_motor_velocity_changed();
+		} break;
+		case JoltPhysicsServer3D::CONE_TWIST_JOINT_SWING_MOTOR_MAX_TORQUE: {
+			swing_motor_max_torque = p_value;
+			_swing_motor_limit_changed();
+		} break;
+		case JoltPhysicsServer3D::CONE_TWIST_JOINT_TWIST_MOTOR_MAX_TORQUE: {
+			twist_motor_max_torque = p_value;
+			_twist_motor_limit_changed();
+		} break;
+		default: {
+			ERR_FAIL_MSG(vformat("Unhandled parameter: '%d'. This should not happen. Please report this.", p_param));
+		} break;
+	}
+}
+
+bool JoltConeTwistJoint3D::get_jolt_flag(JoltFlag p_flag) const {
+	switch (p_flag) {
+		case JoltPhysicsServer3D::CONE_TWIST_JOINT_FLAG_USE_SWING_LIMIT: {
+			return swing_limit_enabled;
+		}
+		case JoltPhysicsServer3D::CONE_TWIST_JOINT_FLAG_USE_TWIST_LIMIT: {
+			return twist_limit_enabled;
+		}
+		case JoltPhysicsServer3D::CONE_TWIST_JOINT_FLAG_ENABLE_SWING_MOTOR: {
+			return swing_motor_enabled;
+		}
+		case JoltPhysicsServer3D::CONE_TWIST_JOINT_FLAG_ENABLE_TWIST_MOTOR: {
+			return twist_motor_enabled;
+		}
+		default: {
+			ERR_FAIL_V_MSG(false, vformat("Unhandled flag: '%d'. This should not happen. Please report this.", p_flag));
+		}
+	}
+}
+
+void JoltConeTwistJoint3D::set_jolt_flag(JoltFlag p_flag, bool p_enabled) {
+	switch (p_flag) {
+		case JoltPhysicsServer3D::CONE_TWIST_JOINT_FLAG_USE_SWING_LIMIT: {
+			swing_limit_enabled = p_enabled;
+			_limits_changed();
+		} break;
+		case JoltPhysicsServer3D::CONE_TWIST_JOINT_FLAG_USE_TWIST_LIMIT: {
+			twist_limit_enabled = p_enabled;
+			_limits_changed();
+		} break;
+		case JoltPhysicsServer3D::CONE_TWIST_JOINT_FLAG_ENABLE_SWING_MOTOR: {
+			swing_motor_enabled = p_enabled;
+			_swing_motor_state_changed();
+		} break;
+		case JoltPhysicsServer3D::CONE_TWIST_JOINT_FLAG_ENABLE_TWIST_MOTOR: {
+			twist_motor_enabled = p_enabled;
+			_twist_motor_state_changed();
+		} break;
+		default: {
+			ERR_FAIL_MSG(vformat("Unhandled flag: '%d'. This should not happen. Please report this.", p_flag));
+		} break;
+	}
+}
+
+float JoltConeTwistJoint3D::get_applied_force() const {
+	JPH::SwingTwistConstraint *constraint = static_cast<JPH::SwingTwistConstraint *>(jolt_ref.GetPtr());
+	ERR_FAIL_NULL_V(constraint, 0.0f);
+
+	JoltSpace3D *space = get_space();
+	ERR_FAIL_NULL_V(space, 0.0f);
+
+	const float last_step = space->get_last_step();
+	if (unlikely(last_step == 0.0f)) {
+		return 0.0f;
+	}
+
+	return constraint->GetTotalLambdaPosition().Length() / last_step;
+}
+
+float JoltConeTwistJoint3D::get_applied_torque() const {
+	JPH::SwingTwistConstraint *constraint = static_cast<JPH::SwingTwistConstraint *>(jolt_ref.GetPtr());
+	ERR_FAIL_NULL_V(constraint, 0.0f);
+
+	JoltSpace3D *space = get_space();
+	ERR_FAIL_NULL_V(space, 0.0f);
+
+	const float last_step = space->get_last_step();
+	if (unlikely(last_step == 0.0f)) {
+		return 0.0f;
+	}
+
+	const JPH::Vec3 swing_twist_lambda = JPH::Vec3(constraint->GetTotalLambdaTwist(), constraint->GetTotalLambdaSwingY(), constraint->GetTotalLambdaSwingZ());
+
+	// Note that the motor lambda is in a different space than the swing twist lambda, and since the two forces can cancel each other it is
+	// technically incorrect to just add them. The bodies themselves have moved, so we can't transform one into the space of the other anymore.
+	const float total_lambda = swing_twist_lambda.Length() + constraint->GetTotalLambdaMotor().Length();
+
+	return total_lambda / last_step;
+}
+
+void JoltConeTwistJoint3D::rebuild() {
+	destroy();
+
+	JoltSpace3D *space = get_space();
+
+	if (space == nullptr) {
+		return;
+	}
+
+	const JPH::BodyID body_ids[2] = {
+		body_a != nullptr ? body_a->get_jolt_id() : JPH::BodyID(),
+		body_b != nullptr ? body_b->get_jolt_id() : JPH::BodyID()
+	};
+
+	const JoltWritableBodies3D jolt_bodies = space->write_bodies(body_ids, 2);
+
+	JPH::Body *jolt_body_a = static_cast<JPH::Body *>(jolt_bodies[0]);
+	JPH::Body *jolt_body_b = static_cast<JPH::Body *>(jolt_bodies[1]);
+
+	ERR_FAIL_COND(jolt_body_a == nullptr && jolt_body_b == nullptr);
+
+	Transform3D shifted_ref_a;
+	Transform3D shifted_ref_b;
+
+	_shift_reference_frames(Vector3(), Vector3(), shifted_ref_a, shifted_ref_b);
+
+	jolt_ref = _build_swing_twist(jolt_body_a, jolt_body_b, shifted_ref_a, shifted_ref_b, (float)swing_limit_span, (float)twist_limit_span);
+
+	space->add_joint(this);
+
+	_update_enabled();
+	_update_iterations();
+	_update_swing_motor_state();
+	_update_twist_motor_state();
+	_update_motor_velocity();
+	_update_swing_motor_limit();
+	_update_twist_motor_limit();
+}
diff --git a/modules/jolt_physics/joints/jolt_cone_twist_joint_3d.h b/modules/jolt_physics/joints/jolt_cone_twist_joint_3d.h
new file mode 100644
index 0000000000..171e8457b0
--- /dev/null
+++ b/modules/jolt_physics/joints/jolt_cone_twist_joint_3d.h
@@ -0,0 +1,97 @@
+/**************************************************************************/
+/*  jolt_cone_twist_joint_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_CONE_TWIST_JOINT_3D_H
+#define JOLT_CONE_TWIST_JOINT_3D_H
+
+#include "../jolt_physics_server_3d.h"
+#include "jolt_joint_3d.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Body/Body.h"
+
+class JoltConeTwistJoint3D final : public JoltJoint3D {
+	typedef PhysicsServer3D::ConeTwistJointParam Parameter;
+	typedef JoltPhysicsServer3D::ConeTwistJointParamJolt JoltParameter;
+	typedef JoltPhysicsServer3D::ConeTwistJointFlagJolt JoltFlag;
+
+	double swing_limit_span = 0.0;
+	double twist_limit_span = 0.0;
+
+	double swing_motor_target_speed_y = 0.0;
+	double swing_motor_target_speed_z = 0.0;
+	double twist_motor_target_speed = 0.0;
+
+	double swing_motor_max_torque = FLT_MAX;
+	double twist_motor_max_torque = FLT_MAX;
+
+	bool swing_limit_enabled = true;
+	bool twist_limit_enabled = true;
+
+	bool swing_motor_enabled = false;
+	bool twist_motor_enabled = false;
+
+	JPH::Constraint *_build_swing_twist(JPH::Body *p_jolt_body_a, JPH::Body *p_jolt_body_b, const Transform3D &p_shifted_ref_a, const Transform3D &p_shifted_ref_b, float p_swing_limit_span, float p_twist_limit_span) const;
+
+	void _update_swing_motor_state();
+	void _update_twist_motor_state();
+	void _update_motor_velocity();
+	void _update_swing_motor_limit();
+	void _update_twist_motor_limit();
+
+	void _limits_changed();
+	void _swing_motor_state_changed();
+	void _twist_motor_state_changed();
+	void _motor_velocity_changed();
+	void _swing_motor_limit_changed();
+	void _twist_motor_limit_changed();
+
+public:
+	JoltConeTwistJoint3D(const JoltJoint3D &p_old_joint, JoltBody3D *p_body_a, JoltBody3D *p_body_b, const Transform3D &p_local_ref_a, const Transform3D &p_local_ref_b);
+
+	virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_CONE_TWIST; }
+
+	double get_param(PhysicsServer3D::ConeTwistJointParam p_param) const;
+	void set_param(PhysicsServer3D::ConeTwistJointParam p_param, double p_value);
+
+	double get_jolt_param(JoltParameter p_param) const;
+	void set_jolt_param(JoltParameter p_param, double p_value);
+
+	bool get_jolt_flag(JoltFlag p_flag) const;
+	void set_jolt_flag(JoltFlag p_flag, bool p_enabled);
+
+	float get_applied_force() const;
+	float get_applied_torque() const;
+
+	virtual void rebuild() override;
+};
+
+#endif // JOLT_CONE_TWIST_JOINT_3D_H
diff --git a/modules/jolt_physics/joints/jolt_generic_6dof_joint_3d.cpp b/modules/jolt_physics/joints/jolt_generic_6dof_joint_3d.cpp
new file mode 100644
index 0000000000..200491ac49
--- /dev/null
+++ b/modules/jolt_physics/joints/jolt_generic_6dof_joint_3d.cpp
@@ -0,0 +1,694 @@
+/**************************************************************************/
+/*  jolt_generic_6dof_joint_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_generic_6dof_joint_3d.h"
+
+#include "../misc/jolt_type_conversions.h"
+#include "../objects/jolt_body_3d.h"
+#include "../spaces/jolt_space_3d.h"
+
+#include "Jolt/Physics/Constraints/SixDOFConstraint.h"
+
+namespace {
+
+constexpr double DEFAULT_LINEAR_LIMIT_SOFTNESS = 0.7;
+constexpr double DEFAULT_LINEAR_RESTITUTION = 0.5;
+constexpr double DEFAULT_LINEAR_DAMPING = 1.0;
+
+constexpr double DEFAULT_ANGULAR_LIMIT_SOFTNESS = 0.5;
+constexpr double DEFAULT_ANGULAR_DAMPING = 1.0;
+constexpr double DEFAULT_ANGULAR_RESTITUTION = 0.0;
+constexpr double DEFAULT_ANGULAR_FORCE_LIMIT = 0.0;
+constexpr double DEFAULT_ANGULAR_ERP = 0.5;
+
+} // namespace
+
+JPH::Constraint *JoltGeneric6DOFJoint3D::_build_6dof(JPH::Body *p_jolt_body_a, JPH::Body *p_jolt_body_b, const Transform3D &p_shifted_ref_a, const Transform3D &p_shifted_ref_b) const {
+	JPH::SixDOFConstraintSettings constraint_settings;
+
+	for (int axis = 0; axis < AXIS_COUNT; ++axis) {
+		double lower = limit_lower[axis];
+		double upper = limit_upper[axis];
+
+		if (axis >= AXIS_ANGULAR_X && axis <= AXIS_ANGULAR_Z) {
+			const double temp = lower;
+			lower = -upper;
+			upper = -temp;
+		}
+
+		if (!limit_enabled[axis] || lower > upper) {
+			constraint_settings.MakeFreeAxis((JoltAxis)axis);
+		} else {
+			constraint_settings.SetLimitedAxis((JoltAxis)axis, (float)lower, (float)upper);
+		}
+	}
+
+	constraint_settings.mSpace = JPH::EConstraintSpace::LocalToBodyCOM;
+	constraint_settings.mPosition1 = to_jolt_r(p_shifted_ref_a.origin);
+	constraint_settings.mAxisX1 = to_jolt(p_shifted_ref_a.basis.get_column(Vector3::AXIS_X));
+	constraint_settings.mAxisY1 = to_jolt(p_shifted_ref_a.basis.get_column(Vector3::AXIS_Y));
+	constraint_settings.mPosition2 = to_jolt_r(p_shifted_ref_b.origin);
+	constraint_settings.mAxisX2 = to_jolt(p_shifted_ref_b.basis.get_column(Vector3::AXIS_X));
+	constraint_settings.mAxisY2 = to_jolt(p_shifted_ref_b.basis.get_column(Vector3::AXIS_Y));
+	constraint_settings.mSwingType = JPH::ESwingType::Pyramid;
+
+	if (p_jolt_body_a == nullptr) {
+		return constraint_settings.Create(JPH::Body::sFixedToWorld, *p_jolt_body_b);
+	} else if (p_jolt_body_b == nullptr) {
+		return constraint_settings.Create(*p_jolt_body_a, JPH::Body::sFixedToWorld);
+	} else {
+		return constraint_settings.Create(*p_jolt_body_a, *p_jolt_body_b);
+	}
+}
+
+void JoltGeneric6DOFJoint3D::_update_limit_spring_parameters(int p_axis) {
+	JPH::SixDOFConstraint *constraint = static_cast<JPH::SixDOFConstraint *>(jolt_ref.GetPtr());
+	if (unlikely(constraint == nullptr)) {
+		return;
+	}
+
+	JPH::SpringSettings settings = constraint->GetLimitsSpringSettings((JoltAxis)p_axis);
+
+	settings.mMode = JPH::ESpringMode::FrequencyAndDamping;
+
+	if (limit_spring_enabled[p_axis]) {
+		settings.mFrequency = (float)limit_spring_frequency[p_axis];
+		settings.mDamping = (float)limit_spring_damping[p_axis];
+	} else {
+		settings.mFrequency = 0.0f;
+		settings.mDamping = 0.0f;
+	}
+
+	constraint->SetLimitsSpringSettings((JoltAxis)p_axis, settings);
+}
+
+void JoltGeneric6DOFJoint3D::_update_motor_state(int p_axis) {
+	if (JPH::SixDOFConstraint *constraint = static_cast<JPH::SixDOFConstraint *>(jolt_ref.GetPtr())) {
+		if (motor_enabled[p_axis]) {
+			constraint->SetMotorState((JoltAxis)p_axis, JPH::EMotorState::Velocity);
+		} else if (spring_enabled[p_axis]) {
+			constraint->SetMotorState((JoltAxis)p_axis, JPH::EMotorState::Position);
+		} else {
+			constraint->SetMotorState((JoltAxis)p_axis, JPH::EMotorState::Off);
+		}
+	}
+}
+
+void JoltGeneric6DOFJoint3D::_update_motor_velocity(int p_axis) {
+	JPH::SixDOFConstraint *constraint = static_cast<JPH::SixDOFConstraint *>(jolt_ref.GetPtr());
+	if (unlikely(constraint == nullptr)) {
+		return;
+	}
+
+	if (p_axis >= AXIS_LINEAR_X && p_axis <= AXIS_LINEAR_Z) {
+		constraint->SetTargetVelocityCS(JPH::Vec3(
+				(float)motor_speed[AXIS_LINEAR_X],
+				(float)motor_speed[AXIS_LINEAR_Y],
+				(float)motor_speed[AXIS_LINEAR_Z]));
+	} else {
+		// We flip the direction since Jolt is CCW but Godot is CW.
+		constraint->SetTargetAngularVelocityCS(JPH::Vec3(
+				(float)-motor_speed[AXIS_ANGULAR_X],
+				(float)-motor_speed[AXIS_ANGULAR_Y],
+				(float)-motor_speed[AXIS_ANGULAR_Z]));
+	}
+}
+
+void JoltGeneric6DOFJoint3D::_update_motor_limit(int p_axis) {
+	JPH::SixDOFConstraint *constraint = static_cast<JPH::SixDOFConstraint *>(jolt_ref.GetPtr());
+	if (unlikely(constraint == nullptr)) {
+		return;
+	}
+
+	JPH::MotorSettings &motor_settings = constraint->GetMotorSettings((JoltAxis)p_axis);
+
+	float limit = FLT_MAX;
+
+	if (motor_enabled[p_axis]) {
+		limit = (float)motor_limit[p_axis];
+	} else if (spring_enabled[p_axis]) {
+		limit = (float)spring_limit[p_axis];
+	}
+
+	if (p_axis >= AXIS_LINEAR_X && p_axis <= AXIS_LINEAR_Z) {
+		motor_settings.SetForceLimit(limit);
+	} else {
+		motor_settings.SetTorqueLimit(limit);
+	}
+}
+
+void JoltGeneric6DOFJoint3D::_update_spring_parameters(int p_axis) {
+	JPH::SixDOFConstraint *constraint = static_cast<JPH::SixDOFConstraint *>(jolt_ref.GetPtr());
+	if (unlikely(constraint == nullptr)) {
+		return;
+	}
+
+	JPH::MotorSettings &motor_settings = constraint->GetMotorSettings((JoltAxis)p_axis);
+
+	if (spring_use_frequency[p_axis]) {
+		motor_settings.mSpringSettings.mMode = JPH::ESpringMode::FrequencyAndDamping;
+		motor_settings.mSpringSettings.mFrequency = (float)spring_frequency[p_axis];
+	} else {
+		motor_settings.mSpringSettings.mMode = JPH::ESpringMode::StiffnessAndDamping;
+		motor_settings.mSpringSettings.mStiffness = (float)spring_stiffness[p_axis];
+	}
+
+	motor_settings.mSpringSettings.mDamping = (float)spring_damping[p_axis];
+}
+
+void JoltGeneric6DOFJoint3D::_update_spring_equilibrium(int p_axis) {
+	JPH::SixDOFConstraint *constraint = static_cast<JPH::SixDOFConstraint *>(jolt_ref.GetPtr());
+	if (unlikely(constraint == nullptr)) {
+		return;
+	}
+
+	if (p_axis >= AXIS_LINEAR_X && p_axis <= AXIS_LINEAR_Z) {
+		const Vector3 target_position = Vector3(
+				(float)spring_equilibrium[AXIS_LINEAR_X],
+				(float)spring_equilibrium[AXIS_LINEAR_Y],
+				(float)spring_equilibrium[AXIS_LINEAR_Z]);
+
+		constraint->SetTargetPositionCS(to_jolt(target_position));
+	} else {
+		// We flip the direction since Jolt is CCW but Godot is CW.
+		const Basis target_orientation = Basis::from_euler(
+				Vector3((float)-spring_equilibrium[AXIS_ANGULAR_X],
+						(float)-spring_equilibrium[AXIS_ANGULAR_Y],
+						(float)-spring_equilibrium[AXIS_ANGULAR_Z]),
+				EulerOrder::ZYX);
+
+		constraint->SetTargetOrientationCS(to_jolt(target_orientation));
+	}
+}
+
+void JoltGeneric6DOFJoint3D::_limits_changed() {
+	rebuild();
+	_wake_up_bodies();
+}
+
+void JoltGeneric6DOFJoint3D::_limit_spring_parameters_changed(int p_axis) {
+	_update_limit_spring_parameters(p_axis);
+	_wake_up_bodies();
+}
+
+void JoltGeneric6DOFJoint3D::_motor_state_changed(int p_axis) {
+	_update_motor_state(p_axis);
+	_update_motor_limit(p_axis);
+	_wake_up_bodies();
+}
+
+void JoltGeneric6DOFJoint3D::_motor_speed_changed(int p_axis) {
+	_update_motor_velocity(p_axis);
+	_wake_up_bodies();
+}
+
+void JoltGeneric6DOFJoint3D::_motor_limit_changed(int p_axis) {
+	_update_motor_limit(p_axis);
+	_wake_up_bodies();
+}
+
+void JoltGeneric6DOFJoint3D::_spring_state_changed(int p_axis) {
+	_update_motor_state(p_axis);
+	_wake_up_bodies();
+}
+
+void JoltGeneric6DOFJoint3D::_spring_parameters_changed(int p_axis) {
+	_update_spring_parameters(p_axis);
+	_wake_up_bodies();
+}
+
+void JoltGeneric6DOFJoint3D::_spring_equilibrium_changed(int p_axis) {
+	_update_spring_equilibrium(p_axis);
+	_wake_up_bodies();
+}
+
+void JoltGeneric6DOFJoint3D::_spring_limit_changed(int p_axis) {
+	_update_motor_limit(p_axis);
+	_wake_up_bodies();
+}
+
+JoltGeneric6DOFJoint3D::JoltGeneric6DOFJoint3D(const JoltJoint3D &p_old_joint, JoltBody3D *p_body_a, JoltBody3D *p_body_b, const Transform3D &p_local_ref_a, const Transform3D &p_local_ref_b) :
+		JoltJoint3D(p_old_joint, p_body_a, p_body_b, p_local_ref_a, p_local_ref_b) {
+	rebuild();
+}
+
+double JoltGeneric6DOFJoint3D::get_param(Axis p_axis, Param p_param) const {
+	const int axis_lin = AXES_LINEAR + (int)p_axis;
+	const int axis_ang = AXES_ANGULAR + (int)p_axis;
+
+	switch ((int)p_param) {
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT: {
+			return limit_lower[axis_lin];
+		}
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_UPPER_LIMIT: {
+			return limit_upper[axis_lin];
+		}
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: {
+			return DEFAULT_LINEAR_LIMIT_SOFTNESS;
+		}
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_RESTITUTION: {
+			return DEFAULT_LINEAR_RESTITUTION;
+		}
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_DAMPING: {
+			return DEFAULT_LINEAR_DAMPING;
+		}
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY: {
+			return motor_speed[axis_lin];
+		}
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT: {
+			return motor_limit[axis_lin];
+		}
+		case JoltPhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS: {
+			return spring_stiffness[axis_lin];
+		}
+		case JoltPhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_DAMPING: {
+			return spring_damping[axis_lin];
+		}
+		case JoltPhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT: {
+			return spring_equilibrium[axis_lin];
+		}
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: {
+			return limit_lower[axis_ang];
+		}
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: {
+			return limit_upper[axis_ang];
+		}
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: {
+			return DEFAULT_ANGULAR_LIMIT_SOFTNESS;
+		}
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_DAMPING: {
+			return DEFAULT_ANGULAR_DAMPING;
+		}
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_RESTITUTION: {
+			return DEFAULT_ANGULAR_RESTITUTION;
+		}
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: {
+			return DEFAULT_ANGULAR_FORCE_LIMIT;
+		}
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_ERP: {
+			return DEFAULT_ANGULAR_ERP;
+		}
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: {
+			return motor_speed[axis_ang];
+		}
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: {
+			return motor_limit[axis_ang];
+		}
+		case JoltPhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS: {
+			return spring_stiffness[axis_ang];
+		}
+		case JoltPhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_DAMPING: {
+			return spring_damping[axis_ang];
+		}
+		case JoltPhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT: {
+			return spring_equilibrium[axis_ang];
+		}
+		default: {
+			ERR_FAIL_V_MSG(0.0, vformat("Unhandled parameter: '%d'. This should not happen. Please report this.", p_param));
+		}
+	}
+}
+
+void JoltGeneric6DOFJoint3D::set_param(Axis p_axis, Param p_param, double p_value) {
+	const int axis_lin = AXES_LINEAR + (int)p_axis;
+	const int axis_ang = AXES_ANGULAR + (int)p_axis;
+
+	switch ((int)p_param) {
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT: {
+			limit_lower[axis_lin] = p_value;
+			_limits_changed();
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_UPPER_LIMIT: {
+			limit_upper[axis_lin] = p_value;
+			_limits_changed();
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_LINEAR_LIMIT_SOFTNESS)) {
+				WARN_PRINT(vformat("6DOF joint linear limit softness is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_RESTITUTION: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_LINEAR_RESTITUTION)) {
+				WARN_PRINT(vformat("6DOF joint linear restitution is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_DAMPING: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_LINEAR_DAMPING)) {
+				WARN_PRINT(vformat("6DOF joint linear damping is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_TARGET_VELOCITY: {
+			motor_speed[axis_lin] = p_value;
+			_motor_speed_changed(axis_lin);
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_LINEAR_MOTOR_FORCE_LIMIT: {
+			motor_limit[axis_lin] = p_value;
+			_motor_limit_changed(axis_lin);
+		} break;
+		case JoltPhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_STIFFNESS: {
+			spring_stiffness[axis_lin] = p_value;
+			_spring_parameters_changed(axis_lin);
+		} break;
+		case JoltPhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_DAMPING: {
+			spring_damping[axis_lin] = p_value;
+			_spring_parameters_changed(axis_lin);
+		} break;
+		case JoltPhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_EQUILIBRIUM_POINT: {
+			spring_equilibrium[axis_lin] = p_value;
+			_spring_equilibrium_changed(axis_lin);
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: {
+			limit_lower[axis_ang] = p_value;
+			_limits_changed();
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: {
+			limit_upper[axis_ang] = p_value;
+			_limits_changed();
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_LIMIT_SOFTNESS)) {
+				WARN_PRINT(vformat("6DOF joint angular limit softness is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_DAMPING: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_DAMPING)) {
+				WARN_PRINT(vformat("6DOF joint angular damping is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_RESTITUTION: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_RESTITUTION)) {
+				WARN_PRINT(vformat("6DOF joint angular restitution is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_FORCE_LIMIT)) {
+				WARN_PRINT(vformat("6DOF joint angular force limit is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_ERP: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_ERP)) {
+				WARN_PRINT(vformat("6DOF joint angular ERP is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: {
+			motor_speed[axis_ang] = p_value;
+			_motor_speed_changed(axis_ang);
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: {
+			motor_limit[axis_ang] = p_value;
+			_motor_limit_changed(axis_ang);
+		} break;
+		case JoltPhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_STIFFNESS: {
+			spring_stiffness[axis_ang] = p_value;
+			_spring_parameters_changed(axis_ang);
+		} break;
+		case JoltPhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_DAMPING: {
+			spring_damping[axis_ang] = p_value;
+			_spring_parameters_changed(axis_ang);
+		} break;
+		case JoltPhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_EQUILIBRIUM_POINT: {
+			spring_equilibrium[axis_ang] = p_value;
+			_spring_equilibrium_changed(axis_ang);
+		} break;
+		default: {
+			ERR_FAIL_MSG(vformat("Unhandled parameter: '%d'. This should not happen. Please report this.", p_param));
+		} break;
+	}
+}
+
+bool JoltGeneric6DOFJoint3D::get_flag(Axis p_axis, Flag p_flag) const {
+	const int axis_lin = AXES_LINEAR + (int)p_axis;
+	const int axis_ang = AXES_ANGULAR + (int)p_axis;
+
+	switch ((int)p_flag) {
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: {
+			return limit_enabled[axis_lin];
+		}
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: {
+			return limit_enabled[axis_ang];
+		}
+		case JoltPhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING: {
+			return spring_enabled[axis_ang];
+		}
+		case JoltPhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING: {
+			return spring_enabled[axis_lin];
+		}
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR: {
+			return motor_enabled[axis_ang];
+		}
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR: {
+			return motor_enabled[axis_lin];
+		}
+		default: {
+			ERR_FAIL_V_MSG(false, vformat("Unhandled flag: '%d'. This should not happen. Please report this.", p_flag));
+		}
+	}
+}
+
+void JoltGeneric6DOFJoint3D::set_flag(Axis p_axis, Flag p_flag, bool p_enabled) {
+	const int axis_lin = AXES_LINEAR + (int)p_axis;
+	const int axis_ang = AXES_ANGULAR + (int)p_axis;
+
+	switch ((int)p_flag) {
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: {
+			limit_enabled[axis_lin] = p_enabled;
+			_limits_changed();
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: {
+			limit_enabled[axis_ang] = p_enabled;
+			_limits_changed();
+		} break;
+		case JoltPhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING: {
+			spring_enabled[axis_ang] = p_enabled;
+			_spring_state_changed(axis_ang);
+		} break;
+		case JoltPhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING: {
+			spring_enabled[axis_lin] = p_enabled;
+			_spring_state_changed(axis_lin);
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_MOTOR: {
+			motor_enabled[axis_ang] = p_enabled;
+			_motor_state_changed(axis_ang);
+		} break;
+		case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_MOTOR: {
+			motor_enabled[axis_lin] = p_enabled;
+			_motor_state_changed(axis_lin);
+		} break;
+		default: {
+			ERR_FAIL_MSG(vformat("Unhandled flag: '%d'. This should not happen. Please report this.", p_flag));
+		} break;
+	}
+}
+
+double JoltGeneric6DOFJoint3D::get_jolt_param(Axis p_axis, JoltParam p_param) const {
+	const int axis_lin = AXES_LINEAR + (int)p_axis;
+	const int axis_ang = AXES_ANGULAR + (int)p_axis;
+
+	switch ((int)p_param) {
+		case JoltPhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_FREQUENCY: {
+			return spring_frequency[axis_lin];
+		}
+		case JoltPhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_MAX_FORCE: {
+			return spring_limit[axis_lin];
+		}
+		case JoltPhysicsServer3D::G6DOF_JOINT_LINEAR_LIMIT_SPRING_FREQUENCY: {
+			return limit_spring_frequency[axis_lin];
+		}
+		case JoltPhysicsServer3D::G6DOF_JOINT_LINEAR_LIMIT_SPRING_DAMPING: {
+			return limit_spring_damping[axis_lin];
+		}
+		case JoltPhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_FREQUENCY: {
+			return spring_frequency[axis_ang];
+		}
+		case JoltPhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_MAX_TORQUE: {
+			return spring_limit[axis_ang];
+		}
+		default: {
+			ERR_FAIL_V_MSG(0.0, vformat("Unhandled parameter: '%d'. This should not happen. Please report this.", p_param));
+		}
+	}
+}
+
+void JoltGeneric6DOFJoint3D::set_jolt_param(Axis p_axis, JoltParam p_param, double p_value) {
+	const int axis_lin = AXES_LINEAR + (int)p_axis;
+	const int axis_ang = AXES_ANGULAR + (int)p_axis;
+
+	switch ((int)p_param) {
+		case JoltPhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_FREQUENCY: {
+			spring_frequency[axis_lin] = p_value;
+			_spring_parameters_changed(axis_lin);
+		} break;
+		case JoltPhysicsServer3D::G6DOF_JOINT_LINEAR_SPRING_MAX_FORCE: {
+			spring_limit[axis_lin] = p_value;
+			_spring_limit_changed(axis_lin);
+		} break;
+		case JoltPhysicsServer3D::G6DOF_JOINT_LINEAR_LIMIT_SPRING_FREQUENCY: {
+			limit_spring_frequency[axis_lin] = p_value;
+			_limit_spring_parameters_changed(axis_lin);
+		} break;
+		case JoltPhysicsServer3D::G6DOF_JOINT_LINEAR_LIMIT_SPRING_DAMPING: {
+			limit_spring_damping[axis_lin] = p_value;
+			_limit_spring_parameters_changed(axis_lin);
+		} break;
+		case JoltPhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_FREQUENCY: {
+			spring_frequency[axis_ang] = p_value;
+			_spring_parameters_changed(axis_ang);
+		} break;
+		case JoltPhysicsServer3D::G6DOF_JOINT_ANGULAR_SPRING_MAX_TORQUE: {
+			spring_limit[axis_ang] = p_value;
+			_spring_limit_changed(axis_ang);
+		} break;
+		default: {
+			ERR_FAIL_MSG(vformat("Unhandled parameter: '%d'. This should not happen. Please report this.", p_param));
+		} break;
+	}
+}
+
+bool JoltGeneric6DOFJoint3D::get_jolt_flag(Axis p_axis, JoltFlag p_flag) const {
+	const int axis_lin = AXES_LINEAR + (int)p_axis;
+	const int axis_ang = AXES_ANGULAR + (int)p_axis;
+
+	switch ((int)p_flag) {
+		case JoltPhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT_SPRING: {
+			return limit_spring_enabled[axis_lin];
+		}
+		case JoltPhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING_FREQUENCY: {
+			return spring_use_frequency[axis_lin];
+		}
+		case JoltPhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING_FREQUENCY: {
+			return spring_use_frequency[axis_ang];
+		}
+		default: {
+			ERR_FAIL_V_MSG(false, vformat("Unhandled flag: '%d'. This should not happen. Please report this.", p_flag));
+		}
+	}
+}
+
+void JoltGeneric6DOFJoint3D::set_jolt_flag(Axis p_axis, JoltFlag p_flag, bool p_enabled) {
+	const int axis_lin = AXES_LINEAR + (int)p_axis;
+	const int axis_ang = AXES_ANGULAR + (int)p_axis;
+
+	switch ((int)p_flag) {
+		case JoltPhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT_SPRING: {
+			limit_spring_enabled[axis_lin] = p_enabled;
+			_limit_spring_parameters_changed(axis_lin);
+		} break;
+		case JoltPhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING_FREQUENCY: {
+			spring_use_frequency[axis_lin] = p_enabled;
+			_spring_parameters_changed(axis_lin);
+		} break;
+		case JoltPhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING_FREQUENCY: {
+			spring_use_frequency[axis_ang] = p_enabled;
+			_spring_parameters_changed(axis_ang);
+		} break;
+		default: {
+			ERR_FAIL_MSG(vformat("Unhandled flag: '%d'. This should not happen. Please report this.", p_flag));
+		} break;
+	}
+}
+
+float JoltGeneric6DOFJoint3D::get_applied_force() const {
+	JPH::SixDOFConstraint *constraint = static_cast<JPH::SixDOFConstraint *>(jolt_ref.GetPtr());
+	ERR_FAIL_NULL_V(constraint, 0.0f);
+
+	JoltSpace3D *space = get_space();
+	ERR_FAIL_NULL_V(space, 0.0f);
+
+	const float last_step = space->get_last_step();
+	if (unlikely(last_step == 0.0f)) {
+		return 0.0f;
+	}
+
+	const JPH::Vec3 total_lambda = constraint->GetTotalLambdaPosition() + constraint->GetTotalLambdaMotorTranslation();
+
+	return total_lambda.Length() / last_step;
+}
+
+float JoltGeneric6DOFJoint3D::get_applied_torque() const {
+	JPH::SixDOFConstraint *constraint = static_cast<JPH::SixDOFConstraint *>(jolt_ref.GetPtr());
+	ERR_FAIL_NULL_V(constraint, 0.0f);
+
+	JoltSpace3D *space = get_space();
+	ERR_FAIL_NULL_V(space, 0.0f);
+
+	const float last_step = space->get_last_step();
+	if (unlikely(last_step == 0.0f)) {
+		return 0.0f;
+	}
+
+	const JPH::Vec3 total_lambda = constraint->GetTotalLambdaRotation() + constraint->GetTotalLambdaMotorRotation();
+
+	return total_lambda.Length() / last_step;
+}
+
+void JoltGeneric6DOFJoint3D::rebuild() {
+	destroy();
+
+	JoltSpace3D *space = get_space();
+	if (space == nullptr) {
+		return;
+	}
+
+	const JPH::BodyID body_ids[2] = {
+		body_a != nullptr ? body_a->get_jolt_id() : JPH::BodyID(),
+		body_b != nullptr ? body_b->get_jolt_id() : JPH::BodyID()
+	};
+
+	const JoltWritableBodies3D jolt_bodies = space->write_bodies(body_ids, 2);
+
+	JPH::Body *jolt_body_a = static_cast<JPH::Body *>(jolt_bodies[0]);
+	JPH::Body *jolt_body_b = static_cast<JPH::Body *>(jolt_bodies[1]);
+
+	ERR_FAIL_COND(jolt_body_a == nullptr && jolt_body_b == nullptr);
+
+	Transform3D shifted_ref_a;
+	Transform3D shifted_ref_b;
+
+	_shift_reference_frames(Vector3(), Vector3(), shifted_ref_a, shifted_ref_b);
+
+	jolt_ref = _build_6dof(jolt_body_a, jolt_body_b, shifted_ref_a, shifted_ref_b);
+
+	space->add_joint(this);
+
+	_update_enabled();
+	_update_iterations();
+
+	_update_limit_spring_parameters(AXIS_LINEAR_X);
+	_update_limit_spring_parameters(AXIS_LINEAR_Y);
+	_update_limit_spring_parameters(AXIS_LINEAR_Z);
+
+	for (int axis = 0; axis < AXIS_COUNT; ++axis) {
+		_update_motor_state(axis);
+		_update_motor_velocity(axis);
+		_update_motor_limit(axis);
+		_update_spring_parameters(axis);
+		_update_spring_equilibrium(axis);
+	}
+}
diff --git a/modules/jolt_physics/joints/jolt_generic_6dof_joint_3d.h b/modules/jolt_physics/joints/jolt_generic_6dof_joint_3d.h
new file mode 100644
index 0000000000..9d72048978
--- /dev/null
+++ b/modules/jolt_physics/joints/jolt_generic_6dof_joint_3d.h
@@ -0,0 +1,127 @@
+/**************************************************************************/
+/*  jolt_generic_6dof_joint_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_GENERIC_6DOF_JOINT_3D_H
+#define JOLT_GENERIC_6DOF_JOINT_3D_H
+
+#include "../jolt_physics_server_3d.h"
+#include "jolt_joint_3d.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Constraints/SixDOFConstraint.h"
+
+class JoltGeneric6DOFJoint3D final : public JoltJoint3D {
+	typedef Vector3::Axis Axis;
+	typedef JPH::SixDOFConstraintSettings::EAxis JoltAxis;
+	typedef PhysicsServer3D::G6DOFJointAxisParam Param;
+	typedef JoltPhysicsServer3D::G6DOFJointAxisParamJolt JoltParam;
+	typedef PhysicsServer3D::G6DOFJointAxisFlag Flag;
+	typedef JoltPhysicsServer3D::G6DOFJointAxisFlagJolt JoltFlag;
+
+	enum {
+		AXIS_LINEAR_X = JoltAxis::TranslationX,
+		AXIS_LINEAR_Y = JoltAxis::TranslationY,
+		AXIS_LINEAR_Z = JoltAxis::TranslationZ,
+		AXIS_ANGULAR_X = JoltAxis::RotationX,
+		AXIS_ANGULAR_Y = JoltAxis::RotationY,
+		AXIS_ANGULAR_Z = JoltAxis::RotationZ,
+		AXIS_COUNT = JoltAxis::Num,
+		AXES_LINEAR = AXIS_LINEAR_X,
+		AXES_ANGULAR = AXIS_ANGULAR_X,
+	};
+
+	double limit_lower[AXIS_COUNT] = {};
+	double limit_upper[AXIS_COUNT] = {};
+
+	double limit_spring_frequency[AXIS_COUNT] = {};
+	double limit_spring_damping[AXIS_COUNT] = {};
+
+	double motor_speed[AXIS_COUNT] = {};
+	double motor_limit[AXIS_COUNT] = { FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX };
+
+	double spring_stiffness[AXIS_COUNT] = {};
+	double spring_frequency[AXIS_COUNT] = {};
+	double spring_damping[AXIS_COUNT] = {};
+	double spring_equilibrium[AXIS_COUNT] = {};
+	double spring_limit[AXIS_COUNT] = { FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX };
+
+	bool limit_enabled[AXIS_COUNT] = {};
+
+	bool limit_spring_enabled[AXIS_COUNT] = {};
+
+	bool motor_enabled[AXIS_COUNT] = {};
+
+	bool spring_enabled[AXIS_COUNT] = {};
+	bool spring_use_frequency[AXIS_COUNT] = {};
+
+	JPH::Constraint *_build_6dof(JPH::Body *p_jolt_body_a, JPH::Body *p_jolt_body_b, const Transform3D &p_shifted_ref_a, const Transform3D &p_shifted_ref_b) const;
+
+	void _update_limit_spring_parameters(int p_axis);
+	void _update_motor_state(int p_axis);
+	void _update_motor_velocity(int p_axis);
+	void _update_motor_limit(int p_axis);
+	void _update_spring_parameters(int p_axis);
+	void _update_spring_equilibrium(int p_axis);
+
+	void _limits_changed();
+	void _limit_spring_parameters_changed(int p_axis);
+	void _motor_state_changed(int p_axis);
+	void _motor_speed_changed(int p_axis);
+	void _motor_limit_changed(int p_axis);
+	void _spring_state_changed(int p_axis);
+	void _spring_parameters_changed(int p_axis);
+	void _spring_equilibrium_changed(int p_axis);
+	void _spring_limit_changed(int p_axis);
+
+public:
+	JoltGeneric6DOFJoint3D(const JoltJoint3D &p_old_joint, JoltBody3D *p_body_a, JoltBody3D *p_body_b, const Transform3D &p_local_ref_a, const Transform3D &p_local_ref_b);
+
+	virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_6DOF; }
+
+	double get_param(Axis p_axis, Param p_param) const;
+	void set_param(Axis p_axis, Param p_param, double p_value);
+
+	bool get_flag(Axis p_axis, Flag p_flag) const;
+	void set_flag(Axis p_axis, Flag p_flag, bool p_enabled);
+
+	double get_jolt_param(Axis p_axis, JoltParam p_param) const;
+	void set_jolt_param(Axis p_axis, JoltParam p_param, double p_value);
+
+	bool get_jolt_flag(Axis p_axis, JoltFlag p_flag) const;
+	void set_jolt_flag(Axis p_axis, JoltFlag p_flag, bool p_enabled);
+
+	float get_applied_force() const;
+	float get_applied_torque() const;
+
+	virtual void rebuild() override;
+};
+
+#endif // JOLT_GENERIC_6DOF_JOINT_3D_H
diff --git a/modules/jolt_physics/joints/jolt_hinge_joint_3d.cpp b/modules/jolt_physics/joints/jolt_hinge_joint_3d.cpp
new file mode 100644
index 0000000000..de5118f25c
--- /dev/null
+++ b/modules/jolt_physics/joints/jolt_hinge_joint_3d.cpp
@@ -0,0 +1,429 @@
+/**************************************************************************/
+/*  jolt_hinge_joint_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_hinge_joint_3d.h"
+
+#include "../misc/jolt_type_conversions.h"
+#include "../objects/jolt_body_3d.h"
+#include "../spaces/jolt_space_3d.h"
+
+#include "core/config/engine.h"
+
+#include "Jolt/Physics/Constraints/FixedConstraint.h"
+#include "Jolt/Physics/Constraints/HingeConstraint.h"
+
+namespace {
+
+constexpr double DEFAULT_BIAS = 0.3;
+constexpr double DEFAULT_LIMIT_BIAS = 0.3;
+constexpr double DEFAULT_SOFTNESS = 0.9;
+constexpr double DEFAULT_RELAXATION = 1.0;
+
+double estimate_physics_step() {
+	Engine *engine = Engine::get_singleton();
+
+	const double step = 1.0 / engine->get_physics_ticks_per_second();
+	const double step_scaled = step * engine->get_time_scale();
+
+	return step_scaled;
+}
+
+} // namespace
+
+JPH::Constraint *JoltHingeJoint3D::_build_hinge(JPH::Body *p_jolt_body_a, JPH::Body *p_jolt_body_b, const Transform3D &p_shifted_ref_a, const Transform3D &p_shifted_ref_b, float p_limit) const {
+	JPH::HingeConstraintSettings constraint_settings;
+
+	constraint_settings.mSpace = JPH::EConstraintSpace::LocalToBodyCOM;
+	constraint_settings.mPoint1 = to_jolt_r(p_shifted_ref_a.origin);
+	constraint_settings.mHingeAxis1 = to_jolt(p_shifted_ref_a.basis.get_column(Vector3::AXIS_Z));
+	constraint_settings.mNormalAxis1 = to_jolt(p_shifted_ref_a.basis.get_column(Vector3::AXIS_X));
+	constraint_settings.mPoint2 = to_jolt_r(p_shifted_ref_b.origin);
+	constraint_settings.mHingeAxis2 = to_jolt(p_shifted_ref_b.basis.get_column(Vector3::AXIS_Z));
+	constraint_settings.mNormalAxis2 = to_jolt(p_shifted_ref_b.basis.get_column(Vector3::AXIS_X));
+	constraint_settings.mLimitsMin = -p_limit;
+	constraint_settings.mLimitsMax = p_limit;
+
+	if (limit_spring_enabled) {
+		constraint_settings.mLimitsSpringSettings.mFrequency = (float)limit_spring_frequency;
+		constraint_settings.mLimitsSpringSettings.mDamping = (float)limit_spring_damping;
+	}
+
+	if (p_jolt_body_a == nullptr) {
+		return constraint_settings.Create(JPH::Body::sFixedToWorld, *p_jolt_body_b);
+	} else if (p_jolt_body_b == nullptr) {
+		return constraint_settings.Create(*p_jolt_body_a, JPH::Body::sFixedToWorld);
+	} else {
+		return constraint_settings.Create(*p_jolt_body_a, *p_jolt_body_b);
+	}
+}
+
+JPH::Constraint *JoltHingeJoint3D::_build_fixed(JPH::Body *p_jolt_body_a, JPH::Body *p_jolt_body_b, const Transform3D &p_shifted_ref_a, const Transform3D &p_shifted_ref_b) const {
+	JPH::FixedConstraintSettings constraint_settings;
+
+	constraint_settings.mSpace = JPH::EConstraintSpace::LocalToBodyCOM;
+	constraint_settings.mAutoDetectPoint = false;
+	constraint_settings.mPoint1 = to_jolt_r(p_shifted_ref_a.origin);
+	constraint_settings.mAxisX1 = to_jolt(p_shifted_ref_a.basis.get_column(Vector3::AXIS_X));
+	constraint_settings.mAxisY1 = to_jolt(p_shifted_ref_a.basis.get_column(Vector3::AXIS_Y));
+	constraint_settings.mPoint2 = to_jolt_r(p_shifted_ref_b.origin);
+	constraint_settings.mAxisX2 = to_jolt(p_shifted_ref_b.basis.get_column(Vector3::AXIS_X));
+	constraint_settings.mAxisY2 = to_jolt(p_shifted_ref_b.basis.get_column(Vector3::AXIS_Y));
+
+	if (p_jolt_body_a == nullptr) {
+		return constraint_settings.Create(JPH::Body::sFixedToWorld, *p_jolt_body_b);
+	} else if (p_jolt_body_b == nullptr) {
+		return constraint_settings.Create(*p_jolt_body_a, JPH::Body::sFixedToWorld);
+	} else {
+		return constraint_settings.Create(*p_jolt_body_a, *p_jolt_body_b);
+	}
+}
+
+void JoltHingeJoint3D::_update_motor_state() {
+	if (unlikely(_is_fixed())) {
+		return;
+	}
+
+	if (JPH::HingeConstraint *constraint = static_cast<JPH::HingeConstraint *>(jolt_ref.GetPtr())) {
+		constraint->SetMotorState(motor_enabled ? JPH::EMotorState::Velocity : JPH::EMotorState::Off);
+	}
+}
+
+void JoltHingeJoint3D::_update_motor_velocity() {
+	if (unlikely(_is_fixed())) {
+		return;
+	}
+
+	if (JPH::HingeConstraint *constraint = static_cast<JPH::HingeConstraint *>(jolt_ref.GetPtr())) {
+		// We flip the direction since Jolt is CCW but Godot is CW.
+		constraint->SetTargetAngularVelocity((float)-motor_target_speed);
+	}
+}
+
+void JoltHingeJoint3D::_update_motor_limit() {
+	if (unlikely(_is_fixed())) {
+		return;
+	}
+
+	if (JPH::HingeConstraint *constraint = static_cast<JPH::HingeConstraint *>(jolt_ref.GetPtr())) {
+		JPH::MotorSettings &motor_settings = constraint->GetMotorSettings();
+		motor_settings.mMinTorqueLimit = (float)-motor_max_torque;
+		motor_settings.mMaxTorqueLimit = (float)motor_max_torque;
+	}
+}
+
+void JoltHingeJoint3D::_limits_changed() {
+	rebuild();
+	_wake_up_bodies();
+}
+
+void JoltHingeJoint3D::_limit_spring_changed() {
+	rebuild();
+	_wake_up_bodies();
+}
+
+void JoltHingeJoint3D::_motor_state_changed() {
+	_update_motor_state();
+	_wake_up_bodies();
+}
+
+void JoltHingeJoint3D::_motor_speed_changed() {
+	_update_motor_velocity();
+	_wake_up_bodies();
+}
+
+void JoltHingeJoint3D::_motor_limit_changed() {
+	_update_motor_limit();
+	_wake_up_bodies();
+}
+
+JoltHingeJoint3D::JoltHingeJoint3D(const JoltJoint3D &p_old_joint, JoltBody3D *p_body_a, JoltBody3D *p_body_b, const Transform3D &p_local_ref_a, const Transform3D &p_local_ref_b) :
+		JoltJoint3D(p_old_joint, p_body_a, p_body_b, p_local_ref_a, p_local_ref_b) {
+	rebuild();
+}
+
+double JoltHingeJoint3D::get_param(Parameter p_param) const {
+	switch (p_param) {
+		case PhysicsServer3D::HINGE_JOINT_BIAS: {
+			return DEFAULT_BIAS;
+		}
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_UPPER: {
+			return limit_upper;
+		}
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_LOWER: {
+			return limit_lower;
+		}
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_BIAS: {
+			return DEFAULT_LIMIT_BIAS;
+		}
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_SOFTNESS: {
+			return DEFAULT_SOFTNESS;
+		}
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_RELAXATION: {
+			return DEFAULT_RELAXATION;
+		}
+		case PhysicsServer3D::HINGE_JOINT_MOTOR_TARGET_VELOCITY: {
+			return motor_target_speed;
+		}
+		case PhysicsServer3D::HINGE_JOINT_MOTOR_MAX_IMPULSE: {
+			// With Godot using max impulse instead of max torque we don't have much choice but to calculate this and hope the timestep doesn't change.
+			return motor_max_torque * estimate_physics_step();
+		}
+		default: {
+			ERR_FAIL_V_MSG(0.0, vformat("Unhandled parameter: '%d'. This should not happen. Please report this.", p_param));
+		}
+	}
+}
+
+void JoltHingeJoint3D::set_param(Parameter p_param, double p_value) {
+	switch (p_param) {
+		case PhysicsServer3D::HINGE_JOINT_BIAS: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_BIAS)) {
+				WARN_PRINT(vformat("Hinge joint bias is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_UPPER: {
+			limit_upper = p_value;
+			_limits_changed();
+		} break;
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_LOWER: {
+			limit_lower = p_value;
+			_limits_changed();
+		} break;
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_BIAS: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_LIMIT_BIAS)) {
+				WARN_PRINT(vformat("Hinge joint bias limit is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_SOFTNESS: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_SOFTNESS)) {
+				WARN_PRINT(vformat("Hinge joint softness is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::HINGE_JOINT_LIMIT_RELAXATION: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_RELAXATION)) {
+				WARN_PRINT(vformat("Hinge joint relaxation is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::HINGE_JOINT_MOTOR_TARGET_VELOCITY: {
+			motor_target_speed = p_value;
+			_motor_speed_changed();
+		} break;
+		case PhysicsServer3D::HINGE_JOINT_MOTOR_MAX_IMPULSE: {
+			// With Godot using max impulse instead of max torque we don't have much choice but to calculate this and hope the timestep doesn't change.
+			motor_max_torque = p_value / estimate_physics_step();
+			_motor_limit_changed();
+		} break;
+		default: {
+			ERR_FAIL_MSG(vformat("Unhandled parameter: '%d'. This should not happen. Please report this.", p_param));
+		} break;
+	}
+}
+
+double JoltHingeJoint3D::get_jolt_param(JoltParameter p_param) const {
+	switch (p_param) {
+		case JoltPhysicsServer3D::HINGE_JOINT_LIMIT_SPRING_FREQUENCY: {
+			return limit_spring_frequency;
+		}
+		case JoltPhysicsServer3D::HINGE_JOINT_LIMIT_SPRING_DAMPING: {
+			return limit_spring_damping;
+		}
+		case JoltPhysicsServer3D::HINGE_JOINT_MOTOR_MAX_TORQUE: {
+			return motor_max_torque;
+		}
+		default: {
+			ERR_FAIL_V_MSG(0.0, vformat("Unhandled parameter: '%d'. This should not happen. Please report this.", p_param));
+		}
+	}
+}
+
+void JoltHingeJoint3D::set_jolt_param(JoltParameter p_param, double p_value) {
+	switch (p_param) {
+		case JoltPhysicsServer3D::HINGE_JOINT_LIMIT_SPRING_FREQUENCY: {
+			limit_spring_frequency = p_value;
+			_limit_spring_changed();
+		} break;
+		case JoltPhysicsServer3D::HINGE_JOINT_LIMIT_SPRING_DAMPING: {
+			limit_spring_damping = p_value;
+			_limit_spring_changed();
+		} break;
+		case JoltPhysicsServer3D::HINGE_JOINT_MOTOR_MAX_TORQUE: {
+			motor_max_torque = p_value;
+			_motor_limit_changed();
+		} break;
+		default: {
+			ERR_FAIL_MSG(vformat("Unhandled parameter: '%d'. This should not happen. Please report this.", p_param));
+		} break;
+	}
+}
+
+bool JoltHingeJoint3D::get_flag(Flag p_flag) const {
+	switch (p_flag) {
+		case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT: {
+			return limits_enabled;
+		}
+		case PhysicsServer3D::HINGE_JOINT_FLAG_ENABLE_MOTOR: {
+			return motor_enabled;
+		}
+		default: {
+			ERR_FAIL_V_MSG(false, vformat("Unhandled flag: '%d'. This should not happen. Please report this.", p_flag));
+		}
+	}
+}
+
+void JoltHingeJoint3D::set_flag(Flag p_flag, bool p_enabled) {
+	switch (p_flag) {
+		case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT: {
+			limits_enabled = p_enabled;
+			_limits_changed();
+		} break;
+		case PhysicsServer3D::HINGE_JOINT_FLAG_ENABLE_MOTOR: {
+			motor_enabled = p_enabled;
+			_motor_state_changed();
+		} break;
+		default: {
+			ERR_FAIL_MSG(vformat("Unhandled flag: '%d'. This should not happen. Please report this.", p_flag));
+		} break;
+	}
+}
+
+bool JoltHingeJoint3D::get_jolt_flag(JoltFlag p_flag) const {
+	switch (p_flag) {
+		case JoltPhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT_SPRING: {
+			return limit_spring_enabled;
+		}
+		default: {
+			ERR_FAIL_V_MSG(false, vformat("Unhandled flag: '%d'. This should not happen. Please report this.", p_flag));
+		}
+	}
+}
+
+void JoltHingeJoint3D::set_jolt_flag(JoltFlag p_flag, bool p_enabled) {
+	switch (p_flag) {
+		case JoltPhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT_SPRING: {
+			limit_spring_enabled = p_enabled;
+			_limit_spring_changed();
+		} break;
+		default: {
+			ERR_FAIL_MSG(vformat("Unhandled flag: '%d'. This should not happen. Please report this.", p_flag));
+		} break;
+	}
+}
+
+float JoltHingeJoint3D::get_applied_force() const {
+	ERR_FAIL_NULL_V(jolt_ref, 0.0f);
+
+	JoltSpace3D *space = get_space();
+	ERR_FAIL_NULL_V(space, 0.0f);
+
+	const float last_step = space->get_last_step();
+	if (unlikely(last_step == 0.0f)) {
+		return 0.0f;
+	}
+
+	if (_is_fixed()) {
+		JPH::FixedConstraint *constraint = static_cast<JPH::FixedConstraint *>(jolt_ref.GetPtr());
+		return constraint->GetTotalLambdaPosition().Length() / last_step;
+	} else {
+		JPH::HingeConstraint *constraint = static_cast<JPH::HingeConstraint *>(jolt_ref.GetPtr());
+		const JPH::Vec3 total_lambda = JPH::Vec3(constraint->GetTotalLambdaRotation()[0], constraint->GetTotalLambdaRotation()[1], constraint->GetTotalLambdaRotationLimits() + constraint->GetTotalLambdaMotor());
+		return total_lambda.Length() / last_step;
+	}
+}
+
+float JoltHingeJoint3D::get_applied_torque() const {
+	ERR_FAIL_NULL_V(jolt_ref, 0.0f);
+
+	JoltSpace3D *space = get_space();
+	ERR_FAIL_NULL_V(space, 0.0f);
+
+	const float last_step = space->get_last_step();
+	if (unlikely(last_step == 0.0f)) {
+		return 0.0f;
+	}
+
+	if (_is_fixed()) {
+		JPH::FixedConstraint *constraint = static_cast<JPH::FixedConstraint *>(jolt_ref.GetPtr());
+		return constraint->GetTotalLambdaRotation().Length() / last_step;
+	} else {
+		JPH::HingeConstraint *constraint = static_cast<JPH::HingeConstraint *>(jolt_ref.GetPtr());
+		return constraint->GetTotalLambdaRotation().Length() / last_step;
+	}
+}
+
+void JoltHingeJoint3D::rebuild() {
+	destroy();
+
+	JoltSpace3D *space = get_space();
+
+	if (space == nullptr) {
+		return;
+	}
+
+	const JPH::BodyID body_ids[2] = {
+		body_a != nullptr ? body_a->get_jolt_id() : JPH::BodyID(),
+		body_b != nullptr ? body_b->get_jolt_id() : JPH::BodyID()
+	};
+
+	const JoltWritableBodies3D jolt_bodies = space->write_bodies(body_ids, 2);
+
+	JPH::Body *jolt_body_a = static_cast<JPH::Body *>(jolt_bodies[0]);
+	JPH::Body *jolt_body_b = static_cast<JPH::Body *>(jolt_bodies[1]);
+
+	ERR_FAIL_COND(jolt_body_a == nullptr && jolt_body_b == nullptr);
+
+	float ref_shift = 0.0f;
+	float limit = JPH::JPH_PI;
+
+	if (limits_enabled && limit_lower <= limit_upper) {
+		const double limit_midpoint = (limit_lower + limit_upper) / 2.0f;
+
+		ref_shift = float(-limit_midpoint);
+		limit = float(limit_upper - limit_midpoint);
+	}
+
+	Transform3D shifted_ref_a;
+	Transform3D shifted_ref_b;
+
+	_shift_reference_frames(Vector3(), Vector3(0.0f, 0.0f, ref_shift), shifted_ref_a, shifted_ref_b);
+
+	if (_is_fixed()) {
+		jolt_ref = _build_fixed(jolt_body_a, jolt_body_b, shifted_ref_a, shifted_ref_b);
+	} else {
+		jolt_ref = _build_hinge(jolt_body_a, jolt_body_b, shifted_ref_a, shifted_ref_b, limit);
+	}
+
+	space->add_joint(this);
+
+	_update_enabled();
+	_update_iterations();
+	_update_motor_state();
+	_update_motor_velocity();
+	_update_motor_limit();
+}
diff --git a/modules/jolt_physics/joints/jolt_hinge_joint_3d.h b/modules/jolt_physics/joints/jolt_hinge_joint_3d.h
new file mode 100644
index 0000000000..05ac9c3ee7
--- /dev/null
+++ b/modules/jolt_physics/joints/jolt_hinge_joint_3d.h
@@ -0,0 +1,100 @@
+/**************************************************************************/
+/*  jolt_hinge_joint_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_HINGE_JOINT_3D_H
+#define JOLT_HINGE_JOINT_3D_H
+
+#include "../jolt_physics_server_3d.h"
+#include "jolt_joint_3d.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Constraints/SliderConstraint.h"
+
+class JoltHingeJoint3D final : public JoltJoint3D {
+	typedef PhysicsServer3D::HingeJointParam Parameter;
+	typedef JoltPhysicsServer3D::HingeJointParamJolt JoltParameter;
+	typedef PhysicsServer3D::HingeJointFlag Flag;
+	typedef JoltPhysicsServer3D::HingeJointFlagJolt JoltFlag;
+
+	double limit_lower = 0.0;
+	double limit_upper = 0.0;
+
+	double limit_spring_frequency = 0.0;
+	double limit_spring_damping = 0.0;
+
+	double motor_target_speed = 0.0f;
+	double motor_max_torque = FLT_MAX;
+
+	bool limits_enabled = false;
+	bool limit_spring_enabled = false;
+
+	bool motor_enabled = false;
+
+	JPH::Constraint *_build_hinge(JPH::Body *p_jolt_body_a, JPH::Body *p_jolt_body_b, const Transform3D &p_shifted_ref_a, const Transform3D &p_shifted_ref_b, float p_limit) const;
+	JPH::Constraint *_build_fixed(JPH::Body *p_jolt_body_a, JPH::Body *p_jolt_body_b, const Transform3D &p_shifted_ref_a, const Transform3D &p_shifted_ref_b) const;
+
+	bool _is_sprung() const { return limit_spring_enabled && limit_spring_frequency > 0.0; }
+	bool _is_fixed() const { return limits_enabled && limit_lower == limit_upper && !_is_sprung(); }
+
+	void _update_motor_state();
+	void _update_motor_velocity();
+	void _update_motor_limit();
+
+	void _limits_changed();
+	void _limit_spring_changed();
+	void _motor_state_changed();
+	void _motor_speed_changed();
+	void _motor_limit_changed();
+
+public:
+	JoltHingeJoint3D(const JoltJoint3D &p_old_joint, JoltBody3D *p_body_a, JoltBody3D *p_body_b, const Transform3D &p_local_ref_a, const Transform3D &p_local_ref_b);
+
+	virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_HINGE; }
+
+	double get_param(Parameter p_param) const;
+	void set_param(Parameter p_param, double p_value);
+
+	double get_jolt_param(JoltParameter p_param) const;
+	void set_jolt_param(JoltParameter p_param, double p_value);
+
+	bool get_flag(Flag p_flag) const;
+	void set_flag(Flag p_flag, bool p_enabled);
+
+	bool get_jolt_flag(JoltFlag p_flag) const;
+	void set_jolt_flag(JoltFlag p_flag, bool p_enabled);
+
+	float get_applied_force() const;
+	float get_applied_torque() const;
+
+	virtual void rebuild() override;
+};
+
+#endif // JOLT_HINGE_JOINT_3D_H
diff --git a/modules/jolt_physics/joints/jolt_joint_3d.cpp b/modules/jolt_physics/joints/jolt_joint_3d.cpp
new file mode 100644
index 0000000000..2f9a9daeef
--- /dev/null
+++ b/modules/jolt_physics/joints/jolt_joint_3d.cpp
@@ -0,0 +1,235 @@
+/**************************************************************************/
+/*  jolt_joint_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_joint_3d.h"
+
+#include "../jolt_project_settings.h"
+#include "../misc/jolt_type_conversions.h"
+#include "../objects/jolt_body_3d.h"
+#include "../spaces/jolt_space_3d.h"
+
+namespace {
+
+constexpr int DEFAULT_SOLVER_PRIORITY = 1;
+
+} // namespace
+
+void JoltJoint3D::_shift_reference_frames(const Vector3 &p_linear_shift, const Vector3 &p_angular_shift, Transform3D &r_shifted_ref_a, Transform3D &r_shifted_ref_b) {
+	Vector3 origin_a = local_ref_a.origin;
+	Vector3 origin_b = local_ref_b.origin;
+
+	if (body_a != nullptr) {
+		origin_a *= body_a->get_scale();
+		origin_a -= to_godot(body_a->get_jolt_shape()->GetCenterOfMass());
+	}
+
+	if (body_b != nullptr) {
+		origin_b *= body_b->get_scale();
+		origin_b -= to_godot(body_b->get_jolt_shape()->GetCenterOfMass());
+	}
+
+	const Basis &basis_a = local_ref_a.basis;
+	const Basis &basis_b = local_ref_b.basis;
+
+	const Basis shifted_basis_a = basis_a * Basis::from_euler(p_angular_shift, EulerOrder::ZYX);
+	const Vector3 shifted_origin_a = origin_a - basis_a.xform(p_linear_shift);
+
+	r_shifted_ref_a = Transform3D(shifted_basis_a, shifted_origin_a);
+	r_shifted_ref_b = Transform3D(basis_b, origin_b);
+}
+
+void JoltJoint3D::_wake_up_bodies() {
+	if (body_a != nullptr) {
+		body_a->wake_up();
+	}
+
+	if (body_b != nullptr) {
+		body_b->wake_up();
+	}
+}
+
+void JoltJoint3D::_update_enabled() {
+	if (jolt_ref != nullptr) {
+		jolt_ref->SetEnabled(enabled);
+	}
+}
+
+void JoltJoint3D::_update_iterations() {
+	if (jolt_ref != nullptr) {
+		jolt_ref->SetNumVelocityStepsOverride((JPH::uint)velocity_iterations);
+		jolt_ref->SetNumPositionStepsOverride((JPH::uint)position_iterations);
+	}
+}
+
+void JoltJoint3D::_enabled_changed() {
+	_update_enabled();
+	_wake_up_bodies();
+}
+
+void JoltJoint3D::_iterations_changed() {
+	_update_iterations();
+	_wake_up_bodies();
+}
+
+String JoltJoint3D::_bodies_to_string() const {
+	return vformat("'%s' and '%s'", body_a != nullptr ? body_a->to_string() : "<unknown>", body_b != nullptr ? body_b->to_string() : "<World>");
+}
+
+JoltJoint3D::JoltJoint3D(const JoltJoint3D &p_old_joint, JoltBody3D *p_body_a, JoltBody3D *p_body_b, const Transform3D &p_local_ref_a, const Transform3D &p_local_ref_b) :
+		enabled(p_old_joint.enabled),
+		collision_disabled(p_old_joint.collision_disabled),
+		body_a(p_body_a),
+		body_b(p_body_b),
+		rid(p_old_joint.rid),
+		local_ref_a(p_local_ref_a),
+		local_ref_b(p_local_ref_b) {
+	if (body_a != nullptr) {
+		body_a->add_joint(this);
+	}
+
+	if (body_b != nullptr) {
+		body_b->add_joint(this);
+	}
+
+	if (body_b == nullptr && JoltProjectSettings::use_joint_world_node_a()) {
+		// The joint scene nodes will, when omitting one of the two body nodes, always pass in a
+		// null `body_b` to indicate it being the "world node", regardless of which body node you
+		// leave blank. So we need to correct for that if we wish to use the arguably more intuitive
+		// alternative where `body_a` is the "world node" instead.
+
+		SWAP(body_a, body_b);
+		SWAP(local_ref_a, local_ref_b);
+	}
+}
+
+JoltJoint3D::~JoltJoint3D() {
+	if (body_a != nullptr) {
+		body_a->remove_joint(this);
+	}
+
+	if (body_b != nullptr) {
+		body_b->remove_joint(this);
+	}
+
+	destroy();
+}
+
+JoltSpace3D *JoltJoint3D::get_space() const {
+	if (body_a != nullptr && body_b != nullptr) {
+		JoltSpace3D *space_a = body_a->get_space();
+		JoltSpace3D *space_b = body_b->get_space();
+
+		if (space_a == nullptr || space_b == nullptr) {
+			return nullptr;
+		}
+
+		ERR_FAIL_COND_V_MSG(space_a != space_b, nullptr, vformat("Joint was found to connect bodies in different physics spaces. This joint will effectively be disabled. This joint connects %s.", _bodies_to_string()));
+
+		return space_a;
+	} else if (body_a != nullptr) {
+		return body_a->get_space();
+	} else if (body_b != nullptr) {
+		return body_b->get_space();
+	}
+
+	return nullptr;
+}
+
+void JoltJoint3D::set_enabled(bool p_enabled) {
+	if (enabled == p_enabled) {
+		return;
+	}
+
+	enabled = p_enabled;
+
+	_enabled_changed();
+}
+
+int JoltJoint3D::get_solver_priority() const {
+	return DEFAULT_SOLVER_PRIORITY;
+}
+
+void JoltJoint3D::set_solver_priority(int p_priority) {
+	if (p_priority != DEFAULT_SOLVER_PRIORITY) {
+		WARN_PRINT(vformat("Joint solver priority is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+	}
+}
+
+void JoltJoint3D::set_solver_velocity_iterations(int p_iterations) {
+	if (velocity_iterations == p_iterations) {
+		return;
+	}
+
+	velocity_iterations = p_iterations;
+
+	_iterations_changed();
+}
+
+void JoltJoint3D::set_solver_position_iterations(int p_iterations) {
+	if (position_iterations == p_iterations) {
+		return;
+	}
+
+	position_iterations = p_iterations;
+
+	_iterations_changed();
+}
+
+void JoltJoint3D::set_collision_disabled(bool p_disabled) {
+	collision_disabled = p_disabled;
+
+	if (body_a == nullptr || body_b == nullptr) {
+		return;
+	}
+
+	PhysicsServer3D *physics_server = PhysicsServer3D::get_singleton();
+
+	if (collision_disabled) {
+		physics_server->body_add_collision_exception(body_a->get_rid(), body_b->get_rid());
+		physics_server->body_add_collision_exception(body_b->get_rid(), body_a->get_rid());
+	} else {
+		physics_server->body_remove_collision_exception(body_a->get_rid(), body_b->get_rid());
+		physics_server->body_remove_collision_exception(body_b->get_rid(), body_a->get_rid());
+	}
+}
+
+void JoltJoint3D::destroy() {
+	if (jolt_ref == nullptr) {
+		return;
+	}
+
+	JoltSpace3D *space = get_space();
+
+	if (space != nullptr) {
+		space->remove_joint(this);
+	}
+
+	jolt_ref = nullptr;
+}
diff --git a/modules/jolt_physics/joints/jolt_joint_3d.h b/modules/jolt_physics/joints/jolt_joint_3d.h
new file mode 100644
index 0000000000..efb219910d
--- /dev/null
+++ b/modules/jolt_physics/joints/jolt_joint_3d.h
@@ -0,0 +1,107 @@
+/**************************************************************************/
+/*  jolt_joint_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_JOINT_3D_H
+#define JOLT_JOINT_3D_H
+
+#include "servers/physics_server_3d.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Constraints/Constraint.h"
+
+class JoltBody3D;
+class JoltSpace3D;
+
+class JoltJoint3D {
+protected:
+	bool enabled = true;
+	bool collision_disabled = false;
+
+	int velocity_iterations = 0;
+	int position_iterations = 0;
+
+	JPH::Ref<JPH::Constraint> jolt_ref;
+
+	JoltBody3D *body_a = nullptr;
+	JoltBody3D *body_b = nullptr;
+
+	RID rid;
+
+	Transform3D local_ref_a;
+	Transform3D local_ref_b;
+
+	void _shift_reference_frames(const Vector3 &p_linear_shift, const Vector3 &p_angular_shift, Transform3D &r_shifted_ref_a, Transform3D &r_shifted_ref_b);
+
+	void _wake_up_bodies();
+
+	void _update_enabled();
+	void _update_iterations();
+
+	void _enabled_changed();
+	void _iterations_changed();
+
+	String _bodies_to_string() const;
+
+public:
+	JoltJoint3D() = default;
+	JoltJoint3D(const JoltJoint3D &p_old_joint, JoltBody3D *p_body_a, JoltBody3D *p_body_b, const Transform3D &p_local_ref_a, const Transform3D &p_local_ref_b);
+	virtual ~JoltJoint3D();
+
+	virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_MAX; }
+
+	RID get_rid() const { return rid; }
+	void set_rid(const RID &p_rid) { rid = p_rid; }
+
+	JoltSpace3D *get_space() const;
+
+	JPH::Constraint *get_jolt_ref() const { return jolt_ref; }
+
+	bool is_enabled() const { return enabled; }
+	void set_enabled(bool p_enabled);
+
+	int get_solver_priority() const;
+	void set_solver_priority(int p_priority);
+
+	int get_solver_velocity_iterations() const { return velocity_iterations; }
+	void set_solver_velocity_iterations(int p_iterations);
+
+	int get_solver_position_iterations() const { return position_iterations; }
+	void set_solver_position_iterations(int p_iterations);
+
+	bool is_collision_disabled() const { return collision_disabled; }
+	void set_collision_disabled(bool p_disabled);
+
+	void destroy();
+
+	virtual void rebuild() {}
+};
+
+#endif // JOLT_JOINT_3D_H
diff --git a/modules/jolt_physics/joints/jolt_pin_joint_3d.cpp b/modules/jolt_physics/joints/jolt_pin_joint_3d.cpp
new file mode 100644
index 0000000000..42ede3bd5b
--- /dev/null
+++ b/modules/jolt_physics/joints/jolt_pin_joint_3d.cpp
@@ -0,0 +1,169 @@
+/**************************************************************************/
+/*  jolt_pin_joint_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_pin_joint_3d.h"
+
+#include "../misc/jolt_type_conversions.h"
+#include "../objects/jolt_body_3d.h"
+#include "../spaces/jolt_space_3d.h"
+
+#include "Jolt/Physics/Constraints/PointConstraint.h"
+
+namespace {
+
+constexpr double DEFAULT_BIAS = 0.3;
+constexpr double DEFAULT_DAMPING = 1.0;
+constexpr double DEFAULT_IMPULSE_CLAMP = 0.0;
+
+} // namespace
+
+JPH::Constraint *JoltPinJoint3D::_build_pin(JPH::Body *p_jolt_body_a, JPH::Body *p_jolt_body_b, const Transform3D &p_shifted_ref_a, const Transform3D &p_shifted_ref_b) {
+	JPH::PointConstraintSettings constraint_settings;
+	constraint_settings.mSpace = JPH::EConstraintSpace::LocalToBodyCOM;
+	constraint_settings.mPoint1 = to_jolt_r(p_shifted_ref_a.origin);
+	constraint_settings.mPoint2 = to_jolt_r(p_shifted_ref_b.origin);
+
+	if (p_jolt_body_a == nullptr) {
+		return constraint_settings.Create(JPH::Body::sFixedToWorld, *p_jolt_body_b);
+	} else if (p_jolt_body_b == nullptr) {
+		return constraint_settings.Create(*p_jolt_body_a, JPH::Body::sFixedToWorld);
+	} else {
+		return constraint_settings.Create(*p_jolt_body_a, *p_jolt_body_b);
+	}
+}
+
+void JoltPinJoint3D::_points_changed() {
+	rebuild();
+	_wake_up_bodies();
+}
+
+JoltPinJoint3D::JoltPinJoint3D(const JoltJoint3D &p_old_joint, JoltBody3D *p_body_a, JoltBody3D *p_body_b, const Vector3 &p_local_a, const Vector3 &p_local_b) :
+		JoltJoint3D(p_old_joint, p_body_a, p_body_b, Transform3D({}, p_local_a), Transform3D({}, p_local_b)) {
+	rebuild();
+}
+
+void JoltPinJoint3D::set_local_a(const Vector3 &p_local_a) {
+	local_ref_a = Transform3D({}, p_local_a);
+	_points_changed();
+}
+
+void JoltPinJoint3D::set_local_b(const Vector3 &p_local_b) {
+	local_ref_b = Transform3D({}, p_local_b);
+	_points_changed();
+}
+
+double JoltPinJoint3D::get_param(PhysicsServer3D::PinJointParam p_param) const {
+	switch (p_param) {
+		case PhysicsServer3D::PIN_JOINT_BIAS: {
+			return DEFAULT_BIAS;
+		}
+		case PhysicsServer3D::PIN_JOINT_DAMPING: {
+			return DEFAULT_DAMPING;
+		}
+		case PhysicsServer3D::PIN_JOINT_IMPULSE_CLAMP: {
+			return DEFAULT_IMPULSE_CLAMP;
+		}
+		default: {
+			ERR_FAIL_V_MSG(0.0, vformat("Unhandled pin joint parameter: '%d'. This should not happen. Please report this.", p_param));
+		}
+	}
+}
+
+void JoltPinJoint3D::set_param(PhysicsServer3D::PinJointParam p_param, double p_value) {
+	switch (p_param) {
+		case PhysicsServer3D::PIN_JOINT_BIAS: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_BIAS)) {
+				WARN_PRINT(vformat("Pin joint bias is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::PIN_JOINT_DAMPING: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_DAMPING)) {
+				WARN_PRINT(vformat("Pin joint damping is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::PIN_JOINT_IMPULSE_CLAMP: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_IMPULSE_CLAMP)) {
+				WARN_PRINT(vformat("Pin joint impulse clamp is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		default: {
+			ERR_FAIL_MSG(vformat("Unhandled pin joint parameter: '%d'. This should not happen. Please report this.", p_param));
+		} break;
+	}
+}
+
+float JoltPinJoint3D::get_applied_force() const {
+	JPH::PointConstraint *constraint = static_cast<JPH::PointConstraint *>(jolt_ref.GetPtr());
+	ERR_FAIL_NULL_V(constraint, 0.0f);
+
+	JoltSpace3D *space = get_space();
+	ERR_FAIL_NULL_V(space, 0.0f);
+
+	const float last_step = space->get_last_step();
+	if (unlikely(last_step == 0.0f)) {
+		return 0.0f;
+	}
+
+	return constraint->GetTotalLambdaPosition().Length() / last_step;
+}
+
+void JoltPinJoint3D::rebuild() {
+	destroy();
+
+	JoltSpace3D *space = get_space();
+
+	if (space == nullptr) {
+		return;
+	}
+
+	const JPH::BodyID body_ids[2] = {
+		body_a != nullptr ? body_a->get_jolt_id() : JPH::BodyID(),
+		body_b != nullptr ? body_b->get_jolt_id() : JPH::BodyID()
+	};
+
+	const JoltWritableBodies3D jolt_bodies = space->write_bodies(body_ids, 2);
+
+	JPH::Body *jolt_body_a = static_cast<JPH::Body *>(jolt_bodies[0]);
+	JPH::Body *jolt_body_b = static_cast<JPH::Body *>(jolt_bodies[1]);
+
+	ERR_FAIL_COND(jolt_body_a == nullptr && jolt_body_b == nullptr);
+
+	Transform3D shifted_ref_a;
+	Transform3D shifted_ref_b;
+
+	_shift_reference_frames(Vector3(), Vector3(), shifted_ref_a, shifted_ref_b);
+
+	jolt_ref = _build_pin(jolt_body_a, jolt_body_b, shifted_ref_a, shifted_ref_b);
+
+	space->add_joint(this);
+
+	_update_enabled();
+	_update_iterations();
+}
diff --git a/modules/jolt_physics/joints/jolt_pin_joint_3d.h b/modules/jolt_physics/joints/jolt_pin_joint_3d.h
new file mode 100644
index 0000000000..55cee1ad2d
--- /dev/null
+++ b/modules/jolt_physics/joints/jolt_pin_joint_3d.h
@@ -0,0 +1,64 @@
+/**************************************************************************/
+/*  jolt_pin_joint_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_PIN_JOINT_3D_H
+#define JOLT_PIN_JOINT_3D_H
+
+#include "jolt_joint_3d.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Constraints/SliderConstraint.h"
+
+class JoltPinJoint3D final : public JoltJoint3D {
+	static JPH::Constraint *_build_pin(JPH::Body *p_jolt_body_a, JPH::Body *p_jolt_body_b, const Transform3D &p_shifted_ref_a, const Transform3D &p_shifted_ref_b);
+
+	void _points_changed();
+
+public:
+	JoltPinJoint3D(const JoltJoint3D &p_old_joint, JoltBody3D *p_body_a, JoltBody3D *p_body_b, const Vector3 &p_local_a, const Vector3 &p_local_b);
+
+	virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_PIN; }
+
+	Vector3 get_local_a() const { return local_ref_a.origin; }
+	void set_local_a(const Vector3 &p_local_a);
+
+	Vector3 get_local_b() const { return local_ref_b.origin; }
+	void set_local_b(const Vector3 &p_local_b);
+
+	double get_param(PhysicsServer3D::PinJointParam p_param) const;
+	void set_param(PhysicsServer3D::PinJointParam p_param, double p_value);
+
+	float get_applied_force() const;
+
+	virtual void rebuild() override;
+};
+
+#endif // JOLT_PIN_JOINT_3D_H
diff --git a/modules/jolt_physics/joints/jolt_slider_joint_3d.cpp b/modules/jolt_physics/joints/jolt_slider_joint_3d.cpp
new file mode 100644
index 0000000000..335e657621
--- /dev/null
+++ b/modules/jolt_physics/joints/jolt_slider_joint_3d.cpp
@@ -0,0 +1,542 @@
+/**************************************************************************/
+/*  jolt_slider_joint_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_slider_joint_3d.h"
+
+#include "../misc/jolt_type_conversions.h"
+#include "../objects/jolt_body_3d.h"
+#include "../spaces/jolt_space_3d.h"
+
+#include "Jolt/Physics/Constraints/FixedConstraint.h"
+#include "Jolt/Physics/Constraints/SliderConstraint.h"
+
+namespace {
+
+constexpr double DEFAULT_LINEAR_LIMIT_SOFTNESS = 1.0;
+constexpr double DEFAULT_LINEAR_LIMIT_RESTITUTION = 0.7;
+constexpr double DEFAULT_LINEAR_LIMIT_DAMPING = 1.0;
+
+constexpr double DEFAULT_LINEAR_MOTION_SOFTNESS = 1.0;
+constexpr double DEFAULT_LINEAR_MOTION_RESTITUTION = 0.7;
+constexpr double DEFAULT_LINEAR_MOTION_DAMPING = 0.0;
+
+constexpr double DEFAULT_LINEAR_ORTHO_SOFTNESS = 1.0;
+constexpr double DEFAULT_LINEAR_ORTHO_RESTITUTION = 0.7;
+constexpr double DEFAULT_LINEAR_ORTHO_DAMPING = 1.0;
+
+constexpr double DEFAULT_ANGULAR_LIMIT_UPPER = 0.0;
+constexpr double DEFAULT_ANGULAR_LIMIT_LOWER = 0.0;
+constexpr double DEFAULT_ANGULAR_LIMIT_SOFTNESS = 1.0;
+constexpr double DEFAULT_ANGULAR_LIMIT_RESTITUTION = 0.7;
+constexpr double DEFAULT_ANGULAR_LIMIT_DAMPING = 0.0;
+
+constexpr double DEFAULT_ANGULAR_MOTION_SOFTNESS = 1.0;
+constexpr double DEFAULT_ANGULAR_MOTION_RESTITUTION = 0.7;
+constexpr double DEFAULT_ANGULAR_MOTION_DAMPING = 1.0;
+
+constexpr double DEFAULT_ANGULAR_ORTHO_SOFTNESS = 1.0;
+constexpr double DEFAULT_ANGULAR_ORTHO_RESTITUTION = 0.7;
+constexpr double DEFAULT_ANGULAR_ORTHO_DAMPING = 1.0;
+
+} // namespace
+
+JPH::Constraint *JoltSliderJoint3D::_build_slider(JPH::Body *p_jolt_body_a, JPH::Body *p_jolt_body_b, const Transform3D &p_shifted_ref_a, const Transform3D &p_shifted_ref_b, float p_limit) const {
+	JPH::SliderConstraintSettings constraint_settings;
+
+	constraint_settings.mSpace = JPH::EConstraintSpace::LocalToBodyCOM;
+	constraint_settings.mAutoDetectPoint = false;
+	constraint_settings.mPoint1 = to_jolt_r(p_shifted_ref_a.origin);
+	constraint_settings.mSliderAxis1 = to_jolt(p_shifted_ref_a.basis.get_column(Vector3::AXIS_X));
+	constraint_settings.mNormalAxis1 = to_jolt(p_shifted_ref_a.basis.get_column(Vector3::AXIS_Z));
+	constraint_settings.mPoint2 = to_jolt_r(p_shifted_ref_b.origin);
+	constraint_settings.mSliderAxis2 = to_jolt(p_shifted_ref_b.basis.get_column(Vector3::AXIS_X));
+	constraint_settings.mNormalAxis2 = to_jolt(p_shifted_ref_b.basis.get_column(Vector3::AXIS_Z));
+	constraint_settings.mLimitsMin = -p_limit;
+	constraint_settings.mLimitsMax = p_limit;
+
+	if (limit_spring_enabled) {
+		constraint_settings.mLimitsSpringSettings.mFrequency = (float)limit_spring_frequency;
+		constraint_settings.mLimitsSpringSettings.mDamping = (float)limit_spring_damping;
+	}
+
+	if (p_jolt_body_a == nullptr) {
+		return constraint_settings.Create(JPH::Body::sFixedToWorld, *p_jolt_body_b);
+	} else if (p_jolt_body_b == nullptr) {
+		return constraint_settings.Create(*p_jolt_body_a, JPH::Body::sFixedToWorld);
+	} else {
+		return constraint_settings.Create(*p_jolt_body_a, *p_jolt_body_b);
+	}
+}
+
+JPH::Constraint *JoltSliderJoint3D::_build_fixed(JPH::Body *p_jolt_body_a, JPH::Body *p_jolt_body_b, const Transform3D &p_shifted_ref_a, const Transform3D &p_shifted_ref_b) const {
+	JPH::FixedConstraintSettings constraint_settings;
+
+	constraint_settings.mSpace = JPH::EConstraintSpace::LocalToBodyCOM;
+	constraint_settings.mAutoDetectPoint = false;
+	constraint_settings.mPoint1 = to_jolt_r(p_shifted_ref_a.origin);
+	constraint_settings.mAxisX1 = to_jolt(p_shifted_ref_a.basis.get_column(Vector3::AXIS_X));
+	constraint_settings.mAxisY1 = to_jolt(p_shifted_ref_a.basis.get_column(Vector3::AXIS_Y));
+	constraint_settings.mPoint2 = to_jolt_r(p_shifted_ref_b.origin);
+	constraint_settings.mAxisX2 = to_jolt(p_shifted_ref_b.basis.get_column(Vector3::AXIS_X));
+	constraint_settings.mAxisY2 = to_jolt(p_shifted_ref_b.basis.get_column(Vector3::AXIS_Y));
+
+	if (p_jolt_body_a == nullptr) {
+		return constraint_settings.Create(JPH::Body::sFixedToWorld, *p_jolt_body_b);
+	} else if (p_jolt_body_b == nullptr) {
+		return constraint_settings.Create(*p_jolt_body_a, JPH::Body::sFixedToWorld);
+	} else {
+		return constraint_settings.Create(*p_jolt_body_a, *p_jolt_body_b);
+	}
+}
+
+void JoltSliderJoint3D::_update_motor_state() {
+	if (unlikely(_is_fixed())) {
+		return;
+	}
+
+	if (JPH::SliderConstraint *constraint = static_cast<JPH::SliderConstraint *>(jolt_ref.GetPtr())) {
+		constraint->SetMotorState(motor_enabled ? JPH::EMotorState::Velocity : JPH::EMotorState::Off);
+	}
+}
+
+void JoltSliderJoint3D::_update_motor_velocity() {
+	if (unlikely(_is_fixed())) {
+		return;
+	}
+
+	if (JPH::SliderConstraint *constraint = static_cast<JPH::SliderConstraint *>(jolt_ref.GetPtr())) {
+		constraint->SetTargetVelocity((float)motor_target_speed);
+	}
+}
+
+void JoltSliderJoint3D::_update_motor_limit() {
+	if (unlikely(_is_fixed())) {
+		return;
+	}
+
+	if (JPH::SliderConstraint *constraint = static_cast<JPH::SliderConstraint *>(jolt_ref.GetPtr())) {
+		JPH::MotorSettings &motor_settings = constraint->GetMotorSettings();
+		motor_settings.mMinForceLimit = (float)-motor_max_force;
+		motor_settings.mMaxForceLimit = (float)motor_max_force;
+	}
+}
+
+void JoltSliderJoint3D::_limits_changed() {
+	rebuild();
+	_wake_up_bodies();
+}
+
+void JoltSliderJoint3D::_limit_spring_changed() {
+	rebuild();
+	_wake_up_bodies();
+}
+
+void JoltSliderJoint3D::_motor_state_changed() {
+	_update_motor_state();
+	_wake_up_bodies();
+}
+
+void JoltSliderJoint3D::_motor_speed_changed() {
+	_update_motor_velocity();
+	_wake_up_bodies();
+}
+
+void JoltSliderJoint3D::_motor_limit_changed() {
+	_update_motor_limit();
+	_wake_up_bodies();
+}
+
+JoltSliderJoint3D::JoltSliderJoint3D(const JoltJoint3D &p_old_joint, JoltBody3D *p_body_a, JoltBody3D *p_body_b, const Transform3D &p_local_ref_a, const Transform3D &p_local_ref_b) :
+		JoltJoint3D(p_old_joint, p_body_a, p_body_b, p_local_ref_a, p_local_ref_b) {
+	rebuild();
+}
+
+double JoltSliderJoint3D::get_param(PhysicsServer3D::SliderJointParam p_param) const {
+	switch (p_param) {
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER: {
+			return limit_upper;
+		}
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_LOWER: {
+			return limit_lower;
+		}
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: {
+			return DEFAULT_LINEAR_LIMIT_SOFTNESS;
+		}
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: {
+			return DEFAULT_LINEAR_LIMIT_RESTITUTION;
+		}
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: {
+			return DEFAULT_LINEAR_LIMIT_DAMPING;
+		}
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: {
+			return DEFAULT_LINEAR_MOTION_SOFTNESS;
+		}
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: {
+			return DEFAULT_LINEAR_MOTION_RESTITUTION;
+		}
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_DAMPING: {
+			return DEFAULT_LINEAR_MOTION_DAMPING;
+		}
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: {
+			return DEFAULT_LINEAR_ORTHO_SOFTNESS;
+		}
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: {
+			return DEFAULT_LINEAR_ORTHO_RESTITUTION;
+		}
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: {
+			return DEFAULT_LINEAR_ORTHO_DAMPING;
+		}
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: {
+			return DEFAULT_ANGULAR_LIMIT_UPPER;
+		}
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: {
+			return DEFAULT_ANGULAR_LIMIT_LOWER;
+		}
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: {
+			return DEFAULT_ANGULAR_LIMIT_SOFTNESS;
+		}
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: {
+			return DEFAULT_ANGULAR_LIMIT_RESTITUTION;
+		}
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: {
+			return DEFAULT_ANGULAR_LIMIT_DAMPING;
+		}
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: {
+			return DEFAULT_ANGULAR_MOTION_SOFTNESS;
+		}
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: {
+			return DEFAULT_ANGULAR_MOTION_RESTITUTION;
+		}
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: {
+			return DEFAULT_ANGULAR_MOTION_DAMPING;
+		}
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: {
+			return DEFAULT_ANGULAR_ORTHO_SOFTNESS;
+		}
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: {
+			return DEFAULT_ANGULAR_ORTHO_RESTITUTION;
+		}
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: {
+			return DEFAULT_ANGULAR_ORTHO_DAMPING;
+		}
+		default: {
+			ERR_FAIL_V_MSG(0.0, vformat("Unhandled slider joint parameter: '%d'. This should not happen. Please report this.", p_param));
+		}
+	}
+}
+
+void JoltSliderJoint3D::set_param(PhysicsServer3D::SliderJointParam p_param, double p_value) {
+	switch (p_param) {
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER: {
+			limit_upper = p_value;
+			_limits_changed();
+		} break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_LOWER: {
+			limit_lower = p_value;
+			_limits_changed();
+		} break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_LINEAR_LIMIT_SOFTNESS)) {
+				WARN_PRINT(vformat("Slider joint linear limit softness is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_LINEAR_LIMIT_RESTITUTION)) {
+				WARN_PRINT(vformat("Slider joint linear limit restitution is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_LINEAR_LIMIT_DAMPING)) {
+				WARN_PRINT(vformat("Slider joint linear limit damping is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_LINEAR_MOTION_SOFTNESS)) {
+				WARN_PRINT(vformat("Slider joint linear motion softness is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_LINEAR_MOTION_RESTITUTION)) {
+				WARN_PRINT(vformat("Slider joint linear motion restitution is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_MOTION_DAMPING: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_LINEAR_MOTION_DAMPING)) {
+				WARN_PRINT(vformat("Slider joint linear motion damping is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_LINEAR_ORTHO_SOFTNESS)) {
+				WARN_PRINT(vformat("Slider joint linear ortho softness is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_LINEAR_ORTHO_RESTITUTION)) {
+				WARN_PRINT(vformat("Slider joint linear ortho restitution is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_LINEAR_ORTHO_DAMPING)) {
+				WARN_PRINT(vformat("Slider joint linear ortho damping is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_LIMIT_UPPER)) {
+				WARN_PRINT(vformat("Slider joint angular limits are not supported when using Jolt Physics. Any such value will be ignored. Try using Generic6DOFJoint3D instead. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_LIMIT_LOWER)) {
+				WARN_PRINT(vformat("Slider joint angular limits are not supported when using Jolt Physics. Any such value will be ignored. Try using Generic6DOFJoint3D instead. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_LIMIT_SOFTNESS)) {
+				WARN_PRINT(vformat("Slider joint angular limit softness is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_LIMIT_RESTITUTION)) {
+				WARN_PRINT(vformat("Slider joint angular limit restitution is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_LIMIT_DAMPING)) {
+				WARN_PRINT(vformat("Slider joint angular limit damping is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_MOTION_SOFTNESS)) {
+				WARN_PRINT(vformat("Slider joint angular motion softness is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_MOTION_RESTITUTION)) {
+				WARN_PRINT(vformat("Slider joint angular motion restitution is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_MOTION_DAMPING)) {
+				WARN_PRINT(vformat("Slider joint angular motion damping is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_ORTHO_SOFTNESS)) {
+				WARN_PRINT(vformat("Slider joint angular ortho softness is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_ORTHO_RESTITUTION)) {
+				WARN_PRINT(vformat("Slider joint angular ortho restitution is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		case PhysicsServer3D::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: {
+			if (!Math::is_equal_approx(p_value, DEFAULT_ANGULAR_ORTHO_DAMPING)) {
+				WARN_PRINT(vformat("Slider joint angular ortho damping is not supported when using Jolt Physics. Any such value will be ignored. This joint connects %s.", _bodies_to_string()));
+			}
+		} break;
+		default: {
+			ERR_FAIL_MSG(vformat("Unhandled slider joint parameter: '%d'. This should not happen. Please report this.", p_param));
+		} break;
+	}
+}
+
+double JoltSliderJoint3D::get_jolt_param(JoltParameter p_param) const {
+	switch (p_param) {
+		case JoltPhysicsServer3D::SLIDER_JOINT_LIMIT_SPRING_FREQUENCY: {
+			return limit_spring_frequency;
+		}
+		case JoltPhysicsServer3D::SLIDER_JOINT_LIMIT_SPRING_DAMPING: {
+			return limit_spring_damping;
+		}
+		case JoltPhysicsServer3D::SLIDER_JOINT_MOTOR_TARGET_VELOCITY: {
+			return motor_target_speed;
+		}
+		case JoltPhysicsServer3D::SLIDER_JOINT_MOTOR_MAX_FORCE: {
+			return motor_max_force;
+		}
+		default: {
+			ERR_FAIL_V_MSG(0.0, vformat("Unhandled parameter: '%d'. This should not happen. Please report this.", p_param));
+		}
+	}
+}
+
+void JoltSliderJoint3D::set_jolt_param(JoltParameter p_param, double p_value) {
+	switch (p_param) {
+		case JoltPhysicsServer3D::SLIDER_JOINT_LIMIT_SPRING_FREQUENCY: {
+			limit_spring_frequency = p_value;
+			_limit_spring_changed();
+		} break;
+		case JoltPhysicsServer3D::SLIDER_JOINT_LIMIT_SPRING_DAMPING: {
+			limit_spring_damping = p_value;
+			_limit_spring_changed();
+		} break;
+		case JoltPhysicsServer3D::SLIDER_JOINT_MOTOR_TARGET_VELOCITY: {
+			motor_target_speed = p_value;
+			_motor_speed_changed();
+		} break;
+		case JoltPhysicsServer3D::SLIDER_JOINT_MOTOR_MAX_FORCE: {
+			motor_max_force = p_value;
+			_motor_limit_changed();
+		} break;
+		default: {
+			ERR_FAIL_MSG(vformat("Unhandled parameter: '%d'. This should not happen. Please report this.", p_param));
+		} break;
+	}
+}
+
+bool JoltSliderJoint3D::get_jolt_flag(JoltFlag p_flag) const {
+	switch (p_flag) {
+		case JoltPhysicsServer3D::SLIDER_JOINT_FLAG_USE_LIMIT: {
+			return limits_enabled;
+		}
+		case JoltPhysicsServer3D::SLIDER_JOINT_FLAG_USE_LIMIT_SPRING: {
+			return limit_spring_enabled;
+		}
+		case JoltPhysicsServer3D::SLIDER_JOINT_FLAG_ENABLE_MOTOR: {
+			return motor_enabled;
+		}
+		default: {
+			ERR_FAIL_V_MSG(false, vformat("Unhandled flag: '%d'. This should not happen. Please report this.", p_flag));
+		}
+	}
+}
+
+void JoltSliderJoint3D::set_jolt_flag(JoltFlag p_flag, bool p_enabled) {
+	switch (p_flag) {
+		case JoltPhysicsServer3D::SLIDER_JOINT_FLAG_USE_LIMIT: {
+			limits_enabled = p_enabled;
+			_limits_changed();
+		} break;
+		case JoltPhysicsServer3D::SLIDER_JOINT_FLAG_USE_LIMIT_SPRING: {
+			limit_spring_enabled = p_enabled;
+			_limit_spring_changed();
+		} break;
+		case JoltPhysicsServer3D::SLIDER_JOINT_FLAG_ENABLE_MOTOR: {
+			motor_enabled = p_enabled;
+			_motor_state_changed();
+		} break;
+		default: {
+			ERR_FAIL_MSG(vformat("Unhandled flag: '%d'. This should not happen. Please report this.", p_flag));
+		} break;
+	}
+}
+
+float JoltSliderJoint3D::get_applied_force() const {
+	ERR_FAIL_NULL_V(jolt_ref, 0.0f);
+
+	JoltSpace3D *space = get_space();
+	ERR_FAIL_NULL_V(space, 0.0f);
+
+	const float last_step = space->get_last_step();
+	if (unlikely(last_step == 0.0f)) {
+		return 0.0f;
+	}
+
+	if (_is_fixed()) {
+		JPH::FixedConstraint *constraint = static_cast<JPH::FixedConstraint *>(jolt_ref.GetPtr());
+		return constraint->GetTotalLambdaPosition().Length() / last_step;
+	} else {
+		JPH::SliderConstraint *constraint = static_cast<JPH::SliderConstraint *>(jolt_ref.GetPtr());
+		const JPH::Vec3 total_lambda = JPH::Vec3(constraint->GetTotalLambdaPosition()[0], constraint->GetTotalLambdaPosition()[1], constraint->GetTotalLambdaPositionLimits() + constraint->GetTotalLambdaMotor());
+		return total_lambda.Length() / last_step;
+	}
+}
+
+float JoltSliderJoint3D::get_applied_torque() const {
+	ERR_FAIL_NULL_V(jolt_ref, 0.0f);
+
+	JoltSpace3D *space = get_space();
+	ERR_FAIL_NULL_V(space, 0.0f);
+
+	const float last_step = space->get_last_step();
+	if (unlikely(last_step == 0.0f)) {
+		return 0.0f;
+	}
+
+	if (_is_fixed()) {
+		JPH::FixedConstraint *constraint = static_cast<JPH::FixedConstraint *>(jolt_ref.GetPtr());
+		return constraint->GetTotalLambdaRotation().Length() / last_step;
+	} else {
+		JPH::SliderConstraint *constraint = static_cast<JPH::SliderConstraint *>(jolt_ref.GetPtr());
+		return constraint->GetTotalLambdaRotation().Length() / last_step;
+	}
+}
+
+void JoltSliderJoint3D::rebuild() {
+	destroy();
+
+	JoltSpace3D *space = get_space();
+
+	if (space == nullptr) {
+		return;
+	}
+
+	const JPH::BodyID body_ids[2] = {
+		body_a != nullptr ? body_a->get_jolt_id() : JPH::BodyID(),
+		body_b != nullptr ? body_b->get_jolt_id() : JPH::BodyID()
+	};
+
+	const JoltWritableBodies3D jolt_bodies = space->write_bodies(body_ids, 2);
+
+	JPH::Body *jolt_body_a = static_cast<JPH::Body *>(jolt_bodies[0]);
+	JPH::Body *jolt_body_b = static_cast<JPH::Body *>(jolt_bodies[1]);
+
+	ERR_FAIL_COND(jolt_body_a == nullptr && jolt_body_b == nullptr);
+
+	float ref_shift = 0.0f;
+	float limit = FLT_MAX;
+
+	if (limits_enabled && limit_lower <= limit_upper) {
+		const double limit_midpoint = (limit_lower + limit_upper) / 2.0f;
+
+		ref_shift = float(-limit_midpoint);
+		limit = float(limit_upper - limit_midpoint);
+	}
+
+	Transform3D shifted_ref_a;
+	Transform3D shifted_ref_b;
+
+	_shift_reference_frames(Vector3(ref_shift, 0.0f, 0.0f), Vector3(), shifted_ref_a, shifted_ref_b);
+
+	if (_is_fixed()) {
+		jolt_ref = _build_fixed(jolt_body_a, jolt_body_b, shifted_ref_a, shifted_ref_b);
+	} else {
+		jolt_ref = _build_slider(jolt_body_a, jolt_body_b, shifted_ref_a, shifted_ref_b, limit);
+	}
+
+	space->add_joint(this);
+
+	_update_enabled();
+	_update_iterations();
+	_update_motor_state();
+	_update_motor_velocity();
+	_update_motor_limit();
+}
diff --git a/modules/jolt_physics/joints/jolt_slider_joint_3d.h b/modules/jolt_physics/joints/jolt_slider_joint_3d.h
new file mode 100644
index 0000000000..31ed1a4c3a
--- /dev/null
+++ b/modules/jolt_physics/joints/jolt_slider_joint_3d.h
@@ -0,0 +1,96 @@
+/**************************************************************************/
+/*  jolt_slider_joint_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_SLIDER_JOINT_3D_H
+#define JOLT_SLIDER_JOINT_3D_H
+
+#include "../jolt_physics_server_3d.h"
+#include "jolt_joint_3d.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Constraints/SliderConstraint.h"
+
+class JoltSliderJoint3D final : public JoltJoint3D {
+	typedef PhysicsServer3D::SliderJointParam Parameter;
+	typedef JoltPhysicsServer3D::SliderJointParamJolt JoltParameter;
+	typedef JoltPhysicsServer3D::SliderJointFlagJolt JoltFlag;
+
+	double limit_upper = 0.0;
+	double limit_lower = 0.0;
+
+	double limit_spring_frequency = 0.0;
+	double limit_spring_damping = 0.0;
+
+	double motor_target_speed = 0.0f;
+	double motor_max_force = FLT_MAX;
+
+	bool limits_enabled = true;
+	bool limit_spring_enabled = false;
+
+	bool motor_enabled = false;
+
+	JPH::Constraint *_build_slider(JPH::Body *p_jolt_body_a, JPH::Body *p_jolt_body_b, const Transform3D &p_shifted_ref_a, const Transform3D &p_shifted_ref_b, float p_limit) const;
+	JPH::Constraint *_build_fixed(JPH::Body *p_jolt_body_a, JPH::Body *p_jolt_body_b, const Transform3D &p_shifted_ref_a, const Transform3D &p_shifted_ref_b) const;
+
+	bool _is_sprung() const { return limit_spring_enabled && limit_spring_frequency > 0.0; }
+	bool _is_fixed() const { return limits_enabled && limit_lower == limit_upper && !_is_sprung(); }
+
+	void _update_motor_state();
+	void _update_motor_velocity();
+	void _update_motor_limit();
+
+	void _limits_changed();
+	void _limit_spring_changed();
+	void _motor_state_changed();
+	void _motor_speed_changed();
+	void _motor_limit_changed();
+
+public:
+	JoltSliderJoint3D(const JoltJoint3D &p_old_joint, JoltBody3D *p_body_a, JoltBody3D *p_body_b, const Transform3D &p_local_ref_a, const Transform3D &p_local_ref_b);
+
+	virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_SLIDER; }
+
+	double get_param(PhysicsServer3D::SliderJointParam p_param) const;
+	void set_param(PhysicsServer3D::SliderJointParam p_param, double p_value);
+
+	double get_jolt_param(JoltParameter p_param) const;
+	void set_jolt_param(JoltParameter p_param, double p_value);
+
+	bool get_jolt_flag(JoltFlag p_flag) const;
+	void set_jolt_flag(JoltFlag p_flag, bool p_enabled);
+
+	float get_applied_force() const;
+	float get_applied_torque() const;
+
+	virtual void rebuild() override;
+};
+
+#endif // JOLT_SLIDER_JOINT_3D_H
diff --git a/modules/jolt_physics/jolt_globals.cpp b/modules/jolt_physics/jolt_globals.cpp
new file mode 100644
index 0000000000..e3e9fb19d0
--- /dev/null
+++ b/modules/jolt_physics/jolt_globals.cpp
@@ -0,0 +1,121 @@
+/**************************************************************************/
+/*  jolt_globals.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_globals.h"
+
+#include "objects/jolt_group_filter.h"
+#include "shapes/jolt_custom_double_sided_shape.h"
+#include "shapes/jolt_custom_ray_shape.h"
+#include "shapes/jolt_custom_user_data_shape.h"
+
+#include "core/os/memory.h"
+#include "core/string/print_string.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/RegisterTypes.h"
+
+#include <stdarg.h>
+
+void *jolt_alloc(size_t p_size) {
+	return Memory::alloc_static(p_size);
+}
+
+void *jolt_realloc(void *p_mem, size_t p_old_size, size_t p_new_size) {
+	return Memory::realloc_static(p_mem, p_new_size);
+}
+
+void jolt_free(void *p_mem) {
+	Memory::free_static(p_mem);
+}
+
+void *jolt_aligned_alloc(size_t p_size, size_t p_alignment) {
+	return Memory::alloc_aligned_static(p_size, p_alignment);
+}
+
+void jolt_aligned_free(void *p_mem) {
+	Memory::free_aligned_static(p_mem);
+}
+
+#ifdef JPH_ENABLE_ASSERTS
+
+void jolt_trace(const char *p_format, ...) {
+	va_list args;
+	va_start(args, p_format);
+	char buffer[1024] = { '\0' };
+	vsnprintf(buffer, sizeof(buffer), p_format, args);
+	va_end(args);
+	print_verbose(buffer);
+}
+
+bool jolt_assert(const char *p_expr, const char *p_msg, const char *p_file, uint32_t p_line) {
+	ERR_PRINT(vformat("Jolt Physics assertion '%s' failed with message '%s' at '%s:%d'", p_expr, p_msg != nullptr ? p_msg : "", p_file, p_line));
+	return false;
+}
+
+#endif
+
+void jolt_initialize() {
+	JPH::Allocate = &jolt_alloc;
+	JPH::Reallocate = &jolt_realloc;
+	JPH::Free = &jolt_free;
+	JPH::AlignedAllocate = &jolt_aligned_alloc;
+	JPH::AlignedFree = &jolt_aligned_free;
+
+#ifdef JPH_ENABLE_ASSERTS
+	JPH::Trace = &jolt_trace;
+	JPH::AssertFailed = &jolt_assert;
+#endif
+
+	JPH::Factory::sInstance = new JPH::Factory();
+
+	JPH::RegisterTypes();
+
+	JoltCustomRayShape::register_type();
+	JoltCustomUserDataShape::register_type();
+	JoltCustomDoubleSidedShape::register_type();
+
+	JoltGroupFilter::instance = new JoltGroupFilter();
+	JoltGroupFilter::instance->SetEmbedded();
+}
+
+void jolt_deinitialize() {
+	if (JoltGroupFilter::instance != nullptr) {
+		delete JoltGroupFilter::instance;
+		JoltGroupFilter::instance = nullptr;
+	}
+
+	JPH::UnregisterTypes();
+
+	if (JPH::Factory::sInstance != nullptr) {
+		delete JPH::Factory::sInstance;
+		JPH::Factory::sInstance = nullptr;
+	}
+}
diff --git a/modules/jolt_physics/jolt_globals.h b/modules/jolt_physics/jolt_globals.h
new file mode 100644
index 0000000000..fd3c7708f0
--- /dev/null
+++ b/modules/jolt_physics/jolt_globals.h
@@ -0,0 +1,37 @@
+/**************************************************************************/
+/*  jolt_globals.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_GLOBALS_H
+#define JOLT_GLOBALS_H
+
+void jolt_initialize();
+void jolt_deinitialize();
+
+#endif // JOLT_GLOBALS_H
diff --git a/modules/jolt_physics/jolt_physics_server_3d.cpp b/modules/jolt_physics/jolt_physics_server_3d.cpp
new file mode 100644
index 0000000000..d126f8bde7
--- /dev/null
+++ b/modules/jolt_physics/jolt_physics_server_3d.cpp
@@ -0,0 +1,1977 @@
+/**************************************************************************/
+/*  jolt_physics_server_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_server_3d.h"
+
+#include "joints/jolt_cone_twist_joint_3d.h"
+#include "joints/jolt_generic_6dof_joint_3d.h"
+#include "joints/jolt_hinge_joint_3d.h"
+#include "joints/jolt_joint_3d.h"
+#include "joints/jolt_pin_joint_3d.h"
+#include "joints/jolt_slider_joint_3d.h"
+#include "objects/jolt_area_3d.h"
+#include "objects/jolt_body_3d.h"
+#include "objects/jolt_soft_body_3d.h"
+#include "servers/physics_server_3d_wrap_mt.h"
+#include "shapes/jolt_box_shape_3d.h"
+#include "shapes/jolt_capsule_shape_3d.h"
+#include "shapes/jolt_concave_polygon_shape_3d.h"
+#include "shapes/jolt_convex_polygon_shape_3d.h"
+#include "shapes/jolt_cylinder_shape_3d.h"
+#include "shapes/jolt_height_map_shape_3d.h"
+#include "shapes/jolt_separation_ray_shape_3d.h"
+#include "shapes/jolt_sphere_shape_3d.h"
+#include "shapes/jolt_world_boundary_shape_3d.h"
+#include "spaces/jolt_job_system.h"
+#include "spaces/jolt_physics_direct_space_state_3d.h"
+#include "spaces/jolt_space_3d.h"
+
+JoltPhysicsServer3D::JoltPhysicsServer3D(bool p_on_separate_thread) :
+		on_separate_thread(p_on_separate_thread) {
+	singleton = this;
+}
+
+JoltPhysicsServer3D::~JoltPhysicsServer3D() {
+	if (singleton == this) {
+		singleton = nullptr;
+	}
+}
+
+RID JoltPhysicsServer3D::world_boundary_shape_create() {
+	JoltShape3D *shape = memnew(JoltWorldBoundaryShape3D);
+	RID rid = shape_owner.make_rid(shape);
+	shape->set_rid(rid);
+	return rid;
+}
+
+RID JoltPhysicsServer3D::separation_ray_shape_create() {
+	JoltShape3D *shape = memnew(JoltSeparationRayShape3D);
+	RID rid = shape_owner.make_rid(shape);
+	shape->set_rid(rid);
+	return rid;
+}
+
+RID JoltPhysicsServer3D::sphere_shape_create() {
+	JoltShape3D *shape = memnew(JoltSphereShape3D);
+	RID rid = shape_owner.make_rid(shape);
+	shape->set_rid(rid);
+	return rid;
+}
+
+RID JoltPhysicsServer3D::box_shape_create() {
+	JoltShape3D *shape = memnew(JoltBoxShape3D);
+	RID rid = shape_owner.make_rid(shape);
+	shape->set_rid(rid);
+	return rid;
+}
+
+RID JoltPhysicsServer3D::capsule_shape_create() {
+	JoltShape3D *shape = memnew(JoltCapsuleShape3D);
+	RID rid = shape_owner.make_rid(shape);
+	shape->set_rid(rid);
+	return rid;
+}
+
+RID JoltPhysicsServer3D::cylinder_shape_create() {
+	JoltShape3D *shape = memnew(JoltCylinderShape3D);
+	RID rid = shape_owner.make_rid(shape);
+	shape->set_rid(rid);
+	return rid;
+}
+
+RID JoltPhysicsServer3D::convex_polygon_shape_create() {
+	JoltShape3D *shape = memnew(JoltConvexPolygonShape3D);
+	RID rid = shape_owner.make_rid(shape);
+	shape->set_rid(rid);
+	return rid;
+}
+
+RID JoltPhysicsServer3D::concave_polygon_shape_create() {
+	JoltShape3D *shape = memnew(JoltConcavePolygonShape3D);
+	RID rid = shape_owner.make_rid(shape);
+	shape->set_rid(rid);
+	return rid;
+}
+
+RID JoltPhysicsServer3D::heightmap_shape_create() {
+	JoltShape3D *shape = memnew(JoltHeightMapShape3D);
+	RID rid = shape_owner.make_rid(shape);
+	shape->set_rid(rid);
+	return rid;
+}
+
+RID JoltPhysicsServer3D::custom_shape_create() {
+	ERR_FAIL_V_MSG(RID(), "Custom shapes are not supported.");
+}
+
+void JoltPhysicsServer3D::shape_set_data(RID p_shape, const Variant &p_data) {
+	JoltShape3D *shape = shape_owner.get_or_null(p_shape);
+	ERR_FAIL_NULL(shape);
+
+	shape->set_data(p_data);
+}
+
+Variant JoltPhysicsServer3D::shape_get_data(RID p_shape) const {
+	const JoltShape3D *shape = shape_owner.get_or_null(p_shape);
+	ERR_FAIL_NULL_V(shape, Variant());
+
+	return shape->get_data();
+}
+
+void JoltPhysicsServer3D::shape_set_custom_solver_bias(RID p_shape, real_t p_bias) {
+	JoltShape3D *shape = shape_owner.get_or_null(p_shape);
+	ERR_FAIL_NULL(shape);
+
+	shape->set_solver_bias((float)p_bias);
+}
+
+PhysicsServer3D::ShapeType JoltPhysicsServer3D::shape_get_type(RID p_shape) const {
+	const JoltShape3D *shape = shape_owner.get_or_null(p_shape);
+	ERR_FAIL_NULL_V(shape, SHAPE_CUSTOM);
+
+	return shape->get_type();
+}
+
+void JoltPhysicsServer3D::shape_set_margin(RID p_shape, real_t p_margin) {
+	JoltShape3D *shape = shape_owner.get_or_null(p_shape);
+	ERR_FAIL_NULL(shape);
+
+	shape->set_margin((float)p_margin);
+}
+
+real_t JoltPhysicsServer3D::shape_get_margin(RID p_shape) const {
+	const JoltShape3D *shape = shape_owner.get_or_null(p_shape);
+	ERR_FAIL_NULL_V(shape, 0.0);
+
+	return (real_t)shape->get_margin();
+}
+
+real_t JoltPhysicsServer3D::shape_get_custom_solver_bias(RID p_shape) const {
+	const JoltShape3D *shape = shape_owner.get_or_null(p_shape);
+	ERR_FAIL_NULL_V(shape, 0.0);
+
+	return (real_t)shape->get_solver_bias();
+}
+
+RID JoltPhysicsServer3D::space_create() {
+	JoltSpace3D *space = memnew(JoltSpace3D(job_system));
+	RID rid = space_owner.make_rid(space);
+	space->set_rid(rid);
+
+	const RID default_area_rid = area_create();
+	JoltArea3D *default_area = area_owner.get_or_null(default_area_rid);
+	ERR_FAIL_NULL_V(default_area, RID());
+	space->set_default_area(default_area);
+	default_area->set_space(space);
+
+	return rid;
+}
+
+void JoltPhysicsServer3D::space_set_active(RID p_space, bool p_active) {
+	JoltSpace3D *space = space_owner.get_or_null(p_space);
+	ERR_FAIL_NULL(space);
+
+	if (p_active) {
+		space->set_active(true);
+		active_spaces.insert(space);
+	} else {
+		space->set_active(false);
+		active_spaces.erase(space);
+	}
+}
+
+bool JoltPhysicsServer3D::space_is_active(RID p_space) const {
+	JoltSpace3D *space = space_owner.get_or_null(p_space);
+	ERR_FAIL_NULL_V(space, false);
+
+	return active_spaces.has(space);
+}
+
+void JoltPhysicsServer3D::space_set_param(RID p_space, SpaceParameter p_param, real_t p_value) {
+	JoltSpace3D *space = space_owner.get_or_null(p_space);
+	ERR_FAIL_NULL(space);
+
+	space->set_param(p_param, (double)p_value);
+}
+
+real_t JoltPhysicsServer3D::space_get_param(RID p_space, SpaceParameter p_param) const {
+	const JoltSpace3D *space = space_owner.get_or_null(p_space);
+	ERR_FAIL_NULL_V(space, 0.0);
+
+	return (real_t)space->get_param(p_param);
+}
+
+PhysicsDirectSpaceState3D *JoltPhysicsServer3D::space_get_direct_state(RID p_space) {
+	JoltSpace3D *space = space_owner.get_or_null(p_space);
+	ERR_FAIL_NULL_V(space, nullptr);
+	ERR_FAIL_COND_V_MSG((on_separate_thread && !doing_sync) || space->is_stepping(), nullptr, "Space state is inaccessible right now, wait for iteration or physics process notification.");
+
+	return space->get_direct_state();
+}
+
+void JoltPhysicsServer3D::space_set_debug_contacts(RID p_space, int p_max_contacts) {
+#ifdef DEBUG_ENABLED
+	JoltSpace3D *space = space_owner.get_or_null(p_space);
+	ERR_FAIL_NULL(space);
+
+	space->set_max_debug_contacts(p_max_contacts);
+#endif
+}
+
+PackedVector3Array JoltPhysicsServer3D::space_get_contacts(RID p_space) const {
+#ifdef DEBUG_ENABLED
+	JoltSpace3D *space = space_owner.get_or_null(p_space);
+	ERR_FAIL_NULL_V(space, PackedVector3Array());
+
+	return space->get_debug_contacts();
+#else
+	return PackedVector3Array();
+#endif
+}
+
+int JoltPhysicsServer3D::space_get_contact_count(RID p_space) const {
+#ifdef DEBUG_ENABLED
+	JoltSpace3D *space = space_owner.get_or_null(p_space);
+	ERR_FAIL_NULL_V(space, 0);
+
+	return space->get_debug_contact_count();
+#else
+	return 0;
+#endif
+}
+
+RID JoltPhysicsServer3D::area_create() {
+	JoltArea3D *area = memnew(JoltArea3D);
+	RID rid = area_owner.make_rid(area);
+	area->set_rid(rid);
+	return rid;
+}
+
+void JoltPhysicsServer3D::area_set_space(RID p_area, RID p_space) {
+	JoltArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+
+	JoltSpace3D *space = nullptr;
+
+	if (p_space.is_valid()) {
+		space = space_owner.get_or_null(p_space);
+		ERR_FAIL_NULL(space);
+	}
+
+	area->set_space(space);
+}
+
+RID JoltPhysicsServer3D::area_get_space(RID p_area) const {
+	const JoltArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL_V(area, RID());
+
+	const JoltSpace3D *space = area->get_space();
+
+	if (space == nullptr) {
+		return RID();
+	}
+
+	return space->get_rid();
+}
+
+void JoltPhysicsServer3D::area_add_shape(RID p_area, RID p_shape, const Transform3D &p_transform, bool p_disabled) {
+	JoltArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+
+	JoltShape3D *shape = shape_owner.get_or_null(p_shape);
+	ERR_FAIL_NULL(shape);
+
+	area->add_shape(shape, p_transform, p_disabled);
+}
+
+void JoltPhysicsServer3D::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) {
+	JoltArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+
+	JoltShape3D *shape = shape_owner.get_or_null(p_shape);
+	ERR_FAIL_NULL(shape);
+
+	area->set_shape(p_shape_idx, shape);
+}
+
+RID JoltPhysicsServer3D::area_get_shape(RID p_area, int p_shape_idx) const {
+	const JoltArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL_V(area, RID());
+
+	const JoltShape3D *shape = area->get_shape(p_shape_idx);
+	ERR_FAIL_NULL_V(shape, RID());
+
+	return shape->get_rid();
+}
+
+void JoltPhysicsServer3D::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform3D &p_transform) {
+	JoltArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+
+	area->set_shape_transform(p_shape_idx, p_transform);
+}
+
+Transform3D JoltPhysicsServer3D::area_get_shape_transform(RID p_area, int p_shape_idx) const {
+	const JoltArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL_V(area, Transform3D());
+
+	return area->get_shape_transform_scaled(p_shape_idx);
+}
+
+int JoltPhysicsServer3D::area_get_shape_count(RID p_area) const {
+	const JoltArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL_V(area, 0);
+
+	return area->get_shape_count();
+}
+
+void JoltPhysicsServer3D::area_remove_shape(RID p_area, int p_shape_idx) {
+	JoltArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+
+	area->remove_shape(p_shape_idx);
+}
+
+void JoltPhysicsServer3D::area_clear_shapes(RID p_area) {
+	JoltArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+
+	area->clear_shapes();
+}
+
+void JoltPhysicsServer3D::area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) {
+	JoltArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+
+	area->set_shape_disabled(p_shape_idx, p_disabled);
+}
+
+void JoltPhysicsServer3D::area_attach_object_instance_id(RID p_area, ObjectID p_id) {
+	RID area_rid = p_area;
+
+	if (space_owner.owns(area_rid)) {
+		const JoltSpace3D *space = space_owner.get_or_null(area_rid);
+		area_rid = space->get_default_area()->get_rid();
+	}
+
+	JoltArea3D *area = area_owner.get_or_null(area_rid);
+	ERR_FAIL_NULL(area);
+
+	area->set_instance_id(p_id);
+}
+
+ObjectID JoltPhysicsServer3D::area_get_object_instance_id(RID p_area) const {
+	RID area_rid = p_area;
+
+	if (space_owner.owns(area_rid)) {
+		const JoltSpace3D *space = space_owner.get_or_null(area_rid);
+		area_rid = space->get_default_area()->get_rid();
+	}
+
+	JoltArea3D *area = area_owner.get_or_null(area_rid);
+	ERR_FAIL_NULL_V(area, ObjectID());
+
+	return area->get_instance_id();
+}
+
+void JoltPhysicsServer3D::area_set_param(RID p_area, AreaParameter p_param, const Variant &p_value) {
+	RID area_rid = p_area;
+
+	if (space_owner.owns(area_rid)) {
+		const JoltSpace3D *space = space_owner.get_or_null(area_rid);
+		area_rid = space->get_default_area()->get_rid();
+	}
+
+	JoltArea3D *area = area_owner.get_or_null(area_rid);
+	ERR_FAIL_NULL(area);
+
+	area->set_param(p_param, p_value);
+}
+
+Variant JoltPhysicsServer3D::area_get_param(RID p_area, AreaParameter p_param) const {
+	RID area_rid = p_area;
+
+	if (space_owner.owns(area_rid)) {
+		const JoltSpace3D *space = space_owner.get_or_null(area_rid);
+		area_rid = space->get_default_area()->get_rid();
+	}
+
+	JoltArea3D *area = area_owner.get_or_null(area_rid);
+	ERR_FAIL_NULL_V(area, Variant());
+
+	return area->get_param(p_param);
+}
+
+void JoltPhysicsServer3D::area_set_transform(RID p_area, const Transform3D &p_transform) {
+	JoltArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+
+	return area->set_transform(p_transform);
+}
+
+Transform3D JoltPhysicsServer3D::area_get_transform(RID p_area) const {
+	const JoltArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL_V(area, Transform3D());
+
+	return area->get_transform_scaled();
+}
+
+void JoltPhysicsServer3D::area_set_collision_mask(RID p_area, uint32_t p_mask) {
+	JoltArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+
+	area->set_collision_mask(p_mask);
+}
+
+uint32_t JoltPhysicsServer3D::area_get_collision_mask(RID p_area) const {
+	const JoltArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL_V(area, 0);
+
+	return area->get_collision_mask();
+}
+
+void JoltPhysicsServer3D::area_set_collision_layer(RID p_area, uint32_t p_layer) {
+	JoltArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+
+	area->set_collision_layer(p_layer);
+}
+
+uint32_t JoltPhysicsServer3D::area_get_collision_layer(RID p_area) const {
+	const JoltArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL_V(area, 0);
+
+	return area->get_collision_layer();
+}
+
+void JoltPhysicsServer3D::area_set_monitorable(RID p_area, bool p_monitorable) {
+	JoltArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+
+	area->set_monitorable(p_monitorable);
+}
+
+void JoltPhysicsServer3D::area_set_monitor_callback(RID p_area, const Callable &p_callback) {
+	JoltArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+
+	area->set_body_monitor_callback(p_callback);
+}
+
+void JoltPhysicsServer3D::area_set_area_monitor_callback(RID p_area, const Callable &p_callback) {
+	JoltArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+
+	area->set_area_monitor_callback(p_callback);
+}
+
+void JoltPhysicsServer3D::area_set_ray_pickable(RID p_area, bool p_enable) {
+	JoltArea3D *area = area_owner.get_or_null(p_area);
+	ERR_FAIL_NULL(area);
+
+	area->set_pickable(p_enable);
+}
+
+RID JoltPhysicsServer3D::body_create() {
+	JoltBody3D *body = memnew(JoltBody3D);
+	RID rid = body_owner.make_rid(body);
+	body->set_rid(rid);
+	return rid;
+}
+
+void JoltPhysicsServer3D::body_set_space(RID p_body, RID p_space) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	JoltSpace3D *space = nullptr;
+
+	if (p_space.is_valid()) {
+		space = space_owner.get_or_null(p_space);
+		ERR_FAIL_NULL(space);
+	}
+
+	body->set_space(space);
+}
+
+RID JoltPhysicsServer3D::body_get_space(RID p_body) const {
+	const JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, RID());
+
+	const JoltSpace3D *space = body->get_space();
+
+	if (space == nullptr) {
+		return RID();
+	}
+
+	return space->get_rid();
+}
+
+void JoltPhysicsServer3D::body_set_mode(RID p_body, BodyMode p_mode) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_mode(p_mode);
+}
+
+PhysicsServer3D::BodyMode JoltPhysicsServer3D::body_get_mode(RID p_body) const {
+	const JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, BODY_MODE_STATIC);
+
+	return body->get_mode();
+}
+
+void JoltPhysicsServer3D::body_add_shape(RID p_body, RID p_shape, const Transform3D &p_transform, bool p_disabled) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	JoltShape3D *shape = shape_owner.get_or_null(p_shape);
+	ERR_FAIL_NULL(shape);
+
+	body->add_shape(shape, p_transform, p_disabled);
+}
+
+void JoltPhysicsServer3D::body_set_shape(RID p_body, int p_shape_idx, RID p_shape) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	JoltShape3D *shape = shape_owner.get_or_null(p_shape);
+	ERR_FAIL_NULL(shape);
+
+	body->set_shape(p_shape_idx, shape);
+}
+
+RID JoltPhysicsServer3D::body_get_shape(RID p_body, int p_shape_idx) const {
+	const JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, RID());
+
+	const JoltShape3D *shape = body->get_shape(p_shape_idx);
+	ERR_FAIL_NULL_V(shape, RID());
+
+	return shape->get_rid();
+}
+
+void JoltPhysicsServer3D::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform3D &p_transform) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_shape_transform(p_shape_idx, p_transform);
+}
+
+Transform3D JoltPhysicsServer3D::body_get_shape_transform(RID p_body, int p_shape_idx) const {
+	const JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, Transform3D());
+
+	return body->get_shape_transform_scaled(p_shape_idx);
+}
+
+int JoltPhysicsServer3D::body_get_shape_count(RID p_body) const {
+	const JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, 0);
+
+	return body->get_shape_count();
+}
+
+void JoltPhysicsServer3D::body_remove_shape(RID p_body, int p_shape_idx) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->remove_shape(p_shape_idx);
+}
+
+void JoltPhysicsServer3D::body_clear_shapes(RID p_body) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->clear_shapes();
+}
+
+void JoltPhysicsServer3D::body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_shape_disabled(p_shape_idx, p_disabled);
+}
+
+void JoltPhysicsServer3D::body_attach_object_instance_id(RID p_body, ObjectID p_id) {
+	if (JoltBody3D *body = body_owner.get_or_null(p_body)) {
+		body->set_instance_id(p_id);
+	} else if (JoltSoftBody3D *soft_body = soft_body_owner.get_or_null(p_body)) {
+		soft_body->set_instance_id(p_id);
+	} else {
+		ERR_FAIL();
+	}
+}
+
+ObjectID JoltPhysicsServer3D::body_get_object_instance_id(RID p_body) const {
+	const JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, ObjectID());
+
+	return body->get_instance_id();
+}
+
+void JoltPhysicsServer3D::body_set_enable_continuous_collision_detection(RID p_body, bool p_enable) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_ccd_enabled(p_enable);
+}
+
+bool JoltPhysicsServer3D::body_is_continuous_collision_detection_enabled(RID p_body) const {
+	const JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, false);
+
+	return body->is_ccd_enabled();
+}
+
+void JoltPhysicsServer3D::body_set_collision_layer(RID p_body, uint32_t p_layer) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_collision_layer(p_layer);
+}
+
+uint32_t JoltPhysicsServer3D::body_get_collision_layer(RID p_body) const {
+	const JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, 0);
+
+	return body->get_collision_layer();
+}
+
+void JoltPhysicsServer3D::body_set_collision_mask(RID p_body, uint32_t p_mask) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_collision_mask(p_mask);
+}
+
+uint32_t JoltPhysicsServer3D::body_get_collision_mask(RID p_body) const {
+	const JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, 0);
+
+	return body->get_collision_mask();
+}
+
+void JoltPhysicsServer3D::body_set_collision_priority(RID p_body, real_t p_priority) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_collision_priority((float)p_priority);
+}
+
+real_t JoltPhysicsServer3D::body_get_collision_priority(RID p_body) const {
+	const JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, 0.0);
+
+	return (real_t)body->get_collision_priority();
+}
+
+void JoltPhysicsServer3D::body_set_user_flags(RID p_body, uint32_t p_flags) {
+	WARN_PRINT("Body user flags are not supported. Any such value will be ignored.");
+}
+
+uint32_t JoltPhysicsServer3D::body_get_user_flags(RID p_body) const {
+	return 0;
+}
+
+void JoltPhysicsServer3D::body_set_param(RID p_body, BodyParameter p_param, const Variant &p_value) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_param(p_param, p_value);
+}
+
+Variant JoltPhysicsServer3D::body_get_param(RID p_body, BodyParameter p_param) const {
+	const JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, Variant());
+
+	return body->get_param(p_param);
+}
+
+void JoltPhysicsServer3D::body_reset_mass_properties(RID p_body) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->reset_mass_properties();
+}
+
+void JoltPhysicsServer3D::body_set_state(RID p_body, BodyState p_state, const Variant &p_value) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_state(p_state, p_value);
+}
+
+Variant JoltPhysicsServer3D::body_get_state(RID p_body, BodyState p_state) const {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, Variant());
+
+	return body->get_state(p_state);
+}
+
+void JoltPhysicsServer3D::body_apply_central_impulse(RID p_body, const Vector3 &p_impulse) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	return body->apply_central_impulse(p_impulse);
+}
+
+void JoltPhysicsServer3D::body_apply_impulse(RID p_body, const Vector3 &p_impulse, const Vector3 &p_position) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	return body->apply_impulse(p_impulse, p_position);
+}
+
+void JoltPhysicsServer3D::body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	return body->apply_torque_impulse(p_impulse);
+}
+
+void JoltPhysicsServer3D::body_apply_central_force(RID p_body, const Vector3 &p_force) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	return body->apply_central_force(p_force);
+}
+
+void JoltPhysicsServer3D::body_apply_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	return body->apply_force(p_force, p_position);
+}
+
+void JoltPhysicsServer3D::body_apply_torque(RID p_body, const Vector3 &p_torque) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	return body->apply_torque(p_torque);
+}
+
+void JoltPhysicsServer3D::body_add_constant_central_force(RID p_body, const Vector3 &p_force) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->add_constant_central_force(p_force);
+}
+
+void JoltPhysicsServer3D::body_add_constant_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->add_constant_force(p_force, p_position);
+}
+
+void JoltPhysicsServer3D::body_add_constant_torque(RID p_body, const Vector3 &p_torque) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->add_constant_torque(p_torque);
+}
+
+void JoltPhysicsServer3D::body_set_constant_force(RID p_body, const Vector3 &p_force) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_constant_force(p_force);
+}
+
+Vector3 JoltPhysicsServer3D::body_get_constant_force(RID p_body) const {
+	const JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, Vector3());
+
+	return body->get_constant_force();
+}
+
+void JoltPhysicsServer3D::body_set_constant_torque(RID p_body, const Vector3 &p_torque) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_constant_torque(p_torque);
+}
+
+Vector3 JoltPhysicsServer3D::body_get_constant_torque(RID p_body) const {
+	const JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, Vector3());
+
+	return body->get_constant_torque();
+}
+
+void JoltPhysicsServer3D::body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_axis_velocity(p_axis_velocity);
+}
+
+void JoltPhysicsServer3D::body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_axis_lock(p_axis, p_lock);
+}
+
+bool JoltPhysicsServer3D::body_is_axis_locked(RID p_body, BodyAxis p_axis) const {
+	const JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, false);
+
+	return body->is_axis_locked(p_axis);
+}
+
+void JoltPhysicsServer3D::body_add_collision_exception(RID p_body, RID p_excepted_body) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->add_collision_exception(p_excepted_body);
+}
+
+void JoltPhysicsServer3D::body_remove_collision_exception(RID p_body, RID p_excepted_body) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->remove_collision_exception(p_excepted_body);
+}
+
+void JoltPhysicsServer3D::body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) {
+	const JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	for (const RID &exception : body->get_collision_exceptions()) {
+		p_exceptions->push_back(exception);
+	}
+}
+
+void JoltPhysicsServer3D::body_set_max_contacts_reported(RID p_body, int p_amount) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	return body->set_max_contacts_reported(p_amount);
+}
+
+int JoltPhysicsServer3D::body_get_max_contacts_reported(RID p_body) const {
+	const JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, 0);
+
+	return body->get_max_contacts_reported();
+}
+
+void JoltPhysicsServer3D::body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) {
+	WARN_PRINT("Per-body contact depth threshold is not supported. Any such value will be ignored.");
+}
+
+real_t JoltPhysicsServer3D::body_get_contacts_reported_depth_threshold(RID p_body) const {
+	return 0.0;
+}
+
+void JoltPhysicsServer3D::body_set_omit_force_integration(RID p_body, bool p_enable) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_custom_integrator(p_enable);
+}
+
+bool JoltPhysicsServer3D::body_is_omitting_force_integration(RID p_body) const {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, false);
+
+	return body->has_custom_integrator();
+}
+
+void JoltPhysicsServer3D::body_set_state_sync_callback(RID p_body, const Callable &p_callable) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_state_sync_callback(p_callable);
+}
+
+void JoltPhysicsServer3D::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_userdata) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_custom_integration_callback(p_callable, p_userdata);
+}
+
+void JoltPhysicsServer3D::body_set_ray_pickable(RID p_body, bool p_enable) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_pickable(p_enable);
+}
+
+bool JoltPhysicsServer3D::body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result) {
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, false);
+
+	JoltSpace3D *space = body->get_space();
+	ERR_FAIL_NULL_V(space, false);
+
+	return space->get_direct_state()->body_test_motion(*body, p_parameters, r_result);
+}
+
+PhysicsDirectBodyState3D *JoltPhysicsServer3D::body_get_direct_state(RID p_body) {
+	ERR_FAIL_COND_V_MSG((on_separate_thread && !doing_sync), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification.");
+
+	JoltBody3D *body = body_owner.get_or_null(p_body);
+	if (unlikely(body == nullptr || body->get_space() == nullptr)) {
+		return nullptr;
+	}
+
+	ERR_FAIL_COND_V_MSG(body->get_space()->is_stepping(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification.");
+
+	return body->get_direct_state();
+}
+
+RID JoltPhysicsServer3D::soft_body_create() {
+	JoltSoftBody3D *body = memnew(JoltSoftBody3D);
+	RID rid = soft_body_owner.make_rid(body);
+	body->set_rid(rid);
+	return rid;
+}
+
+void JoltPhysicsServer3D::soft_body_update_rendering_server(RID p_body, PhysicsServer3DRenderingServerHandler *p_rendering_server_handler) {
+	JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	return body->update_rendering_server(p_rendering_server_handler);
+}
+
+void JoltPhysicsServer3D::soft_body_set_space(RID p_body, RID p_space) {
+	JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	JoltSpace3D *space = nullptr;
+
+	if (p_space.is_valid()) {
+		space = space_owner.get_or_null(p_space);
+		ERR_FAIL_NULL(space);
+	}
+
+	body->set_space(space);
+}
+
+RID JoltPhysicsServer3D::soft_body_get_space(RID p_body) const {
+	const JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, RID());
+
+	const JoltSpace3D *space = body->get_space();
+
+	if (space == nullptr) {
+		return RID();
+	}
+
+	return space->get_rid();
+}
+
+void JoltPhysicsServer3D::soft_body_set_mesh(RID p_body, RID p_mesh) {
+	JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_mesh(p_mesh);
+}
+
+AABB JoltPhysicsServer3D::soft_body_get_bounds(RID p_body) const {
+	const JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, AABB());
+
+	return body->get_bounds();
+}
+
+void JoltPhysicsServer3D::soft_body_set_collision_layer(RID p_body, uint32_t p_layer) {
+	JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_collision_layer(p_layer);
+}
+
+uint32_t JoltPhysicsServer3D::soft_body_get_collision_layer(RID p_body) const {
+	const JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, 0);
+
+	return body->get_collision_layer();
+}
+
+void JoltPhysicsServer3D::soft_body_set_collision_mask(RID p_body, uint32_t p_mask) {
+	JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_collision_mask(p_mask);
+}
+
+uint32_t JoltPhysicsServer3D::soft_body_get_collision_mask(RID p_body) const {
+	const JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, 0);
+
+	return body->get_collision_mask();
+}
+
+void JoltPhysicsServer3D::soft_body_add_collision_exception(RID p_body, RID p_excepted_body) {
+	JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->add_collision_exception(p_excepted_body);
+}
+
+void JoltPhysicsServer3D::soft_body_remove_collision_exception(RID p_body, RID p_excepted_body) {
+	JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->remove_collision_exception(p_excepted_body);
+}
+
+void JoltPhysicsServer3D::soft_body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) {
+	const JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	for (const RID &exception : body->get_collision_exceptions()) {
+		p_exceptions->push_back(exception);
+	}
+}
+
+void JoltPhysicsServer3D::soft_body_set_state(RID p_body, BodyState p_state, const Variant &p_value) {
+	JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_state(p_state, p_value);
+}
+
+Variant JoltPhysicsServer3D::soft_body_get_state(RID p_body, BodyState p_state) const {
+	JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, Variant());
+
+	return body->get_state(p_state);
+}
+
+void JoltPhysicsServer3D::soft_body_set_transform(RID p_body, const Transform3D &p_transform) {
+	JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	return body->set_transform(p_transform);
+}
+
+void JoltPhysicsServer3D::soft_body_set_ray_pickable(RID p_body, bool p_enable) {
+	JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	return body->set_pickable(p_enable);
+}
+
+void JoltPhysicsServer3D::soft_body_set_simulation_precision(RID p_body, int p_precision) {
+	JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	return body->set_simulation_precision(p_precision);
+}
+
+int JoltPhysicsServer3D::soft_body_get_simulation_precision(RID p_body) const {
+	JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, 0);
+
+	return body->get_simulation_precision();
+}
+
+void JoltPhysicsServer3D::soft_body_set_total_mass(RID p_body, real_t p_total_mass) {
+	JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	return body->set_mass((float)p_total_mass);
+}
+
+real_t JoltPhysicsServer3D::soft_body_get_total_mass(RID p_body) const {
+	JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, 0.0);
+
+	return (real_t)body->get_mass();
+}
+
+void JoltPhysicsServer3D::soft_body_set_linear_stiffness(RID p_body, real_t p_coefficient) {
+	JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	return body->set_stiffness_coefficient((float)p_coefficient);
+}
+
+real_t JoltPhysicsServer3D::soft_body_get_linear_stiffness(RID p_body) const {
+	JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, 0.0);
+
+	return (real_t)body->get_stiffness_coefficient();
+}
+
+void JoltPhysicsServer3D::soft_body_set_pressure_coefficient(RID p_body, real_t p_coefficient) {
+	JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	return body->set_pressure((float)p_coefficient);
+}
+
+real_t JoltPhysicsServer3D::soft_body_get_pressure_coefficient(RID p_body) const {
+	JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, 0.0);
+
+	return (real_t)body->get_pressure();
+}
+
+void JoltPhysicsServer3D::soft_body_set_damping_coefficient(RID p_body, real_t p_coefficient) {
+	JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	return body->set_linear_damping((float)p_coefficient);
+}
+
+real_t JoltPhysicsServer3D::soft_body_get_damping_coefficient(RID p_body) const {
+	JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, 0.0);
+
+	return (real_t)body->get_linear_damping();
+}
+
+void JoltPhysicsServer3D::soft_body_set_drag_coefficient(RID p_body, real_t p_coefficient) {
+	JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	return body->set_drag((float)p_coefficient);
+}
+
+real_t JoltPhysicsServer3D::soft_body_get_drag_coefficient(RID p_body) const {
+	JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, 0.0);
+
+	return (real_t)body->get_drag();
+}
+
+void JoltPhysicsServer3D::soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) {
+	JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->set_vertex_position(p_point_index, p_global_position);
+}
+
+Vector3 JoltPhysicsServer3D::soft_body_get_point_global_position(RID p_body, int p_point_index) const {
+	JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, Vector3());
+
+	return body->get_vertex_position(p_point_index);
+}
+
+void JoltPhysicsServer3D::soft_body_remove_all_pinned_points(RID p_body) {
+	JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	body->unpin_all_vertices();
+}
+
+void JoltPhysicsServer3D::soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) {
+	JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL(body);
+
+	if (p_pin) {
+		body->pin_vertex(p_point_index);
+	} else {
+		body->unpin_vertex(p_point_index);
+	}
+}
+
+bool JoltPhysicsServer3D::soft_body_is_point_pinned(RID p_body, int p_point_index) const {
+	JoltSoftBody3D *body = soft_body_owner.get_or_null(p_body);
+	ERR_FAIL_NULL_V(body, false);
+
+	return body->is_vertex_pinned(p_point_index);
+}
+
+RID JoltPhysicsServer3D::joint_create() {
+	JoltJoint3D *joint = memnew(JoltJoint3D);
+	RID rid = joint_owner.make_rid(joint);
+	joint->set_rid(rid);
+	return rid;
+}
+
+void JoltPhysicsServer3D::joint_clear(RID p_joint) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
+
+	if (joint->get_type() != JOINT_TYPE_MAX) {
+		JoltJoint3D *empty_joint = memnew(JoltJoint3D);
+		empty_joint->set_rid(joint->get_rid());
+
+		memdelete(joint);
+		joint = nullptr;
+
+		joint_owner.replace(p_joint, empty_joint);
+	}
+}
+
+void JoltPhysicsServer3D::joint_make_pin(RID p_joint, RID p_body_a, const Vector3 &p_local_a, RID p_body_b, const Vector3 &p_local_b) {
+	JoltJoint3D *old_joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(old_joint);
+
+	JoltBody3D *body_a = body_owner.get_or_null(p_body_a);
+	ERR_FAIL_NULL(body_a);
+
+	JoltBody3D *body_b = body_owner.get_or_null(p_body_b);
+	ERR_FAIL_COND(body_a == body_b);
+
+	JoltJoint3D *new_joint = memnew(JoltPinJoint3D(*old_joint, body_a, body_b, p_local_a, p_local_b));
+
+	memdelete(old_joint);
+	old_joint = nullptr;
+
+	joint_owner.replace(p_joint, new_joint);
+}
+
+void JoltPhysicsServer3D::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
+
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN);
+	JoltPinJoint3D *pin_joint = static_cast<JoltPinJoint3D *>(joint);
+
+	pin_joint->set_param(p_param, (double)p_value);
+}
+
+real_t JoltPhysicsServer3D::pin_joint_get_param(RID p_joint, PinJointParam p_param) const {
+	const JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, 0.0);
+
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, 0.0);
+	const JoltPinJoint3D *pin_joint = static_cast<const JoltPinJoint3D *>(joint);
+
+	return (real_t)pin_joint->get_param(p_param);
+}
+
+void JoltPhysicsServer3D::pin_joint_set_local_a(RID p_joint, const Vector3 &p_local_a) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
+
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN);
+	JoltPinJoint3D *pin_joint = static_cast<JoltPinJoint3D *>(joint);
+
+	pin_joint->set_local_a(p_local_a);
+}
+
+Vector3 JoltPhysicsServer3D::pin_joint_get_local_a(RID p_joint) const {
+	const JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, Vector3());
+
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, Vector3());
+	const JoltPinJoint3D *pin_joint = static_cast<const JoltPinJoint3D *>(joint);
+
+	return pin_joint->get_local_a();
+}
+
+void JoltPhysicsServer3D::pin_joint_set_local_b(RID p_joint, const Vector3 &p_local_b) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
+
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN);
+	JoltPinJoint3D *pin_joint = static_cast<JoltPinJoint3D *>(joint);
+
+	pin_joint->set_local_b(p_local_b);
+}
+
+Vector3 JoltPhysicsServer3D::pin_joint_get_local_b(RID p_joint) const {
+	const JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, Vector3());
+
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, Vector3());
+	const JoltPinJoint3D *pin_joint = static_cast<const JoltPinJoint3D *>(joint);
+
+	return pin_joint->get_local_b();
+}
+
+void JoltPhysicsServer3D::joint_make_hinge(RID p_joint, RID p_body_a, const Transform3D &p_hinge_a, RID p_body_b, const Transform3D &p_hinge_b) {
+	JoltJoint3D *old_joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(old_joint);
+
+	JoltBody3D *body_a = body_owner.get_or_null(p_body_a);
+	ERR_FAIL_NULL(body_a);
+
+	JoltBody3D *body_b = body_owner.get_or_null(p_body_b);
+	ERR_FAIL_COND(body_a == body_b);
+
+	JoltJoint3D *new_joint = memnew(JoltHingeJoint3D(*old_joint, body_a, body_b, p_hinge_a, p_hinge_b));
+
+	memdelete(old_joint);
+	old_joint = nullptr;
+
+	joint_owner.replace(p_joint, new_joint);
+}
+
+void JoltPhysicsServer3D::joint_make_hinge_simple(RID p_joint, RID p_body_a, const Vector3 &p_pivot_a, const Vector3 &p_axis_a, RID p_body_b, const Vector3 &p_pivot_b, const Vector3 &p_axis_b) {
+	ERR_FAIL_MSG("Simple hinge joints are not supported when using Jolt Physics.");
+}
+
+void JoltPhysicsServer3D::hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
+
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_HINGE);
+	JoltHingeJoint3D *hinge_joint = static_cast<JoltHingeJoint3D *>(joint);
+
+	return hinge_joint->set_param(p_param, (double)p_value);
+}
+
+real_t JoltPhysicsServer3D::hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const {
+	const JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, 0.0);
+
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, 0.0);
+	const JoltHingeJoint3D *hinge_joint = static_cast<const JoltHingeJoint3D *>(joint);
+
+	return (real_t)hinge_joint->get_param(p_param);
+}
+
+void JoltPhysicsServer3D::hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_enabled) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
+
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_HINGE);
+	JoltHingeJoint3D *hinge_joint = static_cast<JoltHingeJoint3D *>(joint);
+
+	return hinge_joint->set_flag(p_flag, p_enabled);
+}
+
+bool JoltPhysicsServer3D::hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const {
+	const JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, false);
+
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, false);
+	const JoltHingeJoint3D *hinge_joint = static_cast<const JoltHingeJoint3D *>(joint);
+
+	return hinge_joint->get_flag(p_flag);
+}
+
+void JoltPhysicsServer3D::joint_make_slider(RID p_joint, RID p_body_a, const Transform3D &p_local_ref_a, RID p_body_b, const Transform3D &p_local_ref_b) {
+	JoltJoint3D *old_joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(old_joint);
+
+	JoltBody3D *body_a = body_owner.get_or_null(p_body_a);
+	ERR_FAIL_NULL(body_a);
+
+	JoltBody3D *body_b = body_owner.get_or_null(p_body_b);
+	ERR_FAIL_COND(body_a == body_b);
+
+	JoltJoint3D *new_joint = memnew(JoltSliderJoint3D(*old_joint, body_a, body_b, p_local_ref_a, p_local_ref_b));
+
+	memdelete(old_joint);
+	old_joint = nullptr;
+
+	joint_owner.replace(p_joint, new_joint);
+}
+
+void JoltPhysicsServer3D::slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
+
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_SLIDER);
+	JoltSliderJoint3D *slider_joint = static_cast<JoltSliderJoint3D *>(joint);
+
+	return slider_joint->set_param(p_param, (real_t)p_value);
+}
+
+real_t JoltPhysicsServer3D::slider_joint_get_param(RID p_joint, SliderJointParam p_param) const {
+	const JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, 0.0);
+
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_SLIDER, 0.0);
+	const JoltSliderJoint3D *slider_joint = static_cast<const JoltSliderJoint3D *>(joint);
+
+	return slider_joint->get_param(p_param);
+}
+
+void JoltPhysicsServer3D::joint_make_cone_twist(RID p_joint, RID p_body_a, const Transform3D &p_local_ref_a, RID p_body_b, const Transform3D &p_local_ref_b) {
+	JoltJoint3D *old_joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(old_joint);
+
+	JoltBody3D *body_a = body_owner.get_or_null(p_body_a);
+	ERR_FAIL_NULL(body_a);
+
+	JoltBody3D *body_b = body_owner.get_or_null(p_body_b);
+	ERR_FAIL_COND(body_a == body_b);
+
+	JoltJoint3D *new_joint = memnew(JoltConeTwistJoint3D(*old_joint, body_a, body_b, p_local_ref_a, p_local_ref_b));
+
+	memdelete(old_joint);
+	old_joint = nullptr;
+
+	joint_owner.replace(p_joint, new_joint);
+}
+
+void JoltPhysicsServer3D::cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
+
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_CONE_TWIST);
+	JoltConeTwistJoint3D *cone_twist_joint = static_cast<JoltConeTwistJoint3D *>(joint);
+
+	return cone_twist_joint->set_param(p_param, (double)p_value);
+}
+
+real_t JoltPhysicsServer3D::cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const {
+	const JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, 0.0);
+
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_CONE_TWIST, 0.0);
+	const JoltConeTwistJoint3D *cone_twist_joint = static_cast<const JoltConeTwistJoint3D *>(joint);
+
+	return (real_t)cone_twist_joint->get_param(p_param);
+}
+
+void JoltPhysicsServer3D::joint_make_generic_6dof(RID p_joint, RID p_body_a, const Transform3D &p_local_ref_a, RID p_body_b, const Transform3D &p_local_ref_b) {
+	JoltJoint3D *old_joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(old_joint);
+
+	JoltBody3D *body_a = body_owner.get_or_null(p_body_a);
+	ERR_FAIL_NULL(body_a);
+
+	JoltBody3D *body_b = body_owner.get_or_null(p_body_b);
+	ERR_FAIL_COND(body_a == body_b);
+
+	JoltJoint3D *new_joint = memnew(JoltGeneric6DOFJoint3D(*old_joint, body_a, body_b, p_local_ref_a, p_local_ref_b));
+
+	memdelete(old_joint);
+	old_joint = nullptr;
+
+	joint_owner.replace(p_joint, new_joint);
+}
+
+void JoltPhysicsServer3D::generic_6dof_joint_set_param(RID p_joint, Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param, real_t p_value) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
+
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_6DOF);
+	JoltGeneric6DOFJoint3D *g6dof_joint = static_cast<JoltGeneric6DOFJoint3D *>(joint);
+
+	return g6dof_joint->set_param(p_axis, p_param, (double)p_value);
+}
+
+real_t JoltPhysicsServer3D::generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param) const {
+	const JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, 0.0);
+
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, 0.0);
+	const JoltGeneric6DOFJoint3D *g6dof_joint = static_cast<const JoltGeneric6DOFJoint3D *>(joint);
+
+	return (real_t)g6dof_joint->get_param(p_axis, p_param);
+}
+
+void JoltPhysicsServer3D::generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag, bool p_enable) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
+
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_6DOF);
+	JoltGeneric6DOFJoint3D *g6dof_joint = static_cast<JoltGeneric6DOFJoint3D *>(joint);
+
+	return g6dof_joint->set_flag(p_axis, p_flag, p_enable);
+}
+
+bool JoltPhysicsServer3D::generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const {
+	const JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, false);
+
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, false);
+	const JoltGeneric6DOFJoint3D *g6dof_joint = static_cast<const JoltGeneric6DOFJoint3D *>(joint);
+
+	return g6dof_joint->get_flag(p_axis, p_flag);
+}
+
+PhysicsServer3D::JointType JoltPhysicsServer3D::joint_get_type(RID p_joint) const {
+	const JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, JOINT_TYPE_PIN);
+
+	return joint->get_type();
+}
+
+void JoltPhysicsServer3D::joint_set_solver_priority(RID p_joint, int p_priority) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
+
+	joint->set_solver_priority(p_priority);
+}
+
+int JoltPhysicsServer3D::joint_get_solver_priority(RID p_joint) const {
+	const JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, 0);
+
+	return joint->get_solver_priority();
+}
+
+void JoltPhysicsServer3D::joint_disable_collisions_between_bodies(RID p_joint, bool p_disable) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
+
+	joint->set_collision_disabled(p_disable);
+}
+
+bool JoltPhysicsServer3D::joint_is_disabled_collisions_between_bodies(RID p_joint) const {
+	const JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, false);
+
+	return joint->is_collision_disabled();
+}
+
+void JoltPhysicsServer3D::free(RID p_rid) {
+	if (JoltShape3D *shape = shape_owner.get_or_null(p_rid)) {
+		free_shape(shape);
+	} else if (JoltBody3D *body = body_owner.get_or_null(p_rid)) {
+		free_body(body);
+	} else if (JoltJoint3D *joint = joint_owner.get_or_null(p_rid)) {
+		free_joint(joint);
+	} else if (JoltArea3D *area = area_owner.get_or_null(p_rid)) {
+		free_area(area);
+	} else if (JoltSoftBody3D *soft_body = soft_body_owner.get_or_null(p_rid)) {
+		free_soft_body(soft_body);
+	} else if (JoltSpace3D *space = space_owner.get_or_null(p_rid)) {
+		free_space(space);
+	} else {
+		ERR_FAIL_MSG("Failed to free RID: The specified RID has no owner.");
+	}
+}
+
+void JoltPhysicsServer3D::set_active(bool p_active) {
+	active = p_active;
+}
+
+void JoltPhysicsServer3D::init() {
+	job_system = new JoltJobSystem();
+}
+
+void JoltPhysicsServer3D::finish() {
+	if (job_system != nullptr) {
+		delete job_system;
+		job_system = nullptr;
+	}
+}
+
+void JoltPhysicsServer3D::step(real_t p_step) {
+	if (!active) {
+		return;
+	}
+
+	for (JoltSpace3D *active_space : active_spaces) {
+		job_system->pre_step();
+
+		active_space->step((float)p_step);
+
+		job_system->post_step();
+	}
+}
+
+void JoltPhysicsServer3D::sync() {
+	doing_sync = true;
+}
+
+void JoltPhysicsServer3D::end_sync() {
+	doing_sync = false;
+}
+
+void JoltPhysicsServer3D::flush_queries() {
+	if (!active) {
+		return;
+	}
+
+	flushing_queries = true;
+
+	for (JoltSpace3D *space : active_spaces) {
+		space->call_queries();
+	}
+
+	flushing_queries = false;
+
+#ifdef DEBUG_ENABLED
+	job_system->flush_timings();
+#endif
+}
+
+bool JoltPhysicsServer3D::is_flushing_queries() const {
+	return flushing_queries;
+}
+
+int JoltPhysicsServer3D::get_process_info(ProcessInfo p_process_info) {
+	return 0;
+}
+
+void JoltPhysicsServer3D::free_space(JoltSpace3D *p_space) {
+	ERR_FAIL_NULL(p_space);
+
+	free_area(p_space->get_default_area());
+	space_set_active(p_space->get_rid(), false);
+	space_owner.free(p_space->get_rid());
+	memdelete(p_space);
+}
+
+void JoltPhysicsServer3D::free_area(JoltArea3D *p_area) {
+	ERR_FAIL_NULL(p_area);
+
+	p_area->set_space(nullptr);
+	area_owner.free(p_area->get_rid());
+	memdelete(p_area);
+}
+
+void JoltPhysicsServer3D::free_body(JoltBody3D *p_body) {
+	ERR_FAIL_NULL(p_body);
+
+	p_body->set_space(nullptr);
+	body_owner.free(p_body->get_rid());
+	memdelete(p_body);
+}
+
+void JoltPhysicsServer3D::free_soft_body(JoltSoftBody3D *p_body) {
+	ERR_FAIL_NULL(p_body);
+
+	p_body->set_space(nullptr);
+	soft_body_owner.free(p_body->get_rid());
+	memdelete(p_body);
+}
+
+void JoltPhysicsServer3D::free_shape(JoltShape3D *p_shape) {
+	ERR_FAIL_NULL(p_shape);
+
+	p_shape->remove_self();
+	shape_owner.free(p_shape->get_rid());
+	memdelete(p_shape);
+}
+
+void JoltPhysicsServer3D::free_joint(JoltJoint3D *p_joint) {
+	ERR_FAIL_NULL(p_joint);
+
+	joint_owner.free(p_joint->get_rid());
+	memdelete(p_joint);
+}
+
+#ifdef DEBUG_ENABLED
+
+void JoltPhysicsServer3D::dump_debug_snapshots(const String &p_dir) {
+	for (JoltSpace3D *space : active_spaces) {
+		space->dump_debug_snapshot(p_dir);
+	}
+}
+
+void JoltPhysicsServer3D::space_dump_debug_snapshot(RID p_space, const String &p_dir) {
+	JoltSpace3D *space = space_owner.get_or_null(p_space);
+	ERR_FAIL_NULL(space);
+
+	space->dump_debug_snapshot(p_dir);
+}
+
+#endif
+
+bool JoltPhysicsServer3D::joint_get_enabled(RID p_joint) const {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, false);
+
+	return joint->is_enabled();
+}
+
+void JoltPhysicsServer3D::joint_set_enabled(RID p_joint, bool p_enabled) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
+
+	joint->set_enabled(p_enabled);
+}
+
+int JoltPhysicsServer3D::joint_get_solver_velocity_iterations(RID p_joint) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, 0);
+
+	return joint->get_solver_velocity_iterations();
+}
+
+void JoltPhysicsServer3D::joint_set_solver_velocity_iterations(RID p_joint, int p_value) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
+
+	return joint->set_solver_velocity_iterations(p_value);
+}
+
+int JoltPhysicsServer3D::joint_get_solver_position_iterations(RID p_joint) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, 0);
+
+	return joint->get_solver_position_iterations();
+}
+
+void JoltPhysicsServer3D::joint_set_solver_position_iterations(RID p_joint, int p_value) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
+
+	return joint->set_solver_position_iterations(p_value);
+}
+
+float JoltPhysicsServer3D::pin_joint_get_applied_force(RID p_joint) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, 0.0f);
+
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, 0.0);
+	JoltPinJoint3D *pin_joint = static_cast<JoltPinJoint3D *>(joint);
+
+	return pin_joint->get_applied_force();
+}
+
+double JoltPhysicsServer3D::hinge_joint_get_jolt_param(RID p_joint, HingeJointParamJolt p_param) const {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, 0.0);
+
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, 0.0);
+	JoltHingeJoint3D *hinge_joint = static_cast<JoltHingeJoint3D *>(joint);
+
+	return hinge_joint->get_jolt_param(p_param);
+}
+
+void JoltPhysicsServer3D::hinge_joint_set_jolt_param(RID p_joint, HingeJointParamJolt p_param, double p_value) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
+
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_HINGE);
+	JoltHingeJoint3D *hinge_joint = static_cast<JoltHingeJoint3D *>(joint);
+
+	return hinge_joint->set_jolt_param(p_param, p_value);
+}
+
+bool JoltPhysicsServer3D::hinge_joint_get_jolt_flag(RID p_joint, HingeJointFlagJolt p_flag) const {
+	const JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, false);
+
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, false);
+	const JoltHingeJoint3D *hinge_joint = static_cast<const JoltHingeJoint3D *>(joint);
+
+	return hinge_joint->get_jolt_flag(p_flag);
+}
+
+void JoltPhysicsServer3D::hinge_joint_set_jolt_flag(RID p_joint, HingeJointFlagJolt p_flag, bool p_enabled) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
+
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_HINGE);
+	JoltHingeJoint3D *hinge_joint = static_cast<JoltHingeJoint3D *>(joint);
+
+	return hinge_joint->set_jolt_flag(p_flag, p_enabled);
+}
+
+float JoltPhysicsServer3D::hinge_joint_get_applied_force(RID p_joint) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, 0.0f);
+
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, 0.0f);
+	JoltHingeJoint3D *hinge_joint = static_cast<JoltHingeJoint3D *>(joint);
+
+	return hinge_joint->get_applied_force();
+}
+
+float JoltPhysicsServer3D::hinge_joint_get_applied_torque(RID p_joint) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, 0.0f);
+
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, 0.0f);
+	JoltHingeJoint3D *hinge_joint = static_cast<JoltHingeJoint3D *>(joint);
+
+	return hinge_joint->get_applied_torque();
+}
+
+double JoltPhysicsServer3D::slider_joint_get_jolt_param(RID p_joint, SliderJointParamJolt p_param) const {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, 0.0);
+
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_SLIDER, 0.0);
+	JoltSliderJoint3D *slider_joint = static_cast<JoltSliderJoint3D *>(joint);
+
+	return slider_joint->get_jolt_param(p_param);
+}
+
+void JoltPhysicsServer3D::slider_joint_set_jolt_param(RID p_joint, SliderJointParamJolt p_param, double p_value) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
+
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_SLIDER);
+	JoltSliderJoint3D *slider_joint = static_cast<JoltSliderJoint3D *>(joint);
+
+	return slider_joint->set_jolt_param(p_param, p_value);
+}
+
+bool JoltPhysicsServer3D::slider_joint_get_jolt_flag(RID p_joint, SliderJointFlagJolt p_flag) const {
+	const JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, false);
+
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_SLIDER, false);
+	const JoltSliderJoint3D *slider_joint = static_cast<const JoltSliderJoint3D *>(joint);
+
+	return slider_joint->get_jolt_flag(p_flag);
+}
+
+void JoltPhysicsServer3D::slider_joint_set_jolt_flag(RID p_joint, SliderJointFlagJolt p_flag, bool p_enabled) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
+
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_SLIDER);
+	JoltSliderJoint3D *slider_joint = static_cast<JoltSliderJoint3D *>(joint);
+
+	return slider_joint->set_jolt_flag(p_flag, p_enabled);
+}
+
+float JoltPhysicsServer3D::slider_joint_get_applied_force(RID p_joint) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, 0.0f);
+
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_SLIDER, 0.0f);
+	JoltSliderJoint3D *slider_joint = static_cast<JoltSliderJoint3D *>(joint);
+
+	return slider_joint->get_applied_force();
+}
+
+float JoltPhysicsServer3D::slider_joint_get_applied_torque(RID p_joint) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, 0.0f);
+
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_SLIDER, 0.0f);
+	JoltSliderJoint3D *slider_joint = static_cast<JoltSliderJoint3D *>(joint);
+
+	return slider_joint->get_applied_torque();
+}
+
+double JoltPhysicsServer3D::cone_twist_joint_get_jolt_param(RID p_joint, ConeTwistJointParamJolt p_param) const {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, 0.0);
+
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_CONE_TWIST, 0.0);
+	JoltConeTwistJoint3D *cone_twist_joint = static_cast<JoltConeTwistJoint3D *>(joint);
+
+	return cone_twist_joint->get_jolt_param(p_param);
+}
+
+void JoltPhysicsServer3D::cone_twist_joint_set_jolt_param(RID p_joint, ConeTwistJointParamJolt p_param, double p_value) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
+
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_CONE_TWIST);
+	JoltConeTwistJoint3D *cone_twist_joint = static_cast<JoltConeTwistJoint3D *>(joint);
+
+	return cone_twist_joint->set_jolt_param(p_param, p_value);
+}
+
+bool JoltPhysicsServer3D::cone_twist_joint_get_jolt_flag(RID p_joint, ConeTwistJointFlagJolt p_flag) const {
+	const JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, false);
+
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_CONE_TWIST, false);
+	const JoltConeTwistJoint3D *cone_twist_joint = static_cast<const JoltConeTwistJoint3D *>(joint);
+
+	return cone_twist_joint->get_jolt_flag(p_flag);
+}
+
+void JoltPhysicsServer3D::cone_twist_joint_set_jolt_flag(RID p_joint, ConeTwistJointFlagJolt p_flag, bool p_enabled) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
+
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_CONE_TWIST);
+	JoltConeTwistJoint3D *cone_twist_joint = static_cast<JoltConeTwistJoint3D *>(joint);
+
+	return cone_twist_joint->set_jolt_flag(p_flag, p_enabled);
+}
+
+float JoltPhysicsServer3D::cone_twist_joint_get_applied_force(RID p_joint) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, 0.0f);
+
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_CONE_TWIST, 0.0f);
+	JoltConeTwistJoint3D *cone_twist_joint = static_cast<JoltConeTwistJoint3D *>(joint);
+
+	return cone_twist_joint->get_applied_force();
+}
+
+float JoltPhysicsServer3D::cone_twist_joint_get_applied_torque(RID p_joint) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, 0.0f);
+
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_CONE_TWIST, 0.0f);
+	JoltConeTwistJoint3D *cone_twist_joint = static_cast<JoltConeTwistJoint3D *>(joint);
+
+	return cone_twist_joint->get_applied_torque();
+}
+
+double JoltPhysicsServer3D::generic_6dof_joint_get_jolt_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParamJolt p_param) const {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, 0.0);
+
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, 0.0);
+	JoltGeneric6DOFJoint3D *g6dof_joint = static_cast<JoltGeneric6DOFJoint3D *>(joint);
+
+	return g6dof_joint->get_jolt_param(p_axis, p_param);
+}
+
+void JoltPhysicsServer3D::generic_6dof_joint_set_jolt_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParamJolt p_param, double p_value) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
+
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_6DOF);
+	JoltGeneric6DOFJoint3D *g6dof_joint = static_cast<JoltGeneric6DOFJoint3D *>(joint);
+
+	return g6dof_joint->set_jolt_param(p_axis, p_param, p_value);
+}
+
+bool JoltPhysicsServer3D::generic_6dof_joint_get_jolt_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlagJolt p_flag) const {
+	const JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, false);
+
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, false);
+	const JoltGeneric6DOFJoint3D *g6dof_joint = static_cast<const JoltGeneric6DOFJoint3D *>(joint);
+
+	return g6dof_joint->get_jolt_flag(p_axis, p_flag);
+}
+
+void JoltPhysicsServer3D::generic_6dof_joint_set_jolt_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlagJolt p_flag, bool p_enabled) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL(joint);
+
+	ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_6DOF);
+	JoltGeneric6DOFJoint3D *g6dof_joint = static_cast<JoltGeneric6DOFJoint3D *>(joint);
+
+	return g6dof_joint->set_jolt_flag(p_axis, p_flag, p_enabled);
+}
+
+float JoltPhysicsServer3D::generic_6dof_joint_get_applied_force(RID p_joint) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, 0.0f);
+
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, 0.0f);
+	JoltGeneric6DOFJoint3D *g6dof_joint = static_cast<JoltGeneric6DOFJoint3D *>(joint);
+
+	return g6dof_joint->get_applied_force();
+}
+
+float JoltPhysicsServer3D::generic_6dof_joint_get_applied_torque(RID p_joint) {
+	JoltJoint3D *joint = joint_owner.get_or_null(p_joint);
+	ERR_FAIL_NULL_V(joint, 0.0f);
+
+	ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, 0.0f);
+	JoltGeneric6DOFJoint3D *g6dof_joint = static_cast<JoltGeneric6DOFJoint3D *>(joint);
+
+	return g6dof_joint->get_applied_torque();
+}
diff --git a/modules/jolt_physics/jolt_physics_server_3d.h b/modules/jolt_physics/jolt_physics_server_3d.h
new file mode 100644
index 0000000000..bf71daf038
--- /dev/null
+++ b/modules/jolt_physics/jolt_physics_server_3d.h
@@ -0,0 +1,502 @@
+/**************************************************************************/
+/*  jolt_physics_server_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_SERVER_3D_H
+#define JOLT_PHYSICS_SERVER_3D_H
+
+#include "core/templates/rid_owner.h"
+#include "servers/physics_server_3d.h"
+
+class JoltArea3D;
+class JoltBody3D;
+class JoltJobSystem;
+class JoltJoint3D;
+class JoltShape3D;
+class JoltSoftBody3D;
+class JoltSpace3D;
+
+class JoltPhysicsServer3D final : public PhysicsServer3D {
+	GDCLASS(JoltPhysicsServer3D, PhysicsServer3D)
+
+	inline static JoltPhysicsServer3D *singleton = nullptr;
+
+	mutable RID_PtrOwner<JoltSpace3D> space_owner;
+	mutable RID_PtrOwner<JoltArea3D> area_owner;
+	mutable RID_PtrOwner<JoltBody3D> body_owner;
+	mutable RID_PtrOwner<JoltSoftBody3D> soft_body_owner;
+	mutable RID_PtrOwner<JoltShape3D> shape_owner;
+	mutable RID_PtrOwner<JoltJoint3D> joint_owner;
+
+	HashSet<JoltSpace3D *> active_spaces;
+
+	JoltJobSystem *job_system = nullptr;
+
+	bool on_separate_thread = false;
+	bool active = true;
+	bool flushing_queries = false;
+	bool doing_sync = false;
+
+public:
+	enum HingeJointParamJolt {
+		HINGE_JOINT_LIMIT_SPRING_FREQUENCY = 100,
+		HINGE_JOINT_LIMIT_SPRING_DAMPING,
+		HINGE_JOINT_MOTOR_MAX_TORQUE,
+	};
+
+	enum HingeJointFlagJolt {
+		HINGE_JOINT_FLAG_USE_LIMIT_SPRING = 100,
+	};
+
+	enum SliderJointParamJolt {
+		SLIDER_JOINT_LIMIT_SPRING_FREQUENCY = 100,
+		SLIDER_JOINT_LIMIT_SPRING_DAMPING,
+		SLIDER_JOINT_MOTOR_TARGET_VELOCITY,
+		SLIDER_JOINT_MOTOR_MAX_FORCE,
+	};
+
+	enum SliderJointFlagJolt {
+		SLIDER_JOINT_FLAG_USE_LIMIT = 100,
+		SLIDER_JOINT_FLAG_USE_LIMIT_SPRING,
+		SLIDER_JOINT_FLAG_ENABLE_MOTOR,
+	};
+
+	enum ConeTwistJointParamJolt {
+		CONE_TWIST_JOINT_SWING_MOTOR_TARGET_VELOCITY_Y = 100,
+		CONE_TWIST_JOINT_SWING_MOTOR_TARGET_VELOCITY_Z,
+		CONE_TWIST_JOINT_TWIST_MOTOR_TARGET_VELOCITY,
+		CONE_TWIST_JOINT_SWING_MOTOR_MAX_TORQUE,
+		CONE_TWIST_JOINT_TWIST_MOTOR_MAX_TORQUE,
+	};
+
+	enum ConeTwistJointFlagJolt {
+		CONE_TWIST_JOINT_FLAG_USE_SWING_LIMIT = 100,
+		CONE_TWIST_JOINT_FLAG_USE_TWIST_LIMIT,
+		CONE_TWIST_JOINT_FLAG_ENABLE_SWING_MOTOR,
+		CONE_TWIST_JOINT_FLAG_ENABLE_TWIST_MOTOR,
+	};
+
+	enum G6DOFJointAxisParamJolt {
+		G6DOF_JOINT_LINEAR_SPRING_FREQUENCY = 100,
+		G6DOF_JOINT_LINEAR_LIMIT_SPRING_FREQUENCY,
+		G6DOF_JOINT_LINEAR_LIMIT_SPRING_DAMPING,
+		G6DOF_JOINT_ANGULAR_SPRING_FREQUENCY,
+		G6DOF_JOINT_LINEAR_SPRING_MAX_FORCE,
+		G6DOF_JOINT_ANGULAR_SPRING_MAX_TORQUE,
+	};
+
+	enum G6DOFJointAxisFlagJolt {
+		G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT_SPRING = 100,
+		G6DOF_JOINT_FLAG_ENABLE_LINEAR_SPRING_FREQUENCY,
+		G6DOF_JOINT_FLAG_ENABLE_ANGULAR_SPRING_FREQUENCY,
+	};
+
+private:
+	static void _bind_methods() {}
+
+public:
+	explicit JoltPhysicsServer3D(bool p_on_separate_thread);
+	~JoltPhysicsServer3D();
+
+	static JoltPhysicsServer3D *get_singleton() { return singleton; }
+
+	virtual RID world_boundary_shape_create() override;
+	virtual RID separation_ray_shape_create() override;
+	virtual RID sphere_shape_create() override;
+	virtual RID box_shape_create() override;
+	virtual RID capsule_shape_create() override;
+	virtual RID cylinder_shape_create() override;
+	virtual RID convex_polygon_shape_create() override;
+	virtual RID concave_polygon_shape_create() override;
+	virtual RID heightmap_shape_create() override;
+	virtual RID custom_shape_create() override;
+
+	virtual void shape_set_data(RID p_shape, const Variant &p_data) override;
+	virtual Variant shape_get_data(RID p_shape) const override;
+
+	virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias) override;
+
+	virtual void shape_set_margin(RID p_shape, real_t p_margin) override;
+	virtual real_t shape_get_margin(RID p_shape) const override;
+
+	virtual PhysicsServer3D::ShapeType shape_get_type(RID p_shape) const override;
+
+	virtual real_t shape_get_custom_solver_bias(RID p_shape) const override;
+
+	virtual RID space_create() override;
+
+	virtual void space_set_active(RID p_space, bool p_active) override;
+	virtual bool space_is_active(RID p_space) const override;
+
+	virtual void space_set_param(RID p_space, PhysicsServer3D::SpaceParameter p_param, real_t p_value) override;
+	virtual real_t space_get_param(RID p_space, PhysicsServer3D::SpaceParameter p_param) const override;
+
+	virtual PhysicsDirectSpaceState3D *space_get_direct_state(RID p_space) override;
+
+	virtual void space_set_debug_contacts(RID p_space, int p_max_contacts) override;
+	virtual PackedVector3Array space_get_contacts(RID p_space) const override;
+	virtual int space_get_contact_count(RID p_space) const override;
+
+	virtual RID area_create() override;
+
+	virtual void area_set_space(RID p_area, RID p_space) override;
+	virtual RID area_get_space(RID p_area) const override;
+
+	virtual void area_add_shape(RID p_area, RID p_shape, const Transform3D &p_transform, bool p_disabled) override;
+
+	virtual void area_set_shape(RID p_area, int p_shape_idx, RID p_shape) override;
+	virtual RID area_get_shape(RID p_area, int p_shape_idx) const override;
+
+	virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform3D &p_transform) override;
+	virtual Transform3D area_get_shape_transform(RID p_area, int p_shape_idx) const override;
+
+	virtual void area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) override;
+
+	virtual int area_get_shape_count(RID p_area) const override;
+
+	virtual void area_remove_shape(RID p_area, int p_shape_idx) override;
+	virtual void area_clear_shapes(RID p_area) override;
+
+	virtual void area_attach_object_instance_id(RID p_area, ObjectID p_id) override;
+	virtual ObjectID area_get_object_instance_id(RID p_area) const override;
+
+	virtual void area_set_param(RID p_area, PhysicsServer3D::AreaParameter p_param, const Variant &p_value) override;
+	virtual Variant area_get_param(RID p_area, PhysicsServer3D::AreaParameter p_param) const override;
+
+	virtual void area_set_transform(RID p_area, const Transform3D &p_transform) override;
+	virtual Transform3D area_get_transform(RID p_area) const override;
+
+	virtual void area_set_collision_layer(RID p_area, uint32_t p_layer) override;
+	virtual uint32_t area_get_collision_layer(RID p_area) const override;
+
+	virtual void area_set_collision_mask(RID p_area, uint32_t p_mask) override;
+	virtual uint32_t area_get_collision_mask(RID p_area) const override;
+
+	virtual void area_set_monitorable(RID p_area, bool p_monitorable) override;
+
+	virtual void area_set_ray_pickable(RID p_area, bool p_enable) override;
+
+	virtual void area_set_monitor_callback(RID p_area, const Callable &p_callback) override;
+	virtual void area_set_area_monitor_callback(RID p_area, const Callable &p_callback) override;
+
+	virtual RID body_create() override;
+
+	virtual void body_set_space(RID p_body, RID p_space) override;
+	virtual RID body_get_space(RID p_body) const override;
+
+	virtual void body_set_mode(RID p_body, PhysicsServer3D::BodyMode p_mode) override;
+	virtual PhysicsServer3D::BodyMode body_get_mode(RID p_body) const override;
+
+	virtual void body_add_shape(RID p_body, RID p_shape, const Transform3D &p_transform, bool p_disabled) override;
+
+	virtual void body_set_shape(RID p_body, int p_shape_idx, RID p_shape) override;
+	virtual RID body_get_shape(RID p_body, int p_shape_idx) const override;
+
+	virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform3D &p_transform) override;
+	virtual Transform3D body_get_shape_transform(RID p_body, int p_shape_idx) const override;
+
+	virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled) override;
+
+	virtual int body_get_shape_count(RID p_body) const override;
+
+	virtual void body_remove_shape(RID p_body, int p_shape_idx) override;
+	virtual void body_clear_shapes(RID p_body) override;
+
+	virtual void body_attach_object_instance_id(RID p_body, ObjectID p_id) override;
+	virtual ObjectID body_get_object_instance_id(RID p_body) const override;
+
+	virtual void body_set_enable_continuous_collision_detection(RID p_body, bool p_enable) override;
+	virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const override;
+
+	virtual void body_set_collision_layer(RID p_body, uint32_t p_layer) override;
+	virtual uint32_t body_get_collision_layer(RID p_body) const override;
+
+	virtual void body_set_collision_mask(RID p_body, uint32_t p_mask) override;
+	virtual uint32_t body_get_collision_mask(RID p_body) const override;
+
+	virtual void body_set_collision_priority(RID p_body, real_t p_priority) override;
+	virtual real_t body_get_collision_priority(RID p_body) const override;
+
+	virtual void body_set_user_flags(RID p_body, uint32_t p_flags) override;
+	virtual uint32_t body_get_user_flags(RID p_body) const override;
+
+	virtual void body_set_param(RID p_body, PhysicsServer3D::BodyParameter p_param, const Variant &p_value) override;
+	virtual Variant body_get_param(RID p_body, PhysicsServer3D::BodyParameter p_param) const override;
+
+	virtual void body_reset_mass_properties(RID p_body) override;
+
+	virtual void body_set_state(RID p_body, PhysicsServer3D::BodyState p_state, const Variant &p_value) override;
+	virtual Variant body_get_state(RID p_body, PhysicsServer3D::BodyState p_state) const override;
+
+	virtual void body_apply_central_impulse(RID p_body, const Vector3 &p_impulse) override;
+	virtual void body_apply_impulse(RID p_body, const Vector3 &p_impulse, const Vector3 &p_position) override;
+	virtual void body_apply_torque_impulse(RID p_body, const Vector3 &p_impulse) override;
+
+	virtual void body_apply_central_force(RID p_body, const Vector3 &p_force) override;
+	virtual void body_apply_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position) override;
+	virtual void body_apply_torque(RID p_body, const Vector3 &p_torque) override;
+
+	virtual void body_add_constant_central_force(RID p_body, const Vector3 &p_force) override;
+	virtual void body_add_constant_force(RID p_body, const Vector3 &p_force, const Vector3 &p_position) override;
+	virtual void body_add_constant_torque(RID p_body, const Vector3 &p_torque) override;
+
+	virtual void body_set_constant_force(RID p_body, const Vector3 &p_force) override;
+	virtual Vector3 body_get_constant_force(RID p_body) const override;
+
+	virtual void body_set_constant_torque(RID p_body, const Vector3 &p_torque) override;
+	virtual Vector3 body_get_constant_torque(RID p_body) const override;
+
+	virtual void body_set_axis_velocity(RID p_body, const Vector3 &p_axis_velocity) override;
+
+	virtual void body_set_axis_lock(RID p_body, PhysicsServer3D::BodyAxis p_axis, bool p_lock) override;
+	virtual bool body_is_axis_locked(RID p_body, PhysicsServer3D::BodyAxis p_axis) const override;
+
+	virtual void body_add_collision_exception(RID p_body, RID p_excepted_body) override;
+	virtual void body_remove_collision_exception(RID p_body, RID p_excepted_body) override;
+	virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) override;
+
+	virtual void body_set_max_contacts_reported(RID p_body, int p_amount) override;
+	virtual int body_get_max_contacts_reported(RID p_body) const override;
+
+	virtual void body_set_contacts_reported_depth_threshold(RID p_body, real_t p_threshold) override;
+	virtual real_t body_get_contacts_reported_depth_threshold(RID p_body) const override;
+
+	virtual void body_set_omit_force_integration(RID p_body, bool p_enable) override;
+	virtual bool body_is_omitting_force_integration(RID p_body) const override;
+
+	virtual void body_set_state_sync_callback(RID p_body, const Callable &p_callable) override;
+	virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_userdata) override;
+
+	virtual void body_set_ray_pickable(RID p_body, bool p_enable) override;
+
+	virtual bool body_test_motion(RID p_body, const MotionParameters &p_parameters, MotionResult *r_result) override;
+
+	virtual PhysicsDirectBodyState3D *body_get_direct_state(RID p_body) override;
+
+	virtual RID soft_body_create() override;
+
+	virtual void soft_body_update_rendering_server(RID p_body, PhysicsServer3DRenderingServerHandler *p_rendering_server_handler) override;
+
+	virtual void soft_body_set_space(RID p_body, RID p_space) override;
+	virtual RID soft_body_get_space(RID p_body) const override;
+
+	virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable) override;
+
+	virtual void soft_body_set_collision_layer(RID p_body, uint32_t p_layer) override;
+	virtual uint32_t soft_body_get_collision_layer(RID p_body) const override;
+
+	virtual void soft_body_set_collision_mask(RID p_body, uint32_t p_mask) override;
+	virtual uint32_t soft_body_get_collision_mask(RID p_body) const override;
+
+	virtual void soft_body_add_collision_exception(RID p_body, RID p_excepted_body) override;
+	virtual void soft_body_remove_collision_exception(RID p_body, RID p_excepted_body) override;
+	virtual void soft_body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) override;
+
+	virtual void soft_body_set_state(RID p_body, PhysicsServer3D::BodyState p_state, const Variant &p_value) override;
+	virtual Variant soft_body_get_state(RID p_body, PhysicsServer3D::BodyState p_state) const override;
+
+	virtual void soft_body_set_transform(RID p_body, const Transform3D &p_transform) override;
+
+	virtual void soft_body_set_simulation_precision(RID p_body, int p_precision) override;
+	virtual int soft_body_get_simulation_precision(RID p_body) const override;
+
+	virtual void soft_body_set_total_mass(RID p_body, real_t p_total_mass) override;
+	virtual real_t soft_body_get_total_mass(RID p_body) const override;
+
+	virtual void soft_body_set_linear_stiffness(RID p_body, real_t p_coefficient) override;
+	virtual real_t soft_body_get_linear_stiffness(RID p_body) const override;
+
+	virtual void soft_body_set_pressure_coefficient(RID p_body, real_t p_coefficient) override;
+	virtual real_t soft_body_get_pressure_coefficient(RID p_body) const override;
+
+	virtual void soft_body_set_damping_coefficient(RID p_body, real_t p_coefficient) override;
+	virtual real_t soft_body_get_damping_coefficient(RID p_body) const override;
+
+	virtual void soft_body_set_drag_coefficient(RID p_body, real_t p_coefficient) override;
+	virtual real_t soft_body_get_drag_coefficient(RID p_body) const override;
+
+	virtual void soft_body_set_mesh(RID p_body, RID p_mesh) override;
+
+	virtual AABB soft_body_get_bounds(RID p_body) const override;
+
+	virtual void soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) override;
+
+	virtual Vector3 soft_body_get_point_global_position(RID p_body, int p_point_index) const override;
+
+	virtual void soft_body_remove_all_pinned_points(RID p_body) override;
+
+	virtual void soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) override;
+	virtual bool soft_body_is_point_pinned(RID p_body, int p_point_index) const override;
+
+	virtual RID joint_create() override;
+	virtual void joint_clear(RID p_joint) override;
+
+	virtual void joint_make_pin(RID p_joint, RID p_body_a, const Vector3 &p_local_a, RID p_body_b, const Vector3 &p_local_b) override;
+
+	virtual void pin_joint_set_param(RID p_joint, PhysicsServer3D::PinJointParam p_param, real_t p_value) override;
+	virtual real_t pin_joint_get_param(RID p_joint, PhysicsServer3D::PinJointParam p_param) const override;
+
+	virtual void pin_joint_set_local_a(RID p_joint, const Vector3 &p_local_a) override;
+	virtual Vector3 pin_joint_get_local_a(RID p_joint) const override;
+
+	virtual void pin_joint_set_local_b(RID p_joint, const Vector3 &p_local_b) override;
+	virtual Vector3 pin_joint_get_local_b(RID p_joint) const override;
+
+	virtual void joint_make_hinge(RID p_joint, RID p_body_a, const Transform3D &p_hinge_a, RID p_body_b, const Transform3D &p_hinge_b) override;
+
+	virtual void joint_make_hinge_simple(RID p_joint, RID p_body_a, const Vector3 &p_pivot_a, const Vector3 &p_axis_a, RID p_body_b, const Vector3 &p_pivot_b, const Vector3 &p_axis_b) override;
+
+	virtual void hinge_joint_set_param(RID p_joint, PhysicsServer3D::HingeJointParam p_param, real_t p_value) override;
+	virtual real_t hinge_joint_get_param(RID p_joint, PhysicsServer3D::HingeJointParam p_param) const override;
+
+	virtual void hinge_joint_set_flag(RID p_joint, PhysicsServer3D::HingeJointFlag p_flag, bool p_enabled) override;
+	virtual bool hinge_joint_get_flag(RID p_joint, PhysicsServer3D::HingeJointFlag p_flag) const override;
+
+	virtual void joint_make_slider(RID p_joint, RID p_body_a, const Transform3D &p_local_ref_a, RID p_body_b, const Transform3D &p_local_ref_b) override;
+
+	virtual void slider_joint_set_param(RID p_joint, PhysicsServer3D::SliderJointParam p_param, real_t p_value) override;
+	virtual real_t slider_joint_get_param(RID p_joint, PhysicsServer3D::SliderJointParam p_param) const override;
+
+	virtual void joint_make_cone_twist(RID p_joint, RID p_body_a, const Transform3D &p_local_ref_a, RID p_body_b, const Transform3D &p_local_ref_b) override;
+
+	virtual void cone_twist_joint_set_param(RID p_joint, PhysicsServer3D::ConeTwistJointParam p_param, real_t p_value) override;
+	virtual real_t cone_twist_joint_get_param(RID p_joint, PhysicsServer3D::ConeTwistJointParam p_param) const override;
+
+	virtual void joint_make_generic_6dof(RID p_joint, RID p_body_a, const Transform3D &p_local_ref_a, RID p_body_b, const Transform3D &p_local_ref_b) override;
+
+	virtual void generic_6dof_joint_set_param(RID p_joint, Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param, real_t p_value) override;
+	virtual real_t generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param) const override;
+
+	virtual void generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag, bool p_enable) override;
+	virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const override;
+
+	virtual PhysicsServer3D::JointType joint_get_type(RID p_joint) const override;
+
+	virtual void joint_set_solver_priority(RID p_joint, int p_priority) override;
+	virtual int joint_get_solver_priority(RID p_joint) const override;
+
+	virtual void joint_disable_collisions_between_bodies(RID p_joint, bool p_disable) override;
+	virtual bool joint_is_disabled_collisions_between_bodies(RID p_joint) const override;
+
+	virtual void free(RID p_rid) override;
+
+	virtual void set_active(bool p_active) override;
+
+	virtual void init() override;
+	virtual void finish() override;
+
+	virtual void step(real_t p_step) override;
+
+	virtual void sync() override;
+	virtual void end_sync() override;
+
+	virtual void flush_queries() override;
+	virtual bool is_flushing_queries() const override;
+
+	virtual int get_process_info(PhysicsServer3D::ProcessInfo p_process_info) override;
+
+	bool is_active() const { return active; }
+
+	void free_space(JoltSpace3D *p_space);
+	void free_area(JoltArea3D *p_area);
+	void free_body(JoltBody3D *p_body);
+	void free_soft_body(JoltSoftBody3D *p_body);
+	void free_shape(JoltShape3D *p_shape);
+	void free_joint(JoltJoint3D *p_joint);
+
+	JoltSpace3D *get_space(RID p_rid) const { return space_owner.get_or_null(p_rid); }
+	JoltArea3D *get_area(RID p_rid) const { return area_owner.get_or_null(p_rid); }
+	JoltBody3D *get_body(RID p_rid) const { return body_owner.get_or_null(p_rid); }
+	JoltShape3D *get_shape(RID p_rid) const { return shape_owner.get_or_null(p_rid); }
+	JoltJoint3D *get_joint(RID p_rid) const { return joint_owner.get_or_null(p_rid); }
+
+#ifdef DEBUG_ENABLED
+	void dump_debug_snapshots(const String &p_dir);
+
+	void space_dump_debug_snapshot(RID p_space, const String &p_dir);
+#endif
+
+	bool joint_get_enabled(RID p_joint) const;
+	void joint_set_enabled(RID p_joint, bool p_enabled);
+
+	int joint_get_solver_velocity_iterations(RID p_joint);
+	void joint_set_solver_velocity_iterations(RID p_joint, int p_value);
+
+	int joint_get_solver_position_iterations(RID p_joint);
+	void joint_set_solver_position_iterations(RID p_joint, int p_value);
+
+	float pin_joint_get_applied_force(RID p_joint);
+
+	double hinge_joint_get_jolt_param(RID p_joint, HingeJointParamJolt p_param) const;
+	void hinge_joint_set_jolt_param(RID p_joint, HingeJointParamJolt p_param, double p_value);
+
+	bool hinge_joint_get_jolt_flag(RID p_joint, HingeJointFlagJolt p_flag) const;
+	void hinge_joint_set_jolt_flag(RID p_joint, HingeJointFlagJolt p_flag, bool p_enabled);
+
+	float hinge_joint_get_applied_force(RID p_joint);
+	float hinge_joint_get_applied_torque(RID p_joint);
+
+	double slider_joint_get_jolt_param(RID p_joint, SliderJointParamJolt p_param) const;
+	void slider_joint_set_jolt_param(RID p_joint, SliderJointParamJolt p_param, double p_value);
+
+	bool slider_joint_get_jolt_flag(RID p_joint, SliderJointFlagJolt p_flag) const;
+	void slider_joint_set_jolt_flag(RID p_joint, SliderJointFlagJolt p_flag, bool p_enabled);
+
+	float slider_joint_get_applied_force(RID p_joint);
+	float slider_joint_get_applied_torque(RID p_joint);
+
+	double cone_twist_joint_get_jolt_param(RID p_joint, ConeTwistJointParamJolt p_param) const;
+	void cone_twist_joint_set_jolt_param(RID p_joint, ConeTwistJointParamJolt p_param, double p_value);
+
+	bool cone_twist_joint_get_jolt_flag(RID p_joint, ConeTwistJointFlagJolt p_flag) const;
+	void cone_twist_joint_set_jolt_flag(RID p_joint, ConeTwistJointFlagJolt p_flag, bool p_enabled);
+
+	float cone_twist_joint_get_applied_force(RID p_joint);
+	float cone_twist_joint_get_applied_torque(RID p_joint);
+
+	double generic_6dof_joint_get_jolt_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParamJolt p_param) const;
+	void generic_6dof_joint_set_jolt_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParamJolt p_param, double p_value);
+
+	bool generic_6dof_joint_get_jolt_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlagJolt p_flag) const;
+	void generic_6dof_joint_set_jolt_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlagJolt p_flag, bool p_enabled);
+
+	float generic_6dof_joint_get_applied_force(RID p_joint);
+	float generic_6dof_joint_get_applied_torque(RID p_joint);
+};
+
+VARIANT_ENUM_CAST(JoltPhysicsServer3D::HingeJointParamJolt)
+VARIANT_ENUM_CAST(JoltPhysicsServer3D::HingeJointFlagJolt)
+VARIANT_ENUM_CAST(JoltPhysicsServer3D::SliderJointParamJolt)
+VARIANT_ENUM_CAST(JoltPhysicsServer3D::SliderJointFlagJolt)
+VARIANT_ENUM_CAST(JoltPhysicsServer3D::ConeTwistJointParamJolt)
+VARIANT_ENUM_CAST(JoltPhysicsServer3D::ConeTwistJointFlagJolt)
+VARIANT_ENUM_CAST(JoltPhysicsServer3D::G6DOFJointAxisParamJolt)
+VARIANT_ENUM_CAST(JoltPhysicsServer3D::G6DOFJointAxisFlagJolt)
+
+#endif // JOLT_PHYSICS_SERVER_3D_H
diff --git a/modules/jolt_physics/jolt_project_settings.cpp b/modules/jolt_physics/jolt_project_settings.cpp
new file mode 100644
index 0000000000..54a7140f10
--- /dev/null
+++ b/modules/jolt_physics/jolt_project_settings.cpp
@@ -0,0 +1,227 @@
+/**************************************************************************/
+/*  jolt_project_settings.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_project_settings.h"
+
+#include "core/config/project_settings.h"
+
+namespace {
+
+enum JoltJointWorldNode : int {
+	JOLT_JOINT_WORLD_NODE_A,
+	JOLT_JOINT_WORLD_NODE_B,
+};
+
+} // namespace
+
+void JoltProjectSettings::register_settings() {
+	GLOBAL_DEF(PropertyInfo(Variant::INT, "physics/jolt_physics_3d/simulation/velocity_steps", PROPERTY_HINT_RANGE, U"2,16,or_greater"), 10);
+	GLOBAL_DEF(PropertyInfo(Variant::INT, "physics/jolt_physics_3d/simulation/position_steps", PROPERTY_HINT_RANGE, U"1,16,or_greater"), 2);
+	GLOBAL_DEF(PropertyInfo(Variant::BOOL, "physics/jolt_physics_3d/simulation/use_enhanced_internal_edge_removal"), true);
+	GLOBAL_DEF(PropertyInfo(Variant::BOOL, "physics/jolt_physics_3d/simulation/areas_detect_static_bodies"), false);
+	GLOBAL_DEF(PropertyInfo(Variant::BOOL, "physics/jolt_physics_3d/simulation/generate_all_kinematic_contacts"), false);
+	GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/simulation/penetration_slop", PROPERTY_HINT_RANGE, U"0,1,0.00001,or_greater,suffix:m"), 0.02f);
+	GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/simulation/speculative_contact_distance", PROPERTY_HINT_RANGE, U"0,1,0.00001,or_greater,suffix:m"), 0.02f);
+	GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/simulation/baumgarte_stabilization_factor", PROPERTY_HINT_RANGE, U"0,1,0.01"), 0.2f);
+	GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/simulation/soft_body_point_radius", PROPERTY_HINT_RANGE, U"0,1,0.001,or_greater,suffix:m"), 0.01f);
+	GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/simulation/bounce_velocity_threshold", PROPERTY_HINT_RANGE, U"0,1,0.001,or_greater,suffix:m/s"), 1.0f);
+	GLOBAL_DEF(PropertyInfo(Variant::BOOL, "physics/jolt_physics_3d/simulation/allow_sleep"), true);
+	GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/simulation/sleep_velocity_threshold", PROPERTY_HINT_RANGE, U"0,1,0.001,or_greater,suffix:m/s"), 0.03f);
+	GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/simulation/sleep_time_threshold", PROPERTY_HINT_RANGE, U"0,5,0.01,or_greater,suffix:s"), 0.5f);
+	GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/simulation/continuous_cd_movement_threshold", PROPERTY_HINT_RANGE, U"0,1,0.01"), 0.75f);
+	GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/simulation/continuous_cd_max_penetration", PROPERTY_HINT_RANGE, U"0,1,0.01"), 0.25f);
+	GLOBAL_DEF(PropertyInfo(Variant::BOOL, "physics/jolt_physics_3d/simulation/body_pair_contact_cache_enabled"), true);
+	GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/simulation/body_pair_contact_cache_distance_threshold", PROPERTY_HINT_RANGE, U"0,0.01,0.00001,or_greater,suffix:m"), 0.001f);
+	GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/simulation/body_pair_contact_cache_angle_threshold", PROPERTY_HINT_RANGE, U"0,180,0.01,radians_as_degrees"), Math::deg_to_rad(2.0f));
+
+	GLOBAL_DEF(PropertyInfo(Variant::BOOL, "physics/jolt_physics_3d/queries/use_enhanced_internal_edge_removal"), false);
+	GLOBAL_DEF_RST(PropertyInfo(Variant::BOOL, "physics/jolt_physics_3d/queries/enable_ray_cast_face_index"), false);
+
+	GLOBAL_DEF(PropertyInfo(Variant::BOOL, "physics/jolt_physics_3d/motion_queries/use_enhanced_internal_edge_removal"), true);
+	GLOBAL_DEF(PropertyInfo(Variant::INT, "physics/jolt_physics_3d/motion_queries/recovery_iterations", PROPERTY_HINT_RANGE, U"1,8,or_greater"), 4);
+	GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/motion_queries/recovery_amount", PROPERTY_HINT_RANGE, U"0,1,0.01"), 0.4f);
+
+	GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/collisions/collision_margin_fraction", PROPERTY_HINT_RANGE, U"0,1,0.00001"), 0.08f);
+	GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/collisions/active_edge_threshold", PROPERTY_HINT_RANGE, U"0,90,0.01,radians_as_degrees"), Math::deg_to_rad(50.0f));
+
+	GLOBAL_DEF(PropertyInfo(Variant::INT, "physics/jolt_physics_3d/joints/world_node", PROPERTY_HINT_ENUM, U"Node A,Node B"), JOLT_JOINT_WORLD_NODE_A);
+
+	GLOBAL_DEF(PropertyInfo(Variant::INT, "physics/jolt_physics_3d/limits/temporary_memory_buffer_size", PROPERTY_HINT_RANGE, U"1,32,or_greater,suffix:MiB"), 32);
+	GLOBAL_DEF_RST(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/limits/world_boundary_shape_size", PROPERTY_HINT_RANGE, U"2,2000,0.1,or_greater,suffix:m"), 2000.0f);
+	GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/limits/max_linear_velocity", PROPERTY_HINT_RANGE, U"0,500,0.01,or_greater,suffix:m/s"), 500.0f);
+	GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "physics/jolt_physics_3d/limits/max_angular_velocity", PROPERTY_HINT_RANGE, U"0,2700,0.01,or_greater,radians_as_degrees,suffix:°/s"), Math::deg_to_rad(2700.0f));
+	GLOBAL_DEF_RST(PropertyInfo(Variant::INT, "physics/jolt_physics_3d/limits/max_bodies", PROPERTY_HINT_RANGE, U"1,10240,or_greater"), 10240);
+	GLOBAL_DEF(PropertyInfo(Variant::INT, "physics/jolt_physics_3d/limits/max_body_pairs", PROPERTY_HINT_RANGE, U"8,65536,or_greater"), 65536);
+	GLOBAL_DEF(PropertyInfo(Variant::INT, "physics/jolt_physics_3d/limits/max_contact_constraints", PROPERTY_HINT_RANGE, U"8,20480,or_greater"), 20480);
+}
+
+int JoltProjectSettings::get_simulation_velocity_steps() {
+	return GLOBAL_GET("physics/jolt_physics_3d/simulation/velocity_steps");
+}
+
+int JoltProjectSettings::get_simulation_position_steps() {
+	return GLOBAL_GET("physics/jolt_physics_3d/simulation/position_steps");
+}
+
+bool JoltProjectSettings::use_enhanced_internal_edge_removal_for_bodies() {
+	return GLOBAL_GET("physics/jolt_physics_3d/simulation/use_enhanced_internal_edge_removal");
+}
+
+bool JoltProjectSettings::areas_detect_static_bodies() {
+	return GLOBAL_GET("physics/jolt_physics_3d/simulation/areas_detect_static_bodies");
+}
+
+bool JoltProjectSettings::should_generate_all_kinematic_contacts() {
+	return GLOBAL_GET("physics/jolt_physics_3d/simulation/generate_all_kinematic_contacts");
+}
+
+float JoltProjectSettings::get_penetration_slop() {
+	return GLOBAL_GET("physics/jolt_physics_3d/simulation/penetration_slop");
+}
+
+float JoltProjectSettings::get_speculative_contact_distance() {
+	return GLOBAL_GET("physics/jolt_physics_3d/simulation/speculative_contact_distance");
+}
+
+float JoltProjectSettings::get_baumgarte_stabilization_factor() {
+	return GLOBAL_GET("physics/jolt_physics_3d/simulation/baumgarte_stabilization_factor");
+}
+
+float JoltProjectSettings::get_soft_body_point_radius() {
+	return GLOBAL_GET("physics/jolt_physics_3d/simulation/soft_body_point_radius");
+}
+
+float JoltProjectSettings::get_bounce_velocity_threshold() {
+	return GLOBAL_GET("physics/jolt_physics_3d/simulation/bounce_velocity_threshold");
+}
+
+bool JoltProjectSettings::is_sleep_allowed() {
+	return GLOBAL_GET("physics/jolt_physics_3d/simulation/allow_sleep");
+}
+
+float JoltProjectSettings::get_sleep_velocity_threshold() {
+	return GLOBAL_GET("physics/jolt_physics_3d/simulation/sleep_velocity_threshold");
+}
+
+float JoltProjectSettings::get_sleep_time_threshold() {
+	return GLOBAL_GET("physics/jolt_physics_3d/simulation/sleep_time_threshold");
+}
+
+float JoltProjectSettings::get_ccd_movement_threshold() {
+	return GLOBAL_GET("physics/jolt_physics_3d/simulation/continuous_cd_movement_threshold");
+}
+
+float JoltProjectSettings::get_ccd_max_penetration() {
+	return GLOBAL_GET("physics/jolt_physics_3d/simulation/continuous_cd_max_penetration");
+}
+
+bool JoltProjectSettings::is_body_pair_contact_cache_enabled() {
+	return GLOBAL_GET("physics/jolt_physics_3d/simulation/body_pair_contact_cache_enabled");
+}
+
+float JoltProjectSettings::get_body_pair_cache_distance_sq() {
+	const float value = GLOBAL_GET("physics/jolt_physics_3d/simulation/body_pair_contact_cache_distance_threshold");
+	return value * value;
+}
+
+float JoltProjectSettings::get_body_pair_cache_angle_cos_div2() {
+	return Math::cos((float)GLOBAL_GET("physics/jolt_physics_3d/simulation/body_pair_contact_cache_angle_threshold") / 2.0f);
+}
+
+bool JoltProjectSettings::use_enhanced_internal_edge_removal_for_queries() {
+	static const bool value = GLOBAL_GET("physics/jolt_physics_3d/queries/use_enhanced_internal_edge_removal");
+	return value;
+}
+
+bool JoltProjectSettings::enable_ray_cast_face_index() {
+	static const bool value = GLOBAL_GET("physics/jolt_physics_3d/queries/enable_ray_cast_face_index");
+	return value;
+}
+
+bool JoltProjectSettings::use_enhanced_internal_edge_removal_for_motion_queries() {
+	static const bool value = GLOBAL_GET("physics/jolt_physics_3d/motion_queries/use_enhanced_internal_edge_removal");
+	return value;
+}
+
+int JoltProjectSettings::get_motion_query_recovery_iterations() {
+	static const int value = GLOBAL_GET("physics/jolt_physics_3d/motion_queries/recovery_iterations");
+	return value;
+}
+
+float JoltProjectSettings::get_motion_query_recovery_amount() {
+	static const float value = GLOBAL_GET("physics/jolt_physics_3d/motion_queries/recovery_amount");
+	return value;
+}
+
+float JoltProjectSettings::get_collision_margin_fraction() {
+	static const float value = GLOBAL_GET("physics/jolt_physics_3d/collisions/collision_margin_fraction");
+	return value;
+}
+
+float JoltProjectSettings::get_active_edge_threshold() {
+	static const float value = Math::cos((float)GLOBAL_GET("physics/jolt_physics_3d/collisions/active_edge_threshold"));
+	return value;
+}
+
+bool JoltProjectSettings::use_joint_world_node_a() {
+	return (int)GLOBAL_GET("physics/jolt_physics_3d/joints/world_node") == JOLT_JOINT_WORLD_NODE_A;
+}
+
+int JoltProjectSettings::get_temp_memory_mib() {
+	return GLOBAL_GET("physics/jolt_physics_3d/limits/temporary_memory_buffer_size");
+}
+
+int64_t JoltProjectSettings::get_temp_memory_b() {
+	return get_temp_memory_mib() * 1024 * 1024;
+}
+
+float JoltProjectSettings::get_world_boundary_shape_size() {
+	return GLOBAL_GET("physics/jolt_physics_3d/limits/world_boundary_shape_size");
+}
+
+float JoltProjectSettings::get_max_linear_velocity() {
+	return GLOBAL_GET("physics/jolt_physics_3d/limits/max_linear_velocity");
+}
+
+float JoltProjectSettings::get_max_angular_velocity() {
+	return GLOBAL_GET("physics/jolt_physics_3d/limits/max_angular_velocity");
+}
+
+int JoltProjectSettings::get_max_bodies() {
+	return GLOBAL_GET("physics/jolt_physics_3d/limits/max_bodies");
+}
+
+int JoltProjectSettings::get_max_pairs() {
+	return GLOBAL_GET("physics/jolt_physics_3d/limits/max_body_pairs");
+}
+
+int JoltProjectSettings::get_max_contact_constraints() {
+	return GLOBAL_GET("physics/jolt_physics_3d/limits/max_contact_constraints");
+}
diff --git a/modules/jolt_physics/jolt_project_settings.h b/modules/jolt_physics/jolt_project_settings.h
new file mode 100644
index 0000000000..48710d4103
--- /dev/null
+++ b/modules/jolt_physics/jolt_project_settings.h
@@ -0,0 +1,81 @@
+/**************************************************************************/
+/*  jolt_project_settings.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_PROJECT_SETTINGS_H
+#define JOLT_PROJECT_SETTINGS_H
+
+#include <stdint.h>
+
+class JoltProjectSettings {
+public:
+	static void register_settings();
+
+	static int get_simulation_velocity_steps();
+	static int get_simulation_position_steps();
+	static bool use_enhanced_internal_edge_removal_for_bodies();
+	static bool areas_detect_static_bodies();
+	static bool should_generate_all_kinematic_contacts();
+	static float get_penetration_slop();
+	static float get_speculative_contact_distance();
+	static float get_baumgarte_stabilization_factor();
+	static float get_soft_body_point_radius();
+	static float get_bounce_velocity_threshold();
+	static bool is_sleep_allowed();
+	static float get_sleep_velocity_threshold();
+	static float get_sleep_time_threshold();
+	static float get_ccd_movement_threshold();
+	static float get_ccd_max_penetration();
+	static bool is_body_pair_contact_cache_enabled();
+	static float get_body_pair_cache_distance_sq();
+	static float get_body_pair_cache_angle_cos_div2();
+
+	static bool use_enhanced_internal_edge_removal_for_queries();
+	static bool enable_ray_cast_face_index();
+
+	static bool use_enhanced_internal_edge_removal_for_motion_queries();
+	static int get_motion_query_recovery_iterations();
+	static float get_motion_query_recovery_amount();
+
+	static float get_collision_margin_fraction();
+	static float get_active_edge_threshold();
+
+	static bool use_joint_world_node_a();
+
+	static int get_temp_memory_mib();
+	static int64_t get_temp_memory_b();
+	static float get_world_boundary_shape_size();
+	static float get_max_linear_velocity();
+	static float get_max_angular_velocity();
+	static int get_max_bodies();
+	static int get_max_pairs();
+	static int get_max_contact_constraints();
+};
+
+#endif // JOLT_PROJECT_SETTINGS_H
diff --git a/modules/jolt_physics/misc/jolt_stream_wrappers.h b/modules/jolt_physics/misc/jolt_stream_wrappers.h
new file mode 100644
index 0000000000..0200a45157
--- /dev/null
+++ b/modules/jolt_physics/misc/jolt_stream_wrappers.h
@@ -0,0 +1,81 @@
+/**************************************************************************/
+/*  jolt_stream_wrappers.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_STREAM_WRAPPERS_H
+#define JOLT_STREAM_WRAPPERS_H
+
+#ifdef DEBUG_ENABLED
+
+#include "core/io/file_access.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Core/StreamIn.h"
+#include "Jolt/Core/StreamOut.h"
+
+class JoltStreamOutputWrapper final : public JPH::StreamOut {
+	Ref<FileAccess> file_access;
+
+public:
+	explicit JoltStreamOutputWrapper(const Ref<FileAccess> &p_file_access) :
+			file_access(p_file_access) {}
+
+	virtual void WriteBytes(const void *p_data, size_t p_bytes) override {
+		file_access->store_buffer(static_cast<const uint8_t *>(p_data), static_cast<uint64_t>(p_bytes));
+	}
+
+	virtual bool IsFailed() const override {
+		return file_access->get_error() != OK;
+	}
+};
+
+class JoltStreamInputWrapper final : public JPH::StreamIn {
+	Ref<FileAccess> file_access;
+
+public:
+	explicit JoltStreamInputWrapper(const Ref<FileAccess> &p_file_access) :
+			file_access(p_file_access) {}
+
+	virtual void ReadBytes(void *p_data, size_t p_bytes) override {
+		file_access->get_buffer(static_cast<uint8_t *>(p_data), static_cast<uint64_t>(p_bytes));
+	}
+
+	virtual bool IsEOF() const override {
+		return file_access->eof_reached();
+	}
+
+	virtual bool IsFailed() const override {
+		return file_access->get_error() != OK;
+	}
+};
+
+#endif
+
+#endif // JOLT_STREAM_WRAPPERS_H
diff --git a/modules/jolt_physics/misc/jolt_type_conversions.h b/modules/jolt_physics/misc/jolt_type_conversions.h
new file mode 100644
index 0000000000..f735d6e06b
--- /dev/null
+++ b/modules/jolt_physics/misc/jolt_type_conversions.h
@@ -0,0 +1,147 @@
+/**************************************************************************/
+/*  jolt_type_conversions.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_TYPE_CONVERSIONS_H
+#define JOLT_TYPE_CONVERSIONS_H
+
+#include "core/math/aabb.h"
+#include "core/math/color.h"
+#include "core/math/plane.h"
+#include "core/math/quaternion.h"
+#include "core/math/transform_3d.h"
+#include "core/string/ustring.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Core/Color.h"
+#include "Jolt/Geometry/AABox.h"
+#include "Jolt/Geometry/Plane.h"
+#include "Jolt/Math/Mat44.h"
+#include "Jolt/Math/Quat.h"
+#include "Jolt/Math/Vec3.h"
+
+_FORCE_INLINE_ Vector3 to_godot(const JPH::Vec3 &p_vec) {
+	return Vector3((real_t)p_vec.GetX(), (real_t)p_vec.GetY(), (real_t)p_vec.GetZ());
+}
+
+_FORCE_INLINE_ Vector3 to_godot(const JPH::DVec3 &p_vec) {
+	return Vector3((real_t)p_vec.GetX(), (real_t)p_vec.GetY(), (real_t)p_vec.GetZ());
+}
+
+_FORCE_INLINE_ Basis to_godot(const JPH::Quat &p_quat) {
+	return Basis(Quaternion(p_quat.GetX(), p_quat.GetY(), p_quat.GetZ(), p_quat.GetW()));
+}
+
+_FORCE_INLINE_ Transform3D to_godot(const JPH::Mat44 &p_mat) {
+	return Transform3D(
+			Vector3(p_mat(0, 0), p_mat(1, 0), p_mat(2, 0)),
+			Vector3(p_mat(0, 1), p_mat(1, 1), p_mat(2, 1)),
+			Vector3(p_mat(0, 2), p_mat(1, 2), p_mat(2, 2)),
+			Vector3(p_mat(0, 3), p_mat(1, 3), p_mat(2, 3)));
+}
+
+_FORCE_INLINE_ Color to_godot(const JPH::Color &p_color) {
+	const float r = (float)p_color.r;
+	const float g = (float)p_color.g;
+	const float b = (float)p_color.b;
+	const float a = (float)p_color.a;
+
+	return Color(
+			r == 0.0f ? 0.0f : 255.0f / r,
+			g == 0.0f ? 0.0f : 255.0f / g,
+			b == 0.0f ? 0.0f : 255.0f / b,
+			a == 0.0f ? 0.0f : 255.0f / a);
+}
+
+_FORCE_INLINE_ String to_godot(const JPH::String &p_str) {
+	return String::utf8(p_str.c_str(), (int)p_str.length());
+}
+
+_FORCE_INLINE_ AABB to_godot(const JPH::AABox &p_aabb) {
+	return AABB(to_godot(p_aabb.mMin), to_godot(p_aabb.mMax - p_aabb.mMin));
+}
+
+_FORCE_INLINE_ Plane to_godot(const JPH::Plane &p_plane) {
+	return Plane(to_godot(p_plane.GetNormal()), (real_t)p_plane.GetConstant());
+}
+
+_FORCE_INLINE_ JPH::Vec3 to_jolt(const Vector3 &p_vec) {
+	return JPH::Vec3((float)p_vec.x, (float)p_vec.y, (float)p_vec.z);
+}
+
+_FORCE_INLINE_ JPH::Quat to_jolt(const Basis &p_basis) {
+	const Quaternion quat = p_basis.get_quaternion().normalized();
+	return JPH::Quat((float)quat.x, (float)quat.y, (float)quat.z, (float)quat.w);
+}
+
+_FORCE_INLINE_ JPH::Mat44 to_jolt(const Transform3D &p_transform) {
+	const Basis &b = p_transform.basis;
+	const Vector3 &o = p_transform.origin;
+
+	return JPH::Mat44(
+			JPH::Vec4(b[0][0], b[1][0], b[2][0], 0.0f),
+			JPH::Vec4(b[0][1], b[1][1], b[2][1], 0.0f),
+			JPH::Vec4(b[0][2], b[1][2], b[2][2], 0.0f),
+			JPH::Vec3(o.x, o.y, o.z));
+}
+
+_FORCE_INLINE_ JPH::Color to_jolt(const Color &p_color) {
+	return JPH::Color((JPH::uint32)p_color.to_abgr32());
+}
+
+_FORCE_INLINE_ JPH::String to_jolt(const String &p_str) {
+	const CharString str_utf8 = p_str.utf8();
+	return JPH::String(str_utf8.get_data(), (size_t)str_utf8.length());
+}
+
+_FORCE_INLINE_ JPH::AABox to_jolt(const AABB &p_aabb) {
+	return JPH::AABox(to_jolt(p_aabb.position), to_jolt(p_aabb.position + p_aabb.size));
+}
+
+_FORCE_INLINE_ JPH::Plane to_jolt(const Plane &p_plane) {
+	return JPH::Plane(to_jolt(p_plane.normal), (float)p_plane.d);
+}
+
+_FORCE_INLINE_ JPH::RVec3 to_jolt_r(const Vector3 &p_vec) {
+	return JPH::RVec3(p_vec.x, p_vec.y, p_vec.z);
+}
+
+_FORCE_INLINE_ JPH::RMat44 to_jolt_r(const Transform3D &p_transform) {
+	const Basis &b = p_transform.basis;
+	const Vector3 &o = p_transform.origin;
+
+	return JPH::RMat44(
+			JPH::Vec4(b[0][0], b[1][0], b[2][0], 0.0f),
+			JPH::Vec4(b[0][1], b[1][1], b[2][1], 0.0f),
+			JPH::Vec4(b[0][2], b[1][2], b[2][2], 0.0f),
+			JPH::RVec3(o.x, o.y, o.z));
+}
+
+#endif // JOLT_TYPE_CONVERSIONS_H
diff --git a/modules/jolt_physics/objects/jolt_area_3d.cpp b/modules/jolt_physics/objects/jolt_area_3d.cpp
new file mode 100644
index 0000000000..50ee2699a2
--- /dev/null
+++ b/modules/jolt_physics/objects/jolt_area_3d.cpp
@@ -0,0 +1,665 @@
+/**************************************************************************/
+/*  jolt_area_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_area_3d.h"
+
+#include "../jolt_project_settings.h"
+#include "../misc/jolt_type_conversions.h"
+#include "../shapes/jolt_shape_3d.h"
+#include "../spaces/jolt_broad_phase_layer.h"
+#include "../spaces/jolt_space_3d.h"
+#include "jolt_body_3d.h"
+#include "jolt_group_filter.h"
+#include "jolt_soft_body_3d.h"
+
+namespace {
+
+constexpr double DEFAULT_WIND_FORCE_MAGNITUDE = 0.0;
+constexpr double DEFAULT_WIND_ATTENUATION_FACTOR = 0.0;
+
+const Vector3 DEFAULT_WIND_SOURCE = Vector3();
+const Vector3 DEFAULT_WIND_DIRECTION = Vector3();
+
+} // namespace
+
+JPH::BroadPhaseLayer JoltArea3D::_get_broad_phase_layer() const {
+	return monitorable ? JoltBroadPhaseLayer::AREA_DETECTABLE : JoltBroadPhaseLayer::AREA_UNDETECTABLE;
+}
+
+JPH::ObjectLayer JoltArea3D::_get_object_layer() const {
+	ERR_FAIL_NULL_V(space, 0);
+
+	return space->map_to_object_layer(_get_broad_phase_layer(), collision_layer, collision_mask);
+}
+
+void JoltArea3D::_add_to_space() {
+	jolt_shape = build_shape();
+
+	JPH::CollisionGroup::GroupID group_id = 0;
+	JPH::CollisionGroup::SubGroupID sub_group_id = 0;
+	JoltGroupFilter::encode_object(this, group_id, sub_group_id);
+
+	jolt_settings->mUserData = reinterpret_cast<JPH::uint64>(this);
+	jolt_settings->mObjectLayer = _get_object_layer();
+	jolt_settings->mCollisionGroup = JPH::CollisionGroup(nullptr, group_id, sub_group_id);
+	jolt_settings->mMotionType = _get_motion_type();
+	jolt_settings->mIsSensor = true;
+	jolt_settings->mUseManifoldReduction = false;
+
+	if (JoltProjectSettings::areas_detect_static_bodies()) {
+		jolt_settings->mCollideKinematicVsNonDynamic = true;
+	}
+
+	jolt_settings->SetShape(build_shape());
+
+	const JPH::BodyID new_jolt_id = space->add_rigid_body(*this, *jolt_settings);
+	if (new_jolt_id.IsInvalid()) {
+		return;
+	}
+
+	jolt_id = new_jolt_id;
+
+	delete jolt_settings;
+	jolt_settings = nullptr;
+}
+
+void JoltArea3D::_add_shape_pair(Overlap &p_overlap, const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id) {
+	const JoltReadableBody3D other_jolt_body = space->read_body(p_body_id);
+	const JoltShapedObject3D *other_object = other_jolt_body.as_shaped();
+	ERR_FAIL_NULL(other_object);
+
+	p_overlap.rid = other_object->get_rid();
+	p_overlap.instance_id = other_object->get_instance_id();
+
+	ShapeIndexPair &shape_indices = p_overlap.shape_pairs[{ p_other_shape_id, p_self_shape_id }];
+
+	shape_indices.other = other_object->find_shape_index(p_other_shape_id);
+	shape_indices.self = find_shape_index(p_self_shape_id);
+
+	p_overlap.pending_added.push_back(shape_indices);
+}
+
+bool JoltArea3D::_remove_shape_pair(Overlap &p_overlap, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id) {
+	HashMap<ShapeIDPair, ShapeIndexPair, ShapeIDPair>::Iterator shape_pair = p_overlap.shape_pairs.find(ShapeIDPair(p_other_shape_id, p_self_shape_id));
+
+	if (shape_pair == p_overlap.shape_pairs.end()) {
+		return false;
+	}
+
+	p_overlap.pending_removed.push_back(shape_pair->value);
+	p_overlap.shape_pairs.remove(shape_pair);
+
+	return true;
+}
+
+void JoltArea3D::_flush_events(OverlapsById &p_objects, const Callable &p_callback) {
+	for (OverlapsById::Iterator E = p_objects.begin(); E;) {
+		Overlap &overlap = E->value;
+
+		if (p_callback.is_valid()) {
+			for (ShapeIndexPair &shape_indices : overlap.pending_removed) {
+				_report_event(p_callback, PhysicsServer3D::AREA_BODY_REMOVED, overlap.rid, overlap.instance_id, shape_indices.other, shape_indices.self);
+			}
+
+			for (ShapeIndexPair &shape_indices : overlap.pending_added) {
+				_report_event(p_callback, PhysicsServer3D::AREA_BODY_ADDED, overlap.rid, overlap.instance_id, shape_indices.other, shape_indices.self);
+			}
+		}
+
+		overlap.pending_removed.clear();
+		overlap.pending_added.clear();
+
+		OverlapsById::Iterator next = E;
+		++next;
+
+		if (overlap.shape_pairs.is_empty()) {
+			p_objects.remove(E);
+		}
+
+		E = next;
+	}
+}
+
+void JoltArea3D::_report_event(const Callable &p_callback, PhysicsServer3D::AreaBodyStatus p_status, const RID &p_other_rid, ObjectID p_other_instance_id, int p_other_shape_index, int p_self_shape_index) const {
+	ERR_FAIL_COND(!p_callback.is_valid());
+
+	const Variant arg1 = p_status;
+	const Variant arg2 = p_other_rid;
+	const Variant arg3 = p_other_instance_id;
+	const Variant arg4 = p_other_shape_index;
+	const Variant arg5 = p_self_shape_index;
+	const Variant *args[5] = { &arg1, &arg2, &arg3, &arg4, &arg5 };
+
+	Callable::CallError ce;
+	Variant ret;
+	p_callback.callp(args, 5, ret, ce);
+
+	if (unlikely(ce.error != Callable::CallError::CALL_OK)) {
+		ERR_PRINT_ONCE(vformat("Failed to call area monitor callback for '%s'. It returned the following error: '%s'.", to_string(), Variant::get_callable_error_text(p_callback, args, 5, ce)));
+	}
+}
+
+void JoltArea3D::_notify_body_entered(const JPH::BodyID &p_body_id) {
+	const JoltReadableBody3D jolt_body = space->read_body(p_body_id);
+
+	JoltBody3D *body = jolt_body.as_body();
+	if (unlikely(body == nullptr)) {
+		return;
+	}
+
+	body->add_area(this);
+}
+
+void JoltArea3D::_notify_body_exited(const JPH::BodyID &p_body_id) {
+	const JoltReadableBody3D jolt_body = space->read_body(p_body_id);
+
+	JoltBody3D *body = jolt_body.as_body();
+	if (unlikely(body == nullptr)) {
+		return;
+	}
+
+	body->remove_area(this);
+}
+
+void JoltArea3D::_force_bodies_entered() {
+	for (KeyValue<JPH::BodyID, Overlap> &E : bodies_by_id) {
+		Overlap &body = E.value;
+
+		for (const KeyValue<ShapeIDPair, ShapeIndexPair> &P : body.shape_pairs) {
+			body.pending_removed.erase(P.value);
+			body.pending_added.push_back(P.value);
+		}
+	}
+}
+
+void JoltArea3D::_force_bodies_exited(bool p_remove) {
+	for (KeyValue<JPH::BodyID, Overlap> &E : bodies_by_id) {
+		const JPH::BodyID &id = E.key;
+		Overlap &body = E.value;
+
+		for (const KeyValue<ShapeIDPair, ShapeIndexPair> &P : body.shape_pairs) {
+			body.pending_added.erase(P.value);
+			body.pending_removed.push_back(P.value);
+		}
+
+		if (p_remove) {
+			body.shape_pairs.clear();
+			_notify_body_exited(id);
+		}
+	}
+}
+
+void JoltArea3D::_force_areas_entered() {
+	for (KeyValue<JPH::BodyID, Overlap> &E : areas_by_id) {
+		Overlap &area = E.value;
+
+		for (const KeyValue<ShapeIDPair, ShapeIndexPair> &P : area.shape_pairs) {
+			area.pending_removed.erase(P.value);
+			area.pending_added.push_back(P.value);
+		}
+	}
+}
+
+void JoltArea3D::_force_areas_exited(bool p_remove) {
+	for (KeyValue<JPH::BodyID, Overlap> &E : areas_by_id) {
+		Overlap &area = E.value;
+
+		for (const KeyValue<ShapeIDPair, ShapeIndexPair> &P : area.shape_pairs) {
+			area.pending_added.erase(P.value);
+			area.pending_removed.push_back(P.value);
+		}
+
+		if (p_remove) {
+			area.shape_pairs.clear();
+		}
+	}
+}
+
+void JoltArea3D::_update_group_filter() {
+	if (!in_space()) {
+		return;
+	}
+
+	const JoltWritableBody3D body = space->write_body(jolt_id);
+	ERR_FAIL_COND(body.is_invalid());
+
+	body->GetCollisionGroup().SetGroupFilter(JoltGroupFilter::instance);
+}
+
+void JoltArea3D::_update_default_gravity() {
+	if (is_default_area()) {
+		space->get_physics_system().SetGravity(to_jolt(gravity_vector) * gravity);
+	}
+}
+
+void JoltArea3D::_space_changing() {
+	JoltShapedObject3D::_space_changing();
+
+	if (space != nullptr) {
+		// Ideally we would rely on our contact listener to report all the exits when we move
+		// between (or out of) spaces, but because our Jolt body is going to be destroyed when we
+		// leave this space the contact listener won't be able to retrieve the corresponding area
+		// and as such cannot report any exits, so we're forced to do it manually instead.
+		_force_bodies_exited(true);
+		_force_areas_exited(true);
+	}
+}
+
+void JoltArea3D::_space_changed() {
+	JoltShapedObject3D::_space_changed();
+
+	_update_group_filter();
+	_update_default_gravity();
+}
+
+void JoltArea3D::_body_monitoring_changed() {
+	if (has_body_monitor_callback()) {
+		_force_bodies_entered();
+	} else {
+		_force_bodies_exited(false);
+	}
+}
+
+void JoltArea3D::_area_monitoring_changed() {
+	if (has_area_monitor_callback()) {
+		_force_areas_entered();
+	} else {
+		_force_areas_exited(false);
+	}
+}
+
+void JoltArea3D::_monitorable_changed() {
+	_update_object_layer();
+}
+
+void JoltArea3D::_gravity_changed() {
+	_update_default_gravity();
+}
+
+JoltArea3D::JoltArea3D() :
+		JoltShapedObject3D(OBJECT_TYPE_AREA) {
+}
+
+bool JoltArea3D::is_default_area() const {
+	return space != nullptr && space->get_default_area() == this;
+}
+
+void JoltArea3D::set_default_area(bool p_value) {
+	if (p_value) {
+		_update_default_gravity();
+	}
+}
+
+void JoltArea3D::set_transform(Transform3D p_transform) {
+	JOLT_ENSURE_SCALE_NOT_ZERO(p_transform, vformat("An invalid transform was passed to area '%s'.", to_string()));
+
+	const Vector3 new_scale = p_transform.basis.get_scale();
+
+	// Ideally we would do an exact comparison here, but due to floating-point precision this would be invalidated very often.
+	if (!scale.is_equal_approx(new_scale)) {
+		scale = new_scale;
+		_shapes_changed();
+	}
+
+	p_transform.basis.orthonormalize();
+
+	if (!in_space()) {
+		jolt_settings->mPosition = to_jolt_r(p_transform.origin);
+		jolt_settings->mRotation = to_jolt(p_transform.basis);
+	} else {
+		space->get_body_iface().SetPositionAndRotation(jolt_id, to_jolt_r(p_transform.origin), to_jolt(p_transform.basis), JPH::EActivation::DontActivate);
+	}
+}
+
+Variant JoltArea3D::get_param(PhysicsServer3D::AreaParameter p_param) const {
+	switch (p_param) {
+		case PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE: {
+			return get_gravity_mode();
+		}
+		case PhysicsServer3D::AREA_PARAM_GRAVITY: {
+			return get_gravity();
+		}
+		case PhysicsServer3D::AREA_PARAM_GRAVITY_VECTOR: {
+			return get_gravity_vector();
+		}
+		case PhysicsServer3D::AREA_PARAM_GRAVITY_IS_POINT: {
+			return is_point_gravity();
+		}
+		case PhysicsServer3D::AREA_PARAM_GRAVITY_POINT_UNIT_DISTANCE: {
+			return get_point_gravity_distance();
+		}
+		case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE: {
+			return get_linear_damp_mode();
+		}
+		case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP: {
+			return get_linear_damp();
+		}
+		case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE: {
+			return get_angular_damp_mode();
+		}
+		case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP: {
+			return get_angular_damp();
+		}
+		case PhysicsServer3D::AREA_PARAM_PRIORITY: {
+			return get_priority();
+		}
+		case PhysicsServer3D::AREA_PARAM_WIND_FORCE_MAGNITUDE: {
+			return DEFAULT_WIND_FORCE_MAGNITUDE;
+		}
+		case PhysicsServer3D::AREA_PARAM_WIND_SOURCE: {
+			return DEFAULT_WIND_SOURCE;
+		}
+		case PhysicsServer3D::AREA_PARAM_WIND_DIRECTION: {
+			return DEFAULT_WIND_DIRECTION;
+		}
+		case PhysicsServer3D::AREA_PARAM_WIND_ATTENUATION_FACTOR: {
+			return DEFAULT_WIND_ATTENUATION_FACTOR;
+		}
+		default: {
+			ERR_FAIL_V_MSG(Variant(), vformat("Unhandled area parameter: '%d'. This should not happen. Please report this.", p_param));
+		}
+	}
+}
+
+void JoltArea3D::set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value) {
+	switch (p_param) {
+		case PhysicsServer3D::AREA_PARAM_GRAVITY_OVERRIDE_MODE: {
+			set_gravity_mode((OverrideMode)(int)p_value);
+		} break;
+		case PhysicsServer3D::AREA_PARAM_GRAVITY: {
+			set_gravity(p_value);
+		} break;
+		case PhysicsServer3D::AREA_PARAM_GRAVITY_VECTOR: {
+			set_gravity_vector(p_value);
+		} break;
+		case PhysicsServer3D::AREA_PARAM_GRAVITY_IS_POINT: {
+			set_point_gravity(p_value);
+		} break;
+		case PhysicsServer3D::AREA_PARAM_GRAVITY_POINT_UNIT_DISTANCE: {
+			set_point_gravity_distance(p_value);
+		} break;
+		case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP_OVERRIDE_MODE: {
+			set_linear_damp_mode((OverrideMode)(int)p_value);
+		} break;
+		case PhysicsServer3D::AREA_PARAM_LINEAR_DAMP: {
+			set_area_linear_damp(p_value);
+		} break;
+		case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP_OVERRIDE_MODE: {
+			set_angular_damp_mode((OverrideMode)(int)p_value);
+		} break;
+		case PhysicsServer3D::AREA_PARAM_ANGULAR_DAMP: {
+			set_area_angular_damp(p_value);
+		} break;
+		case PhysicsServer3D::AREA_PARAM_PRIORITY: {
+			set_priority(p_value);
+		} break;
+		case PhysicsServer3D::AREA_PARAM_WIND_FORCE_MAGNITUDE: {
+			if (!Math::is_equal_approx((double)p_value, DEFAULT_WIND_FORCE_MAGNITUDE)) {
+				WARN_PRINT(vformat("Invalid wind force magnitude for '%s'. Area wind force magnitude is not supported when using Jolt Physics. Any such value will be ignored.", to_string()));
+			}
+		} break;
+		case PhysicsServer3D::AREA_PARAM_WIND_SOURCE: {
+			if (!((Vector3)p_value).is_equal_approx(DEFAULT_WIND_SOURCE)) {
+				WARN_PRINT(vformat("Invalid wind source for '%s'. Area wind source is not supported when using Jolt Physics. Any such value will be ignored.", to_string()));
+			}
+		} break;
+		case PhysicsServer3D::AREA_PARAM_WIND_DIRECTION: {
+			if (!((Vector3)p_value).is_equal_approx(DEFAULT_WIND_DIRECTION)) {
+				WARN_PRINT(vformat("Invalid wind direction for '%s'. Area wind direction is not supported when using Jolt Physics. Any such value will be ignored.", to_string()));
+			}
+		} break;
+		case PhysicsServer3D::AREA_PARAM_WIND_ATTENUATION_FACTOR: {
+			if (!Math::is_equal_approx((double)p_value, DEFAULT_WIND_ATTENUATION_FACTOR)) {
+				WARN_PRINT(vformat("Invalid wind attenuation for '%s'. Area wind attenuation is not supported when using Jolt Physics. Any such value will be ignored.", to_string()));
+			}
+		} break;
+		default: {
+			ERR_FAIL_MSG(vformat("Unhandled area parameter: '%d'. This should not happen. Please report this.", p_param));
+		} break;
+	}
+}
+
+void JoltArea3D::set_body_monitor_callback(const Callable &p_callback) {
+	if (p_callback == body_monitor_callback) {
+		return;
+	}
+
+	body_monitor_callback = p_callback;
+
+	_body_monitoring_changed();
+}
+
+void JoltArea3D::set_area_monitor_callback(const Callable &p_callback) {
+	if (p_callback == area_monitor_callback) {
+		return;
+	}
+
+	area_monitor_callback = p_callback;
+
+	_area_monitoring_changed();
+}
+
+void JoltArea3D::set_monitorable(bool p_monitorable) {
+	if (p_monitorable == monitorable) {
+		return;
+	}
+
+	monitorable = p_monitorable;
+
+	_monitorable_changed();
+}
+
+bool JoltArea3D::can_monitor(const JoltBody3D &p_other) const {
+	return (collision_mask & p_other.get_collision_layer()) != 0;
+}
+
+bool JoltArea3D::can_monitor(const JoltSoftBody3D &p_other) const {
+	return false;
+}
+
+bool JoltArea3D::can_monitor(const JoltArea3D &p_other) const {
+	return p_other.is_monitorable() && (collision_mask & p_other.get_collision_layer()) != 0;
+}
+
+bool JoltArea3D::can_interact_with(const JoltBody3D &p_other) const {
+	return can_monitor(p_other);
+}
+
+bool JoltArea3D::can_interact_with(const JoltSoftBody3D &p_other) const {
+	return false;
+}
+
+bool JoltArea3D::can_interact_with(const JoltArea3D &p_other) const {
+	return can_monitor(p_other) || p_other.can_monitor(*this);
+}
+
+Vector3 JoltArea3D::get_velocity_at_position(const Vector3 &p_position) const {
+	return Vector3();
+}
+
+void JoltArea3D::set_point_gravity(bool p_enabled) {
+	if (point_gravity == p_enabled) {
+		return;
+	}
+
+	point_gravity = p_enabled;
+
+	_gravity_changed();
+}
+
+void JoltArea3D::set_gravity(float p_gravity) {
+	if (gravity == p_gravity) {
+		return;
+	}
+
+	gravity = p_gravity;
+
+	_gravity_changed();
+}
+
+void JoltArea3D::set_point_gravity_distance(float p_distance) {
+	if (point_gravity_distance == p_distance) {
+		return;
+	}
+
+	point_gravity_distance = p_distance;
+
+	_gravity_changed();
+}
+
+void JoltArea3D::set_gravity_mode(OverrideMode p_mode) {
+	if (gravity_mode == p_mode) {
+		return;
+	}
+
+	gravity_mode = p_mode;
+
+	_gravity_changed();
+}
+
+void JoltArea3D::set_gravity_vector(const Vector3 &p_vector) {
+	if (gravity_vector == p_vector) {
+		return;
+	}
+
+	gravity_vector = p_vector;
+
+	_gravity_changed();
+}
+
+Vector3 JoltArea3D::compute_gravity(const Vector3 &p_position) const {
+	if (!point_gravity) {
+		return gravity_vector * gravity;
+	}
+
+	const Vector3 point = get_transform_scaled().xform(gravity_vector);
+	const Vector3 to_point = point - p_position;
+	const real_t to_point_dist_sq = MAX(to_point.length_squared(), (real_t)CMP_EPSILON);
+	const Vector3 to_point_dir = to_point / Math::sqrt(to_point_dist_sq);
+
+	if (point_gravity_distance == 0.0f) {
+		return to_point_dir * gravity;
+	}
+
+	const float gravity_dist_sq = point_gravity_distance * point_gravity_distance;
+
+	return to_point_dir * (gravity * gravity_dist_sq / to_point_dist_sq);
+}
+
+void JoltArea3D::body_shape_entered(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id) {
+	Overlap &overlap = bodies_by_id[p_body_id];
+
+	if (overlap.shape_pairs.is_empty()) {
+		_notify_body_entered(p_body_id);
+	}
+
+	_add_shape_pair(overlap, p_body_id, p_other_shape_id, p_self_shape_id);
+}
+
+bool JoltArea3D::body_shape_exited(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id) {
+	Overlap *overlap = bodies_by_id.getptr(p_body_id);
+
+	if (overlap == nullptr) {
+		return false;
+	}
+
+	if (!_remove_shape_pair(*overlap, p_other_shape_id, p_self_shape_id)) {
+		return false;
+	}
+
+	if (overlap->shape_pairs.is_empty()) {
+		_notify_body_exited(p_body_id);
+	}
+
+	return true;
+}
+
+void JoltArea3D::area_shape_entered(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id) {
+	_add_shape_pair(areas_by_id[p_body_id], p_body_id, p_other_shape_id, p_self_shape_id);
+}
+
+bool JoltArea3D::area_shape_exited(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id) {
+	Overlap *overlap = areas_by_id.getptr(p_body_id);
+
+	if (overlap == nullptr) {
+		return false;
+	}
+
+	return _remove_shape_pair(*overlap, p_other_shape_id, p_self_shape_id);
+}
+
+bool JoltArea3D::shape_exited(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id) {
+	return body_shape_exited(p_body_id, p_other_shape_id, p_self_shape_id) || area_shape_exited(p_body_id, p_other_shape_id, p_self_shape_id);
+}
+
+void JoltArea3D::body_exited(const JPH::BodyID &p_body_id, bool p_notify) {
+	Overlap *overlap = bodies_by_id.getptr(p_body_id);
+	if (unlikely(overlap == nullptr)) {
+		return;
+	}
+
+	if (unlikely(overlap->shape_pairs.is_empty())) {
+		return;
+	}
+
+	for (KeyValue<ShapeIDPair, ShapeIndexPair> &E : overlap->shape_pairs) {
+		overlap->pending_added.erase(E.value);
+		overlap->pending_removed.push_back(E.value);
+	}
+
+	overlap->shape_pairs.clear();
+
+	if (p_notify) {
+		_notify_body_exited(p_body_id);
+	}
+}
+
+void JoltArea3D::area_exited(const JPH::BodyID &p_body_id) {
+	Overlap *overlap = areas_by_id.getptr(p_body_id);
+	if (unlikely(overlap == nullptr)) {
+		return;
+	}
+
+	if (unlikely(overlap->shape_pairs.is_empty())) {
+		return;
+	}
+
+	for (const KeyValue<ShapeIDPair, ShapeIndexPair> &E : overlap->shape_pairs) {
+		overlap->pending_added.erase(E.value);
+		overlap->pending_removed.push_back(E.value);
+	}
+
+	overlap->shape_pairs.clear();
+}
+
+void JoltArea3D::call_queries(JPH::Body &p_jolt_body) {
+	_flush_events(bodies_by_id, body_monitor_callback);
+	_flush_events(areas_by_id, area_monitor_callback);
+}
diff --git a/modules/jolt_physics/objects/jolt_area_3d.h b/modules/jolt_physics/objects/jolt_area_3d.h
new file mode 100644
index 0000000000..d151a7d05a
--- /dev/null
+++ b/modules/jolt_physics/objects/jolt_area_3d.h
@@ -0,0 +1,226 @@
+/**************************************************************************/
+/*  jolt_area_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_AREA_3D_H
+#define JOLT_AREA_3D_H
+
+#include "jolt_shaped_object_3d.h"
+
+#include "servers/physics_server_3d.h"
+
+class JoltBody3D;
+class JoltSoftBody3D;
+
+class JoltArea3D final : public JoltShapedObject3D {
+public:
+	typedef PhysicsServer3D::AreaSpaceOverrideMode OverrideMode;
+
+private:
+	struct BodyIDHasher {
+		static uint32_t hash(const JPH::BodyID &p_id) { return hash_fmix32(p_id.GetIndexAndSequenceNumber()); }
+	};
+
+	struct ShapeIDPair {
+		JPH::SubShapeID other;
+		JPH::SubShapeID self;
+
+		ShapeIDPair(JPH::SubShapeID p_other, JPH::SubShapeID p_self) :
+				other(p_other), self(p_self) {}
+
+		static uint32_t hash(const ShapeIDPair &p_pair) {
+			uint32_t hash = hash_murmur3_one_32(p_pair.other.GetValue());
+			hash = hash_murmur3_one_32(p_pair.self.GetValue(), hash);
+			return hash_fmix32(hash);
+		}
+
+		friend bool operator==(const ShapeIDPair &p_lhs, const ShapeIDPair &p_rhs) {
+			return (p_lhs.other == p_rhs.other) && (p_lhs.self == p_rhs.self);
+		}
+	};
+
+	struct ShapeIndexPair {
+		int other = -1;
+		int self = -1;
+
+		ShapeIndexPair() = default;
+
+		ShapeIndexPair(int p_other, int p_self) :
+				other(p_other), self(p_self) {}
+
+		friend bool operator==(const ShapeIndexPair &p_lhs, const ShapeIndexPair &p_rhs) {
+			return (p_lhs.other == p_rhs.other) && (p_lhs.self == p_rhs.self);
+		}
+	};
+
+	struct Overlap {
+		HashMap<ShapeIDPair, ShapeIndexPair, ShapeIDPair> shape_pairs;
+		LocalVector<ShapeIndexPair> pending_added;
+		LocalVector<ShapeIndexPair> pending_removed;
+		RID rid;
+		ObjectID instance_id;
+	};
+
+	typedef HashMap<JPH::BodyID, Overlap, BodyIDHasher> OverlapsById;
+
+	OverlapsById bodies_by_id;
+	OverlapsById areas_by_id;
+
+	Vector3 gravity_vector = Vector3(0, -1, 0);
+
+	Callable body_monitor_callback;
+	Callable area_monitor_callback;
+
+	float priority = 0.0f;
+	float gravity = 9.8f;
+	float point_gravity_distance = 0.0f;
+	float linear_damp = 0.1f;
+	float angular_damp = 0.1f;
+
+	OverrideMode gravity_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED;
+	OverrideMode linear_damp_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED;
+	OverrideMode angular_damp_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED;
+
+	bool monitorable = false;
+	bool point_gravity = false;
+
+	virtual JPH::BroadPhaseLayer _get_broad_phase_layer() const override;
+	virtual JPH::ObjectLayer _get_object_layer() const override;
+
+	virtual JPH::EMotionType _get_motion_type() const override { return JPH::EMotionType::Kinematic; }
+
+	virtual void _add_to_space() override;
+
+	void _add_shape_pair(Overlap &p_overlap, const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id);
+	bool _remove_shape_pair(Overlap &p_overlap, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id);
+
+	void _flush_events(OverlapsById &p_objects, const Callable &p_callback);
+
+	void _report_event(const Callable &p_callback, PhysicsServer3D::AreaBodyStatus p_status, const RID &p_other_rid, ObjectID p_other_instance_id, int p_other_shape_index, int p_self_shape_index) const;
+
+	void _notify_body_entered(const JPH::BodyID &p_body_id);
+	void _notify_body_exited(const JPH::BodyID &p_body_id);
+
+	void _force_bodies_entered();
+	void _force_bodies_exited(bool p_remove);
+
+	void _force_areas_entered();
+	void _force_areas_exited(bool p_remove);
+
+	void _update_group_filter();
+	void _update_default_gravity();
+
+	virtual void _space_changing() override;
+	virtual void _space_changed() override;
+	void _body_monitoring_changed();
+	void _area_monitoring_changed();
+	void _monitorable_changed();
+	void _gravity_changed();
+
+public:
+	JoltArea3D();
+
+	bool is_default_area() const;
+	void set_default_area(bool p_value);
+
+	void set_transform(Transform3D p_transform);
+
+	Variant get_param(PhysicsServer3D::AreaParameter p_param) const;
+	void set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value);
+
+	bool has_body_monitor_callback() const { return body_monitor_callback.is_valid(); }
+	void set_body_monitor_callback(const Callable &p_callback);
+
+	bool has_area_monitor_callback() const { return area_monitor_callback.is_valid(); }
+	void set_area_monitor_callback(const Callable &p_callback);
+
+	bool is_monitorable() const { return monitorable; }
+	void set_monitorable(bool p_monitorable);
+
+	bool can_monitor(const JoltBody3D &p_other) const;
+	bool can_monitor(const JoltSoftBody3D &p_other) const;
+	bool can_monitor(const JoltArea3D &p_other) const;
+
+	virtual bool can_interact_with(const JoltBody3D &p_other) const override;
+	virtual bool can_interact_with(const JoltSoftBody3D &p_other) const override;
+	virtual bool can_interact_with(const JoltArea3D &p_other) const override;
+
+	virtual Vector3 get_velocity_at_position(const Vector3 &p_position) const override;
+
+	virtual bool reports_contacts() const override { return false; }
+
+	bool is_point_gravity() const { return point_gravity; }
+	void set_point_gravity(bool p_enabled);
+
+	float get_priority() const { return priority; }
+	void set_priority(float p_priority) { priority = p_priority; }
+
+	float get_gravity() const { return gravity; }
+	void set_gravity(float p_gravity);
+
+	float get_point_gravity_distance() const { return point_gravity_distance; }
+	void set_point_gravity_distance(float p_distance);
+
+	float get_linear_damp() const { return linear_damp; }
+	void set_area_linear_damp(float p_damp) { linear_damp = p_damp; }
+
+	float get_angular_damp() const { return angular_damp; }
+	void set_area_angular_damp(float p_damp) { angular_damp = p_damp; }
+
+	OverrideMode get_gravity_mode() const { return gravity_mode; }
+	void set_gravity_mode(OverrideMode p_mode);
+
+	OverrideMode get_linear_damp_mode() const { return linear_damp_mode; }
+	void set_linear_damp_mode(OverrideMode p_mode) { linear_damp_mode = p_mode; }
+
+	OverrideMode get_angular_damp_mode() const { return angular_damp_mode; }
+	void set_angular_damp_mode(OverrideMode p_mode) { angular_damp_mode = p_mode; }
+
+	Vector3 get_gravity_vector() const { return gravity_vector; }
+	void set_gravity_vector(const Vector3 &p_vector);
+
+	Vector3 compute_gravity(const Vector3 &p_position) const;
+
+	void body_shape_entered(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id);
+	bool body_shape_exited(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id);
+
+	void area_shape_entered(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id);
+	bool area_shape_exited(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id);
+
+	bool shape_exited(const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_other_shape_id, const JPH::SubShapeID &p_self_shape_id);
+	void body_exited(const JPH::BodyID &p_body_id, bool p_notify = true);
+	void area_exited(const JPH::BodyID &p_body_id);
+
+	void call_queries(JPH::Body &p_jolt_body);
+
+	virtual bool has_custom_center_of_mass() const override { return false; }
+	virtual Vector3 get_center_of_mass_custom() const override { return Vector3(); }
+};
+
+#endif // JOLT_AREA_3D_H
diff --git a/modules/jolt_physics/objects/jolt_body_3d.cpp b/modules/jolt_physics/objects/jolt_body_3d.cpp
new file mode 100644
index 0000000000..408a935973
--- /dev/null
+++ b/modules/jolt_physics/objects/jolt_body_3d.cpp
@@ -0,0 +1,1435 @@
+/**************************************************************************/
+/*  jolt_body_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_3d.h"
+
+#include "../joints/jolt_joint_3d.h"
+#include "../jolt_project_settings.h"
+#include "../misc/jolt_type_conversions.h"
+#include "../shapes/jolt_shape_3d.h"
+#include "../spaces/jolt_broad_phase_layer.h"
+#include "../spaces/jolt_space_3d.h"
+#include "jolt_area_3d.h"
+#include "jolt_group_filter.h"
+#include "jolt_physics_direct_body_state_3d.h"
+#include "jolt_soft_body_3d.h"
+
+namespace {
+
+template <typename TValue, typename TGetter>
+bool integrate(TValue &p_value, PhysicsServer3D::AreaSpaceOverrideMode p_mode, TGetter &&p_getter) {
+	switch (p_mode) {
+		case PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED: {
+			return false;
+		}
+		case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE: {
+			p_value += p_getter();
+			return false;
+		}
+		case PhysicsServer3D::AREA_SPACE_OVERRIDE_COMBINE_REPLACE: {
+			p_value += p_getter();
+			return true;
+		}
+		case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE: {
+			p_value = p_getter();
+			return true;
+		}
+		case PhysicsServer3D::AREA_SPACE_OVERRIDE_REPLACE_COMBINE: {
+			p_value = p_getter();
+			return false;
+		}
+		default: {
+			ERR_FAIL_V_MSG(false, vformat("Unhandled override mode: '%d'. This should not happen. Please report this.", p_mode));
+		}
+	}
+}
+
+} // namespace
+
+JPH::BroadPhaseLayer JoltBody3D::_get_broad_phase_layer() const {
+	switch (mode) {
+		case PhysicsServer3D::BODY_MODE_STATIC: {
+			return _is_big() ? JoltBroadPhaseLayer::BODY_STATIC_BIG : JoltBroadPhaseLayer::BODY_STATIC;
+		}
+		case PhysicsServer3D::BODY_MODE_KINEMATIC:
+		case PhysicsServer3D::BODY_MODE_RIGID:
+		case PhysicsServer3D::BODY_MODE_RIGID_LINEAR: {
+			return JoltBroadPhaseLayer::BODY_DYNAMIC;
+		}
+		default: {
+			ERR_FAIL_V_MSG(JoltBroadPhaseLayer::BODY_STATIC, vformat("Unhandled body mode: '%d'. This should not happen. Please report this.", mode));
+		}
+	}
+}
+
+JPH::ObjectLayer JoltBody3D::_get_object_layer() const {
+	ERR_FAIL_NULL_V(space, 0);
+
+	return space->map_to_object_layer(_get_broad_phase_layer(), collision_layer, collision_mask);
+}
+
+JPH::EMotionType JoltBody3D::_get_motion_type() const {
+	switch (mode) {
+		case PhysicsServer3D::BODY_MODE_STATIC: {
+			return JPH::EMotionType::Static;
+		}
+		case PhysicsServer3D::BODY_MODE_KINEMATIC: {
+			return JPH::EMotionType::Kinematic;
+		}
+		case PhysicsServer3D::BODY_MODE_RIGID:
+		case PhysicsServer3D::BODY_MODE_RIGID_LINEAR: {
+			return JPH::EMotionType::Dynamic;
+		}
+		default: {
+			ERR_FAIL_V_MSG(JPH::EMotionType::Static, vformat("Unhandled body mode: '%d'. This should not happen. Please report this.", mode));
+		}
+	}
+}
+
+void JoltBody3D::_add_to_space() {
+	jolt_shape = build_shape();
+
+	JPH::CollisionGroup::GroupID group_id = 0;
+	JPH::CollisionGroup::SubGroupID sub_group_id = 0;
+	JoltGroupFilter::encode_object(this, group_id, sub_group_id);
+
+	jolt_settings->mUserData = reinterpret_cast<JPH::uint64>(this);
+	jolt_settings->mObjectLayer = _get_object_layer();
+	jolt_settings->mCollisionGroup = JPH::CollisionGroup(nullptr, group_id, sub_group_id);
+	jolt_settings->mMotionType = _get_motion_type();
+	jolt_settings->mAllowedDOFs = _calculate_allowed_dofs();
+	jolt_settings->mAllowDynamicOrKinematic = true;
+	jolt_settings->mCollideKinematicVsNonDynamic = reports_all_kinematic_contacts();
+	jolt_settings->mUseManifoldReduction = !reports_contacts();
+	jolt_settings->mLinearDamping = 0.0f;
+	jolt_settings->mAngularDamping = 0.0f;
+	jolt_settings->mMaxLinearVelocity = JoltProjectSettings::get_max_linear_velocity();
+	jolt_settings->mMaxAngularVelocity = JoltProjectSettings::get_max_angular_velocity();
+
+	if (JoltProjectSettings::use_enhanced_internal_edge_removal_for_bodies()) {
+		jolt_settings->mEnhancedInternalEdgeRemoval = true;
+	}
+
+	jolt_settings->mOverrideMassProperties = JPH::EOverrideMassProperties::MassAndInertiaProvided;
+	jolt_settings->mMassPropertiesOverride = _calculate_mass_properties();
+
+	jolt_settings->SetShape(jolt_shape);
+
+	const JPH::BodyID new_jolt_id = space->add_rigid_body(*this, *jolt_settings, sleep_initially);
+	if (new_jolt_id.IsInvalid()) {
+		return;
+	}
+
+	jolt_id = new_jolt_id;
+
+	delete jolt_settings;
+	jolt_settings = nullptr;
+}
+
+void JoltBody3D::_integrate_forces(float p_step, JPH::Body &p_jolt_body) {
+	if (!p_jolt_body.IsActive()) {
+		return;
+	}
+
+	_update_gravity(p_jolt_body);
+
+	if (!custom_integrator) {
+		JPH::MotionProperties &motion_properties = *p_jolt_body.GetMotionPropertiesUnchecked();
+
+		JPH::Vec3 linear_velocity = motion_properties.GetLinearVelocity();
+		JPH::Vec3 angular_velocity = motion_properties.GetAngularVelocity();
+
+		// Jolt applies damping differently from Godot Physics, where Godot Physics applies damping before integrating
+		// forces whereas Jolt does it after integrating forces. The way Godot Physics does it seems to yield more
+		// consistent results across different update frequencies when using high (>1) damping values, so we apply the
+		// damping ourselves instead, before any force integration happens.
+
+		linear_velocity *= MAX(1.0f - total_linear_damp * p_step, 0.0f);
+		angular_velocity *= MAX(1.0f - total_angular_damp * p_step, 0.0f);
+
+		linear_velocity += to_jolt(gravity) * p_step;
+
+		motion_properties.SetLinearVelocityClamped(linear_velocity);
+		motion_properties.SetAngularVelocityClamped(angular_velocity);
+
+		p_jolt_body.AddForce(to_jolt(constant_force));
+		p_jolt_body.AddTorque(to_jolt(constant_torque));
+	}
+
+	sync_state = true;
+}
+
+void JoltBody3D::_move_kinematic(float p_step, JPH::Body &p_jolt_body) {
+	p_jolt_body.SetLinearVelocity(JPH::Vec3::sZero());
+	p_jolt_body.SetAngularVelocity(JPH::Vec3::sZero());
+
+	const JPH::RVec3 current_position = p_jolt_body.GetPosition();
+	const JPH::Quat current_rotation = p_jolt_body.GetRotation();
+
+	const JPH::RVec3 new_position = to_jolt_r(kinematic_transform.origin);
+	const JPH::Quat new_rotation = to_jolt(kinematic_transform.basis);
+
+	if (new_position == current_position && new_rotation == current_rotation) {
+		return;
+	}
+
+	p_jolt_body.MoveKinematic(new_position, new_rotation, p_step);
+
+	sync_state = true;
+}
+
+void JoltBody3D::_pre_step_static(float p_step, JPH::Body &p_jolt_body) {
+	// Nothing to do.
+}
+
+void JoltBody3D::_pre_step_rigid(float p_step, JPH::Body &p_jolt_body) {
+	_integrate_forces(p_step, p_jolt_body);
+}
+
+void JoltBody3D::_pre_step_kinematic(float p_step, JPH::Body &p_jolt_body) {
+	_update_gravity(p_jolt_body);
+
+	_move_kinematic(p_step, p_jolt_body);
+
+	if (reports_contacts()) {
+		// This seems to emulate the behavior of Godot Physics, where kinematic bodies are set as active (and thereby
+		// have their state synchronized on every step) only if its max reported contacts is non-zero.
+		sync_state = true;
+	}
+}
+
+JPH::EAllowedDOFs JoltBody3D::_calculate_allowed_dofs() const {
+	if (is_static()) {
+		return JPH::EAllowedDOFs::All;
+	}
+
+	JPH::EAllowedDOFs allowed_dofs = JPH::EAllowedDOFs::All;
+
+	if (is_axis_locked(PhysicsServer3D::BODY_AXIS_LINEAR_X)) {
+		allowed_dofs &= ~JPH::EAllowedDOFs::TranslationX;
+	}
+
+	if (is_axis_locked(PhysicsServer3D::BODY_AXIS_LINEAR_Y)) {
+		allowed_dofs &= ~JPH::EAllowedDOFs::TranslationY;
+	}
+
+	if (is_axis_locked(PhysicsServer3D::BODY_AXIS_LINEAR_Z)) {
+		allowed_dofs &= ~JPH::EAllowedDOFs::TranslationZ;
+	}
+
+	if (is_axis_locked(PhysicsServer3D::BODY_AXIS_ANGULAR_X) || is_rigid_linear()) {
+		allowed_dofs &= ~JPH::EAllowedDOFs::RotationX;
+	}
+
+	if (is_axis_locked(PhysicsServer3D::BODY_AXIS_ANGULAR_Y) || is_rigid_linear()) {
+		allowed_dofs &= ~JPH::EAllowedDOFs::RotationY;
+	}
+
+	if (is_axis_locked(PhysicsServer3D::BODY_AXIS_ANGULAR_Z) || is_rigid_linear()) {
+		allowed_dofs &= ~JPH::EAllowedDOFs::RotationZ;
+	}
+
+	ERR_FAIL_COND_V_MSG(allowed_dofs == JPH::EAllowedDOFs::None, JPH::EAllowedDOFs::All, vformat("Invalid axis locks for '%s'. Locking all axes is not supported when using Jolt Physics. All axes will be unlocked. Considering freezing the body instead.", to_string()));
+
+	return allowed_dofs;
+}
+
+JPH::MassProperties JoltBody3D::_calculate_mass_properties(const JPH::Shape &p_shape) const {
+	const bool calculate_mass = mass <= 0;
+	const bool calculate_inertia = inertia.x <= 0 || inertia.y <= 0 || inertia.z <= 0;
+
+	JPH::MassProperties mass_properties = p_shape.GetMassProperties();
+
+	if (calculate_mass && calculate_inertia) {
+		// Use the mass properties calculated by the shape
+	} else if (calculate_inertia) {
+		mass_properties.ScaleToMass(mass);
+	} else {
+		mass_properties.mMass = mass;
+	}
+
+	if (inertia.x > 0) {
+		mass_properties.mInertia(0, 0) = (float)inertia.x;
+		mass_properties.mInertia(0, 1) = 0;
+		mass_properties.mInertia(0, 2) = 0;
+		mass_properties.mInertia(1, 0) = 0;
+		mass_properties.mInertia(2, 0) = 0;
+	}
+
+	if (inertia.y > 0) {
+		mass_properties.mInertia(1, 1) = (float)inertia.y;
+		mass_properties.mInertia(1, 0) = 0;
+		mass_properties.mInertia(1, 2) = 0;
+		mass_properties.mInertia(0, 1) = 0;
+		mass_properties.mInertia(2, 1) = 0;
+	}
+
+	if (inertia.z > 0) {
+		mass_properties.mInertia(2, 2) = (float)inertia.z;
+		mass_properties.mInertia(2, 0) = 0;
+		mass_properties.mInertia(2, 1) = 0;
+		mass_properties.mInertia(0, 2) = 0;
+		mass_properties.mInertia(1, 2) = 0;
+	}
+
+	mass_properties.mInertia(3, 3) = 1.0f;
+
+	return mass_properties;
+}
+
+JPH::MassProperties JoltBody3D::_calculate_mass_properties() const {
+	return _calculate_mass_properties(*jolt_shape);
+}
+
+void JoltBody3D::_update_mass_properties() {
+	if (!in_space()) {
+		return;
+	}
+
+	const JoltWritableBody3D body = space->write_body(jolt_id);
+	ERR_FAIL_COND(body.is_invalid());
+
+	body->GetMotionPropertiesUnchecked()->SetMassProperties(_calculate_allowed_dofs(), _calculate_mass_properties());
+}
+
+void JoltBody3D::_update_gravity(JPH::Body &p_jolt_body) {
+	gravity = Vector3();
+
+	const Vector3 position = to_godot(p_jolt_body.GetPosition());
+
+	bool gravity_done = false;
+
+	for (const JoltArea3D *area : areas) {
+		gravity_done = integrate(gravity, area->get_gravity_mode(), [&]() {
+			return area->compute_gravity(position);
+		});
+
+		if (gravity_done) {
+			break;
+		}
+	}
+
+	if (!gravity_done) {
+		gravity += space->get_default_area()->compute_gravity(position);
+	}
+
+	gravity *= gravity_scale;
+}
+
+void JoltBody3D::_update_damp() {
+	if (!in_space()) {
+		return;
+	}
+
+	total_linear_damp = 0.0;
+	total_angular_damp = 0.0;
+
+	bool linear_damp_done = linear_damp_mode == PhysicsServer3D::BODY_DAMP_MODE_REPLACE;
+	bool angular_damp_done = angular_damp_mode == PhysicsServer3D::BODY_DAMP_MODE_REPLACE;
+
+	for (const JoltArea3D *area : areas) {
+		if (!linear_damp_done) {
+			linear_damp_done = integrate(total_linear_damp, area->get_linear_damp_mode(), [&]() {
+				return area->get_linear_damp();
+			});
+		}
+
+		if (!angular_damp_done) {
+			angular_damp_done = integrate(total_angular_damp, area->get_angular_damp_mode(), [&]() {
+				return area->get_angular_damp();
+			});
+		}
+
+		if (linear_damp_done && angular_damp_done) {
+			break;
+		}
+	}
+
+	const JoltArea3D *default_area = space->get_default_area();
+
+	if (!linear_damp_done) {
+		total_linear_damp += default_area->get_linear_damp();
+	}
+
+	if (!angular_damp_done) {
+		total_angular_damp += default_area->get_angular_damp();
+	}
+
+	switch (linear_damp_mode) {
+		case PhysicsServer3D::BODY_DAMP_MODE_COMBINE: {
+			total_linear_damp += linear_damp;
+		} break;
+		case PhysicsServer3D::BODY_DAMP_MODE_REPLACE: {
+			total_linear_damp = linear_damp;
+		} break;
+	}
+
+	switch (angular_damp_mode) {
+		case PhysicsServer3D::BODY_DAMP_MODE_COMBINE: {
+			total_angular_damp += angular_damp;
+		} break;
+		case PhysicsServer3D::BODY_DAMP_MODE_REPLACE: {
+			total_angular_damp = angular_damp;
+		} break;
+	}
+
+	_motion_changed();
+}
+
+void JoltBody3D::_update_kinematic_transform() {
+	if (is_kinematic()) {
+		kinematic_transform = get_transform_unscaled();
+	}
+}
+
+void JoltBody3D::_update_joint_constraints() {
+	for (JoltJoint3D *joint : joints) {
+		joint->rebuild();
+	}
+}
+
+void JoltBody3D::_update_possible_kinematic_contacts() {
+	const bool value = reports_all_kinematic_contacts();
+
+	if (!in_space()) {
+		jolt_settings->mCollideKinematicVsNonDynamic = value;
+	} else {
+		const JoltWritableBody3D body = space->write_body(jolt_id);
+		ERR_FAIL_COND(body.is_invalid());
+
+		body->SetCollideKinematicVsNonDynamic(value);
+	}
+}
+
+void JoltBody3D::_destroy_joint_constraints() {
+	for (JoltJoint3D *joint : joints) {
+		joint->destroy();
+	}
+}
+
+void JoltBody3D::_exit_all_areas() {
+	for (JoltArea3D *area : areas) {
+		area->body_exited(jolt_id, false);
+	}
+
+	areas.clear();
+}
+
+void JoltBody3D::_update_group_filter() {
+	JPH::GroupFilter *group_filter = !exceptions.is_empty() ? JoltGroupFilter::instance : nullptr;
+
+	if (!in_space()) {
+		jolt_settings->mCollisionGroup.SetGroupFilter(group_filter);
+		return;
+	}
+
+	const JoltWritableBody3D body = space->write_body(jolt_id);
+	ERR_FAIL_COND(body.is_invalid());
+
+	body->GetCollisionGroup().SetGroupFilter(group_filter);
+}
+
+void JoltBody3D::_mode_changed() {
+	_update_object_layer();
+	_update_kinematic_transform();
+	_update_mass_properties();
+	wake_up();
+}
+
+void JoltBody3D::_shapes_built() {
+	JoltShapedObject3D::_shapes_built();
+
+	_update_mass_properties();
+	_update_joint_constraints();
+	wake_up();
+}
+
+void JoltBody3D::_space_changing() {
+	JoltShapedObject3D::_space_changing();
+
+	sleep_initially = is_sleeping();
+
+	_destroy_joint_constraints();
+	_exit_all_areas();
+}
+
+void JoltBody3D::_space_changed() {
+	JoltShapedObject3D::_space_changed();
+
+	_update_kinematic_transform();
+	_update_group_filter();
+	_update_joint_constraints();
+	_areas_changed();
+
+	sync_state = false;
+}
+
+void JoltBody3D::_areas_changed() {
+	_update_damp();
+	wake_up();
+}
+
+void JoltBody3D::_joints_changed() {
+	wake_up();
+}
+
+void JoltBody3D::_transform_changed() {
+	wake_up();
+}
+
+void JoltBody3D::_motion_changed() {
+	wake_up();
+}
+
+void JoltBody3D::_exceptions_changed() {
+	_update_group_filter();
+}
+
+void JoltBody3D::_axis_lock_changed() {
+	_update_mass_properties();
+	wake_up();
+}
+
+void JoltBody3D::_contact_reporting_changed() {
+	_update_possible_kinematic_contacts();
+	wake_up();
+}
+
+JoltBody3D::JoltBody3D() :
+		JoltShapedObject3D(OBJECT_TYPE_BODY) {
+}
+
+JoltBody3D::~JoltBody3D() {
+	if (direct_state != nullptr) {
+		memdelete(direct_state);
+		direct_state = nullptr;
+	}
+}
+
+void JoltBody3D::set_transform(Transform3D p_transform) {
+	JOLT_ENSURE_SCALE_NOT_ZERO(p_transform, vformat("An invalid transform was passed to physics body '%s'.", to_string()));
+
+	const Vector3 new_scale = p_transform.basis.get_scale();
+
+	// Ideally we would do an exact comparison here, but due to floating-point precision this would be invalidated very often.
+	if (!scale.is_equal_approx(new_scale)) {
+		scale = new_scale;
+		_shapes_changed();
+	}
+
+	p_transform.basis.orthonormalize();
+
+	if (!in_space()) {
+		jolt_settings->mPosition = to_jolt_r(p_transform.origin);
+		jolt_settings->mRotation = to_jolt(p_transform.basis);
+	} else if (is_kinematic()) {
+		kinematic_transform = p_transform;
+	} else {
+		space->get_body_iface().SetPositionAndRotation(jolt_id, to_jolt_r(p_transform.origin), to_jolt(p_transform.basis), JPH::EActivation::DontActivate);
+	}
+
+	_transform_changed();
+}
+
+Variant JoltBody3D::get_state(PhysicsServer3D::BodyState p_state) const {
+	switch (p_state) {
+		case PhysicsServer3D::BODY_STATE_TRANSFORM: {
+			return get_transform_scaled();
+		}
+		case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: {
+			return get_linear_velocity();
+		}
+		case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: {
+			return get_angular_velocity();
+		}
+		case PhysicsServer3D::BODY_STATE_SLEEPING: {
+			return is_sleeping();
+		}
+		case PhysicsServer3D::BODY_STATE_CAN_SLEEP: {
+			return can_sleep();
+		}
+		default: {
+			ERR_FAIL_V_MSG(Variant(), vformat("Unhandled body state: '%d'. This should not happen. Please report this.", p_state));
+		}
+	}
+}
+
+void JoltBody3D::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_value) {
+	switch (p_state) {
+		case PhysicsServer3D::BODY_STATE_TRANSFORM: {
+			set_transform(p_value);
+		} break;
+		case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: {
+			set_linear_velocity(p_value);
+		} break;
+		case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: {
+			set_angular_velocity(p_value);
+		} break;
+		case PhysicsServer3D::BODY_STATE_SLEEPING: {
+			set_is_sleeping(p_value);
+		} break;
+		case PhysicsServer3D::BODY_STATE_CAN_SLEEP: {
+			set_can_sleep(p_value);
+		} break;
+		default: {
+			ERR_FAIL_MSG(vformat("Unhandled body state: '%d'. This should not happen. Please report this.", p_state));
+		} break;
+	}
+}
+
+Variant JoltBody3D::get_param(PhysicsServer3D::BodyParameter p_param) const {
+	switch (p_param) {
+		case PhysicsServer3D::BODY_PARAM_BOUNCE: {
+			return get_bounce();
+		}
+		case PhysicsServer3D::BODY_PARAM_FRICTION: {
+			return get_friction();
+		}
+		case PhysicsServer3D::BODY_PARAM_MASS: {
+			return get_mass();
+		}
+		case PhysicsServer3D::BODY_PARAM_INERTIA: {
+			return get_inertia();
+		}
+		case PhysicsServer3D::BODY_PARAM_CENTER_OF_MASS: {
+			return get_center_of_mass_custom();
+		}
+		case PhysicsServer3D::BODY_PARAM_GRAVITY_SCALE: {
+			return get_gravity_scale();
+		}
+		case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP_MODE: {
+			return get_linear_damp_mode();
+		}
+		case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP_MODE: {
+			return get_angular_damp_mode();
+		}
+		case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP: {
+			return get_linear_damp();
+		}
+		case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP: {
+			return get_angular_damp();
+		}
+		default: {
+			ERR_FAIL_V_MSG(Variant(), vformat("Unhandled body parameter: '%d'. This should not happen. Please report this.", p_param));
+		}
+	}
+}
+
+void JoltBody3D::set_param(PhysicsServer3D::BodyParameter p_param, const Variant &p_value) {
+	switch (p_param) {
+		case PhysicsServer3D::BODY_PARAM_BOUNCE: {
+			set_bounce(p_value);
+		} break;
+		case PhysicsServer3D::BODY_PARAM_FRICTION: {
+			set_friction(p_value);
+		} break;
+		case PhysicsServer3D::BODY_PARAM_MASS: {
+			set_mass(p_value);
+		} break;
+		case PhysicsServer3D::BODY_PARAM_INERTIA: {
+			set_inertia(p_value);
+		} break;
+		case PhysicsServer3D::BODY_PARAM_CENTER_OF_MASS: {
+			set_center_of_mass_custom(p_value);
+		} break;
+		case PhysicsServer3D::BODY_PARAM_GRAVITY_SCALE: {
+			set_gravity_scale(p_value);
+		} break;
+		case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP_MODE: {
+			set_linear_damp_mode((DampMode)(int)p_value);
+		} break;
+		case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP_MODE: {
+			set_angular_damp_mode((DampMode)(int)p_value);
+		} break;
+		case PhysicsServer3D::BODY_PARAM_LINEAR_DAMP: {
+			set_linear_damp(p_value);
+		} break;
+		case PhysicsServer3D::BODY_PARAM_ANGULAR_DAMP: {
+			set_angular_damp(p_value);
+		} break;
+		default: {
+			ERR_FAIL_MSG(vformat("Unhandled body parameter: '%d'. This should not happen. Please report this.", p_param));
+		} break;
+	}
+}
+
+void JoltBody3D::set_custom_integrator(bool p_enabled) {
+	if (custom_integrator == p_enabled) {
+		return;
+	}
+
+	custom_integrator = p_enabled;
+
+	if (!in_space()) {
+		_motion_changed();
+		return;
+	}
+
+	const JoltWritableBody3D body = space->write_body(jolt_id);
+	ERR_FAIL_COND(body.is_invalid());
+
+	body->ResetForce();
+	body->ResetTorque();
+
+	_motion_changed();
+}
+
+bool JoltBody3D::is_sleeping() const {
+	if (!in_space()) {
+		return sleep_initially;
+	}
+
+	const JoltReadableBody3D body = space->read_body(jolt_id);
+	ERR_FAIL_COND_V(body.is_invalid(), false);
+
+	return !body->IsActive();
+}
+
+void JoltBody3D::set_is_sleeping(bool p_enabled) {
+	if (!in_space()) {
+		sleep_initially = p_enabled;
+		return;
+	}
+
+	JPH::BodyInterface &body_iface = space->get_body_iface();
+
+	if (p_enabled) {
+		body_iface.DeactivateBody(jolt_id);
+	} else {
+		body_iface.ActivateBody(jolt_id);
+	}
+}
+
+bool JoltBody3D::can_sleep() const {
+	if (!in_space()) {
+		return jolt_settings->mAllowSleeping;
+	}
+
+	const JoltReadableBody3D body = space->read_body(jolt_id);
+	ERR_FAIL_COND_V(body.is_invalid(), false);
+
+	return body->GetAllowSleeping();
+}
+
+void JoltBody3D::set_can_sleep(bool p_enabled) {
+	if (!in_space()) {
+		jolt_settings->mAllowSleeping = p_enabled;
+		return;
+	}
+
+	const JoltWritableBody3D body = space->write_body(jolt_id);
+	ERR_FAIL_COND(body.is_invalid());
+
+	body->SetAllowSleeping(p_enabled);
+}
+
+Basis JoltBody3D::get_principal_inertia_axes() const {
+	ERR_FAIL_NULL_V_MSG(space, Basis(), vformat("Failed to retrieve principal inertia axes of '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
+
+	if (unlikely(is_static() || is_kinematic())) {
+		return Basis();
+	}
+
+	const JoltReadableBody3D body = space->read_body(jolt_id);
+	ERR_FAIL_COND_V(body.is_invalid(), Basis());
+
+	return to_godot(body->GetRotation() * body->GetMotionProperties()->GetInertiaRotation());
+}
+
+Vector3 JoltBody3D::get_inverse_inertia() const {
+	ERR_FAIL_NULL_V_MSG(space, Vector3(), vformat("Failed to retrieve inverse inertia of '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
+
+	if (unlikely(is_static() || is_kinematic())) {
+		return Vector3();
+	}
+
+	const JoltReadableBody3D body = space->read_body(jolt_id);
+	ERR_FAIL_COND_V(body.is_invalid(), Vector3());
+
+	const JPH::MotionProperties &motion_properties = *body->GetMotionPropertiesUnchecked();
+
+	return to_godot(motion_properties.GetLocalSpaceInverseInertia().GetDiagonal3());
+}
+
+Basis JoltBody3D::get_inverse_inertia_tensor() const {
+	ERR_FAIL_NULL_V_MSG(space, Basis(), vformat("Failed to retrieve inverse inertia tensor of '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
+
+	if (unlikely(is_static() || is_kinematic())) {
+		return Basis();
+	}
+
+	const JoltReadableBody3D body = space->read_body(jolt_id);
+	ERR_FAIL_COND_V(body.is_invalid(), Basis());
+
+	return to_godot(body->GetInverseInertia()).basis;
+}
+
+void JoltBody3D::set_linear_velocity(const Vector3 &p_velocity) {
+	if (is_static() || is_kinematic()) {
+		linear_surface_velocity = p_velocity;
+		_motion_changed();
+		return;
+	}
+
+	if (!in_space()) {
+		jolt_settings->mLinearVelocity = to_jolt(p_velocity);
+		_motion_changed();
+		return;
+	}
+
+	const JoltWritableBody3D body = space->write_body(jolt_id);
+	ERR_FAIL_COND(body.is_invalid());
+
+	body->GetMotionPropertiesUnchecked()->SetLinearVelocityClamped(to_jolt(p_velocity));
+
+	_motion_changed();
+}
+
+void JoltBody3D::set_angular_velocity(const Vector3 &p_velocity) {
+	if (is_static() || is_kinematic()) {
+		angular_surface_velocity = p_velocity;
+		_motion_changed();
+		return;
+	}
+
+	if (!in_space()) {
+		jolt_settings->mAngularVelocity = to_jolt(p_velocity);
+		_motion_changed();
+		return;
+	}
+
+	const JoltWritableBody3D body = space->write_body(jolt_id);
+	ERR_FAIL_COND(body.is_invalid());
+
+	body->GetMotionPropertiesUnchecked()->SetAngularVelocityClamped(to_jolt(p_velocity));
+
+	_motion_changed();
+}
+
+void JoltBody3D::set_axis_velocity(const Vector3 &p_axis_velocity) {
+	const Vector3 axis = p_axis_velocity.normalized();
+
+	if (!in_space()) {
+		Vector3 linear_velocity = to_godot(jolt_settings->mLinearVelocity);
+		linear_velocity -= axis * axis.dot(linear_velocity);
+		linear_velocity += p_axis_velocity;
+		jolt_settings->mLinearVelocity = to_jolt(linear_velocity);
+	} else {
+		const JoltWritableBody3D body = space->write_body(jolt_id);
+		ERR_FAIL_COND(body.is_invalid());
+
+		Vector3 linear_velocity = get_linear_velocity();
+		linear_velocity -= axis * axis.dot(linear_velocity);
+		linear_velocity += p_axis_velocity;
+		set_linear_velocity(linear_velocity);
+	}
+
+	_motion_changed();
+}
+
+Vector3 JoltBody3D::get_velocity_at_position(const Vector3 &p_position) const {
+	if (unlikely(!in_space())) {
+		return Vector3();
+	}
+
+	const JoltReadableBody3D body = space->read_body(jolt_id);
+	ERR_FAIL_COND_V(body.is_invalid(), Vector3());
+
+	const JPH::MotionProperties &motion_properties = *body->GetMotionPropertiesUnchecked();
+
+	const Vector3 total_linear_velocity = to_godot(motion_properties.GetLinearVelocity()) + linear_surface_velocity;
+	const Vector3 total_angular_velocity = to_godot(motion_properties.GetAngularVelocity()) + angular_surface_velocity;
+	const Vector3 com_to_pos = p_position - to_godot(body->GetCenterOfMassPosition());
+
+	return total_linear_velocity + total_angular_velocity.cross(com_to_pos);
+}
+
+void JoltBody3D::set_center_of_mass_custom(const Vector3 &p_center_of_mass) {
+	if (custom_center_of_mass && p_center_of_mass == center_of_mass_custom) {
+		return;
+	}
+
+	custom_center_of_mass = true;
+	center_of_mass_custom = p_center_of_mass;
+
+	_shapes_changed();
+}
+
+void JoltBody3D::set_max_contacts_reported(int p_count) {
+	ERR_FAIL_COND(p_count < 0);
+
+	if (unlikely((int)contacts.size() == p_count)) {
+		return;
+	}
+
+	contacts.resize(p_count);
+	contact_count = MIN(contact_count, p_count);
+
+	const bool use_manifold_reduction = !reports_contacts();
+
+	if (!in_space()) {
+		jolt_settings->mUseManifoldReduction = use_manifold_reduction;
+		_contact_reporting_changed();
+		return;
+	}
+
+	JPH::BodyInterface &body_iface = space->get_body_iface();
+
+	body_iface.SetUseManifoldReduction(jolt_id, use_manifold_reduction);
+
+	_contact_reporting_changed();
+}
+
+bool JoltBody3D::reports_all_kinematic_contacts() const {
+	return reports_contacts() && JoltProjectSettings::should_generate_all_kinematic_contacts();
+}
+
+void JoltBody3D::add_contact(const JoltBody3D *p_collider, float p_depth, int p_shape_index, int p_collider_shape_index, const Vector3 &p_normal, const Vector3 &p_position, const Vector3 &p_collider_position, const Vector3 &p_velocity, const Vector3 &p_collider_velocity, const Vector3 &p_impulse) {
+	const int max_contacts = get_max_contacts_reported();
+
+	if (max_contacts == 0) {
+		return;
+	}
+
+	Contact *contact = nullptr;
+
+	if (contact_count < max_contacts) {
+		contact = &contacts[contact_count++];
+	} else {
+		Contact *shallowest_contact = &contacts[0];
+
+		for (int i = 1; i < (int)contacts.size(); i++) {
+			Contact &other_contact = contacts[i];
+			if (other_contact.depth < shallowest_contact->depth) {
+				shallowest_contact = &other_contact;
+			}
+		}
+
+		if (shallowest_contact->depth < p_depth) {
+			contact = shallowest_contact;
+		}
+	}
+
+	if (contact != nullptr) {
+		contact->normal = p_normal;
+		contact->position = p_position;
+		contact->collider_position = p_collider_position;
+		contact->velocity = p_velocity;
+		contact->collider_velocity = p_collider_velocity;
+		contact->impulse = p_impulse;
+		contact->collider_id = p_collider->get_instance_id();
+		contact->collider_rid = p_collider->get_rid();
+		contact->shape_index = p_shape_index;
+		contact->collider_shape_index = p_collider_shape_index;
+	}
+}
+
+void JoltBody3D::reset_mass_properties() {
+	if (custom_center_of_mass) {
+		custom_center_of_mass = false;
+		center_of_mass_custom.zero();
+
+		_shapes_changed();
+	}
+
+	inertia.zero();
+
+	_update_mass_properties();
+}
+
+void JoltBody3D::apply_force(const Vector3 &p_force, const Vector3 &p_position) {
+	ERR_FAIL_NULL_MSG(space, vformat("Failed to apply force to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
+
+	if (unlikely(!is_rigid())) {
+		return;
+	}
+
+	if (custom_integrator || p_force == Vector3()) {
+		return;
+	}
+
+	const JoltWritableBody3D body = space->write_body(jolt_id);
+	ERR_FAIL_COND(body.is_invalid());
+
+	body->AddForce(to_jolt(p_force), body->GetPosition() + to_jolt(p_position));
+
+	_motion_changed();
+}
+
+void JoltBody3D::apply_central_force(const Vector3 &p_force) {
+	ERR_FAIL_NULL_MSG(space, vformat("Failed to apply central force to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
+
+	if (unlikely(!is_rigid())) {
+		return;
+	}
+
+	if (custom_integrator || p_force == Vector3()) {
+		return;
+	}
+
+	const JoltWritableBody3D body = space->write_body(jolt_id);
+	ERR_FAIL_COND(body.is_invalid());
+
+	body->AddForce(to_jolt(p_force));
+
+	_motion_changed();
+}
+
+void JoltBody3D::apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position) {
+	ERR_FAIL_NULL_MSG(space, vformat("Failed to apply impulse to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
+
+	if (unlikely(!is_rigid())) {
+		return;
+	}
+
+	if (p_impulse == Vector3()) {
+		return;
+	}
+
+	const JoltWritableBody3D body = space->write_body(jolt_id);
+	ERR_FAIL_COND(body.is_invalid());
+
+	body->AddImpulse(to_jolt(p_impulse), body->GetPosition() + to_jolt(p_position));
+
+	_motion_changed();
+}
+
+void JoltBody3D::apply_central_impulse(const Vector3 &p_impulse) {
+	ERR_FAIL_NULL_MSG(space, vformat("Failed to apply central impulse to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
+
+	if (unlikely(!is_rigid())) {
+		return;
+	}
+
+	if (p_impulse == Vector3()) {
+		return;
+	}
+
+	const JoltWritableBody3D body = space->write_body(jolt_id);
+	ERR_FAIL_COND(body.is_invalid());
+
+	body->AddImpulse(to_jolt(p_impulse));
+
+	_motion_changed();
+}
+
+void JoltBody3D::apply_torque(const Vector3 &p_torque) {
+	ERR_FAIL_NULL_MSG(space, vformat("Failed to apply torque to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
+
+	if (unlikely(!is_rigid())) {
+		return;
+	}
+
+	if (custom_integrator || p_torque == Vector3()) {
+		return;
+	}
+
+	const JoltWritableBody3D body = space->write_body(jolt_id);
+	ERR_FAIL_COND(body.is_invalid());
+
+	body->AddTorque(to_jolt(p_torque));
+
+	_motion_changed();
+}
+
+void JoltBody3D::apply_torque_impulse(const Vector3 &p_impulse) {
+	ERR_FAIL_NULL_MSG(space, vformat("Failed to apply torque impulse to '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
+
+	if (unlikely(!is_rigid())) {
+		return;
+	}
+
+	if (p_impulse == Vector3()) {
+		return;
+	}
+
+	const JoltWritableBody3D body = space->write_body(jolt_id);
+	ERR_FAIL_COND(body.is_invalid());
+
+	body->AddAngularImpulse(to_jolt(p_impulse));
+
+	_motion_changed();
+}
+
+void JoltBody3D::add_constant_central_force(const Vector3 &p_force) {
+	if (p_force == Vector3()) {
+		return;
+	}
+
+	constant_force += p_force;
+
+	_motion_changed();
+}
+
+void JoltBody3D::add_constant_force(const Vector3 &p_force, const Vector3 &p_position) {
+	if (p_force == Vector3()) {
+		return;
+	}
+
+	const JoltWritableBody3D body = space->write_body(jolt_id);
+	ERR_FAIL_COND(body.is_invalid());
+
+	constant_force += p_force;
+	constant_torque += (p_position - get_center_of_mass_relative()).cross(p_force);
+
+	_motion_changed();
+}
+
+void JoltBody3D::add_constant_torque(const Vector3 &p_torque) {
+	if (p_torque == Vector3()) {
+		return;
+	}
+
+	constant_torque += p_torque;
+
+	_motion_changed();
+}
+
+Vector3 JoltBody3D::get_constant_force() const {
+	return constant_force;
+}
+
+void JoltBody3D::set_constant_force(const Vector3 &p_force) {
+	if (constant_force == p_force) {
+		return;
+	}
+
+	constant_force = p_force;
+
+	_motion_changed();
+}
+
+Vector3 JoltBody3D::get_constant_torque() const {
+	return constant_torque;
+}
+
+void JoltBody3D::set_constant_torque(const Vector3 &p_torque) {
+	if (constant_torque == p_torque) {
+		return;
+	}
+
+	constant_torque = p_torque;
+
+	_motion_changed();
+}
+
+void JoltBody3D::add_collision_exception(const RID &p_excepted_body) {
+	exceptions.push_back(p_excepted_body);
+
+	_exceptions_changed();
+}
+
+void JoltBody3D::remove_collision_exception(const RID &p_excepted_body) {
+	exceptions.erase(p_excepted_body);
+
+	_exceptions_changed();
+}
+
+bool JoltBody3D::has_collision_exception(const RID &p_excepted_body) const {
+	return exceptions.find(p_excepted_body) >= 0;
+}
+
+void JoltBody3D::add_area(JoltArea3D *p_area) {
+	int i = 0;
+	for (; i < (int)areas.size(); i++) {
+		if (p_area->get_priority() > areas[i]->get_priority()) {
+			break;
+		}
+	}
+
+	areas.insert(i, p_area);
+
+	_areas_changed();
+}
+
+void JoltBody3D::remove_area(JoltArea3D *p_area) {
+	areas.erase(p_area);
+
+	_areas_changed();
+}
+
+void JoltBody3D::add_joint(JoltJoint3D *p_joint) {
+	joints.push_back(p_joint);
+
+	_joints_changed();
+}
+
+void JoltBody3D::remove_joint(JoltJoint3D *p_joint) {
+	joints.erase(p_joint);
+
+	_joints_changed();
+}
+
+void JoltBody3D::call_queries(JPH::Body &p_jolt_body) {
+	if (!sync_state) {
+		return;
+	}
+
+	if (custom_integration_callback.is_valid()) {
+		const Variant direct_state_variant = get_direct_state();
+		const Variant *args[2] = { &direct_state_variant, &custom_integration_userdata };
+		const int argc = custom_integration_userdata.get_type() != Variant::NIL ? 2 : 1;
+
+		Callable::CallError ce;
+		Variant ret;
+		custom_integration_callback.callp(args, argc, ret, ce);
+
+		if (unlikely(ce.error != Callable::CallError::CALL_OK)) {
+			ERR_PRINT_ONCE(vformat("Failed to call force integration callback for '%s'. It returned the following error: '%s'.", to_string(), Variant::get_callable_error_text(custom_integration_callback, args, argc, ce)));
+		}
+	}
+
+	if (state_sync_callback.is_valid()) {
+		const Variant direct_state_variant = get_direct_state();
+		const Variant *args[1] = { &direct_state_variant };
+
+		Callable::CallError ce;
+		Variant ret;
+		state_sync_callback.callp(args, 1, ret, ce);
+
+		if (unlikely(ce.error != Callable::CallError::CALL_OK)) {
+			ERR_PRINT_ONCE(vformat("Failed to call state synchronization callback for '%s'. It returned the following error: '%s'.", to_string(), Variant::get_callable_error_text(state_sync_callback, args, 1, ce)));
+		}
+	}
+
+	sync_state = false;
+}
+
+void JoltBody3D::pre_step(float p_step, JPH::Body &p_jolt_body) {
+	JoltObject3D::pre_step(p_step, p_jolt_body);
+
+	switch (mode) {
+		case PhysicsServer3D::BODY_MODE_STATIC: {
+			_pre_step_static(p_step, p_jolt_body);
+		} break;
+		case PhysicsServer3D::BODY_MODE_RIGID:
+		case PhysicsServer3D::BODY_MODE_RIGID_LINEAR: {
+			_pre_step_rigid(p_step, p_jolt_body);
+		} break;
+		case PhysicsServer3D::BODY_MODE_KINEMATIC: {
+			_pre_step_kinematic(p_step, p_jolt_body);
+		} break;
+	}
+
+	contact_count = 0;
+}
+
+JoltPhysicsDirectBodyState3D *JoltBody3D::get_direct_state() {
+	if (direct_state == nullptr) {
+		direct_state = memnew(JoltPhysicsDirectBodyState3D(this));
+	}
+
+	return direct_state;
+}
+
+void JoltBody3D::set_mode(PhysicsServer3D::BodyMode p_mode) {
+	if (p_mode == mode) {
+		return;
+	}
+
+	mode = p_mode;
+
+	if (!in_space()) {
+		_mode_changed();
+		return;
+	}
+
+	const JPH::EMotionType motion_type = _get_motion_type();
+
+	const JoltWritableBody3D body = space->write_body(jolt_id);
+	ERR_FAIL_COND(body.is_invalid());
+
+	if (motion_type == JPH::EMotionType::Static) {
+		put_to_sleep();
+	}
+
+	body->SetMotionType(motion_type);
+
+	if (motion_type != JPH::EMotionType::Static) {
+		wake_up();
+	}
+
+	if (motion_type == JPH::EMotionType::Kinematic) {
+		body->SetLinearVelocity(JPH::Vec3::sZero());
+		body->SetAngularVelocity(JPH::Vec3::sZero());
+	}
+
+	linear_surface_velocity = Vector3();
+	angular_surface_velocity = Vector3();
+
+	_mode_changed();
+}
+
+bool JoltBody3D::is_ccd_enabled() const {
+	if (!in_space()) {
+		return jolt_settings->mMotionQuality == JPH::EMotionQuality::LinearCast;
+	}
+
+	const JPH::BodyInterface &body_iface = space->get_body_iface();
+
+	return body_iface.GetMotionQuality(jolt_id) == JPH::EMotionQuality::LinearCast;
+}
+
+void JoltBody3D::set_ccd_enabled(bool p_enabled) {
+	const JPH::EMotionQuality motion_quality = p_enabled ? JPH::EMotionQuality::LinearCast : JPH::EMotionQuality::Discrete;
+
+	if (!in_space()) {
+		jolt_settings->mMotionQuality = motion_quality;
+		return;
+	}
+
+	JPH::BodyInterface &body_iface = space->get_body_iface();
+
+	body_iface.SetMotionQuality(jolt_id, motion_quality);
+}
+
+void JoltBody3D::set_mass(float p_mass) {
+	if (p_mass != mass) {
+		mass = p_mass;
+		_update_mass_properties();
+	}
+}
+
+void JoltBody3D::set_inertia(const Vector3 &p_inertia) {
+	if (p_inertia != inertia) {
+		inertia = p_inertia;
+		_update_mass_properties();
+	}
+}
+
+float JoltBody3D::get_bounce() const {
+	if (!in_space()) {
+		return jolt_settings->mRestitution;
+	}
+
+	const JoltReadableBody3D body = space->read_body(jolt_id);
+	ERR_FAIL_COND_V(body.is_invalid(), 0.0f);
+
+	return body->GetRestitution();
+}
+
+void JoltBody3D::set_bounce(float p_bounce) {
+	if (!in_space()) {
+		jolt_settings->mRestitution = p_bounce;
+		return;
+	}
+
+	const JoltWritableBody3D body = space->write_body(jolt_id);
+	ERR_FAIL_COND(body.is_invalid());
+
+	body->SetRestitution(p_bounce);
+}
+
+float JoltBody3D::get_friction() const {
+	if (!in_space()) {
+		return jolt_settings->mFriction;
+	}
+
+	const JoltReadableBody3D body = space->read_body(jolt_id);
+	ERR_FAIL_COND_V(body.is_invalid(), 0.0f);
+
+	return body->GetFriction();
+}
+
+void JoltBody3D::set_friction(float p_friction) {
+	if (!in_space()) {
+		jolt_settings->mFriction = p_friction;
+		return;
+	}
+
+	const JoltWritableBody3D body = space->write_body(jolt_id);
+	ERR_FAIL_COND(body.is_invalid());
+
+	body->SetFriction(p_friction);
+}
+
+void JoltBody3D::set_gravity_scale(float p_scale) {
+	if (gravity_scale == p_scale) {
+		return;
+	}
+
+	gravity_scale = p_scale;
+
+	_motion_changed();
+}
+
+void JoltBody3D::set_linear_damp(float p_damp) {
+	p_damp = MAX(0.0f, p_damp);
+
+	if (p_damp == linear_damp) {
+		return;
+	}
+
+	linear_damp = p_damp;
+
+	_update_damp();
+}
+
+void JoltBody3D::set_angular_damp(float p_damp) {
+	p_damp = MAX(0.0f, p_damp);
+
+	if (p_damp == angular_damp) {
+		return;
+	}
+
+	angular_damp = p_damp;
+
+	_update_damp();
+}
+
+bool JoltBody3D::is_axis_locked(PhysicsServer3D::BodyAxis p_axis) const {
+	return (locked_axes & (uint32_t)p_axis) != 0;
+}
+
+void JoltBody3D::set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool p_enabled) {
+	const uint32_t previous_locked_axes = locked_axes;
+
+	if (p_enabled) {
+		locked_axes |= (uint32_t)p_axis;
+	} else {
+		locked_axes &= ~(uint32_t)p_axis;
+	}
+
+	if (previous_locked_axes != locked_axes) {
+		_axis_lock_changed();
+	}
+}
+
+bool JoltBody3D::can_interact_with(const JoltBody3D &p_other) const {
+	return (can_collide_with(p_other) || p_other.can_collide_with(*this)) && !has_collision_exception(p_other.get_rid()) && !p_other.has_collision_exception(rid);
+}
+
+bool JoltBody3D::can_interact_with(const JoltSoftBody3D &p_other) const {
+	return p_other.can_interact_with(*this);
+}
+
+bool JoltBody3D::can_interact_with(const JoltArea3D &p_other) const {
+	return p_other.can_interact_with(*this);
+}
diff --git a/modules/jolt_physics/objects/jolt_body_3d.h b/modules/jolt_physics/objects/jolt_body_3d.h
new file mode 100644
index 0000000000..50a5c2a293
--- /dev/null
+++ b/modules/jolt_physics/objects/jolt_body_3d.h
@@ -0,0 +1,304 @@
+/**************************************************************************/
+/*  jolt_body_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_3D_H
+#define JOLT_BODY_3D_H
+
+#include "jolt_physics_direct_body_state_3d.h"
+#include "jolt_shaped_object_3d.h"
+
+class JoltArea3D;
+class JoltJoint3D;
+class JoltSoftBody3D;
+
+class JoltBody3D final : public JoltShapedObject3D {
+public:
+	typedef PhysicsServer3D::BodyDampMode DampMode;
+
+	struct Contact {
+		Vector3 normal;
+		Vector3 position;
+		Vector3 collider_position;
+		Vector3 velocity;
+		Vector3 collider_velocity;
+		Vector3 impulse;
+		ObjectID collider_id;
+		RID collider_rid;
+		float depth = 0.0f;
+		int shape_index = 0;
+		int collider_shape_index = 0;
+	};
+
+private:
+	LocalVector<RID> exceptions;
+	LocalVector<Contact> contacts;
+	LocalVector<JoltArea3D *> areas;
+	LocalVector<JoltJoint3D *> joints;
+
+	Variant custom_integration_userdata;
+
+	Transform3D kinematic_transform;
+
+	Vector3 inertia;
+	Vector3 center_of_mass_custom;
+	Vector3 constant_force;
+	Vector3 constant_torque;
+	Vector3 linear_surface_velocity;
+	Vector3 angular_surface_velocity;
+	Vector3 gravity;
+
+	Callable state_sync_callback;
+	Callable custom_integration_callback;
+
+	JoltPhysicsDirectBodyState3D *direct_state = nullptr;
+
+	PhysicsServer3D::BodyMode mode = PhysicsServer3D::BODY_MODE_RIGID;
+
+	DampMode linear_damp_mode = PhysicsServer3D::BODY_DAMP_MODE_COMBINE;
+	DampMode angular_damp_mode = PhysicsServer3D::BODY_DAMP_MODE_COMBINE;
+
+	float mass = 1.0f;
+	float linear_damp = 0.0f;
+	float angular_damp = 0.0f;
+	float total_linear_damp = 0.0f;
+	float total_angular_damp = 0.0f;
+	float gravity_scale = 1.0f;
+	float collision_priority = 1.0f;
+
+	int contact_count = 0;
+
+	uint32_t locked_axes = 0;
+
+	bool sync_state = false;
+	bool sleep_initially = false;
+	bool custom_center_of_mass = false;
+	bool custom_integrator = false;
+
+	virtual JPH::BroadPhaseLayer _get_broad_phase_layer() const override;
+	virtual JPH::ObjectLayer _get_object_layer() const override;
+
+	virtual JPH::EMotionType _get_motion_type() const override;
+
+	virtual void _add_to_space() override;
+
+	void _integrate_forces(float p_step, JPH::Body &p_jolt_body);
+
+	void _move_kinematic(float p_step, JPH::Body &p_jolt_body);
+
+	void _pre_step_static(float p_step, JPH::Body &p_jolt_body);
+	void _pre_step_rigid(float p_step, JPH::Body &p_jolt_body);
+	void _pre_step_kinematic(float p_step, JPH::Body &p_jolt_body);
+
+	JPH::EAllowedDOFs _calculate_allowed_dofs() const;
+
+	JPH::MassProperties _calculate_mass_properties(const JPH::Shape &p_shape) const;
+	JPH::MassProperties _calculate_mass_properties() const;
+
+	void _update_mass_properties();
+	void _update_gravity(JPH::Body &p_jolt_body);
+	void _update_damp();
+	void _update_kinematic_transform();
+	void _update_group_filter();
+	void _update_joint_constraints();
+	void _update_possible_kinematic_contacts();
+
+	void _destroy_joint_constraints();
+
+	void _exit_all_areas();
+
+	void _mode_changed();
+	virtual void _shapes_built() override;
+	virtual void _space_changing() override;
+	virtual void _space_changed() override;
+	void _areas_changed();
+	void _joints_changed();
+	void _transform_changed();
+	void _motion_changed();
+	void _exceptions_changed();
+	void _axis_lock_changed();
+	void _contact_reporting_changed();
+
+public:
+	JoltBody3D();
+	virtual ~JoltBody3D() override;
+
+	void set_transform(Transform3D p_transform);
+
+	Variant get_state(PhysicsServer3D::BodyState p_state) const;
+	void set_state(PhysicsServer3D::BodyState p_state, const Variant &p_value);
+
+	Variant get_param(PhysicsServer3D::BodyParameter p_param) const;
+	void set_param(PhysicsServer3D::BodyParameter p_param, const Variant &p_value);
+
+	bool has_state_sync_callback() const { return state_sync_callback.is_valid(); }
+	void set_state_sync_callback(const Callable &p_callback) { state_sync_callback = p_callback; }
+
+	bool has_custom_integration_callback() const { return custom_integration_callback.is_valid(); }
+	void set_custom_integration_callback(const Callable &p_callback, const Variant &p_userdata) {
+		custom_integration_callback = p_callback;
+		custom_integration_userdata = p_userdata;
+	}
+
+	bool has_custom_integrator() const { return custom_integrator; }
+	void set_custom_integrator(bool p_enabled);
+
+	bool is_sleeping() const;
+	void set_is_sleeping(bool p_enabled);
+
+	void put_to_sleep() { set_is_sleeping(true); }
+	void wake_up() { set_is_sleeping(false); }
+
+	bool can_sleep() const;
+	void set_can_sleep(bool p_enabled);
+
+	Basis get_principal_inertia_axes() const;
+	Vector3 get_inverse_inertia() const;
+	Basis get_inverse_inertia_tensor() const;
+
+	void set_linear_velocity(const Vector3 &p_velocity);
+	void set_angular_velocity(const Vector3 &p_velocity);
+	void set_axis_velocity(const Vector3 &p_axis_velocity);
+
+	virtual Vector3 get_velocity_at_position(const Vector3 &p_position) const override;
+
+	virtual bool has_custom_center_of_mass() const override { return custom_center_of_mass; }
+	virtual Vector3 get_center_of_mass_custom() const override { return center_of_mass_custom; }
+	void set_center_of_mass_custom(const Vector3 &p_center_of_mass);
+
+	int get_max_contacts_reported() const { return contacts.size(); }
+	void set_max_contacts_reported(int p_count);
+
+	int get_contact_count() const { return contact_count; }
+	const Contact &get_contact(int p_index) { return contacts[p_index]; }
+	virtual bool reports_contacts() const override { return !contacts.is_empty(); }
+
+	bool reports_all_kinematic_contacts() const;
+
+	void add_contact(const JoltBody3D *p_collider, float p_depth, int p_shape_index, int p_collider_shape_index, const Vector3 &p_normal, const Vector3 &p_position, const Vector3 &p_collider_position, const Vector3 &p_velocity, const Vector3 &p_collider_velocity, const Vector3 &p_impulse);
+
+	void reset_mass_properties();
+
+	void apply_force(const Vector3 &p_force, const Vector3 &p_position);
+	void apply_central_force(const Vector3 &p_force);
+	void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position);
+
+	void apply_central_impulse(const Vector3 &p_impulse);
+	void apply_torque(const Vector3 &p_torque);
+	void apply_torque_impulse(const Vector3 &p_impulse);
+
+	void add_constant_central_force(const Vector3 &p_force);
+	void add_constant_force(const Vector3 &p_force, const Vector3 &p_position);
+	void add_constant_torque(const Vector3 &p_torque);
+
+	Vector3 get_constant_force() const;
+	void set_constant_force(const Vector3 &p_force);
+
+	Vector3 get_constant_torque() const;
+	void set_constant_torque(const Vector3 &p_torque);
+
+	Vector3 get_linear_surface_velocity() const { return linear_surface_velocity; }
+	Vector3 get_angular_surface_velocity() const { return angular_surface_velocity; }
+
+	void add_collision_exception(const RID &p_excepted_body);
+	void remove_collision_exception(const RID &p_excepted_body);
+	bool has_collision_exception(const RID &p_excepted_body) const;
+
+	const LocalVector<RID> &get_collision_exceptions() const { return exceptions; }
+
+	void add_area(JoltArea3D *p_area);
+	void remove_area(JoltArea3D *p_area);
+
+	void add_joint(JoltJoint3D *p_joint);
+	void remove_joint(JoltJoint3D *p_joint);
+
+	void call_queries(JPH::Body &p_jolt_body);
+
+	virtual void pre_step(float p_step, JPH::Body &p_jolt_body) override;
+
+	JoltPhysicsDirectBodyState3D *get_direct_state();
+
+	PhysicsServer3D::BodyMode get_mode() const { return mode; }
+
+	void set_mode(PhysicsServer3D::BodyMode p_mode);
+
+	bool is_static() const { return mode == PhysicsServer3D::BODY_MODE_STATIC; }
+	bool is_kinematic() const { return mode == PhysicsServer3D::BODY_MODE_KINEMATIC; }
+	bool is_rigid_free() const { return mode == PhysicsServer3D::BODY_MODE_RIGID; }
+	bool is_rigid_linear() const { return mode == PhysicsServer3D::BODY_MODE_RIGID_LINEAR; }
+	bool is_rigid() const { return is_rigid_free() || is_rigid_linear(); }
+
+	bool is_ccd_enabled() const;
+	void set_ccd_enabled(bool p_enabled);
+
+	float get_mass() const { return mass; }
+	void set_mass(float p_mass);
+
+	Vector3 get_inertia() const { return inertia; }
+	void set_inertia(const Vector3 &p_inertia);
+
+	float get_bounce() const;
+	void set_bounce(float p_bounce);
+
+	float get_friction() const;
+	void set_friction(float p_friction);
+
+	float get_gravity_scale() const { return gravity_scale; }
+	void set_gravity_scale(float p_scale);
+
+	Vector3 get_gravity() const { return gravity; }
+
+	float get_linear_damp() const { return linear_damp; }
+	void set_linear_damp(float p_damp);
+
+	float get_angular_damp() const { return angular_damp; }
+	void set_angular_damp(float p_damp);
+
+	float get_total_linear_damp() const { return total_linear_damp; }
+	float get_total_angular_damp() const { return total_angular_damp; }
+
+	float get_collision_priority() const { return collision_priority; }
+	void set_collision_priority(float p_priority) { collision_priority = p_priority; }
+
+	DampMode get_linear_damp_mode() const { return linear_damp_mode; }
+	void set_linear_damp_mode(DampMode p_mode) { linear_damp_mode = p_mode; }
+
+	DampMode get_angular_damp_mode() const { return angular_damp_mode; }
+	void set_angular_damp_mode(DampMode p_mode) { angular_damp_mode = p_mode; }
+
+	bool is_axis_locked(PhysicsServer3D::BodyAxis p_axis) const;
+	void set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool p_enabled);
+	bool are_axes_locked() const { return locked_axes != 0; }
+
+	virtual bool can_interact_with(const JoltBody3D &p_other) const override;
+	virtual bool can_interact_with(const JoltSoftBody3D &p_other) const override;
+	virtual bool can_interact_with(const JoltArea3D &p_other) const override;
+};
+
+#endif // JOLT_BODY_3D_H
diff --git a/modules/jolt_physics/objects/jolt_group_filter.cpp b/modules/jolt_physics/objects/jolt_group_filter.cpp
new file mode 100644
index 0000000000..b06ce8c66b
--- /dev/null
+++ b/modules/jolt_physics/objects/jolt_group_filter.cpp
@@ -0,0 +1,60 @@
+/**************************************************************************/
+/*  jolt_group_filter.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_group_filter.h"
+
+#include "jolt_area_3d.h"
+#include "jolt_body_3d.h"
+#include "jolt_object_3d.h"
+
+bool JoltGroupFilter::CanCollide(const JPH::CollisionGroup &p_group1, const JPH::CollisionGroup &p_group2) const {
+	const JoltObject3D *object1 = decode_object(p_group1.GetGroupID(), p_group1.GetSubGroupID());
+	const JoltObject3D *object2 = decode_object(p_group2.GetGroupID(), p_group2.GetSubGroupID());
+	return object1->can_interact_with(*object2);
+}
+
+void JoltGroupFilter::encode_object(const JoltObject3D *p_object, JPH::CollisionGroup::GroupID &r_group_id, JPH::CollisionGroup::SubGroupID &r_sub_group_id) {
+	// Since group filters don't grant us access to the bodies or their user data we are instead forced use the
+	// collision group to carry the upper and lower bits of our pointer, which we can access and decode in `CanCollide`.
+	const uint64_t address = reinterpret_cast<uint64_t>(p_object);
+	r_group_id = JPH::CollisionGroup::GroupID(address >> 32U);
+	r_sub_group_id = JPH::CollisionGroup::SubGroupID(address & 0xFFFFFFFFULL);
+}
+
+const JoltObject3D *JoltGroupFilter::decode_object(JPH::CollisionGroup::GroupID p_group_id, JPH::CollisionGroup::SubGroupID p_sub_group_id) {
+	const uint64_t upper_bits = (uint64_t)p_group_id << 32U;
+	const uint64_t lower_bits = (uint64_t)p_sub_group_id;
+	const uint64_t address = uint64_t(upper_bits | lower_bits);
+	return reinterpret_cast<const JoltObject3D *>(address);
+}
+
+static_assert(sizeof(JoltObject3D *) <= 8, "Pointer size greater than expected.");
+static_assert(sizeof(JPH::CollisionGroup::GroupID) == 4, "Size of Jolt's collision group ID has changed.");
+static_assert(sizeof(JPH::CollisionGroup::SubGroupID) == 4, "Size of Jolt's collision sub-group ID has changed.");
diff --git a/modules/jolt_physics/objects/jolt_group_filter.h b/modules/jolt_physics/objects/jolt_group_filter.h
new file mode 100644
index 0000000000..cadffb83ec
--- /dev/null
+++ b/modules/jolt_physics/objects/jolt_group_filter.h
@@ -0,0 +1,51 @@
+/**************************************************************************/
+/*  jolt_group_filter.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_GROUP_FILTER_H
+#define JOLT_GROUP_FILTER_H
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Collision/CollisionGroup.h"
+#include "Jolt/Physics/Collision/GroupFilter.h"
+
+class JoltObject3D;
+
+class JoltGroupFilter final : public JPH::GroupFilter {
+	virtual bool CanCollide(const JPH::CollisionGroup &p_group1, const JPH::CollisionGroup &p_group2) const override;
+
+public:
+	inline static JoltGroupFilter *instance = nullptr;
+
+	static void encode_object(const JoltObject3D *p_object, JPH::CollisionGroup::GroupID &r_group_id, JPH::CollisionGroup::SubGroupID &r_sub_group_id);
+	static const JoltObject3D *decode_object(JPH::CollisionGroup::GroupID p_group_id, JPH::CollisionGroup::SubGroupID p_sub_group_id);
+};
+
+#endif // JOLT_GROUP_FILTER_H
diff --git a/modules/jolt_physics/objects/jolt_object_3d.cpp b/modules/jolt_physics/objects/jolt_object_3d.cpp
new file mode 100644
index 0000000000..7ee46acbef
--- /dev/null
+++ b/modules/jolt_physics/objects/jolt_object_3d.cpp
@@ -0,0 +1,148 @@
+/**************************************************************************/
+/*  jolt_object_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_object_3d.h"
+
+#include "../jolt_project_settings.h"
+#include "../spaces/jolt_layers.h"
+#include "../spaces/jolt_space_3d.h"
+#include "jolt_group_filter.h"
+
+void JoltObject3D::_remove_from_space() {
+	if (unlikely(jolt_id.IsInvalid())) {
+		return;
+	}
+
+	space->remove_body(jolt_id);
+
+	jolt_id = JPH::BodyID();
+}
+
+void JoltObject3D::_reset_space() {
+	ERR_FAIL_NULL(space);
+
+	_space_changing();
+	_remove_from_space();
+	_add_to_space();
+	_space_changed();
+}
+
+void JoltObject3D::_update_object_layer() {
+	if (!in_space()) {
+		return;
+	}
+
+	space->get_body_iface().SetObjectLayer(jolt_id, _get_object_layer());
+}
+
+void JoltObject3D::_collision_layer_changed() {
+	_update_object_layer();
+}
+
+void JoltObject3D::_collision_mask_changed() {
+	_update_object_layer();
+}
+
+JoltObject3D::JoltObject3D(ObjectType p_object_type) :
+		object_type(p_object_type) {
+}
+
+JoltObject3D::~JoltObject3D() = default;
+
+Object *JoltObject3D::get_instance() const {
+	return ObjectDB::get_instance(instance_id);
+}
+
+void JoltObject3D::set_space(JoltSpace3D *p_space) {
+	if (space == p_space) {
+		return;
+	}
+
+	_space_changing();
+
+	if (space != nullptr) {
+		_remove_from_space();
+	}
+
+	space = p_space;
+
+	if (space != nullptr) {
+		_add_to_space();
+	}
+
+	_space_changed();
+}
+
+void JoltObject3D::set_collision_layer(uint32_t p_layer) {
+	if (p_layer == collision_layer) {
+		return;
+	}
+
+	collision_layer = p_layer;
+
+	_collision_layer_changed();
+}
+
+void JoltObject3D::set_collision_mask(uint32_t p_mask) {
+	if (p_mask == collision_mask) {
+		return;
+	}
+
+	collision_mask = p_mask;
+
+	_collision_mask_changed();
+}
+
+bool JoltObject3D::can_collide_with(const JoltObject3D &p_other) const {
+	return (collision_mask & p_other.get_collision_layer()) != 0;
+}
+
+bool JoltObject3D::can_interact_with(const JoltObject3D &p_other) const {
+	if (const JoltBody3D *other_body = p_other.as_body()) {
+		return can_interact_with(*other_body);
+	} else if (const JoltArea3D *other_area = p_other.as_area()) {
+		return can_interact_with(*other_area);
+	} else if (const JoltSoftBody3D *other_soft_body = p_other.as_soft_body()) {
+		return can_interact_with(*other_soft_body);
+	} else {
+		ERR_FAIL_V_MSG(false, vformat("Unhandled object type: '%d'. This should not happen. Please report this.", p_other.get_type()));
+	}
+}
+
+void JoltObject3D::pre_step(float p_step, JPH::Body &p_jolt_body) {
+}
+
+void JoltObject3D::post_step(float p_step, JPH::Body &p_jolt_body) {
+}
+
+String JoltObject3D::to_string() const {
+	Object *instance = get_instance();
+	return instance != nullptr ? instance->to_string() : "<unknown>";
+}
diff --git a/modules/jolt_physics/objects/jolt_object_3d.h b/modules/jolt_physics/objects/jolt_object_3d.h
new file mode 100644
index 0000000000..6fa9cc7905
--- /dev/null
+++ b/modules/jolt_physics/objects/jolt_object_3d.h
@@ -0,0 +1,157 @@
+/**************************************************************************/
+/*  jolt_object_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_OBJECT_3D_H
+#define JOLT_OBJECT_3D_H
+
+#include "../shapes/jolt_shape_instance_3d.h"
+
+#include "core/math/vector3.h"
+#include "core/object/object.h"
+#include "core/string/ustring.h"
+#include "core/templates/local_vector.h"
+#include "core/templates/rid.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Body/Body.h"
+#include "Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h"
+#include "Jolt/Physics/Collision/ObjectLayer.h"
+
+class JoltArea3D;
+class JoltBody3D;
+class JoltShapedObject3D;
+class JoltShape3D;
+class JoltSoftBody3D;
+class JoltSpace3D;
+
+class JoltObject3D {
+public:
+	enum ObjectType : char {
+		OBJECT_TYPE_INVALID,
+		OBJECT_TYPE_BODY,
+		OBJECT_TYPE_SOFT_BODY,
+		OBJECT_TYPE_AREA,
+	};
+
+protected:
+	LocalVector<JoltShapeInstance3D> shapes;
+
+	RID rid;
+	ObjectID instance_id;
+	JoltSpace3D *space = nullptr;
+	JPH::BodyID jolt_id;
+
+	uint32_t collision_layer = 1;
+	uint32_t collision_mask = 1;
+
+	ObjectType object_type = OBJECT_TYPE_INVALID;
+
+	bool pickable = false;
+
+	virtual JPH::BroadPhaseLayer _get_broad_phase_layer() const = 0;
+	virtual JPH::ObjectLayer _get_object_layer() const = 0;
+
+	virtual void _add_to_space() = 0;
+	virtual void _remove_from_space();
+
+	void _reset_space();
+
+	void _update_object_layer();
+
+	virtual void _collision_layer_changed();
+	virtual void _collision_mask_changed();
+
+	virtual void _space_changing() {}
+	virtual void _space_changed() {}
+
+public:
+	explicit JoltObject3D(ObjectType p_object_type);
+	virtual ~JoltObject3D() = 0;
+
+	ObjectType get_type() const { return object_type; }
+
+	bool is_body() const { return object_type == OBJECT_TYPE_BODY; }
+	bool is_soft_body() const { return object_type == OBJECT_TYPE_SOFT_BODY; }
+	bool is_area() const { return object_type == OBJECT_TYPE_AREA; }
+	bool is_shaped() const { return object_type != OBJECT_TYPE_SOFT_BODY; }
+
+	JoltShapedObject3D *as_shaped() { return is_shaped() ? reinterpret_cast<JoltShapedObject3D *>(this) : nullptr; }
+	const JoltShapedObject3D *as_shaped() const { return is_shaped() ? reinterpret_cast<const JoltShapedObject3D *>(this) : nullptr; }
+
+	JoltBody3D *as_body() { return is_body() ? reinterpret_cast<JoltBody3D *>(this) : nullptr; }
+	const JoltBody3D *as_body() const { return is_body() ? reinterpret_cast<const JoltBody3D *>(this) : nullptr; }
+
+	JoltSoftBody3D *as_soft_body() { return is_soft_body() ? reinterpret_cast<JoltSoftBody3D *>(this) : nullptr; }
+	const JoltSoftBody3D *as_soft_body() const { return is_soft_body() ? reinterpret_cast<const JoltSoftBody3D *>(this) : nullptr; }
+
+	JoltArea3D *as_area() { return is_area() ? reinterpret_cast<JoltArea3D *>(this) : nullptr; }
+	const JoltArea3D *as_area() const { return is_area() ? reinterpret_cast<const JoltArea3D *>(this) : nullptr; }
+
+	RID get_rid() const { return rid; }
+	void set_rid(const RID &p_rid) { rid = p_rid; }
+
+	ObjectID get_instance_id() const { return instance_id; }
+	void set_instance_id(ObjectID p_id) { instance_id = p_id; }
+	Object *get_instance() const;
+
+	JPH::BodyID get_jolt_id() const { return jolt_id; }
+
+	JoltSpace3D *get_space() const { return space; }
+	void set_space(JoltSpace3D *p_space);
+	bool in_space() const { return space != nullptr && !jolt_id.IsInvalid(); }
+
+	uint32_t get_collision_layer() const { return collision_layer; }
+	void set_collision_layer(uint32_t p_layer);
+
+	uint32_t get_collision_mask() const { return collision_mask; }
+	void set_collision_mask(uint32_t p_mask);
+
+	virtual Vector3 get_velocity_at_position(const Vector3 &p_position) const = 0;
+
+	bool is_pickable() const { return pickable; }
+	void set_pickable(bool p_enabled) { pickable = p_enabled; }
+
+	bool can_collide_with(const JoltObject3D &p_other) const;
+	bool can_interact_with(const JoltObject3D &p_other) const;
+
+	virtual bool can_interact_with(const JoltBody3D &p_other) const = 0;
+	virtual bool can_interact_with(const JoltSoftBody3D &p_other) const = 0;
+	virtual bool can_interact_with(const JoltArea3D &p_other) const = 0;
+
+	virtual bool reports_contacts() const = 0;
+
+	virtual void pre_step(float p_step, JPH::Body &p_jolt_body);
+	virtual void post_step(float p_step, JPH::Body &p_jolt_body);
+
+	String to_string() const;
+};
+
+#endif // JOLT_OBJECT_3D_H
diff --git a/modules/jolt_physics/objects/jolt_physics_direct_body_state_3d.cpp b/modules/jolt_physics/objects/jolt_physics_direct_body_state_3d.cpp
new file mode 100644
index 0000000000..2e45044611
--- /dev/null
+++ b/modules/jolt_physics/objects/jolt_physics_direct_body_state_3d.cpp
@@ -0,0 +1,245 @@
+/**************************************************************************/
+/*  jolt_physics_direct_body_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_body_state_3d.h"
+
+#include "../spaces/jolt_physics_direct_space_state_3d.h"
+#include "../spaces/jolt_space_3d.h"
+#include "jolt_body_3d.h"
+
+JoltPhysicsDirectBodyState3D::JoltPhysicsDirectBodyState3D(JoltBody3D *p_body) :
+		body(p_body) {
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_total_gravity() const {
+	return body->get_gravity();
+}
+
+real_t JoltPhysicsDirectBodyState3D::get_total_angular_damp() const {
+	return (real_t)body->get_total_angular_damp();
+}
+
+real_t JoltPhysicsDirectBodyState3D::get_total_linear_damp() const {
+	return (real_t)body->get_total_linear_damp();
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_center_of_mass() const {
+	return body->get_center_of_mass_relative();
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_center_of_mass_local() const {
+	return body->get_center_of_mass_local();
+}
+
+Basis JoltPhysicsDirectBodyState3D::get_principal_inertia_axes() const {
+	return body->get_principal_inertia_axes();
+}
+
+real_t JoltPhysicsDirectBodyState3D::get_inverse_mass() const {
+	return 1.0 / body->get_mass();
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_inverse_inertia() const {
+	return body->get_inverse_inertia();
+}
+
+Basis JoltPhysicsDirectBodyState3D::get_inverse_inertia_tensor() const {
+	return body->get_inverse_inertia_tensor();
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_linear_velocity() const {
+	return body->get_linear_velocity();
+}
+
+void JoltPhysicsDirectBodyState3D::set_linear_velocity(const Vector3 &p_velocity) {
+	return body->set_linear_velocity(p_velocity);
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_angular_velocity() const {
+	return body->get_angular_velocity();
+}
+
+void JoltPhysicsDirectBodyState3D::set_angular_velocity(const Vector3 &p_velocity) {
+	return body->set_angular_velocity(p_velocity);
+}
+
+void JoltPhysicsDirectBodyState3D::set_transform(const Transform3D &p_transform) {
+	return body->set_transform(p_transform);
+}
+
+Transform3D JoltPhysicsDirectBodyState3D::get_transform() const {
+	return body->get_transform_scaled();
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_velocity_at_local_position(const Vector3 &p_local_position) const {
+	return body->get_velocity_at_position(body->get_position() + p_local_position);
+}
+
+void JoltPhysicsDirectBodyState3D::apply_central_impulse(const Vector3 &p_impulse) {
+	return body->apply_central_impulse(p_impulse);
+}
+
+void JoltPhysicsDirectBodyState3D::apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position) {
+	return body->apply_impulse(p_impulse, p_position);
+}
+
+void JoltPhysicsDirectBodyState3D::apply_torque_impulse(const Vector3 &p_impulse) {
+	return body->apply_torque_impulse(p_impulse);
+}
+
+void JoltPhysicsDirectBodyState3D::apply_central_force(const Vector3 &p_force) {
+	return body->apply_central_force(p_force);
+}
+
+void JoltPhysicsDirectBodyState3D::apply_force(const Vector3 &p_force, const Vector3 &p_position) {
+	return body->apply_force(p_force, p_position);
+}
+
+void JoltPhysicsDirectBodyState3D::apply_torque(const Vector3 &p_torque) {
+	return body->apply_torque(p_torque);
+}
+
+void JoltPhysicsDirectBodyState3D::add_constant_central_force(const Vector3 &p_force) {
+	return body->add_constant_central_force(p_force);
+}
+
+void JoltPhysicsDirectBodyState3D::add_constant_force(const Vector3 &p_force, const Vector3 &p_position) {
+	return body->add_constant_force(p_force, p_position);
+}
+
+void JoltPhysicsDirectBodyState3D::add_constant_torque(const Vector3 &p_torque) {
+	return body->add_constant_torque(p_torque);
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_constant_force() const {
+	return body->get_constant_force();
+}
+
+void JoltPhysicsDirectBodyState3D::set_constant_force(const Vector3 &p_force) {
+	return body->set_constant_force(p_force);
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_constant_torque() const {
+	return body->get_constant_torque();
+}
+
+void JoltPhysicsDirectBodyState3D::set_constant_torque(const Vector3 &p_torque) {
+	return body->set_constant_torque(p_torque);
+}
+
+bool JoltPhysicsDirectBodyState3D::is_sleeping() const {
+	return body->is_sleeping();
+}
+
+void JoltPhysicsDirectBodyState3D::set_sleep_state(bool p_enabled) {
+	body->set_is_sleeping(p_enabled);
+}
+
+int JoltPhysicsDirectBodyState3D::get_contact_count() const {
+	return body->get_contact_count();
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_contact_local_position(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), Vector3());
+	return body->get_contact(p_contact_idx).position;
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_contact_local_normal(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), Vector3());
+	return body->get_contact(p_contact_idx).normal;
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_contact_impulse(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), Vector3());
+	return body->get_contact(p_contact_idx).impulse;
+}
+
+int JoltPhysicsDirectBodyState3D::get_contact_local_shape(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), 0);
+	return body->get_contact(p_contact_idx).shape_index;
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_contact_local_velocity_at_position(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), Vector3());
+	return body->get_contact(p_contact_idx).velocity;
+}
+
+RID JoltPhysicsDirectBodyState3D::get_contact_collider(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), RID());
+	return body->get_contact(p_contact_idx).collider_rid;
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_contact_collider_position(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), Vector3());
+	return body->get_contact(p_contact_idx).collider_position;
+}
+
+ObjectID JoltPhysicsDirectBodyState3D::get_contact_collider_id(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), ObjectID());
+	return body->get_contact(p_contact_idx).collider_id;
+}
+
+Object *JoltPhysicsDirectBodyState3D::get_contact_collider_object(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), nullptr);
+	return ObjectDB::get_instance(body->get_contact(p_contact_idx).collider_id);
+}
+
+int JoltPhysicsDirectBodyState3D::get_contact_collider_shape(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), 0);
+	return body->get_contact(p_contact_idx).collider_shape_index;
+}
+
+Vector3 JoltPhysicsDirectBodyState3D::get_contact_collider_velocity_at_position(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, (int)body->get_contact_count(), Vector3());
+	return body->get_contact(p_contact_idx).collider_velocity;
+}
+
+real_t JoltPhysicsDirectBodyState3D::get_step() const {
+	return (real_t)body->get_space()->get_last_step();
+}
+
+void JoltPhysicsDirectBodyState3D::integrate_forces() {
+	const float step = (float)get_step();
+
+	Vector3 linear_velocity = get_linear_velocity();
+	Vector3 angular_velocity = get_angular_velocity();
+
+	linear_velocity *= MAX(1.0f - (float)get_total_linear_damp() * step, 0.0f);
+	angular_velocity *= MAX(1.0f - (float)get_total_angular_damp() * step, 0.0f);
+
+	linear_velocity += get_total_gravity() * step;
+
+	set_linear_velocity(linear_velocity);
+	set_angular_velocity(angular_velocity);
+}
+
+PhysicsDirectSpaceState3D *JoltPhysicsDirectBodyState3D::get_space_state() {
+	return body->get_space()->get_direct_state();
+}
diff --git a/modules/jolt_physics/objects/jolt_physics_direct_body_state_3d.h b/modules/jolt_physics/objects/jolt_physics_direct_body_state_3d.h
new file mode 100644
index 0000000000..e2a7ad78cb
--- /dev/null
+++ b/modules/jolt_physics/objects/jolt_physics_direct_body_state_3d.h
@@ -0,0 +1,116 @@
+/**************************************************************************/
+/*  jolt_physics_direct_body_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_BODY_STATE_3D_H
+#define JOLT_PHYSICS_DIRECT_BODY_STATE_3D_H
+
+#include "servers/physics_server_3d.h"
+
+class JoltBody3D;
+
+class JoltPhysicsDirectBodyState3D final : public PhysicsDirectBodyState3D {
+	GDCLASS(JoltPhysicsDirectBodyState3D, PhysicsDirectBodyState3D)
+
+	JoltBody3D *body = nullptr;
+
+	static void _bind_methods() {}
+
+public:
+	JoltPhysicsDirectBodyState3D() = default;
+
+	explicit JoltPhysicsDirectBodyState3D(JoltBody3D *p_body);
+
+	virtual Vector3 get_total_gravity() const override;
+	virtual real_t get_total_linear_damp() const override;
+	virtual real_t get_total_angular_damp() const override;
+
+	virtual Vector3 get_center_of_mass() const override;
+	virtual Vector3 get_center_of_mass_local() const override;
+	virtual Basis get_principal_inertia_axes() const override;
+
+	virtual real_t get_inverse_mass() const override;
+	virtual Vector3 get_inverse_inertia() const override;
+	virtual Basis get_inverse_inertia_tensor() const override;
+
+	virtual void set_linear_velocity(const Vector3 &p_velocity) override;
+	virtual Vector3 get_linear_velocity() const override;
+
+	virtual void set_angular_velocity(const Vector3 &p_velocity) override;
+	virtual Vector3 get_angular_velocity() const override;
+
+	virtual void set_transform(const Transform3D &p_transform) override;
+	virtual Transform3D get_transform() const override;
+
+	virtual Vector3 get_velocity_at_local_position(const Vector3 &p_local_position) const override;
+
+	virtual void apply_central_impulse(const Vector3 &p_impulse) override;
+	virtual void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position) override;
+	virtual void apply_torque_impulse(const Vector3 &p_impulse) override;
+
+	virtual void apply_central_force(const Vector3 &p_force) override;
+	virtual void apply_force(const Vector3 &p_force, const Vector3 &p_position) override;
+	virtual void apply_torque(const Vector3 &p_torque) override;
+
+	virtual void add_constant_central_force(const Vector3 &p_force) override;
+	virtual void add_constant_force(const Vector3 &p_force, const Vector3 &p_position) override;
+	virtual void add_constant_torque(const Vector3 &p_torque) override;
+
+	virtual void set_constant_force(const Vector3 &p_force) override;
+	virtual Vector3 get_constant_force() const override;
+
+	virtual void set_constant_torque(const Vector3 &p_torque) override;
+	virtual Vector3 get_constant_torque() const override;
+
+	virtual void set_sleep_state(bool p_enabled) override;
+	virtual bool is_sleeping() const override;
+
+	virtual int get_contact_count() const override;
+
+	virtual Vector3 get_contact_local_position(int p_contact_idx) const override;
+	virtual Vector3 get_contact_local_normal(int p_contact_idx) const override;
+	virtual Vector3 get_contact_impulse(int p_contact_idx) const override;
+	virtual int get_contact_local_shape(int p_contact_idx) const override;
+	virtual Vector3 get_contact_local_velocity_at_position(int p_contact_idx) const override;
+
+	virtual RID get_contact_collider(int p_contact_idx) const override;
+	virtual Vector3 get_contact_collider_position(int p_contact_idx) const override;
+	virtual ObjectID get_contact_collider_id(int p_contact_idx) const override;
+	virtual Object *get_contact_collider_object(int p_contact_idx) const override;
+	virtual int get_contact_collider_shape(int p_contact_idx) const override;
+	virtual Vector3 get_contact_collider_velocity_at_position(int p_contact_idx) const override;
+
+	virtual real_t get_step() const override;
+
+	virtual void integrate_forces() override;
+
+	virtual PhysicsDirectSpaceState3D *get_space_state() override;
+};
+
+#endif // JOLT_PHYSICS_DIRECT_BODY_STATE_3D_H
diff --git a/modules/jolt_physics/objects/jolt_shaped_object_3d.cpp b/modules/jolt_physics/objects/jolt_shaped_object_3d.cpp
new file mode 100644
index 0000000000..9c83718fb6
--- /dev/null
+++ b/modules/jolt_physics/objects/jolt_shaped_object_3d.cpp
@@ -0,0 +1,431 @@
+/**************************************************************************/
+/*  jolt_shaped_object_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_shaped_object_3d.h"
+
+#include "../misc/jolt_type_conversions.h"
+#include "../shapes/jolt_custom_double_sided_shape.h"
+#include "../shapes/jolt_shape_3d.h"
+#include "../spaces/jolt_space_3d.h"
+
+#include "Jolt/Physics/Collision/Shape/EmptyShape.h"
+#include "Jolt/Physics/Collision/Shape/StaticCompoundShape.h"
+
+bool JoltShapedObject3D::_is_big() const {
+	// This number is completely arbitrary, and mostly just needs to capture `WorldBoundaryShape3D`, which needs to be kept out of the normal broadphase layers.
+	return get_aabb().get_longest_axis_size() >= 1000.0f;
+}
+
+JPH::ShapeRefC JoltShapedObject3D::_try_build_shape() {
+	int built_shapes = 0;
+
+	for (JoltShapeInstance3D &shape : shapes) {
+		if (shape.is_enabled() && shape.try_build()) {
+			built_shapes += 1;
+		}
+	}
+
+	if (unlikely(built_shapes == 0)) {
+		return nullptr;
+	}
+
+	JPH::ShapeRefC result = built_shapes == 1 ? _try_build_single_shape() : _try_build_compound_shape();
+	if (unlikely(result == nullptr)) {
+		return nullptr;
+	}
+
+	if (has_custom_center_of_mass()) {
+		result = JoltShape3D::with_center_of_mass(result, get_center_of_mass_custom());
+	}
+
+	if (scale != Vector3(1, 1, 1)) {
+		Vector3 actual_scale = scale;
+		JOLT_ENSURE_SCALE_VALID(result, actual_scale, vformat("Failed to correctly scale body '%s'.", to_string()));
+		result = JoltShape3D::with_scale(result, actual_scale);
+	}
+
+	if (is_area()) {
+		result = JoltShape3D::with_double_sided(result, true);
+	}
+
+	return result;
+}
+
+JPH::ShapeRefC JoltShapedObject3D::_try_build_single_shape() {
+	for (int shape_index = 0; shape_index < (int)shapes.size(); ++shape_index) {
+		const JoltShapeInstance3D &sub_shape = shapes[shape_index];
+
+		if (!sub_shape.is_enabled() || !sub_shape.is_built()) {
+			continue;
+		}
+
+		JPH::ShapeRefC jolt_sub_shape = sub_shape.get_jolt_ref();
+
+		Vector3 sub_shape_scale = sub_shape.get_scale();
+		const Transform3D sub_shape_transform = sub_shape.get_transform_unscaled();
+
+		if (sub_shape_scale != Vector3(1, 1, 1)) {
+			JOLT_ENSURE_SCALE_VALID(jolt_sub_shape, sub_shape_scale, vformat("Failed to correctly scale shape at index %d in body '%s'.", shape_index, to_string()));
+			jolt_sub_shape = JoltShape3D::with_scale(jolt_sub_shape, sub_shape_scale);
+		}
+
+		if (sub_shape_transform != Transform3D()) {
+			jolt_sub_shape = JoltShape3D::with_basis_origin(jolt_sub_shape, sub_shape_transform.basis, sub_shape_transform.origin);
+		}
+
+		return jolt_sub_shape;
+	}
+
+	return nullptr;
+}
+
+JPH::ShapeRefC JoltShapedObject3D::_try_build_compound_shape() {
+	JPH::StaticCompoundShapeSettings compound_shape_settings;
+
+	for (int shape_index = 0; shape_index < (int)shapes.size(); ++shape_index) {
+		const JoltShapeInstance3D &sub_shape = shapes[shape_index];
+
+		if (!sub_shape.is_enabled() || !sub_shape.is_built()) {
+			continue;
+		}
+
+		JPH::ShapeRefC jolt_sub_shape = sub_shape.get_jolt_ref();
+
+		Vector3 sub_shape_scale = sub_shape.get_scale();
+		const Transform3D sub_shape_transform = sub_shape.get_transform_unscaled();
+
+		if (sub_shape_scale != Vector3(1, 1, 1)) {
+			JOLT_ENSURE_SCALE_VALID(jolt_sub_shape, sub_shape_scale, vformat("Failed to correctly scale shape at index %d in body '%s'.", shape_index, to_string()));
+			jolt_sub_shape = JoltShape3D::with_scale(jolt_sub_shape, sub_shape_scale);
+		}
+
+		compound_shape_settings.AddShape(to_jolt(sub_shape_transform.origin), to_jolt(sub_shape_transform.basis), jolt_sub_shape);
+	}
+
+	const JPH::ShapeSettings::ShapeResult shape_result = compound_shape_settings.Create();
+	ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to create compound shape with sub-shape count '%d'. It returned the following error: '%s'.", (int)compound_shape_settings.mSubShapes.size(), to_godot(shape_result.GetError())));
+
+	return shape_result.Get();
+}
+
+void JoltShapedObject3D::_shapes_changed() {
+	_update_shape();
+	_update_object_layer();
+}
+
+void JoltShapedObject3D::_space_changing() {
+	JoltObject3D::_space_changing();
+
+	if (space != nullptr) {
+		const JoltWritableBody3D body = space->write_body(jolt_id);
+		ERR_FAIL_COND(body.is_invalid());
+
+		jolt_settings = new JPH::BodyCreationSettings(body->GetBodyCreationSettings());
+	}
+}
+
+void JoltShapedObject3D::_update_shape() {
+	if (!in_space()) {
+		_shapes_built();
+		return;
+	}
+
+	const JoltWritableBody3D body = space->write_body(jolt_id);
+	ERR_FAIL_COND(body.is_invalid());
+
+	previous_jolt_shape = jolt_shape;
+	jolt_shape = build_shape();
+
+	if (jolt_shape == previous_jolt_shape) {
+		return;
+	}
+
+	space->get_body_iface().SetShape(jolt_id, jolt_shape, false, JPH::EActivation::DontActivate);
+
+	_shapes_built();
+}
+
+JoltShapedObject3D::JoltShapedObject3D(ObjectType p_object_type) :
+		JoltObject3D(p_object_type) {
+	jolt_settings->mAllowSleeping = true;
+	jolt_settings->mFriction = 1.0f;
+	jolt_settings->mRestitution = 0.0f;
+	jolt_settings->mLinearDamping = 0.0f;
+	jolt_settings->mAngularDamping = 0.0f;
+	jolt_settings->mGravityFactor = 0.0f;
+}
+
+JoltShapedObject3D::~JoltShapedObject3D() {
+	if (jolt_settings != nullptr) {
+		delete jolt_settings;
+		jolt_settings = nullptr;
+	}
+}
+
+Transform3D JoltShapedObject3D::get_transform_unscaled() const {
+	if (!in_space()) {
+		return Transform3D(to_godot(jolt_settings->mRotation), to_godot(jolt_settings->mPosition));
+	}
+
+	const JoltReadableBody3D body = space->read_body(jolt_id);
+	ERR_FAIL_COND_V(body.is_invalid(), Transform3D());
+
+	return Transform3D(to_godot(body->GetRotation()), to_godot(body->GetPosition()));
+}
+
+Transform3D JoltShapedObject3D::get_transform_scaled() const {
+	return get_transform_unscaled().scaled_local(scale);
+}
+
+Basis JoltShapedObject3D::get_basis() const {
+	if (!in_space()) {
+		return to_godot(jolt_settings->mRotation);
+	}
+
+	const JoltReadableBody3D body = space->read_body(jolt_id);
+	ERR_FAIL_COND_V(body.is_invalid(), Basis());
+
+	return to_godot(body->GetRotation());
+}
+
+Vector3 JoltShapedObject3D::get_position() const {
+	if (!in_space()) {
+		return to_godot(jolt_settings->mPosition);
+	}
+
+	const JoltReadableBody3D body = space->read_body(jolt_id);
+	ERR_FAIL_COND_V(body.is_invalid(), Vector3());
+
+	return to_godot(body->GetPosition());
+}
+
+Vector3 JoltShapedObject3D::get_center_of_mass() const {
+	ERR_FAIL_NULL_V_MSG(space, Vector3(), vformat("Failed to retrieve center-of-mass of '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
+
+	const JoltReadableBody3D body = space->read_body(jolt_id);
+	ERR_FAIL_COND_V(body.is_invalid(), Vector3());
+
+	return to_godot(body->GetCenterOfMassPosition());
+}
+
+Vector3 JoltShapedObject3D::get_center_of_mass_relative() const {
+	return get_center_of_mass() - get_position();
+}
+
+Vector3 JoltShapedObject3D::get_center_of_mass_local() const {
+	ERR_FAIL_NULL_V_MSG(space, Vector3(), vformat("Failed to retrieve local center-of-mass of '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
+
+	return get_transform_scaled().xform_inv(get_center_of_mass());
+}
+
+Vector3 JoltShapedObject3D::get_linear_velocity() const {
+	if (!in_space()) {
+		return to_godot(jolt_settings->mLinearVelocity);
+	}
+
+	const JoltReadableBody3D body = space->read_body(jolt_id);
+	ERR_FAIL_COND_V(body.is_invalid(), Vector3());
+
+	return to_godot(body->GetLinearVelocity());
+}
+
+Vector3 JoltShapedObject3D::get_angular_velocity() const {
+	if (!in_space()) {
+		return to_godot(jolt_settings->mAngularVelocity);
+	}
+
+	const JoltReadableBody3D body = space->read_body(jolt_id);
+	ERR_FAIL_COND_V(body.is_invalid(), Vector3());
+
+	return to_godot(body->GetAngularVelocity());
+}
+
+AABB JoltShapedObject3D::get_aabb() const {
+	AABB result;
+
+	for (const JoltShapeInstance3D &shape : shapes) {
+		if (shape.is_disabled()) {
+			continue;
+		}
+
+		if (result == AABB()) {
+			result = shape.get_aabb();
+		} else {
+			result.merge_with(shape.get_aabb());
+		}
+	}
+
+	return get_transform_scaled().xform(result);
+}
+
+JPH::ShapeRefC JoltShapedObject3D::build_shape() {
+	JPH::ShapeRefC new_shape = _try_build_shape();
+
+	if (new_shape == nullptr) {
+		if (has_custom_center_of_mass()) {
+			new_shape = JPH::EmptyShapeSettings(to_jolt(get_center_of_mass_custom())).Create().Get();
+		} else {
+			new_shape = new JPH::EmptyShape();
+		}
+	}
+
+	return new_shape;
+}
+
+void JoltShapedObject3D::add_shape(JoltShape3D *p_shape, Transform3D p_transform, bool p_disabled) {
+	JOLT_ENSURE_SCALE_NOT_ZERO(p_transform, vformat("An invalid transform was passed when adding shape at index %d to physics body '%s'.", shapes.size(), to_string()));
+
+	shapes.push_back(JoltShapeInstance3D(this, p_shape, p_transform.orthonormalized(), p_transform.basis.get_scale(), p_disabled));
+
+	_shapes_changed();
+}
+
+void JoltShapedObject3D::remove_shape(const JoltShape3D *p_shape) {
+	for (int i = shapes.size() - 1; i >= 0; i--) {
+		if (shapes[i].get_shape() == p_shape) {
+			shapes.remove_at(i);
+		}
+	}
+
+	_shapes_changed();
+}
+
+void JoltShapedObject3D::remove_shape(int p_index) {
+	ERR_FAIL_INDEX(p_index, (int)shapes.size());
+	shapes.remove_at(p_index);
+
+	_shapes_changed();
+}
+
+JoltShape3D *JoltShapedObject3D::get_shape(int p_index) const {
+	ERR_FAIL_INDEX_V(p_index, (int)shapes.size(), nullptr);
+	return shapes[p_index].get_shape();
+}
+
+void JoltShapedObject3D::set_shape(int p_index, JoltShape3D *p_shape) {
+	ERR_FAIL_INDEX(p_index, (int)shapes.size());
+	shapes[p_index] = JoltShapeInstance3D(this, p_shape);
+
+	_shapes_changed();
+}
+
+void JoltShapedObject3D::clear_shapes() {
+	shapes.clear();
+
+	_shapes_changed();
+}
+
+int JoltShapedObject3D::find_shape_index(uint32_t p_shape_instance_id) const {
+	for (int i = 0; i < (int)shapes.size(); ++i) {
+		if (shapes[i].get_id() == p_shape_instance_id) {
+			return i;
+		}
+	}
+
+	return -1;
+}
+
+int JoltShapedObject3D::find_shape_index(const JPH::SubShapeID &p_sub_shape_id) const {
+	ERR_FAIL_NULL_V(jolt_shape, -1);
+	return find_shape_index((uint32_t)jolt_shape->GetSubShapeUserData(p_sub_shape_id));
+}
+
+JoltShape3D *JoltShapedObject3D::find_shape(uint32_t p_shape_instance_id) const {
+	const int shape_index = find_shape_index(p_shape_instance_id);
+	return shape_index != -1 ? shapes[shape_index].get_shape() : nullptr;
+}
+
+JoltShape3D *JoltShapedObject3D::find_shape(const JPH::SubShapeID &p_sub_shape_id) const {
+	const int shape_index = find_shape_index(p_sub_shape_id);
+	return shape_index != -1 ? shapes[shape_index].get_shape() : nullptr;
+}
+
+Transform3D JoltShapedObject3D::get_shape_transform_unscaled(int p_index) const {
+	ERR_FAIL_INDEX_V(p_index, (int)shapes.size(), Transform3D());
+	return shapes[p_index].get_transform_unscaled();
+}
+
+Transform3D JoltShapedObject3D::get_shape_transform_scaled(int p_index) const {
+	ERR_FAIL_INDEX_V(p_index, (int)shapes.size(), Transform3D());
+	return shapes[p_index].get_transform_scaled();
+}
+
+void JoltShapedObject3D::set_shape_transform(int p_index, Transform3D p_transform) {
+	ERR_FAIL_INDEX(p_index, (int)shapes.size());
+	JOLT_ENSURE_SCALE_NOT_ZERO(p_transform, "Failed to correctly set transform for shape at index %d in body '%s'.");
+
+	Vector3 new_scale = p_transform.basis.get_scale();
+	p_transform.basis.orthonormalize();
+
+	JoltShapeInstance3D &shape = shapes[p_index];
+
+	if (shape.get_transform_unscaled() == p_transform && shape.get_scale() == new_scale) {
+		return;
+	}
+
+	shape.set_transform(p_transform);
+	shape.set_scale(new_scale);
+
+	_shapes_changed();
+}
+
+Vector3 JoltShapedObject3D::get_shape_scale(int p_index) const {
+	ERR_FAIL_INDEX_V(p_index, (int)shapes.size(), Vector3());
+	return shapes[p_index].get_scale();
+}
+
+bool JoltShapedObject3D::is_shape_disabled(int p_index) const {
+	ERR_FAIL_INDEX_V(p_index, (int)shapes.size(), false);
+	return shapes[p_index].is_disabled();
+}
+
+void JoltShapedObject3D::set_shape_disabled(int p_index, bool p_disabled) {
+	ERR_FAIL_INDEX(p_index, (int)shapes.size());
+
+	JoltShapeInstance3D &shape = shapes[p_index];
+
+	if (shape.is_disabled() == p_disabled) {
+		return;
+	}
+
+	if (p_disabled) {
+		shape.disable();
+	} else {
+		shape.enable();
+	}
+
+	_shapes_changed();
+}
+
+void JoltShapedObject3D::post_step(float p_step, JPH::Body &p_jolt_body) {
+	JoltObject3D::post_step(p_step, p_jolt_body);
+
+	previous_jolt_shape = nullptr;
+}
diff --git a/modules/jolt_physics/objects/jolt_shaped_object_3d.h b/modules/jolt_physics/objects/jolt_shaped_object_3d.h
new file mode 100644
index 0000000000..9632746287
--- /dev/null
+++ b/modules/jolt_physics/objects/jolt_shaped_object_3d.h
@@ -0,0 +1,123 @@
+/**************************************************************************/
+/*  jolt_shaped_object_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_SHAPED_OBJECT_3D_H
+#define JOLT_SHAPED_OBJECT_3D_H
+
+#include "jolt_object_3d.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Body/Body.h"
+#include "Jolt/Physics/Body/BodyCreationSettings.h"
+
+class JoltShapedObject3D : public JoltObject3D {
+	friend class JoltShape3D;
+
+protected:
+	Vector3 scale = Vector3(1, 1, 1);
+
+	JPH::ShapeRefC jolt_shape;
+	JPH::ShapeRefC previous_jolt_shape;
+
+	JPH::BodyCreationSettings *jolt_settings = new JPH::BodyCreationSettings();
+
+	virtual JPH::EMotionType _get_motion_type() const = 0;
+
+	bool _is_big() const;
+
+	JPH::ShapeRefC _try_build_shape();
+	JPH::ShapeRefC _try_build_single_shape();
+	JPH::ShapeRefC _try_build_compound_shape();
+
+	virtual void _shapes_changed();
+	virtual void _shapes_built() {}
+	virtual void _space_changing() override;
+
+	void _update_shape();
+
+public:
+	explicit JoltShapedObject3D(ObjectType p_object_type);
+	virtual ~JoltShapedObject3D() override;
+
+	Transform3D get_transform_unscaled() const;
+	Transform3D get_transform_scaled() const;
+
+	Vector3 get_scale() const { return scale; }
+	Basis get_basis() const;
+	Vector3 get_position() const;
+
+	Vector3 get_center_of_mass() const;
+	Vector3 get_center_of_mass_relative() const;
+	Vector3 get_center_of_mass_local() const;
+
+	Vector3 get_linear_velocity() const;
+	Vector3 get_angular_velocity() const;
+
+	AABB get_aabb() const;
+
+	virtual bool has_custom_center_of_mass() const = 0;
+	virtual Vector3 get_center_of_mass_custom() const = 0;
+
+	JPH::ShapeRefC build_shape();
+
+	const JPH::Shape *get_jolt_shape() const { return jolt_shape; }
+	const JPH::Shape *get_previous_jolt_shape() const { return previous_jolt_shape; }
+
+	void add_shape(JoltShape3D *p_shape, Transform3D p_transform, bool p_disabled);
+	void remove_shape(const JoltShape3D *p_shape);
+	void remove_shape(int p_index);
+
+	JoltShape3D *get_shape(int p_index) const;
+	void set_shape(int p_index, JoltShape3D *p_shape);
+
+	void clear_shapes();
+
+	int get_shape_count() const { return shapes.size(); }
+
+	int find_shape_index(uint32_t p_shape_instance_id) const;
+	int find_shape_index(const JPH::SubShapeID &p_sub_shape_id) const;
+
+	JoltShape3D *find_shape(uint32_t p_shape_instance_id) const;
+	JoltShape3D *find_shape(const JPH::SubShapeID &p_sub_shape_id) const;
+
+	Transform3D get_shape_transform_unscaled(int p_index) const;
+	Transform3D get_shape_transform_scaled(int p_index) const;
+	void set_shape_transform(int p_index, Transform3D p_transform);
+
+	Vector3 get_shape_scale(int p_index) const;
+
+	bool is_shape_disabled(int p_index) const;
+	void set_shape_disabled(int p_index, bool p_disabled);
+
+	virtual void post_step(float p_step, JPH::Body &p_jolt_body) override;
+};
+
+#endif // JOLT_SHAPED_OBJECT_3D_H
diff --git a/modules/jolt_physics/objects/jolt_soft_body_3d.cpp b/modules/jolt_physics/objects/jolt_soft_body_3d.cpp
new file mode 100644
index 0000000000..c9e834fd42
--- /dev/null
+++ b/modules/jolt_physics/objects/jolt_soft_body_3d.cpp
@@ -0,0 +1,734 @@
+/**************************************************************************/
+/*  jolt_soft_body_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_soft_body_3d.h"
+
+#include "../jolt_project_settings.h"
+#include "../misc/jolt_type_conversions.h"
+#include "../spaces/jolt_broad_phase_layer.h"
+#include "../spaces/jolt_space_3d.h"
+#include "jolt_area_3d.h"
+#include "jolt_body_3d.h"
+#include "jolt_group_filter.h"
+
+#include "servers/rendering_server.h"
+
+#include "Jolt/Physics/SoftBody/SoftBodyMotionProperties.h"
+
+namespace {
+
+bool is_face_degenerate(const int p_face[3]) {
+	return p_face[0] == p_face[1] || p_face[0] == p_face[2] || p_face[1] == p_face[2];
+}
+
+template <typename TJoltVertex>
+void pin_vertices(const JoltSoftBody3D &p_body, const HashSet<int> &p_pinned_vertices, const LocalVector<int> &p_mesh_to_physics, JPH::Array<TJoltVertex> &r_physics_vertices) {
+	const int mesh_vertex_count = p_mesh_to_physics.size();
+	const int physics_vertex_count = (int)r_physics_vertices.size();
+
+	for (int mesh_index : p_pinned_vertices) {
+		ERR_CONTINUE_MSG(mesh_index < 0 || mesh_index >= mesh_vertex_count, vformat("Index %d of pinned vertex in soft body '%s' is out of bounds. There are only %d vertices in the current mesh.", mesh_index, p_body.to_string(), mesh_vertex_count));
+
+		const int physics_index = p_mesh_to_physics[mesh_index];
+		ERR_CONTINUE_MSG(physics_index < 0 || physics_index >= physics_vertex_count, vformat("Index %d of pinned vertex in soft body '%s' is out of bounds. There are only %d vertices in the current mesh. This should not happen. Please report this.", physics_index, p_body.to_string(), physics_vertex_count));
+
+		r_physics_vertices[physics_index].mInvMass = 0.0f;
+	}
+}
+
+} // namespace
+
+JPH::BroadPhaseLayer JoltSoftBody3D::_get_broad_phase_layer() const {
+	return JoltBroadPhaseLayer::BODY_DYNAMIC;
+}
+
+JPH::ObjectLayer JoltSoftBody3D::_get_object_layer() const {
+	ERR_FAIL_NULL_V(space, 0);
+
+	return space->map_to_object_layer(_get_broad_phase_layer(), collision_layer, collision_mask);
+}
+
+void JoltSoftBody3D::_space_changing() {
+	JoltObject3D::_space_changing();
+
+	_deref_shared_data();
+
+	if (space != nullptr && !jolt_id.IsInvalid()) {
+		const JoltReadableBody3D body = space->read_body(jolt_id);
+		ERR_FAIL_COND(body.is_invalid());
+
+		jolt_settings = new JPH::SoftBodyCreationSettings(body->GetSoftBodyCreationSettings());
+		jolt_settings->mSettings = nullptr;
+	}
+}
+
+void JoltSoftBody3D::_space_changed() {
+	JoltObject3D::_space_changed();
+
+	_update_mass();
+	_update_pressure();
+	_update_damping();
+	_update_simulation_precision();
+	_update_group_filter();
+}
+
+void JoltSoftBody3D::_add_to_space() {
+	if (unlikely(space == nullptr || !mesh.is_valid())) {
+		return;
+	}
+
+	const bool has_valid_shared = _ref_shared_data();
+	ERR_FAIL_COND(!has_valid_shared);
+
+	JPH::CollisionGroup::GroupID group_id = 0;
+	JPH::CollisionGroup::SubGroupID sub_group_id = 0;
+	JoltGroupFilter::encode_object(this, group_id, sub_group_id);
+
+	jolt_settings->mSettings = shared->settings;
+	jolt_settings->mUserData = reinterpret_cast<JPH::uint64>(this);
+	jolt_settings->mObjectLayer = _get_object_layer();
+	jolt_settings->mCollisionGroup = JPH::CollisionGroup(nullptr, group_id, sub_group_id);
+	jolt_settings->mMaxLinearVelocity = JoltProjectSettings::get_max_linear_velocity();
+
+	const JPH::BodyID new_jolt_id = space->add_soft_body(*this, *jolt_settings);
+	if (new_jolt_id.IsInvalid()) {
+		return;
+	}
+
+	jolt_id = new_jolt_id;
+
+	delete jolt_settings;
+	jolt_settings = nullptr;
+}
+
+bool JoltSoftBody3D::_ref_shared_data() {
+	HashMap<RID, Shared>::Iterator iter_shared_data = mesh_to_shared.find(mesh);
+
+	if (iter_shared_data == mesh_to_shared.end()) {
+		RenderingServer *rendering = RenderingServer::get_singleton();
+
+		const Array mesh_data = rendering->mesh_surface_get_arrays(mesh, 0);
+		ERR_FAIL_COND_V(mesh_data.is_empty(), false);
+
+		const PackedInt32Array mesh_indices = mesh_data[RenderingServer::ARRAY_INDEX];
+		ERR_FAIL_COND_V(mesh_indices.is_empty(), false);
+
+		const PackedVector3Array mesh_vertices = mesh_data[RenderingServer::ARRAY_VERTEX];
+		ERR_FAIL_COND_V(mesh_vertices.is_empty(), false);
+
+		iter_shared_data = mesh_to_shared.insert(mesh, Shared());
+
+		LocalVector<int> &mesh_to_physics = iter_shared_data->value.mesh_to_physics;
+
+		JPH::SoftBodySharedSettings &settings = *iter_shared_data->value.settings;
+		settings.mVertexRadius = JoltProjectSettings::get_soft_body_point_radius();
+
+		JPH::Array<JPH::SoftBodySharedSettings::Vertex> &physics_vertices = settings.mVertices;
+		JPH::Array<JPH::SoftBodySharedSettings::Face> &physics_faces = settings.mFaces;
+
+		HashMap<Vector3, int> vertex_to_physics;
+
+		const int mesh_vertex_count = mesh_vertices.size();
+		const int mesh_index_count = mesh_indices.size();
+
+		mesh_to_physics.resize(mesh_vertex_count);
+		physics_vertices.reserve(mesh_vertex_count);
+		vertex_to_physics.reserve(mesh_vertex_count);
+
+		int physics_index_count = 0;
+
+		for (int i = 0; i < mesh_index_count; i += 3) {
+			int physics_face[3];
+			int mesh_face[3];
+
+			for (int j = 0; j < 3; ++j) {
+				const int mesh_index = mesh_indices[i + j];
+				const Vector3 vertex = mesh_vertices[mesh_index];
+
+				HashMap<Vector3, int>::Iterator iter_physics_index = vertex_to_physics.find(vertex);
+
+				if (iter_physics_index == vertex_to_physics.end()) {
+					physics_vertices.emplace_back(JPH::Float3((float)vertex.x, (float)vertex.y, (float)vertex.z), JPH::Float3(0.0f, 0.0f, 0.0f), 1.0f);
+
+					iter_physics_index = vertex_to_physics.insert(vertex, physics_index_count++);
+				}
+
+				mesh_face[j] = mesh_index;
+				physics_face[j] = iter_physics_index->value;
+				mesh_to_physics[mesh_index] = iter_physics_index->value;
+			}
+
+			ERR_CONTINUE_MSG(is_face_degenerate(physics_face), vformat("Failed to append face to soft body '%s'. Face was found to be degenerate. Face consist of indices %d, %d and %d.", to_string(), mesh_face[0], mesh_face[1], mesh_face[2]));
+
+			// Jolt uses a different winding order, so we swap the indices to account for that.
+			physics_faces.emplace_back((JPH::uint32)physics_face[2], (JPH::uint32)physics_face[1], (JPH::uint32)physics_face[0]);
+		}
+
+		// Pin whatever pinned vertices we have currently. This is used during the `Optimize` call below to order the
+		// constraints. Note that it's fine if the pinned vertices change later, but that will reduce the effectiveness
+		// of the constraints a bit.
+		pin_vertices(*this, pinned_vertices, mesh_to_physics, physics_vertices);
+
+		// Since Godot's stiffness is input as a coefficient between 0 and 1, and Jolt uses actual stiffness for its
+		// edge constraints, we crudely map one to the other with an arbitrary constant.
+		const float stiffness = MAX(Math::pow(stiffness_coefficient, 3.0f) * 100000.0f, 0.000001f);
+		const float inverse_stiffness = 1.0f / stiffness;
+
+		JPH::SoftBodySharedSettings::VertexAttributes vertex_attrib;
+		vertex_attrib.mCompliance = vertex_attrib.mShearCompliance = inverse_stiffness;
+
+		settings.CreateConstraints(&vertex_attrib, 1, JPH::SoftBodySharedSettings::EBendType::None);
+		settings.Optimize();
+	} else {
+		iter_shared_data->value.ref_count++;
+	}
+
+	shared = &iter_shared_data->value;
+
+	return true;
+}
+
+void JoltSoftBody3D::_deref_shared_data() {
+	if (unlikely(shared == nullptr)) {
+		return;
+	}
+
+	HashMap<RID, Shared>::Iterator iter = mesh_to_shared.find(mesh);
+	if (unlikely(iter == mesh_to_shared.end())) {
+		return;
+	}
+
+	if (--iter->value.ref_count == 0) {
+		mesh_to_shared.remove(iter);
+	}
+
+	shared = nullptr;
+}
+
+void JoltSoftBody3D::_update_mass() {
+	if (!in_space()) {
+		return;
+	}
+
+	JoltWritableBody3D body = space->write_body(jolt_id);
+	ERR_FAIL_COND(body.is_invalid());
+
+	JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*body->GetMotionPropertiesUnchecked());
+
+	JPH::Array<JPH::SoftBodyVertex> &physics_vertices = motion_properties.GetVertices();
+
+	const float inverse_vertex_mass = mass == 0.0f ? 1.0f : (float)physics_vertices.size() / mass;
+
+	for (JPH::SoftBodyVertex &vertex : physics_vertices) {
+		vertex.mInvMass = inverse_vertex_mass;
+	}
+
+	pin_vertices(*this, pinned_vertices, shared->mesh_to_physics, physics_vertices);
+}
+
+void JoltSoftBody3D::_update_pressure() {
+	if (!in_space()) {
+		jolt_settings->mPressure = pressure;
+		return;
+	}
+
+	JoltWritableBody3D body = space->write_body(jolt_id);
+	ERR_FAIL_COND(body.is_invalid());
+
+	JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*body->GetMotionPropertiesUnchecked());
+
+	motion_properties.SetPressure(pressure);
+}
+
+void JoltSoftBody3D::_update_damping() {
+	if (!in_space()) {
+		jolt_settings->mLinearDamping = linear_damping;
+		return;
+	}
+
+	JoltWritableBody3D body = space->write_body(jolt_id);
+	ERR_FAIL_COND(body.is_invalid());
+
+	JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*body->GetMotionPropertiesUnchecked());
+
+	motion_properties.SetLinearDamping(linear_damping);
+}
+
+void JoltSoftBody3D::_update_simulation_precision() {
+	if (!in_space()) {
+		jolt_settings->mNumIterations = (JPH::uint32)simulation_precision;
+		return;
+	}
+
+	JoltWritableBody3D body = space->write_body(jolt_id);
+	ERR_FAIL_COND(body.is_invalid());
+
+	JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*body->GetMotionPropertiesUnchecked());
+
+	motion_properties.SetNumIterations((JPH::uint32)simulation_precision);
+}
+
+void JoltSoftBody3D::_update_group_filter() {
+	JPH::GroupFilter *group_filter = !exceptions.is_empty() ? JoltGroupFilter::instance : nullptr;
+
+	if (!in_space()) {
+		jolt_settings->mCollisionGroup.SetGroupFilter(group_filter);
+		return;
+	}
+
+	const JoltWritableBody3D body = space->write_body(jolt_id);
+	ERR_FAIL_COND(body.is_invalid());
+
+	body->GetCollisionGroup().SetGroupFilter(group_filter);
+}
+
+void JoltSoftBody3D::_try_rebuild() {
+	if (space != nullptr) {
+		_reset_space();
+	}
+}
+
+void JoltSoftBody3D::_mesh_changed() {
+	_try_rebuild();
+}
+
+void JoltSoftBody3D::_simulation_precision_changed() {
+	wake_up();
+}
+
+void JoltSoftBody3D::_mass_changed() {
+	wake_up();
+}
+
+void JoltSoftBody3D::_pressure_changed() {
+	_update_pressure();
+	wake_up();
+}
+
+void JoltSoftBody3D::_damping_changed() {
+	_update_damping();
+	wake_up();
+}
+
+void JoltSoftBody3D::_pins_changed() {
+	_update_mass();
+	wake_up();
+}
+
+void JoltSoftBody3D::_vertices_changed() {
+	wake_up();
+}
+
+void JoltSoftBody3D::_exceptions_changed() {
+	_update_group_filter();
+}
+
+JoltSoftBody3D::JoltSoftBody3D() :
+		JoltObject3D(OBJECT_TYPE_SOFT_BODY) {
+	jolt_settings->mRestitution = 0.0f;
+	jolt_settings->mFriction = 1.0f;
+	jolt_settings->mUpdatePosition = false;
+	jolt_settings->mMakeRotationIdentity = false;
+}
+
+JoltSoftBody3D::~JoltSoftBody3D() {
+	if (jolt_settings != nullptr) {
+		delete jolt_settings;
+		jolt_settings = nullptr;
+	}
+}
+
+bool JoltSoftBody3D::in_space() const {
+	return JoltObject3D::in_space() && shared != nullptr;
+}
+
+void JoltSoftBody3D::add_collision_exception(const RID &p_excepted_body) {
+	exceptions.push_back(p_excepted_body);
+
+	_exceptions_changed();
+}
+
+void JoltSoftBody3D::remove_collision_exception(const RID &p_excepted_body) {
+	exceptions.erase(p_excepted_body);
+
+	_exceptions_changed();
+}
+
+bool JoltSoftBody3D::has_collision_exception(const RID &p_excepted_body) const {
+	return exceptions.find(p_excepted_body) >= 0;
+}
+
+bool JoltSoftBody3D::can_interact_with(const JoltBody3D &p_other) const {
+	return (can_collide_with(p_other) || p_other.can_collide_with(*this)) && !has_collision_exception(p_other.get_rid()) && !p_other.has_collision_exception(rid);
+}
+
+bool JoltSoftBody3D::can_interact_with(const JoltSoftBody3D &p_other) const {
+	return (can_collide_with(p_other) || p_other.can_collide_with(*this)) && !has_collision_exception(p_other.get_rid()) && !p_other.has_collision_exception(rid);
+}
+
+bool JoltSoftBody3D::can_interact_with(const JoltArea3D &p_other) const {
+	return p_other.can_interact_with(*this);
+}
+
+Vector3 JoltSoftBody3D::get_velocity_at_position(const Vector3 &p_position) const {
+	return Vector3();
+}
+
+void JoltSoftBody3D::set_mesh(const RID &p_mesh) {
+	if (unlikely(mesh == p_mesh)) {
+		return;
+	}
+
+	_deref_shared_data();
+
+	mesh = p_mesh;
+
+	_mesh_changed();
+}
+
+bool JoltSoftBody3D::is_sleeping() const {
+	if (!in_space()) {
+		return false;
+	}
+
+	const JoltReadableBody3D body = space->read_body(jolt_id);
+	ERR_FAIL_COND_V(body.is_invalid(), false);
+
+	return !body->IsActive();
+}
+
+void JoltSoftBody3D::set_is_sleeping(bool p_enabled) {
+	if (!in_space()) {
+		return;
+	}
+
+	JPH::BodyInterface &body_iface = space->get_body_iface();
+
+	if (p_enabled) {
+		body_iface.DeactivateBody(jolt_id);
+	} else {
+		body_iface.ActivateBody(jolt_id);
+	}
+}
+
+bool JoltSoftBody3D::can_sleep() const {
+	if (!in_space()) {
+		return true;
+	}
+
+	const JoltReadableBody3D body = space->read_body(jolt_id);
+	ERR_FAIL_COND_V(body.is_invalid(), false);
+
+	return body->GetAllowSleeping();
+}
+
+void JoltSoftBody3D::set_can_sleep(bool p_enabled) {
+	if (!in_space()) {
+		return;
+	}
+
+	const JoltWritableBody3D body = space->write_body(jolt_id);
+	ERR_FAIL_COND(body.is_invalid());
+
+	body->SetAllowSleeping(p_enabled);
+}
+
+void JoltSoftBody3D::set_simulation_precision(int p_precision) {
+	if (unlikely(simulation_precision == p_precision)) {
+		return;
+	}
+
+	simulation_precision = MAX(p_precision, 0);
+
+	_simulation_precision_changed();
+}
+
+void JoltSoftBody3D::set_mass(float p_mass) {
+	if (unlikely(mass == p_mass)) {
+		return;
+	}
+
+	mass = MAX(p_mass, 0.0f);
+
+	_mass_changed();
+}
+
+float JoltSoftBody3D::get_stiffness_coefficient() const {
+	return stiffness_coefficient;
+}
+
+void JoltSoftBody3D::set_stiffness_coefficient(float p_coefficient) {
+	stiffness_coefficient = CLAMP(p_coefficient, 0.0f, 1.0f);
+}
+
+void JoltSoftBody3D::set_pressure(float p_pressure) {
+	if (unlikely(pressure == p_pressure)) {
+		return;
+	}
+
+	pressure = MAX(p_pressure, 0.0f);
+
+	_pressure_changed();
+}
+
+void JoltSoftBody3D::set_linear_damping(float p_damping) {
+	if (unlikely(linear_damping == p_damping)) {
+		return;
+	}
+
+	linear_damping = MAX(p_damping, 0.0f);
+
+	_damping_changed();
+}
+
+float JoltSoftBody3D::get_drag() const {
+	// Drag is not a thing in Jolt, and not supported by Godot Physics either.
+	return 0.0f;
+}
+
+void JoltSoftBody3D::set_drag(float p_drag) {
+	// Drag is not a thing in Jolt, and not supported by Godot Physics either.
+}
+
+Variant JoltSoftBody3D::get_state(PhysicsServer3D::BodyState p_state) const {
+	switch (p_state) {
+		case PhysicsServer3D::BODY_STATE_TRANSFORM: {
+			return get_transform();
+		}
+		case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: {
+			ERR_FAIL_V_MSG(Variant(), "Linear velocity is not supported for soft bodies.");
+		}
+		case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: {
+			ERR_FAIL_V_MSG(Variant(), "Angular velocity is not supported for soft bodies.");
+		}
+		case PhysicsServer3D::BODY_STATE_SLEEPING: {
+			return is_sleeping();
+		}
+		case PhysicsServer3D::BODY_STATE_CAN_SLEEP: {
+			return can_sleep();
+		}
+		default: {
+			ERR_FAIL_V_MSG(Variant(), vformat("Unhandled body state: '%d'. This should not happen. Please report this.", p_state));
+		}
+	}
+}
+
+void JoltSoftBody3D::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_value) {
+	switch (p_state) {
+		case PhysicsServer3D::BODY_STATE_TRANSFORM: {
+			set_transform(p_value);
+		} break;
+		case PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY: {
+			ERR_FAIL_MSG("Linear velocity is not supported for soft bodies.");
+		} break;
+		case PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY: {
+			ERR_FAIL_MSG("Angular velocity is not supported for soft bodies.");
+		} break;
+		case PhysicsServer3D::BODY_STATE_SLEEPING: {
+			set_is_sleeping(p_value);
+		} break;
+		case PhysicsServer3D::BODY_STATE_CAN_SLEEP: {
+			set_can_sleep(p_value);
+		} break;
+		default: {
+			ERR_FAIL_MSG(vformat("Unhandled body state: '%d'. This should not happen. Please report this.", p_state));
+		} break;
+	}
+}
+
+Transform3D JoltSoftBody3D::get_transform() const {
+	// Since any transform gets baked into the vertices anyway we can just return identity here.
+	return Transform3D();
+}
+
+void JoltSoftBody3D::set_transform(const Transform3D &p_transform) {
+	ERR_FAIL_COND_MSG(!in_space(), vformat("Failed to set transform for '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
+
+	JoltWritableBody3D body = space->write_body(jolt_id);
+	ERR_FAIL_COND(body.is_invalid());
+
+	// For whatever reason this has to be interpreted as a relative global-space transform rather than an absolute one,
+	// because `SoftBody3D` will immediately upon entering the scene tree set itself to be top-level and also set its
+	// transform to be identity, while still expecting to stay in its original position.
+	//
+	// We also discard any scaling, since we have no way of scaling the actual edge lengths.
+	const JPH::Mat44 relative_transform = to_jolt(p_transform.orthonormalized());
+
+	JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*body->GetMotionPropertiesUnchecked());
+	JPH::Array<JPH::SoftBodyVertex> &physics_vertices = motion_properties.GetVertices();
+
+	for (JPH::SoftBodyVertex &vertex : physics_vertices) {
+		vertex.mPosition = vertex.mPreviousPosition = relative_transform * vertex.mPosition;
+		vertex.mVelocity = JPH::Vec3::sZero();
+	}
+}
+
+AABB JoltSoftBody3D::get_bounds() const {
+	ERR_FAIL_COND_V_MSG(!in_space(), AABB(), vformat("Failed to retrieve world bounds of '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
+
+	const JoltReadableBody3D body = space->read_body(jolt_id);
+	ERR_FAIL_COND_V(body.is_invalid(), AABB());
+
+	return to_godot(body->GetWorldSpaceBounds());
+}
+
+void JoltSoftBody3D::update_rendering_server(PhysicsServer3DRenderingServerHandler *p_rendering_server_handler) {
+	// Ideally we would emit an actual error here, but that would spam the logs to the point where the actual cause will be drowned out.
+	if (unlikely(!in_space())) {
+		return;
+	}
+
+	const JoltReadableBody3D body = space->read_body(jolt_id);
+	ERR_FAIL_COND(body.is_invalid());
+
+	const JPH::SoftBodyMotionProperties &motion_properties = static_cast<const JPH::SoftBodyMotionProperties &>(*body->GetMotionPropertiesUnchecked());
+
+	typedef JPH::SoftBodyMotionProperties::Vertex SoftBodyVertex;
+	typedef JPH::SoftBodyMotionProperties::Face SoftBodyFace;
+
+	const JPH::Array<SoftBodyVertex> &physics_vertices = motion_properties.GetVertices();
+	const JPH::Array<SoftBodyFace> &physics_faces = motion_properties.GetFaces();
+
+	const int physics_vertex_count = (int)physics_vertices.size();
+
+	normals.resize(physics_vertex_count);
+
+	for (const SoftBodyFace &physics_face : physics_faces) {
+		// Jolt uses a different winding order, so we swap the indices to account for that.
+
+		const uint32_t i0 = physics_face.mVertex[2];
+		const uint32_t i1 = physics_face.mVertex[1];
+		const uint32_t i2 = physics_face.mVertex[0];
+
+		const Vector3 v0 = to_godot(physics_vertices[i0].mPosition);
+		const Vector3 v1 = to_godot(physics_vertices[i1].mPosition);
+		const Vector3 v2 = to_godot(physics_vertices[i2].mPosition);
+
+		const Vector3 normal = (v2 - v0).cross(v1 - v0).normalized();
+
+		normals[i0] = normal;
+		normals[i1] = normal;
+		normals[i2] = normal;
+	}
+
+	const int mesh_vertex_count = shared->mesh_to_physics.size();
+
+	for (int i = 0; i < mesh_vertex_count; ++i) {
+		const int physics_index = shared->mesh_to_physics[i];
+
+		const Vector3 vertex = to_godot(physics_vertices[(size_t)physics_index].mPosition);
+		const Vector3 normal = normals[(uint32_t)physics_index];
+
+		p_rendering_server_handler->set_vertex(i, vertex);
+		p_rendering_server_handler->set_normal(i, normal);
+	}
+
+	p_rendering_server_handler->set_aabb(get_bounds());
+}
+
+Vector3 JoltSoftBody3D::get_vertex_position(int p_index) {
+	ERR_FAIL_COND_V_MSG(!in_space(), Vector3(), vformat("Failed to retrieve point position for '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
+
+	ERR_FAIL_NULL_V(shared, Vector3());
+	ERR_FAIL_INDEX_V(p_index, (int)shared->mesh_to_physics.size(), Vector3());
+	const size_t physics_index = (size_t)shared->mesh_to_physics[p_index];
+
+	const JoltReadableBody3D body = space->read_body(jolt_id);
+	ERR_FAIL_COND_V(body.is_invalid(), Vector3());
+
+	const JPH::SoftBodyMotionProperties &motion_properties = static_cast<const JPH::SoftBodyMotionProperties &>(*body->GetMotionPropertiesUnchecked());
+	const JPH::Array<JPH::SoftBodyVertex> &physics_vertices = motion_properties.GetVertices();
+	const JPH::SoftBodyVertex &physics_vertex = physics_vertices[physics_index];
+
+	return to_godot(body->GetCenterOfMassPosition() + physics_vertex.mPosition);
+}
+
+void JoltSoftBody3D::set_vertex_position(int p_index, const Vector3 &p_position) {
+	ERR_FAIL_COND_MSG(!in_space(), vformat("Failed to set point position for '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
+
+	ERR_FAIL_NULL(shared);
+	ERR_FAIL_INDEX(p_index, (int)shared->mesh_to_physics.size());
+	const size_t physics_index = (size_t)shared->mesh_to_physics[p_index];
+
+	const float last_step = space->get_last_step();
+	if (unlikely(last_step == 0.0f)) {
+		return;
+	}
+
+	JoltWritableBody3D body = space->write_body(jolt_id);
+	ERR_FAIL_COND(body.is_invalid());
+
+	JPH::SoftBodyMotionProperties &motion_properties = static_cast<JPH::SoftBodyMotionProperties &>(*body->GetMotionPropertiesUnchecked());
+
+	JPH::Array<JPH::SoftBodyVertex> &physics_vertices = motion_properties.GetVertices();
+	JPH::SoftBodyVertex &physics_vertex = physics_vertices[physics_index];
+
+	const JPH::RVec3 center_of_mass = body->GetCenterOfMassPosition();
+	const JPH::Vec3 local_position = JPH::Vec3(to_jolt_r(p_position) - center_of_mass);
+	const JPH::Vec3 displacement = local_position - physics_vertex.mPosition;
+	const JPH::Vec3 velocity = displacement / last_step;
+
+	physics_vertex.mVelocity = velocity;
+
+	_vertices_changed();
+}
+
+void JoltSoftBody3D::pin_vertex(int p_index) {
+	pinned_vertices.insert(p_index);
+
+	_pins_changed();
+}
+
+void JoltSoftBody3D::unpin_vertex(int p_index) {
+	pinned_vertices.erase(p_index);
+
+	_pins_changed();
+}
+
+void JoltSoftBody3D::unpin_all_vertices() {
+	pinned_vertices.clear();
+
+	_pins_changed();
+}
+
+bool JoltSoftBody3D::is_vertex_pinned(int p_index) const {
+	ERR_FAIL_COND_V_MSG(!in_space(), false, vformat("Failed retrieve pin status of point for '%s'. Doing so without a physics space is not supported when using Jolt Physics. If this relates to a node, try adding the node to a scene tree first.", to_string()));
+
+	ERR_FAIL_NULL_V(shared, false);
+	ERR_FAIL_INDEX_V(p_index, (int)shared->mesh_to_physics.size(), false);
+	const int physics_index = shared->mesh_to_physics[p_index];
+
+	return pinned_vertices.has(physics_index);
+}
+
+String JoltSoftBody3D::to_string() const {
+	Object *instance = get_instance();
+	return instance != nullptr ? instance->to_string() : "<unknown>";
+}
diff --git a/modules/jolt_physics/objects/jolt_soft_body_3d.h b/modules/jolt_physics/objects/jolt_soft_body_3d.h
new file mode 100644
index 0000000000..45e8be2e7b
--- /dev/null
+++ b/modules/jolt_physics/objects/jolt_soft_body_3d.h
@@ -0,0 +1,174 @@
+/**************************************************************************/
+/*  jolt_soft_body_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_SOFT_BODY_3D_H
+#define JOLT_SOFT_BODY_3D_H
+
+#include "jolt_object_3d.h"
+
+#include "servers/physics_server_3d.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/SoftBody/SoftBodyCreationSettings.h"
+#include "Jolt/Physics/SoftBody/SoftBodySharedSettings.h"
+
+class JoltSpace3D;
+
+class JoltSoftBody3D final : public JoltObject3D {
+	struct Shared {
+		LocalVector<int> mesh_to_physics;
+		JPH::Ref<JPH::SoftBodySharedSettings> settings = new JPH::SoftBodySharedSettings();
+		int ref_count = 1;
+	};
+
+	inline static HashMap<RID, Shared> mesh_to_shared;
+
+	HashSet<int> pinned_vertices;
+	LocalVector<RID> exceptions;
+	LocalVector<Vector3> normals;
+
+	const Shared *shared = nullptr;
+
+	RID mesh;
+
+	JPH::SoftBodyCreationSettings *jolt_settings = new JPH::SoftBodyCreationSettings();
+
+	float mass = 0.0f;
+	float pressure = 0.0f;
+	float linear_damping = 0.01f;
+	float stiffness_coefficient = 0.5f;
+
+	int simulation_precision = 5;
+
+	virtual JPH::BroadPhaseLayer _get_broad_phase_layer() const override;
+	virtual JPH::ObjectLayer _get_object_layer() const override;
+
+	virtual void _space_changing() override;
+	virtual void _space_changed() override;
+
+	virtual void _add_to_space() override;
+
+	bool _ref_shared_data();
+	void _deref_shared_data();
+
+	void _update_mass();
+	void _update_pressure();
+	void _update_damping();
+	void _update_simulation_precision();
+	void _update_group_filter();
+
+	void _try_rebuild();
+
+	void _mesh_changed();
+	void _simulation_precision_changed();
+	void _mass_changed();
+	void _pressure_changed();
+	void _damping_changed();
+	void _pins_changed();
+	void _vertices_changed();
+	void _exceptions_changed();
+
+public:
+	JoltSoftBody3D();
+	virtual ~JoltSoftBody3D() override;
+
+	bool in_space() const;
+
+	void add_collision_exception(const RID &p_excepted_body);
+	void remove_collision_exception(const RID &p_excepted_body);
+	bool has_collision_exception(const RID &p_excepted_body) const;
+
+	const LocalVector<RID> &get_collision_exceptions() const { return exceptions; }
+
+	virtual bool can_interact_with(const JoltBody3D &p_other) const override;
+	virtual bool can_interact_with(const JoltSoftBody3D &p_other) const override;
+	virtual bool can_interact_with(const JoltArea3D &p_other) const override;
+
+	virtual bool reports_contacts() const override { return false; }
+
+	virtual Vector3 get_velocity_at_position(const Vector3 &p_position) const override;
+
+	void set_mesh(const RID &p_mesh);
+
+	bool is_pickable() const { return pickable; }
+	void set_pickable(bool p_enabled) { pickable = p_enabled; }
+
+	bool is_sleeping() const;
+	void set_is_sleeping(bool p_enabled);
+
+	bool can_sleep() const;
+	void set_can_sleep(bool p_enabled);
+
+	void put_to_sleep() { set_is_sleeping(true); }
+	void wake_up() { set_is_sleeping(false); }
+
+	int get_simulation_precision() const { return simulation_precision; }
+	void set_simulation_precision(int p_precision);
+
+	float get_mass() const { return mass; }
+	void set_mass(float p_mass);
+
+	float get_stiffness_coefficient() const;
+	void set_stiffness_coefficient(float p_coefficient);
+
+	float get_pressure() const { return pressure; }
+	void set_pressure(float p_pressure);
+
+	float get_linear_damping() const { return linear_damping; }
+	void set_linear_damping(float p_damping);
+
+	float get_drag() const;
+	void set_drag(float p_drag);
+
+	Variant get_state(PhysicsServer3D::BodyState p_state) const;
+	void set_state(PhysicsServer3D::BodyState p_state, const Variant &p_value);
+
+	Transform3D get_transform() const;
+	void set_transform(const Transform3D &p_transform);
+
+	AABB get_bounds() const;
+
+	void update_rendering_server(PhysicsServer3DRenderingServerHandler *p_rendering_server_handler);
+
+	Vector3 get_vertex_position(int p_index);
+	void set_vertex_position(int p_index, const Vector3 &p_position);
+
+	void pin_vertex(int p_index);
+	void unpin_vertex(int p_index);
+
+	void unpin_all_vertices();
+
+	bool is_vertex_pinned(int p_index) const;
+
+	String to_string() const;
+};
+
+#endif // JOLT_SOFT_BODY_3D_H
diff --git a/modules/jolt_physics/register_types.cpp b/modules/jolt_physics/register_types.cpp
new file mode 100644
index 0000000000..9a3fcca8b7
--- /dev/null
+++ b/modules/jolt_physics/register_types.cpp
@@ -0,0 +1,79 @@
+/**************************************************************************/
+/*  register_types.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 "register_types.h"
+
+#include "jolt_globals.h"
+#include "jolt_physics_server_3d.h"
+#include "jolt_project_settings.h"
+
+#include "servers/physics_server_3d_wrap_mt.h"
+
+PhysicsServer3D *create_jolt_physics_server() {
+#ifdef THREADS_ENABLED
+	bool run_on_separate_thread = GLOBAL_GET("physics/3d/run_on_separate_thread");
+#else
+	bool run_on_separate_thread = false;
+#endif
+
+	JoltPhysicsServer3D *physics_server = memnew(JoltPhysicsServer3D(run_on_separate_thread));
+
+	return memnew(PhysicsServer3DWrapMT(physics_server, run_on_separate_thread));
+}
+
+void initialize_jolt_physics_module(ModuleInitializationLevel p_level) {
+	switch (p_level) {
+		case MODULE_INITIALIZATION_LEVEL_CORE: {
+		} break;
+		case MODULE_INITIALIZATION_LEVEL_SERVERS: {
+			jolt_initialize();
+			PhysicsServer3DManager::get_singleton()->register_server("Jolt Physics", callable_mp_static(&create_jolt_physics_server));
+			JoltProjectSettings::register_settings();
+		} break;
+		case MODULE_INITIALIZATION_LEVEL_SCENE: {
+		} break;
+		case MODULE_INITIALIZATION_LEVEL_EDITOR: {
+		} break;
+	}
+}
+
+void uninitialize_jolt_physics_module(ModuleInitializationLevel p_level) {
+	switch (p_level) {
+		case MODULE_INITIALIZATION_LEVEL_CORE: {
+		} break;
+		case MODULE_INITIALIZATION_LEVEL_SERVERS: {
+			jolt_deinitialize();
+		} break;
+		case MODULE_INITIALIZATION_LEVEL_SCENE: {
+		} break;
+		case MODULE_INITIALIZATION_LEVEL_EDITOR: {
+		} break;
+	}
+}
diff --git a/modules/jolt_physics/register_types.h b/modules/jolt_physics/register_types.h
new file mode 100644
index 0000000000..1be106d02a
--- /dev/null
+++ b/modules/jolt_physics/register_types.h
@@ -0,0 +1,39 @@
+/**************************************************************************/
+/*  register_types.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_REGISTER_TYPES_H
+#define JOLT_PHYSICS_REGISTER_TYPES_H
+
+#include "modules/register_module_types.h"
+
+void initialize_jolt_physics_module(ModuleInitializationLevel p_level);
+void uninitialize_jolt_physics_module(ModuleInitializationLevel p_level);
+
+#endif // JOLT_PHYSICS_REGISTER_TYPES_H
diff --git a/modules/jolt_physics/shapes/jolt_box_shape_3d.cpp b/modules/jolt_physics/shapes/jolt_box_shape_3d.cpp
new file mode 100644
index 0000000000..98c7177dca
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_box_shape_3d.cpp
@@ -0,0 +1,83 @@
+/**************************************************************************/
+/*  jolt_box_shape_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_box_shape_3d.h"
+
+#include "../jolt_project_settings.h"
+#include "../misc/jolt_type_conversions.h"
+
+#include "Jolt/Physics/Collision/Shape/BoxShape.h"
+
+JPH::ShapeRefC JoltBoxShape3D::_build() const {
+	const float min_half_extent = (float)half_extents[half_extents.min_axis_index()];
+	const float actual_margin = MIN(margin, min_half_extent * JoltProjectSettings::get_collision_margin_fraction());
+
+	const JPH::BoxShapeSettings shape_settings(to_jolt(half_extents), actual_margin);
+	const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create();
+
+	ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to build Jolt Physics box shape with %s. It returned the following error: '%s'. This shape belongs to %s.", to_string(), to_godot(shape_result.GetError()), _owners_to_string()));
+
+	return shape_result.Get();
+}
+
+Variant JoltBoxShape3D::get_data() const {
+	return half_extents;
+}
+
+void JoltBoxShape3D::set_data(const Variant &p_data) {
+	ERR_FAIL_COND(p_data.get_type() != Variant::VECTOR3);
+
+	const Vector3 new_half_extents = p_data;
+	if (unlikely(new_half_extents == half_extents)) {
+		return;
+	}
+
+	half_extents = new_half_extents;
+
+	destroy();
+}
+
+void JoltBoxShape3D::set_margin(float p_margin) {
+	if (unlikely(margin == p_margin)) {
+		return;
+	}
+
+	margin = p_margin;
+
+	destroy();
+}
+
+String JoltBoxShape3D::to_string() const {
+	return vformat("{half_extents=%v margin=%f}", half_extents, margin);
+}
+
+AABB JoltBoxShape3D::get_aabb() const {
+	return AABB(-half_extents, half_extents * 2.0f);
+}
diff --git a/modules/jolt_physics/shapes/jolt_box_shape_3d.h b/modules/jolt_physics/shapes/jolt_box_shape_3d.h
new file mode 100644
index 0000000000..896caffefd
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_box_shape_3d.h
@@ -0,0 +1,57 @@
+/**************************************************************************/
+/*  jolt_box_shape_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_BOX_SHAPE_3D_H
+#define JOLT_BOX_SHAPE_3D_H
+
+#include "jolt_shape_3d.h"
+
+class JoltBoxShape3D final : public JoltShape3D {
+	Vector3 half_extents;
+	float margin = 0.04f;
+
+	virtual JPH::ShapeRefC _build() const override;
+
+public:
+	virtual ShapeType get_type() const override { return ShapeType::SHAPE_BOX; }
+	virtual bool is_convex() const override { return true; }
+
+	virtual Variant get_data() const override;
+	virtual void set_data(const Variant &p_data) override;
+
+	virtual float get_margin() const override { return margin; }
+	virtual void set_margin(float p_margin) override;
+
+	virtual AABB get_aabb() const override;
+
+	String to_string() const;
+};
+
+#endif // JOLT_BOX_SHAPE_3D_H
diff --git a/modules/jolt_physics/shapes/jolt_capsule_shape_3d.cpp b/modules/jolt_physics/shapes/jolt_capsule_shape_3d.cpp
new file mode 100644
index 0000000000..a38cb6bdbe
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_capsule_shape_3d.cpp
@@ -0,0 +1,90 @@
+/**************************************************************************/
+/*  jolt_capsule_shape_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_capsule_shape_3d.h"
+
+#include "../misc/jolt_type_conversions.h"
+
+#include "Jolt/Physics/Collision/Shape/CapsuleShape.h"
+
+JPH::ShapeRefC JoltCapsuleShape3D::_build() const {
+	ERR_FAIL_COND_V_MSG(radius <= 0.0f, nullptr, vformat("Failed to build Jolt Physics capsule shape with %s. Its radius must be greater than 0. This shape belongs to %s.", to_string(), _owners_to_string()));
+	ERR_FAIL_COND_V_MSG(height <= 0.0f, nullptr, vformat("Failed to build Jolt Physics capsule shape with %s. Its height must be greater than 0. This shape belongs to %s.", to_string(), _owners_to_string()));
+	ERR_FAIL_COND_V_MSG(height < radius * 2.0f, nullptr, vformat("Failed to build Jolt Physics capsule shape with %s. Its height must be at least double that of its radius. This shape belongs to %s.", to_string(), _owners_to_string()));
+
+	const float half_height = height / 2.0f;
+	const float cylinder_height = half_height - radius;
+
+	const JPH::CapsuleShapeSettings shape_settings(cylinder_height, radius);
+	const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create();
+	ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to build Jolt Physics capsule shape with %s. It returned the following error: '%s'. This shape belongs to %s.", to_string(), to_godot(shape_result.GetError()), _owners_to_string()));
+
+	return shape_result.Get();
+}
+
+Variant JoltCapsuleShape3D::get_data() const {
+	Dictionary data;
+	data["height"] = height;
+	data["radius"] = radius;
+	return data;
+}
+
+void JoltCapsuleShape3D::set_data(const Variant &p_data) {
+	ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY);
+
+	const Dictionary data = p_data;
+
+	const Variant maybe_height = data.get("height", Variant());
+	ERR_FAIL_COND(maybe_height.get_type() != Variant::FLOAT);
+
+	const Variant maybe_radius = data.get("radius", Variant());
+	ERR_FAIL_COND(maybe_radius.get_type() != Variant::FLOAT);
+
+	const float new_height = maybe_height;
+	const float new_radius = maybe_radius;
+
+	if (unlikely(new_height == height && new_radius == radius)) {
+		return;
+	}
+
+	height = new_height;
+	radius = new_radius;
+
+	destroy();
+}
+
+AABB JoltCapsuleShape3D::get_aabb() const {
+	const Vector3 half_extents(radius, height / 2.0f, radius);
+	return AABB(-half_extents, half_extents * 2.0f);
+}
+
+String JoltCapsuleShape3D::to_string() const {
+	return vformat("{height=%f radius=%f}", height, radius);
+}
diff --git a/modules/jolt_physics/shapes/jolt_capsule_shape_3d.h b/modules/jolt_physics/shapes/jolt_capsule_shape_3d.h
new file mode 100644
index 0000000000..8e3f932e91
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_capsule_shape_3d.h
@@ -0,0 +1,57 @@
+/**************************************************************************/
+/*  jolt_capsule_shape_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_CAPSULE_SHAPE_3D_H
+#define JOLT_CAPSULE_SHAPE_3D_H
+
+#include "jolt_shape_3d.h"
+
+class JoltCapsuleShape3D final : public JoltShape3D {
+	float height = 0.0f;
+	float radius = 0.0f;
+
+	virtual JPH::ShapeRefC _build() const override;
+
+public:
+	virtual ShapeType get_type() const override { return ShapeType::SHAPE_CAPSULE; }
+	virtual bool is_convex() const override { return true; }
+
+	virtual Variant get_data() const override;
+	virtual void set_data(const Variant &p_data) override;
+
+	virtual float get_margin() const override { return 0.0f; }
+	virtual void set_margin(float p_margin) override {}
+
+	virtual AABB get_aabb() const override;
+
+	String to_string() const;
+};
+
+#endif // JOLT_CAPSULE_SHAPE_3D_H
diff --git a/modules/jolt_physics/shapes/jolt_concave_polygon_shape_3d.cpp b/modules/jolt_physics/shapes/jolt_concave_polygon_shape_3d.cpp
new file mode 100644
index 0000000000..e245738a8f
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_concave_polygon_shape_3d.cpp
@@ -0,0 +1,125 @@
+/**************************************************************************/
+/*  jolt_concave_polygon_shape_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_concave_polygon_shape_3d.h"
+
+#include "../jolt_project_settings.h"
+#include "../misc/jolt_type_conversions.h"
+
+#include "Jolt/Physics/Collision/Shape/MeshShape.h"
+
+JPH::ShapeRefC JoltConcavePolygonShape3D::_build() const {
+	const int vertex_count = (int)faces.size();
+	const int face_count = vertex_count / 3;
+	const int excess_vertex_count = vertex_count % 3;
+
+	if (unlikely(vertex_count == 0)) {
+		return nullptr;
+	}
+
+	ERR_FAIL_COND_V_MSG(vertex_count < 3, nullptr, vformat("Failed to build Jolt Physics concave polygon shape with %s. It must have a vertex count of at least 3. This shape belongs to %s.", to_string(), _owners_to_string()));
+	ERR_FAIL_COND_V_MSG(excess_vertex_count != 0, nullptr, vformat("Failed to build Jolt Physics concave polygon shape with %s. It must have a vertex count that is divisible by 3. This shape belongs to %s.", to_string(), _owners_to_string()));
+
+	JPH::TriangleList jolt_faces;
+	jolt_faces.reserve((size_t)face_count);
+
+	const Vector3 *faces_begin = &faces[0];
+	const Vector3 *faces_end = faces_begin + vertex_count;
+	JPH::uint32 triangle_index = 0;
+
+	for (const Vector3 *vertex = faces_begin; vertex != faces_end; vertex += 3) {
+		const Vector3 *v0 = vertex + 0;
+		const Vector3 *v1 = vertex + 1;
+		const Vector3 *v2 = vertex + 2;
+
+		// Jolt uses a different winding order, so we swizzle the vertices to account for that.
+		jolt_faces.emplace_back(
+				JPH::Float3((float)v2->x, (float)v2->y, (float)v2->z),
+				JPH::Float3((float)v1->x, (float)v1->y, (float)v1->z),
+				JPH::Float3((float)v0->x, (float)v0->y, (float)v0->z),
+				0,
+				triangle_index++);
+	}
+
+	JPH::MeshShapeSettings shape_settings(jolt_faces);
+	shape_settings.mActiveEdgeCosThresholdAngle = JoltProjectSettings::get_active_edge_threshold();
+	shape_settings.mPerTriangleUserData = JoltProjectSettings::enable_ray_cast_face_index();
+
+	const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create();
+	ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to build Jolt Physics concave polygon shape with %s. It returned the following error: '%s'. This shape belongs to %s.", to_string(), to_godot(shape_result.GetError()), _owners_to_string()));
+
+	return JoltShape3D::with_double_sided(shape_result.Get(), back_face_collision);
+}
+
+AABB JoltConcavePolygonShape3D::_calculate_aabb() const {
+	AABB result;
+
+	for (int i = 0; i < faces.size(); ++i) {
+		const Vector3 &vertex = faces[i];
+
+		if (i == 0) {
+			result.position = vertex;
+		} else {
+			result.expand_to(vertex);
+		}
+	}
+
+	return result;
+}
+
+Variant JoltConcavePolygonShape3D::get_data() const {
+	Dictionary data;
+	data["faces"] = faces;
+	data["backface_collision"] = back_face_collision;
+	return data;
+}
+
+void JoltConcavePolygonShape3D::set_data(const Variant &p_data) {
+	ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY);
+
+	const Dictionary data = p_data;
+
+	const Variant maybe_faces = data.get("faces", Variant());
+	ERR_FAIL_COND(maybe_faces.get_type() != Variant::PACKED_VECTOR3_ARRAY);
+
+	const Variant maybe_back_face_collision = data.get("backface_collision", Variant());
+	ERR_FAIL_COND(maybe_back_face_collision.get_type() != Variant::BOOL);
+
+	faces = maybe_faces;
+	back_face_collision = maybe_back_face_collision;
+
+	aabb = _calculate_aabb();
+
+	destroy();
+}
+
+String JoltConcavePolygonShape3D::to_string() const {
+	return vformat("{vertex_count=%d}", faces.size());
+}
diff --git a/modules/jolt_physics/shapes/jolt_concave_polygon_shape_3d.h b/modules/jolt_physics/shapes/jolt_concave_polygon_shape_3d.h
new file mode 100644
index 0000000000..d865c86aa7
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_concave_polygon_shape_3d.h
@@ -0,0 +1,60 @@
+/**************************************************************************/
+/*  jolt_concave_polygon_shape_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_CONCAVE_POLYGON_SHAPE_3D_H
+#define JOLT_CONCAVE_POLYGON_SHAPE_3D_H
+
+#include "jolt_shape_3d.h"
+
+class JoltConcavePolygonShape3D final : public JoltShape3D {
+	AABB aabb;
+	PackedVector3Array faces;
+	bool back_face_collision = false;
+
+	virtual JPH::ShapeRefC _build() const override;
+
+	AABB _calculate_aabb() const;
+
+public:
+	virtual ShapeType get_type() const override { return ShapeType::SHAPE_CONCAVE_POLYGON; }
+	virtual bool is_convex() const override { return false; }
+
+	virtual Variant get_data() const override;
+	virtual void set_data(const Variant &p_data) override;
+
+	virtual float get_margin() const override { return 0.0f; }
+	virtual void set_margin(float p_margin) override {}
+
+	virtual AABB get_aabb() const override { return aabb; }
+
+	String to_string() const;
+};
+
+#endif // JOLT_CONCAVE_POLYGON_SHAPE_3D_H
diff --git a/modules/jolt_physics/shapes/jolt_convex_polygon_shape_3d.cpp b/modules/jolt_physics/shapes/jolt_convex_polygon_shape_3d.cpp
new file mode 100644
index 0000000000..78608c6213
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_convex_polygon_shape_3d.cpp
@@ -0,0 +1,107 @@
+/**************************************************************************/
+/*  jolt_convex_polygon_shape_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_convex_polygon_shape_3d.h"
+
+#include "../jolt_project_settings.h"
+#include "../misc/jolt_type_conversions.h"
+
+#include "Jolt/Physics/Collision/Shape/ConvexHullShape.h"
+
+JPH::ShapeRefC JoltConvexPolygonShape3D::_build() const {
+	const int vertex_count = (int)vertices.size();
+
+	if (unlikely(vertex_count == 0)) {
+		return nullptr;
+	}
+
+	ERR_FAIL_COND_V_MSG(vertex_count < 3, nullptr, vformat("Failed to build Jolt Physics convex polygon shape with %s. It must have a vertex count of at least 3. This shape belongs to %s.", to_string(), _owners_to_string()));
+
+	JPH::Array<JPH::Vec3> jolt_vertices;
+	jolt_vertices.reserve((size_t)vertex_count);
+
+	const Vector3 *vertices_begin = &vertices[0];
+	const Vector3 *vertices_end = vertices_begin + vertex_count;
+
+	for (const Vector3 *vertex = vertices_begin; vertex != vertices_end; ++vertex) {
+		jolt_vertices.emplace_back((float)vertex->x, (float)vertex->y, (float)vertex->z);
+	}
+
+	const float min_half_extent = _calculate_aabb().get_shortest_axis_size() * 0.5f;
+	const float actual_margin = MIN(margin, min_half_extent * JoltProjectSettings::get_collision_margin_fraction());
+
+	const JPH::ConvexHullShapeSettings shape_settings(jolt_vertices, actual_margin);
+	const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create();
+	ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to build Jolt Physics convex polygon shape with %s. It returned the following error: '%s'. This shape belongs to %s.", to_string(), to_godot(shape_result.GetError()), _owners_to_string()));
+
+	return shape_result.Get();
+}
+
+AABB JoltConvexPolygonShape3D::_calculate_aabb() const {
+	AABB result;
+
+	for (int i = 0; i < vertices.size(); ++i) {
+		if (i == 0) {
+			result.position = vertices[i];
+		} else {
+			result.expand_to(vertices[i]);
+		}
+	}
+
+	return result;
+}
+
+Variant JoltConvexPolygonShape3D::get_data() const {
+	return vertices;
+}
+
+void JoltConvexPolygonShape3D::set_data(const Variant &p_data) {
+	ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR3_ARRAY);
+
+	vertices = p_data;
+
+	aabb = _calculate_aabb();
+
+	destroy();
+}
+
+void JoltConvexPolygonShape3D::set_margin(float p_margin) {
+	if (unlikely(margin == p_margin)) {
+		return;
+	}
+
+	margin = p_margin;
+
+	destroy();
+}
+
+String JoltConvexPolygonShape3D::to_string() const {
+	return vformat("{vertex_count=%d margin=%f}", vertices.size(), margin);
+}
diff --git a/modules/jolt_physics/shapes/jolt_convex_polygon_shape_3d.h b/modules/jolt_physics/shapes/jolt_convex_polygon_shape_3d.h
new file mode 100644
index 0000000000..f623ce7599
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_convex_polygon_shape_3d.h
@@ -0,0 +1,60 @@
+/**************************************************************************/
+/*  jolt_convex_polygon_shape_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_CONVEX_POLYGON_SHAPE_3D_H
+#define JOLT_CONVEX_POLYGON_SHAPE_3D_H
+
+#include "jolt_shape_3d.h"
+
+class JoltConvexPolygonShape3D final : public JoltShape3D {
+	AABB aabb;
+	PackedVector3Array vertices;
+	float margin = 0.04f;
+
+	virtual JPH::ShapeRefC _build() const override;
+
+	AABB _calculate_aabb() const;
+
+public:
+	virtual ShapeType get_type() const override { return ShapeType::SHAPE_CONVEX_POLYGON; }
+	virtual bool is_convex() const override { return true; }
+
+	virtual Variant get_data() const override;
+	virtual void set_data(const Variant &p_data) override;
+
+	virtual float get_margin() const override { return margin; }
+	virtual void set_margin(float p_margin) override;
+
+	virtual AABB get_aabb() const override { return aabb; }
+
+	String to_string() const;
+};
+
+#endif // JOLT_CONVEX_POLYGON_SHAPE_3D_H
diff --git a/modules/jolt_physics/shapes/jolt_custom_decorated_shape.h b/modules/jolt_physics/shapes/jolt_custom_decorated_shape.h
new file mode 100644
index 0000000000..bbdb7b9025
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_custom_decorated_shape.h
@@ -0,0 +1,95 @@
+/**************************************************************************/
+/*  jolt_custom_decorated_shape.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_CUSTOM_DECORATED_SHAPE_H
+#define JOLT_CUSTOM_DECORATED_SHAPE_H
+
+#include "jolt_custom_shape_type.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Collision/Shape/DecoratedShape.h"
+#include "Jolt/Physics/Collision/TransformedShape.h"
+
+class JoltCustomDecoratedShapeSettings : public JPH::DecoratedShapeSettings {
+public:
+	using JPH::DecoratedShapeSettings::DecoratedShapeSettings;
+};
+
+class JoltCustomDecoratedShape : public JPH::DecoratedShape {
+public:
+	using JPH::DecoratedShape::DecoratedShape;
+
+	virtual JPH::AABox GetLocalBounds() const override { return mInnerShape->GetLocalBounds(); }
+
+	virtual JPH::AABox GetWorldSpaceBounds(JPH::Mat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale) const override { return mInnerShape->GetWorldSpaceBounds(p_center_of_mass_transform, p_scale); }
+
+	virtual float GetInnerRadius() const override { return mInnerShape->GetInnerRadius(); }
+
+	virtual JPH::MassProperties GetMassProperties() const override { return mInnerShape->GetMassProperties(); }
+
+	virtual JPH::Vec3 GetSurfaceNormal(const JPH::SubShapeID &p_sub_shape_id, JPH::Vec3Arg p_local_surface_position) const override { return mInnerShape->GetSurfaceNormal(p_sub_shape_id, p_local_surface_position); }
+
+	virtual JPH::uint64 GetSubShapeUserData(const JPH::SubShapeID &p_sub_shape_id) const override { return mInnerShape->GetSubShapeUserData(p_sub_shape_id); }
+
+	virtual JPH::TransformedShape GetSubShapeTransformedShape(const JPH::SubShapeID &p_sub_shape_id, JPH::Vec3Arg p_position_com, JPH::QuatArg p_rotation, JPH::Vec3Arg p_scale, JPH::SubShapeID &p_remainder) const override { return mInnerShape->GetSubShapeTransformedShape(p_sub_shape_id, p_position_com, p_rotation, p_scale, p_remainder); }
+
+	virtual void GetSubmergedVolume(JPH::Mat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale, const JPH::Plane &p_surface, float &p_total_volume, float &p_submerged_volume, JPH::Vec3 &p_center_of_buoyancy JPH_IF_DEBUG_RENDERER(, JPH::RVec3Arg p_base_offset)) const override { mInnerShape->GetSubmergedVolume(p_center_of_mass_transform, p_scale, p_surface, p_total_volume, p_submerged_volume, p_center_of_buoyancy JPH_IF_DEBUG_RENDERER(, p_base_offset)); }
+
+#ifdef JPH_DEBUG_RENDERER
+	virtual void Draw(JPH::DebugRenderer *p_renderer, JPH::RMat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale, JPH::ColorArg p_color, bool p_use_material_colors, bool p_draw_wireframe) const override { mInnerShape->Draw(p_renderer, p_center_of_mass_transform, p_scale, p_color, p_use_material_colors, p_draw_wireframe); }
+
+	virtual void DrawGetSupportFunction(JPH::DebugRenderer *p_renderer, JPH::RMat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale, JPH::ColorArg p_color, bool p_draw_support_direction) const override { mInnerShape->DrawGetSupportFunction(p_renderer, p_center_of_mass_transform, p_scale, p_color, p_draw_support_direction); }
+
+	virtual void DrawGetSupportingFace(JPH::DebugRenderer *p_renderer, JPH::RMat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale) const override { mInnerShape->DrawGetSupportingFace(p_renderer, p_center_of_mass_transform, p_scale); }
+#endif
+
+	virtual bool CastRay(const JPH::RayCast &p_ray, const JPH::SubShapeIDCreator &p_sub_shape_id_creator, JPH::RayCastResult &p_hit) const override { return mInnerShape->CastRay(p_ray, p_sub_shape_id_creator, p_hit); }
+
+	virtual void CastRay(const JPH::RayCast &p_ray, const JPH::RayCastSettings &p_ray_cast_settings, const JPH::SubShapeIDCreator &p_sub_shape_id_creator, JPH::CastRayCollector &p_collector, const JPH::ShapeFilter &p_shape_filter = JPH::ShapeFilter()) const override { return mInnerShape->CastRay(p_ray, p_ray_cast_settings, p_sub_shape_id_creator, p_collector, p_shape_filter); }
+
+	virtual void CollidePoint(JPH::Vec3Arg p_point, const JPH::SubShapeIDCreator &p_sub_shape_id_creator, JPH::CollidePointCollector &p_collector, const JPH::ShapeFilter &p_shape_filter = JPH::ShapeFilter()) const override { mInnerShape->CollidePoint(p_point, p_sub_shape_id_creator, p_collector, p_shape_filter); }
+
+	virtual void CollideSoftBodyVertices(JPH::Mat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale, const JPH::CollideSoftBodyVertexIterator &p_vertices, JPH::uint p_num_vertices, int p_colliding_shape_index) const override { mInnerShape->CollideSoftBodyVertices(p_center_of_mass_transform, p_scale, p_vertices, p_num_vertices, p_colliding_shape_index); }
+
+	virtual void CollectTransformedShapes(const JPH::AABox &p_box, JPH::Vec3Arg p_position_com, JPH::QuatArg p_rotation, JPH::Vec3Arg p_scale, const JPH::SubShapeIDCreator &p_sub_shape_id_creator, JPH::TransformedShapeCollector &p_collector, const JPH::ShapeFilter &p_shape_filter = JPH::ShapeFilter()) const override { mInnerShape->CollectTransformedShapes(p_box, p_position_com, p_rotation, p_scale, p_sub_shape_id_creator, p_collector, p_shape_filter); }
+
+	virtual void TransformShape(JPH::Mat44Arg p_center_of_mass_transform, JPH::TransformedShapeCollector &p_collector) const override { mInnerShape->TransformShape(p_center_of_mass_transform, p_collector); }
+
+	virtual void GetTrianglesStart(JPH::Shape::GetTrianglesContext &p_context, const JPH::AABox &p_box, JPH::Vec3Arg p_position_com, JPH::QuatArg p_rotation, JPH::Vec3Arg p_scale) const override { mInnerShape->GetTrianglesStart(p_context, p_box, p_position_com, p_rotation, p_scale); }
+
+	virtual int GetTrianglesNext(JPH::Shape::GetTrianglesContext &p_context, int p_max_triangles_requested, JPH::Float3 *p_triangle_vertices, const JPH::PhysicsMaterial **p_materials = nullptr) const override { return mInnerShape->GetTrianglesNext(p_context, p_max_triangles_requested, p_triangle_vertices, p_materials); }
+
+	virtual JPH::Shape::Stats GetStats() const override { return JPH::Shape::Stats(sizeof(*this), 0); }
+
+	virtual float GetVolume() const override { return mInnerShape->GetVolume(); }
+};
+
+#endif // JOLT_CUSTOM_DECORATED_SHAPE_H
diff --git a/modules/jolt_physics/shapes/jolt_custom_double_sided_shape.cpp b/modules/jolt_physics/shapes/jolt_custom_double_sided_shape.cpp
new file mode 100644
index 0000000000..5d9c06df4d
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_custom_double_sided_shape.cpp
@@ -0,0 +1,113 @@
+/**************************************************************************/
+/*  jolt_custom_double_sided_shape.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_custom_double_sided_shape.h"
+
+#include "../jolt_project_settings.h"
+
+#include "core/error/error_macros.h"
+
+#include "Jolt/Physics/Collision/CollisionDispatch.h"
+#include "Jolt/Physics/Collision/RayCast.h"
+
+namespace {
+
+JPH::Shape *construct_double_sided() {
+	return new JoltCustomDoubleSidedShape();
+}
+
+void collide_double_sided_vs_shape(const JPH::Shape *p_shape1, const JPH::Shape *p_shape2, JPH::Vec3Arg p_scale1, JPH::Vec3Arg p_scale2, JPH::Mat44Arg p_center_of_mass_transform1, JPH::Mat44Arg p_center_of_mass_transform2, const JPH::SubShapeIDCreator &p_sub_shape_id_creator1, const JPH::SubShapeIDCreator &p_sub_shape_id_creator2, const JPH::CollideShapeSettings &p_collide_shape_settings, JPH::CollideShapeCollector &p_collector, const JPH::ShapeFilter &p_shape_filter) {
+	ERR_FAIL_COND(p_shape1->GetSubType() != JoltCustomShapeSubType::DOUBLE_SIDED);
+
+	const JoltCustomDoubleSidedShape *shape1 = static_cast<const JoltCustomDoubleSidedShape *>(p_shape1);
+
+	JPH::CollideShapeSettings new_collide_shape_settings = p_collide_shape_settings;
+	new_collide_shape_settings.mBackFaceMode = JPH::EBackFaceMode::CollideWithBackFaces;
+
+	JPH::CollisionDispatch::sCollideShapeVsShape(shape1->GetInnerShape(), p_shape2, p_scale1, p_scale2, p_center_of_mass_transform1, p_center_of_mass_transform2, p_sub_shape_id_creator1, p_sub_shape_id_creator2, new_collide_shape_settings, p_collector, p_shape_filter);
+}
+
+void collide_shape_vs_double_sided(const JPH::Shape *p_shape1, const JPH::Shape *p_shape2, JPH::Vec3Arg p_scale1, JPH::Vec3Arg p_scale2, JPH::Mat44Arg p_center_of_mass_transform1, JPH::Mat44Arg p_center_of_mass_transform2, const JPH::SubShapeIDCreator &p_sub_shape_id_creator1, const JPH::SubShapeIDCreator &p_sub_shape_id_creator2, const JPH::CollideShapeSettings &p_collide_shape_settings, JPH::CollideShapeCollector &p_collector, const JPH::ShapeFilter &p_shape_filter) {
+	ERR_FAIL_COND(p_shape2->GetSubType() != JoltCustomShapeSubType::DOUBLE_SIDED);
+
+	const JoltCustomDoubleSidedShape *shape2 = static_cast<const JoltCustomDoubleSidedShape *>(p_shape2);
+
+	JPH::CollideShapeSettings new_collide_shape_settings = p_collide_shape_settings;
+	new_collide_shape_settings.mBackFaceMode = JPH::EBackFaceMode::CollideWithBackFaces;
+
+	JPH::CollisionDispatch::sCollideShapeVsShape(p_shape1, shape2->GetInnerShape(), p_scale1, p_scale2, p_center_of_mass_transform1, p_center_of_mass_transform2, p_sub_shape_id_creator1, p_sub_shape_id_creator2, new_collide_shape_settings, p_collector, p_shape_filter);
+}
+
+void cast_shape_vs_double_sided(const JPH::ShapeCast &p_shape_cast, const JPH::ShapeCastSettings &p_shape_cast_settings, const JPH::Shape *p_shape, JPH::Vec3Arg p_scale, const JPH::ShapeFilter &p_shape_filter, JPH::Mat44Arg p_center_of_mass_transform2, const JPH::SubShapeIDCreator &p_sub_shape_id_creator1, const JPH::SubShapeIDCreator &p_sub_shape_id_creator2, JPH::CastShapeCollector &p_collector) {
+	ERR_FAIL_COND(p_shape->GetSubType() != JoltCustomShapeSubType::DOUBLE_SIDED);
+
+	const JoltCustomDoubleSidedShape *shape = static_cast<const JoltCustomDoubleSidedShape *>(p_shape);
+
+	JPH::ShapeCastSettings new_shape_cast_settings = p_shape_cast_settings;
+	new_shape_cast_settings.mBackFaceModeTriangles = JPH::EBackFaceMode::CollideWithBackFaces;
+
+	JPH::CollisionDispatch::sCastShapeVsShapeLocalSpace(p_shape_cast, new_shape_cast_settings, shape->GetInnerShape(), p_scale, p_shape_filter, p_center_of_mass_transform2, p_sub_shape_id_creator1, p_sub_shape_id_creator2, p_collector);
+}
+
+} // namespace
+
+JPH::ShapeSettings::ShapeResult JoltCustomDoubleSidedShapeSettings::Create() const {
+	if (mCachedResult.IsEmpty()) {
+		new JoltCustomDoubleSidedShape(*this, mCachedResult);
+	}
+
+	return mCachedResult;
+}
+
+void JoltCustomDoubleSidedShape::register_type() {
+	JPH::ShapeFunctions &shape_functions = JPH::ShapeFunctions::sGet(JoltCustomShapeSubType::DOUBLE_SIDED);
+
+	shape_functions.mConstruct = construct_double_sided;
+	shape_functions.mColor = JPH::Color::sPurple;
+
+	for (const JPH::EShapeSubType sub_type : JPH::sAllSubShapeTypes) {
+		JPH::CollisionDispatch::sRegisterCollideShape(JoltCustomShapeSubType::DOUBLE_SIDED, sub_type, collide_double_sided_vs_shape);
+		JPH::CollisionDispatch::sRegisterCollideShape(sub_type, JoltCustomShapeSubType::DOUBLE_SIDED, collide_shape_vs_double_sided);
+	}
+
+	for (const JPH::EShapeSubType sub_type : JPH::sConvexSubShapeTypes) {
+		JPH::CollisionDispatch::sRegisterCastShape(sub_type, JoltCustomShapeSubType::DOUBLE_SIDED, cast_shape_vs_double_sided);
+	}
+}
+
+void JoltCustomDoubleSidedShape::CastRay(const JPH::RayCast &p_ray, const JPH::RayCastSettings &p_ray_cast_settings, const JPH::SubShapeIDCreator &p_sub_shape_id_creator, JPH::CastRayCollector &p_collector, const JPH::ShapeFilter &p_shape_filter) const {
+	JPH::RayCastSettings new_ray_cast_settings = p_ray_cast_settings;
+
+	if (!back_face_collision) {
+		new_ray_cast_settings.SetBackFaceMode(JPH::EBackFaceMode::IgnoreBackFaces);
+	}
+
+	return mInnerShape->CastRay(p_ray, new_ray_cast_settings, p_sub_shape_id_creator, p_collector, p_shape_filter);
+}
diff --git a/modules/jolt_physics/shapes/jolt_custom_double_sided_shape.h b/modules/jolt_physics/shapes/jolt_custom_double_sided_shape.h
new file mode 100644
index 0000000000..664b633a21
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_custom_double_sided_shape.h
@@ -0,0 +1,74 @@
+/**************************************************************************/
+/*  jolt_custom_double_sided_shape.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_CUSTOM_DOUBLE_SIDED_SHAPE_H
+#define JOLT_CUSTOM_DOUBLE_SIDED_SHAPE_H
+
+#include "jolt_custom_decorated_shape.h"
+#include "jolt_custom_shape_type.h"
+
+class JoltCustomDoubleSidedShapeSettings final : public JoltCustomDecoratedShapeSettings {
+public:
+	bool back_face_collision = false;
+
+	JoltCustomDoubleSidedShapeSettings() = default;
+
+	JoltCustomDoubleSidedShapeSettings(const ShapeSettings *p_inner_settings, bool p_back_face_collision) :
+			JoltCustomDecoratedShapeSettings(p_inner_settings), back_face_collision(p_back_face_collision) {}
+
+	JoltCustomDoubleSidedShapeSettings(const JPH::Shape *p_inner_shape, bool p_back_face_collision) :
+			JoltCustomDecoratedShapeSettings(p_inner_shape), back_face_collision(p_back_face_collision) {}
+
+	virtual JPH::Shape::ShapeResult Create() const override;
+};
+
+class JoltCustomDoubleSidedShape final : public JoltCustomDecoratedShape {
+	bool back_face_collision = false;
+
+public:
+	static void register_type();
+
+	JoltCustomDoubleSidedShape() :
+			JoltCustomDecoratedShape(JoltCustomShapeSubType::DOUBLE_SIDED) {}
+
+	JoltCustomDoubleSidedShape(const JoltCustomDoubleSidedShapeSettings &p_settings, JPH::Shape::ShapeResult &p_result) :
+			JoltCustomDecoratedShape(JoltCustomShapeSubType::DOUBLE_SIDED, p_settings, p_result), back_face_collision(p_settings.back_face_collision) {
+		if (!p_result.HasError()) {
+			p_result.Set(this);
+		}
+	}
+
+	JoltCustomDoubleSidedShape(const JPH::Shape *p_inner_shape, bool p_back_face_collision) :
+			JoltCustomDecoratedShape(JoltCustomShapeSubType::DOUBLE_SIDED, p_inner_shape), back_face_collision(p_back_face_collision) {}
+
+	virtual void CastRay(const JPH::RayCast &p_ray, const JPH::RayCastSettings &p_ray_cast_settings, const JPH::SubShapeIDCreator &p_sub_shape_id_creator, JPH::CastRayCollector &p_collector, const JPH::ShapeFilter &p_shape_filter = JPH::ShapeFilter()) const override;
+};
+
+#endif // JOLT_CUSTOM_DOUBLE_SIDED_SHAPE_H
diff --git a/modules/jolt_physics/shapes/jolt_custom_motion_shape.cpp b/modules/jolt_physics/shapes/jolt_custom_motion_shape.cpp
new file mode 100644
index 0000000000..c0cd638337
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_custom_motion_shape.cpp
@@ -0,0 +1,78 @@
+/**************************************************************************/
+/*  jolt_custom_motion_shape.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_custom_motion_shape.h"
+
+namespace {
+
+class JoltMotionConvexSupport final : public JPH::ConvexShape::Support {
+public:
+	JoltMotionConvexSupport(JPH::Vec3Arg p_motion, const JPH::ConvexShape::Support *p_inner_support) :
+			motion(p_motion),
+			inner_support(p_inner_support) {}
+
+	virtual JPH::Vec3 GetSupport(JPH::Vec3Arg p_direction) const override {
+		JPH::Vec3 support = inner_support->GetSupport(p_direction);
+
+		if (p_direction.Dot(motion) > 0) {
+			support += motion;
+		}
+
+		return support;
+	}
+
+	virtual float GetConvexRadius() const override { return inner_support->GetConvexRadius(); }
+
+private:
+	JPH::Vec3 motion = JPH::Vec3::sZero();
+
+	const JPH::ConvexShape::Support *inner_support = nullptr;
+};
+
+} // namespace
+
+JPH::AABox JoltCustomMotionShape::GetLocalBounds() const {
+	JPH::AABox aabb = inner_shape.GetLocalBounds();
+	JPH::AABox aabb_translated = aabb;
+	aabb_translated.Translate(motion);
+	aabb.Encapsulate(aabb_translated);
+
+	return aabb;
+}
+
+void JoltCustomMotionShape::GetSupportingFace(const JPH::SubShapeID &p_sub_shape_id, JPH::Vec3Arg p_direction, JPH::Vec3Arg p_scale, JPH::Mat44Arg p_center_of_mass_transform, JPH::Shape::SupportingFace &p_vertices) const {
+	// This is technically called when using the enhanced internal edge removal, but `JPH::InternalEdgeRemovingCollector` will
+	// only ever use the faces of the second shape in the collision pair, and this shape will always be the first in the pair, so
+	// we can safely skip this.
+}
+
+const JPH::ConvexShape::Support *JoltCustomMotionShape::GetSupportFunction(JPH::ConvexShape::ESupportMode p_mode, JPH::ConvexShape::SupportBuffer &p_buffer, JPH::Vec3Arg p_scale) const {
+	return new (&p_buffer) JoltMotionConvexSupport(motion, inner_shape.GetSupportFunction(p_mode, inner_support_buffer, p_scale));
+}
diff --git a/modules/jolt_physics/shapes/jolt_custom_motion_shape.h b/modules/jolt_physics/shapes/jolt_custom_motion_shape.h
new file mode 100644
index 0000000000..329b5b8511
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_custom_motion_shape.h
@@ -0,0 +1,117 @@
+/**************************************************************************/
+/*  jolt_custom_motion_shape.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_CUSTOM_MOTION_SHAPE_H
+#define JOLT_CUSTOM_MOTION_SHAPE_H
+
+#include "jolt_custom_shape_type.h"
+
+#include "core/error/error_macros.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Collision/Shape/ConvexShape.h"
+#include "Jolt/Physics/Collision/TransformedShape.h"
+
+class JoltCustomMotionShape final : public JPH::ConvexShape {
+	mutable JPH::ConvexShape::SupportBuffer inner_support_buffer;
+
+	JPH::Vec3 motion = JPH::Vec3::sZero();
+
+	const JPH::ConvexShape &inner_shape;
+
+public:
+	explicit JoltCustomMotionShape(const JPH::ConvexShape &p_shape) :
+			JPH::ConvexShape(JoltCustomShapeSubType::MOTION), inner_shape(p_shape) {}
+
+	virtual bool MustBeStatic() const override { return false; }
+
+	virtual JPH::Vec3 GetCenterOfMass() const override { ERR_FAIL_V_MSG(JPH::Vec3::sZero(), "Not implemented."); }
+
+	virtual JPH::AABox GetLocalBounds() const override;
+
+	virtual JPH::uint GetSubShapeIDBitsRecursive() const override { ERR_FAIL_V_MSG(0, "Not implemented."); }
+
+	virtual JPH::AABox GetWorldSpaceBounds(JPH::Mat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale) const override { ERR_FAIL_V_MSG(JPH::AABox(), "Not implemented."); }
+
+	virtual float GetInnerRadius() const override { ERR_FAIL_V_MSG(0.0f, "Not implemented."); }
+
+	virtual JPH::MassProperties GetMassProperties() const override { ERR_FAIL_V_MSG(JPH::MassProperties(), "Not implemented."); }
+
+	virtual const JPH::PhysicsMaterial *GetMaterial(const JPH::SubShapeID &p_sub_shape_id) const override { ERR_FAIL_V_MSG(nullptr, "Not implemented."); }
+
+	virtual JPH::Vec3 GetSurfaceNormal(const JPH::SubShapeID &p_sub_shape_id, JPH::Vec3Arg p_local_surface_position) const override { ERR_FAIL_V_MSG(JPH::Vec3::sZero(), "Not implemented."); }
+
+	virtual void GetSupportingFace(const JPH::SubShapeID &p_sub_shape_id, JPH::Vec3Arg p_direction, JPH::Vec3Arg p_scale, JPH::Mat44Arg p_center_of_mass_transform, JPH::Shape::SupportingFace &p_vertices) const override;
+
+	virtual JPH::uint64 GetSubShapeUserData(const JPH::SubShapeID &p_sub_shape_id) const override { ERR_FAIL_V_MSG(0, "Not implemented."); }
+
+	virtual JPH::TransformedShape GetSubShapeTransformedShape(const JPH::SubShapeID &p_sub_shape_id, JPH::Vec3Arg p_position_com, JPH::QuatArg p_rotation, JPH::Vec3Arg p_scale, JPH::SubShapeID &p_remainder) const override { ERR_FAIL_V_MSG(JPH::TransformedShape(), "Not implemented."); }
+
+	virtual void GetSubmergedVolume(JPH::Mat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale, const JPH::Plane &p_surface, float &p_total_volume, float &p_submerged_volume, JPH::Vec3 &p_center_of_buoyancy JPH_IF_DEBUG_RENDERER(, JPH::RVec3Arg p_base_offset)) const override { ERR_FAIL_MSG("Not implemented."); }
+
+	virtual const JPH::ConvexShape::Support *GetSupportFunction(JPH::ConvexShape::ESupportMode p_mode, JPH::ConvexShape::SupportBuffer &p_buffer, JPH::Vec3Arg p_scale) const override;
+
+#ifdef JPH_DEBUG_RENDERER
+	virtual void Draw(JPH::DebugRenderer *p_renderer, JPH::RMat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale, JPH::ColorArg p_color, bool p_use_material_colors, bool p_draw_wireframe) const override { ERR_FAIL_MSG("Not implemented."); }
+
+	virtual void DrawGetSupportFunction(JPH::DebugRenderer *p_renderer, JPH::RMat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale, JPH::ColorArg p_color, bool p_draw_support_direction) const override { ERR_FAIL_MSG("Not implemented."); }
+
+	virtual void DrawGetSupportingFace(JPH::DebugRenderer *p_renderer, JPH::RMat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale) const override { ERR_FAIL_MSG("Not implemented."); }
+#endif
+
+	virtual bool CastRay(const JPH::RayCast &p_ray, const JPH::SubShapeIDCreator &p_sub_shape_id_creator, JPH::RayCastResult &p_hit) const override { ERR_FAIL_V_MSG(false, "Not implemented."); }
+
+	virtual void CastRay(const JPH::RayCast &p_ray, const JPH::RayCastSettings &p_ray_cast_settings, const JPH::SubShapeIDCreator &p_sub_shape_id_creator, JPH::CastRayCollector &p_collector, const JPH::ShapeFilter &p_shape_filter = JPH::ShapeFilter()) const override { ERR_FAIL_MSG("Not implemented."); }
+
+	virtual void CollidePoint(JPH::Vec3Arg p_point, const JPH::SubShapeIDCreator &p_sub_shape_id_creator, JPH::CollidePointCollector &p_collector, const JPH::ShapeFilter &p_shape_filter = JPH::ShapeFilter()) const override { ERR_FAIL_MSG("Not implemented."); }
+
+	virtual void CollideSoftBodyVertices(JPH::Mat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale, const JPH::CollideSoftBodyVertexIterator &p_vertices, JPH::uint p_num_vertices, int p_colliding_shape_index) const override { ERR_FAIL_MSG("Not implemented."); }
+
+	virtual void CollectTransformedShapes(const JPH::AABox &p_box, JPH::Vec3Arg p_position_com, JPH::QuatArg p_rotation, JPH::Vec3Arg p_scale, const JPH::SubShapeIDCreator &p_sub_shape_id_creator, JPH::TransformedShapeCollector &p_collector, const JPH::ShapeFilter &p_shape_filter = JPH::ShapeFilter()) const override { ERR_FAIL_MSG("Not implemented."); }
+
+	virtual void TransformShape(JPH::Mat44Arg p_center_of_mass_transform, JPH::TransformedShapeCollector &p_collector) const override { ERR_FAIL_MSG("Not implemented."); }
+
+	virtual void GetTrianglesStart(JPH::Shape::GetTrianglesContext &p_context, const JPH::AABox &p_box, JPH::Vec3Arg p_position_com, JPH::QuatArg p_rotation, JPH::Vec3Arg p_scale) const override { ERR_FAIL_MSG("Not implemented."); }
+
+	virtual int GetTrianglesNext(JPH::Shape::GetTrianglesContext &p_context, int p_max_triangles_requested, JPH::Float3 *p_triangle_vertices, const JPH::PhysicsMaterial **p_materials = nullptr) const override { ERR_FAIL_V_MSG(0, "Not implemented."); }
+
+	virtual JPH::Shape::Stats GetStats() const override { return JPH::Shape::Stats(sizeof(*this), 0); }
+
+	virtual float GetVolume() const override { ERR_FAIL_V_MSG(0.0f, "Not implemented."); }
+
+	virtual bool IsValidScale(JPH::Vec3Arg p_scale) const override { ERR_FAIL_V_MSG(false, "Not implemented."); }
+
+	const JPH::ConvexShape &get_inner_shape() const { return inner_shape; }
+
+	void set_motion(JPH::Vec3Arg p_motion) { motion = p_motion; }
+};
+
+#endif // JOLT_CUSTOM_MOTION_SHAPE_H
diff --git a/modules/jolt_physics/shapes/jolt_custom_ray_shape.cpp b/modules/jolt_physics/shapes/jolt_custom_ray_shape.cpp
new file mode 100644
index 0000000000..c00e43b485
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_custom_ray_shape.cpp
@@ -0,0 +1,232 @@
+/**************************************************************************/
+/*  jolt_custom_ray_shape.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_custom_ray_shape.h"
+
+#include "../spaces/jolt_query_collectors.h"
+
+#include "Jolt/Physics/Collision/CastResult.h"
+#include "Jolt/Physics/Collision/RayCast.h"
+#include "Jolt/Physics/Collision/TransformedShape.h"
+
+#ifdef JPH_DEBUG_RENDERER
+#include "Jolt/Renderer/DebugRenderer.h"
+#endif
+
+namespace {
+
+class JoltCustomRayShapeSupport final : public JPH::ConvexShape::Support {
+public:
+	explicit JoltCustomRayShapeSupport(float p_length) :
+			length(p_length) {}
+
+	virtual JPH::Vec3 GetSupport(JPH::Vec3Arg p_direction) const override {
+		if (p_direction.GetZ() > 0.0f) {
+			return JPH::Vec3(0.0f, 0.0f, length);
+		} else {
+			return JPH::Vec3::sZero();
+		}
+	}
+
+	virtual float GetConvexRadius() const override { return 0.0f; }
+
+private:
+	float length = 0.0f;
+};
+
+static_assert(sizeof(JoltCustomRayShapeSupport) <= sizeof(JPH::ConvexShape::SupportBuffer), "Size of SeparationRayShape3D support is larger than size of support buffer.");
+
+JPH::Shape *construct_ray() {
+	return new JoltCustomRayShape();
+}
+
+void collide_ray_vs_shape(const JPH::Shape *p_shape1, const JPH::Shape *p_shape2, JPH::Vec3Arg p_scale1, JPH::Vec3Arg p_scale2, JPH::Mat44Arg p_center_of_mass_transform1, JPH::Mat44Arg p_center_of_mass_transform2, const JPH::SubShapeIDCreator &p_sub_shape_id_creator1, const JPH::SubShapeIDCreator &p_sub_shape_id_creator2, const JPH::CollideShapeSettings &p_collide_shape_settings, JPH::CollideShapeCollector &p_collector, const JPH::ShapeFilter &p_shape_filter) {
+	ERR_FAIL_COND(p_shape1->GetSubType() != JoltCustomShapeSubType::RAY);
+
+	const JoltCustomRayShape *shape1 = static_cast<const JoltCustomRayShape *>(p_shape1);
+
+	const float margin = p_collide_shape_settings.mMaxSeparationDistance;
+	const float ray_length = shape1->length;
+	const float ray_length_padded = ray_length + margin;
+
+	const JPH::Mat44 transform1 = p_center_of_mass_transform1 * JPH::Mat44::sScale(p_scale1);
+	const JPH::Mat44 transform2 = p_center_of_mass_transform2 * JPH::Mat44::sScale(p_scale2);
+	const JPH::Mat44 transform_inv2 = transform2.Inversed();
+
+	const JPH::Vec3 ray_start = transform1.GetTranslation();
+	const JPH::Vec3 ray_direction = transform1.GetAxisZ();
+	const JPH::Vec3 ray_vector = ray_direction * ray_length;
+	const JPH::Vec3 ray_vector_padded = ray_direction * ray_length_padded;
+
+	const JPH::Vec3 ray_start2 = transform_inv2 * ray_start;
+	const JPH::Vec3 ray_direction2 = transform_inv2.Multiply3x3(ray_direction);
+	const JPH::Vec3 ray_vector_padded2 = transform_inv2.Multiply3x3(ray_vector_padded);
+
+	const JPH::RayCast ray_cast(ray_start2, ray_vector_padded2);
+
+	JPH::RayCastSettings ray_cast_settings;
+	ray_cast_settings.mTreatConvexAsSolid = false;
+	ray_cast_settings.mBackFaceModeTriangles = p_collide_shape_settings.mBackFaceMode;
+
+	JoltQueryCollectorClosest<JPH::CastRayCollector> ray_collector;
+
+	p_shape2->CastRay(ray_cast, ray_cast_settings, p_sub_shape_id_creator2, ray_collector);
+
+	if (!ray_collector.had_hit()) {
+		return;
+	}
+
+	const JPH::RayCastResult &hit = ray_collector.get_hit();
+
+	const float hit_distance = ray_length_padded * hit.mFraction;
+	const float hit_depth = ray_length - hit_distance;
+
+	if (-hit_depth >= p_collector.GetEarlyOutFraction()) {
+		return;
+	}
+
+	// Since `hit.mSubShapeID2` could represent a path not only from `p_shape2` but also any
+	// compound shape that it's contained within, we need to split this path into something that
+	// `p_shape2` can actually understand.
+	JPH::SubShapeID local_sub_shape_id2;
+	hit.mSubShapeID2.PopID(p_sub_shape_id_creator2.GetNumBitsWritten(), local_sub_shape_id2);
+
+	const JPH::Vec3 hit_point2 = ray_cast.GetPointOnRay(hit.mFraction);
+
+	const JPH::Vec3 hit_point_on_1 = ray_start + ray_vector;
+	const JPH::Vec3 hit_point_on_2 = transform2 * hit_point2;
+
+	JPH::Vec3 hit_normal2 = JPH::Vec3::sZero();
+
+	if (shape1->slide_on_slope) {
+		hit_normal2 = p_shape2->GetSurfaceNormal(local_sub_shape_id2, hit_point2);
+
+		// If we got a back-face normal we need to flip it.
+		if (hit_normal2.Dot(ray_direction2) > 0) {
+			hit_normal2 = -hit_normal2;
+		}
+	} else {
+		hit_normal2 = -ray_direction2;
+	}
+
+	const JPH::Vec3 hit_normal = transform2.Multiply3x3(hit_normal2);
+
+	JPH::CollideShapeResult result(hit_point_on_1, hit_point_on_2, -hit_normal, hit_depth, p_sub_shape_id_creator1.GetID(), hit.mSubShapeID2, JPH::TransformedShape::sGetBodyID(p_collector.GetContext()));
+
+	if (p_collide_shape_settings.mCollectFacesMode == JPH::ECollectFacesMode::CollectFaces) {
+		p_shape2->GetSupportingFace(local_sub_shape_id2, ray_direction2, p_scale2, p_center_of_mass_transform2, result.mShape2Face);
+	}
+
+	p_collector.AddHit(result);
+}
+
+void collide_noop(const JPH::Shape *p_shape1, const JPH::Shape *p_shape2, JPH::Vec3Arg p_scale1, JPH::Vec3Arg p_scale2, JPH::Mat44Arg p_center_of_mass_transform1, JPH::Mat44Arg p_center_of_mass_transform2, const JPH::SubShapeIDCreator &p_sub_shape_id_creator1, const JPH::SubShapeIDCreator &p_sub_shape_id_creator2, const JPH::CollideShapeSettings &p_collide_shape_settings, JPH::CollideShapeCollector &p_collector, const JPH::ShapeFilter &p_shape_filter) {
+}
+
+void cast_noop(const JPH::ShapeCast &p_shape_cast, const JPH::ShapeCastSettings &p_shape_cast_settings, const JPH::Shape *p_shape, JPH::Vec3Arg p_scale, const JPH::ShapeFilter &p_shape_filter, JPH::Mat44Arg p_center_of_mass_transform2, const JPH::SubShapeIDCreator &p_sub_shape_id_creator1, const JPH::SubShapeIDCreator &p_sub_shape_id_creator2, JPH::CastShapeCollector &p_collector) {
+}
+
+} // namespace
+
+JPH::ShapeSettings::ShapeResult JoltCustomRayShapeSettings::Create() const {
+	if (mCachedResult.IsEmpty()) {
+		new JoltCustomRayShape(*this, mCachedResult);
+	}
+
+	return mCachedResult;
+}
+
+void JoltCustomRayShape::register_type() {
+	JPH::ShapeFunctions &shape_functions = JPH::ShapeFunctions::sGet(JoltCustomShapeSubType::RAY);
+
+	shape_functions.mConstruct = construct_ray;
+	shape_functions.mColor = JPH::Color::sDarkRed;
+
+	static constexpr JPH::EShapeSubType concrete_sub_types[] = {
+		JPH::EShapeSubType::Sphere,
+		JPH::EShapeSubType::Box,
+		JPH::EShapeSubType::Triangle,
+		JPH::EShapeSubType::Capsule,
+		JPH::EShapeSubType::TaperedCapsule,
+		JPH::EShapeSubType::Cylinder,
+		JPH::EShapeSubType::ConvexHull,
+		JPH::EShapeSubType::Mesh,
+		JPH::EShapeSubType::HeightField,
+		JPH::EShapeSubType::Plane,
+		JPH::EShapeSubType::TaperedCylinder
+	};
+
+	for (const JPH::EShapeSubType concrete_sub_type : concrete_sub_types) {
+		JPH::CollisionDispatch::sRegisterCollideShape(JoltCustomShapeSubType::RAY, concrete_sub_type, collide_ray_vs_shape);
+		JPH::CollisionDispatch::sRegisterCollideShape(concrete_sub_type, JoltCustomShapeSubType::RAY, JPH::CollisionDispatch::sReversedCollideShape);
+	}
+
+	JPH::CollisionDispatch::sRegisterCollideShape(JoltCustomShapeSubType::RAY, JoltCustomShapeSubType::RAY, collide_noop);
+
+	for (const JPH::EShapeSubType sub_type : JPH::sAllSubShapeTypes) {
+		JPH::CollisionDispatch::sRegisterCastShape(JoltCustomShapeSubType::RAY, sub_type, cast_noop);
+		JPH::CollisionDispatch::sRegisterCastShape(sub_type, JoltCustomShapeSubType::RAY, cast_noop);
+	}
+}
+
+JPH::AABox JoltCustomRayShape::GetLocalBounds() const {
+	const float radius = GetInnerRadius();
+	return JPH::AABox(JPH::Vec3(-radius, -radius, 0.0f), JPH::Vec3(radius, radius, length));
+}
+
+float JoltCustomRayShape::GetInnerRadius() const {
+	// There is no sensible value here, since this shape is infinitely thin, so we pick something
+	// that's hopefully small enough to effectively be zero, but big enough to not cause any
+	// numerical issues.
+	return 0.0001f;
+}
+
+JPH::MassProperties JoltCustomRayShape::GetMassProperties() const {
+	JPH::MassProperties mass_properties;
+
+	// Since this shape has no volume we can't really give it a correct set of mass properties, so
+	// instead we just give it some arbitrary ones.
+	mass_properties.mMass = 1.0f;
+	mass_properties.mInertia = JPH::Mat44::sIdentity();
+
+	return mass_properties;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+
+void JoltCustomRayShape::Draw(JPH::DebugRenderer *p_renderer, JPH::RMat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale, JPH::ColorArg p_color, bool p_use_material_colors, bool p_draw_wireframe) const {
+	p_renderer->DrawArrow(p_center_of_mass_transform.GetTranslation(), p_center_of_mass_transform * JPH::Vec3(0, 0, length * p_scale.GetZ()), p_use_material_colors ? GetMaterial()->GetDebugColor() : p_color, 0.1f);
+}
+
+#endif
+
+const JPH::ConvexShape::Support *JoltCustomRayShape::GetSupportFunction(JPH::ConvexShape::ESupportMode p_mode, JPH::ConvexShape::SupportBuffer &p_buffer, JPH::Vec3Arg p_scale) const {
+	return new (&p_buffer) JoltCustomRayShapeSupport(p_scale.GetZ() * length);
+}
diff --git a/modules/jolt_physics/shapes/jolt_custom_ray_shape.h b/modules/jolt_physics/shapes/jolt_custom_ray_shape.h
new file mode 100644
index 0000000000..faa0a1f76a
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_custom_ray_shape.h
@@ -0,0 +1,107 @@
+/**************************************************************************/
+/*  jolt_custom_ray_shape.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_CUSTOM_RAY_SHAPE_H
+#define JOLT_CUSTOM_RAY_SHAPE_H
+
+#include "jolt_custom_shape_type.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Collision/Shape/ConvexShape.h"
+
+class JoltCustomRayShapeSettings final : public JPH::ConvexShapeSettings {
+public:
+	JPH::RefConst<JPH::PhysicsMaterial> material;
+	float length = 1.0f;
+	bool slide_on_slope = false;
+
+	JoltCustomRayShapeSettings() = default;
+	JoltCustomRayShapeSettings(float p_length, bool p_slide_on_slope, const JPH::PhysicsMaterial *p_material = nullptr) :
+			material(p_material), length(p_length), slide_on_slope(p_slide_on_slope) {}
+
+	virtual JPH::ShapeSettings::ShapeResult Create() const override;
+};
+
+class JoltCustomRayShape final : public JPH::ConvexShape {
+public:
+	JPH::RefConst<JPH::PhysicsMaterial> material;
+	float length = 0.0f;
+	bool slide_on_slope = false;
+
+	static void register_type();
+
+	JoltCustomRayShape() :
+			JPH::ConvexShape(JoltCustomShapeSubType::RAY) {}
+
+	JoltCustomRayShape(const JoltCustomRayShapeSettings &p_settings, JPH::Shape::ShapeResult &p_result) :
+			JPH::ConvexShape(JoltCustomShapeSubType::RAY, p_settings, p_result), material(p_settings.material), length(p_settings.length), slide_on_slope(p_settings.slide_on_slope) {
+		if (!p_result.HasError()) {
+			p_result.Set(this);
+		}
+	}
+
+	JoltCustomRayShape(float p_length, bool p_slide_on_slope, const JPH::PhysicsMaterial *p_material = nullptr) :
+			JPH::ConvexShape(JoltCustomShapeSubType::RAY), material(p_material), length(p_length), slide_on_slope(p_slide_on_slope) {}
+
+	virtual JPH::AABox GetLocalBounds() const override;
+
+	virtual float GetInnerRadius() const override;
+
+	virtual JPH::MassProperties GetMassProperties() const override;
+
+	virtual JPH::Vec3 GetSurfaceNormal(const JPH::SubShapeID &p_sub_shape_id, JPH::Vec3Arg p_local_surface_position) const override { return JPH::Vec3::sAxisZ(); }
+
+	virtual void GetSubmergedVolume(JPH::Mat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale, const JPH::Plane &p_surface, float &p_total_volume, float &p_submerged_volume, JPH::Vec3 &p_center_of_buoyancy JPH_IF_DEBUG_RENDERER(, JPH::RVec3Arg p_base_offset)) const override {
+		p_total_volume = 0.0f;
+		p_submerged_volume = 0.0f;
+		p_center_of_buoyancy = JPH::Vec3::sZero();
+	}
+
+#ifdef JPH_DEBUG_RENDERER
+	virtual void Draw(JPH::DebugRenderer *p_renderer, JPH::RMat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale, JPH::ColorArg p_color, bool p_use_material_colors, bool p_draw_wireframe) const override;
+#endif
+
+	virtual bool CastRay(const JPH::RayCast &p_ray, const JPH::SubShapeIDCreator &p_sub_shape_id_creator, JPH::RayCastResult &p_hit) const override { return false; }
+
+	virtual void CastRay(const JPH::RayCast &p_ray, const JPH::RayCastSettings &p_ray_cast_settings, const JPH::SubShapeIDCreator &p_sub_shape_id_creator, JPH::CastRayCollector &p_collector, const JPH::ShapeFilter &p_shape_filter = JPH::ShapeFilter()) const override {}
+
+	virtual void CollidePoint(JPH::Vec3Arg p_point, const JPH::SubShapeIDCreator &p_sub_shape_id_creator, JPH::CollidePointCollector &p_collector, const JPH::ShapeFilter &p_shape_filter = JPH::ShapeFilter()) const override {}
+
+	virtual void CollideSoftBodyVertices(JPH::Mat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale, const JPH::CollideSoftBodyVertexIterator &p_vertices, JPH::uint p_num_vertices, int p_colliding_shape_index) const override {}
+
+	virtual JPH::Shape::Stats GetStats() const override { return JPH::Shape::Stats(sizeof(*this), 0); }
+
+	virtual float GetVolume() const override { return 0.0f; }
+
+	virtual const JPH::ConvexShape::Support *GetSupportFunction(JPH::ConvexShape::ESupportMode p_mode, JPH::ConvexShape::SupportBuffer &p_buffer, JPH::Vec3Arg p_scale) const override;
+};
+
+#endif // JOLT_CUSTOM_RAY_SHAPE_H
diff --git a/modules/jolt_physics/shapes/jolt_custom_shape_type.h b/modules/jolt_physics/shapes/jolt_custom_shape_type.h
new file mode 100644
index 0000000000..77371640c2
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_custom_shape_type.h
@@ -0,0 +1,47 @@
+/**************************************************************************/
+/*  jolt_custom_shape_type.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_CUSTOM_SHAPE_TYPE_H
+#define JOLT_CUSTOM_SHAPE_TYPE_H
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Collision/Shape/Shape.h"
+
+namespace JoltCustomShapeSubType {
+
+constexpr JPH::EShapeSubType OVERRIDE_USER_DATA = JPH::EShapeSubType::User1;
+constexpr JPH::EShapeSubType DOUBLE_SIDED = JPH::EShapeSubType::User2;
+constexpr JPH::EShapeSubType RAY = JPH::EShapeSubType::UserConvex1;
+constexpr JPH::EShapeSubType MOTION = JPH::EShapeSubType::UserConvex2;
+
+} // namespace JoltCustomShapeSubType
+
+#endif // JOLT_CUSTOM_SHAPE_TYPE_H
diff --git a/modules/jolt_physics/shapes/jolt_custom_user_data_shape.cpp b/modules/jolt_physics/shapes/jolt_custom_user_data_shape.cpp
new file mode 100644
index 0000000000..53637ebe63
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_custom_user_data_shape.cpp
@@ -0,0 +1,99 @@
+/**************************************************************************/
+/*  jolt_custom_user_data_shape.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_custom_user_data_shape.h"
+
+#include "core/error/error_macros.h"
+
+#include "Jolt/Physics/Collision/CollisionDispatch.h"
+#include "Jolt/Physics/Collision/ShapeCast.h"
+
+namespace {
+
+JPH::Shape *construct_override_user_data() {
+	return new JoltCustomUserDataShape();
+}
+
+void collide_override_user_data_vs_shape(const JPH::Shape *p_shape1, const JPH::Shape *p_shape2, JPH::Vec3Arg p_scale1, JPH::Vec3Arg p_scale2, JPH::Mat44Arg p_center_of_mass_transform1, JPH::Mat44Arg p_center_of_mass_transform2, const JPH::SubShapeIDCreator &p_sub_shape_id_creator1, const JPH::SubShapeIDCreator &p_sub_shape_id_creator2, const JPH::CollideShapeSettings &p_collide_shape_settings, JPH::CollideShapeCollector &p_collector, const JPH::ShapeFilter &p_shape_filter) {
+	ERR_FAIL_COND(p_shape1->GetSubType() != JoltCustomShapeSubType::OVERRIDE_USER_DATA);
+
+	const JoltCustomUserDataShape *shape1 = static_cast<const JoltCustomUserDataShape *>(p_shape1);
+
+	JPH::CollisionDispatch::sCollideShapeVsShape(shape1->GetInnerShape(), p_shape2, p_scale1, p_scale2, p_center_of_mass_transform1, p_center_of_mass_transform2, p_sub_shape_id_creator1, p_sub_shape_id_creator2, p_collide_shape_settings, p_collector, p_shape_filter);
+}
+
+void collide_shape_vs_override_user_data(const JPH::Shape *p_shape1, const JPH::Shape *p_shape2, JPH::Vec3Arg p_scale1, JPH::Vec3Arg p_scale2, JPH::Mat44Arg p_center_of_mass_transform1, JPH::Mat44Arg p_center_of_mass_transform2, const JPH::SubShapeIDCreator &p_sub_shape_id_creator1, const JPH::SubShapeIDCreator &p_sub_shape_id_creator2, const JPH::CollideShapeSettings &p_collide_shape_settings, JPH::CollideShapeCollector &p_collector, const JPH::ShapeFilter &p_shape_filter) {
+	ERR_FAIL_COND(p_shape2->GetSubType() != JoltCustomShapeSubType::OVERRIDE_USER_DATA);
+
+	const JoltCustomUserDataShape *shape2 = static_cast<const JoltCustomUserDataShape *>(p_shape2);
+
+	JPH::CollisionDispatch::sCollideShapeVsShape(p_shape1, shape2->GetInnerShape(), p_scale1, p_scale2, p_center_of_mass_transform1, p_center_of_mass_transform2, p_sub_shape_id_creator1, p_sub_shape_id_creator2, p_collide_shape_settings, p_collector, p_shape_filter);
+}
+
+void cast_override_user_data_vs_shape(const JPH::ShapeCast &p_shape_cast, const JPH::ShapeCastSettings &p_shape_cast_settings, const JPH::Shape *p_shape, JPH::Vec3Arg p_scale, const JPH::ShapeFilter &p_shape_filter, JPH::Mat44Arg p_center_of_mass_transform2, const JPH::SubShapeIDCreator &p_sub_shape_id_creator1, const JPH::SubShapeIDCreator &p_sub_shape_id_creator2, JPH::CastShapeCollector &p_collector) {
+	ERR_FAIL_COND(p_shape_cast.mShape->GetSubType() != JoltCustomShapeSubType::OVERRIDE_USER_DATA);
+
+	const JoltCustomUserDataShape *shape = static_cast<const JoltCustomUserDataShape *>(p_shape_cast.mShape);
+	const JPH::ShapeCast shape_cast(shape->GetInnerShape(), p_shape_cast.mScale, p_shape_cast.mCenterOfMassStart, p_shape_cast.mDirection);
+
+	JPH::CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, p_shape_cast_settings, p_shape, p_scale, p_shape_filter, p_center_of_mass_transform2, p_sub_shape_id_creator1, p_sub_shape_id_creator2, p_collector);
+}
+
+void cast_shape_vs_override_user_data(const JPH::ShapeCast &p_shape_cast, const JPH::ShapeCastSettings &p_shape_cast_settings, const JPH::Shape *p_shape, JPH::Vec3Arg p_scale, const JPH::ShapeFilter &p_shape_filter, JPH::Mat44Arg p_center_of_mass_transform2, const JPH::SubShapeIDCreator &p_sub_shape_id_creator1, const JPH::SubShapeIDCreator &p_sub_shape_id_creator2, JPH::CastShapeCollector &p_collector) {
+	ERR_FAIL_COND(p_shape->GetSubType() != JoltCustomShapeSubType::OVERRIDE_USER_DATA);
+
+	const JoltCustomUserDataShape *shape = static_cast<const JoltCustomUserDataShape *>(p_shape);
+
+	JPH::CollisionDispatch::sCastShapeVsShapeLocalSpace(p_shape_cast, p_shape_cast_settings, shape->GetInnerShape(), p_scale, p_shape_filter, p_center_of_mass_transform2, p_sub_shape_id_creator1, p_sub_shape_id_creator2, p_collector);
+}
+
+} // namespace
+
+JPH::ShapeSettings::ShapeResult JoltCustomUserDataShapeSettings::Create() const {
+	if (mCachedResult.IsEmpty()) {
+		new JoltCustomUserDataShape(*this, mCachedResult);
+	}
+
+	return mCachedResult;
+}
+
+void JoltCustomUserDataShape::register_type() {
+	JPH::ShapeFunctions &shape_functions = JPH::ShapeFunctions::sGet(JoltCustomShapeSubType::OVERRIDE_USER_DATA);
+
+	shape_functions.mConstruct = construct_override_user_data;
+	shape_functions.mColor = JPH::Color::sCyan;
+
+	for (const JPH::EShapeSubType sub_type : JPH::sAllSubShapeTypes) {
+		JPH::CollisionDispatch::sRegisterCollideShape(JoltCustomShapeSubType::OVERRIDE_USER_DATA, sub_type, collide_override_user_data_vs_shape);
+		JPH::CollisionDispatch::sRegisterCollideShape(sub_type, JoltCustomShapeSubType::OVERRIDE_USER_DATA, collide_shape_vs_override_user_data);
+		JPH::CollisionDispatch::sRegisterCastShape(JoltCustomShapeSubType::OVERRIDE_USER_DATA, sub_type, cast_override_user_data_vs_shape);
+		JPH::CollisionDispatch::sRegisterCastShape(sub_type, JoltCustomShapeSubType::OVERRIDE_USER_DATA, cast_shape_vs_override_user_data);
+	}
+}
diff --git a/modules/jolt_physics/shapes/jolt_custom_user_data_shape.h b/modules/jolt_physics/shapes/jolt_custom_user_data_shape.h
new file mode 100644
index 0000000000..a6610c7462
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_custom_user_data_shape.h
@@ -0,0 +1,61 @@
+/**************************************************************************/
+/*  jolt_custom_user_data_shape.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_CUSTOM_USER_DATA_SHAPE_H
+#define JOLT_CUSTOM_USER_DATA_SHAPE_H
+
+#include "jolt_custom_decorated_shape.h"
+#include "jolt_custom_shape_type.h"
+
+class JoltCustomUserDataShapeSettings final : public JoltCustomDecoratedShapeSettings {
+public:
+	using JoltCustomDecoratedShapeSettings::JoltCustomDecoratedShapeSettings;
+
+	virtual ShapeResult Create() const override;
+};
+
+class JoltCustomUserDataShape final : public JoltCustomDecoratedShape {
+public:
+	static void register_type();
+
+	JoltCustomUserDataShape() :
+			JoltCustomDecoratedShape(JoltCustomShapeSubType::OVERRIDE_USER_DATA) {}
+
+	JoltCustomUserDataShape(const JoltCustomUserDataShapeSettings &p_settings, ShapeResult &p_result) :
+			JoltCustomDecoratedShape(JoltCustomShapeSubType::OVERRIDE_USER_DATA, p_settings, p_result) {
+		if (!p_result.HasError()) {
+			p_result.Set(this);
+		}
+	}
+
+	virtual JPH::uint64 GetSubShapeUserData(const JPH::SubShapeID &p_sub_shape_id) const override { return GetUserData(); }
+};
+
+#endif // JOLT_CUSTOM_USER_DATA_SHAPE_H
diff --git a/modules/jolt_physics/shapes/jolt_cylinder_shape_3d.cpp b/modules/jolt_physics/shapes/jolt_cylinder_shape_3d.cpp
new file mode 100644
index 0000000000..0d6371b1e0
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_cylinder_shape_3d.cpp
@@ -0,0 +1,98 @@
+/**************************************************************************/
+/*  jolt_cylinder_shape_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_cylinder_shape_3d.h"
+
+#include "../jolt_project_settings.h"
+#include "../misc/jolt_type_conversions.h"
+
+#include "Jolt/Physics/Collision/Shape/CylinderShape.h"
+
+JPH::ShapeRefC JoltCylinderShape3D::_build() const {
+	const float half_height = height / 2.0f;
+	const float min_half_extent = MIN(half_height, radius);
+	const float actual_margin = MIN(margin, min_half_extent * JoltProjectSettings::get_collision_margin_fraction());
+
+	const JPH::CylinderShapeSettings shape_settings(half_height, radius, actual_margin);
+	const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create();
+	ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to build Jolt Physics cylinder shape with %s. It returned the following error: '%s'. This shape belongs to %s.", to_string(), to_godot(shape_result.GetError()), _owners_to_string()));
+
+	return shape_result.Get();
+}
+
+Variant JoltCylinderShape3D::get_data() const {
+	Dictionary data;
+	data["height"] = height;
+	data["radius"] = radius;
+	return data;
+}
+
+void JoltCylinderShape3D::set_data(const Variant &p_data) {
+	ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY);
+
+	const Dictionary data = p_data;
+
+	const Variant maybe_height = data.get("height", Variant());
+	ERR_FAIL_COND(maybe_height.get_type() != Variant::FLOAT);
+
+	const Variant maybe_radius = data.get("radius", Variant());
+	ERR_FAIL_COND(maybe_radius.get_type() != Variant::FLOAT);
+
+	const float new_height = maybe_height;
+	const float new_radius = maybe_radius;
+
+	if (unlikely(new_height == height && new_radius == radius)) {
+		return;
+	}
+
+	height = new_height;
+	radius = new_radius;
+
+	destroy();
+}
+
+void JoltCylinderShape3D::set_margin(float p_margin) {
+	if (unlikely(margin == p_margin)) {
+		return;
+	}
+
+	margin = p_margin;
+
+	destroy();
+}
+
+AABB JoltCylinderShape3D::get_aabb() const {
+	const Vector3 half_extents(radius, height / 2.0f, radius);
+	return AABB(-half_extents, half_extents * 2.0f);
+}
+
+String JoltCylinderShape3D::to_string() const {
+	return vformat("{height=%f radius=%f margin=%f}", height, radius, margin);
+}
diff --git a/modules/jolt_physics/shapes/jolt_cylinder_shape_3d.h b/modules/jolt_physics/shapes/jolt_cylinder_shape_3d.h
new file mode 100644
index 0000000000..889a038c74
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_cylinder_shape_3d.h
@@ -0,0 +1,58 @@
+/**************************************************************************/
+/*  jolt_cylinder_shape_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_CYLINDER_SHAPE_3D_H
+#define JOLT_CYLINDER_SHAPE_3D_H
+
+#include "jolt_shape_3d.h"
+
+class JoltCylinderShape3D final : public JoltShape3D {
+	float height = 0.0f;
+	float radius = 0.0f;
+	float margin = 0.04f;
+
+	virtual JPH::ShapeRefC _build() const override;
+
+public:
+	virtual ShapeType get_type() const override { return ShapeType::SHAPE_CYLINDER; }
+	virtual bool is_convex() const override { return true; }
+
+	virtual Variant get_data() const override;
+	virtual void set_data(const Variant &p_data) override;
+
+	virtual float get_margin() const override { return margin; }
+	virtual void set_margin(float p_margin) override;
+
+	virtual AABB get_aabb() const override;
+
+	String to_string() const;
+};
+
+#endif // JOLT_CYLINDER_SHAPE_3D_H
diff --git a/modules/jolt_physics/shapes/jolt_height_map_shape_3d.cpp b/modules/jolt_physics/shapes/jolt_height_map_shape_3d.cpp
new file mode 100644
index 0000000000..29672a7649
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_height_map_shape_3d.cpp
@@ -0,0 +1,233 @@
+/**************************************************************************/
+/*  jolt_height_map_shape_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_height_map_shape_3d.h"
+
+#include "../jolt_project_settings.h"
+#include "../misc/jolt_type_conversions.h"
+
+#include "Jolt/Physics/Collision/Shape/HeightFieldShape.h"
+#include "Jolt/Physics/Collision/Shape/MeshShape.h"
+
+namespace {
+
+bool _is_vertex_hole(const JPH::VertexList &p_vertices, int p_index) {
+	const float height = p_vertices[(size_t)p_index].y;
+	return height == FLT_MAX || Math::is_nan(height);
+}
+
+bool _is_triangle_hole(const JPH::VertexList &p_vertices, int p_index0, int p_index1, int p_index2) {
+	return _is_vertex_hole(p_vertices, p_index0) || _is_vertex_hole(p_vertices, p_index1) || _is_vertex_hole(p_vertices, p_index2);
+}
+
+} // namespace
+
+JPH::ShapeRefC JoltHeightMapShape3D::_build() const {
+	const int height_count = (int)heights.size();
+	if (unlikely(height_count == 0)) {
+		return nullptr;
+	}
+
+	ERR_FAIL_COND_V_MSG(height_count != width * depth, nullptr, vformat("Failed to build Jolt Physics height map shape with %s. Height count must be the product of width and depth. This shape belongs to %s.", to_string(), _owners_to_string()));
+	ERR_FAIL_COND_V_MSG(width < 2 || depth < 2, nullptr, vformat("Failed to build Jolt Physics height map shape with %s. The height map must be at least 2x2. This shape belongs to %s.", to_string(), _owners_to_string()));
+
+	if (width != depth) {
+		return JoltShape3D::with_double_sided(_build_mesh(), true);
+	}
+
+	const int block_size = 2; // Default of JPH::HeightFieldShapeSettings::mBlockSize
+	const int block_count = width / block_size;
+
+	if (block_count < 2) {
+		return JoltShape3D::with_double_sided(_build_mesh(), true);
+	}
+
+	return JoltShape3D::with_double_sided(_build_height_field(), true);
+}
+
+JPH::ShapeRefC JoltHeightMapShape3D::_build_height_field() const {
+	const int quad_count_x = width - 1;
+	const int quad_count_y = depth - 1;
+
+	const float offset_x = (float)-quad_count_x / 2.0f;
+	const float offset_y = (float)-quad_count_y / 2.0f;
+
+	// Jolt triangulates the height map differently from how Godot Physics does it, so we mirror the shape along the
+	// Z-axis to get the desired triangulation and reverse the rows to undo the mirroring.
+
+	LocalVector<float> heights_rev;
+	heights_rev.resize(heights.size());
+
+	const real_t *heights_ptr = heights.ptr();
+	float *heights_rev_ptr = heights_rev.ptr();
+
+	for (int z = 0; z < depth; ++z) {
+		const int z_rev = (depth - 1) - z;
+
+		const real_t *row = heights_ptr + ptrdiff_t(z * width);
+		float *row_rev = heights_rev_ptr + ptrdiff_t(z_rev * width);
+
+		for (int x = 0; x < width; ++x) {
+			const real_t height = row[x];
+
+			// Godot has undocumented (accidental?) support for holes by passing NaN as the height value, whereas Jolt
+			// uses `FLT_MAX` instead, so we translate any NaN to `FLT_MAX` in order to be drop-in compatible.
+			row_rev[x] = Math::is_nan(height) ? FLT_MAX : (float)height;
+		}
+	}
+
+	JPH::HeightFieldShapeSettings shape_settings(heights_rev.ptr(), JPH::Vec3(offset_x, 0, offset_y), JPH::Vec3::sReplicate(1.0f), (JPH::uint32)width);
+
+	shape_settings.mBitsPerSample = shape_settings.CalculateBitsPerSampleForError(0.0f);
+	shape_settings.mActiveEdgeCosThresholdAngle = JoltProjectSettings::get_active_edge_threshold();
+
+	const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create();
+	ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to build Jolt Physics height map shape with %s. It returned the following error: '%s'. This shape belongs to %s.", to_string(), to_godot(shape_result.GetError()), _owners_to_string()));
+
+	return with_scale(shape_result.Get(), Vector3(1, 1, -1));
+}
+
+JPH::ShapeRefC JoltHeightMapShape3D::_build_mesh() const {
+	const int height_count = (int)heights.size();
+
+	const int quad_count_x = width - 1;
+	const int quad_count_z = depth - 1;
+
+	const int quad_count = quad_count_x * quad_count_z;
+	const int triangle_count = quad_count * 2;
+
+	JPH::VertexList vertices;
+	vertices.reserve((size_t)height_count);
+
+	JPH::IndexedTriangleList indices;
+	indices.reserve((size_t)triangle_count);
+
+	const float offset_x = (float)-quad_count_x / 2.0f;
+	const float offset_z = (float)-quad_count_z / 2.0f;
+
+	for (int z = 0; z < depth; ++z) {
+		for (int x = 0; x < width; ++x) {
+			const float vertex_x = offset_x + (float)x;
+			const float vertex_y = (float)heights[z * width + x];
+			const float vertex_z = offset_z + (float)z;
+
+			vertices.emplace_back(vertex_x, vertex_y, vertex_z);
+		}
+	}
+
+	for (int z = 0; z < quad_count_z; ++z) {
+		for (int x = 0; x < quad_count_x; ++x) {
+			const int index_lower_right = z * width + x;
+			const int index_lower_left = z * width + (x + 1);
+			const int index_upper_right = (z + 1) * width + x;
+			const int index_upper_left = (z + 1) * width + (x + 1);
+
+			if (!_is_triangle_hole(vertices, index_lower_right, index_upper_right, index_lower_left)) {
+				indices.emplace_back(index_lower_right, index_upper_right, index_lower_left);
+			}
+
+			if (!_is_triangle_hole(vertices, index_lower_left, index_upper_right, index_upper_left)) {
+				indices.emplace_back(index_lower_left, index_upper_right, index_upper_left);
+			}
+		}
+	}
+
+	JPH::MeshShapeSettings shape_settings(std::move(vertices), std::move(indices));
+	shape_settings.mActiveEdgeCosThresholdAngle = JoltProjectSettings::get_active_edge_threshold();
+
+	const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create();
+	ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to build Jolt Physics height map shape (as polygon) with %s. It returned the following error: '%s'. This shape belongs to %s.", to_string(), to_godot(shape_result.GetError()), _owners_to_string()));
+
+	return shape_result.Get();
+}
+
+AABB JoltHeightMapShape3D::_calculate_aabb() const {
+	AABB result;
+
+	const int quad_count_x = width - 1;
+	const int quad_count_z = depth - 1;
+
+	const float offset_x = (float)-quad_count_x / 2.0f;
+	const float offset_z = (float)-quad_count_z / 2.0f;
+
+	for (int z = 0; z < depth; ++z) {
+		for (int x = 0; x < width; ++x) {
+			const Vector3 vertex(offset_x + (float)x, (float)heights[z * width + x], offset_z + (float)z);
+
+			if (x == 0 && z == 0) {
+				result.position = vertex;
+			} else {
+				result.expand_to(vertex);
+			}
+		}
+	}
+
+	return result;
+}
+
+Variant JoltHeightMapShape3D::get_data() const {
+	Dictionary data;
+	data["width"] = width;
+	data["depth"] = depth;
+	data["heights"] = heights;
+	return data;
+}
+
+void JoltHeightMapShape3D::set_data(const Variant &p_data) {
+	ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY);
+
+	const Dictionary data = p_data;
+
+	const Variant maybe_heights = data.get("heights", Variant());
+
+#ifdef REAL_T_IS_DOUBLE
+	ERR_FAIL_COND(maybe_heights.get_type() != Variant::PACKED_FLOAT64_ARRAY);
+#else
+	ERR_FAIL_COND(maybe_heights.get_type() != Variant::PACKED_FLOAT32_ARRAY);
+#endif
+
+	const Variant maybe_width = data.get("width", Variant());
+	ERR_FAIL_COND(maybe_width.get_type() != Variant::INT);
+
+	const Variant maybe_depth = data.get("depth", Variant());
+	ERR_FAIL_COND(maybe_depth.get_type() != Variant::INT);
+
+	heights = maybe_heights;
+	width = maybe_width;
+	depth = maybe_depth;
+
+	aabb = _calculate_aabb();
+
+	destroy();
+}
+
+String JoltHeightMapShape3D::to_string() const {
+	return vformat("{height_count=%d width=%d depth=%d}", heights.size(), width, depth);
+}
diff --git a/modules/jolt_physics/shapes/jolt_height_map_shape_3d.h b/modules/jolt_physics/shapes/jolt_height_map_shape_3d.h
new file mode 100644
index 0000000000..a929970f78
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_height_map_shape_3d.h
@@ -0,0 +1,69 @@
+/**************************************************************************/
+/*  jolt_height_map_shape_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_HEIGHT_MAP_SHAPE_3D_H
+#define JOLT_HEIGHT_MAP_SHAPE_3D_H
+
+#include "jolt_shape_3d.h"
+
+class JoltHeightMapShape3D final : public JoltShape3D {
+	AABB aabb;
+
+#ifdef REAL_T_IS_DOUBLE
+	PackedFloat64Array heights;
+#else
+	PackedFloat32Array heights;
+#endif
+
+	int width = 0;
+	int depth = 0;
+
+	virtual JPH::ShapeRefC _build() const override;
+	JPH::ShapeRefC _build_height_field() const;
+	JPH::ShapeRefC _build_mesh() const;
+
+	AABB _calculate_aabb() const;
+
+public:
+	virtual ShapeType get_type() const override { return ShapeType::SHAPE_HEIGHTMAP; }
+	virtual bool is_convex() const override { return false; }
+
+	virtual Variant get_data() const override;
+	virtual void set_data(const Variant &p_data) override;
+
+	virtual float get_margin() const override { return 0.0f; }
+	virtual void set_margin(float p_margin) override {}
+
+	virtual AABB get_aabb() const override { return aabb; }
+
+	String to_string() const;
+};
+
+#endif // JOLT_HEIGHT_MAP_SHAPE_3D_H
diff --git a/modules/jolt_physics/shapes/jolt_separation_ray_shape_3d.cpp b/modules/jolt_physics/shapes/jolt_separation_ray_shape_3d.cpp
new file mode 100644
index 0000000000..95c18429e7
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_separation_ray_shape_3d.cpp
@@ -0,0 +1,85 @@
+/**************************************************************************/
+/*  jolt_separation_ray_shape_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_separation_ray_shape_3d.h"
+
+#include "../misc/jolt_type_conversions.h"
+#include "jolt_custom_ray_shape.h"
+
+JPH::ShapeRefC JoltSeparationRayShape3D::_build() const {
+	ERR_FAIL_COND_V_MSG(length <= 0.0f, nullptr, vformat("Failed to build Jolt Physics separation ray shape with %s. Its length must be greater than 0. This shape belongs to %s.", to_string(), _owners_to_string()));
+
+	const JoltCustomRayShapeSettings shape_settings(length, slide_on_slope);
+	const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create();
+	ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to build Jolt Physics separation ray shape with %s. It returned the following error: '%s'. This shape belongs to %s.", to_string(), to_godot(shape_result.GetError()), _owners_to_string()));
+
+	return shape_result.Get();
+}
+
+Variant JoltSeparationRayShape3D::get_data() const {
+	Dictionary data;
+	data["length"] = length;
+	data["slide_on_slope"] = slide_on_slope;
+	return data;
+}
+
+void JoltSeparationRayShape3D::set_data(const Variant &p_data) {
+	ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY);
+
+	const Dictionary data = p_data;
+
+	const Variant maybe_length = data.get("length", Variant());
+	ERR_FAIL_COND(maybe_length.get_type() != Variant::FLOAT);
+
+	const Variant maybe_slide_on_slope = data.get("slide_on_slope", Variant());
+	ERR_FAIL_COND(maybe_slide_on_slope.get_type() != Variant::BOOL);
+
+	const float new_length = maybe_length;
+	const bool new_slide_on_slope = maybe_slide_on_slope;
+
+	if (unlikely(new_length == length && new_slide_on_slope == slide_on_slope)) {
+		return;
+	}
+
+	length = new_length;
+	slide_on_slope = new_slide_on_slope;
+
+	destroy();
+}
+
+AABB JoltSeparationRayShape3D::get_aabb() const {
+	constexpr float size_xy = 0.1f;
+	constexpr float half_size_xy = size_xy / 2.0f;
+	return AABB(Vector3(-half_size_xy, -half_size_xy, 0.0f), Vector3(size_xy, size_xy, length));
+}
+
+String JoltSeparationRayShape3D::to_string() const {
+	return vformat("{length=%f slide_on_slope=%s}", length, slide_on_slope);
+}
diff --git a/modules/jolt_physics/shapes/jolt_separation_ray_shape_3d.h b/modules/jolt_physics/shapes/jolt_separation_ray_shape_3d.h
new file mode 100644
index 0000000000..b858d84feb
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_separation_ray_shape_3d.h
@@ -0,0 +1,57 @@
+/**************************************************************************/
+/*  jolt_separation_ray_shape_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_SEPARATION_RAY_SHAPE_3D_H
+#define JOLT_SEPARATION_RAY_SHAPE_3D_H
+
+#include "jolt_shape_3d.h"
+
+class JoltSeparationRayShape3D final : public JoltShape3D {
+	float length = 0.0f;
+	bool slide_on_slope = false;
+
+	virtual JPH::ShapeRefC _build() const override;
+
+public:
+	virtual ShapeType get_type() const override { return ShapeType::SHAPE_SEPARATION_RAY; }
+	virtual bool is_convex() const override { return true; }
+
+	virtual Variant get_data() const override;
+	virtual void set_data(const Variant &p_data) override;
+
+	virtual float get_margin() const override { return 0.0f; }
+	virtual void set_margin(float p_margin) override {}
+
+	virtual AABB get_aabb() const override;
+
+	String to_string() const;
+};
+
+#endif // JOLT_SEPARATION_RAY_SHAPE_3D_H
diff --git a/modules/jolt_physics/shapes/jolt_shape_3d.cpp b/modules/jolt_physics/shapes/jolt_shape_3d.cpp
new file mode 100644
index 0000000000..8bd1977bac
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_shape_3d.cpp
@@ -0,0 +1,278 @@
+/**************************************************************************/
+/*  jolt_shape_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_shape_3d.h"
+
+#include "../misc/jolt_type_conversions.h"
+#include "../objects/jolt_shaped_object_3d.h"
+#include "jolt_custom_double_sided_shape.h"
+#include "jolt_custom_user_data_shape.h"
+
+#include "Jolt/Physics/Collision/Shape/MutableCompoundShape.h"
+#include "Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h"
+#include "Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h"
+#include "Jolt/Physics/Collision/Shape/ScaledShape.h"
+#include "Jolt/Physics/Collision/Shape/SphereShape.h"
+#include "Jolt/Physics/Collision/Shape/StaticCompoundShape.h"
+
+namespace {
+
+constexpr float DEFAULT_SOLVER_BIAS = 0.0;
+
+} // namespace
+
+String JoltShape3D::_owners_to_string() const {
+	const int owner_count = ref_counts_by_owner.size();
+
+	if (owner_count == 0) {
+		return "'<unknown>' and 0 other object(s)";
+	}
+
+	const JoltShapedObject3D &random_owner = *ref_counts_by_owner.begin()->key;
+
+	return vformat("'%s' and %d other object(s)", random_owner.to_string(), owner_count - 1);
+}
+
+JoltShape3D::~JoltShape3D() = default;
+
+void JoltShape3D::add_owner(JoltShapedObject3D *p_owner) {
+	ref_counts_by_owner[p_owner]++;
+}
+
+void JoltShape3D::remove_owner(JoltShapedObject3D *p_owner) {
+	if (--ref_counts_by_owner[p_owner] <= 0) {
+		ref_counts_by_owner.erase(p_owner);
+	}
+}
+
+void JoltShape3D::remove_self() {
+	// `remove_owner` will be called when we `remove_shape`, so we need to copy the map since the
+	// iterator would be invalidated from underneath us.
+	const HashMap<JoltShapedObject3D *, int> ref_counts_by_owner_copy = ref_counts_by_owner;
+
+	for (const KeyValue<JoltShapedObject3D *, int> &E : ref_counts_by_owner_copy) {
+		E.key->remove_shape(this);
+	}
+}
+
+float JoltShape3D::get_solver_bias() const {
+	return DEFAULT_SOLVER_BIAS;
+}
+
+void JoltShape3D::set_solver_bias(float p_bias) {
+	if (!Math::is_equal_approx(p_bias, DEFAULT_SOLVER_BIAS)) {
+		WARN_PRINT(vformat("Custom solver bias for shapes is not supported when using Jolt Physics. Any such value will be ignored. This shape belongs to %s.", _owners_to_string()));
+	}
+}
+
+JPH::ShapeRefC JoltShape3D::try_build() {
+	jolt_ref_mutex.lock();
+
+	if (jolt_ref == nullptr) {
+		jolt_ref = _build();
+	}
+
+	jolt_ref_mutex.unlock();
+
+	return jolt_ref;
+}
+
+void JoltShape3D::destroy() {
+	jolt_ref_mutex.lock();
+	jolt_ref = nullptr;
+	jolt_ref_mutex.unlock();
+
+	for (const KeyValue<JoltShapedObject3D *, int> &E : ref_counts_by_owner) {
+		E.key->_shapes_changed();
+	}
+}
+
+JPH::ShapeRefC JoltShape3D::with_scale(const JPH::Shape *p_shape, const Vector3 &p_scale) {
+	ERR_FAIL_NULL_V(p_shape, nullptr);
+
+	const JPH::ScaledShapeSettings shape_settings(p_shape, to_jolt(p_scale));
+	const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create();
+	ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to scale shape with {scale=%v}. It returned the following error: '%s'.", p_scale, to_godot(shape_result.GetError())));
+
+	return shape_result.Get();
+}
+
+JPH::ShapeRefC JoltShape3D::with_basis_origin(const JPH::Shape *p_shape, const Basis &p_basis, const Vector3 &p_origin) {
+	ERR_FAIL_NULL_V(p_shape, nullptr);
+
+	const JPH::RotatedTranslatedShapeSettings shape_settings(to_jolt(p_origin), to_jolt(p_basis), p_shape);
+
+	const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create();
+	ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to offset shape with {basis=%s origin=%v}. It returned the following error: '%s'.", p_basis, p_origin, to_godot(shape_result.GetError())));
+
+	return shape_result.Get();
+}
+
+JPH::ShapeRefC JoltShape3D::with_center_of_mass_offset(const JPH::Shape *p_shape, const Vector3 &p_offset) {
+	ERR_FAIL_NULL_V(p_shape, nullptr);
+
+	const JPH::OffsetCenterOfMassShapeSettings shape_settings(to_jolt(p_offset), p_shape);
+	const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create();
+	ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to offset center of mass with {offset=%v}. It returned the following error: '%s'.", p_offset, to_godot(shape_result.GetError())));
+
+	return shape_result.Get();
+}
+
+JPH::ShapeRefC JoltShape3D::with_center_of_mass(const JPH::Shape *p_shape, const Vector3 &p_center_of_mass) {
+	ERR_FAIL_NULL_V(p_shape, nullptr);
+
+	const Vector3 center_of_mass_inner = to_godot(p_shape->GetCenterOfMass());
+	const Vector3 center_of_mass_offset = p_center_of_mass - center_of_mass_inner;
+
+	if (center_of_mass_offset == Vector3()) {
+		return p_shape;
+	}
+
+	return with_center_of_mass_offset(p_shape, center_of_mass_offset);
+}
+
+JPH::ShapeRefC JoltShape3D::with_user_data(const JPH::Shape *p_shape, uint64_t p_user_data) {
+	JoltCustomUserDataShapeSettings shape_settings(p_shape);
+	shape_settings.mUserData = (JPH::uint64)p_user_data;
+
+	const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create();
+	ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to override user data. It returned the following error: '%s'.", to_godot(shape_result.GetError())));
+
+	return shape_result.Get();
+}
+
+JPH::ShapeRefC JoltShape3D::with_double_sided(const JPH::Shape *p_shape, bool p_back_face_collision) {
+	ERR_FAIL_NULL_V(p_shape, nullptr);
+
+	const JoltCustomDoubleSidedShapeSettings shape_settings(p_shape, p_back_face_collision);
+	const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create();
+	ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to make shape double-sided. It returned the following error: '%s'.", to_godot(shape_result.GetError())));
+
+	return shape_result.Get();
+}
+
+JPH::ShapeRefC JoltShape3D::without_custom_shapes(const JPH::Shape *p_shape) {
+	switch (p_shape->GetSubType()) {
+		case JoltCustomShapeSubType::RAY:
+		case JoltCustomShapeSubType::MOTION: {
+			// Replace unsupported shapes with a small sphere.
+			return new JPH::SphereShape(0.1f);
+		}
+
+		case JoltCustomShapeSubType::OVERRIDE_USER_DATA:
+		case JoltCustomShapeSubType::DOUBLE_SIDED: {
+			const JPH::DecoratedShape *shape = static_cast<const JPH::DecoratedShape *>(p_shape);
+
+			// Replace unsupported decorator shapes with the inner shape.
+			return without_custom_shapes(shape->GetInnerShape());
+		}
+
+		case JPH::EShapeSubType::StaticCompound: {
+			const JPH::StaticCompoundShape *shape = static_cast<const JPH::StaticCompoundShape *>(p_shape);
+
+			JPH::StaticCompoundShapeSettings settings;
+
+			for (const JPH::CompoundShape::SubShape &sub_shape : shape->GetSubShapes()) {
+				settings.AddShape(shape->GetCenterOfMass() + sub_shape.GetPositionCOM() - sub_shape.GetRotation() * sub_shape.mShape->GetCenterOfMass(), sub_shape.GetRotation(), without_custom_shapes(sub_shape.mShape));
+			}
+
+			const JPH::ShapeSettings::ShapeResult shape_result = settings.Create();
+			ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to recreate static compound shape during filtering of custom shapes. It returned the following error: '%s'.", to_godot(shape_result.GetError())));
+
+			return shape_result.Get();
+		}
+
+		case JPH::EShapeSubType::MutableCompound: {
+			const JPH::MutableCompoundShape *shape = static_cast<const JPH::MutableCompoundShape *>(p_shape);
+
+			JPH::MutableCompoundShapeSettings settings;
+
+			for (const JPH::MutableCompoundShape::SubShape &sub_shape : shape->GetSubShapes()) {
+				settings.AddShape(shape->GetCenterOfMass() + sub_shape.GetPositionCOM() - sub_shape.GetRotation() * sub_shape.mShape->GetCenterOfMass(), sub_shape.GetRotation(), without_custom_shapes(sub_shape.mShape));
+			}
+
+			const JPH::ShapeSettings::ShapeResult shape_result = settings.Create();
+			ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to recreate mutable compound shape during filtering of custom shapes. It returned the following error: '%s'.", to_godot(shape_result.GetError())));
+
+			return shape_result.Get();
+		}
+
+		case JPH::EShapeSubType::RotatedTranslated: {
+			const JPH::RotatedTranslatedShape *shape = static_cast<const JPH::RotatedTranslatedShape *>(p_shape);
+
+			const JPH::Shape *inner_shape = shape->GetInnerShape();
+			const JPH::ShapeRefC new_inner_shape = without_custom_shapes(inner_shape);
+
+			if (inner_shape == new_inner_shape) {
+				return p_shape;
+			}
+
+			return new JPH::RotatedTranslatedShape(shape->GetPosition(), shape->GetRotation(), new_inner_shape);
+		}
+
+		case JPH::EShapeSubType::Scaled: {
+			const JPH::ScaledShape *shape = static_cast<const JPH::ScaledShape *>(p_shape);
+
+			const JPH::Shape *inner_shape = shape->GetInnerShape();
+			const JPH::ShapeRefC new_inner_shape = without_custom_shapes(inner_shape);
+
+			if (inner_shape == new_inner_shape) {
+				return p_shape;
+			}
+
+			return new JPH::ScaledShape(new_inner_shape, shape->GetScale());
+		}
+
+		case JPH::EShapeSubType::OffsetCenterOfMass: {
+			const JPH::OffsetCenterOfMassShape *shape = static_cast<const JPH::OffsetCenterOfMassShape *>(p_shape);
+
+			const JPH::Shape *inner_shape = shape->GetInnerShape();
+			const JPH::ShapeRefC new_inner_shape = without_custom_shapes(inner_shape);
+
+			if (inner_shape == new_inner_shape) {
+				return p_shape;
+			}
+
+			return new JPH::OffsetCenterOfMassShape(new_inner_shape, shape->GetOffset());
+		}
+
+		default: {
+			return p_shape;
+		}
+	}
+}
+
+Vector3 JoltShape3D::make_scale_valid(const JPH::Shape *p_shape, const Vector3 &p_scale) {
+	return to_godot(p_shape->MakeScaleValid(to_jolt(p_scale)));
+}
+
+bool JoltShape3D::is_scale_valid(const Vector3 &p_scale, const Vector3 &p_valid_scale, real_t p_tolerance) {
+	return Math::is_equal_approx(p_scale.x, p_valid_scale.x, p_tolerance) && Math::is_equal_approx(p_scale.y, p_valid_scale.y, p_tolerance) && Math::is_equal_approx(p_scale.z, p_valid_scale.z, p_tolerance);
+}
diff --git a/modules/jolt_physics/shapes/jolt_shape_3d.h b/modules/jolt_physics/shapes/jolt_shape_3d.h
new file mode 100644
index 0000000000..a7645ec6c6
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_shape_3d.h
@@ -0,0 +1,136 @@
+/**************************************************************************/
+/*  jolt_shape_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_SHAPE_3D_H
+#define JOLT_SHAPE_3D_H
+
+#include "servers/physics_server_3d.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Collision/Shape/Shape.h"
+
+class JoltShapedObject3D;
+
+class JoltShape3D {
+protected:
+	HashMap<JoltShapedObject3D *, int> ref_counts_by_owner;
+	Mutex jolt_ref_mutex;
+	RID rid;
+	JPH::ShapeRefC jolt_ref;
+
+	virtual JPH::ShapeRefC _build() const = 0;
+
+	String _owners_to_string() const;
+
+public:
+	typedef PhysicsServer3D::ShapeType ShapeType;
+
+	virtual ~JoltShape3D() = 0;
+
+	RID get_rid() const { return rid; }
+	void set_rid(const RID &p_rid) { rid = p_rid; }
+
+	void add_owner(JoltShapedObject3D *p_owner);
+	void remove_owner(JoltShapedObject3D *p_owner);
+	void remove_self();
+
+	virtual ShapeType get_type() const = 0;
+	virtual bool is_convex() const = 0;
+
+	virtual Variant get_data() const = 0;
+	virtual void set_data(const Variant &p_data) = 0;
+
+	virtual float get_margin() const = 0;
+	virtual void set_margin(float p_margin) = 0;
+
+	virtual AABB get_aabb() const = 0;
+
+	float get_solver_bias() const;
+	void set_solver_bias(float p_bias);
+
+	JPH::ShapeRefC try_build();
+
+	void destroy();
+
+	const JPH::Shape *get_jolt_ref() const { return jolt_ref; }
+
+	static JPH::ShapeRefC with_scale(const JPH::Shape *p_shape, const Vector3 &p_scale);
+	static JPH::ShapeRefC with_basis_origin(const JPH::Shape *p_shape, const Basis &p_basis, const Vector3 &p_origin);
+	static JPH::ShapeRefC with_center_of_mass_offset(const JPH::Shape *p_shape, const Vector3 &p_offset);
+	static JPH::ShapeRefC with_center_of_mass(const JPH::Shape *p_shape, const Vector3 &p_center_of_mass);
+	static JPH::ShapeRefC with_user_data(const JPH::Shape *p_shape, uint64_t p_user_data);
+	static JPH::ShapeRefC with_double_sided(const JPH::Shape *p_shape, bool p_back_face_collision);
+	static JPH::ShapeRefC without_custom_shapes(const JPH::Shape *p_shape);
+
+	static Vector3 make_scale_valid(const JPH::Shape *p_shape, const Vector3 &p_scale);
+	static bool is_scale_valid(const Vector3 &p_scale, const Vector3 &p_valid_scale, real_t p_tolerance = 0.01f);
+};
+
+#ifdef DEBUG_ENABLED
+
+#define JOLT_ENSURE_SCALE_NOT_ZERO(m_transform, m_msg)                                                         \
+	if (unlikely((m_transform).basis.determinant() == 0.0f)) {                                                 \
+		WARN_PRINT(vformat("%s "                                                                               \
+						   "The basis of the transform was singular, which is not supported by Jolt Physics. " \
+						   "This is likely caused by one or more axes having a scale of zero. "                \
+						   "The basis (and thus its scale) will be treated as identity.",                      \
+				m_msg));                                                                                       \
+                                                                                                               \
+		(m_transform).basis = Basis();                                                                         \
+	} else                                                                                                     \
+		((void)0)
+
+#define ERR_PRINT_INVALID_SCALE_MSG(m_scale, m_valid_scale, m_msg)                               \
+	if (unlikely(!JoltShape3D::is_scale_valid(m_scale, valid_scale))) {                          \
+		ERR_PRINT(vformat("%s "                                                                  \
+						  "A scale of %v is not supported by Jolt Physics for this shape/body. " \
+						  "The scale will instead be treated as %v.",                            \
+				m_msg, m_scale, valid_scale));                                                   \
+	} else                                                                                       \
+		((void)0)
+
+#else
+
+#define JOLT_ENSURE_SCALE_NOT_ZERO(m_transform, m_msg)
+
+#define ERR_PRINT_INVALID_SCALE_MSG(m_scale, m_valid_scale, m_msg)
+
+#endif
+
+#define JOLT_ENSURE_SCALE_VALID(m_shape, m_scale, m_msg)                             \
+	if (true) {                                                                      \
+		const Vector3 valid_scale = JoltShape3D::make_scale_valid(m_shape, m_scale); \
+		ERR_PRINT_INVALID_SCALE_MSG(m_scale, valid_scale, m_msg);                    \
+		(m_scale) = valid_scale;                                                     \
+	} else                                                                           \
+		((void)0)
+
+#endif // JOLT_SHAPE_3D_H
diff --git a/modules/jolt_physics/shapes/jolt_shape_instance_3d.cpp b/modules/jolt_physics/shapes/jolt_shape_instance_3d.cpp
new file mode 100644
index 0000000000..95b763680b
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_shape_instance_3d.cpp
@@ -0,0 +1,106 @@
+/**************************************************************************/
+/*  jolt_shape_instance_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_shape_instance_3d.h"
+
+#include "jolt_shape_3d.h"
+
+#include "Jolt/Physics/Collision/Shape/DecoratedShape.h"
+
+JoltShapeInstance3D::ShapeReference::ShapeReference(JoltShapedObject3D *p_parent, JoltShape3D *p_shape) :
+		parent(p_parent),
+		shape(p_shape) {
+	if (shape != nullptr) {
+		shape->add_owner(parent);
+	}
+}
+
+JoltShapeInstance3D::ShapeReference::ShapeReference(const ShapeReference &p_other) :
+		parent(p_other.parent),
+		shape(p_other.shape) {
+	if (shape != nullptr) {
+		shape->add_owner(parent);
+	}
+}
+
+JoltShapeInstance3D::ShapeReference::~ShapeReference() {
+	if (shape != nullptr) {
+		shape->remove_owner(parent);
+	}
+}
+
+JoltShapeInstance3D::ShapeReference &JoltShapeInstance3D::ShapeReference::operator=(const ShapeReference &p_other) {
+	if (shape != nullptr) {
+		shape->remove_owner(parent);
+	}
+
+	parent = p_other.parent;
+	shape = p_other.shape;
+
+	if (shape != nullptr) {
+		shape->add_owner(parent);
+	}
+
+	return *this;
+}
+
+JoltShapeInstance3D::JoltShapeInstance3D(JoltShapedObject3D *p_parent, JoltShape3D *p_shape, const Transform3D &p_transform, const Vector3 &p_scale, bool p_disabled) :
+		transform(p_transform),
+		scale(p_scale),
+		shape(p_parent, p_shape),
+		disabled(p_disabled) {
+}
+
+AABB JoltShapeInstance3D::get_aabb() const {
+	return get_transform_scaled().xform(shape->get_aabb());
+}
+
+bool JoltShapeInstance3D::try_build() {
+	ERR_FAIL_COND_V(is_disabled(), false);
+
+	const JPH::ShapeRefC maybe_new_shape = shape->try_build();
+
+	if (maybe_new_shape == nullptr) {
+		jolt_ref = nullptr;
+		return false;
+	}
+
+	if (jolt_ref != nullptr) {
+		const JPH::DecoratedShape *outer_shape = static_cast<const JPH::DecoratedShape *>(jolt_ref.GetPtr());
+
+		if (outer_shape->GetInnerShape() == maybe_new_shape) {
+			return true;
+		}
+	}
+
+	jolt_ref = JoltShape3D::with_user_data(maybe_new_shape, (uint64_t)id);
+
+	return true;
+}
diff --git a/modules/jolt_physics/shapes/jolt_shape_instance_3d.h b/modules/jolt_physics/shapes/jolt_shape_instance_3d.h
new file mode 100644
index 0000000000..0a24d06a90
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_shape_instance_3d.h
@@ -0,0 +1,103 @@
+/**************************************************************************/
+/*  jolt_shape_instance_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_SHAPE_INSTANCE_3D_H
+#define JOLT_SHAPE_INSTANCE_3D_H
+
+#include "core/math/transform_3d.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Collision/Shape/Shape.h"
+
+class JoltShapedObject3D;
+class JoltShape3D;
+
+class JoltShapeInstance3D {
+	// This RAII helper exists solely to avoid needing to maintain copy construction/assignment in the shape instance.
+	// Ideally this would be move-only instead, but Godot's containers don't support that at the moment.
+	struct ShapeReference {
+		JoltShapedObject3D *parent = nullptr;
+		JoltShape3D *shape = nullptr;
+
+		ShapeReference() = default;
+		ShapeReference(JoltShapedObject3D *p_parent, JoltShape3D *p_shape);
+		ShapeReference(const ShapeReference &p_other);
+		ShapeReference(ShapeReference &&p_other) = delete;
+		~ShapeReference();
+
+		ShapeReference &operator=(const ShapeReference &p_other);
+		ShapeReference &operator=(ShapeReference &&p_other) = delete;
+
+		JoltShape3D *operator*() const { return shape; }
+		JoltShape3D *operator->() const { return shape; }
+		operator JoltShape3D *() const { return shape; }
+	};
+
+	inline static uint32_t next_id = 1;
+
+	Transform3D transform;
+	Vector3 scale;
+	ShapeReference shape;
+	JPH::ShapeRefC jolt_ref;
+	uint32_t id = next_id++;
+	bool disabled = false;
+
+public:
+	JoltShapeInstance3D() = default;
+	JoltShapeInstance3D(JoltShapedObject3D *p_parent, JoltShape3D *p_shape, const Transform3D &p_transform = Transform3D(), const Vector3 &p_scale = Vector3(1.0f, 1.0f, 1.0f), bool p_disabled = false);
+
+	uint32_t get_id() const { return id; }
+
+	JoltShape3D *get_shape() const { return shape; }
+
+	const JPH::Shape *get_jolt_ref() const { return jolt_ref; }
+
+	const Transform3D &get_transform_unscaled() const { return transform; }
+	Transform3D get_transform_scaled() const { return transform.scaled_local(scale); }
+	void set_transform(const Transform3D &p_transform) { transform = p_transform; }
+
+	const Vector3 &get_scale() const { return scale; }
+	void set_scale(const Vector3 &p_scale) { scale = p_scale; }
+
+	AABB get_aabb() const;
+
+	bool is_built() const { return jolt_ref != nullptr; }
+
+	bool is_enabled() const { return !disabled; }
+	bool is_disabled() const { return disabled; }
+
+	void enable() { disabled = false; }
+	void disable() { disabled = true; }
+
+	bool try_build();
+};
+
+#endif // JOLT_SHAPE_INSTANCE_3D_H
diff --git a/modules/jolt_physics/shapes/jolt_sphere_shape_3d.cpp b/modules/jolt_physics/shapes/jolt_sphere_shape_3d.cpp
new file mode 100644
index 0000000000..cc0ffd927a
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_sphere_shape_3d.cpp
@@ -0,0 +1,71 @@
+/**************************************************************************/
+/*  jolt_sphere_shape_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_sphere_shape_3d.h"
+
+#include "../misc/jolt_type_conversions.h"
+
+#include "Jolt/Physics/Collision/Shape/SphereShape.h"
+
+JPH::ShapeRefC JoltSphereShape3D::_build() const {
+	ERR_FAIL_COND_V_MSG(radius <= 0.0f, nullptr, vformat("Failed to build Jolt Physics sphere shape with %s. Its radius must be greater than 0. This shape belongs to %s.", to_string(), _owners_to_string()));
+
+	const JPH::SphereShapeSettings shape_settings(radius);
+	const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create();
+	ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to build Jolt Physics sphere shape with %s. It returned the following error: '%s'. This shape belongs to %s.", to_string(), to_godot(shape_result.GetError()), _owners_to_string()));
+
+	return shape_result.Get();
+}
+
+Variant JoltSphereShape3D::get_data() const {
+	return radius;
+}
+
+void JoltSphereShape3D::set_data(const Variant &p_data) {
+	ERR_FAIL_COND(p_data.get_type() != Variant::FLOAT);
+
+	const float new_radius = p_data;
+	if (unlikely(new_radius == radius)) {
+		return;
+	}
+
+	radius = new_radius;
+
+	destroy();
+}
+
+AABB JoltSphereShape3D::get_aabb() const {
+	const Vector3 half_extents(radius, radius, radius);
+	return AABB(-half_extents, half_extents * 2.0f);
+}
+
+String JoltSphereShape3D::to_string() const {
+	return vformat("{radius=%f}", radius);
+}
diff --git a/modules/jolt_physics/shapes/jolt_sphere_shape_3d.h b/modules/jolt_physics/shapes/jolt_sphere_shape_3d.h
new file mode 100644
index 0000000000..4127f6a5f9
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_sphere_shape_3d.h
@@ -0,0 +1,56 @@
+/**************************************************************************/
+/*  jolt_sphere_shape_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_SPHERE_SHAPE_3D_H
+#define JOLT_SPHERE_SHAPE_3D_H
+
+#include "jolt_shape_3d.h"
+
+class JoltSphereShape3D final : public JoltShape3D {
+	float radius = 0.0f;
+
+	virtual JPH::ShapeRefC _build() const override;
+
+public:
+	virtual ShapeType get_type() const override { return ShapeType::SHAPE_SPHERE; }
+	virtual bool is_convex() const override { return true; }
+
+	virtual Variant get_data() const override;
+	virtual void set_data(const Variant &p_data) override;
+
+	virtual float get_margin() const override { return 0.0f; }
+	virtual void set_margin(float p_margin) override {}
+
+	virtual AABB get_aabb() const override;
+
+	String to_string() const;
+};
+
+#endif // JOLT_SPHERE_SHAPE_3D_H
diff --git a/modules/jolt_physics/shapes/jolt_world_boundary_shape_3d.cpp b/modules/jolt_physics/shapes/jolt_world_boundary_shape_3d.cpp
new file mode 100644
index 0000000000..7d9b07536a
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_world_boundary_shape_3d.cpp
@@ -0,0 +1,75 @@
+/**************************************************************************/
+/*  jolt_world_boundary_shape_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_world_boundary_shape_3d.h"
+
+#include "../jolt_project_settings.h"
+#include "../misc/jolt_type_conversions.h"
+
+#include "Jolt/Physics/Collision/Shape/PlaneShape.h"
+
+JPH::ShapeRefC JoltWorldBoundaryShape3D::_build() const {
+	const Plane normalized_plane = plane.normalized();
+	ERR_FAIL_COND_V_MSG(normalized_plane == Plane(), nullptr, vformat("Failed to build Jolt Physics world boundary shape with %s. The plane's normal must not be zero. This shape belongs to %s.", to_string(), _owners_to_string()));
+
+	const float half_size = JoltProjectSettings::get_world_boundary_shape_size() / 2.0f;
+	const JPH::PlaneShapeSettings shape_settings(to_jolt(normalized_plane), nullptr, half_size);
+	const JPH::ShapeSettings::ShapeResult shape_result = shape_settings.Create();
+	ERR_FAIL_COND_V_MSG(shape_result.HasError(), nullptr, vformat("Failed to build Jolt Physics world boundary shape with %s. It returned the following error: '%s'. This shape belongs to %s.", to_string(), to_godot(shape_result.GetError()), _owners_to_string()));
+
+	return shape_result.Get();
+}
+
+Variant JoltWorldBoundaryShape3D::get_data() const {
+	return plane;
+}
+
+void JoltWorldBoundaryShape3D::set_data(const Variant &p_data) {
+	ERR_FAIL_COND(p_data.get_type() != Variant::PLANE);
+
+	const Plane new_plane = p_data;
+	if (unlikely(new_plane == plane)) {
+		return;
+	}
+
+	plane = p_data;
+
+	destroy();
+}
+
+AABB JoltWorldBoundaryShape3D::get_aabb() const {
+	const float size = JoltProjectSettings::get_world_boundary_shape_size();
+	const float half_size = size / 2.0f;
+	return AABB(Vector3(-half_size, -half_size, -half_size), Vector3(size, half_size, size));
+}
+
+String JoltWorldBoundaryShape3D::to_string() const {
+	return vformat("{plane=%s}", plane);
+}
diff --git a/modules/jolt_physics/shapes/jolt_world_boundary_shape_3d.h b/modules/jolt_physics/shapes/jolt_world_boundary_shape_3d.h
new file mode 100644
index 0000000000..2d22c0f5f5
--- /dev/null
+++ b/modules/jolt_physics/shapes/jolt_world_boundary_shape_3d.h
@@ -0,0 +1,56 @@
+/**************************************************************************/
+/*  jolt_world_boundary_shape_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_WORLD_BOUNDARY_SHAPE_3D_H
+#define JOLT_WORLD_BOUNDARY_SHAPE_3D_H
+
+#include "jolt_shape_3d.h"
+
+class JoltWorldBoundaryShape3D final : public JoltShape3D {
+	Plane plane;
+
+	virtual JPH::ShapeRefC _build() const override;
+
+public:
+	virtual ShapeType get_type() const override { return ShapeType::SHAPE_WORLD_BOUNDARY; }
+	virtual bool is_convex() const override { return false; }
+
+	virtual Variant get_data() const override;
+	virtual void set_data(const Variant &p_data) override;
+
+	virtual float get_margin() const override { return 0.0f; }
+	virtual void set_margin(float p_margin) override {}
+
+	virtual AABB get_aabb() const override;
+
+	String to_string() const;
+};
+
+#endif // JOLT_WORLD_BOUNDARY_SHAPE_3D_H
diff --git a/modules/jolt_physics/spaces/jolt_body_accessor_3d.cpp b/modules/jolt_physics/spaces/jolt_body_accessor_3d.cpp
new file mode 100644
index 0000000000..e9d40dd2ee
--- /dev/null
+++ b/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);
+}
diff --git a/modules/jolt_physics/spaces/jolt_body_accessor_3d.h b/modules/jolt_physics/spaces/jolt_body_accessor_3d.h
new file mode 100644
index 0000000000..f1a8896266
--- /dev/null
+++ b/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
diff --git a/modules/jolt_physics/spaces/jolt_broad_phase_layer.h b/modules/jolt_physics/spaces/jolt_broad_phase_layer.h
new file mode 100644
index 0000000000..8cc115e1d4
--- /dev/null
+++ b/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
diff --git a/modules/jolt_physics/spaces/jolt_contact_listener_3d.cpp b/modules/jolt_physics/spaces/jolt_contact_listener_3d.cpp
new file mode 100644
index 0000000000..21fb76464b
--- /dev/null
+++ b/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();
+}
diff --git a/modules/jolt_physics/spaces/jolt_contact_listener_3d.h b/modules/jolt_physics/spaces/jolt_contact_listener_3d.h
new file mode 100644
index 0000000000..26136abd83
--- /dev/null
+++ b/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
diff --git a/modules/jolt_physics/spaces/jolt_job_system.cpp b/modules/jolt_physics/spaces/jolt_job_system.cpp
new file mode 100644
index 0000000000..2022c12160
--- /dev/null
+++ b/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
diff --git a/modules/jolt_physics/spaces/jolt_job_system.h b/modules/jolt_physics/spaces/jolt_job_system.h
new file mode 100644
index 0000000000..573a8b1741
--- /dev/null
+++ b/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
diff --git a/modules/jolt_physics/spaces/jolt_layers.cpp b/modules/jolt_physics/spaces/jolt_layers.cpp
new file mode 100644
index 0000000000..c39c3a3397
--- /dev/null
+++ b/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);
+}
diff --git a/modules/jolt_physics/spaces/jolt_layers.h b/modules/jolt_physics/spaces/jolt_layers.h
new file mode 100644
index 0000000000..f610477a66
--- /dev/null
+++ b/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
diff --git a/modules/jolt_physics/spaces/jolt_motion_filter_3d.cpp b/modules/jolt_physics/spaces/jolt_motion_filter_3d.cpp
new file mode 100644
index 0000000000..f6bd42ac44
--- /dev/null
+++ b/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;
+}
diff --git a/modules/jolt_physics/spaces/jolt_motion_filter_3d.h b/modules/jolt_physics/spaces/jolt_motion_filter_3d.h
new file mode 100644
index 0000000000..1a3bd6379f
--- /dev/null
+++ b/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
diff --git a/modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.cpp b/modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.cpp
new file mode 100644
index 0000000000..fc1c125556
--- /dev/null
+++ b/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;
+}
diff --git a/modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.h b/modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.h
new file mode 100644
index 0000000000..ced8d8dd5a
--- /dev/null
+++ b/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
diff --git a/modules/jolt_physics/spaces/jolt_query_collectors.h b/modules/jolt_physics/spaces/jolt_query_collectors.h
new file mode 100644
index 0000000000..5eaac2cd6b
--- /dev/null
+++ b/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
diff --git a/modules/jolt_physics/spaces/jolt_query_filter_3d.cpp b/modules/jolt_physics/spaces/jolt_query_filter_3d.cpp
new file mode 100644
index 0000000000..47cadca618
--- /dev/null
+++ b/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());
+}
diff --git a/modules/jolt_physics/spaces/jolt_query_filter_3d.h b/modules/jolt_physics/spaces/jolt_query_filter_3d.h
new file mode 100644
index 0000000000..cba5986b02
--- /dev/null
+++ b/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
diff --git a/modules/jolt_physics/spaces/jolt_space_3d.cpp b/modules/jolt_physics/spaces/jolt_space_3d.cpp
new file mode 100644
index 0000000000..57d813df2b
--- /dev/null
+++ b/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
diff --git a/modules/jolt_physics/spaces/jolt_space_3d.h b/modules/jolt_physics/spaces/jolt_space_3d.h
new file mode 100644
index 0000000000..eb666644ea
--- /dev/null
+++ b/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
diff --git a/modules/jolt_physics/spaces/jolt_temp_allocator.cpp b/modules/jolt_physics/spaces/jolt_temp_allocator.cpp
new file mode 100644
index 0000000000..640ec02411
--- /dev/null
+++ b/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;
+}
diff --git a/modules/jolt_physics/spaces/jolt_temp_allocator.h b/modules/jolt_physics/spaces/jolt_temp_allocator.h
new file mode 100644
index 0000000000..8d519ae00e
--- /dev/null
+++ b/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
diff --git a/pyproject.toml b/pyproject.toml
index 403d9fd675..9728af9856 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -84,6 +84,7 @@ ignore-words-list = [
 	"outin",
 	"parm",
 	"requestor",
+	"streamin",
 	"te",
 	"textin",
 	"thirdparty",
diff --git a/servers/physics_server_2d.cpp b/servers/physics_server_2d.cpp
index f4f9a2e8b7..c2dafc6aaf 100644
--- a/servers/physics_server_2d.cpp
+++ b/servers/physics_server_2d.cpp
@@ -926,6 +926,7 @@ void PhysicsServer2DManager::on_servers_changed() {
 	}
 	ProjectSettings::get_singleton()->set_custom_property_info(PropertyInfo(Variant::STRING, setting_property_name, PROPERTY_HINT_ENUM, physics_servers));
 	ProjectSettings::get_singleton()->set_restart_if_changed(setting_property_name, true);
+	ProjectSettings::get_singleton()->set_as_basic(setting_property_name, true);
 }
 
 void PhysicsServer2DManager::_bind_methods() {
diff --git a/servers/physics_server_3d.cpp b/servers/physics_server_3d.cpp
index 312b67ada9..336bd4ab9a 100644
--- a/servers/physics_server_3d.cpp
+++ b/servers/physics_server_3d.cpp
@@ -1155,6 +1155,7 @@ void PhysicsServer3DManager::on_servers_changed() {
 	}
 	ProjectSettings::get_singleton()->set_custom_property_info(PropertyInfo(Variant::STRING, setting_property_name, PROPERTY_HINT_ENUM, physics_servers2));
 	ProjectSettings::get_singleton()->set_restart_if_changed(setting_property_name, true);
+	ProjectSettings::get_singleton()->set_as_basic(setting_property_name, true);
 }
 
 void PhysicsServer3DManager::_bind_methods() {
diff --git a/thirdparty/README.md b/thirdparty/README.md
index 144ca870a4..2869bf95bf 100644
--- a/thirdparty/README.md
+++ b/thirdparty/README.md
@@ -433,6 +433,18 @@ Files generated from upstream source:
 5. Copy `source/data/out/icudt76l.dat` to the `{GODOT_SOURCE}/thirdparty/icu4c/icudt76l.dat`
 
 
+## jolt_physics
+
+- Upstream: https://github.com/jrouwe/JoltPhysics
+- Version: 5.2.1 (e3d3cdf644389b621914bb6e73d52ee3137591a7, 2024)
+- License: MIT
+
+Files extracted from upstream source:
+
+- All files in `Jolt/`, except `Jolt/Jolt.cmake` and any files dependent on `ENABLE_OBJECT_STREAM`, as seen in `Jolt/Jolt.cmake`
+- `LICENSE`
+
+
 ## jpeg-compressor
 
 - Upstream: https://github.com/richgel999/jpeg-compressor
diff --git a/thirdparty/jolt_physics/Jolt/AABBTree/AABBTreeBuilder.cpp b/thirdparty/jolt_physics/Jolt/AABBTree/AABBTreeBuilder.cpp
new file mode 100644
index 0000000000..4024132a63
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/AABBTree/AABBTreeBuilder.cpp
@@ -0,0 +1,242 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/AABBTree/AABBTreeBuilder.h>
+
+JPH_NAMESPACE_BEGIN
+
+uint AABBTreeBuilder::Node::GetMinDepth(const Array<Node> &inNodes) const
+{
+	if (HasChildren())
+	{
+		uint left = inNodes[mChild[0]].GetMinDepth(inNodes);
+		uint right = inNodes[mChild[1]].GetMinDepth(inNodes);
+		return min(left, right) + 1;
+	}
+	else
+		return 1;
+}
+
+uint AABBTreeBuilder::Node::GetMaxDepth(const Array<Node> &inNodes) const
+{
+	if (HasChildren())
+	{
+		uint left = inNodes[mChild[0]].GetMaxDepth(inNodes);
+		uint right = inNodes[mChild[1]].GetMaxDepth(inNodes);
+		return max(left, right) + 1;
+	}
+	else
+		return 1;
+}
+
+uint AABBTreeBuilder::Node::GetNodeCount(const Array<Node> &inNodes) const
+{
+	if (HasChildren())
+		return inNodes[mChild[0]].GetNodeCount(inNodes) + inNodes[mChild[1]].GetNodeCount(inNodes) + 1;
+	else
+		return 1;
+}
+
+uint AABBTreeBuilder::Node::GetLeafNodeCount(const Array<Node> &inNodes) const
+{
+	if (HasChildren())
+		return inNodes[mChild[0]].GetLeafNodeCount(inNodes) + inNodes[mChild[1]].GetLeafNodeCount(inNodes);
+	else
+		return 1;
+}
+
+uint AABBTreeBuilder::Node::GetTriangleCountInTree(const Array<Node> &inNodes) const
+{
+	if (HasChildren())
+		return inNodes[mChild[0]].GetTriangleCountInTree(inNodes) + inNodes[mChild[1]].GetTriangleCountInTree(inNodes);
+	else
+		return GetTriangleCount();
+}
+
+void AABBTreeBuilder::Node::GetTriangleCountPerNode(const Array<Node> &inNodes, float &outAverage, uint &outMin, uint &outMax) const
+{
+	outMin = INT_MAX;
+	outMax = 0;
+	outAverage = 0;
+	uint avg_divisor = 0;
+	GetTriangleCountPerNodeInternal(inNodes, outAverage, avg_divisor, outMin, outMax);
+	if (avg_divisor > 0)
+		outAverage /= avg_divisor;
+}
+
+float AABBTreeBuilder::Node::CalculateSAHCost(const Array<Node> &inNodes, float inCostTraversal, float inCostLeaf) const
+{
+	float surface_area = mBounds.GetSurfaceArea();
+	return surface_area > 0.0f? CalculateSAHCostInternal(inNodes, inCostTraversal / surface_area, inCostLeaf / surface_area) : 0.0f;
+}
+
+void AABBTreeBuilder::Node::GetNChildren(const Array<Node> &inNodes, uint inN, Array<const Node*> &outChildren) const
+{
+	JPH_ASSERT(outChildren.empty());
+
+	// Check if there is anything to expand
+	if (!HasChildren())
+		return;
+
+	// Start with the children of this node
+	outChildren.push_back(&inNodes[mChild[0]]);
+	outChildren.push_back(&inNodes[mChild[1]]);
+
+	size_t next = 0;
+	bool all_triangles = true;
+	while (outChildren.size() < inN)
+	{
+		// If we have looped over all nodes, start over with the first node again
+		if (next >= outChildren.size())
+		{
+			// If there only triangle nodes left, we have to terminate
+			if (all_triangles)
+				return;
+			next = 0;
+			all_triangles = true;
+		}
+
+		// Try to expand this node into its two children
+		const Node *to_expand = outChildren[next];
+		if (to_expand->HasChildren())
+		{
+			outChildren.erase(outChildren.begin() + next);
+			outChildren.push_back(&inNodes[to_expand->mChild[0]]);
+			outChildren.push_back(&inNodes[to_expand->mChild[1]]);
+			all_triangles = false;
+		}
+		else
+		{
+			++next;
+		}
+	}
+}
+
+float AABBTreeBuilder::Node::CalculateSAHCostInternal(const Array<Node> &inNodes, float inCostTraversalDivSurfaceArea, float inCostLeafDivSurfaceArea) const
+{
+	if (HasChildren())
+		return inCostTraversalDivSurfaceArea * mBounds.GetSurfaceArea()
+			+ inNodes[mChild[0]].CalculateSAHCostInternal(inNodes, inCostTraversalDivSurfaceArea, inCostLeafDivSurfaceArea)
+			+ inNodes[mChild[1]].CalculateSAHCostInternal(inNodes, inCostTraversalDivSurfaceArea, inCostLeafDivSurfaceArea);
+	else
+		return inCostLeafDivSurfaceArea * mBounds.GetSurfaceArea() * GetTriangleCount();
+}
+
+void AABBTreeBuilder::Node::GetTriangleCountPerNodeInternal(const Array<Node> &inNodes, float &outAverage, uint &outAverageDivisor, uint &outMin, uint &outMax) const
+{
+	if (HasChildren())
+	{
+		inNodes[mChild[0]].GetTriangleCountPerNodeInternal(inNodes, outAverage, outAverageDivisor, outMin, outMax);
+		inNodes[mChild[1]].GetTriangleCountPerNodeInternal(inNodes, outAverage, outAverageDivisor, outMin, outMax);
+	}
+	else
+	{
+		outAverage += GetTriangleCount();
+		outAverageDivisor++;
+		outMin = min(outMin, GetTriangleCount());
+		outMax = max(outMax, GetTriangleCount());
+	}
+}
+
+AABBTreeBuilder::AABBTreeBuilder(TriangleSplitter &inSplitter, uint inMaxTrianglesPerLeaf) :
+	mTriangleSplitter(inSplitter),
+	mMaxTrianglesPerLeaf(inMaxTrianglesPerLeaf)
+{
+}
+
+AABBTreeBuilder::Node *AABBTreeBuilder::Build(AABBTreeBuilderStats &outStats)
+{
+	TriangleSplitter::Range initial = mTriangleSplitter.GetInitialRange();
+
+	// Worst case for number of nodes: 1 leaf node per triangle. At each level above, the number of nodes is half that of the level below.
+	// This means that at most we'll be allocating 2x the number of triangles in nodes.
+	mNodes.reserve(2 * initial.Count());
+	mTriangles.reserve(initial.Count());
+
+	// Build the tree
+	Node &root = mNodes[BuildInternal(initial)];
+
+	// Collect stats
+	float avg_triangles_per_leaf;
+	uint min_triangles_per_leaf, max_triangles_per_leaf;
+	root.GetTriangleCountPerNode(mNodes, avg_triangles_per_leaf, min_triangles_per_leaf, max_triangles_per_leaf);
+
+	mTriangleSplitter.GetStats(outStats.mSplitterStats);
+
+	outStats.mSAHCost = root.CalculateSAHCost(mNodes, 1.0f, 1.0f);
+	outStats.mMinDepth = root.GetMinDepth(mNodes);
+	outStats.mMaxDepth = root.GetMaxDepth(mNodes);
+	outStats.mNodeCount = root.GetNodeCount(mNodes);
+	outStats.mLeafNodeCount = root.GetLeafNodeCount(mNodes);
+	outStats.mMaxTrianglesPerLeaf = mMaxTrianglesPerLeaf;
+	outStats.mTreeMinTrianglesPerLeaf = min_triangles_per_leaf;
+	outStats.mTreeMaxTrianglesPerLeaf = max_triangles_per_leaf;
+	outStats.mTreeAvgTrianglesPerLeaf = avg_triangles_per_leaf;
+
+	return &root;
+}
+
+uint AABBTreeBuilder::BuildInternal(const TriangleSplitter::Range &inTriangles)
+{
+	// Check if there are too many triangles left
+	if (inTriangles.Count() > mMaxTrianglesPerLeaf)
+	{
+		// Split triangles in two batches
+		TriangleSplitter::Range left, right;
+		if (!mTriangleSplitter.Split(inTriangles, left, right))
+		{
+			// When the trace below triggers:
+			//
+			// This code builds a tree structure to accelerate collision detection.
+			// At top level it will start with all triangles in a mesh and then divides the triangles into two batches.
+			// This process repeats until until the batch size is smaller than mMaxTrianglePerLeaf.
+			//
+			// It uses a TriangleSplitter to find a good split. When this warning triggers, the splitter was not able
+			// to create a reasonable split for the triangles. This usually happens when the triangles in a batch are
+			// intersecting. They could also be overlapping when projected on the 3 coordinate axis.
+			//
+			// To solve this issue, you could try to pass your mesh through a mesh cleaning / optimization algorithm.
+			// You could also inspect the triangles that cause this issue and see if that part of the mesh can be fixed manually.
+			//
+			// When you do not fix this warning, the tree will be less efficient for collision detection, but it will still work.
+			JPH_IF_DEBUG(Trace("AABBTreeBuilder: Doing random split for %d triangles (max per node: %u)!", (int)inTriangles.Count(), mMaxTrianglesPerLeaf);)
+			int half = inTriangles.Count() / 2;
+			JPH_ASSERT(half > 0);
+			left = TriangleSplitter::Range(inTriangles.mBegin, inTriangles.mBegin + half);
+			right = TriangleSplitter::Range(inTriangles.mBegin + half, inTriangles.mEnd);
+		}
+
+		// Recursively build
+		const uint node_index = (uint)mNodes.size();
+		mNodes.push_back(Node());
+		uint left_index = BuildInternal(left);
+		uint right_index = BuildInternal(right);
+		Node &node = mNodes[node_index];
+		node.mChild[0] = left_index;
+		node.mChild[1] = right_index;
+		node.mBounds = mNodes[node.mChild[0]].mBounds;
+		node.mBounds.Encapsulate(mNodes[node.mChild[1]].mBounds);
+		return node_index;
+	}
+
+	// Create leaf node
+	const uint node_index = (uint)mNodes.size();
+	mNodes.push_back(Node());
+	Node &node = mNodes.back();
+	node.mTrianglesBegin = (uint)mTriangles.size();
+	node.mNumTriangles = inTriangles.mEnd - inTriangles.mBegin;
+	const VertexList &v = mTriangleSplitter.GetVertices();
+	for (uint i = inTriangles.mBegin; i < inTriangles.mEnd; ++i)
+	{
+		const IndexedTriangle &t = mTriangleSplitter.GetTriangle(i);
+		mTriangles.push_back(t);
+		node.mBounds.Encapsulate(v, t);
+	}
+
+	return node_index;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/AABBTree/AABBTreeBuilder.h b/thirdparty/jolt_physics/Jolt/AABBTree/AABBTreeBuilder.h
new file mode 100644
index 0000000000..3b0635c11a
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/AABBTree/AABBTreeBuilder.h
@@ -0,0 +1,118 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/TriangleSplitter/TriangleSplitter.h>
+#include <Jolt/Geometry/AABox.h>
+#include <Jolt/Core/NonCopyable.h>
+
+JPH_NAMESPACE_BEGIN
+
+struct AABBTreeBuilderStats
+{
+	///@name Splitter stats
+	TriangleSplitter::Stats	mSplitterStats;							///< Stats returned by the triangle splitter algorithm
+
+	///@name Tree structure
+	float					mSAHCost = 0.0f;						///< Surface Area Heuristic cost of this tree
+	int						mMinDepth = 0;							///< Minimal depth of tree (number of nodes)
+	int						mMaxDepth = 0;							///< Maximum depth of tree (number of nodes)
+	int						mNodeCount = 0;							///< Number of nodes in the tree
+	int						mLeafNodeCount = 0;						///< Number of leaf nodes (that contain triangles)
+
+	///@name Configured stats
+	int						mMaxTrianglesPerLeaf = 0;				///< Configured max triangles per leaf
+
+	///@name Actual stats
+	int						mTreeMinTrianglesPerLeaf = 0;			///< Minimal amount of triangles in a leaf
+	int						mTreeMaxTrianglesPerLeaf = 0;			///< Maximal amount of triangles in a leaf
+	float					mTreeAvgTrianglesPerLeaf = 0.0f;		///< Average amount of triangles in leaf nodes
+};
+
+/// Helper class to build an AABB tree
+class JPH_EXPORT AABBTreeBuilder
+{
+public:
+	/// A node in the tree, contains the AABox for the tree and any child nodes or triangles
+	class Node
+	{
+	public:
+		JPH_OVERRIDE_NEW_DELETE
+
+		/// Indicates that there is no child
+		static constexpr uint cInvalidNodeIndex = ~uint(0);
+
+		/// Get number of triangles in this node
+		inline uint			GetTriangleCount() const				{ return mNumTriangles; }
+
+		/// Check if this node has any children
+		inline bool			HasChildren() const						{ return mChild[0] != cInvalidNodeIndex || mChild[1] != cInvalidNodeIndex; }
+
+		/// Min depth of tree
+		uint				GetMinDepth(const Array<Node> &inNodes) const;
+
+		/// Max depth of tree
+		uint				GetMaxDepth(const Array<Node> &inNodes) const;
+
+		/// Number of nodes in tree
+		uint				GetNodeCount(const Array<Node> &inNodes) const;
+
+		/// Number of leaf nodes in tree
+		uint				GetLeafNodeCount(const Array<Node> &inNodes) const;
+
+		/// Get triangle count in tree
+		uint				GetTriangleCountInTree(const Array<Node> &inNodes) const;
+
+		/// Calculate min and max triangles per node
+		void				GetTriangleCountPerNode(const Array<Node> &inNodes, float &outAverage, uint &outMin, uint &outMax) const;
+
+		/// Calculate the total cost of the tree using the surface area heuristic
+		float				CalculateSAHCost(const Array<Node> &inNodes, float inCostTraversal, float inCostLeaf) const;
+
+		/// Recursively get children (breadth first) to get in total inN children (or less if there are no more)
+		void				GetNChildren(const Array<Node> &inNodes, uint inN, Array<const Node *> &outChildren) const;
+
+		/// Bounding box
+		AABox				mBounds;
+
+		/// Triangles (if no child nodes)
+		uint				mTrianglesBegin; // Index into mTriangles
+		uint				mNumTriangles = 0;
+
+		/// Child node indices (if no triangles)
+		uint				mChild[2] = { cInvalidNodeIndex, cInvalidNodeIndex };
+
+	private:
+		friend class AABBTreeBuilder;
+
+		/// Recursive helper function to calculate cost of the tree
+		float				CalculateSAHCostInternal(const Array<Node> &inNodes, float inCostTraversalDivSurfaceArea, float inCostLeafDivSurfaceArea) const;
+
+		/// Recursive helper function to calculate min and max triangles per node
+		void				GetTriangleCountPerNodeInternal(const Array<Node> &inNodes, float &outAverage, uint &outAverageDivisor, uint &outMin, uint &outMax) const;
+	};
+
+	/// Constructor
+							AABBTreeBuilder(TriangleSplitter &inSplitter, uint inMaxTrianglesPerLeaf = 16);
+
+	/// Recursively build tree, returns the root node of the tree
+	Node *					Build(AABBTreeBuilderStats &outStats);
+
+	/// Get all nodes
+	const Array<Node> &		GetNodes() const						{ return mNodes; }
+
+	/// Get all triangles
+	const Array<IndexedTriangle> &GetTriangles() const				{ return mTriangles; }
+
+private:
+	uint					BuildInternal(const TriangleSplitter::Range &inTriangles);
+
+	TriangleSplitter &		mTriangleSplitter;
+	const uint				mMaxTrianglesPerLeaf;
+	Array<Node>				mNodes;
+	Array<IndexedTriangle>	mTriangles;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/AABBTree/AABBTreeToBuffer.h b/thirdparty/jolt_physics/Jolt/AABBTree/AABBTreeToBuffer.h
new file mode 100644
index 0000000000..d50c75061a
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/AABBTree/AABBTreeToBuffer.h
@@ -0,0 +1,296 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/AABBTree/AABBTreeBuilder.h>
+#include <Jolt/Core/ByteBuffer.h>
+#include <Jolt/Geometry/IndexedTriangle.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Conversion algorithm that converts an AABB tree to an optimized binary buffer
+template <class TriangleCodec, class NodeCodec>
+class AABBTreeToBuffer
+{
+public:
+	/// Header for the tree
+	using NodeHeader = typename NodeCodec::Header;
+
+	/// Size in bytes of the header of the tree
+	static const int HeaderSize = NodeCodec::HeaderSize;
+
+	/// Maximum number of children per node in the tree
+	static const int NumChildrenPerNode = NodeCodec::NumChildrenPerNode;
+
+	/// Header for the triangles
+	using TriangleHeader = typename TriangleCodec::TriangleHeader;
+
+	/// Size in bytes of the header for the triangles
+	static const int TriangleHeaderSize = TriangleCodec::TriangleHeaderSize;
+
+	/// Convert AABB tree. Returns false if failed.
+	bool							Convert(const Array<IndexedTriangle> &inTriangles, const Array<AABBTreeBuilder::Node> &inNodes, const VertexList &inVertices, const AABBTreeBuilder::Node *inRoot, bool inStoreUserData, const char *&outError)
+	{
+		typename NodeCodec::EncodingContext node_ctx;
+		typename TriangleCodec::EncodingContext tri_ctx(inVertices);
+
+		// Child nodes out of loop so we don't constantly realloc it
+		Array<const AABBTreeBuilder::Node *> child_nodes;
+		child_nodes.reserve(NumChildrenPerNode);
+
+		// First calculate how big the tree is going to be.
+		// Since the tree can be huge for very large meshes, we don't want
+		// to reallocate the buffer as it may cause out of memory situations.
+		// This loop mimics the construction loop below.
+		uint64 total_size = HeaderSize + TriangleHeaderSize;
+		size_t node_count = 1; // Start with root node
+		size_t to_process_max_size = 1; // Track size of queues so we can do a single reserve below
+		size_t to_process_triangles_max_size = 0;
+		{	// A scope to free the memory associated with to_estimate and to_estimate_triangles
+			Array<const AABBTreeBuilder::Node *> to_estimate;
+			Array<const AABBTreeBuilder::Node *> to_estimate_triangles;
+			to_estimate.push_back(inRoot);
+			for (;;)
+			{
+				while (!to_estimate.empty())
+				{
+					// Get the next node to process
+					const AABBTreeBuilder::Node *node = to_estimate.back();
+					to_estimate.pop_back();
+
+					// Update total size
+					node_ctx.PrepareNodeAllocate(node, total_size);
+
+					if (node->HasChildren())
+					{
+						// Collect the first NumChildrenPerNode sub-nodes in the tree
+						child_nodes.clear(); // Won't free the memory
+						node->GetNChildren(inNodes, NumChildrenPerNode, child_nodes);
+
+						// Increment the number of nodes we're going to store
+						node_count += child_nodes.size();
+
+						// Insert in reverse order so we estimate left child first when taking nodes from the back
+						for (int idx = int(child_nodes.size()) - 1; idx >= 0; --idx)
+						{
+							// Store triangles in separate list so we process them last
+							const AABBTreeBuilder::Node *child = child_nodes[idx];
+							if (child->HasChildren())
+							{
+								to_estimate.push_back(child);
+								to_process_max_size = max(to_estimate.size(), to_process_max_size);
+							}
+							else
+							{
+								to_estimate_triangles.push_back(child);
+								to_process_triangles_max_size = max(to_estimate_triangles.size(), to_process_triangles_max_size);
+							}
+						}
+					}
+					else
+					{
+						// Update total size
+						tri_ctx.PreparePack(&inTriangles[node->mTrianglesBegin], node->mNumTriangles, inStoreUserData, total_size);
+					}
+				}
+
+				// If we've got triangles to estimate, loop again with just the triangles
+				if (to_estimate_triangles.empty())
+					break;
+				else
+					to_estimate.swap(to_estimate_triangles);
+			}
+		}
+
+		// Finalize the prepare stage for the triangle context
+		tri_ctx.FinalizePreparePack(total_size);
+
+		// Reserve the buffer
+		if (size_t(total_size) != total_size)
+		{
+			outError = "AABBTreeToBuffer: Out of memory!";
+			return false;
+		}
+		mTree.reserve(size_t(total_size));
+
+		// Add headers
+		NodeHeader *header = HeaderSize > 0? mTree.Allocate<NodeHeader>() : nullptr;
+		TriangleHeader *triangle_header = TriangleHeaderSize > 0? mTree.Allocate<TriangleHeader>() : nullptr;
+
+		struct NodeData
+		{
+			const AABBTreeBuilder::Node *	mNode = nullptr;							// Node that this entry belongs to
+			Vec3							mNodeBoundsMin;								// Quantized node bounds
+			Vec3							mNodeBoundsMax;
+			size_t							mNodeStart = size_t(-1);					// Start of node in mTree
+			size_t							mTriangleStart = size_t(-1);				// Start of the triangle data in mTree
+			size_t							mChildNodeStart[NumChildrenPerNode];		// Start of the children of the node in mTree
+			size_t							mChildTrianglesStart[NumChildrenPerNode];	// Start of the triangle data in mTree
+			size_t *						mParentChildNodeStart = nullptr;			// Where to store mNodeStart (to patch mChildNodeStart of my parent)
+			size_t *						mParentTrianglesStart = nullptr;			// Where to store mTriangleStart (to patch mChildTrianglesStart of my parent)
+			uint							mNumChildren = 0;							// Number of children
+		};
+
+		Array<NodeData *> to_process;
+		to_process.reserve(to_process_max_size);
+		Array<NodeData *> to_process_triangles;
+		to_process_triangles.reserve(to_process_triangles_max_size);
+		Array<NodeData> node_list;
+		node_list.reserve(node_count); // Needed to ensure that array is not reallocated, so we can keep pointers in the array
+
+		NodeData root;
+		root.mNode = inRoot;
+		root.mNodeBoundsMin = inRoot->mBounds.mMin;
+		root.mNodeBoundsMax = inRoot->mBounds.mMax;
+		node_list.push_back(root);
+		to_process.push_back(&node_list.back());
+
+		for (;;)
+		{
+			while (!to_process.empty())
+			{
+				// Get the next node to process
+				NodeData *node_data = to_process.back();
+				to_process.pop_back();
+
+				// Due to quantization box could have become bigger, not smaller
+				JPH_ASSERT(AABox(node_data->mNodeBoundsMin, node_data->mNodeBoundsMax).Contains(node_data->mNode->mBounds), "AABBTreeToBuffer: Bounding box became smaller!");
+
+				// Collect the first NumChildrenPerNode sub-nodes in the tree
+				child_nodes.clear(); // Won't free the memory
+				node_data->mNode->GetNChildren(inNodes, NumChildrenPerNode, child_nodes);
+				node_data->mNumChildren = (uint)child_nodes.size();
+
+				// Fill in default child bounds
+				Vec3 child_bounds_min[NumChildrenPerNode], child_bounds_max[NumChildrenPerNode];
+				for (size_t i = 0; i < NumChildrenPerNode; ++i)
+					if (i < child_nodes.size())
+					{
+						child_bounds_min[i] = child_nodes[i]->mBounds.mMin;
+						child_bounds_max[i] = child_nodes[i]->mBounds.mMax;
+					}
+					else
+					{
+						child_bounds_min[i] = Vec3::sZero();
+						child_bounds_max[i] = Vec3::sZero();
+					}
+
+				// Start a new node
+				node_data->mNodeStart = node_ctx.NodeAllocate(node_data->mNode, node_data->mNodeBoundsMin, node_data->mNodeBoundsMax, child_nodes, child_bounds_min, child_bounds_max, mTree, outError);
+				if (node_data->mNodeStart == size_t(-1))
+					return false;
+
+				if (node_data->mNode->HasChildren())
+				{
+					// Insert in reverse order so we process left child first when taking nodes from the back
+					for (int idx = int(child_nodes.size()) - 1; idx >= 0; --idx)
+					{
+						const AABBTreeBuilder::Node *child_node = child_nodes[idx];
+
+						// Due to quantization box could have become bigger, not smaller
+						JPH_ASSERT(AABox(child_bounds_min[idx], child_bounds_max[idx]).Contains(child_node->mBounds), "AABBTreeToBuffer: Bounding box became smaller!");
+
+						// Add child to list of nodes to be processed
+						NodeData child;
+						child.mNode = child_node;
+						child.mNodeBoundsMin = child_bounds_min[idx];
+						child.mNodeBoundsMax = child_bounds_max[idx];
+						child.mParentChildNodeStart = &node_data->mChildNodeStart[idx];
+						child.mParentTrianglesStart = &node_data->mChildTrianglesStart[idx];
+						node_list.push_back(child);
+
+						// Store triangles in separate list so we process them last
+						if (child_node->HasChildren())
+							to_process.push_back(&node_list.back());
+						else
+							to_process_triangles.push_back(&node_list.back());
+					}
+				}
+				else
+				{
+					// Add triangles
+					node_data->mTriangleStart = tri_ctx.Pack(&inTriangles[node_data->mNode->mTrianglesBegin], node_data->mNode->mNumTriangles, inStoreUserData, mTree, outError);
+					if (node_data->mTriangleStart == size_t(-1))
+						return false;
+				}
+
+				// Patch offset into parent
+				if (node_data->mParentChildNodeStart != nullptr)
+				{
+					*node_data->mParentChildNodeStart = node_data->mNodeStart;
+					*node_data->mParentTrianglesStart = node_data->mTriangleStart;
+				}
+			}
+
+			// If we've got triangles to process, loop again with just the triangles
+			if (to_process_triangles.empty())
+				break;
+			else
+				to_process.swap(to_process_triangles);
+		}
+
+		// Assert that our reservation was correct (we don't know if we swapped the arrays or not)
+		JPH_ASSERT(to_process_max_size == to_process.capacity() || to_process_triangles_max_size == to_process.capacity());
+		JPH_ASSERT(to_process_max_size == to_process_triangles.capacity() || to_process_triangles_max_size == to_process_triangles.capacity());
+
+		// Finalize all nodes
+		for (NodeData &n : node_list)
+			if (!node_ctx.NodeFinalize(n.mNode, n.mNodeStart, n.mNumChildren, n.mChildNodeStart, n.mChildTrianglesStart, mTree, outError))
+				return false;
+
+		// Finalize the triangles
+		tri_ctx.Finalize(inVertices, triangle_header, mTree);
+
+		// Validate that our reservations were correct
+		if (node_count != node_list.size())
+		{
+			outError = "Internal Error: Node memory estimate was incorrect, memory corruption!";
+			return false;
+		}
+		if (total_size != mTree.size())
+		{
+			outError = "Internal Error: Tree memory estimate was incorrect, memory corruption!";
+			return false;
+		}
+
+		// Finalize the nodes
+		return node_ctx.Finalize(header, inRoot, node_list[0].mNodeStart, node_list[0].mTriangleStart, outError);
+	}
+
+	/// Get resulting data
+	inline const ByteBuffer &		GetBuffer() const
+	{
+		return mTree;
+	}
+
+	/// Get resulting data
+	inline ByteBuffer &				GetBuffer()
+	{
+		return mTree;
+	}
+
+	/// Get header for tree
+	inline const NodeHeader *		GetNodeHeader() const
+	{
+		return mTree.Get<NodeHeader>(0);
+	}
+
+	/// Get header for triangles
+	inline const TriangleHeader *	GetTriangleHeader() const
+	{
+		return mTree.Get<TriangleHeader>(HeaderSize);
+	}
+
+	/// Get root of resulting tree
+	inline const void *				GetRoot() const
+	{
+		return mTree.Get<void>(HeaderSize + TriangleHeaderSize);
+	}
+
+private:
+	ByteBuffer						mTree;									///< Resulting tree structure
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/AABBTree/NodeCodec/NodeCodecQuadTreeHalfFloat.h b/thirdparty/jolt_physics/Jolt/AABBTree/NodeCodec/NodeCodecQuadTreeHalfFloat.h
new file mode 100644
index 0000000000..15717a2da7
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/AABBTree/NodeCodec/NodeCodecQuadTreeHalfFloat.h
@@ -0,0 +1,313 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/ByteBuffer.h>
+#include <Jolt/Math/HalfFloat.h>
+#include <Jolt/AABBTree/AABBTreeBuilder.h>
+
+JPH_NAMESPACE_BEGIN
+
+class NodeCodecQuadTreeHalfFloat
+{
+public:
+	/// Number of child nodes of this node
+	static constexpr int				NumChildrenPerNode = 4;
+
+	/// Header for the tree
+	struct Header
+	{
+		Float3							mRootBoundsMin;
+		Float3							mRootBoundsMax;
+		uint32							mRootProperties;
+		uint8							mBlockIDBits;			///< Number of bits to address a triangle block
+		uint8							mPadding[3] = { 0 };
+	};
+
+	/// Size of the header (an empty struct is always > 0 bytes so this needs a separate variable)
+	static constexpr int				HeaderSize = sizeof(Header);
+
+	/// Stack size to use during DecodingContext::sWalkTree
+	static constexpr int				StackSize = 128;
+
+	/// Node properties
+	enum : uint32
+	{
+		TRIANGLE_COUNT_BITS				= 4,
+		TRIANGLE_COUNT_SHIFT			= 28,
+		TRIANGLE_COUNT_MASK				= (1 << TRIANGLE_COUNT_BITS) - 1,
+		OFFSET_BITS						= 28,
+		OFFSET_MASK						= (1 << OFFSET_BITS) - 1,
+		OFFSET_NON_SIGNIFICANT_BITS		= 2,
+		OFFSET_NON_SIGNIFICANT_MASK		= (1 << OFFSET_NON_SIGNIFICANT_BITS) - 1,
+	};
+
+	/// Node structure
+	struct Node
+	{
+		HalfFloat						mBoundsMinX[4];			///< 4 child bounding boxes
+		HalfFloat						mBoundsMinY[4];
+		HalfFloat						mBoundsMinZ[4];
+		HalfFloat						mBoundsMaxX[4];
+		HalfFloat						mBoundsMaxY[4];
+		HalfFloat						mBoundsMaxZ[4];
+		uint32							mNodeProperties[4];		///< 4 child node properties
+	};
+
+	static_assert(sizeof(Node) == 64, "Node should be 64 bytes");
+
+	/// This class encodes and compresses quad tree nodes
+	class EncodingContext
+	{
+	public:
+		/// Mimics the size a call to NodeAllocate() would add to the buffer
+		void							PrepareNodeAllocate(const AABBTreeBuilder::Node *inNode, uint64 &ioBufferSize) const
+		{
+			// We don't emit nodes for leafs
+			if (!inNode->HasChildren())
+				return;
+
+			// Add size of node
+			ioBufferSize += sizeof(Node);
+		}
+
+		/// Allocate a new node for inNode.
+		/// Algorithm can modify the order of ioChildren to indicate in which order children should be compressed
+		/// Algorithm can enlarge the bounding boxes of the children during compression and returns these in outChildBoundsMin, outChildBoundsMax
+		/// inNodeBoundsMin, inNodeBoundsMax is the bounding box if inNode possibly widened by compressing the parent node
+		/// Returns size_t(-1) on error and reports the error in outError
+		size_t							NodeAllocate(const AABBTreeBuilder::Node *inNode, Vec3Arg inNodeBoundsMin, Vec3Arg inNodeBoundsMax, Array<const AABBTreeBuilder::Node *> &ioChildren, Vec3 outChildBoundsMin[NumChildrenPerNode], Vec3 outChildBoundsMax[NumChildrenPerNode], ByteBuffer &ioBuffer, const char *&outError) const
+		{
+			// We don't emit nodes for leafs
+			if (!inNode->HasChildren())
+				return ioBuffer.size();
+
+			// Remember the start of the node
+			size_t node_start = ioBuffer.size();
+
+			// Fill in bounds
+			Node *node = ioBuffer.Allocate<Node>();
+
+			for (size_t i = 0; i < 4; ++i)
+			{
+				if (i < ioChildren.size())
+				{
+					const AABBTreeBuilder::Node *this_node = ioChildren[i];
+
+					// Copy bounding box
+					node->mBoundsMinX[i] = HalfFloatConversion::FromFloat<HalfFloatConversion::ROUND_TO_NEG_INF>(this_node->mBounds.mMin.GetX());
+					node->mBoundsMinY[i] = HalfFloatConversion::FromFloat<HalfFloatConversion::ROUND_TO_NEG_INF>(this_node->mBounds.mMin.GetY());
+					node->mBoundsMinZ[i] = HalfFloatConversion::FromFloat<HalfFloatConversion::ROUND_TO_NEG_INF>(this_node->mBounds.mMin.GetZ());
+					node->mBoundsMaxX[i] = HalfFloatConversion::FromFloat<HalfFloatConversion::ROUND_TO_POS_INF>(this_node->mBounds.mMax.GetX());
+					node->mBoundsMaxY[i] = HalfFloatConversion::FromFloat<HalfFloatConversion::ROUND_TO_POS_INF>(this_node->mBounds.mMax.GetY());
+					node->mBoundsMaxZ[i] = HalfFloatConversion::FromFloat<HalfFloatConversion::ROUND_TO_POS_INF>(this_node->mBounds.mMax.GetZ());
+
+					// Store triangle count
+					node->mNodeProperties[i] = this_node->GetTriangleCount() << TRIANGLE_COUNT_SHIFT;
+					if (this_node->GetTriangleCount() >= TRIANGLE_COUNT_MASK)
+					{
+						outError = "NodeCodecQuadTreeHalfFloat: Too many triangles";
+						return size_t(-1);
+					}
+				}
+				else
+				{
+					// Make this an invalid triangle node
+					node->mNodeProperties[i] = uint32(TRIANGLE_COUNT_MASK) << TRIANGLE_COUNT_SHIFT;
+
+					// Make bounding box invalid
+					node->mBoundsMinX[i] = HALF_FLT_MAX;
+					node->mBoundsMinY[i] = HALF_FLT_MAX;
+					node->mBoundsMinZ[i] = HALF_FLT_MAX;
+					node->mBoundsMaxX[i] = HALF_FLT_MAX;
+					node->mBoundsMaxY[i] = HALF_FLT_MAX;
+					node->mBoundsMaxZ[i] = HALF_FLT_MAX;
+				}
+			}
+
+			// Since we don't keep track of the bounding box while descending the tree, we keep the root bounds at all levels for triangle compression
+			for (int i = 0; i < NumChildrenPerNode; ++i)
+			{
+				outChildBoundsMin[i] = inNodeBoundsMin;
+				outChildBoundsMax[i] = inNodeBoundsMax;
+			}
+
+			return node_start;
+		}
+
+		/// Once all nodes have been added, this call finalizes all nodes by patching in the offsets of the child nodes (that were added after the node itself was added)
+		bool						NodeFinalize(const AABBTreeBuilder::Node *inNode, size_t inNodeStart, uint inNumChildren, const size_t *inChildrenNodeStart, const size_t *inChildrenTrianglesStart, ByteBuffer &ioBuffer, const char *&outError)
+		{
+			if (!inNode->HasChildren())
+				return true;
+
+			Node *node = ioBuffer.Get<Node>(inNodeStart);
+			for (uint i = 0; i < inNumChildren; ++i)
+			{
+				size_t offset;
+				if (node->mNodeProperties[i] != 0)
+				{
+					// This is a triangle block
+					offset = inChildrenTrianglesStart[i];
+
+					// Store highest block with triangles so we can count the number of bits we need
+					mHighestTriangleBlock = max(mHighestTriangleBlock, offset);
+				}
+				else
+				{
+					// This is a node block
+					offset = inChildrenNodeStart[i];
+				}
+
+				// Store offset of next node / triangles
+				if (offset & OFFSET_NON_SIGNIFICANT_MASK)
+				{
+					outError = "NodeCodecQuadTreeHalfFloat: Internal Error: Offset has non-significant bits set";
+					return false;
+				}
+				offset >>= OFFSET_NON_SIGNIFICANT_BITS;
+				if (offset > OFFSET_MASK)
+				{
+					outError = "NodeCodecQuadTreeHalfFloat: Offset too large. Too much data.";
+					return false;
+				}
+				node->mNodeProperties[i] |= uint32(offset);
+			}
+
+			return true;
+		}
+
+		/// Once all nodes have been finalized, this will finalize the header of the nodes
+		bool						Finalize(Header *outHeader, const AABBTreeBuilder::Node *inRoot, size_t inRootNodeStart, size_t inRootTrianglesStart, const char *&outError) const
+		{
+			// Check if we can address the root node
+			size_t offset = inRoot->HasChildren()? inRootNodeStart : inRootTrianglesStart;
+			if (offset & OFFSET_NON_SIGNIFICANT_MASK)
+			{
+				outError = "NodeCodecQuadTreeHalfFloat: Internal Error: Offset has non-significant bits set";
+				return false;
+			}
+			offset >>= OFFSET_NON_SIGNIFICANT_BITS;
+			if (offset > OFFSET_MASK)
+			{
+				outError = "NodeCodecQuadTreeHalfFloat: Offset too large. Too much data.";
+				return false;
+			}
+
+			// If the root has triangles, we need to take that offset instead since the mHighestTriangleBlock will be zero
+			size_t highest_triangle_block = inRootTrianglesStart != size_t(-1)? inRootTrianglesStart : mHighestTriangleBlock;
+			highest_triangle_block >>= OFFSET_NON_SIGNIFICANT_BITS;
+
+			inRoot->mBounds.mMin.StoreFloat3(&outHeader->mRootBoundsMin);
+			inRoot->mBounds.mMax.StoreFloat3(&outHeader->mRootBoundsMax);
+			outHeader->mRootProperties = uint32(offset) + (inRoot->GetTriangleCount() << TRIANGLE_COUNT_SHIFT);
+			outHeader->mBlockIDBits = uint8(32 - CountLeadingZeros(uint32(highest_triangle_block)));
+			if (inRoot->GetTriangleCount() >= TRIANGLE_COUNT_MASK)
+			{
+				outError = "NodeCodecQuadTreeHalfFloat: Too many triangles";
+				return false;
+			}
+
+			return true;
+		}
+
+	private:
+		size_t						mHighestTriangleBlock = 0;
+	};
+
+	/// This class decodes and decompresses quad tree nodes
+	class DecodingContext
+	{
+	public:
+		/// Get the amount of bits needed to store an ID to a triangle block
+		inline static uint			sTriangleBlockIDBits(const Header *inHeader)
+		{
+			return inHeader->mBlockIDBits;
+		}
+
+		/// Convert a triangle block ID to the start of the triangle buffer
+		inline static const void *	sGetTriangleBlockStart(const uint8 *inBufferStart, uint inTriangleBlockID)
+		{
+			return inBufferStart + (inTriangleBlockID << OFFSET_NON_SIGNIFICANT_BITS);
+		}
+
+		/// Constructor
+		JPH_INLINE explicit			DecodingContext(const Header *inHeader)
+		{
+			// Start with the root node on the stack
+			mNodeStack[0] = inHeader->mRootProperties;
+		}
+
+		/// Walk the node tree calling the Visitor::VisitNodes for each node encountered and Visitor::VisitTriangles for each triangle encountered
+		template <class TriangleContext, class Visitor>
+		JPH_INLINE void				WalkTree(const uint8 *inBufferStart, const TriangleContext &inTriangleContext, Visitor &ioVisitor)
+		{
+			do
+			{
+				// Test if node contains triangles
+				uint32 node_properties = mNodeStack[mTop];
+				uint32 tri_count = node_properties >> TRIANGLE_COUNT_SHIFT;
+				if (tri_count == 0)
+				{
+					const Node *node = reinterpret_cast<const Node *>(inBufferStart + (node_properties << OFFSET_NON_SIGNIFICANT_BITS));
+
+					// Unpack bounds
+					UVec4 bounds_minxy = UVec4::sLoadInt4(reinterpret_cast<const uint32 *>(&node->mBoundsMinX[0]));
+					Vec4 bounds_minx = HalfFloatConversion::ToFloat(bounds_minxy);
+					Vec4 bounds_miny = HalfFloatConversion::ToFloat(bounds_minxy.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_UNUSED, SWIZZLE_UNUSED>());
+
+					UVec4 bounds_minzmaxx = UVec4::sLoadInt4(reinterpret_cast<const uint32 *>(&node->mBoundsMinZ[0]));
+					Vec4 bounds_minz = HalfFloatConversion::ToFloat(bounds_minzmaxx);
+					Vec4 bounds_maxx = HalfFloatConversion::ToFloat(bounds_minzmaxx.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_UNUSED, SWIZZLE_UNUSED>());
+
+					UVec4 bounds_maxyz = UVec4::sLoadInt4(reinterpret_cast<const uint32 *>(&node->mBoundsMaxY[0]));
+					Vec4 bounds_maxy = HalfFloatConversion::ToFloat(bounds_maxyz);
+					Vec4 bounds_maxz = HalfFloatConversion::ToFloat(bounds_maxyz.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_UNUSED, SWIZZLE_UNUSED>());
+
+					// Load properties for 4 children
+					UVec4 properties = UVec4::sLoadInt4(&node->mNodeProperties[0]);
+
+					// Check which sub nodes to visit
+					int num_results = ioVisitor.VisitNodes(bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz, properties, mTop);
+
+					// Push them onto the stack
+					JPH_ASSERT(mTop + 4 < StackSize);
+					properties.StoreInt4(&mNodeStack[mTop]);
+					mTop += num_results;
+				}
+				else if (tri_count != TRIANGLE_COUNT_MASK) // TRIANGLE_COUNT_MASK indicates a padding node, normally we shouldn't visit these nodes but when querying with a big enough box you could touch HALF_FLT_MAX (about 65K)
+				{
+					// Node contains triangles, do individual tests
+					uint32 triangle_block_id = node_properties & OFFSET_MASK;
+					const void *triangles = sGetTriangleBlockStart(inBufferStart, triangle_block_id);
+
+					ioVisitor.VisitTriangles(inTriangleContext, triangles, tri_count, triangle_block_id);
+				}
+
+				// Check if we're done
+				if (ioVisitor.ShouldAbort())
+					break;
+
+				// Fetch next node until we find one that the visitor wants to see
+				do
+					--mTop;
+				while (mTop >= 0 && !ioVisitor.ShouldVisitNode(mTop));
+			}
+			while (mTop >= 0);
+		}
+
+		/// This can be used to have the visitor early out (ioVisitor.ShouldAbort() returns true) and later continue again (call WalkTree() again)
+		bool						IsDoneWalking() const
+		{
+			return mTop < 0;
+		}
+
+	private:
+		uint32						mNodeStack[StackSize];
+		int							mTop = 0;
+	};
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/AABBTree/TriangleCodec/TriangleCodecIndexed8BitPackSOA4Flags.h b/thirdparty/jolt_physics/Jolt/AABBTree/TriangleCodec/TriangleCodecIndexed8BitPackSOA4Flags.h
new file mode 100644
index 0000000000..2168a5e43a
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/AABBTree/TriangleCodec/TriangleCodecIndexed8BitPackSOA4Flags.h
@@ -0,0 +1,549 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/RayTriangle.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Store vertices in 64 bits and indices in 8 bits + 8 bit of flags per triangle like this:
+///
+/// TriangleBlockHeader,
+/// TriangleBlock (4 triangles and their flags in 16 bytes),
+/// TriangleBlock...
+/// [Optional] UserData (4 bytes per triangle)
+///
+/// Vertices are stored:
+///
+/// VertexData (1 vertex in 64 bits),
+/// VertexData...
+///
+/// They're compressed relative to the bounding box as provided by the node codec.
+class TriangleCodecIndexed8BitPackSOA4Flags
+{
+public:
+	class TriangleHeader
+	{
+	public:
+		Float3						mOffset;			///< Offset of all vertices
+		Float3						mScale;				///< Scale of all vertices, vertex_position = mOffset + mScale * compressed_vertex_position
+	};
+
+	/// Size of the header (an empty struct is always > 0 bytes so this needs a separate variable)
+	static constexpr int			TriangleHeaderSize = sizeof(TriangleHeader);
+
+	/// If this codec could return a different offset than the current buffer size when calling Pack()
+	static constexpr bool			ChangesOffsetOnPack = false;
+
+	/// Amount of bits per component
+	enum EComponentData : uint32
+	{
+		COMPONENT_BITS = 21,
+		COMPONENT_MASK = (1 << COMPONENT_BITS) - 1,
+	};
+
+	/// Packed X and Y coordinate
+	enum EVertexXY : uint32
+	{
+		COMPONENT_X = 0,
+		COMPONENT_Y1 = COMPONENT_BITS,
+		COMPONENT_Y1_BITS = 32 - COMPONENT_BITS,
+	};
+
+	/// Packed Z and Y coordinate
+	enum EVertexZY : uint32
+	{
+		COMPONENT_Z = 0,
+		COMPONENT_Y2 = COMPONENT_BITS,
+		COMPONENT_Y2_BITS = 31 - COMPONENT_BITS,
+	};
+
+	/// A single packed vertex
+	struct VertexData
+	{
+		uint32						mVertexXY;
+		uint32						mVertexZY;
+	};
+
+	static_assert(sizeof(VertexData) == 8, "Compiler added padding");
+
+	/// A block of 4 triangles
+	struct TriangleBlock
+	{
+		uint8						mIndices[3][4];				///< 8 bit indices to triangle vertices for 4 triangles in the form mIndices[vertex][triangle] where vertex in [0, 2] and triangle in [0, 3]
+		uint8						mFlags[4];					///< Triangle flags (could contain material and active edges)
+	};
+
+	static_assert(sizeof(TriangleBlock) == 16, "Compiler added padding");
+
+	enum ETriangleBlockHeaderFlags : uint32
+	{
+		OFFSET_TO_VERTICES_BITS = 29,							///< Offset from current block to start of vertices in bytes
+		OFFSET_TO_VERTICES_MASK = (1 << OFFSET_TO_VERTICES_BITS) - 1,
+		OFFSET_NON_SIGNIFICANT_BITS = 2,						///< The offset from the current block to the start of the vertices must be a multiple of 4 bytes
+		OFFSET_NON_SIGNIFICANT_MASK = (1 << OFFSET_NON_SIGNIFICANT_BITS) - 1,
+		OFFSET_TO_USERDATA_BITS = 3,							///< When user data is stored, this is the number of blocks to skip to get to the user data (0 = no user data)
+		OFFSET_TO_USERDATA_MASK = (1 << OFFSET_TO_USERDATA_BITS) - 1,
+	};
+
+	/// A triangle header, will be followed by one or more TriangleBlocks
+	struct TriangleBlockHeader
+	{
+		const VertexData *			GetVertexData() const		{ return reinterpret_cast<const VertexData *>(reinterpret_cast<const uint8 *>(this) + ((mFlags & OFFSET_TO_VERTICES_MASK) << OFFSET_NON_SIGNIFICANT_BITS)); }
+		const TriangleBlock *		GetTriangleBlock() const	{ return reinterpret_cast<const TriangleBlock *>(reinterpret_cast<const uint8 *>(this) + sizeof(TriangleBlockHeader)); }
+		const uint32 *				GetUserData() const			{ uint32 offset = mFlags >> OFFSET_TO_VERTICES_BITS; return offset == 0? nullptr : reinterpret_cast<const uint32 *>(GetTriangleBlock() + offset); }
+
+		uint32						mFlags;
+	};
+
+	static_assert(sizeof(TriangleBlockHeader) == 4, "Compiler added padding");
+
+	/// This class is used to validate that the triangle data will not be degenerate after compression
+	class ValidationContext
+	{
+	public:
+		/// Constructor
+									ValidationContext(const IndexedTriangleList &inTriangles, const VertexList &inVertices) :
+			mVertices(inVertices)
+		{
+			// Only used the referenced triangles, just like EncodingContext::Finalize does
+			for (const IndexedTriangle &i : inTriangles)
+				for (uint32 idx : i.mIdx)
+					mBounds.Encapsulate(Vec3(inVertices[idx]));
+		}
+
+		/// Test if a triangle will be degenerate after quantization
+		bool						IsDegenerate(const IndexedTriangle &inTriangle) const
+		{
+			// Quantize the triangle in the same way as EncodingContext::Finalize does
+			UVec4 quantized_vertex[3];
+			Vec3 compress_scale = Vec3::sReplicate(COMPONENT_MASK) / Vec3::sMax(mBounds.GetSize(), Vec3::sReplicate(1.0e-20f));
+			for (int i = 0; i < 3; ++i)
+				quantized_vertex[i] = ((Vec3(mVertices[inTriangle.mIdx[i]]) - mBounds.mMin) * compress_scale + Vec3::sReplicate(0.5f)).ToInt();
+			return quantized_vertex[0] == quantized_vertex[1] || quantized_vertex[1] == quantized_vertex[2] || quantized_vertex[0] == quantized_vertex[2];
+		}
+
+	private:
+		const VertexList &			mVertices;
+		AABox						mBounds;
+	};
+
+	/// This class is used to encode and compress triangle data into a byte buffer
+	class EncodingContext
+	{
+	public:
+		/// Indicates a vertex hasn't been seen yet in the triangle list
+		static constexpr uint32		cNotFound = 0xffffffff;
+
+		/// Construct the encoding context
+		explicit					EncodingContext(const VertexList &inVertices) :
+			mVertexMap(inVertices.size(), cNotFound)
+		{
+		}
+
+		/// Mimics the size a call to Pack() would add to the buffer
+		void						PreparePack(const IndexedTriangle *inTriangles, uint inNumTriangles, bool inStoreUserData, uint64 &ioBufferSize)
+		{
+			// Add triangle block header
+			ioBufferSize += sizeof(TriangleBlockHeader);
+
+			// Compute first vertex that this batch will use (ensuring there's enough room if none of the vertices are shared)
+			uint start_vertex = Clamp((int)mVertexCount - 256 + (int)inNumTriangles * 3, 0, (int)mVertexCount);
+
+			// Pack vertices
+			uint padded_triangle_count = AlignUp(inNumTriangles, 4);
+			for (uint t = 0; t < padded_triangle_count; t += 4)
+			{
+				// Add triangle block header
+				ioBufferSize += sizeof(TriangleBlock);
+
+				for (uint vertex_nr = 0; vertex_nr < 3; ++vertex_nr)
+					for (uint block_tri_idx = 0; block_tri_idx < 4; ++block_tri_idx)
+					{
+						// Fetch vertex index. Create degenerate triangles for padding triangles.
+						bool triangle_available = t + block_tri_idx < inNumTriangles;
+						uint32 src_vertex_index = triangle_available? inTriangles[t + block_tri_idx].mIdx[vertex_nr] : inTriangles[inNumTriangles - 1].mIdx[0];
+
+						// Check if we've seen this vertex before and if it is in the range that we can encode
+						uint32 &vertex_index = mVertexMap[src_vertex_index];
+						if (vertex_index == cNotFound || vertex_index < start_vertex)
+						{
+							// Add vertex
+							vertex_index = mVertexCount;
+							mVertexCount++;
+						}
+					}
+			}
+
+			// Add user data
+			if (inStoreUserData)
+				ioBufferSize += inNumTriangles * sizeof(uint32);
+		}
+
+		/// Mimics the size the Finalize() call would add to ioBufferSize
+		void						FinalizePreparePack(uint64 &ioBufferSize)
+		{
+			// Remember where the vertices are going to start in the output buffer
+			JPH_ASSERT(IsAligned(ioBufferSize, 4));
+			mVerticesStartIdx = size_t(ioBufferSize);
+
+			// Add vertices to buffer
+			ioBufferSize += uint64(mVertexCount) * sizeof(VertexData);
+
+			// Reserve the amount of memory we need for the vertices
+			mVertices.reserve(mVertexCount);
+
+			// Set vertex map back to 'not found'
+			for (uint32 &v : mVertexMap)
+				v = cNotFound;
+		}
+
+		/// Pack the triangles in inContainer to ioBuffer. This stores the mMaterialIndex of a triangle in the 8 bit flags.
+		/// Returns size_t(-1) on error.
+		size_t						Pack(const IndexedTriangle *inTriangles, uint inNumTriangles, bool inStoreUserData, ByteBuffer &ioBuffer, const char *&outError)
+		{
+			JPH_ASSERT(inNumTriangles > 0);
+
+			// Determine position of triangles start
+			size_t triangle_block_start = ioBuffer.size();
+
+			// Allocate triangle block header
+			TriangleBlockHeader *header = ioBuffer.Allocate<TriangleBlockHeader>();
+
+			// Compute first vertex that this batch will use (ensuring there's enough room if none of the vertices are shared)
+			uint start_vertex = Clamp((int)mVertices.size() - 256 + (int)inNumTriangles * 3, 0, (int)mVertices.size());
+
+			// Store the start vertex offset relative to TriangleBlockHeader
+			size_t offset_to_vertices = mVerticesStartIdx - triangle_block_start + size_t(start_vertex) * sizeof(VertexData);
+			if (offset_to_vertices & OFFSET_NON_SIGNIFICANT_MASK)
+			{
+				outError = "TriangleCodecIndexed8BitPackSOA4Flags: Internal Error: Offset has non-significant bits set";
+				return size_t(-1);
+			}
+			offset_to_vertices >>= OFFSET_NON_SIGNIFICANT_BITS;
+			if (offset_to_vertices > OFFSET_TO_VERTICES_MASK)
+			{
+				outError = "TriangleCodecIndexed8BitPackSOA4Flags: Offset to vertices doesn't fit. Too much data.";
+				return size_t(-1);
+			}
+			header->mFlags = uint32(offset_to_vertices);
+
+			// When we store user data we need to store the offset to the user data in TriangleBlocks
+			uint padded_triangle_count = AlignUp(inNumTriangles, 4);
+			if (inStoreUserData)
+			{
+				uint32 num_blocks = padded_triangle_count >> 2;
+				JPH_ASSERT(num_blocks <= OFFSET_TO_USERDATA_MASK);
+				header->mFlags |= num_blocks << OFFSET_TO_VERTICES_BITS;
+			}
+
+			// Pack vertices
+			for (uint t = 0; t < padded_triangle_count; t += 4)
+			{
+				TriangleBlock *block = ioBuffer.Allocate<TriangleBlock>();
+				for (uint vertex_nr = 0; vertex_nr < 3; ++vertex_nr)
+					for (uint block_tri_idx = 0; block_tri_idx < 4; ++block_tri_idx)
+					{
+						// Fetch vertex index. Create degenerate triangles for padding triangles.
+						bool triangle_available = t + block_tri_idx < inNumTriangles;
+						uint32 src_vertex_index = triangle_available? inTriangles[t + block_tri_idx].mIdx[vertex_nr] : inTriangles[inNumTriangles - 1].mIdx[0];
+
+						// Check if we've seen this vertex before and if it is in the range that we can encode
+						uint32 &vertex_index = mVertexMap[src_vertex_index];
+						if (vertex_index == cNotFound || vertex_index < start_vertex)
+						{
+							// Add vertex
+							vertex_index = (uint32)mVertices.size();
+							mVertices.push_back(src_vertex_index);
+						}
+
+						// Store vertex index
+						uint32 vertex_offset = vertex_index - start_vertex;
+						if (vertex_offset > 0xff)
+						{
+							outError = "TriangleCodecIndexed8BitPackSOA4Flags: Offset doesn't fit in 8 bit";
+							return size_t(-1);
+						}
+						block->mIndices[vertex_nr][block_tri_idx] = (uint8)vertex_offset;
+
+						// Store flags
+						uint32 flags = triangle_available? inTriangles[t + block_tri_idx].mMaterialIndex : 0;
+						if (flags > 0xff)
+						{
+							outError = "TriangleCodecIndexed8BitPackSOA4Flags: Material index doesn't fit in 8 bit";
+							return size_t(-1);
+						}
+						block->mFlags[block_tri_idx] = (uint8)flags;
+					}
+			}
+
+			// Store user data
+			if (inStoreUserData)
+			{
+				uint32 *user_data = ioBuffer.Allocate<uint32>(inNumTriangles);
+				for (uint t = 0; t < inNumTriangles; ++t)
+					user_data[t] = inTriangles[t].mUserData;
+			}
+
+			return triangle_block_start;
+		}
+
+		/// After all triangles have been packed, this finalizes the header and triangle buffer
+		void						Finalize(const VertexList &inVertices, TriangleHeader *ioHeader, ByteBuffer &ioBuffer) const
+		{
+			// Assert that our reservations were correct
+			JPH_ASSERT(mVertices.size() == mVertexCount);
+			JPH_ASSERT(ioBuffer.size() == mVerticesStartIdx);
+
+			// Check if anything to do
+			if (mVertices.empty())
+				return;
+
+			// Calculate bounding box
+			AABox bounds;
+			for (uint32 v : mVertices)
+				bounds.Encapsulate(Vec3(inVertices[v]));
+
+			// Compress vertices
+			VertexData *vertices = ioBuffer.Allocate<VertexData>(mVertices.size());
+			Vec3 compress_scale = Vec3::sReplicate(COMPONENT_MASK) / Vec3::sMax(bounds.GetSize(), Vec3::sReplicate(1.0e-20f));
+			for (uint32 v : mVertices)
+			{
+				UVec4 c = ((Vec3(inVertices[v]) - bounds.mMin) * compress_scale + Vec3::sReplicate(0.5f)).ToInt();
+				JPH_ASSERT(c.GetX() <= COMPONENT_MASK);
+				JPH_ASSERT(c.GetY() <= COMPONENT_MASK);
+				JPH_ASSERT(c.GetZ() <= COMPONENT_MASK);
+				vertices->mVertexXY = c.GetX() + (c.GetY() << COMPONENT_Y1);
+				vertices->mVertexZY = c.GetZ() + ((c.GetY() >> COMPONENT_Y1_BITS) << COMPONENT_Y2);
+				++vertices;
+			}
+
+			// Store decompression information
+			bounds.mMin.StoreFloat3(&ioHeader->mOffset);
+			(bounds.GetSize() / Vec3::sReplicate(COMPONENT_MASK)).StoreFloat3(&ioHeader->mScale);
+		}
+
+	private:
+		using VertexMap = Array<uint32>;
+
+		uint32						mVertexCount = 0;			///< Number of vertices calculated during PreparePack
+		size_t						mVerticesStartIdx = 0;		///< Start of the vertices in the output buffer, calculated during PreparePack
+		Array<uint32>				mVertices;					///< Output vertices as an index into the original vertex list (inVertices), sorted according to occurrence
+		VertexMap					mVertexMap;					///< Maps from the original mesh vertex index (inVertices) to the index in our output vertices (mVertices)
+	};
+
+	/// This class is used to decode and decompress triangle data packed by the EncodingContext
+	class DecodingContext
+	{
+	private:
+		/// Private helper functions to unpack the 1 vertex of 4 triangles (outX contains the x coordinate of triangle 0 .. 3 etc.)
+		JPH_INLINE void				Unpack(const VertexData *inVertices, UVec4Arg inIndex, Vec4 &outX, Vec4 &outY, Vec4 &outZ) const
+		{
+			// Get compressed data
+			UVec4 c1 = UVec4::sGatherInt4<8>(&inVertices->mVertexXY, inIndex);
+			UVec4 c2 = UVec4::sGatherInt4<8>(&inVertices->mVertexZY, inIndex);
+
+			// Unpack the x y and z component
+			UVec4 xc = UVec4::sAnd(c1, UVec4::sReplicate(COMPONENT_MASK));
+			UVec4 yc = UVec4::sOr(c1.LogicalShiftRight<COMPONENT_Y1>(), c2.LogicalShiftRight<COMPONENT_Y2>().LogicalShiftLeft<COMPONENT_Y1_BITS>());
+			UVec4 zc = UVec4::sAnd(c2, UVec4::sReplicate(COMPONENT_MASK));
+
+			// Convert to float
+			outX = Vec4::sFusedMultiplyAdd(xc.ToFloat(), mScaleX, mOffsetX);
+			outY = Vec4::sFusedMultiplyAdd(yc.ToFloat(), mScaleY, mOffsetY);
+			outZ = Vec4::sFusedMultiplyAdd(zc.ToFloat(), mScaleZ, mOffsetZ);
+		}
+
+	public:
+		JPH_INLINE explicit			DecodingContext(const TriangleHeader *inHeader) :
+			mOffsetX(Vec4::sReplicate(inHeader->mOffset.x)),
+			mOffsetY(Vec4::sReplicate(inHeader->mOffset.y)),
+			mOffsetZ(Vec4::sReplicate(inHeader->mOffset.z)),
+			mScaleX(Vec4::sReplicate(inHeader->mScale.x)),
+			mScaleY(Vec4::sReplicate(inHeader->mScale.y)),
+			mScaleZ(Vec4::sReplicate(inHeader->mScale.z))
+		{
+		}
+
+		/// Unpacks triangles in the format t1v1,t1v2,t1v3, t2v1,t2v2,t2v3, ...
+		JPH_INLINE void				Unpack(const void *inTriangleStart, uint32 inNumTriangles, Vec3 *outTriangles) const
+		{
+			JPH_ASSERT(inNumTriangles > 0);
+			const TriangleBlockHeader *header = reinterpret_cast<const TriangleBlockHeader *>(inTriangleStart);
+			const VertexData *vertices = header->GetVertexData();
+			const TriangleBlock *t = header->GetTriangleBlock();
+			const TriangleBlock *end = t + ((inNumTriangles + 3) >> 2);
+
+			int triangles_left = inNumTriangles;
+
+			do
+			{
+				// Get the indices for the three vertices (reads 4 bytes extra, but these are the flags so that's ok)
+				UVec4 indices = UVec4::sLoadInt4(reinterpret_cast<const uint32 *>(&t->mIndices[0]));
+				UVec4 iv1 = indices.Expand4Byte0();
+				UVec4 iv2 = indices.Expand4Byte4();
+				UVec4 iv3 = indices.Expand4Byte8();
+
+				// Decompress the triangle data
+				Vec4 v1x, v1y, v1z, v2x, v2y, v2z, v3x, v3y, v3z;
+				Unpack(vertices, iv1, v1x, v1y, v1z);
+				Unpack(vertices, iv2, v2x, v2y, v2z);
+				Unpack(vertices, iv3, v3x, v3y, v3z);
+
+				// Transpose it so we get normal vectors
+				Mat44 v1 = Mat44(v1x, v1y, v1z, Vec4::sZero()).Transposed();
+				Mat44 v2 = Mat44(v2x, v2y, v2z, Vec4::sZero()).Transposed();
+				Mat44 v3 = Mat44(v3x, v3y, v3z, Vec4::sZero()).Transposed();
+
+				// Store triangle data
+				for (int i = 0; i < 4 && triangles_left > 0; ++i, --triangles_left)
+				{
+					*outTriangles++ = v1.GetColumn3(i);
+					*outTriangles++ = v2.GetColumn3(i);
+					*outTriangles++ = v3.GetColumn3(i);
+				}
+
+				++t;
+			}
+			while (t < end);
+		}
+
+		/// Tests a ray against the packed triangles
+		JPH_INLINE float			TestRay(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, const void *inTriangleStart, uint32 inNumTriangles, float inClosest, uint32 &outClosestTriangleIndex) const
+		{
+			JPH_ASSERT(inNumTriangles > 0);
+			const TriangleBlockHeader *header = reinterpret_cast<const TriangleBlockHeader *>(inTriangleStart);
+			const VertexData *vertices = header->GetVertexData();
+			const TriangleBlock *t = header->GetTriangleBlock();
+			const TriangleBlock *end = t + ((inNumTriangles + 3) >> 2);
+
+			Vec4 closest = Vec4::sReplicate(inClosest);
+			UVec4 closest_triangle_idx = UVec4::sZero();
+
+			UVec4 start_triangle_idx = UVec4::sZero();
+			do
+			{
+				// Get the indices for the three vertices (reads 4 bytes extra, but these are the flags so that's ok)
+				UVec4 indices = UVec4::sLoadInt4(reinterpret_cast<const uint32 *>(&t->mIndices[0]));
+				UVec4 iv1 = indices.Expand4Byte0();
+				UVec4 iv2 = indices.Expand4Byte4();
+				UVec4 iv3 = indices.Expand4Byte8();
+
+				// Decompress the triangle data
+				Vec4 v1x, v1y, v1z, v2x, v2y, v2z, v3x, v3y, v3z;
+				Unpack(vertices, iv1, v1x, v1y, v1z);
+				Unpack(vertices, iv2, v2x, v2y, v2z);
+				Unpack(vertices, iv3, v3x, v3y, v3z);
+
+				// Perform ray vs triangle test
+				Vec4 distance = RayTriangle4(inRayOrigin, inRayDirection, v1x, v1y, v1z, v2x, v2y, v2z, v3x, v3y, v3z);
+
+				// Update closest with the smaller values
+				UVec4 smaller = Vec4::sLess(distance, closest);
+				closest = Vec4::sSelect(closest, distance, smaller);
+
+				// Update triangle index with the smallest values
+				UVec4 triangle_idx = start_triangle_idx + UVec4(0, 1, 2, 3);
+				closest_triangle_idx = UVec4::sSelect(closest_triangle_idx, triangle_idx, smaller);
+
+				// Next block
+				++t;
+				start_triangle_idx += UVec4::sReplicate(4);
+			}
+			while (t < end);
+
+			// Get the smallest component
+			Vec4::sSort4(closest, closest_triangle_idx);
+			outClosestTriangleIndex = closest_triangle_idx.GetX();
+			return closest.GetX();
+		}
+
+		/// Decode a single triangle
+		inline void					GetTriangle(const void *inTriangleStart, uint32 inTriangleIdx, Vec3 &outV1, Vec3 &outV2, Vec3 &outV3) const
+		{
+			const TriangleBlockHeader *header = reinterpret_cast<const TriangleBlockHeader *>(inTriangleStart);
+			const VertexData *vertices = header->GetVertexData();
+			const TriangleBlock *block = header->GetTriangleBlock() + (inTriangleIdx >> 2);
+			uint32 block_triangle_idx = inTriangleIdx & 0b11;
+
+			// Get the 3 vertices
+			const VertexData &v1 = vertices[block->mIndices[0][block_triangle_idx]];
+			const VertexData &v2 = vertices[block->mIndices[1][block_triangle_idx]];
+			const VertexData &v3 = vertices[block->mIndices[2][block_triangle_idx]];
+
+			// Pack the vertices
+			UVec4 c1(v1.mVertexXY, v2.mVertexXY, v3.mVertexXY, 0);
+			UVec4 c2(v1.mVertexZY, v2.mVertexZY, v3.mVertexZY, 0);
+
+			// Unpack the x y and z component
+			UVec4 xc = UVec4::sAnd(c1, UVec4::sReplicate(COMPONENT_MASK));
+			UVec4 yc = UVec4::sOr(c1.LogicalShiftRight<COMPONENT_Y1>(), c2.LogicalShiftRight<COMPONENT_Y2>().LogicalShiftLeft<COMPONENT_Y1_BITS>());
+			UVec4 zc = UVec4::sAnd(c2, UVec4::sReplicate(COMPONENT_MASK));
+
+			// Convert to float
+			Vec4 vx = Vec4::sFusedMultiplyAdd(xc.ToFloat(), mScaleX, mOffsetX);
+			Vec4 vy = Vec4::sFusedMultiplyAdd(yc.ToFloat(), mScaleY, mOffsetY);
+			Vec4 vz = Vec4::sFusedMultiplyAdd(zc.ToFloat(), mScaleZ, mOffsetZ);
+
+			// Transpose it so we get normal vectors
+			Mat44 trans = Mat44(vx, vy, vz, Vec4::sZero()).Transposed();
+			outV1 = trans.GetAxisX();
+			outV2 = trans.GetAxisY();
+			outV3 = trans.GetAxisZ();
+		}
+
+		/// Get user data for a triangle
+		JPH_INLINE uint32			GetUserData(const void *inTriangleStart, uint32 inTriangleIdx) const
+		{
+			const TriangleBlockHeader *header = reinterpret_cast<const TriangleBlockHeader *>(inTriangleStart);
+			const uint32 *user_data = header->GetUserData();
+			return user_data != nullptr? user_data[inTriangleIdx] : 0;
+		}
+
+		/// Get flags for entire triangle block
+		JPH_INLINE static void		sGetFlags(const void *inTriangleStart, uint32 inNumTriangles, uint8 *outTriangleFlags)
+		{
+			JPH_ASSERT(inNumTriangles > 0);
+			const TriangleBlockHeader *header = reinterpret_cast<const TriangleBlockHeader *>(inTriangleStart);
+			const TriangleBlock *t = header->GetTriangleBlock();
+			const TriangleBlock *end = t + ((inNumTriangles + 3) >> 2);
+
+			int triangles_left = inNumTriangles;
+			do
+			{
+				for (int i = 0; i < 4 && triangles_left > 0; ++i, --triangles_left)
+					*outTriangleFlags++ = t->mFlags[i];
+
+				++t;
+			}
+			while (t < end);
+		}
+
+		/// Get flags for a particular triangle
+		JPH_INLINE static uint8		sGetFlags(const void *inTriangleStart, int inTriangleIndex)
+		{
+			const TriangleBlockHeader *header = reinterpret_cast<const TriangleBlockHeader *>(inTriangleStart);
+			const TriangleBlock *first_block = header->GetTriangleBlock();
+			return first_block[inTriangleIndex >> 2].mFlags[inTriangleIndex & 0b11];
+		}
+
+		/// Unpacks triangles and flags, convenience function
+		JPH_INLINE void				Unpack(const void *inTriangleStart, uint32 inNumTriangles, Vec3 *outTriangles, uint8 *outTriangleFlags) const
+		{
+			Unpack(inTriangleStart, inNumTriangles, outTriangles);
+			sGetFlags(inTriangleStart, inNumTriangles, outTriangleFlags);
+		}
+
+	private:
+		Vec4						mOffsetX;
+		Vec4						mOffsetY;
+		Vec4						mOffsetZ;
+		Vec4						mScaleX;
+		Vec4						mScaleY;
+		Vec4						mScaleZ;
+	};
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/ConfigurationString.h b/thirdparty/jolt_physics/Jolt/ConfigurationString.h
new file mode 100644
index 0000000000..55e5d8c35f
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/ConfigurationString.h
@@ -0,0 +1,94 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Construct a string that lists the most important configuration settings
+inline const char *GetConfigurationString()
+{
+	return JPH_IF_SINGLE_PRECISION_ELSE("Single", "Double") " precision "
+#if defined(JPH_CPU_X86)
+		"x86 "
+#elif defined(JPH_CPU_ARM)
+		"ARM "
+#elif defined(JPH_PLATFORM_WASM)
+		"WASM "
+#endif
+#if JPH_CPU_ADDRESS_BITS == 64
+		"64-bit "
+#elif JPH_CPU_ADDRESS_BITS == 32
+		"32-bit "
+#endif
+		"with instructions: "
+#ifdef JPH_USE_NEON
+		"NEON "
+#endif
+#ifdef JPH_USE_SSE
+		"SSE2 "
+#endif
+#ifdef JPH_USE_SSE4_1
+		"SSE4.1 "
+#endif
+#ifdef JPH_USE_SSE4_2
+		"SSE4.2 "
+#endif
+#ifdef JPH_USE_AVX
+		"AVX "
+#endif
+#ifdef JPH_USE_AVX2
+		"AVX2 "
+#endif
+#ifdef JPH_USE_AVX512
+		"AVX512 "
+#endif
+#ifdef JPH_USE_F16C
+		"F16C "
+#endif
+#ifdef JPH_USE_LZCNT
+		"LZCNT "
+#endif
+#ifdef JPH_USE_TZCNT
+		"TZCNT "
+#endif
+#ifdef JPH_USE_FMADD
+		"FMADD "
+#endif
+#ifdef JPH_CROSS_PLATFORM_DETERMINISTIC
+		"(Cross Platform Deterministic) "
+#endif
+#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+		"(FP Exceptions) "
+#endif
+#ifdef JPH_DEBUG_RENDERER
+		"(Debug Renderer) "
+#endif
+#ifdef JPH_PROFILE_ENABLED
+		"(Profile) "
+#endif
+#if defined(JPH_OBJECT_LAYER_BITS) && JPH_OBJECT_LAYER_BITS == 32
+		"(32-bit ObjectLayer) "
+#else
+		"(16-bit ObjectLayer) "
+#endif
+#ifdef JPH_ENABLE_ASSERTS
+		"(Assertions) "
+#endif
+#ifdef JPH_OBJECT_STREAM
+		"(ObjectStream) "
+#endif
+#ifdef JPH_DEBUG
+		"(Debug) "
+#endif
+#if defined(__cpp_rtti) && __cpp_rtti
+		"(C++ RTTI) "
+#endif
+#if defined(__cpp_exceptions) && __cpp_exceptions
+		"(C++ Exceptions) "
+#endif
+		;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/ARMNeon.h b/thirdparty/jolt_physics/Jolt/Core/ARMNeon.h
new file mode 100644
index 0000000000..6273945dc0
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/ARMNeon.h
@@ -0,0 +1,94 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#ifdef JPH_USE_NEON
+
+// Constructing NEON values
+#ifdef JPH_COMPILER_MSVC
+	#define JPH_NEON_INT32x4(v1, v2, v3, v4) { int64_t(v1) + (int64_t(v2) << 32), int64_t(v3) + (int64_t(v4) << 32) }
+	#define JPH_NEON_UINT32x4(v1, v2, v3, v4) { uint64_t(v1) + (uint64_t(v2) << 32), uint64_t(v3) + (uint64_t(v4) << 32) }
+	#define JPH_NEON_INT8x16(v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16) { int64_t(v1) + (int64_t(v2) << 8) + (int64_t(v3) << 16) + (int64_t(v4) << 24) + (int64_t(v5) << 32) + (int64_t(v6) << 40) + (int64_t(v7) << 48) + (int64_t(v8) << 56), int64_t(v9) + (int64_t(v10) << 8) + (int64_t(v11) << 16) + (int64_t(v12) << 24) + (int64_t(v13) << 32) + (int64_t(v14) << 40) + (int64_t(v15) << 48) + (int64_t(v16) << 56) }
+	#define JPH_NEON_UINT8x16(v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16) { uint64_t(v1) + (uint64_t(v2) << 8) + (uint64_t(v3) << 16) + (uint64_t(v4) << 24) + (uint64_t(v5) << 32) + (uint64_t(v6) << 40) + (uint64_t(v7) << 48) + (uint64_t(v8) << 56), uint64_t(v9) + (uint64_t(v10) << 8) + (uint64_t(v11) << 16) + (uint64_t(v12) << 24) + (uint64_t(v13) << 32) + (uint64_t(v14) << 40) + (uint64_t(v15) << 48) + (uint64_t(v16) << 56) }
+#else
+	#define JPH_NEON_INT32x4(v1, v2, v3, v4) { v1, v2, v3, v4 }
+	#define JPH_NEON_UINT32x4(v1, v2, v3, v4) { v1, v2, v3, v4 }
+	#define JPH_NEON_INT8x16(v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16) { v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16 }
+	#define JPH_NEON_UINT8x16(v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16) { v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16 }
+#endif
+
+// MSVC and GCC prior to version 12 don't define __builtin_shufflevector
+#if defined(JPH_COMPILER_MSVC) || (defined(JPH_COMPILER_GCC) && __GNUC__ < 12)
+	JPH_NAMESPACE_BEGIN
+
+	// Generic shuffle vector template
+	template <unsigned I1, unsigned I2, unsigned I3, unsigned I4>
+	JPH_INLINE float32x4_t NeonShuffleFloat32x4(float32x4_t inV1, float32x4_t inV2)
+	{
+		float32x4_t ret;
+		ret = vmovq_n_f32(vgetq_lane_f32(I1 >= 4? inV2 : inV1, I1 & 0b11));
+		ret = vsetq_lane_f32(vgetq_lane_f32(I2 >= 4? inV2 : inV1, I2 & 0b11), ret, 1);
+		ret = vsetq_lane_f32(vgetq_lane_f32(I3 >= 4? inV2 : inV1, I3 & 0b11), ret, 2);
+		ret = vsetq_lane_f32(vgetq_lane_f32(I4 >= 4? inV2 : inV1, I4 & 0b11), ret, 3);
+		return ret;
+	}
+
+	// Specializations
+	template <>
+	JPH_INLINE float32x4_t NeonShuffleFloat32x4<0, 1, 2, 2>(float32x4_t inV1, float32x4_t inV2)
+	{
+		return vcombine_f32(vget_low_f32(inV1), vdup_lane_f32(vget_high_f32(inV1), 0));
+	}
+
+	template <>
+	JPH_INLINE float32x4_t NeonShuffleFloat32x4<0, 1, 3, 3>(float32x4_t inV1, float32x4_t inV2)
+	{
+		return vcombine_f32(vget_low_f32(inV1), vdup_lane_f32(vget_high_f32(inV1), 1));
+	}
+
+	template <>
+	JPH_INLINE float32x4_t NeonShuffleFloat32x4<0, 1, 2, 3>(float32x4_t inV1, float32x4_t inV2)
+	{
+		return inV1;
+	}
+
+	template <>
+	JPH_INLINE float32x4_t NeonShuffleFloat32x4<1, 0, 3, 2>(float32x4_t inV1, float32x4_t inV2)
+	{
+		return vcombine_f32(vrev64_f32(vget_low_f32(inV1)), vrev64_f32(vget_high_f32(inV1)));
+	}
+
+	template <>
+	JPH_INLINE float32x4_t NeonShuffleFloat32x4<2, 2, 1, 0>(float32x4_t inV1, float32x4_t inV2)
+	{
+		return vcombine_f32(vdup_lane_f32(vget_high_f32(inV1), 0), vrev64_f32(vget_low_f32(inV1)));
+	}
+
+	template <>
+	JPH_INLINE float32x4_t NeonShuffleFloat32x4<2, 3, 0, 1>(float32x4_t inV1, float32x4_t inV2)
+	{
+		return vcombine_f32(vget_high_f32(inV1), vget_low_f32(inV1));
+	}
+
+	// Used extensively by cross product
+	template <>
+	JPH_INLINE float32x4_t NeonShuffleFloat32x4<1, 2, 0, 0>(float32x4_t inV1, float32x4_t inV2)
+	{
+		static uint8x16_t table = JPH_NEON_UINT8x16(0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x00, 0x01, 0x02, 0x03, 0x00, 0x01, 0x02, 0x03);
+		return vreinterpretq_f32_u8(vqtbl1q_u8(vreinterpretq_u8_f32(inV1), table));
+	}
+
+	// Shuffle a vector
+	#define JPH_NEON_SHUFFLE_F32x4(vec1, vec2, index1, index2, index3, index4) NeonShuffleFloat32x4<index1, index2, index3, index4>(vec1, vec2)
+	#define JPH_NEON_SHUFFLE_U32x4(vec1, vec2, index1, index2, index3, index4) vreinterpretq_u32_f32((NeonShuffleFloat32x4<index1, index2, index3, index4>(vreinterpretq_f32_u32(vec1), vreinterpretq_f32_u32(vec2))))
+
+	JPH_NAMESPACE_END
+#else
+	// Shuffle a vector
+	#define JPH_NEON_SHUFFLE_F32x4(vec1, vec2, index1, index2, index3, index4) __builtin_shufflevector(vec1, vec2, index1, index2, index3, index4)
+	#define JPH_NEON_SHUFFLE_U32x4(vec1, vec2, index1, index2, index3, index4) __builtin_shufflevector(vec1, vec2, index1, index2, index3, index4)
+#endif
+
+#endif // JPH_USE_NEON
diff --git a/thirdparty/jolt_physics/Jolt/Core/Array.h b/thirdparty/jolt_physics/Jolt/Core/Array.h
new file mode 100644
index 0000000000..dd0a9e1ea0
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/Array.h
@@ -0,0 +1,604 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/STLAllocator.h>
+#include <Jolt/Core/HashCombine.h>
+
+#ifdef JPH_USE_STD_VECTOR
+
+JPH_SUPPRESS_WARNINGS_STD_BEGIN
+#include <vector>
+JPH_SUPPRESS_WARNINGS_STD_END
+
+JPH_NAMESPACE_BEGIN
+
+template <class T, class Allocator = STLAllocator<T>> using Array = std::vector<T, Allocator>;
+
+JPH_NAMESPACE_END
+
+#else
+
+JPH_NAMESPACE_BEGIN
+
+/// Simple replacement for std::vector
+///
+/// Major differences:
+/// - Memory is not initialized to zero (this was causing a lot of page faults when deserializing large MeshShapes / HeightFieldShapes)
+/// - Iterators are simple pointers (for now)
+/// - No exception safety
+/// - No specialization like std::vector<bool> has
+/// - Not all functions have been implemented
+template <class T, class Allocator = STLAllocator<T>>
+class [[nodiscard]] Array : private Allocator
+{
+public:
+	using value_type = T;
+	using allocator_type = Allocator;
+	using size_type = size_t;
+	using difference_type = typename Allocator::difference_type;
+	using pointer = T *;
+	using const_pointer = const T *;
+	using reference = T &;
+	using const_reference = const T &;
+
+	using const_iterator = const T *;
+	using iterator = T *;
+
+private:
+	/// Move elements from one location to another
+	inline void				move(pointer inDestination, pointer inSource, size_type inCount)
+	{
+		if constexpr (std::is_trivially_copyable<T>())
+			memmove(inDestination, inSource, inCount * sizeof(T));
+		else
+		{
+			if (inDestination < inSource)
+			{
+				for (T *destination_end = inDestination + inCount; inDestination < destination_end; ++inDestination, ++inSource)
+				{
+					::new (inDestination) T(std::move(*inSource));
+					inSource->~T();
+				}
+			}
+			else
+			{
+				for (T *destination = inDestination + inCount - 1, *source = inSource + inCount - 1; destination >= inDestination; --destination, --source)
+				{
+					::new (destination) T(std::move(*source));
+					source->~T();
+				}
+			}
+		}
+	}
+
+	/// Reallocate the data block to inNewCapacity
+	inline void				reallocate(size_type inNewCapacity)
+	{
+		JPH_ASSERT(inNewCapacity > 0 && inNewCapacity >= mSize);
+
+		pointer ptr;
+		if constexpr (AllocatorHasReallocate<Allocator>::sValue)
+		{
+			// Reallocate data block
+			ptr = get_allocator().reallocate(mElements, mCapacity, inNewCapacity);
+		}
+		else
+		{
+			// Copy data to a new location
+			ptr = get_allocator().allocate(inNewCapacity);
+			if (mElements != nullptr)
+			{
+				move(ptr, mElements, mSize);
+				get_allocator().deallocate(mElements, mCapacity);
+			}
+		}
+		mElements = ptr;
+		mCapacity = inNewCapacity;
+	}
+
+	/// Destruct elements [inStart, inEnd - 1]
+	inline void				destruct(size_type inStart, size_type inEnd)
+	{
+		if constexpr (!std::is_trivially_destructible<T>())
+			if (inStart < inEnd)
+				for (T *element = mElements + inStart, *element_end = mElements + inEnd; element < element_end; ++element)
+					element->~T();
+	}
+
+public:
+	/// Reserve array space
+	inline void				reserve(size_type inNewSize)
+	{
+		if (mCapacity < inNewSize)
+			reallocate(inNewSize);
+	}
+
+	/// Resize array to new length
+	inline void				resize(size_type inNewSize)
+	{
+		destruct(inNewSize, mSize);
+		reserve(inNewSize);
+
+		if constexpr (!std::is_trivially_constructible<T>())
+			for (T *element = mElements + mSize, *element_end = mElements + inNewSize; element < element_end; ++element)
+				::new (element) T;
+		mSize = inNewSize;
+	}
+
+	/// Resize array to new length and initialize all elements with inValue
+	inline void				resize(size_type inNewSize, const T &inValue)
+	{
+		JPH_ASSERT(&inValue < mElements || &inValue >= mElements + mSize, "Can't pass an element from the array to resize");
+
+		destruct(inNewSize, mSize);
+		reserve(inNewSize);
+
+		for (T *element = mElements + mSize, *element_end = mElements + inNewSize; element < element_end; ++element)
+			::new (element) T(inValue);
+		mSize = inNewSize;
+	}
+
+	/// Destruct all elements and set length to zero
+	inline void				clear()
+	{
+		destruct(0, mSize);
+		mSize = 0;
+	}
+
+private:
+	/// Grow the array by at least inAmount elements
+	inline void				grow(size_type inAmount = 1)
+	{
+		size_type min_size = mSize + inAmount;
+		if (min_size > mCapacity)
+		{
+			size_type new_capacity = max(min_size, mCapacity * 2);
+			reserve(new_capacity);
+		}
+	}
+
+	/// Free memory
+	inline void				free()
+	{
+		get_allocator().deallocate(mElements, mCapacity);
+		mElements = nullptr;
+		mCapacity = 0;
+	}
+
+	/// Destroy all elements and free memory
+	inline void				destroy()
+	{
+		if (mElements != nullptr)
+		{
+			clear();
+			free();
+		}
+	}
+
+public:
+	/// Replace the contents of this array with inBegin .. inEnd
+	template <class Iterator>
+	inline void				assign(Iterator inBegin, Iterator inEnd)
+	{
+		clear();
+		reserve(size_type(std::distance(inBegin, inEnd)));
+
+		for (Iterator element = inBegin; element != inEnd; ++element)
+			::new (&mElements[mSize++]) T(*element);
+	}
+
+	/// Replace the contents of this array with inList
+	inline void				assign(std::initializer_list<T> inList)
+	{
+		clear();
+		reserve(size_type(inList.size()));
+
+		for (const T &v : inList)
+			::new (&mElements[mSize++]) T(v);
+	}
+
+	/// Default constructor
+							Array() = default;
+
+	/// Constructor with allocator
+	explicit inline			Array(const Allocator &inAllocator) :
+		Allocator(inAllocator)
+	{
+	}
+
+	/// Constructor with length
+	explicit inline			Array(size_type inLength, const Allocator &inAllocator = { }) :
+		Allocator(inAllocator)
+	{
+		resize(inLength);
+	}
+
+	/// Constructor with length and value
+	inline					Array(size_type inLength, const T &inValue, const Allocator &inAllocator = { }) :
+		Allocator(inAllocator)
+	{
+		resize(inLength, inValue);
+	}
+
+	/// Constructor from initializer list
+	inline					Array(std::initializer_list<T> inList, const Allocator &inAllocator = { }) :
+		Allocator(inAllocator)
+	{
+		assign(inList);
+	}
+
+	/// Constructor from iterator
+	inline					Array(const_iterator inBegin, const_iterator inEnd, const Allocator &inAllocator = { }) :
+		Allocator(inAllocator)
+	{
+		assign(inBegin, inEnd);
+	}
+
+	/// Copy constructor
+	inline					Array(const Array<T, Allocator> &inRHS) :
+		Allocator(inRHS.get_allocator())
+	{
+		assign(inRHS.begin(), inRHS.end());
+	}
+
+	/// Move constructor
+	inline					Array(Array<T, Allocator> &&inRHS) noexcept :
+		Allocator(std::move(inRHS.get_allocator())),
+		mSize(inRHS.mSize),
+		mCapacity(inRHS.mCapacity),
+		mElements(inRHS.mElements)
+	{
+		inRHS.mSize = 0;
+		inRHS.mCapacity = 0;
+		inRHS.mElements = nullptr;
+	}
+
+	/// Destruct all elements
+	inline					~Array()
+	{
+		destroy();
+	}
+
+	/// Get the allocator
+	inline Allocator &		get_allocator()
+	{
+		return *this;
+	}
+
+	inline const Allocator &get_allocator() const
+	{
+		return *this;
+	}
+
+	/// Add element to the back of the array
+	inline void				push_back(const T &inValue)
+	{
+		JPH_ASSERT(&inValue < mElements || &inValue >= mElements + mSize, "Can't pass an element from the array to push_back");
+
+		grow();
+
+		T *element = mElements + mSize++;
+		::new (element) T(inValue);
+	}
+
+	inline void				push_back(T &&inValue)
+	{
+		grow();
+
+		T *element = mElements + mSize++;
+		::new (element) T(std::move(inValue));
+	}
+
+	/// Construct element at the back of the array
+	template <class... A>
+	inline T &				emplace_back(A &&... inValue)
+	{
+		grow();
+
+		T *element = mElements + mSize++;
+		::new (element) T(std::forward<A>(inValue)...);
+		return *element;
+	}
+
+	/// Remove element from the back of the array
+	inline void				pop_back()
+	{
+		JPH_ASSERT(mSize > 0);
+		mElements[--mSize].~T();
+	}
+
+	/// Returns true if there are no elements in the array
+	inline bool				empty() const
+	{
+		return mSize == 0;
+	}
+
+	/// Returns amount of elements in the array
+	inline size_type		size() const
+	{
+		return mSize;
+	}
+
+	/// Returns maximum amount of elements the array can hold
+	inline size_type		capacity() const
+	{
+		return mCapacity;
+	}
+
+	/// Reduce the capacity of the array to match its size
+	void					shrink_to_fit()
+	{
+		if (mElements != nullptr)
+		{
+			if (mSize == 0)
+				free();
+			else if (mCapacity > mSize)
+				reallocate(mSize);
+		}
+	}
+
+	/// Swap the contents of two arrays
+	void					swap(Array<T, Allocator> &inRHS) noexcept
+	{
+		std::swap(get_allocator(), inRHS.get_allocator());
+		std::swap(mSize, inRHS.mSize);
+		std::swap(mCapacity, inRHS.mCapacity);
+		std::swap(mElements, inRHS.mElements);
+	}
+
+	template <class Iterator>
+	void					insert(const_iterator inPos, Iterator inBegin, Iterator inEnd)
+	{
+		size_type num_elements = size_type(std::distance(inBegin, inEnd));
+		if (num_elements > 0)
+		{
+			// After grow() inPos may be invalid
+			size_type first_element = inPos - mElements;
+
+			grow(num_elements);
+
+			T *element_begin = mElements + first_element;
+			T *element_end = element_begin + num_elements;
+			move(element_end, element_begin, mSize - first_element);
+
+			for (T *element = element_begin; element < element_end; ++element, ++inBegin)
+				::new (element) T(*inBegin);
+
+			mSize += num_elements;
+		}
+	}
+
+	void					insert(const_iterator inPos, const T &inValue)
+	{
+		JPH_ASSERT(&inValue < mElements || &inValue >= mElements + mSize, "Can't pass an element from the array to insert");
+
+		// After grow() inPos may be invalid
+		size_type first_element = inPos - mElements;
+
+		grow();
+
+		T *element = mElements + first_element;
+		move(element + 1, element, mSize - first_element);
+
+		::new (element) T(inValue);
+		mSize++;
+	}
+
+	/// Remove one element from the array
+	void					erase(const_iterator inIter)
+	{
+		size_type p = size_type(inIter - begin());
+		JPH_ASSERT(p < mSize);
+		mElements[p].~T();
+		if (p + 1 < mSize)
+			move(mElements + p, mElements + p + 1, mSize - p - 1);
+		--mSize;
+	}
+
+	/// Remove multiple element from the array
+	void					erase(const_iterator inBegin, const_iterator inEnd)
+	{
+		size_type p = size_type(inBegin - begin());
+		size_type n = size_type(inEnd - inBegin);
+		JPH_ASSERT(inEnd <= end());
+		destruct(p, p + n);
+		if (p + n < mSize)
+			move(mElements + p, mElements + p + n, mSize - p - n);
+		mSize -= n;
+	}
+
+	/// Iterators
+	inline const_iterator	begin() const
+	{
+		return mElements;
+	}
+
+	inline const_iterator	end() const
+	{
+		return mElements + mSize;
+	}
+
+	inline const_iterator	cbegin() const
+	{
+		return mElements;
+	}
+
+	inline const_iterator	cend() const
+	{
+		return mElements + mSize;
+	}
+
+	inline iterator			begin()
+	{
+		return mElements;
+	}
+
+	inline iterator			end()
+	{
+		return mElements + mSize;
+	}
+
+	inline const T *		data() const
+	{
+		return mElements;
+	}
+
+	inline T *				data()
+	{
+		return mElements;
+	}
+
+	/// Access element
+	inline T &				operator [] (size_type inIdx)
+	{
+		JPH_ASSERT(inIdx < mSize);
+		return mElements[inIdx];
+	}
+
+	inline const T &		operator [] (size_type inIdx) const
+	{
+		JPH_ASSERT(inIdx < mSize);
+		return mElements[inIdx];
+	}
+
+	/// Access element
+	inline T &				at(size_type inIdx)
+	{
+		JPH_ASSERT(inIdx < mSize);
+		return mElements[inIdx];
+	}
+
+	inline const T &		at(size_type inIdx) const
+	{
+		JPH_ASSERT(inIdx < mSize);
+		return mElements[inIdx];
+	}
+
+	/// First element in the array
+	inline const T &		front() const
+	{
+		JPH_ASSERT(mSize > 0);
+		return mElements[0];
+	}
+
+	inline T &				front()
+	{
+		JPH_ASSERT(mSize > 0);
+		return mElements[0];
+	}
+
+	/// Last element in the array
+	inline const T &		back() const
+	{
+		JPH_ASSERT(mSize > 0);
+		return mElements[mSize - 1];
+	}
+
+	inline T &				back()
+	{
+		JPH_ASSERT(mSize > 0);
+		return mElements[mSize - 1];
+	}
+
+	/// Assignment operator
+	Array<T, Allocator> &	operator = (const Array<T, Allocator> &inRHS)
+	{
+		if (static_cast<const void *>(this) != static_cast<const void *>(&inRHS))
+			assign(inRHS.begin(), inRHS.end());
+
+		return *this;
+	}
+
+	/// Assignment move operator
+	Array<T, Allocator> &	operator = (Array<T, Allocator> &&inRHS) noexcept
+	{
+		if (static_cast<const void *>(this) != static_cast<const void *>(&inRHS))
+		{
+			destroy();
+
+			get_allocator() = std::move(inRHS.get_allocator());
+
+			mSize = inRHS.mSize;
+			mCapacity = inRHS.mCapacity;
+			mElements = inRHS.mElements;
+
+			inRHS.mSize = 0;
+			inRHS.mCapacity = 0;
+			inRHS.mElements = nullptr;
+		}
+
+		return *this;
+	}
+
+	/// Assignment operator
+	Array<T, Allocator> &	operator = (std::initializer_list<T> inRHS)
+	{
+		assign(inRHS);
+
+		return *this;
+	}
+
+	/// Comparing arrays
+	bool					operator == (const Array<T, Allocator> &inRHS) const
+	{
+		if (mSize != inRHS.mSize)
+			return false;
+		for (size_type i = 0; i < mSize; ++i)
+			if (!(mElements[i] == inRHS.mElements[i]))
+				return false;
+		return true;
+	}
+
+	bool					operator != (const Array<T, Allocator> &inRHS) const
+	{
+		if (mSize != inRHS.mSize)
+			return true;
+		for (size_type i = 0; i < mSize; ++i)
+			if (mElements[i] != inRHS.mElements[i])
+				return true;
+		return false;
+	}
+
+	/// Get hash for this array
+	uint64					GetHash() const
+	{
+		// Hash length first
+		uint64 ret = Hash<uint32> { } (uint32(size()));
+
+		// Then hash elements
+		for (const T *element = mElements, *element_end = mElements + mSize; element < element_end; ++element)
+			HashCombine(ret, *element);
+
+		return ret;
+	}
+
+private:
+	size_type				mSize = 0;
+	size_type				mCapacity = 0;
+	T *						mElements = nullptr;
+};
+
+JPH_NAMESPACE_END
+
+JPH_SUPPRESS_WARNING_PUSH
+JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat")
+
+namespace std
+{
+	/// Declare std::hash for Array
+	template <class T, class Allocator>
+	struct hash<JPH::Array<T, Allocator>>
+	{
+		size_t operator () (const JPH::Array<T, Allocator> &inRHS) const
+		{
+			return std::size_t(inRHS.GetHash());
+		}
+	};
+}
+
+JPH_SUPPRESS_WARNING_POP
+
+#endif // JPH_USE_STD_VECTOR
diff --git a/thirdparty/jolt_physics/Jolt/Core/Atomics.h b/thirdparty/jolt_physics/Jolt/Core/Atomics.h
new file mode 100644
index 0000000000..a53faa5c87
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/Atomics.h
@@ -0,0 +1,44 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_SUPPRESS_WARNINGS_STD_BEGIN
+#include <atomic>
+JPH_SUPPRESS_WARNINGS_STD_END
+
+JPH_NAMESPACE_BEGIN
+
+// Things we're using from STL
+using std::atomic;
+using std::memory_order;
+using std::memory_order_relaxed;
+using std::memory_order_acquire;
+using std::memory_order_release;
+using std::memory_order_acq_rel;
+using std::memory_order_seq_cst;
+
+/// Atomically compute the min(ioAtomic, inValue) and store it in ioAtomic, returns true if value was updated
+template <class T>
+bool AtomicMin(atomic<T> &ioAtomic, const T inValue, const memory_order inMemoryOrder = memory_order_seq_cst)
+{
+	T cur_value = ioAtomic.load(memory_order_relaxed);
+	while (cur_value > inValue)
+		if (ioAtomic.compare_exchange_weak(cur_value, inValue, inMemoryOrder))
+			return true;
+	return false;
+}
+
+/// Atomically compute the max(ioAtomic, inValue) and store it in ioAtomic, returns true if value was updated
+template <class T>
+bool AtomicMax(atomic<T> &ioAtomic, const T inValue, const memory_order inMemoryOrder = memory_order_seq_cst)
+{
+	T cur_value = ioAtomic.load(memory_order_relaxed);
+	while (cur_value < inValue)
+		if (ioAtomic.compare_exchange_weak(cur_value, inValue, inMemoryOrder))
+			return true;
+	return false;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/BinaryHeap.h b/thirdparty/jolt_physics/Jolt/Core/BinaryHeap.h
new file mode 100644
index 0000000000..3c542e7eaa
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/BinaryHeap.h
@@ -0,0 +1,96 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Push a new element into a binary max-heap.
+/// [inBegin, inEnd - 1) must be a a valid heap. Element inEnd - 1 will be inserted into the heap. The heap will be [inBegin, inEnd) after this call.
+/// inPred is a function that returns true if the first element is less or equal than the second element.
+/// See: https://en.wikipedia.org/wiki/Binary_heap
+template <typename Iterator, typename Pred>
+void BinaryHeapPush(Iterator inBegin, Iterator inEnd, Pred inPred)
+{
+	using diff_t = typename std::iterator_traits<Iterator>::difference_type;
+	using elem_t = typename std::iterator_traits<Iterator>::value_type;
+
+	// New heap size
+	diff_t count = std::distance(inBegin, inEnd);
+
+	// Start from the last element
+	diff_t current = count - 1;
+	while (current > 0)
+	{
+		// Get current element
+		elem_t &current_elem = *(inBegin + current);
+
+		// Get parent element
+		diff_t parent = (current - 1) >> 1;
+		elem_t &parent_elem = *(inBegin + parent);
+
+		// Sort them so that the parent is larger than the child
+		if (inPred(parent_elem, current_elem))
+		{
+			std::swap(parent_elem, current_elem);
+			current = parent;
+		}
+		else
+		{
+			// When there's no change, we're done
+			break;
+		}
+	}
+}
+
+/// Pop an element from a binary max-heap.
+/// [inBegin, inEnd) must be a valid heap. The largest element will be removed from the heap. The heap will be [inBegin, inEnd - 1) after this call.
+/// inPred is a function that returns true if the first element is less or equal than the second element.
+/// See: https://en.wikipedia.org/wiki/Binary_heap
+template <typename Iterator, typename Pred>
+void BinaryHeapPop(Iterator inBegin, Iterator inEnd, Pred inPred)
+{
+	using diff_t = typename std::iterator_traits<Iterator>::difference_type;
+
+	// Begin by moving the highest element to the end, this is the popped element
+	std::swap(*(inEnd - 1), *inBegin);
+
+	// New heap size
+	diff_t count = std::distance(inBegin, inEnd) - 1;
+
+	// Start from the root
+	diff_t largest = 0;
+	for (;;)
+	{
+		// Get first child
+		diff_t child = (largest << 1) + 1;
+
+		// Check if we're beyond the end of the heap, if so the 2nd child is also beyond the end
+		if (child >= count)
+			break;
+
+		// Remember the largest element from the previous iteration
+		diff_t prev_largest = largest;
+
+		// Check if first child is bigger, if so select it
+		if (inPred(*(inBegin + largest), *(inBegin + child)))
+			largest = child;
+
+		// Switch to the second child
+		++child;
+
+		// Check if second child is bigger, if so select it
+		if (child < count && inPred(*(inBegin + largest), *(inBegin + child)))
+			largest = child;
+
+		// If there was no change, we're done
+		if (prev_largest == largest)
+			break;
+
+		// Swap element
+		std::swap(*(inBegin + prev_largest), *(inBegin + largest));
+	}
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/ByteBuffer.h b/thirdparty/jolt_physics/Jolt/Core/ByteBuffer.h
new file mode 100644
index 0000000000..48d19703f3
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/ByteBuffer.h
@@ -0,0 +1,74 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/STLAlignedAllocator.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Underlying data type for ByteBuffer
+using ByteBufferVector = Array<uint8, STLAlignedAllocator<uint8, JPH_CACHE_LINE_SIZE>>;
+
+/// Simple byte buffer, aligned to a cache line
+class ByteBuffer : public ByteBufferVector
+{
+public:
+	/// Align the size to a multiple of inSize, returns the length after alignment
+	size_t			Align(size_t inSize)
+	{
+		// Assert power of 2
+		JPH_ASSERT(IsPowerOf2(inSize));
+
+		// Calculate new size and resize buffer
+		size_t s = AlignUp(size(), inSize);
+		resize(s, 0);
+
+		return s;
+	}
+
+	/// Allocate block of data of inSize elements and return the pointer
+	template <class Type>
+	Type *			Allocate(size_t inSize = 1)
+	{
+		// Reserve space
+		size_t s = size();
+		resize(s + inSize * sizeof(Type));
+
+		// Get data pointer
+		Type *data = reinterpret_cast<Type *>(&at(s));
+
+		// Construct elements
+		for (Type *d = data, *d_end = data + inSize; d < d_end; ++d)
+			::new (d) Type;
+
+		// Return pointer
+		return data;
+	}
+
+	/// Append inData to the buffer
+	template <class Type>
+	void			AppendVector(const Array<Type> &inData)
+	{
+		size_t size = inData.size() * sizeof(Type);
+		uint8 *data = Allocate<uint8>(size);
+		memcpy(data, &inData[0], size);
+	}
+
+	/// Get object at inPosition (an offset in bytes)
+	template <class Type>
+	const Type *	Get(size_t inPosition) const
+	{
+		return reinterpret_cast<const Type *>(&at(inPosition));
+	}
+
+	/// Get object at inPosition (an offset in bytes)
+	template <class Type>
+	Type *			Get(size_t inPosition)
+	{
+		return reinterpret_cast<Type *>(&at(inPosition));
+	}
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/Color.cpp b/thirdparty/jolt_physics/Jolt/Core/Color.cpp
new file mode 100644
index 0000000000..93d3cabc75
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/Color.cpp
@@ -0,0 +1,38 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Core/Color.h>
+
+JPH_NAMESPACE_BEGIN
+
+// Predefined colors
+const Color Color::sBlack(0, 0, 0);
+const Color Color::sDarkRed(128, 0, 0);
+const Color Color::sRed(255, 0, 0);
+const Color Color::sDarkGreen(0, 128, 0);
+const Color Color::sGreen(0, 255, 0);
+const Color Color::sDarkBlue(0, 0, 128);
+const Color Color::sBlue(0, 0, 255);
+const Color Color::sYellow(255, 255, 0);
+const Color Color::sPurple(255, 0, 255);
+const Color Color::sCyan(0, 255, 255);
+const Color Color::sOrange(255, 128, 0);
+const Color Color::sDarkOrange(128, 64, 0);
+const Color Color::sGrey(128, 128, 128);
+const Color Color::sLightGrey(192, 192, 192);
+const Color Color::sWhite(255, 255, 255);
+
+// Generated by: http://phrogz.net/css/distinct-colors.html (this algo: https://en.wikipedia.org/wiki/Color_difference#CMC_l:c_.281984.29)
+static constexpr Color sColors[] = { Color(255, 0, 0), Color(204, 143, 102), Color(226, 242, 0), Color(41, 166, 124), Color(0, 170, 255), Color(69, 38, 153), Color(153, 38, 130), Color(229, 57, 80), Color(204, 0, 0), Color(255, 170, 0), Color(85, 128, 0), Color(64, 255, 217), Color(0, 75, 140), Color(161, 115, 230), Color(242, 61, 157), Color(178, 101, 89), Color(140, 94, 0), Color(181, 217, 108), Color(64, 242, 255), Color(77, 117, 153), Color(157, 61, 242), Color(140, 0, 56), Color(127, 57, 32), Color(204, 173, 51), Color(64, 255, 64), Color(38, 145, 153), Color(0, 102, 255), Color(242, 0, 226), Color(153, 77, 107), Color(229, 92, 0), Color(140, 126, 70), Color(0, 179, 71), Color(0, 194, 242), Color(27, 0, 204), Color(230, 115, 222), Color(127, 0, 17) };
+
+Color Color::sGetDistinctColor(int inIndex)
+{
+	JPH_ASSERT(inIndex >= 0);
+
+	return sColors[inIndex % (sizeof(sColors) / sizeof(uint32))];
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/Color.h b/thirdparty/jolt_physics/Jolt/Core/Color.h
new file mode 100644
index 0000000000..d07390de1c
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/Color.h
@@ -0,0 +1,84 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+class Color;
+
+/// Type to use for passing arguments to a function
+using ColorArg = Color;
+
+/// Class that holds an RGBA color with 8-bits per component
+class [[nodiscard]] JPH_EXPORT_GCC_BUG_WORKAROUND Color
+{
+public:
+	/// Constructors
+							Color() = default; ///< Intentionally not initialized for performance reasons
+							Color(const Color &inRHS) = default;
+	Color &					operator = (const Color &inRHS) = default;
+	explicit constexpr		Color(uint32 inColor)													: mU32(inColor) { }
+	constexpr				Color(uint8 inRed, uint8 inGreen, uint8 inBlue, uint8 inAlpha = 255)	: r(inRed), g(inGreen), b(inBlue), a(inAlpha) { }
+	constexpr				Color(ColorArg inRHS, uint8 inAlpha)									: r(inRHS.r), g(inRHS.g), b(inRHS.b), a(inAlpha) { }
+
+	/// Comparison
+	inline bool				operator == (ColorArg inRHS) const										{ return mU32 == inRHS.mU32; }
+	inline bool				operator != (ColorArg inRHS) const										{ return mU32 != inRHS.mU32; }
+
+	/// Convert to uint32
+	uint32					GetUInt32() const														{ return mU32; }
+
+	/// Element access, 0 = red, 1 = green, 2 = blue, 3 = alpha
+	inline uint8			operator () (uint inIdx) const											{ JPH_ASSERT(inIdx < 4); return (&r)[inIdx]; }
+	inline uint8 &			operator () (uint inIdx)												{ JPH_ASSERT(inIdx < 4); return (&r)[inIdx]; }
+
+	/// Multiply two colors
+	inline Color			operator * (const Color &inRHS) const									{ return Color(uint8((uint32(r) * inRHS.r) >> 8), uint8((uint32(g) * inRHS.g) >> 8), uint8((uint32(b) * inRHS.b) >> 8), uint8((uint32(a) * inRHS.a) >> 8)); }
+
+	/// Multiply color with intensity in the range [0, 1]
+	inline Color			operator * (float inIntensity) const									{ return Color(uint8(r * inIntensity), uint8(g * inIntensity), uint8(b * inIntensity), a); }
+
+	/// Convert to Vec4 with range [0, 1]
+	inline Vec4				ToVec4() const															{ return Vec4(r, g, b, a) / 255.0f; }
+
+	/// Get grayscale intensity of color
+	inline uint8			GetIntensity() const													{ return uint8((uint32(r) * 54 + g * 183 + b * 19) >> 8); }
+
+	/// Get a visually distinct color
+	static Color			sGetDistinctColor(int inIndex);
+
+	/// Predefined colors
+	static const Color		sBlack;
+	static const Color		sDarkRed;
+	static const Color		sRed;
+	static const Color		sDarkGreen;
+	static const Color		sGreen;
+	static const Color		sDarkBlue;
+	static const Color		sBlue;
+	static const Color		sYellow;
+	static const Color		sPurple;
+	static const Color		sCyan;
+	static const Color		sOrange;
+	static const Color		sDarkOrange;
+	static const Color		sGrey;
+	static const Color		sLightGrey;
+	static const Color		sWhite;
+
+	union
+	{
+		uint32				mU32;																	///< Combined value for red, green, blue and alpha
+		struct
+		{
+			uint8			r;																		///< Red channel
+			uint8			g;																		///< Green channel
+			uint8			b;																		///< Blue channel
+			uint8			a;																		///< Alpha channel
+		};
+	};
+};
+
+static_assert(std::is_trivial<Color>(), "Is supposed to be a trivial type!");
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/Core.h b/thirdparty/jolt_physics/Jolt/Core/Core.h
new file mode 100644
index 0000000000..a0ad97eeb6
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/Core.h
@@ -0,0 +1,593 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+// Jolt library version
+#define JPH_VERSION_MAJOR 5
+#define JPH_VERSION_MINOR 2
+#define JPH_VERSION_PATCH 1
+
+// Determine which features the library was compiled with
+#ifdef JPH_DOUBLE_PRECISION
+	#define JPH_VERSION_FEATURE_BIT_1 1
+#else
+	#define JPH_VERSION_FEATURE_BIT_1 0
+#endif
+#ifdef JPH_CROSS_PLATFORM_DETERMINISTIC
+	#define JPH_VERSION_FEATURE_BIT_2 1
+#else
+	#define JPH_VERSION_FEATURE_BIT_2 0
+#endif
+#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+	#define JPH_VERSION_FEATURE_BIT_3 1
+#else
+	#define JPH_VERSION_FEATURE_BIT_3 0
+#endif
+#ifdef JPH_PROFILE_ENABLED
+	#define JPH_VERSION_FEATURE_BIT_4 1
+#else
+	#define JPH_VERSION_FEATURE_BIT_4 0
+#endif
+#ifdef JPH_EXTERNAL_PROFILE
+	#define JPH_VERSION_FEATURE_BIT_5 1
+#else
+	#define JPH_VERSION_FEATURE_BIT_5 0
+#endif
+#ifdef JPH_DEBUG_RENDERER
+	#define JPH_VERSION_FEATURE_BIT_6 1
+#else
+	#define JPH_VERSION_FEATURE_BIT_6 0
+#endif
+#ifdef JPH_DISABLE_TEMP_ALLOCATOR
+	#define JPH_VERSION_FEATURE_BIT_7 1
+#else
+	#define JPH_VERSION_FEATURE_BIT_7 0
+#endif
+#ifdef JPH_DISABLE_CUSTOM_ALLOCATOR
+	#define JPH_VERSION_FEATURE_BIT_8 1
+#else
+	#define JPH_VERSION_FEATURE_BIT_8 0
+#endif
+#if defined(JPH_OBJECT_LAYER_BITS) && JPH_OBJECT_LAYER_BITS == 32
+	#define JPH_VERSION_FEATURE_BIT_9 1
+#else
+	#define JPH_VERSION_FEATURE_BIT_9 0
+#endif
+#ifdef JPH_ENABLE_ASSERTS
+	#define JPH_VERSION_FEATURE_BIT_10 1
+#else
+	#define JPH_VERSION_FEATURE_BIT_10 0
+#endif
+#ifdef JPH_OBJECT_STREAM
+	#define JPH_VERSION_FEATURE_BIT_11 1
+#else
+	#define JPH_VERSION_FEATURE_BIT_11 0
+#endif
+#define JPH_VERSION_FEATURES (uint64(JPH_VERSION_FEATURE_BIT_1) | (JPH_VERSION_FEATURE_BIT_2 << 1) | (JPH_VERSION_FEATURE_BIT_3 << 2) | (JPH_VERSION_FEATURE_BIT_4 << 3) | (JPH_VERSION_FEATURE_BIT_5 << 4) | (JPH_VERSION_FEATURE_BIT_6 << 5) | (JPH_VERSION_FEATURE_BIT_7 << 6) | (JPH_VERSION_FEATURE_BIT_8 << 7) | (JPH_VERSION_FEATURE_BIT_9 << 8) | (JPH_VERSION_FEATURE_BIT_10 << 9) | (JPH_VERSION_FEATURE_BIT_11 << 10))
+
+// Combine the version and features in a single ID
+#define JPH_VERSION_ID ((JPH_VERSION_FEATURES << 24) | (JPH_VERSION_MAJOR << 16) | (JPH_VERSION_MINOR << 8) | JPH_VERSION_PATCH)
+
+// Determine platform
+#if defined(JPH_PLATFORM_BLUE)
+	// Correct define already defined, this overrides everything else
+#elif defined(_WIN32) || defined(_WIN64)
+	#include <winapifamily.h>
+	#if WINAPI_FAMILY == WINAPI_FAMILY_APP
+		#define JPH_PLATFORM_WINDOWS_UWP // Building for Universal Windows Platform
+	#endif
+	#define JPH_PLATFORM_WINDOWS
+#elif defined(__ANDROID__) // Android is linux too, so that's why we check it first
+	#define JPH_PLATFORM_ANDROID
+#elif defined(__linux__)
+	#define JPH_PLATFORM_LINUX
+#elif defined(__FreeBSD__)
+	#define JPH_PLATFORM_FREEBSD
+#elif defined(__APPLE__)
+	#include <TargetConditionals.h>
+	#if defined(TARGET_OS_IPHONE) && !TARGET_OS_IPHONE
+		#define JPH_PLATFORM_MACOS
+	#else
+		#define JPH_PLATFORM_IOS
+	#endif
+#elif defined(__EMSCRIPTEN__)
+	#define JPH_PLATFORM_WASM
+#endif
+
+// Platform helper macros
+#ifdef JPH_PLATFORM_ANDROID
+	#define JPH_IF_NOT_ANDROID(x)
+#else
+	#define JPH_IF_NOT_ANDROID(x) x
+#endif
+
+// Determine compiler
+#if defined(__clang__)
+	#define JPH_COMPILER_CLANG
+#elif defined(__GNUC__)
+	#define JPH_COMPILER_GCC
+#elif defined(_MSC_VER)
+	#define JPH_COMPILER_MSVC
+#endif
+
+#if defined(__MINGW64__) || defined (__MINGW32__)
+	#define JPH_COMPILER_MINGW
+#endif
+
+// Detect CPU architecture
+#if defined(__x86_64__) || defined(_M_X64) || defined(__i386__) || defined(_M_IX86)
+	// X86 CPU architecture
+	#define JPH_CPU_X86
+	#if defined(__x86_64__) || defined(_M_X64)
+		#define JPH_CPU_ADDRESS_BITS 64
+	#else
+		#define JPH_CPU_ADDRESS_BITS 32
+	#endif
+	#define JPH_USE_SSE
+	#define JPH_VECTOR_ALIGNMENT 16
+	#define JPH_DVECTOR_ALIGNMENT 32
+
+	// Detect enabled instruction sets
+	#if defined(__AVX512F__) && defined(__AVX512VL__) && defined(__AVX512DQ__) && !defined(JPH_USE_AVX512)
+		#define JPH_USE_AVX512
+	#endif
+	#if (defined(__AVX2__) || defined(JPH_USE_AVX512)) && !defined(JPH_USE_AVX2)
+		#define JPH_USE_AVX2
+	#endif
+	#if (defined(__AVX__) || defined(JPH_USE_AVX2)) && !defined(JPH_USE_AVX)
+		#define JPH_USE_AVX
+	#endif
+	#if (defined(__SSE4_2__) || defined(JPH_USE_AVX)) && !defined(JPH_USE_SSE4_2)
+		#define JPH_USE_SSE4_2
+	#endif
+	#if (defined(__SSE4_1__) || defined(JPH_USE_SSE4_2)) && !defined(JPH_USE_SSE4_1)
+		#define JPH_USE_SSE4_1
+	#endif
+	#if (defined(__F16C__) || defined(JPH_USE_AVX2)) && !defined(JPH_USE_F16C)
+		#define JPH_USE_F16C
+	#endif
+	#if (defined(__LZCNT__) || defined(JPH_USE_AVX2)) && !defined(JPH_USE_LZCNT)
+		#define JPH_USE_LZCNT
+	#endif
+	#if (defined(__BMI__) || defined(JPH_USE_AVX2)) && !defined(JPH_USE_TZCNT)
+		#define JPH_USE_TZCNT
+	#endif
+	#ifndef JPH_CROSS_PLATFORM_DETERMINISTIC // FMA is not compatible with cross platform determinism
+		#if defined(JPH_COMPILER_CLANG) || defined(JPH_COMPILER_GCC)
+			#if defined(__FMA__) && !defined(JPH_USE_FMADD)
+				#define JPH_USE_FMADD
+			#endif
+		#elif defined(JPH_COMPILER_MSVC)
+			#if defined(__AVX2__) && !defined(JPH_USE_FMADD) // AVX2 also enables fused multiply add
+				#define JPH_USE_FMADD
+			#endif
+		#else
+			#error Undefined compiler
+		#endif
+	#endif
+#elif defined(__aarch64__) || defined(_M_ARM64) || defined(__arm__) || defined(_M_ARM)
+	// ARM CPU architecture
+	#define JPH_CPU_ARM
+	#if defined(__aarch64__) || defined(_M_ARM64)
+		#define JPH_CPU_ADDRESS_BITS 64
+		#define JPH_USE_NEON
+		#define JPH_VECTOR_ALIGNMENT 16
+		#define JPH_DVECTOR_ALIGNMENT 32
+	#else
+		#define JPH_CPU_ADDRESS_BITS 32
+		#define JPH_VECTOR_ALIGNMENT 8 // 32-bit ARM does not support aligning on the stack on 16 byte boundaries
+		#define JPH_DVECTOR_ALIGNMENT 8
+	#endif
+#elif defined(JPH_PLATFORM_WASM)
+	// WebAssembly CPU architecture
+	#define JPH_CPU_WASM
+	#define JPH_CPU_ADDRESS_BITS 32
+	#define JPH_VECTOR_ALIGNMENT 16
+	#define JPH_DVECTOR_ALIGNMENT 32
+	#ifdef __wasm_simd128__
+		#define JPH_USE_SSE
+		#define JPH_USE_SSE4_1
+		#define JPH_USE_SSE4_2
+	#endif
+#elif defined(__e2k__)
+	// E2K CPU architecture (MCST Elbrus 2000)
+	#define JPH_CPU_E2K
+	#define JPH_CPU_ADDRESS_BITS 64
+	#define JPH_VECTOR_ALIGNMENT 16
+	#define JPH_DVECTOR_ALIGNMENT 32
+
+	// Compiler flags on e2k arch determine CPU features
+	#if defined(__SSE__) && !defined(JPH_USE_SSE)
+		#define JPH_USE_SSE
+	#endif
+#else
+	#error Unsupported CPU architecture
+#endif
+
+// If this define is set, Jolt is compiled as a shared library
+#ifdef JPH_SHARED_LIBRARY
+	#ifdef JPH_BUILD_SHARED_LIBRARY
+		// While building the shared library, we must export these symbols
+		#if defined(JPH_PLATFORM_WINDOWS) && !defined(JPH_COMPILER_MINGW)
+			#define JPH_EXPORT __declspec(dllexport)
+		#else
+			#define JPH_EXPORT __attribute__ ((visibility ("default")))
+			#if defined(JPH_COMPILER_GCC)
+				// Prevents an issue with GCC attribute parsing (see https://gcc.gnu.org/bugzilla/show_bug.cgi?id=69585)
+				#define JPH_EXPORT_GCC_BUG_WORKAROUND [[gnu::visibility("default")]]
+			#endif
+		#endif
+	#else
+		// When linking against Jolt, we must import these symbols
+		#if defined(JPH_PLATFORM_WINDOWS) && !defined(JPH_COMPILER_MINGW)
+			#define JPH_EXPORT __declspec(dllimport)
+		#else
+			#define JPH_EXPORT __attribute__ ((visibility ("default")))
+			#if defined(JPH_COMPILER_GCC)
+				// Prevents an issue with GCC attribute parsing (see https://gcc.gnu.org/bugzilla/show_bug.cgi?id=69585)
+				#define JPH_EXPORT_GCC_BUG_WORKAROUND [[gnu::visibility("default")]]
+			#endif
+		#endif
+	#endif
+#else
+	// If the define is not set, we use static linking and symbols don't need to be imported or exported
+	#define JPH_EXPORT
+#endif
+
+#ifndef JPH_EXPORT_GCC_BUG_WORKAROUND
+	#define JPH_EXPORT_GCC_BUG_WORKAROUND JPH_EXPORT
+#endif
+
+// Macro used by the RTTI macros to not export a function
+#define JPH_NO_EXPORT
+
+// Pragmas to store / restore the warning state and to disable individual warnings
+#ifdef JPH_COMPILER_CLANG
+#define JPH_PRAGMA(x)					_Pragma(#x)
+#define JPH_SUPPRESS_WARNING_PUSH		JPH_PRAGMA(clang diagnostic push)
+#define JPH_SUPPRESS_WARNING_POP		JPH_PRAGMA(clang diagnostic pop)
+#define JPH_CLANG_SUPPRESS_WARNING(w)	JPH_PRAGMA(clang diagnostic ignored w)
+#if __clang_major__ >= 13
+	#define JPH_CLANG_13_PLUS_SUPPRESS_WARNING(w) JPH_CLANG_SUPPRESS_WARNING(w)
+#else
+	#define JPH_CLANG_13_PLUS_SUPPRESS_WARNING(w)
+#endif
+#if __clang_major__ >= 16
+	#define JPH_CLANG_16_PLUS_SUPPRESS_WARNING(w) JPH_CLANG_SUPPRESS_WARNING(w)
+#else
+	#define JPH_CLANG_16_PLUS_SUPPRESS_WARNING(w)
+#endif
+#else
+#define JPH_CLANG_SUPPRESS_WARNING(w)
+#define JPH_CLANG_13_PLUS_SUPPRESS_WARNING(w)
+#define JPH_CLANG_16_PLUS_SUPPRESS_WARNING(w)
+#endif
+#ifdef JPH_COMPILER_GCC
+#define JPH_PRAGMA(x)					_Pragma(#x)
+#define JPH_SUPPRESS_WARNING_PUSH		JPH_PRAGMA(GCC diagnostic push)
+#define JPH_SUPPRESS_WARNING_POP		JPH_PRAGMA(GCC diagnostic pop)
+#define JPH_GCC_SUPPRESS_WARNING(w)		JPH_PRAGMA(GCC diagnostic ignored w)
+#else
+#define JPH_GCC_SUPPRESS_WARNING(w)
+#endif
+#ifdef JPH_COMPILER_MSVC
+#define JPH_PRAGMA(x)					__pragma(x)
+#define JPH_SUPPRESS_WARNING_PUSH		JPH_PRAGMA(warning (push))
+#define JPH_SUPPRESS_WARNING_POP		JPH_PRAGMA(warning (pop))
+#define JPH_MSVC_SUPPRESS_WARNING(w)	JPH_PRAGMA(warning (disable : w))
+#if _MSC_VER >= 1920 && _MSC_VER < 1930
+	#define JPH_MSVC2019_SUPPRESS_WARNING(w) JPH_MSVC_SUPPRESS_WARNING(w)
+#else
+	#define JPH_MSVC2019_SUPPRESS_WARNING(w)
+#endif
+#else
+#define JPH_MSVC_SUPPRESS_WARNING(w)
+#define JPH_MSVC2019_SUPPRESS_WARNING(w)
+#endif
+
+// Disable common warnings triggered by Jolt when compiling with -Wall
+#define JPH_SUPPRESS_WARNINGS																	\
+	JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat")												\
+	JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat-pedantic")										\
+	JPH_CLANG_SUPPRESS_WARNING("-Wfloat-equal")													\
+	JPH_CLANG_SUPPRESS_WARNING("-Wsign-conversion")												\
+	JPH_CLANG_SUPPRESS_WARNING("-Wold-style-cast")												\
+	JPH_CLANG_SUPPRESS_WARNING("-Wgnu-anonymous-struct")										\
+	JPH_CLANG_SUPPRESS_WARNING("-Wnested-anon-types")											\
+	JPH_CLANG_SUPPRESS_WARNING("-Wglobal-constructors")											\
+	JPH_CLANG_SUPPRESS_WARNING("-Wexit-time-destructors")										\
+	JPH_CLANG_SUPPRESS_WARNING("-Wnonportable-system-include-path")								\
+	JPH_CLANG_SUPPRESS_WARNING("-Wlanguage-extension-token")									\
+	JPH_CLANG_SUPPRESS_WARNING("-Wunused-parameter")											\
+	JPH_CLANG_SUPPRESS_WARNING("-Wformat-nonliteral")											\
+	JPH_CLANG_SUPPRESS_WARNING("-Wcovered-switch-default")										\
+	JPH_CLANG_SUPPRESS_WARNING("-Wcast-align")													\
+	JPH_CLANG_SUPPRESS_WARNING("-Winvalid-offsetof")											\
+	JPH_CLANG_SUPPRESS_WARNING("-Wgnu-zero-variadic-macro-arguments")							\
+	JPH_CLANG_SUPPRESS_WARNING("-Wdocumentation-unknown-command")								\
+	JPH_CLANG_SUPPRESS_WARNING("-Wctad-maybe-unsupported")										\
+	JPH_CLANG_SUPPRESS_WARNING("-Wswitch-default")												\
+	JPH_CLANG_13_PLUS_SUPPRESS_WARNING("-Wdeprecated-copy")										\
+	JPH_CLANG_13_PLUS_SUPPRESS_WARNING("-Wdeprecated-copy-with-dtor")							\
+	JPH_CLANG_16_PLUS_SUPPRESS_WARNING("-Wunsafe-buffer-usage")									\
+	JPH_IF_NOT_ANDROID(JPH_CLANG_SUPPRESS_WARNING("-Wimplicit-int-float-conversion"))			\
+																								\
+	JPH_GCC_SUPPRESS_WARNING("-Wcomment")														\
+	JPH_GCC_SUPPRESS_WARNING("-Winvalid-offsetof")												\
+	JPH_GCC_SUPPRESS_WARNING("-Wclass-memaccess")												\
+	JPH_GCC_SUPPRESS_WARNING("-Wpedantic")														\
+	JPH_GCC_SUPPRESS_WARNING("-Wunused-parameter")												\
+	JPH_GCC_SUPPRESS_WARNING("-Wmaybe-uninitialized")											\
+																								\
+	JPH_MSVC_SUPPRESS_WARNING(4619) /* #pragma warning: there is no warning number 'XXXX' */	\
+	JPH_MSVC_SUPPRESS_WARNING(4514) /* 'X' : unreferenced inline function has been removed */	\
+	JPH_MSVC_SUPPRESS_WARNING(4710) /* 'X' : function not inlined */							\
+	JPH_MSVC_SUPPRESS_WARNING(4711) /* function 'X' selected for automatic inline expansion */	\
+	JPH_MSVC_SUPPRESS_WARNING(4820) /* 'X': 'Y' bytes padding added after data member 'Z' */	\
+	JPH_MSVC_SUPPRESS_WARNING(4100) /* 'X' : unreferenced formal parameter */					\
+	JPH_MSVC_SUPPRESS_WARNING(4626) /* 'X' : assignment operator was implicitly defined as deleted because a base class assignment operator is inaccessible or deleted */ \
+	JPH_MSVC_SUPPRESS_WARNING(5027) /* 'X' : move assignment operator was implicitly defined as deleted because a base class move assignment operator is inaccessible or deleted */ \
+	JPH_MSVC_SUPPRESS_WARNING(4365) /* 'argument' : conversion from 'X' to 'Y', signed / unsigned mismatch */ \
+	JPH_MSVC_SUPPRESS_WARNING(4324) /* 'X' : structure was padded due to alignment specifier */ \
+	JPH_MSVC_SUPPRESS_WARNING(4625) /* 'X' : copy constructor was implicitly defined as deleted because a base class copy constructor is inaccessible or deleted */ \
+	JPH_MSVC_SUPPRESS_WARNING(5026) /* 'X': move constructor was implicitly defined as deleted because a base class move constructor is inaccessible or deleted */ \
+	JPH_MSVC_SUPPRESS_WARNING(4623) /* 'X' : default constructor was implicitly defined as deleted */ \
+	JPH_MSVC_SUPPRESS_WARNING(4201) /* nonstandard extension used: nameless struct/union */		\
+	JPH_MSVC_SUPPRESS_WARNING(4371) /* 'X': layout of class may have changed from a previous version of the compiler due to better packing of member 'Y' */ \
+	JPH_MSVC_SUPPRESS_WARNING(5045) /* Compiler will insert Spectre mitigation for memory load if /Qspectre switch specified */ \
+	JPH_MSVC_SUPPRESS_WARNING(4583) /* 'X': destructor is not implicitly called */				\
+	JPH_MSVC_SUPPRESS_WARNING(4582) /* 'X': constructor is not implicitly called */				\
+	JPH_MSVC_SUPPRESS_WARNING(5219) /* implicit conversion from 'X' to 'Y', possible loss of data  */ \
+	JPH_MSVC_SUPPRESS_WARNING(4826) /* Conversion from 'X *' to 'JPH::uint64' is sign-extended. This may cause unexpected runtime behavior. (32-bit) */ \
+	JPH_MSVC_SUPPRESS_WARNING(5264) /* 'X': 'const' variable is not used */						\
+	JPH_MSVC_SUPPRESS_WARNING(4251) /* class 'X' needs to have DLL-interface to be used by clients of class 'Y' */ \
+	JPH_MSVC_SUPPRESS_WARNING(4738) /* storing 32-bit float result in memory, possible loss of performance */ \
+	JPH_MSVC2019_SUPPRESS_WARNING(5246) /* the initialization of a subobject should be wrapped in braces */
+
+// OS-specific includes
+#if defined(JPH_PLATFORM_WINDOWS)
+	#define JPH_BREAKPOINT		__debugbreak()
+#elif defined(JPH_PLATFORM_BLUE)
+	// Configuration for a popular game console.
+	// This file is not distributed because it would violate an NDA.
+	// Creating one should only be a couple of minutes of work if you have the documentation for the platform
+	// (you only need to define JPH_BREAKPOINT, JPH_PLATFORM_BLUE_GET_TICKS, JPH_PLATFORM_BLUE_MUTEX*, JPH_PLATFORM_BLUE_RWLOCK* and include the right header).
+	#include <Jolt/Core/PlatformBlue.h>
+#elif defined(JPH_PLATFORM_LINUX) || defined(JPH_PLATFORM_ANDROID) || defined(JPH_PLATFORM_MACOS) || defined(JPH_PLATFORM_IOS) || defined(JPH_PLATFORM_FREEBSD)
+	#if defined(JPH_CPU_X86)
+		#define JPH_BREAKPOINT	__asm volatile ("int $0x3")
+	#elif defined(JPH_CPU_ARM)
+		#define JPH_BREAKPOINT	__builtin_trap()
+	#elif defined(JPH_CPU_E2K)
+		#define JPH_BREAKPOINT	__builtin_trap()
+	#endif
+#elif defined(JPH_PLATFORM_WASM)
+	#define JPH_BREAKPOINT		do { } while (false) // Not supported
+#else
+	#error Unknown platform
+#endif
+
+// Begin the JPH namespace
+#define JPH_NAMESPACE_BEGIN																		\
+	JPH_SUPPRESS_WARNING_PUSH																	\
+	JPH_SUPPRESS_WARNINGS																		\
+	namespace JPH {
+
+// End the JPH namespace
+#define JPH_NAMESPACE_END																		\
+	}																							\
+	JPH_SUPPRESS_WARNING_POP
+
+// Suppress warnings generated by the standard template library
+#define JPH_SUPPRESS_WARNINGS_STD_BEGIN															\
+	JPH_SUPPRESS_WARNING_PUSH																	\
+	JPH_MSVC_SUPPRESS_WARNING(4365)																\
+	JPH_MSVC_SUPPRESS_WARNING(4619)																\
+	JPH_MSVC_SUPPRESS_WARNING(4710)																\
+	JPH_MSVC_SUPPRESS_WARNING(4711)																\
+	JPH_MSVC_SUPPRESS_WARNING(4820)																\
+	JPH_MSVC_SUPPRESS_WARNING(4514)																\
+	JPH_MSVC_SUPPRESS_WARNING(5262)																\
+	JPH_MSVC_SUPPRESS_WARNING(5264)																\
+	JPH_MSVC_SUPPRESS_WARNING(4738)
+
+#define JPH_SUPPRESS_WARNINGS_STD_END															\
+	JPH_SUPPRESS_WARNING_POP
+
+// Standard C++ includes
+JPH_SUPPRESS_WARNINGS_STD_BEGIN
+#include <float.h>
+#include <limits.h>
+#include <string.h>
+#include <utility>
+#include <cmath>
+#include <sstream>
+#include <functional>
+#include <algorithm>
+#include <cstdint>
+#ifdef JPH_COMPILER_MSVC
+	#include <malloc.h> // for alloca
+#endif
+#if defined(JPH_USE_SSE)
+	#include <immintrin.h>
+#elif defined(JPH_USE_NEON)
+	#ifdef JPH_COMPILER_MSVC
+		#include <intrin.h>
+		#include <arm64_neon.h>
+	#else
+		#include <arm_neon.h>
+	#endif
+#endif
+JPH_SUPPRESS_WARNINGS_STD_END
+
+JPH_NAMESPACE_BEGIN
+
+// Commonly used STL types
+using std::min;
+using std::max;
+using std::abs;
+using std::sqrt;
+using std::ceil;
+using std::floor;
+using std::trunc;
+using std::round;
+using std::fmod;
+using std::string_view;
+using std::function;
+using std::numeric_limits;
+using std::isfinite;
+using std::isnan;
+using std::ostream;
+using std::istream;
+
+// Standard types
+using uint = unsigned int;
+using uint8 = std::uint8_t;
+using uint16 = std::uint16_t;
+using uint32 = std::uint32_t;
+using uint64 = std::uint64_t;
+
+// Assert sizes of types
+static_assert(sizeof(uint) >= 4, "Invalid size of uint");
+static_assert(sizeof(uint8) == 1, "Invalid size of uint8");
+static_assert(sizeof(uint16) == 2, "Invalid size of uint16");
+static_assert(sizeof(uint32) == 4, "Invalid size of uint32");
+static_assert(sizeof(uint64) == 8, "Invalid size of uint64");
+static_assert(sizeof(void *) == (JPH_CPU_ADDRESS_BITS == 64? 8 : 4), "Invalid size of pointer" );
+
+// Determine if we want extra debugging code to be active
+#if !defined(NDEBUG) && !defined(JPH_NO_DEBUG)
+	#define JPH_DEBUG
+#endif
+
+// Define inline macro
+#if defined(JPH_NO_FORCE_INLINE)
+	#define JPH_INLINE inline
+#elif defined(JPH_COMPILER_CLANG)
+	#define JPH_INLINE __inline__ __attribute__((always_inline))
+#elif defined(JPH_COMPILER_GCC)
+	// On gcc 14 using always_inline in debug mode causes error: "inlining failed in call to 'always_inline' 'XXX': function not considered for inlining"
+	// See: https://github.com/jrouwe/JoltPhysics/issues/1096
+	#if __GNUC__ >= 14 && defined(JPH_DEBUG)
+		#define JPH_INLINE inline
+	#else
+		#define JPH_INLINE __inline__ __attribute__((always_inline))
+	#endif
+#elif defined(JPH_COMPILER_MSVC)
+	#define JPH_INLINE __forceinline
+#else
+	#error Undefined
+#endif
+
+// Cache line size (used for aligning to cache line)
+#ifndef JPH_CACHE_LINE_SIZE
+	#define JPH_CACHE_LINE_SIZE 64
+#endif
+
+// Define macro to get current function name
+#if defined(JPH_COMPILER_CLANG) || defined(JPH_COMPILER_GCC)
+	#define JPH_FUNCTION_NAME	__PRETTY_FUNCTION__
+#elif defined(JPH_COMPILER_MSVC)
+	#define JPH_FUNCTION_NAME	__FUNCTION__
+#else
+	#error Undefined
+#endif
+
+// Stack allocation
+#define JPH_STACK_ALLOC(n)		alloca(n)
+
+// Shorthand for #ifdef JPH_DEBUG / #endif
+#ifdef JPH_DEBUG
+	#define JPH_IF_DEBUG(...)	__VA_ARGS__
+	#define JPH_IF_NOT_DEBUG(...)
+#else
+	#define JPH_IF_DEBUG(...)
+	#define JPH_IF_NOT_DEBUG(...) __VA_ARGS__
+#endif
+
+// Shorthand for #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED / #endif
+#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+	#define JPH_IF_FLOATING_POINT_EXCEPTIONS_ENABLED(...)	__VA_ARGS__
+#else
+	#define JPH_IF_FLOATING_POINT_EXCEPTIONS_ENABLED(...)
+#endif
+
+// Helper macros to detect if we're running in single or double precision mode
+#ifdef JPH_DOUBLE_PRECISION
+	#define JPH_IF_SINGLE_PRECISION(...)
+	#define JPH_IF_SINGLE_PRECISION_ELSE(s, d) d
+	#define JPH_IF_DOUBLE_PRECISION(...) __VA_ARGS__
+#else
+	#define JPH_IF_SINGLE_PRECISION(...) __VA_ARGS__
+	#define JPH_IF_SINGLE_PRECISION_ELSE(s, d) s
+	#define JPH_IF_DOUBLE_PRECISION(...)
+#endif
+
+// Helper macro to detect if the debug renderer is active
+#ifdef JPH_DEBUG_RENDERER
+	#define JPH_IF_DEBUG_RENDERER(...) __VA_ARGS__
+	#define JPH_IF_NOT_DEBUG_RENDERER(...)
+#else
+	#define JPH_IF_DEBUG_RENDERER(...)
+	#define JPH_IF_NOT_DEBUG_RENDERER(...) __VA_ARGS__
+#endif
+
+// Macro to indicate that a parameter / variable is unused
+#define JPH_UNUSED(x)			(void)x
+
+// Macro to enable floating point precise mode and to disable fused multiply add instructions
+#if defined(JPH_COMPILER_GCC) || defined(JPH_CROSS_PLATFORM_DETERMINISTIC)
+	// We compile without -ffast-math and -ffp-contract=fast, so we don't need to disable anything
+	#define JPH_PRECISE_MATH_ON
+	#define JPH_PRECISE_MATH_OFF
+#elif defined(JPH_COMPILER_CLANG)
+	// We compile without -ffast-math because pragma float_control(precise, on) doesn't seem to actually negate all of the -ffast-math effects and causes the unit tests to fail (even if the pragma is added to all files)
+	// On clang 14 and later we can turn off float contraction through a pragma (before it was buggy), so if FMA is on we can disable it through this macro
+	#if (defined(JPH_CPU_ARM) && !defined(JPH_PLATFORM_ANDROID) && __clang_major__ >= 16) || (defined(JPH_CPU_X86) && __clang_major__ >= 14)
+		#define JPH_PRECISE_MATH_ON						\
+			_Pragma("float_control(precise, on, push)")	\
+			_Pragma("clang fp contract(off)")
+		#define JPH_PRECISE_MATH_OFF					\
+			_Pragma("float_control(pop)")
+	#elif __clang_major__ >= 14 && (defined(JPH_USE_FMADD) || defined(FP_FAST_FMA))
+		#define JPH_PRECISE_MATH_ON						\
+			_Pragma("clang fp contract(off)")
+		#define JPH_PRECISE_MATH_OFF					\
+			_Pragma("clang fp contract(on)")
+	#else
+		#define JPH_PRECISE_MATH_ON
+		#define JPH_PRECISE_MATH_OFF
+	#endif
+#elif defined(JPH_COMPILER_MSVC)
+	// Unfortunately there is no way to push the state of fp_contract, so we have to assume it was turned on before JPH_PRECISE_MATH_ON
+	#define JPH_PRECISE_MATH_ON							\
+		__pragma(float_control(precise, on, push))		\
+		__pragma(fp_contract(off))
+	#define JPH_PRECISE_MATH_OFF						\
+		__pragma(fp_contract(on))						\
+		__pragma(float_control(pop))
+#else
+	#error Undefined
+#endif
+
+// Check if Thread Sanitizer is enabled
+#ifdef __has_feature
+	#if __has_feature(thread_sanitizer)
+		#define JPH_TSAN_ENABLED
+	#endif
+#else
+	#ifdef __SANITIZE_THREAD__
+		#define JPH_TSAN_ENABLED
+	#endif
+#endif
+
+// Attribute to disable Thread Sanitizer for a particular function
+#ifdef JPH_TSAN_ENABLED
+	#define JPH_TSAN_NO_SANITIZE __attribute__((no_sanitize("thread")))
+#else
+	#define JPH_TSAN_NO_SANITIZE
+#endif
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/FPControlWord.h b/thirdparty/jolt_physics/Jolt/Core/FPControlWord.h
new file mode 100644
index 0000000000..0c8b3f1ff0
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/FPControlWord.h
@@ -0,0 +1,135 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/NonCopyable.h>
+
+JPH_NAMESPACE_BEGIN
+
+#if defined(JPH_CPU_WASM)
+
+// Not supported
+
+#elif defined(JPH_USE_SSE)
+
+/// Helper class that needs to be put on the stack to update the state of the floating point control word.
+/// This state is kept per thread.
+template <uint Value, uint Mask>
+class FPControlWord : public NonCopyable
+{
+public:
+				FPControlWord()
+	{
+		mPrevState = _mm_getcsr();
+		_mm_setcsr((mPrevState & ~Mask) | Value);
+	}
+
+				~FPControlWord()
+	{
+		_mm_setcsr((_mm_getcsr() & ~Mask) | (mPrevState & Mask));
+	}
+
+private:
+	uint		mPrevState;
+};
+
+#elif defined(JPH_CPU_ARM) && defined(JPH_COMPILER_MSVC)
+
+/// Helper class that needs to be put on the stack to update the state of the floating point control word.
+/// This state is kept per thread.
+template <unsigned int Value, unsigned int Mask>
+class FPControlWord : public NonCopyable
+{
+public:
+				FPControlWord()
+	{
+		// Read state before change
+		_controlfp_s(&mPrevState, 0, 0);
+
+		// Update the state
+		unsigned int dummy;
+		_controlfp_s(&dummy, Value, Mask);
+	}
+
+				~FPControlWord()
+	{
+		// Restore state
+		unsigned int dummy;
+		_controlfp_s(&dummy, mPrevState, Mask);
+	}
+
+private:
+	unsigned int mPrevState;
+};
+
+#elif defined(JPH_CPU_ARM) && defined(JPH_USE_NEON)
+
+/// Helper class that needs to be put on the stack to update the state of the floating point control word.
+/// This state is kept per thread.
+template <uint64 Value, uint64 Mask>
+class FPControlWord : public NonCopyable
+{
+public:
+				FPControlWord()
+	{
+		uint64 val;
+		asm volatile("mrs %0, fpcr" : "=r" (val));
+		mPrevState = val;
+		val &= ~Mask;
+		val |= Value;
+		asm volatile("msr fpcr, %0" : /* no output */ : "r" (val));
+	}
+
+				~FPControlWord()
+	{
+		uint64 val;
+		asm volatile("mrs %0, fpcr" : "=r" (val));
+		val &= ~Mask;
+		val |= mPrevState & Mask;
+		asm volatile("msr fpcr, %0" : /* no output */ : "r" (val));
+	}
+
+private:
+	uint64		mPrevState;
+};
+
+#elif defined(JPH_CPU_ARM)
+
+/// Helper class that needs to be put on the stack to update the state of the floating point control word.
+/// This state is kept per thread.
+template <uint32 Value, uint32 Mask>
+class FPControlWord : public NonCopyable
+{
+public:
+	FPControlWord()
+	{
+		uint32 val;
+		asm volatile("vmrs %0, fpscr" : "=r" (val));
+		mPrevState = val;
+		val &= ~Mask;
+		val |= Value;
+		asm volatile("vmsr fpscr, %0" : /* no output */ : "r" (val));
+	}
+
+	~FPControlWord()
+	{
+		uint32 val;
+		asm volatile("vmrs %0, fpscr" : "=r" (val));
+		val &= ~Mask;
+		val |= mPrevState & Mask;
+		asm volatile("vmsr fpscr, %0" : /* no output */ : "r" (val));
+	}
+
+private:
+	uint32		mPrevState;
+};
+
+#else
+
+#error Unsupported CPU architecture
+
+#endif
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/FPException.h b/thirdparty/jolt_physics/Jolt/Core/FPException.h
new file mode 100644
index 0000000000..3083f05c05
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/FPException.h
@@ -0,0 +1,74 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/FPControlWord.h>
+
+JPH_NAMESPACE_BEGIN
+
+#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+
+#if defined(JPH_CPU_WASM)
+
+// Not supported
+class FPExceptionsEnable { };
+class FPExceptionDisableInvalid { };
+class FPExceptionDisableDivByZero { };
+
+#elif defined(JPH_USE_SSE)
+
+/// Enable floating point divide by zero exception and exceptions on invalid numbers
+class FPExceptionsEnable : public FPControlWord<0, _MM_MASK_DIV_ZERO | _MM_MASK_INVALID> { };
+
+/// Disable invalid floating point value exceptions
+class FPExceptionDisableInvalid : public FPControlWord<_MM_MASK_INVALID, _MM_MASK_INVALID> { };
+
+/// Disable division by zero floating point exceptions
+class FPExceptionDisableDivByZero : public FPControlWord<_MM_MASK_DIV_ZERO, _MM_MASK_DIV_ZERO> { };
+
+#elif defined(JPH_CPU_ARM) && defined(JPH_COMPILER_MSVC)
+
+/// Enable floating point divide by zero exception and exceptions on invalid numbers
+class FPExceptionsEnable : public FPControlWord<0, _EM_INVALID | _EM_ZERODIVIDE> { };
+
+/// Disable invalid floating point value exceptions
+class FPExceptionDisableInvalid : public FPControlWord<_EM_INVALID, _EM_INVALID> { };
+
+/// Disable division by zero floating point exceptions
+class FPExceptionDisableDivByZero : public FPControlWord<_EM_ZERODIVIDE, _EM_ZERODIVIDE> { };
+
+#elif defined(JPH_CPU_ARM)
+
+/// Invalid operation exception bit
+static constexpr uint64 FP_IOE = 1 << 8;
+
+/// Enable divide by zero exception bit
+static constexpr uint64 FP_DZE = 1 << 9;
+
+/// Enable floating point divide by zero exception and exceptions on invalid numbers
+class FPExceptionsEnable : public FPControlWord<FP_IOE | FP_DZE, FP_IOE | FP_DZE> { };
+
+/// Disable invalid floating point value exceptions
+class FPExceptionDisableInvalid : public FPControlWord<0, FP_IOE> { };
+
+/// Disable division by zero floating point exceptions
+class FPExceptionDisableDivByZero : public FPControlWord<0, FP_DZE> { };
+
+#else
+
+#error Unsupported CPU architecture
+
+#endif
+
+#else
+
+/// Dummy implementations
+class FPExceptionsEnable { };
+class FPExceptionDisableInvalid { };
+class FPExceptionDisableDivByZero { };
+
+#endif
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/FPFlushDenormals.h b/thirdparty/jolt_physics/Jolt/Core/FPFlushDenormals.h
new file mode 100644
index 0000000000..672a19dbec
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/FPFlushDenormals.h
@@ -0,0 +1,41 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/FPControlWord.h>
+
+JPH_NAMESPACE_BEGIN
+
+#if defined(JPH_CPU_WASM)
+
+// Not supported
+class FPFlushDenormals { };
+
+#elif defined(JPH_USE_SSE)
+
+/// Helper class that needs to be put on the stack to enable flushing denormals to zero
+/// This can make floating point operations much faster when working with very small numbers
+class FPFlushDenormals : public FPControlWord<_MM_FLUSH_ZERO_ON, _MM_FLUSH_ZERO_MASK> { };
+
+#elif defined(JPH_CPU_ARM) && defined(JPH_COMPILER_MSVC)
+
+class FPFlushDenormals : public FPControlWord<_DN_FLUSH, _MCW_DN> { };
+
+#elif defined(JPH_CPU_ARM)
+
+/// Flush denormals to zero bit
+static constexpr uint64 FP_FZ = 1 << 24;
+
+/// Helper class that needs to be put on the stack to enable flushing denormals to zero
+/// This can make floating point operations much faster when working with very small numbers
+class FPFlushDenormals : public FPControlWord<FP_FZ, FP_FZ> { };
+
+#else
+
+#error Unsupported CPU architecture
+
+#endif
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/Factory.cpp b/thirdparty/jolt_physics/Jolt/Core/Factory.cpp
new file mode 100644
index 0000000000..1890c037f3
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/Factory.cpp
@@ -0,0 +1,92 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Core/Factory.h>
+
+JPH_NAMESPACE_BEGIN
+
+Factory *Factory::sInstance = nullptr;
+
+void *Factory::CreateObject(const char *inName)
+{
+	const RTTI *ci = Find(inName);
+	return ci != nullptr? ci->CreateObject() : nullptr;
+}
+
+const RTTI *Factory::Find(const char *inName)
+{
+	ClassNameMap::iterator c = mClassNameMap.find(inName);
+	return c != mClassNameMap.end()? c->second : nullptr;
+}
+
+const RTTI *Factory::Find(uint32 inHash)
+{
+	ClassHashMap::iterator c = mClassHashMap.find(inHash);
+	return c != mClassHashMap.end()? c->second : nullptr;
+}
+
+bool Factory::Register(const RTTI *inRTTI)
+{
+	// Check if we already know the type
+	if (Find(inRTTI->GetName()) != nullptr)
+		return true;
+
+	// Insert this class by name
+	mClassNameMap.try_emplace(inRTTI->GetName(), inRTTI);
+
+	// Insert this class by hash
+	if (!mClassHashMap.try_emplace(inRTTI->GetHash(), inRTTI).second)
+	{
+		JPH_ASSERT(false, "Hash collision registering type!");
+		return false;
+	}
+
+	// Register base classes
+	for (int i = 0; i < inRTTI->GetBaseClassCount(); ++i)
+		if (!Register(inRTTI->GetBaseClass(i)))
+			return false;
+
+#ifdef JPH_OBJECT_STREAM
+	// Register attribute classes
+	for (int i = 0; i < inRTTI->GetAttributeCount(); ++i)
+	{
+		const RTTI *rtti = inRTTI->GetAttribute(i).GetMemberPrimitiveType();
+		if (rtti != nullptr && !Register(rtti))
+			return false;
+	}
+#endif // JPH_OBJECT_STREAM
+
+	return true;
+}
+
+bool Factory::Register(const RTTI **inRTTIs, uint inNumber)
+{
+	mClassHashMap.reserve(mClassHashMap.size() + inNumber);
+	mClassNameMap.reserve(mClassNameMap.size() + inNumber);
+
+	for (const RTTI **rtti = inRTTIs; rtti < inRTTIs + inNumber; ++rtti)
+		if (!Register(*rtti))
+			return false;
+
+	return true;
+}
+
+void Factory::Clear()
+{
+	mClassNameMap.clear();
+	mClassHashMap.clear();
+}
+
+Array<const RTTI *> Factory::GetAllClasses() const
+{
+	Array<const RTTI *> all_classes;
+	all_classes.reserve(mClassNameMap.size());
+	for (const ClassNameMap::value_type &c : mClassNameMap)
+		all_classes.push_back(c.second);
+	return all_classes;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/Factory.h b/thirdparty/jolt_physics/Jolt/Core/Factory.h
new file mode 100644
index 0000000000..557f38173f
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/Factory.h
@@ -0,0 +1,54 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/RTTI.h>
+#include <Jolt/Core/UnorderedMap.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// This class is responsible for creating instances of classes based on their name or hash and is mainly used for deserialization of saved data.
+class JPH_EXPORT Factory
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Create an object
+	void *						CreateObject(const char *inName);
+
+	/// Find type info for a specific class by name
+	const RTTI *				Find(const char *inName);
+
+	/// Find type info for a specific class by hash
+	const RTTI *				Find(uint32 inHash);
+
+	/// Register an object with the factory. Returns false on failure.
+	bool						Register(const RTTI *inRTTI);
+
+	/// Register a list of objects with the factory. Returns false on failure.
+	bool						Register(const RTTI **inRTTIs, uint inNumber);
+
+	/// Unregisters all types
+	void						Clear();
+
+	/// Get all registered classes
+	Array<const RTTI *>			GetAllClasses() const;
+
+	/// Singleton factory instance
+	static Factory *			sInstance;
+
+private:
+	using ClassNameMap = UnorderedMap<string_view, const RTTI *>;
+
+	using ClassHashMap = UnorderedMap<uint32, const RTTI *>;
+
+	/// Map of class names to type info
+	ClassNameMap				mClassNameMap;
+
+	// Map of class hash to type info
+	ClassHashMap				mClassHashMap;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/FixedSizeFreeList.h b/thirdparty/jolt_physics/Jolt/Core/FixedSizeFreeList.h
new file mode 100644
index 0000000000..51f2d1368b
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/FixedSizeFreeList.h
@@ -0,0 +1,122 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/NonCopyable.h>
+#include <Jolt/Core/Mutex.h>
+#include <Jolt/Core/Atomics.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that allows lock free creation / destruction of objects (unless a new page of objects needs to be allocated)
+/// It contains a fixed pool of objects and also allows batching up a lot of objects to be destroyed
+/// and doing the actual free in a single atomic operation
+template <typename Object>
+class FixedSizeFreeList : public NonCopyable
+{
+private:
+	/// Storage type for an Object
+	struct ObjectStorage
+	{
+		/// The object we're storing
+		Object				mObject;
+
+		/// When the object is freed (or in the process of being freed as a batch) this will contain the next free object
+		/// When an object is in use it will contain the object's index in the free list
+		atomic<uint32>		mNextFreeObject;
+	};
+
+	static_assert(alignof(ObjectStorage) == alignof(Object), "Object not properly aligned");
+
+	/// Access the object storage given the object index
+	const ObjectStorage &	GetStorage(uint32 inObjectIndex) const	{ return mPages[inObjectIndex >> mPageShift][inObjectIndex & mObjectMask]; }
+	ObjectStorage &			GetStorage(uint32 inObjectIndex)		{ return mPages[inObjectIndex >> mPageShift][inObjectIndex & mObjectMask]; }
+
+	/// Size (in objects) of a single page
+	uint32					mPageSize;
+
+	/// Number of bits to shift an object index to the right to get the page number
+	uint32					mPageShift;
+
+	/// Mask to and an object index with to get the page number
+	uint32					mObjectMask;
+
+	/// Total number of pages that are usable
+	uint32					mNumPages;
+
+	/// Total number of objects that have been allocated
+	uint32					mNumObjectsAllocated;
+
+	/// Array of pages of objects
+	ObjectStorage **		mPages = nullptr;
+
+	/// Mutex that is used to allocate a new page if the storage runs out
+	/// This variable is aligned to the cache line to prevent false sharing with
+	/// the constants used to index into the list via `Get()`.
+	alignas(JPH_CACHE_LINE_SIZE) Mutex mPageMutex;
+
+	/// Number of objects that we currently have in the free list / new pages
+#ifdef JPH_ENABLE_ASSERTS
+	atomic<uint32>			mNumFreeObjects;
+#endif // JPH_ENABLE_ASSERTS
+
+	/// Simple counter that makes the first free object pointer update with every CAS so that we don't suffer from the ABA problem
+	atomic<uint32>			mAllocationTag;
+
+	/// Index of first free object, the first 32 bits of an object are used to point to the next free object
+	atomic<uint64>			mFirstFreeObjectAndTag;
+
+	/// The first free object to use when the free list is empty (may need to allocate a new page)
+	atomic<uint32>			mFirstFreeObjectInNewPage;
+
+public:
+	/// Invalid index
+	static const uint32		cInvalidObjectIndex = 0xffffffff;
+
+	/// Size of an object + bookkeeping for the freelist
+	static const int		ObjectStorageSize = sizeof(ObjectStorage);
+
+	/// Destructor
+	inline					~FixedSizeFreeList();
+
+	/// Initialize the free list, up to inMaxObjects can be allocated
+	inline void				Init(uint inMaxObjects, uint inPageSize);
+
+	/// Lockless construct a new object, inParameters are passed on to the constructor
+	template <typename... Parameters>
+	inline uint32			ConstructObject(Parameters &&... inParameters);
+
+	/// Lockless destruct an object and return it to the free pool
+	inline void				DestructObject(uint32 inObjectIndex);
+
+	/// Lockless destruct an object and return it to the free pool
+	inline void				DestructObject(Object *inObject);
+
+	/// A batch of objects that can be destructed
+	struct Batch
+	{
+		uint32				mFirstObjectIndex = cInvalidObjectIndex;
+		uint32				mLastObjectIndex = cInvalidObjectIndex;
+		uint32				mNumObjects = 0;
+	};
+
+	/// Add a object to an existing batch to be destructed.
+	/// Adding objects to a batch does not destroy or modify the objects, this will merely link them
+	/// so that the entire batch can be returned to the free list in a single atomic operation
+	inline void				AddObjectToBatch(Batch &ioBatch, uint32 inObjectIndex);
+
+	/// Lockless destruct batch of objects
+	inline void				DestructObjectBatch(Batch &ioBatch);
+
+	/// Access an object by index.
+	inline Object &			Get(uint32 inObjectIndex)				{ return GetStorage(inObjectIndex).mObject; }
+
+	/// Access an object by index.
+	inline const Object &	Get(uint32 inObjectIndex) const			{ return GetStorage(inObjectIndex).mObject; }
+};
+
+JPH_NAMESPACE_END
+
+#include "FixedSizeFreeList.inl"
diff --git a/thirdparty/jolt_physics/Jolt/Core/FixedSizeFreeList.inl b/thirdparty/jolt_physics/Jolt/Core/FixedSizeFreeList.inl
new file mode 100644
index 0000000000..29d4734c00
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/FixedSizeFreeList.inl
@@ -0,0 +1,215 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+JPH_NAMESPACE_BEGIN
+
+template <typename Object>
+FixedSizeFreeList<Object>::~FixedSizeFreeList()
+{
+	// Check if we got our Init call
+	if (mPages != nullptr)
+	{
+		// Ensure everything is freed before the freelist is destructed
+		JPH_ASSERT(mNumFreeObjects.load(memory_order_relaxed) == mNumPages * mPageSize);
+
+		// Free memory for pages
+		uint32 num_pages = mNumObjectsAllocated / mPageSize;
+		for (uint32 page = 0; page < num_pages; ++page)
+			AlignedFree(mPages[page]);
+		Free(mPages);
+	}
+}
+
+template <typename Object>
+void FixedSizeFreeList<Object>::Init(uint inMaxObjects, uint inPageSize)
+{
+	// Check sanity
+	JPH_ASSERT(inPageSize > 0 && IsPowerOf2(inPageSize));
+	JPH_ASSERT(mPages == nullptr);
+
+	// Store configuration parameters
+	mNumPages = (inMaxObjects + inPageSize - 1) / inPageSize;
+	mPageSize = inPageSize;
+	mPageShift = CountTrailingZeros(inPageSize);
+	mObjectMask = inPageSize - 1;
+	JPH_IF_ENABLE_ASSERTS(mNumFreeObjects = mNumPages * inPageSize;)
+
+	// Allocate page table
+	mPages = reinterpret_cast<ObjectStorage **>(Allocate(mNumPages * sizeof(ObjectStorage *)));
+
+	// We didn't yet use any objects of any page
+	mNumObjectsAllocated = 0;
+	mFirstFreeObjectInNewPage = 0;
+
+	// Start with 1 as the first tag
+	mAllocationTag = 1;
+
+	// Set first free object (with tag 0)
+	mFirstFreeObjectAndTag = cInvalidObjectIndex;
+}
+
+template <typename Object>
+template <typename... Parameters>
+uint32 FixedSizeFreeList<Object>::ConstructObject(Parameters &&... inParameters)
+{
+	for (;;)
+	{
+		// Get first object from the linked list
+		uint64 first_free_object_and_tag = mFirstFreeObjectAndTag.load(memory_order_acquire);
+		uint32 first_free = uint32(first_free_object_and_tag);
+		if (first_free == cInvalidObjectIndex)
+		{
+			// The free list is empty, we take an object from the page that has never been used before
+			first_free = mFirstFreeObjectInNewPage.fetch_add(1, memory_order_relaxed);
+			if (first_free >= mNumObjectsAllocated)
+			{
+				// Allocate new page
+				lock_guard lock(mPageMutex);
+				while (first_free >= mNumObjectsAllocated)
+				{
+					uint32 next_page = mNumObjectsAllocated / mPageSize;
+					if (next_page == mNumPages)
+						return cInvalidObjectIndex; // Out of space!
+					mPages[next_page] = reinterpret_cast<ObjectStorage *>(AlignedAllocate(mPageSize * sizeof(ObjectStorage), max<size_t>(alignof(ObjectStorage), JPH_CACHE_LINE_SIZE)));
+					mNumObjectsAllocated += mPageSize;
+				}
+			}
+
+			// Allocation successful
+			JPH_IF_ENABLE_ASSERTS(mNumFreeObjects.fetch_sub(1, memory_order_relaxed);)
+			ObjectStorage &storage = GetStorage(first_free);
+			::new (&storage.mObject) Object(std::forward<Parameters>(inParameters)...);
+			storage.mNextFreeObject.store(first_free, memory_order_release);
+			return first_free;
+		}
+		else
+		{
+			// Load next pointer
+			uint32 new_first_free = GetStorage(first_free).mNextFreeObject.load(memory_order_acquire);
+
+			// Construct a new first free object tag
+			uint64 new_first_free_object_and_tag = uint64(new_first_free) + (uint64(mAllocationTag.fetch_add(1, memory_order_relaxed)) << 32);
+
+			// Compare and swap
+			if (mFirstFreeObjectAndTag.compare_exchange_weak(first_free_object_and_tag, new_first_free_object_and_tag, memory_order_release))
+			{
+				// Allocation successful
+				JPH_IF_ENABLE_ASSERTS(mNumFreeObjects.fetch_sub(1, memory_order_relaxed);)
+				ObjectStorage &storage = GetStorage(first_free);
+				::new (&storage.mObject) Object(std::forward<Parameters>(inParameters)...);
+				storage.mNextFreeObject.store(first_free, memory_order_release);
+				return first_free;
+			}
+		}
+	}
+}
+
+template <typename Object>
+void FixedSizeFreeList<Object>::AddObjectToBatch(Batch &ioBatch, uint32 inObjectIndex)
+{
+	JPH_ASSERT(ioBatch.mNumObjects != uint32(-1), "Trying to reuse a batch that has already been freed");
+
+	// Reset next index
+	atomic<uint32> &next_free_object = GetStorage(inObjectIndex).mNextFreeObject;
+	JPH_ASSERT(next_free_object.load(memory_order_relaxed) == inObjectIndex, "Trying to add a object to the batch that is already in a free list");
+	next_free_object.store(cInvalidObjectIndex, memory_order_release);
+
+	// Link object in batch to free
+	if (ioBatch.mFirstObjectIndex == cInvalidObjectIndex)
+		ioBatch.mFirstObjectIndex = inObjectIndex;
+	else
+		GetStorage(ioBatch.mLastObjectIndex).mNextFreeObject.store(inObjectIndex, memory_order_release);
+	ioBatch.mLastObjectIndex = inObjectIndex;
+	ioBatch.mNumObjects++;
+}
+
+template <typename Object>
+void FixedSizeFreeList<Object>::DestructObjectBatch(Batch &ioBatch)
+{
+	if (ioBatch.mFirstObjectIndex != cInvalidObjectIndex)
+	{
+		// Call destructors
+		if constexpr (!std::is_trivially_destructible<Object>())
+		{
+			uint32 object_idx = ioBatch.mFirstObjectIndex;
+			do
+			{
+				ObjectStorage &storage = GetStorage(object_idx);
+				storage.mObject.~Object();
+				object_idx = storage.mNextFreeObject.load(memory_order_relaxed);
+			}
+			while (object_idx != cInvalidObjectIndex);
+		}
+
+		// Add to objects free list
+		ObjectStorage &storage = GetStorage(ioBatch.mLastObjectIndex);
+		for (;;)
+		{
+			// Get first object from the list
+			uint64 first_free_object_and_tag = mFirstFreeObjectAndTag.load(memory_order_acquire);
+			uint32 first_free = uint32(first_free_object_and_tag);
+
+			// Make it the next pointer of the last object in the batch that is to be freed
+			storage.mNextFreeObject.store(first_free, memory_order_release);
+
+			// Construct a new first free object tag
+			uint64 new_first_free_object_and_tag = uint64(ioBatch.mFirstObjectIndex) + (uint64(mAllocationTag.fetch_add(1, memory_order_relaxed)) << 32);
+
+			// Compare and swap
+			if (mFirstFreeObjectAndTag.compare_exchange_weak(first_free_object_and_tag, new_first_free_object_and_tag, memory_order_release))
+			{
+				// Free successful
+				JPH_IF_ENABLE_ASSERTS(mNumFreeObjects.fetch_add(ioBatch.mNumObjects, memory_order_relaxed);)
+
+				// Mark the batch as freed
+#ifdef JPH_ENABLE_ASSERTS
+				ioBatch.mNumObjects = uint32(-1);
+#endif
+				return;
+			}
+		}
+	}
+}
+
+template <typename Object>
+void FixedSizeFreeList<Object>::DestructObject(uint32 inObjectIndex)
+{
+	JPH_ASSERT(inObjectIndex != cInvalidObjectIndex);
+
+	// Call destructor
+	ObjectStorage &storage = GetStorage(inObjectIndex);
+	storage.mObject.~Object();
+
+	// Add to object free list
+	for (;;)
+	{
+		// Get first object from the list
+		uint64 first_free_object_and_tag = mFirstFreeObjectAndTag.load(memory_order_acquire);
+		uint32 first_free = uint32(first_free_object_and_tag);
+
+		// Make it the next pointer of the last object in the batch that is to be freed
+		storage.mNextFreeObject.store(first_free, memory_order_release);
+
+		// Construct a new first free object tag
+		uint64 new_first_free_object_and_tag = uint64(inObjectIndex) + (uint64(mAllocationTag.fetch_add(1, memory_order_relaxed)) << 32);
+
+		// Compare and swap
+		if (mFirstFreeObjectAndTag.compare_exchange_weak(first_free_object_and_tag, new_first_free_object_and_tag, memory_order_release))
+		{
+			// Free successful
+			JPH_IF_ENABLE_ASSERTS(mNumFreeObjects.fetch_add(1, memory_order_relaxed);)
+			return;
+		}
+	}
+}
+
+template<typename Object>
+inline void FixedSizeFreeList<Object>::DestructObject(Object *inObject)
+{
+	uint32 index = reinterpret_cast<ObjectStorage *>(inObject)->mNextFreeObject.load(memory_order_relaxed);
+	JPH_ASSERT(index < mNumObjectsAllocated);
+	DestructObject(index);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/HashCombine.h b/thirdparty/jolt_physics/Jolt/Core/HashCombine.h
new file mode 100644
index 0000000000..e04b54b744
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/HashCombine.h
@@ -0,0 +1,212 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Implements the FNV-1a hash algorithm
+/// @see https://en.wikipedia.org/wiki/Fowler%E2%80%93Noll%E2%80%93Vo_hash_function
+/// @param inData Data block of bytes
+/// @param inSize Number of bytes
+/// @param inSeed Seed of the hash (can be used to pass in the hash of a previous operation, otherwise leave default)
+/// @return Hash
+inline uint64 HashBytes(const void *inData, uint inSize, uint64 inSeed = 0xcbf29ce484222325UL)
+{
+	uint64 hash = inSeed;
+	for (const uint8 *data = reinterpret_cast<const uint8 *>(inData); data < reinterpret_cast<const uint8 *>(inData) + inSize; ++data)
+	{
+		hash = hash ^ uint64(*data);
+		hash = hash * 0x100000001b3UL;
+	}
+	return hash;
+}
+
+/// Calculate the FNV-1a hash of inString.
+/// @see https://en.wikipedia.org/wiki/Fowler%E2%80%93Noll%E2%80%93Vo_hash_function
+constexpr uint64 HashString(const char *inString, uint64 inSeed = 0xcbf29ce484222325UL)
+{
+	uint64 hash = inSeed;
+	for (const char *c = inString; *c != 0; ++c)
+	{
+		hash ^= uint64(*c);
+		hash = hash * 0x100000001b3UL;
+	}
+	return hash;
+}
+
+/// A 64 bit hash function by Thomas Wang, Jan 1997
+/// See: http://web.archive.org/web/20071223173210/http://www.concentric.net/~Ttwang/tech/inthash.htm
+/// @param inValue Value to hash
+/// @return Hash
+inline uint64 Hash64(uint64 inValue)
+{
+	uint64 hash = inValue;
+	hash = (~hash) + (hash << 21); // hash = (hash << 21) - hash - 1;
+	hash = hash ^ (hash >> 24);
+	hash = (hash + (hash << 3)) + (hash << 8); // hash * 265
+	hash = hash ^ (hash >> 14);
+	hash = (hash + (hash << 2)) + (hash << 4); // hash * 21
+	hash = hash ^ (hash >> 28);
+	hash = hash + (hash << 31);
+	return hash;
+}
+
+/// Fallback hash function that calls T::GetHash()
+template <class T>
+struct Hash
+{
+	uint64		operator () (const T &inValue) const
+	{
+		return inValue.GetHash();
+	}
+};
+
+/// A hash function for floats
+template <>
+struct Hash<float>
+{
+	uint64		operator () (float inValue) const
+	{
+		float value = inValue == 0.0f? 0.0f : inValue; // Convert -0.0f to 0.0f
+		return HashBytes(&value, sizeof(value));
+	}
+};
+
+/// A hash function for doubles
+template <>
+struct Hash<double>
+{
+	uint64		operator () (double inValue) const
+	{
+		double value = inValue == 0.0? 0.0 : inValue; // Convert -0.0 to 0.0
+		return HashBytes(&value, sizeof(value));
+	}
+};
+
+/// A hash function for character pointers
+template <>
+struct Hash<const char *>
+{
+	uint64		operator () (const char *inValue) const
+	{
+		return HashString(inValue);
+	}
+};
+
+/// A hash function for std::string_view
+template <>
+struct Hash<std::string_view>
+{
+	uint64		operator () (const std::string_view &inValue) const
+	{
+		return HashBytes(inValue.data(), uint(inValue.size()));
+	}
+};
+
+/// A hash function for String
+template <>
+struct Hash<String>
+{
+	uint64		operator () (const String &inValue) const
+	{
+		return HashBytes(inValue.data(), uint(inValue.size()));
+	}
+};
+
+/// A fallback function for generic pointers
+template <class T>
+struct Hash<T *>
+{
+	uint64		operator () (T *inValue) const
+	{
+		return HashBytes(&inValue, sizeof(inValue));
+	}
+};
+
+/// Helper macro to define a hash function for trivial types
+#define JPH_DEFINE_TRIVIAL_HASH(type)						\
+template <>													\
+struct Hash<type>											\
+{															\
+	uint64		operator () (const type &inValue) const		\
+	{														\
+		return HashBytes(&inValue, sizeof(inValue));		\
+	}														\
+};
+
+/// Commonly used types
+JPH_DEFINE_TRIVIAL_HASH(char)
+JPH_DEFINE_TRIVIAL_HASH(int)
+JPH_DEFINE_TRIVIAL_HASH(uint32)
+JPH_DEFINE_TRIVIAL_HASH(uint64)
+
+/// @brief Helper function that hashes a single value into ioSeed
+/// Taken from: https://stackoverflow.com/questions/2590677/how-do-i-combine-hash-values-in-c0x
+template <typename T>
+inline void HashCombine(uint64 &ioSeed, const T &inValue)
+{
+	ioSeed ^= Hash<T> { } (inValue) + 0x9e3779b9 + (ioSeed << 6) + (ioSeed >> 2);
+}
+
+/// Hash combiner to use a custom struct in an unordered map or set
+///
+/// Usage:
+///
+///		struct SomeHashKey
+///		{
+///			std::string key1;
+///			std::string key2;
+///			bool key3;
+///		};
+///
+///		JPH_MAKE_HASHABLE(SomeHashKey, t.key1, t.key2, t.key3)
+template <typename FirstValue, typename... Values>
+inline uint64 HashCombineArgs(const FirstValue &inFirstValue, Values... inValues)
+{
+	// Prime the seed by hashing the first value
+	uint64 seed = Hash<FirstValue> { } (inFirstValue);
+
+	// Hash all remaining values together using a fold expression
+	(HashCombine(seed, inValues), ...);
+
+	return seed;
+}
+
+JPH_NAMESPACE_END
+
+JPH_SUPPRESS_WARNING_PUSH
+JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat-pedantic")
+
+#define JPH_MAKE_HASH_STRUCT(type, name, ...)				\
+	struct [[nodiscard]] name								\
+	{														\
+		::JPH::uint64 operator()(const type &t) const		\
+		{													\
+			return ::JPH::HashCombineArgs(__VA_ARGS__);		\
+		}													\
+	};
+
+#define JPH_MAKE_HASHABLE(type, ...)						\
+	JPH_SUPPRESS_WARNING_PUSH								\
+	JPH_SUPPRESS_WARNINGS									\
+	namespace JPH											\
+	{														\
+		template<>											\
+		JPH_MAKE_HASH_STRUCT(type, Hash<type>, __VA_ARGS__) \
+	}														\
+	namespace std											\
+	{														\
+		template<>											\
+		struct [[nodiscard]] hash<type>						\
+		{													\
+			std::size_t operator()(const type &t) const		\
+			{												\
+				return std::size_t(::JPH::Hash<type>{ }(t));\
+			}												\
+		};													\
+	}														\
+	JPH_SUPPRESS_WARNING_POP
+
+JPH_SUPPRESS_WARNING_POP
diff --git a/thirdparty/jolt_physics/Jolt/Core/HashTable.h b/thirdparty/jolt_physics/Jolt/Core/HashTable.h
new file mode 100644
index 0000000000..baca08255b
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/HashTable.h
@@ -0,0 +1,854 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Math/BVec16.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Helper class for implementing an UnorderedSet or UnorderedMap
+/// Based on CppCon 2017: Matt Kulukundis "Designing a Fast, Efficient, Cache-friendly Hash Table, Step by Step"
+/// See: https://www.youtube.com/watch?v=ncHmEUmJZf4
+template <class Key, class KeyValue, class HashTableDetail, class Hash, class KeyEqual>
+class HashTable
+{
+public:
+	/// Properties
+	using value_type = KeyValue;
+	using size_type = uint32;
+	using difference_type = ptrdiff_t;
+
+private:
+	/// Base class for iterators
+	template <class Table, class Iterator>
+	class IteratorBase
+	{
+	public:
+        /// Properties
+		using difference_type = typename Table::difference_type;
+        using value_type = typename Table::value_type;
+        using iterator_category = std::forward_iterator_tag;
+
+		/// Copy constructor
+							IteratorBase(const IteratorBase &inRHS) = default;
+
+		/// Assignment operator
+		IteratorBase &		operator = (const IteratorBase &inRHS) = default;
+
+		/// Iterator at start of table
+		explicit			IteratorBase(Table *inTable) :
+			mTable(inTable),
+			mIndex(0)
+		{
+			while (mIndex < mTable->mMaxSize && (mTable->mControl[mIndex] & cBucketUsed) == 0)
+				++mIndex;
+		}
+
+		/// Iterator at specific index
+							IteratorBase(Table *inTable, size_type inIndex) :
+			mTable(inTable),
+			mIndex(inIndex)
+		{
+		}
+
+		/// Prefix increment
+		Iterator &			operator ++ ()
+		{
+			JPH_ASSERT(IsValid());
+
+			do
+			{
+				++mIndex;
+			}
+			while (mIndex < mTable->mMaxSize && (mTable->mControl[mIndex] & cBucketUsed) == 0);
+
+			return static_cast<Iterator &>(*this);
+		}
+
+		/// Postfix increment
+		Iterator			operator ++ (int)
+		{
+			Iterator result(mTable, mIndex);
+			++(*this);
+			return result;
+		}
+
+		/// Access to key value pair
+		const KeyValue &	operator * () const
+		{
+			JPH_ASSERT(IsValid());
+			return mTable->mData[mIndex];
+		}
+
+		/// Access to key value pair
+		const KeyValue *	operator -> () const
+		{
+			JPH_ASSERT(IsValid());
+			return mTable->mData + mIndex;
+		}
+
+		/// Equality operator
+		bool				operator == (const Iterator &inRHS) const
+		{
+			return mIndex == inRHS.mIndex && mTable == inRHS.mTable;
+		}
+
+		/// Inequality operator
+		bool				operator != (const Iterator &inRHS) const
+		{
+			return !(*this == inRHS);
+		}
+
+		/// Check that the iterator is valid
+		bool				IsValid() const
+		{
+			return mIndex < mTable->mMaxSize
+				&& (mTable->mControl[mIndex] & cBucketUsed) != 0;
+		}
+
+		Table *				mTable;
+		size_type			mIndex;
+	};
+
+	/// Get the maximum number of elements that we can support given a number of buckets
+	static constexpr size_type sGetMaxLoad(size_type inBucketCount)
+	{
+		return uint32((cMaxLoadFactorNumerator * inBucketCount) / cMaxLoadFactorDenominator);
+	}
+
+	/// Update the control value for a bucket
+	JPH_INLINE void			SetControlValue(size_type inIndex, uint8 inValue)
+	{
+		JPH_ASSERT(inIndex < mMaxSize);
+		mControl[inIndex] = inValue;
+
+		// Mirror the first 15 bytes to the 15 bytes beyond mMaxSize
+		// Note that this is equivalent to:
+		// if (inIndex < 15)
+		//   mControl[inIndex + mMaxSize] = inValue
+		// else
+		//   mControl[inIndex] = inValue
+		// Which performs a needless write if inIndex >= 15 but at least it is branch-less
+		mControl[((inIndex - 15) & (mMaxSize - 1)) + 15] = inValue;
+	}
+
+	/// Get the index and control value for a particular key
+	JPH_INLINE void			GetIndexAndControlValue(const Key &inKey, size_type &outIndex, uint8 &outControl) const
+	{
+		// Calculate hash
+		uint64 hash_value = Hash { } (inKey);
+
+		// Split hash into index and control value
+		outIndex = size_type(hash_value >> 7) & (mMaxSize - 1);
+		outControl = cBucketUsed | uint8(hash_value);
+	}
+
+	/// Allocate space for the hash table
+	void					AllocateTable(size_type inMaxSize)
+	{
+		JPH_ASSERT(mData == nullptr);
+
+		mMaxSize = inMaxSize;
+		mLoadLeft = sGetMaxLoad(inMaxSize);
+		size_t required_size = size_t(mMaxSize) * (sizeof(KeyValue) + 1) + 15; // Add 15 bytes to mirror the first 15 bytes of the control values
+		if constexpr (cNeedsAlignedAllocate)
+			mData = reinterpret_cast<KeyValue *>(AlignedAllocate(required_size, alignof(KeyValue)));
+		else
+			mData = reinterpret_cast<KeyValue *>(Allocate(required_size));
+		mControl = reinterpret_cast<uint8 *>(mData + mMaxSize);
+	}
+
+	/// Copy the contents of another hash table
+	void					CopyTable(const HashTable &inRHS)
+	{
+		if (inRHS.empty())
+			return;
+
+		AllocateTable(inRHS.mMaxSize);
+
+		// Copy control bytes
+		memcpy(mControl, inRHS.mControl, mMaxSize + 15);
+
+		// Copy elements
+		uint index = 0;
+		for (const uint8 *control = mControl, *control_end = mControl + mMaxSize; control != control_end; ++control, ++index)
+			if (*control & cBucketUsed)
+				::new (mData + index) KeyValue(inRHS.mData[index]);
+		mSize = inRHS.mSize;
+	}
+
+	/// Grow the table to the next power of 2
+	void					GrowTable()
+	{
+		// Calculate new size
+		size_type new_max_size = max<size_type>(mMaxSize << 1, 16);
+		if (new_max_size < mMaxSize)
+		{
+			JPH_ASSERT(false, "Overflow in hash table size, can't grow!");
+			return;
+		}
+
+		// Move the old table to a temporary structure
+		size_type old_max_size = mMaxSize;
+		KeyValue *old_data = mData;
+		const uint8 *old_control = mControl;
+		mData = nullptr;
+		mControl = nullptr;
+		mSize = 0;
+		mMaxSize = 0;
+		mLoadLeft = 0;
+
+		// Allocate new table
+		AllocateTable(new_max_size);
+
+		// Reset all control bytes
+		memset(mControl, cBucketEmpty, mMaxSize + 15);
+
+		if (old_data != nullptr)
+		{
+			// Copy all elements from the old table
+			for (size_type i = 0; i < old_max_size; ++i)
+				if (old_control[i] & cBucketUsed)
+				{
+					size_type index;
+					KeyValue *element = old_data + i;
+					JPH_IF_ENABLE_ASSERTS(bool inserted =) InsertKey</* InsertAfterGrow= */ true>(HashTableDetail::sGetKey(*element), index);
+					JPH_ASSERT(inserted);
+					::new (mData + index) KeyValue(std::move(*element));
+					element->~KeyValue();
+				}
+
+			// Free memory
+			if constexpr (cNeedsAlignedAllocate)
+				AlignedFree(old_data);
+			else
+				Free(old_data);
+		}
+	}
+
+protected:
+	/// Get an element by index
+	KeyValue &				GetElement(size_type inIndex) const
+	{
+		return mData[inIndex];
+	}
+
+	/// Insert a key into the map, returns true if the element was inserted, false if it already existed.
+	/// outIndex is the index at which the element should be constructed / where it is located.
+	template <bool InsertAfterGrow = false>
+	bool					InsertKey(const Key &inKey, size_type &outIndex)
+	{
+		// Ensure we have enough space
+		if (mLoadLeft == 0)
+		{
+			// Should not be growing if we're already growing!
+			if constexpr (InsertAfterGrow)
+				JPH_ASSERT(false);
+
+			// Decide if we need to clean up all tombstones or if we need to grow the map
+			size_type num_deleted = sGetMaxLoad(mMaxSize) - mSize;
+			if (num_deleted * cMaxDeletedElementsDenominator > mMaxSize * cMaxDeletedElementsNumerator)
+				rehash(0);
+			else
+				GrowTable();
+		}
+
+		// Split hash into index and control value
+		size_type index;
+		uint8 control;
+		GetIndexAndControlValue(inKey, index, control);
+
+		// Keeps track of the index of the first deleted bucket we found
+		constexpr size_type cNoDeleted = ~size_type(0);
+		size_type first_deleted_index = cNoDeleted;
+
+		// Linear probing
+		KeyEqual equal;
+		size_type bucket_mask = mMaxSize - 1;
+		BVec16 control16 = BVec16::sReplicate(control);
+		BVec16 bucket_empty = BVec16::sZero();
+		BVec16 bucket_deleted = BVec16::sReplicate(cBucketDeleted);
+		for (;;)
+		{
+			// Read 16 control values (note that we added 15 bytes at the end of the control values that mirror the first 15 bytes)
+			BVec16 control_bytes = BVec16::sLoadByte16(mControl + index);
+
+			// Check if we must find the element before we can insert
+			if constexpr (!InsertAfterGrow)
+			{
+				// Check for the control value we're looking for
+				// Note that when deleting we can create empty buckets instead of deleted buckets.
+				// This means we must unconditionally check all buckets in this batch for equality
+				// (also beyond the first empty bucket).
+				uint32 control_equal = uint32(BVec16::sEquals(control_bytes, control16).GetTrues());
+
+				// Index within the 16 buckets
+				size_type local_index = index;
+
+				// Loop while there's still buckets to process
+				while (control_equal != 0)
+				{
+					// Get the first equal bucket
+					uint first_equal = CountTrailingZeros(control_equal);
+
+					// Skip to the bucket
+					local_index += first_equal;
+
+					// Make sure that our index is not beyond the end of the table
+					local_index &= bucket_mask;
+
+					// We found a bucket with same control value
+					if (equal(HashTableDetail::sGetKey(mData[local_index]), inKey))
+					{
+						// Element already exists
+						outIndex = local_index;
+						return false;
+					}
+
+					// Skip past this bucket
+					control_equal >>= first_equal + 1;
+					local_index++;
+				}
+
+				// Check if we're still scanning for deleted buckets
+				if (first_deleted_index == cNoDeleted)
+				{
+					// Check if any buckets have been deleted, if so store the first one
+					uint32 control_deleted = uint32(BVec16::sEquals(control_bytes, bucket_deleted).GetTrues());
+					if (control_deleted != 0)
+						first_deleted_index = index + CountTrailingZeros(control_deleted);
+				}
+			}
+
+			// Check for empty buckets
+			uint32 control_empty = uint32(BVec16::sEquals(control_bytes, bucket_empty).GetTrues());
+			if (control_empty != 0)
+			{
+				// If we found a deleted bucket, use it.
+				// It doesn't matter if it is before or after the first empty bucket we found
+				// since we will always be scanning in batches of 16 buckets.
+				if (first_deleted_index == cNoDeleted || InsertAfterGrow)
+				{
+					index += CountTrailingZeros(control_empty);
+					--mLoadLeft; // Using an empty bucket decreases the load left
+				}
+				else
+				{
+					index = first_deleted_index;
+				}
+
+				// Make sure that our index is not beyond the end of the table
+				index &= bucket_mask;
+
+				// Update control byte
+				SetControlValue(index, control);
+				++mSize;
+
+				// Return index to newly allocated bucket
+				outIndex = index;
+				return true;
+			}
+
+			// Move to next batch of 16 buckets
+			index = (index + 16) & bucket_mask;
+		}
+	}
+
+public:
+	/// Non-const iterator
+	class iterator : public IteratorBase<HashTable, iterator>
+	{
+		using Base = IteratorBase<HashTable, iterator>;
+
+	public:
+        /// Properties
+        using reference = typename Base::value_type &;
+        using pointer = typename Base::value_type *;
+
+		/// Constructors
+		explicit			iterator(HashTable *inTable) : Base(inTable) { }
+							iterator(HashTable *inTable, size_type inIndex) : Base(inTable, inIndex) { }
+							iterator(const iterator &inIterator) : Base(inIterator) { }
+
+		/// Assignment
+		iterator &			operator = (const iterator &inRHS) { Base::operator = (inRHS); return *this; }
+
+		using Base::operator *;
+
+		/// Non-const access to key value pair
+		KeyValue &			operator * ()
+		{
+			JPH_ASSERT(this->IsValid());
+			return this->mTable->mData[this->mIndex];
+		}
+
+		using Base::operator ->;
+
+		/// Non-const access to key value pair
+		KeyValue *			operator -> ()
+		{
+			JPH_ASSERT(this->IsValid());
+			return this->mTable->mData + this->mIndex;
+		}
+	};
+
+	/// Const iterator
+	class const_iterator : public IteratorBase<const HashTable, const_iterator>
+	{
+		using Base = IteratorBase<const HashTable, const_iterator>;
+
+	public:
+        /// Properties
+        using reference = const typename Base::value_type &;
+        using pointer = const typename Base::value_type *;
+
+		/// Constructors
+		explicit			const_iterator(const HashTable *inTable) : Base(inTable) { }
+							const_iterator(const HashTable *inTable, size_type inIndex) : Base(inTable, inIndex) { }
+							const_iterator(const const_iterator &inRHS) : Base(inRHS) { }
+							const_iterator(const iterator &inIterator) : Base(inIterator.mTable, inIterator.mIndex) { }
+
+		/// Assignment
+		const_iterator &	operator = (const iterator &inRHS) { this->mTable = inRHS.mTable; this->mIndex = inRHS.mIndex; return *this; }
+		const_iterator &	operator = (const const_iterator &inRHS) { Base::operator = (inRHS); return *this; }
+	};
+
+	/// Default constructor
+							HashTable() = default;
+
+	/// Copy constructor
+							HashTable(const HashTable &inRHS)
+	{
+		CopyTable(inRHS);
+	}
+
+	/// Move constructor
+							HashTable(HashTable &&ioRHS) noexcept :
+		mData(ioRHS.mData),
+		mControl(ioRHS.mControl),
+		mSize(ioRHS.mSize),
+		mMaxSize(ioRHS.mMaxSize),
+		mLoadLeft(ioRHS.mLoadLeft)
+	{
+		ioRHS.mData = nullptr;
+		ioRHS.mControl = nullptr;
+		ioRHS.mSize = 0;
+		ioRHS.mMaxSize = 0;
+		ioRHS.mLoadLeft = 0;
+	}
+
+	/// Assignment operator
+	HashTable &				operator = (const HashTable &inRHS)
+	{
+		if (this != &inRHS)
+		{
+			clear();
+
+			CopyTable(inRHS);
+		}
+
+		return *this;
+	}
+
+	/// Move assignment operator
+	HashTable &				operator = (HashTable &&ioRHS) noexcept
+	{
+		if (this != &ioRHS)
+		{
+			clear();
+
+			mData = ioRHS.mData;
+			mControl = ioRHS.mControl;
+			mSize = ioRHS.mSize;
+			mMaxSize = ioRHS.mMaxSize;
+			mLoadLeft = ioRHS.mLoadLeft;
+
+			ioRHS.mData = nullptr;
+			ioRHS.mControl = nullptr;
+			ioRHS.mSize = 0;
+			ioRHS.mMaxSize = 0;
+			ioRHS.mLoadLeft = 0;
+		}
+
+		return *this;
+	}
+
+	/// Destructor
+							~HashTable()
+	{
+		clear();
+	}
+
+	/// Reserve memory for a certain number of elements
+	void					reserve(size_type inMaxSize)
+	{
+		// Calculate max size based on load factor
+		size_type max_size = GetNextPowerOf2(max<uint32>((cMaxLoadFactorDenominator * inMaxSize) / cMaxLoadFactorNumerator, 16));
+		if (max_size <= mMaxSize)
+			return;
+
+		// Allocate buffers
+		AllocateTable(max_size);
+
+		// Reset all control bytes
+		memset(mControl, cBucketEmpty, mMaxSize + 15);
+	}
+
+	/// Destroy the entire hash table
+	void					clear()
+	{
+		// Delete all elements
+		if constexpr (!std::is_trivially_destructible<KeyValue>())
+			if (!empty())
+				for (size_type i = 0; i < mMaxSize; ++i)
+					if (mControl[i] & cBucketUsed)
+						mData[i].~KeyValue();
+
+		if (mData != nullptr)
+		{
+			// Free memory
+			if constexpr (cNeedsAlignedAllocate)
+				AlignedFree(mData);
+			else
+				Free(mData);
+
+			// Reset members
+			mData = nullptr;
+			mControl = nullptr;
+			mSize = 0;
+			mMaxSize = 0;
+			mLoadLeft = 0;
+		}
+	}
+
+	/// Iterator to first element
+	iterator				begin()
+	{
+		return iterator(this);
+	}
+
+	/// Iterator to one beyond last element
+	iterator				end()
+	{
+		return iterator(this, mMaxSize);
+	}
+
+	/// Iterator to first element
+	const_iterator			begin() const
+	{
+		return const_iterator(this);
+	}
+
+	/// Iterator to one beyond last element
+	const_iterator			end() const
+	{
+		return const_iterator(this, mMaxSize);
+	}
+
+	/// Iterator to first element
+	const_iterator			cbegin() const
+	{
+		return const_iterator(this);
+	}
+
+	/// Iterator to one beyond last element
+	const_iterator			cend() const
+	{
+		return const_iterator(this, mMaxSize);
+	}
+
+	/// Number of buckets in the table
+	size_type				bucket_count() const
+	{
+		return mMaxSize;
+	}
+
+	/// Max number of buckets that the table can have
+	constexpr size_type		max_bucket_count() const
+	{
+		return size_type(1) << (sizeof(size_type) * 8 - 1);
+	}
+
+	/// Check if there are no elements in the table
+	bool					empty() const
+	{
+		return mSize == 0;
+	}
+
+	/// Number of elements in the table
+	size_type				size() const
+	{
+		return mSize;
+	}
+
+	/// Max number of elements that the table can hold
+	constexpr size_type		max_size() const
+	{
+		return size_type((uint64(max_bucket_count()) * cMaxLoadFactorNumerator) / cMaxLoadFactorDenominator);
+	}
+
+	/// Get the max load factor for this table (max number of elements / number of buckets)
+	constexpr float			max_load_factor() const
+	{
+		return float(cMaxLoadFactorNumerator) / float(cMaxLoadFactorDenominator);
+	}
+
+	/// Insert a new element, returns iterator and if the element was inserted
+	std::pair<iterator, bool> insert(const value_type &inValue)
+	{
+		size_type index;
+		bool inserted = InsertKey(HashTableDetail::sGetKey(inValue), index);
+		if (inserted)
+			::new (mData + index) KeyValue(inValue);
+		return std::make_pair(iterator(this, index), inserted);
+	}
+
+	/// Find an element, returns iterator to element or end() if not found
+	const_iterator			find(const Key &inKey) const
+	{
+		// Check if we have any data
+		if (empty())
+			return cend();
+
+		// Split hash into index and control value
+		size_type index;
+		uint8 control;
+		GetIndexAndControlValue(inKey, index, control);
+
+		// Linear probing
+		KeyEqual equal;
+		size_type bucket_mask = mMaxSize - 1;
+		BVec16 control16 = BVec16::sReplicate(control);
+		BVec16 bucket_empty = BVec16::sZero();
+		for (;;)
+		{
+			// Read 16 control values
+			// (note that we added 15 bytes at the end of the control values that mirror the first 15 bytes)
+			BVec16 control_bytes = BVec16::sLoadByte16(mControl + index);
+
+			// Check for the control value we're looking for
+			// Note that when deleting we can create empty buckets instead of deleted buckets.
+			// This means we must unconditionally check all buckets in this batch for equality
+			// (also beyond the first empty bucket).
+			uint32 control_equal = uint32(BVec16::sEquals(control_bytes, control16).GetTrues());
+
+			// Index within the 16 buckets
+			size_type local_index = index;
+
+			// Loop while there's still buckets to process
+			while (control_equal != 0)
+			{
+				// Get the first equal bucket
+				uint first_equal = CountTrailingZeros(control_equal);
+
+				// Skip to the bucket
+				local_index += first_equal;
+
+				// Make sure that our index is not beyond the end of the table
+				local_index &= bucket_mask;
+
+				// We found a bucket with same control value
+				if (equal(HashTableDetail::sGetKey(mData[local_index]), inKey))
+				{
+					// Element found
+					return const_iterator(this, local_index);
+				}
+
+				// Skip past this bucket
+				control_equal >>= first_equal + 1;
+				local_index++;
+			}
+
+			// Check for empty buckets
+			uint32 control_empty = uint32(BVec16::sEquals(control_bytes, bucket_empty).GetTrues());
+			if (control_empty != 0)
+			{
+				// An empty bucket was found, we didn't find the element
+				return cend();
+			}
+
+			// Move to next batch of 16 buckets
+			index = (index + 16) & bucket_mask;
+		}
+	}
+
+	/// @brief Erase an element by iterator
+	void					erase(const const_iterator &inIterator)
+	{
+		JPH_ASSERT(inIterator.IsValid());
+
+		// Read 16 control values before and after the current index
+		// (note that we added 15 bytes at the end of the control values that mirror the first 15 bytes)
+		BVec16 control_bytes_before = BVec16::sLoadByte16(mControl + ((inIterator.mIndex - 16) & (mMaxSize - 1)));
+		BVec16 control_bytes_after = BVec16::sLoadByte16(mControl + inIterator.mIndex);
+		BVec16 bucket_empty = BVec16::sZero();
+		uint32 control_empty_before = uint32(BVec16::sEquals(control_bytes_before, bucket_empty).GetTrues());
+		uint32 control_empty_after = uint32(BVec16::sEquals(control_bytes_after, bucket_empty).GetTrues());
+
+		// If (this index including) there exist 16 consecutive non-empty slots (represented by a bit being 0) then
+		// a probe looking for some element needs to continue probing so we cannot mark the bucket as empty
+		// but must mark it as deleted instead.
+		// Note that we use: CountLeadingZeros(uint16) = CountLeadingZeros(uint32) - 16.
+		uint8 control_value = CountLeadingZeros(control_empty_before) - 16 + CountTrailingZeros(control_empty_after) < 16? cBucketEmpty : cBucketDeleted;
+
+		// Mark the bucket as empty/deleted
+		SetControlValue(inIterator.mIndex, control_value);
+
+		// Destruct the element
+		mData[inIterator.mIndex].~KeyValue();
+
+		// If we marked the bucket as empty we can increase the load left
+		if (control_value == cBucketEmpty)
+			++mLoadLeft;
+
+		// Decrease size
+		--mSize;
+	}
+
+	/// @brief Erase an element by key
+	size_type				erase(const Key &inKey)
+	{
+		const_iterator it = find(inKey);
+		if (it == cend())
+			return 0;
+
+		erase(it);
+		return 1;
+	}
+
+	/// Swap the contents of two hash tables
+	void					swap(HashTable &ioRHS) noexcept
+	{
+		std::swap(mData, ioRHS.mData);
+		std::swap(mControl, ioRHS.mControl);
+		std::swap(mSize, ioRHS.mSize);
+		std::swap(mMaxSize, ioRHS.mMaxSize);
+		std::swap(mLoadLeft, ioRHS.mLoadLeft);
+	}
+
+	/// In place re-hashing of all elements in the table. Removes all cBucketDeleted elements
+	/// The std version takes a bucket count, but we just re-hash to the same size.
+	void					rehash(size_type)
+	{
+		// Update the control value for all buckets
+		for (size_type i = 0; i < mMaxSize; ++i)
+		{
+			uint8 &control = mControl[i];
+			switch (control)
+			{
+			case cBucketDeleted:
+				// Deleted buckets become empty
+				control = cBucketEmpty;
+				break;
+			case cBucketEmpty:
+				// Remains empty
+				break;
+			default:
+				// Mark all occupied as deleted, to indicate it needs to move to the correct place
+				control = cBucketDeleted;
+				break;
+			}
+		}
+
+		// Replicate control values to the last 15 entries
+		for (size_type i = 0; i < 15; ++i)
+			mControl[mMaxSize + i] = mControl[i];
+
+		// Loop over all elements that have been 'deleted' and move them to their new spot
+		BVec16 bucket_used = BVec16::sReplicate(cBucketUsed);
+		size_type bucket_mask = mMaxSize - 1;
+		uint32 probe_mask = bucket_mask & ~uint32(0b1111); // Mask out lower 4 bits because we test 16 buckets at a time
+		for (size_type src = 0; src < mMaxSize; ++src)
+			if (mControl[src] == cBucketDeleted)
+				for (;;)
+				{
+					// Split hash into index and control value
+					size_type src_index;
+					uint8 src_control;
+					GetIndexAndControlValue(HashTableDetail::sGetKey(mData[src]), src_index, src_control);
+
+					// Linear probing
+					size_type dst = src_index;
+					for (;;)
+					{
+						// Check if any buckets are free
+						BVec16 control_bytes = BVec16::sLoadByte16(mControl + dst);
+						uint32 control_free = uint32(BVec16::sAnd(control_bytes, bucket_used).GetTrues()) ^ 0xffff;
+						if (control_free != 0)
+						{
+							// Select this bucket as destination
+							dst += CountTrailingZeros(control_free);
+							dst &= bucket_mask;
+							break;
+						}
+
+						// Move to next batch of 16 buckets
+						dst = (dst + 16) & bucket_mask;
+					}
+
+					// Check if we stay in the same probe group
+					if (((dst - src_index) & probe_mask) == ((src - src_index) & probe_mask))
+					{
+						// We stay in the same group, we can stay where we are
+						SetControlValue(src, src_control);
+						break;
+					}
+					else if (mControl[dst] == cBucketEmpty)
+					{
+						// There's an empty bucket, move us there
+						SetControlValue(dst, src_control);
+						SetControlValue(src, cBucketEmpty);
+						::new (mData + dst) KeyValue(std::move(mData[src]));
+						mData[src].~KeyValue();
+						break;
+					}
+					else
+					{
+						// There's an element in the bucket we want to move to, swap them
+						JPH_ASSERT(mControl[dst] == cBucketDeleted);
+						SetControlValue(dst, src_control);
+						std::swap(mData[src], mData[dst]);
+						// Iterate again with the same source bucket
+					}
+				}
+
+		// Reinitialize load left
+		mLoadLeft = sGetMaxLoad(mMaxSize) - mSize;
+	}
+
+private:
+	/// If this allocator needs to fall back to aligned allocations because the type requires it
+	static constexpr bool	cNeedsAlignedAllocate = alignof(KeyValue) > (JPH_CPU_ADDRESS_BITS == 32? 8 : 16);
+
+	/// Max load factor is cMaxLoadFactorNumerator / cMaxLoadFactorDenominator
+	static constexpr uint64	cMaxLoadFactorNumerator = 7;
+	static constexpr uint64	cMaxLoadFactorDenominator = 8;
+
+	/// If we can recover this fraction of deleted elements, we'll reshuffle the buckets in place rather than growing the table
+	static constexpr uint64 cMaxDeletedElementsNumerator = 1;
+	static constexpr uint64 cMaxDeletedElementsDenominator = 8;
+
+	/// Values that the control bytes can have
+	static constexpr uint8	cBucketEmpty = 0;
+	static constexpr uint8	cBucketDeleted = 0x7f;
+	static constexpr uint8	cBucketUsed = 0x80;	// Lowest 7 bits are lowest 7 bits of the hash value
+
+	/// The buckets, an array of size mMaxSize
+	KeyValue *				mData = nullptr;
+
+	/// Control bytes, an array of size mMaxSize + 15
+	uint8 *					mControl = nullptr;
+
+	/// Number of elements in the table
+	size_type				mSize = 0;
+
+	/// Max number of elements that can be stored in the table
+	size_type				mMaxSize = 0;
+
+	/// Number of elements we can add to the table before we need to grow
+	size_type				mLoadLeft = 0;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/InsertionSort.h b/thirdparty/jolt_physics/Jolt/Core/InsertionSort.h
new file mode 100644
index 0000000000..8cd8798be2
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/InsertionSort.h
@@ -0,0 +1,58 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Implementation of the insertion sort algorithm.
+template <typename Iterator, typename Compare>
+inline void InsertionSort(Iterator inBegin, Iterator inEnd, Compare inCompare)
+{
+	// Empty arrays don't need to be sorted
+	if (inBegin != inEnd)
+	{
+		// Start at the second element
+		for (Iterator i = inBegin + 1; i != inEnd; ++i)
+		{
+			// Move this element to a temporary value
+			auto x = std::move(*i);
+
+			// Check if the element goes before inBegin (we can't decrement the iterator before inBegin so this needs to be a separate branch)
+			if (inCompare(x, *inBegin))
+			{
+				// Move all elements to the right to make space for x
+				Iterator prev;
+				for (Iterator j = i; j != inBegin; j = prev)
+				{
+					prev = j - 1;
+					*j = *prev;
+				}
+
+				// Move x to the first place
+				*inBegin = std::move(x);
+			}
+			else
+			{
+				// Move elements to the right as long as they are bigger than x
+				Iterator j = i;
+				for (Iterator prev = j - 1; inCompare(x, *prev); j = prev, --prev)
+					*j = std::move(*prev);
+
+				// Move x into place
+				*j = std::move(x);
+			}
+		}
+	}
+}
+
+/// Implementation of insertion sort algorithm without comparator.
+template <typename Iterator>
+inline void InsertionSort(Iterator inBegin, Iterator inEnd)
+{
+	std::less<> compare;
+	InsertionSort(inBegin, inEnd, compare);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/IssueReporting.cpp b/thirdparty/jolt_physics/Jolt/Core/IssueReporting.cpp
new file mode 100644
index 0000000000..ff32448000
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/IssueReporting.cpp
@@ -0,0 +1,27 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+JPH_NAMESPACE_BEGIN
+
+static void DummyTrace([[maybe_unused]] const char *inFMT, ...)
+{
+	JPH_ASSERT(false);
+};
+
+TraceFunction Trace = DummyTrace;
+
+#ifdef JPH_ENABLE_ASSERTS
+
+static bool DummyAssertFailed(const char *inExpression, const char *inMessage, const char *inFile, uint inLine)
+{
+	return true; // Trigger breakpoint
+};
+
+AssertFailedFunction AssertFailed = DummyAssertFailed;
+
+#endif // JPH_ENABLE_ASSERTS
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/IssueReporting.h b/thirdparty/jolt_physics/Jolt/Core/IssueReporting.h
new file mode 100644
index 0000000000..efbf9a0d88
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/IssueReporting.h
@@ -0,0 +1,38 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Trace function, needs to be overridden by application. This should output a line of text to the log / TTY.
+using TraceFunction = void (*)(const char *inFMT, ...);
+JPH_EXPORT extern TraceFunction Trace;
+
+// Always turn on asserts in Debug mode
+#if defined(JPH_DEBUG) && !defined(JPH_ENABLE_ASSERTS)
+	#define JPH_ENABLE_ASSERTS
+#endif
+
+#ifdef JPH_ENABLE_ASSERTS
+	/// Function called when an assertion fails. This function should return true if a breakpoint needs to be triggered
+	using AssertFailedFunction = bool(*)(const char *inExpression, const char *inMessage, const char *inFile, uint inLine);
+	JPH_EXPORT extern AssertFailedFunction AssertFailed;
+
+	// Helper functions to pass message on to failed function
+	struct AssertLastParam { };
+	inline bool AssertFailedParamHelper(const char *inExpression, const char *inFile, uint inLine, AssertLastParam) { return AssertFailed(inExpression, nullptr, inFile, inLine); }
+	inline bool AssertFailedParamHelper(const char *inExpression, const char *inFile, uint inLine, const char *inMessage, AssertLastParam) { return AssertFailed(inExpression, inMessage, inFile, inLine); }
+
+	/// Main assert macro, usage: JPH_ASSERT(condition, message) or JPH_ASSERT(condition)
+	#define JPH_ASSERT(inExpression, ...)	do { if (!(inExpression) && AssertFailedParamHelper(#inExpression, __FILE__, JPH::uint(__LINE__), ##__VA_ARGS__, JPH::AssertLastParam())) JPH_BREAKPOINT; } while (false)
+
+	#define JPH_IF_ENABLE_ASSERTS(...)		__VA_ARGS__
+#else
+	#define JPH_ASSERT(...)					((void)0)
+
+	#define JPH_IF_ENABLE_ASSERTS(...)
+#endif // JPH_ENABLE_ASSERTS
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/JobSystem.h b/thirdparty/jolt_physics/Jolt/Core/JobSystem.h
new file mode 100644
index 0000000000..1bd621a466
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/JobSystem.h
@@ -0,0 +1,311 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/Reference.h>
+#include <Jolt/Core/Color.h>
+#include <Jolt/Core/Profiler.h>
+#include <Jolt/Core/NonCopyable.h>
+#include <Jolt/Core/StaticArray.h>
+#include <Jolt/Core/Atomics.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// A class that allows units of work (Jobs) to be scheduled across multiple threads.
+/// It allows dependencies between the jobs so that the jobs form a graph.
+///
+/// The pattern for using this class is:
+///
+///		// Create job system
+///		JobSystem *job_system = new JobSystemThreadPool(...);
+///
+///		// Create some jobs
+///		JobHandle second_job = job_system->CreateJob("SecondJob", Color::sRed, []() { ... }, 1); // Create a job with 1 dependency
+///		JobHandle first_job = job_system->CreateJob("FirstJob", Color::sGreen, [second_job]() { ....; second_job.RemoveDependency(); }, 0); // Job can start immediately, will start second job when it's done
+///		JobHandle third_job = job_system->CreateJob("ThirdJob", Color::sBlue, []() { ... }, 0); // This job can run immediately as well and can run in parallel to job 1 and 2
+///
+///		// Add the jobs to the barrier so that we can execute them while we're waiting
+///		Barrier *barrier = job_system->CreateBarrier();
+///		barrier->AddJob(first_job);
+///		barrier->AddJob(second_job);
+///		barrier->AddJob(third_job);
+///		job_system->WaitForJobs(barrier);
+///
+///		// Clean up
+///		job_system->DestroyBarrier(barrier);
+///		delete job_system;
+///
+///	Jobs are guaranteed to be started in the order that their dependency counter becomes zero (in case they're scheduled on a background thread)
+///	or in the order they're added to the barrier (when dependency count is zero and when executing on the thread that calls WaitForJobs).
+///
+/// If you want to implement your own job system, inherit from JobSystem and implement:
+///
+/// * JobSystem::GetMaxConcurrency - This should return the maximum number of jobs that can run in parallel.
+/// * JobSystem::CreateJob - This should create a Job object and return it to the caller.
+/// * JobSystem::FreeJob - This should free the memory associated with the job object. It is called by the Job destructor when it is Release()-ed for the last time.
+/// * JobSystem::QueueJob/QueueJobs - These should store the job pointer in an internal queue to run immediately (dependencies are tracked internally, this function is called when the job can run).
+/// The Job objects are reference counted and are guaranteed to stay alive during the QueueJob(s) call. If you store the job in your own data structure you need to call AddRef() to take a reference.
+/// After the job has been executed you need to call Release() to release the reference. Make sure you no longer dereference the job pointer after calling Release().
+///
+/// JobSystem::Barrier is used to track the completion of a set of jobs. Jobs will be created by other jobs and added to the barrier while it is being waited on. This means that you cannot
+/// create a dependency graph beforehand as the graph changes while jobs are running. Implement the following functions:
+///
+/// * Barrier::AddJob/AddJobs - Add a job to the barrier, any call to WaitForJobs will now also wait for this job to complete.
+/// If you store the job in a data structure in the Barrier you need to call AddRef() on the job to keep it alive and Release() after you're done with it.
+/// * Barrier::OnJobFinished - This function is called when a job has finished executing, you can use this to track completion and remove the job from the list of jobs to wait on.
+///
+/// The functions on JobSystem that need to be implemented to support barriers are:
+///
+/// * JobSystem::CreateBarrier - Create a new barrier.
+/// * JobSystem::DestroyBarrier - Destroy a barrier.
+/// * JobSystem::WaitForJobs - This is the main function that is used to wait for all jobs that have been added to a Barrier. WaitForJobs can execute jobs that have
+/// been added to the barrier while waiting. It is not wise to execute other jobs that touch physics structures as this can cause race conditions and deadlocks. Please keep in mind that the barrier is
+/// only intended to wait on the completion of the Jolt jobs added to it, if you scheduled any jobs in your engine's job system to execute the Jolt jobs as part of QueueJob/QueueJobs, you might still need
+/// to wait for these in this function after the barrier is finished waiting.
+///
+/// An example implementation is JobSystemThreadPool. If you don't want to write the Barrier class you can also inherit from JobSystemWithBarrier.
+class JPH_EXPORT JobSystem : public NonCopyable
+{
+protected:
+	class Job;
+
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// A job handle contains a reference to a job. The job will be deleted as soon as there are no JobHandles.
+	/// referring to the job and when it is not in the job queue / being processed.
+	class JobHandle : private Ref<Job>
+	{
+	public:
+		/// Constructor
+		inline				JobHandle()									= default;
+		inline				JobHandle(const JobHandle &inHandle)		= default;
+		inline				JobHandle(JobHandle &&inHandle) noexcept	: Ref<Job>(std::move(inHandle)) { }
+
+		/// Constructor, only to be used by JobSystem
+		inline explicit		JobHandle(Job *inJob)						: Ref<Job>(inJob) { }
+
+		/// Assignment
+		inline JobHandle &	operator = (const JobHandle &inHandle)		= default;
+		inline JobHandle &	operator = (JobHandle &&inHandle) noexcept	= default;
+
+		/// Check if this handle contains a job
+		inline bool			IsValid() const								{ return GetPtr() != nullptr; }
+
+		/// Check if this job has finished executing
+		inline bool			IsDone() const								{ return GetPtr() != nullptr && GetPtr()->IsDone(); }
+
+		/// Add to the dependency counter.
+		inline void			AddDependency(int inCount = 1) const		{ GetPtr()->AddDependency(inCount); }
+
+		/// Remove from the dependency counter. Job will start whenever the dependency counter reaches zero
+		/// and if it does it is no longer valid to call the AddDependency/RemoveDependency functions.
+		inline void			RemoveDependency(int inCount = 1) const		{ GetPtr()->RemoveDependencyAndQueue(inCount); }
+
+		/// Remove a dependency from a batch of jobs at once, this can be more efficient than removing them one by one as it requires less locking
+		static inline void	sRemoveDependencies(const JobHandle *inHandles, uint inNumHandles, int inCount = 1);
+
+		/// Helper function to remove dependencies on a static array of job handles
+		template <uint N>
+		static inline void	sRemoveDependencies(StaticArray<JobHandle, N> &inHandles, int inCount = 1)
+		{
+			sRemoveDependencies(inHandles.data(), inHandles.size(), inCount);
+		}
+
+		/// Inherit the GetPtr function, only to be used by the JobSystem
+		using Ref<Job>::GetPtr;
+	};
+
+	/// A job barrier keeps track of a number of jobs and allows waiting until they are all completed.
+	class Barrier : public NonCopyable
+	{
+	public:
+		JPH_OVERRIDE_NEW_DELETE
+
+		/// Add a job to this barrier
+		/// Note that jobs can keep being added to the barrier while waiting for the barrier
+		virtual void		AddJob(const JobHandle &inJob) = 0;
+
+		/// Add multiple jobs to this barrier
+		/// Note that jobs can keep being added to the barrier while waiting for the barrier
+		virtual void		AddJobs(const JobHandle *inHandles, uint inNumHandles) = 0;
+
+	protected:
+		/// Job needs to be able to call OnJobFinished
+		friend class Job;
+
+		/// Destructor, you should call JobSystem::DestroyBarrier instead of destructing this object directly
+		virtual				~Barrier() = default;
+
+		/// Called by a Job to mark that it is finished
+		virtual void		OnJobFinished(Job *inJob) = 0;
+	};
+
+	/// Main function of the job
+	using JobFunction = function<void()>;
+
+	/// Destructor
+	virtual					~JobSystem() = default;
+
+	/// Get maximum number of concurrently executing jobs
+	virtual int				GetMaxConcurrency() const = 0;
+
+	/// Create a new job, the job is started immediately if inNumDependencies == 0 otherwise it starts when
+	/// RemoveDependency causes the dependency counter to reach 0.
+	virtual JobHandle		CreateJob(const char *inName, ColorArg inColor, const JobFunction &inJobFunction, uint32 inNumDependencies = 0) = 0;
+
+	/// Create a new barrier, used to wait on jobs
+	virtual Barrier *		CreateBarrier() = 0;
+
+	/// Destroy a barrier when it is no longer used. The barrier should be empty at this point.
+	virtual void			DestroyBarrier(Barrier *inBarrier) = 0;
+
+	/// Wait for a set of jobs to be finished, note that only 1 thread can be waiting on a barrier at a time
+	virtual void			WaitForJobs(Barrier *inBarrier) = 0;
+
+protected:
+	/// A class that contains information for a single unit of work
+	class Job
+	{
+	public:
+		JPH_OVERRIDE_NEW_DELETE
+
+		/// Constructor
+							Job([[maybe_unused]] const char *inJobName, [[maybe_unused]] ColorArg inColor, JobSystem *inJobSystem, const JobFunction &inJobFunction, uint32 inNumDependencies) :
+		#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+			mJobName(inJobName),
+			mColor(inColor),
+		#endif // defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+			mJobSystem(inJobSystem),
+			mJobFunction(inJobFunction),
+			mNumDependencies(inNumDependencies)
+		{
+		}
+
+		/// Get the jobs system to which this job belongs
+		inline JobSystem *	GetJobSystem()								{ return mJobSystem; }
+
+		/// Add or release a reference to this object
+		inline void			AddRef()
+		{
+			// Adding a reference can use relaxed memory ordering
+			mReferenceCount.fetch_add(1, memory_order_relaxed);
+		}
+		inline void			Release()
+		{
+		#ifndef JPH_TSAN_ENABLED
+			// Releasing a reference must use release semantics...
+			if (mReferenceCount.fetch_sub(1, memory_order_release) == 1)
+			{
+				// ... so that we can use acquire to ensure that we see any updates from other threads that released a ref before freeing the job
+				atomic_thread_fence(memory_order_acquire);
+				mJobSystem->FreeJob(this);
+			}
+		#else
+			// But under TSAN, we cannot use atomic_thread_fence, so we use an acq_rel operation unconditionally instead
+			if (mReferenceCount.fetch_sub(1, memory_order_acq_rel) == 1)
+				mJobSystem->FreeJob(this);
+		#endif
+		}
+
+		/// Add to the dependency counter.
+		inline void			AddDependency(int inCount);
+
+		/// Remove from the dependency counter. Returns true whenever the dependency counter reaches zero
+		/// and if it does it is no longer valid to call the AddDependency/RemoveDependency functions.
+		inline bool			RemoveDependency(int inCount);
+
+		/// Remove from the dependency counter. Job will be queued whenever the dependency counter reaches zero
+		/// and if it does it is no longer valid to call the AddDependency/RemoveDependency functions.
+		inline void			RemoveDependencyAndQueue(int inCount);
+
+		/// Set the job barrier that this job belongs to and returns false if this was not possible because the job already finished
+		inline bool			SetBarrier(Barrier *inBarrier)
+		{
+			intptr_t barrier = 0;
+			if (mBarrier.compare_exchange_strong(barrier, reinterpret_cast<intptr_t>(inBarrier), memory_order_relaxed))
+				return true;
+			JPH_ASSERT(barrier == cBarrierDoneState, "A job can only belong to 1 barrier");
+			return false;
+		}
+
+		/// Run the job function, returns the number of dependencies that this job still has or cExecutingState or cDoneState
+		inline uint32		Execute()
+		{
+			// Transition job to executing state
+			uint32 state = 0; // We can only start running with a dependency counter of 0
+			if (!mNumDependencies.compare_exchange_strong(state, cExecutingState, memory_order_acquire))
+				return state; // state is updated by compare_exchange_strong to the current value
+
+			// Run the job function
+			{
+				JPH_PROFILE(mJobName, mColor.GetUInt32());
+				mJobFunction();
+			}
+
+			// Fetch the barrier pointer and exchange it for the done state, so we're sure that no barrier gets set after we want to call the callback
+			intptr_t barrier = mBarrier.load(memory_order_relaxed);
+			for (;;)
+			{
+				if (mBarrier.compare_exchange_weak(barrier, cBarrierDoneState, memory_order_relaxed))
+					break;
+			}
+			JPH_ASSERT(barrier != cBarrierDoneState);
+
+			// Mark job as done
+			state = cExecutingState;
+			mNumDependencies.compare_exchange_strong(state, cDoneState, memory_order_relaxed);
+			JPH_ASSERT(state == cExecutingState);
+
+			// Notify the barrier after we've changed the job to the done state so that any thread reading the state after receiving the callback will see that the job has finished
+			if (barrier != 0)
+				reinterpret_cast<Barrier *>(barrier)->OnJobFinished(this);
+
+			return cDoneState;
+		}
+
+		/// Test if the job can be executed
+		inline bool			CanBeExecuted() const						{ return mNumDependencies.load(memory_order_relaxed) == 0; }
+
+		/// Test if the job finished executing
+		inline bool			IsDone() const								{ return mNumDependencies.load(memory_order_relaxed) == cDoneState; }
+
+	#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+		/// Get the name of the job
+		const char *		GetName() const								{ return mJobName; }
+	#endif // defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+
+		static constexpr uint32 cExecutingState = 0xe0e0e0e0;			///< Value of mNumDependencies when job is executing
+		static constexpr uint32 cDoneState		= 0xd0d0d0d0;			///< Value of mNumDependencies when job is done executing
+
+		static constexpr intptr_t cBarrierDoneState = ~intptr_t(0);		///< Value to use when the barrier has been triggered
+
+private:
+	#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+		const char *		mJobName;									///< Name of the job
+		Color				mColor;										///< Color of the job in the profiler
+	#endif // defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+		JobSystem *			mJobSystem;									///< The job system we belong to
+		atomic<intptr_t>	mBarrier = 0;								///< Barrier that this job is associated with (is a Barrier pointer)
+		JobFunction			mJobFunction;								///< Main job function
+		atomic<uint32>		mReferenceCount = 0;						///< Amount of JobHandles pointing to this job
+		atomic<uint32>		mNumDependencies;							///< Amount of jobs that need to complete before this job can run
+	};
+
+	/// Adds a job to the job queue
+	virtual void			QueueJob(Job *inJob) = 0;
+
+	/// Adds a number of jobs at once to the job queue
+	virtual void			QueueJobs(Job **inJobs, uint inNumJobs) = 0;
+
+	/// Frees a job
+	virtual void			FreeJob(Job *inJob) = 0;
+};
+
+using JobHandle = JobSystem::JobHandle;
+
+JPH_NAMESPACE_END
+
+#include "JobSystem.inl"
diff --git a/thirdparty/jolt_physics/Jolt/Core/JobSystem.inl b/thirdparty/jolt_physics/Jolt/Core/JobSystem.inl
new file mode 100644
index 0000000000..bee4f13b50
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/JobSystem.inl
@@ -0,0 +1,56 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+JPH_NAMESPACE_BEGIN
+
+void JobSystem::Job::AddDependency(int inCount)
+{
+	JPH_IF_ENABLE_ASSERTS(uint32 old_value =) mNumDependencies.fetch_add(inCount, memory_order_relaxed);
+	JPH_ASSERT(old_value > 0 && old_value != cExecutingState && old_value != cDoneState, "Job is queued, running or done, it is not allowed to add a dependency to a running job");
+}
+
+bool JobSystem::Job::RemoveDependency(int inCount)
+{
+	uint32 old_value = mNumDependencies.fetch_sub(inCount, memory_order_release);
+	JPH_ASSERT(old_value != cExecutingState && old_value != cDoneState, "Job is running or done, it is not allowed to add a dependency to a running job");
+	uint32 new_value = old_value - inCount;
+	JPH_ASSERT(old_value > new_value, "Test wrap around, this is a logic error");
+	return new_value == 0;
+}
+
+void JobSystem::Job::RemoveDependencyAndQueue(int inCount)
+{
+	if (RemoveDependency(inCount))
+		mJobSystem->QueueJob(this);
+}
+
+void JobSystem::JobHandle::sRemoveDependencies(const JobHandle *inHandles, uint inNumHandles, int inCount)
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(inNumHandles > 0);
+
+	// Get the job system, all jobs should be part of the same job system
+	JobSystem *job_system = inHandles->GetPtr()->GetJobSystem();
+
+	// Allocate a buffer to store the jobs that need to be queued
+	Job **jobs_to_queue = (Job **)JPH_STACK_ALLOC(inNumHandles * sizeof(Job *));
+	Job **next_job = jobs_to_queue;
+
+	// Remove the dependencies on all jobs
+	for (const JobHandle *handle = inHandles, *handle_end = inHandles + inNumHandles; handle < handle_end; ++handle)
+	{
+		Job *job = handle->GetPtr();
+		JPH_ASSERT(job->GetJobSystem() == job_system); // All jobs should belong to the same job system
+		if (job->RemoveDependency(inCount))
+			*(next_job++) = job;
+	}
+
+	// If any jobs need to be scheduled, schedule them as a batch
+	uint num_jobs_to_queue = uint(next_job - jobs_to_queue);
+	if (num_jobs_to_queue != 0)
+		job_system->QueueJobs(jobs_to_queue, num_jobs_to_queue);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/JobSystemSingleThreaded.cpp b/thirdparty/jolt_physics/Jolt/Core/JobSystemSingleThreaded.cpp
new file mode 100644
index 0000000000..5f09d1a04c
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/JobSystemSingleThreaded.cpp
@@ -0,0 +1,65 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Core/JobSystemSingleThreaded.h>
+
+JPH_NAMESPACE_BEGIN
+
+void JobSystemSingleThreaded::Init(uint inMaxJobs)
+{
+	mJobs.Init(inMaxJobs, inMaxJobs);
+}
+
+JobHandle JobSystemSingleThreaded::CreateJob(const char *inJobName, ColorArg inColor, const JobFunction &inJobFunction, uint32 inNumDependencies)
+{
+	// Construct an object
+	uint32 index = mJobs.ConstructObject(inJobName, inColor, this, inJobFunction, inNumDependencies);
+	JPH_ASSERT(index != AvailableJobs::cInvalidObjectIndex);
+	Job *job = &mJobs.Get(index);
+
+	// Construct handle to keep a reference, the job is queued below and will immediately complete
+	JobHandle handle(job);
+
+	// If there are no dependencies, queue the job now
+	if (inNumDependencies == 0)
+		QueueJob(job);
+
+	// Return the handle
+	return handle;
+}
+
+void JobSystemSingleThreaded::FreeJob(Job *inJob)
+{
+	mJobs.DestructObject(inJob);
+}
+
+void JobSystemSingleThreaded::QueueJob(Job *inJob)
+{
+	inJob->Execute();
+}
+
+void JobSystemSingleThreaded::QueueJobs(Job **inJobs, uint inNumJobs)
+{
+	for (uint i = 0; i < inNumJobs; ++i)
+		QueueJob(inJobs[i]);
+}
+
+JobSystem::Barrier *JobSystemSingleThreaded::CreateBarrier()
+{
+	return &mDummyBarrier;
+}
+
+void JobSystemSingleThreaded::DestroyBarrier(Barrier *inBarrier)
+{
+	// There's nothing to do here, the barrier is just a dummy
+}
+
+void JobSystemSingleThreaded::WaitForJobs(Barrier *inBarrier)
+{
+	// There's nothing to do here, the barrier is just a dummy, we just execute the jobs immediately
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/JobSystemSingleThreaded.h b/thirdparty/jolt_physics/Jolt/Core/JobSystemSingleThreaded.h
new file mode 100644
index 0000000000..4db599b313
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/JobSystemSingleThreaded.h
@@ -0,0 +1,62 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/JobSystem.h>
+#include <Jolt/Core/FixedSizeFreeList.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Implementation of a JobSystem without threads, runs jobs as soon as they are added
+class JPH_EXPORT JobSystemSingleThreaded final : public JobSystem
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+							JobSystemSingleThreaded() = default;
+	explicit				JobSystemSingleThreaded(uint inMaxJobs)			{ Init(inMaxJobs); }
+
+	/// Initialize the job system
+	/// @param inMaxJobs Max number of jobs that can be allocated at any time
+	void					Init(uint inMaxJobs);
+
+	// See JobSystem
+	virtual int				GetMaxConcurrency() const override				{ return 1; }
+	virtual JobHandle		CreateJob(const char *inName, ColorArg inColor, const JobFunction &inJobFunction, uint32 inNumDependencies = 0) override;
+	virtual Barrier *		CreateBarrier() override;
+	virtual void			DestroyBarrier(Barrier *inBarrier) override;
+	virtual void			WaitForJobs(Barrier *inBarrier) override;
+
+protected:
+	// Dummy implementation of Barrier, all jobs are executed immediately
+	class BarrierImpl : public Barrier
+	{
+	public:
+		JPH_OVERRIDE_NEW_DELETE
+
+		// See Barrier
+		virtual void		AddJob(const JobHandle &inJob) override			{ /* We don't need to track jobs */ }
+		virtual void		AddJobs(const JobHandle *inHandles, uint inNumHandles) override { /* We don't need to track jobs */ }
+
+	protected:
+		/// Called by a Job to mark that it is finished
+		virtual void		OnJobFinished(Job *inJob) override				{ /* We don't need to track jobs */ }
+	};
+
+	// See JobSystem
+	virtual void			QueueJob(Job *inJob) override;
+	virtual void			QueueJobs(Job **inJobs, uint inNumJobs) override;
+	virtual void			FreeJob(Job *inJob) override;
+
+	/// Shared barrier since the barrier implementation does nothing
+	BarrierImpl				mDummyBarrier;
+
+	/// Array of jobs (fixed size)
+	using AvailableJobs = FixedSizeFreeList<Job>;
+	AvailableJobs			mJobs;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/JobSystemThreadPool.cpp b/thirdparty/jolt_physics/Jolt/Core/JobSystemThreadPool.cpp
new file mode 100644
index 0000000000..6d4dc10f50
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/JobSystemThreadPool.cpp
@@ -0,0 +1,364 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Core/JobSystemThreadPool.h>
+#include <Jolt/Core/Profiler.h>
+#include <Jolt/Core/FPException.h>
+
+#ifdef JPH_PLATFORM_WINDOWS
+	JPH_SUPPRESS_WARNING_PUSH
+	JPH_MSVC_SUPPRESS_WARNING(5039) // winbase.h(13179): warning C5039: 'TpSetCallbackCleanupGroup': pointer or reference to potentially throwing function passed to 'extern "C"' function under -EHc. Undefined behavior may occur if this function throws an exception.
+	#ifndef WIN32_LEAN_AND_MEAN
+		#define WIN32_LEAN_AND_MEAN
+	#endif
+#ifndef JPH_COMPILER_MINGW
+	#include <Windows.h>
+#else
+	#include <windows.h>
+#endif
+
+	JPH_SUPPRESS_WARNING_POP
+#endif
+#ifdef JPH_PLATFORM_LINUX
+	#include <sys/prctl.h>
+#endif
+
+JPH_NAMESPACE_BEGIN
+
+void JobSystemThreadPool::Init(uint inMaxJobs, uint inMaxBarriers, int inNumThreads)
+{
+	JobSystemWithBarrier::Init(inMaxBarriers);
+
+	// Init freelist of jobs
+	mJobs.Init(inMaxJobs, inMaxJobs);
+
+	// Init queue
+	for (atomic<Job *> &j : mQueue)
+		j = nullptr;
+
+	// Start the worker threads
+	StartThreads(inNumThreads);
+}
+
+JobSystemThreadPool::JobSystemThreadPool(uint inMaxJobs, uint inMaxBarriers, int inNumThreads)
+{
+	Init(inMaxJobs, inMaxBarriers, inNumThreads);
+}
+
+void JobSystemThreadPool::StartThreads([[maybe_unused]] int inNumThreads)
+{
+#if !defined(JPH_CPU_WASM) || defined(__EMSCRIPTEN_PTHREADS__) // If we're running without threads support we cannot create threads and we ignore the inNumThreads parameter
+	// Auto detect number of threads
+	if (inNumThreads < 0)
+		inNumThreads = thread::hardware_concurrency() - 1;
+
+	// If no threads are requested we're done
+	if (inNumThreads == 0)
+		return;
+
+	// Don't quit the threads
+	mQuit = false;
+
+	// Allocate heads
+	mHeads = reinterpret_cast<atomic<uint> *>(Allocate(sizeof(atomic<uint>) * inNumThreads));
+	for (int i = 0; i < inNumThreads; ++i)
+		mHeads[i] = 0;
+
+	// Start running threads
+	JPH_ASSERT(mThreads.empty());
+	mThreads.reserve(inNumThreads);
+	for (int i = 0; i < inNumThreads; ++i)
+		mThreads.emplace_back([this, i] { ThreadMain(i); });
+#endif
+}
+
+JobSystemThreadPool::~JobSystemThreadPool()
+{
+	// Stop all worker threads
+	StopThreads();
+}
+
+void JobSystemThreadPool::StopThreads()
+{
+	if (mThreads.empty())
+		return;
+
+	// Signal threads that we want to stop and wake them up
+	mQuit = true;
+	mSemaphore.Release((uint)mThreads.size());
+
+	// Wait for all threads to finish
+	for (thread &t : mThreads)
+		if (t.joinable())
+			t.join();
+
+	// Delete all threads
+	mThreads.clear();
+
+	// Ensure that there are no lingering jobs in the queue
+	for (uint head = 0; head != mTail; ++head)
+	{
+		// Fetch job
+		Job *job_ptr = mQueue[head & (cQueueLength - 1)].exchange(nullptr);
+		if (job_ptr != nullptr)
+		{
+			// And execute it
+			job_ptr->Execute();
+			job_ptr->Release();
+		}
+	}
+
+	// Destroy heads and reset tail
+	Free(mHeads);
+	mHeads = nullptr;
+	mTail = 0;
+}
+
+JobHandle JobSystemThreadPool::CreateJob(const char *inJobName, ColorArg inColor, const JobFunction &inJobFunction, uint32 inNumDependencies)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Loop until we can get a job from the free list
+	uint32 index;
+	for (;;)
+	{
+		index = mJobs.ConstructObject(inJobName, inColor, this, inJobFunction, inNumDependencies);
+		if (index != AvailableJobs::cInvalidObjectIndex)
+			break;
+		JPH_ASSERT(false, "No jobs available!");
+		std::this_thread::sleep_for(std::chrono::microseconds(100));
+	}
+	Job *job = &mJobs.Get(index);
+
+	// Construct handle to keep a reference, the job is queued below and may immediately complete
+	JobHandle handle(job);
+
+	// If there are no dependencies, queue the job now
+	if (inNumDependencies == 0)
+		QueueJob(job);
+
+	// Return the handle
+	return handle;
+}
+
+void JobSystemThreadPool::FreeJob(Job *inJob)
+{
+	mJobs.DestructObject(inJob);
+}
+
+uint JobSystemThreadPool::GetHead() const
+{
+	// Find the minimal value across all threads
+	uint head = mTail;
+	for (size_t i = 0; i < mThreads.size(); ++i)
+		head = min(head, mHeads[i].load());
+	return head;
+}
+
+void JobSystemThreadPool::QueueJobInternal(Job *inJob)
+{
+	// Add reference to job because we're adding the job to the queue
+	inJob->AddRef();
+
+	// Need to read head first because otherwise the tail can already have passed the head
+	// We read the head outside of the loop since it involves iterating over all threads and we only need to update
+	// it if there's not enough space in the queue.
+	uint head = GetHead();
+
+	for (;;)
+	{
+		// Check if there's space in the queue
+		uint old_value = mTail;
+		if (old_value - head >= cQueueLength)
+		{
+			// We calculated the head outside of the loop, update head (and we also need to update tail to prevent it from passing head)
+			head = GetHead();
+			old_value = mTail;
+
+			// Second check if there's space in the queue
+			if (old_value - head >= cQueueLength)
+			{
+				// Wake up all threads in order to ensure that they can clear any nullptrs they may not have processed yet
+				mSemaphore.Release((uint)mThreads.size());
+
+				// Sleep a little (we have to wait for other threads to update their head pointer in order for us to be able to continue)
+				std::this_thread::sleep_for(std::chrono::microseconds(100));
+				continue;
+			}
+		}
+
+		// Write the job pointer if the slot is empty
+		Job *expected_job = nullptr;
+		bool success = mQueue[old_value & (cQueueLength - 1)].compare_exchange_strong(expected_job, inJob);
+
+		// Regardless of who wrote the slot, we will update the tail (if the successful thread got scheduled out
+		// after writing the pointer we still want to be able to continue)
+		mTail.compare_exchange_strong(old_value, old_value + 1);
+
+		// If we successfully added our job we're done
+		if (success)
+			break;
+	}
+}
+
+void JobSystemThreadPool::QueueJob(Job *inJob)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// If we have no worker threads, we can't queue the job either. We assume in this case that the job will be added to a barrier and that the barrier will execute the job when it's Wait() function is called.
+	if (mThreads.empty())
+		return;
+
+	// Queue the job
+	QueueJobInternal(inJob);
+
+	// Wake up thread
+	mSemaphore.Release();
+}
+
+void JobSystemThreadPool::QueueJobs(Job **inJobs, uint inNumJobs)
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(inNumJobs > 0);
+
+	// If we have no worker threads, we can't queue the job either. We assume in this case that the job will be added to a barrier and that the barrier will execute the job when it's Wait() function is called.
+	if (mThreads.empty())
+		return;
+
+	// Queue all jobs
+	for (Job **job = inJobs, **job_end = inJobs + inNumJobs; job < job_end; ++job)
+		QueueJobInternal(*job);
+
+	// Wake up threads
+	mSemaphore.Release(min(inNumJobs, (uint)mThreads.size()));
+}
+
+#if defined(JPH_PLATFORM_WINDOWS)
+
+#if !defined(JPH_COMPILER_MINGW) // MinGW doesn't support __try/__except)
+	// Sets the current thread name in MSVC debugger
+	static void RaiseThreadNameException(const char *inName)
+	{
+		#pragma pack(push, 8)
+
+		struct THREADNAME_INFO
+		{
+			DWORD	dwType;			// Must be 0x1000.
+			LPCSTR	szName;			// Pointer to name (in user addr space).
+			DWORD	dwThreadID;		// Thread ID (-1=caller thread).
+			DWORD	dwFlags;		// Reserved for future use, must be zero.
+		};
+
+		#pragma pack(pop)
+
+		THREADNAME_INFO info;
+		info.dwType = 0x1000;
+		info.szName = inName;
+		info.dwThreadID = (DWORD)-1;
+		info.dwFlags = 0;
+
+		__try
+		{
+			RaiseException(0x406D1388, 0, sizeof(info) / sizeof(ULONG_PTR), (ULONG_PTR *)&info);
+		}
+		__except(EXCEPTION_EXECUTE_HANDLER)
+		{
+		}
+	}
+#endif // !JPH_COMPILER_MINGW
+
+	static void SetThreadName(const char* inName)
+	{
+		JPH_SUPPRESS_WARNING_PUSH
+
+		// Suppress casting warning, it's fine here as GetProcAddress doesn't really return a FARPROC
+		JPH_CLANG_SUPPRESS_WARNING("-Wcast-function-type") // error : cast from 'FARPROC' (aka 'long long (*)()') to 'SetThreadDescriptionFunc' (aka 'long (*)(void *, const wchar_t *)') converts to incompatible function type
+		JPH_CLANG_SUPPRESS_WARNING("-Wcast-function-type-strict") // error : cast from 'FARPROC' (aka 'long long (*)()') to 'SetThreadDescriptionFunc' (aka 'long (*)(void *, const wchar_t *)') converts to incompatible function type
+		JPH_MSVC_SUPPRESS_WARNING(4191) // reinterpret_cast' : unsafe conversion from 'FARPROC' to 'SetThreadDescriptionFunc'. Calling this function through the result pointer may cause your program to fail
+
+		using SetThreadDescriptionFunc = HRESULT(WINAPI*)(HANDLE hThread, PCWSTR lpThreadDescription);
+		static SetThreadDescriptionFunc SetThreadDescription = reinterpret_cast<SetThreadDescriptionFunc>(GetProcAddress(GetModuleHandleW(L"Kernel32.dll"), "SetThreadDescription"));
+
+		JPH_SUPPRESS_WARNING_POP
+
+		if (SetThreadDescription)
+		{
+			wchar_t name_buffer[64] = { 0 };
+			if (MultiByteToWideChar(CP_UTF8, 0, inName, -1, name_buffer, sizeof(name_buffer) / sizeof(wchar_t) - 1) == 0)
+				return;
+
+			SetThreadDescription(GetCurrentThread(), name_buffer);
+		}
+#if !defined(JPH_COMPILER_MINGW)
+		else if (IsDebuggerPresent())
+			RaiseThreadNameException(inName);
+#endif // !JPH_COMPILER_MINGW
+	}
+#elif defined(JPH_PLATFORM_LINUX)
+	static void SetThreadName(const char *inName)
+	{
+		JPH_ASSERT(strlen(inName) < 16); // String will be truncated if it is longer
+		prctl(PR_SET_NAME, inName, 0, 0, 0);
+	}
+#endif // JPH_PLATFORM_LINUX
+
+void JobSystemThreadPool::ThreadMain(int inThreadIndex)
+{
+	// Name the thread
+	char name[64];
+	snprintf(name, sizeof(name), "Worker %d", int(inThreadIndex + 1));
+
+#if defined(JPH_PLATFORM_WINDOWS) || defined(JPH_PLATFORM_LINUX)
+	SetThreadName(name);
+#endif // JPH_PLATFORM_WINDOWS && !JPH_COMPILER_MINGW
+
+	// Enable floating point exceptions
+	FPExceptionsEnable enable_exceptions;
+	JPH_UNUSED(enable_exceptions);
+
+	JPH_PROFILE_THREAD_START(name);
+
+	// Call the thread init function
+	mThreadInitFunction(inThreadIndex);
+
+	atomic<uint> &head = mHeads[inThreadIndex];
+
+	while (!mQuit)
+	{
+		// Wait for jobs
+		mSemaphore.Acquire();
+
+		{
+			JPH_PROFILE("Executing Jobs");
+
+			// Loop over the queue
+			while (head != mTail)
+			{
+				// Exchange any job pointer we find with a nullptr
+				atomic<Job *> &job = mQueue[head & (cQueueLength - 1)];
+				if (job.load() != nullptr)
+				{
+					Job *job_ptr = job.exchange(nullptr);
+					if (job_ptr != nullptr)
+					{
+						// And execute it
+						job_ptr->Execute();
+						job_ptr->Release();
+					}
+				}
+				head++;
+			}
+		}
+	}
+
+	// Call the thread exit function
+	mThreadExitFunction(inThreadIndex);
+
+	JPH_PROFILE_THREAD_END();
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/JobSystemThreadPool.h b/thirdparty/jolt_physics/Jolt/Core/JobSystemThreadPool.h
new file mode 100644
index 0000000000..2e44cb69e0
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/JobSystemThreadPool.h
@@ -0,0 +1,101 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/JobSystemWithBarrier.h>
+#include <Jolt/Core/FixedSizeFreeList.h>
+#include <Jolt/Core/Semaphore.h>
+
+JPH_SUPPRESS_WARNINGS_STD_BEGIN
+#include <thread>
+JPH_SUPPRESS_WARNINGS_STD_END
+
+JPH_NAMESPACE_BEGIN
+
+// Things we're using from STL
+using std::thread;
+
+/// Implementation of a JobSystem using a thread pool
+///
+/// Note that this is considered an example implementation. It is expected that when you integrate
+/// the physics engine into your own project that you'll provide your own implementation of the
+/// JobSystem built on top of whatever job system your project uses.
+class JPH_EXPORT JobSystemThreadPool final : public JobSystemWithBarrier
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Creates a thread pool.
+	/// @see JobSystemThreadPool::Init
+							JobSystemThreadPool(uint inMaxJobs, uint inMaxBarriers, int inNumThreads = -1);
+							JobSystemThreadPool() = default;
+	virtual					~JobSystemThreadPool() override;
+
+	/// Functions to call when a thread is initialized or exits, must be set before calling Init()
+	using InitExitFunction = function<void(int)>;
+	void					SetThreadInitFunction(const InitExitFunction &inInitFunction)	{ mThreadInitFunction = inInitFunction; }
+	void					SetThreadExitFunction(const InitExitFunction &inExitFunction)	{ mThreadExitFunction = inExitFunction; }
+
+	/// Initialize the thread pool
+	/// @param inMaxJobs Max number of jobs that can be allocated at any time
+	/// @param inMaxBarriers Max number of barriers that can be allocated at any time
+	/// @param inNumThreads Number of threads to start (the number of concurrent jobs is 1 more because the main thread will also run jobs while waiting for a barrier to complete). Use -1 to auto detect the amount of CPU's.
+	void					Init(uint inMaxJobs, uint inMaxBarriers, int inNumThreads = -1);
+
+	// See JobSystem
+	virtual int				GetMaxConcurrency() const override				{ return int(mThreads.size()) + 1; }
+	virtual JobHandle		CreateJob(const char *inName, ColorArg inColor, const JobFunction &inJobFunction, uint32 inNumDependencies = 0) override;
+
+	/// Change the max concurrency after initialization
+	void					SetNumThreads(int inNumThreads)					{ StopThreads(); StartThreads(inNumThreads); }
+
+protected:
+	// See JobSystem
+	virtual void			QueueJob(Job *inJob) override;
+	virtual void			QueueJobs(Job **inJobs, uint inNumJobs) override;
+	virtual void			FreeJob(Job *inJob) override;
+
+private:
+	/// Start/stop the worker threads
+	void					StartThreads(int inNumThreads);
+	void					StopThreads();
+
+	/// Entry point for a thread
+	void					ThreadMain(int inThreadIndex);
+
+	/// Get the head of the thread that has processed the least amount of jobs
+	inline uint				GetHead() const;
+
+	/// Internal helper function to queue a job
+	inline void				QueueJobInternal(Job *inJob);
+
+	/// Functions to call when initializing or exiting a thread
+	InitExitFunction		mThreadInitFunction = [](int) { };
+	InitExitFunction		mThreadExitFunction = [](int) { };
+
+	/// Array of jobs (fixed size)
+	using AvailableJobs = FixedSizeFreeList<Job>;
+	AvailableJobs			mJobs;
+
+	/// Threads running jobs
+	Array<thread>			mThreads;
+
+	// The job queue
+	static constexpr uint32 cQueueLength = 1024;
+	static_assert(IsPowerOf2(cQueueLength));								// We do bit operations and require queue length to be a power of 2
+	atomic<Job *>			mQueue[cQueueLength];
+
+	// Head and tail of the queue, do this value modulo cQueueLength - 1 to get the element in the mQueue array
+	atomic<uint> *			mHeads = nullptr;								///< Per executing thread the head of the current queue
+	alignas(JPH_CACHE_LINE_SIZE) atomic<uint> mTail = 0;					///< Tail (write end) of the queue
+
+	// Semaphore used to signal worker threads that there is new work
+	Semaphore				mSemaphore;
+
+	/// Boolean to indicate that we want to stop the job system
+	atomic<bool>			mQuit = false;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/JobSystemWithBarrier.cpp b/thirdparty/jolt_physics/Jolt/Core/JobSystemWithBarrier.cpp
new file mode 100644
index 0000000000..3d4c7268b4
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/JobSystemWithBarrier.cpp
@@ -0,0 +1,230 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Core/JobSystemWithBarrier.h>
+#include <Jolt/Core/Profiler.h>
+
+JPH_SUPPRESS_WARNINGS_STD_BEGIN
+#include <thread>
+JPH_SUPPRESS_WARNINGS_STD_END
+
+JPH_NAMESPACE_BEGIN
+
+JobSystemWithBarrier::BarrierImpl::BarrierImpl()
+{
+	for (atomic<Job *> &j : mJobs)
+		j = nullptr;
+}
+
+JobSystemWithBarrier::BarrierImpl::~BarrierImpl()
+{
+	JPH_ASSERT(IsEmpty());
+}
+
+void JobSystemWithBarrier::BarrierImpl::AddJob(const JobHandle &inJob)
+{
+	JPH_PROFILE_FUNCTION();
+
+	bool release_semaphore = false;
+
+	// Set the barrier on the job, this returns true if the barrier was successfully set (otherwise the job is already done and we don't need to add it to our list)
+	Job *job = inJob.GetPtr();
+	if (job->SetBarrier(this))
+	{
+		// If the job can be executed we want to release the semaphore an extra time to allow the waiting thread to start executing it
+		mNumToAcquire++;
+		if (job->CanBeExecuted())
+		{
+			release_semaphore = true;
+			mNumToAcquire++;
+		}
+
+		// Add the job to our job list
+		job->AddRef();
+		uint write_index = mJobWriteIndex++;
+		while (write_index - mJobReadIndex >= cMaxJobs)
+		{
+			JPH_ASSERT(false, "Barrier full, stalling!");
+			std::this_thread::sleep_for(std::chrono::microseconds(100));
+		}
+		mJobs[write_index & (cMaxJobs - 1)] = job;
+	}
+
+	// Notify waiting thread that a new executable job is available
+	if (release_semaphore)
+		mSemaphore.Release();
+}
+
+void JobSystemWithBarrier::BarrierImpl::AddJobs(const JobHandle *inHandles, uint inNumHandles)
+{
+	JPH_PROFILE_FUNCTION();
+
+	bool release_semaphore = false;
+
+	for (const JobHandle *handle = inHandles, *handles_end = inHandles + inNumHandles; handle < handles_end; ++handle)
+	{
+		// Set the barrier on the job, this returns true if the barrier was successfully set (otherwise the job is already done and we don't need to add it to our list)
+		Job *job = handle->GetPtr();
+		if (job->SetBarrier(this))
+		{
+			// If the job can be executed we want to release the semaphore an extra time to allow the waiting thread to start executing it
+			mNumToAcquire++;
+			if (!release_semaphore && job->CanBeExecuted())
+			{
+				release_semaphore = true;
+				mNumToAcquire++;
+			}
+
+			// Add the job to our job list
+			job->AddRef();
+			uint write_index = mJobWriteIndex++;
+			while (write_index - mJobReadIndex >= cMaxJobs)
+			{
+				JPH_ASSERT(false, "Barrier full, stalling!");
+				std::this_thread::sleep_for(std::chrono::microseconds(100));
+			}
+			mJobs[write_index & (cMaxJobs - 1)] = job;
+		}
+	}
+
+	// Notify waiting thread that a new executable job is available
+	if (release_semaphore)
+		mSemaphore.Release();
+}
+
+void JobSystemWithBarrier::BarrierImpl::OnJobFinished(Job *inJob)
+{
+	JPH_PROFILE_FUNCTION();
+
+	mSemaphore.Release();
+}
+
+void JobSystemWithBarrier::BarrierImpl::Wait()
+{
+	while (mNumToAcquire > 0)
+	{
+		{
+			JPH_PROFILE("Execute Jobs");
+
+			// Go through all jobs
+			bool has_executed;
+			do
+			{
+				has_executed = false;
+
+				// Loop through the jobs and erase jobs from the beginning of the list that are done
+				while (mJobReadIndex < mJobWriteIndex)
+				{
+					atomic<Job *> &job = mJobs[mJobReadIndex & (cMaxJobs - 1)];
+					Job *job_ptr = job.load();
+					if (job_ptr == nullptr || !job_ptr->IsDone())
+						break;
+
+					// Job is finished, release it
+					job_ptr->Release();
+					job = nullptr;
+					++mJobReadIndex;
+				}
+
+				// Loop through the jobs and execute the first executable job
+				for (uint index = mJobReadIndex; index < mJobWriteIndex; ++index)
+				{
+					const atomic<Job *> &job = mJobs[index & (cMaxJobs - 1)];
+					Job *job_ptr = job.load();
+					if (job_ptr != nullptr && job_ptr->CanBeExecuted())
+					{
+						// This will only execute the job if it has not already executed
+						job_ptr->Execute();
+						has_executed = true;
+						break;
+					}
+				}
+
+			} while (has_executed);
+		}
+
+		// Wait for another thread to wake us when either there is more work to do or when all jobs have completed.
+		// When there have been multiple releases, we acquire them all at the same time to avoid needlessly spinning on executing jobs.
+		// Note that using GetValue is inherently unsafe since we can read a stale value, but this is not an issue here as this is the only
+		// place where we acquire the semaphore. Other threads only release it, so we can only read a value that is lower or equal to the actual value.
+		int num_to_acquire = max(1, mSemaphore.GetValue());
+		mSemaphore.Acquire(num_to_acquire);
+		mNumToAcquire -= num_to_acquire;
+	}
+
+	// All jobs should be done now, release them
+	while (mJobReadIndex < mJobWriteIndex)
+	{
+		atomic<Job *> &job = mJobs[mJobReadIndex & (cMaxJobs - 1)];
+		Job *job_ptr = job.load();
+		JPH_ASSERT(job_ptr != nullptr && job_ptr->IsDone());
+		job_ptr->Release();
+		job = nullptr;
+		++mJobReadIndex;
+	}
+}
+
+void JobSystemWithBarrier::Init(uint inMaxBarriers)
+{
+	JPH_ASSERT(mBarriers == nullptr); // Already initialized?
+
+	// Init freelist of barriers
+	mMaxBarriers = inMaxBarriers;
+	mBarriers = new BarrierImpl [inMaxBarriers];
+}
+
+JobSystemWithBarrier::JobSystemWithBarrier(uint inMaxBarriers)
+{
+	Init(inMaxBarriers);
+}
+
+JobSystemWithBarrier::~JobSystemWithBarrier()
+{
+	// Ensure that none of the barriers are used
+#ifdef JPH_ENABLE_ASSERTS
+	for (const BarrierImpl *b = mBarriers, *b_end = mBarriers + mMaxBarriers; b < b_end; ++b)
+		JPH_ASSERT(!b->mInUse);
+#endif // JPH_ENABLE_ASSERTS
+	delete [] mBarriers;
+}
+
+JobSystem::Barrier *JobSystemWithBarrier::CreateBarrier()
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Find the first unused barrier
+	for (uint32 index = 0; index < mMaxBarriers; ++index)
+	{
+		bool expected = false;
+		if (mBarriers[index].mInUse.compare_exchange_strong(expected, true))
+			return &mBarriers[index];
+	}
+
+	return nullptr;
+}
+
+void JobSystemWithBarrier::DestroyBarrier(Barrier *inBarrier)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Check that no jobs are in the barrier
+	JPH_ASSERT(static_cast<BarrierImpl *>(inBarrier)->IsEmpty());
+
+	// Flag the barrier as unused
+	bool expected = true;
+	static_cast<BarrierImpl *>(inBarrier)->mInUse.compare_exchange_strong(expected, false);
+	JPH_ASSERT(expected);
+}
+
+void JobSystemWithBarrier::WaitForJobs(Barrier *inBarrier)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Let our barrier implementation wait for the jobs
+	static_cast<BarrierImpl *>(inBarrier)->Wait();
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/JobSystemWithBarrier.h b/thirdparty/jolt_physics/Jolt/Core/JobSystemWithBarrier.h
new file mode 100644
index 0000000000..0fc663330e
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/JobSystemWithBarrier.h
@@ -0,0 +1,85 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/JobSystem.h>
+#include <Jolt/Core/Semaphore.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Implementation of the Barrier class for a JobSystem
+///
+/// This class can be used to make it easier to create a new JobSystem implementation that integrates with your own job system.
+/// It will implement all functionality relating to barriers, so the only functions that are left to be implemented are:
+///
+/// * JobSystem::GetMaxConcurrency
+/// * JobSystem::CreateJob
+/// * JobSystem::FreeJob
+/// * JobSystem::QueueJob/QueueJobs
+///
+/// See instructions in JobSystem for more information on how to implement these.
+class JPH_EXPORT JobSystemWithBarrier : public JobSystem
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructs barriers
+	/// @see JobSystemWithBarrier::Init
+	explicit				JobSystemWithBarrier(uint inMaxBarriers);
+							JobSystemWithBarrier() = default;
+	virtual					~JobSystemWithBarrier() override;
+
+	/// Initialize the barriers
+	/// @param inMaxBarriers Max number of barriers that can be allocated at any time
+	void					Init(uint inMaxBarriers);
+
+	// See JobSystem
+	virtual Barrier *		CreateBarrier() override;
+	virtual void			DestroyBarrier(Barrier *inBarrier) override;
+	virtual void			WaitForJobs(Barrier *inBarrier) override;
+
+private:
+	class BarrierImpl : public Barrier
+	{
+	public:
+		JPH_OVERRIDE_NEW_DELETE
+
+		/// Constructor
+							BarrierImpl();
+		virtual				~BarrierImpl() override;
+
+		// See Barrier
+		virtual void		AddJob(const JobHandle &inJob) override;
+		virtual void		AddJobs(const JobHandle *inHandles, uint inNumHandles) override;
+
+		/// Check if there are any jobs in the job barrier
+		inline bool			IsEmpty() const									{ return mJobReadIndex == mJobWriteIndex; }
+
+		/// Wait for all jobs in this job barrier, while waiting, execute jobs that are part of this barrier on the current thread
+		void				Wait();
+
+		/// Flag to indicate if a barrier has been handed out
+		atomic<bool>		mInUse { false };
+
+	protected:
+		/// Called by a Job to mark that it is finished
+		virtual void		OnJobFinished(Job *inJob) override;
+
+		/// Jobs queue for the barrier
+		static constexpr uint cMaxJobs = 2048;
+		static_assert(IsPowerOf2(cMaxJobs));								// We do bit operations and require max jobs to be a power of 2
+		atomic<Job *>		mJobs[cMaxJobs];								///< List of jobs that are part of this barrier, nullptrs for empty slots
+		alignas(JPH_CACHE_LINE_SIZE) atomic<uint> mJobReadIndex { 0 };		///< First job that could be valid (modulo cMaxJobs), can be nullptr if other thread is still working on adding the job
+		alignas(JPH_CACHE_LINE_SIZE) atomic<uint> mJobWriteIndex { 0 };		///< First job that can be written (modulo cMaxJobs)
+		atomic<int>			mNumToAcquire { 0 };							///< Number of times the semaphore has been released, the barrier should acquire the semaphore this many times (written at the same time as mJobWriteIndex so ok to put in same cache line)
+		Semaphore			mSemaphore;										///< Semaphore used by finishing jobs to signal the barrier that they're done
+	};
+
+	/// Array of barriers (we keep them constructed all the time since constructing a semaphore/mutex is not cheap)
+	uint					mMaxBarriers = 0;								///< Max amount of barriers
+	BarrierImpl *			mBarriers = nullptr;							///< List of the actual barriers
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/LinearCurve.cpp b/thirdparty/jolt_physics/Jolt/Core/LinearCurve.cpp
new file mode 100644
index 0000000000..8c8f7682ec
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/LinearCurve.cpp
@@ -0,0 +1,51 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Core/LinearCurve.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(LinearCurve::Point)
+{
+	JPH_ADD_ATTRIBUTE(Point, mX)
+	JPH_ADD_ATTRIBUTE(Point, mY)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(LinearCurve)
+{
+	JPH_ADD_ATTRIBUTE(LinearCurve, mPoints)
+}
+
+float LinearCurve::GetValue(float inX) const
+{
+	if (mPoints.empty())
+		return 0.0f;
+
+	Points::const_iterator i2 = std::lower_bound(mPoints.begin(), mPoints.end(), inX, [](const Point &inPoint, float inValue) { return inPoint.mX < inValue; });
+
+	if (i2 == mPoints.begin())
+		return mPoints.front().mY;
+	else if (i2 == mPoints.end())
+		return mPoints.back().mY;
+
+	Points::const_iterator i1 = i2 - 1;
+	return i1->mY + (inX - i1->mX) * (i2->mY - i1->mY) / (i2->mX - i1->mX);
+}
+
+void LinearCurve::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(mPoints);
+}
+
+void LinearCurve::RestoreBinaryState(StreamIn &inStream)
+{
+	inStream.Read(mPoints);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/LinearCurve.h b/thirdparty/jolt_physics/Jolt/Core/LinearCurve.h
new file mode 100644
index 0000000000..8700144068
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/LinearCurve.h
@@ -0,0 +1,67 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/ObjectStream/SerializableObject.h>
+#include <Jolt/Core/QuickSort.h>
+
+JPH_NAMESPACE_BEGIN
+
+class StreamOut;
+class StreamIn;
+
+// A set of points (x, y) that form a linear curve
+class JPH_EXPORT LinearCurve
+{
+	JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, LinearCurve)
+
+public:
+	/// A point on the curve
+	class Point
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Point)
+
+	public:
+		float			mX = 0.0f;
+		float			mY = 0.0f;
+	};
+
+	/// Remove all points
+	void				Clear()											{ mPoints.clear(); }
+
+	/// Reserve memory for inNumPoints points
+	void				Reserve(uint inNumPoints)						{ mPoints.reserve(inNumPoints); }
+
+	/// Add a point to the curve. Points must be inserted in ascending X or Sort() needs to be called when all points have been added.
+	/// @param inX X value
+	/// @param inY Y value
+	void				AddPoint(float inX, float inY)					{ mPoints.push_back({ inX, inY }); }
+
+	/// Sort the points on X ascending
+	void				Sort()											{ QuickSort(mPoints.begin(), mPoints.end(), [](const Point &inLHS, const Point &inRHS) { return inLHS.mX < inRHS.mX; }); }
+
+	/// Get the lowest X value
+	float				GetMinX() const									{ return mPoints.empty()? 0.0f : mPoints.front().mX; }
+
+	/// Get the highest X value
+	float				GetMaxX() const									{ return mPoints.empty()? 0.0f : mPoints.back().mX; }
+
+	/// Sample value on the curve
+	/// @param inX X value to sample at
+	/// @return Interpolated Y value
+	float				GetValue(float inX) const;
+
+	/// Saves the state of this object in binary form to inStream.
+	void				SaveBinaryState(StreamOut &inStream) const;
+
+	/// Restore the state of this object from inStream.
+	void				RestoreBinaryState(StreamIn &inStream);
+
+	/// The points on the curve, should be sorted ascending by x
+	using Points = Array<Point>;
+	Points				mPoints;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/LockFreeHashMap.h b/thirdparty/jolt_physics/Jolt/Core/LockFreeHashMap.h
new file mode 100644
index 0000000000..ac1557b39a
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/LockFreeHashMap.h
@@ -0,0 +1,182 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/NonCopyable.h>
+#include <Jolt/Core/Atomics.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Allocator for a lock free hash map
+class LFHMAllocator : public NonCopyable
+{
+public:
+	/// Destructor
+	inline					~LFHMAllocator();
+
+	/// Initialize the allocator
+	/// @param inObjectStoreSizeBytes Number of bytes to reserve for all key value pairs
+	inline void				Init(uint inObjectStoreSizeBytes);
+
+	/// Clear all allocations
+	inline void				Clear();
+
+	/// Allocate a new block of data
+	/// @param inBlockSize Size of block to allocate (will potentially return a smaller block if memory is full).
+	/// @param ioBegin Should be the start of the first free byte in current memory block on input, will contain the start of the first free byte in allocated block on return.
+	/// @param ioEnd Should be the byte beyond the current memory block on input, will contain the byte beyond the allocated block on return.
+	inline void				Allocate(uint32 inBlockSize, uint32 &ioBegin, uint32 &ioEnd);
+
+	/// Convert a pointer to an offset
+	template <class T>
+	inline uint32			ToOffset(const T *inData) const;
+
+	/// Convert an offset to a pointer
+	template <class T>
+	inline T *				FromOffset(uint32 inOffset) const;
+
+private:
+	uint8 *					mObjectStore = nullptr;			///< This contains a contiguous list of objects (possibly of varying size)
+	uint32					mObjectStoreSizeBytes = 0;		///< The size of mObjectStore in bytes
+	atomic<uint32>			mWriteOffset { 0 };				///< Next offset to write to in mObjectStore
+};
+
+/// Allocator context object for a lock free hash map that allocates a larger memory block at once and hands it out in smaller portions.
+/// This avoids contention on the atomic LFHMAllocator::mWriteOffset.
+class LFHMAllocatorContext : public NonCopyable
+{
+public:
+	/// Construct a new allocator context
+	inline					LFHMAllocatorContext(LFHMAllocator &inAllocator, uint32 inBlockSize);
+
+	/// @brief Allocate data block
+	/// @param inSize Size of block to allocate.
+	/// @param inAlignment Alignment of block to allocate.
+	/// @param outWriteOffset Offset in buffer where block is located
+	/// @return True if allocation succeeded
+	inline bool				Allocate(uint32 inSize, uint32 inAlignment, uint32 &outWriteOffset);
+
+private:
+	LFHMAllocator &			mAllocator;
+	uint32					mBlockSize;
+	uint32					mBegin = 0;
+	uint32					mEnd = 0;
+};
+
+/// Very simple lock free hash map that only allows insertion, retrieval and provides a fixed amount of buckets and fixed storage.
+/// Note: This class currently assumes key and value are simple types that need no calls to the destructor.
+template <class Key, class Value>
+class LockFreeHashMap : public NonCopyable
+{
+public:
+	using MapType = LockFreeHashMap<Key, Value>;
+
+	/// Destructor
+	explicit				LockFreeHashMap(LFHMAllocator &inAllocator) : mAllocator(inAllocator) { }
+							~LockFreeHashMap();
+
+	/// Initialization
+	/// @param inMaxBuckets Max amount of buckets to use in the hashmap. Must be power of 2.
+	void					Init(uint32 inMaxBuckets);
+
+	/// Remove all elements.
+	/// Note that this cannot happen simultaneously with adding new elements.
+	void					Clear();
+
+	/// Get the current amount of buckets that the map is using
+	uint32					GetNumBuckets() const			{ return mNumBuckets; }
+
+	/// Get the maximum amount of buckets that this map supports
+	uint32					GetMaxBuckets() const			{ return mMaxBuckets; }
+
+	/// Update the number of buckets. This must be done after clearing the map and cannot be done concurrently with any other operations on the map.
+	/// Note that the number of buckets can never become bigger than the specified max buckets during initialization and that it must be a power of 2.
+	void					SetNumBuckets(uint32 inNumBuckets);
+
+	/// A key / value pair that is inserted in the map
+	class KeyValue
+	{
+	public:
+		const Key &			GetKey() const					{ return mKey; }
+		Value &				GetValue()						{ return mValue; }
+		const Value &		GetValue() const				{ return mValue; }
+
+	private:
+		template <class K, class V> friend class LockFreeHashMap;
+
+		Key					mKey;							///< Key for this entry
+		uint32				mNextOffset;					///< Offset in mObjectStore of next KeyValue entry with same hash
+		Value				mValue;							///< Value for this entry + optionally extra bytes
+	};
+
+	/// Insert a new element, returns null if map full.
+	/// Multiple threads can be inserting in the map at the same time.
+	template <class... Params>
+	inline KeyValue *		Create(LFHMAllocatorContext &ioContext, const Key &inKey, uint64 inKeyHash, int inExtraBytes, Params &&... inConstructorParams);
+
+	/// Find an element, returns null if not found
+	inline const KeyValue *	Find(const Key &inKey, uint64 inKeyHash) const;
+
+	/// Value of an invalid handle
+	const static uint32		cInvalidHandle = uint32(-1);
+
+	/// Get convert key value pair to uint32 handle
+	inline uint32			ToHandle(const KeyValue *inKeyValue) const;
+
+	/// Convert uint32 handle back to key and value
+	inline const KeyValue *	FromHandle(uint32 inHandle) const;
+
+#ifdef JPH_ENABLE_ASSERTS
+	/// Get the number of key value pairs that this map currently contains.
+	/// Available only when asserts are enabled because adding elements creates contention on this atomic and negatively affects performance.
+	inline uint32			GetNumKeyValues() const			{ return mNumKeyValues; }
+#endif // JPH_ENABLE_ASSERTS
+
+	/// Get all key/value pairs
+	inline void				GetAllKeyValues(Array<const KeyValue *> &outAll) const;
+
+	/// Non-const iterator
+	struct Iterator
+	{
+		/// Comparison
+		bool				operator == (const Iterator &inRHS) const	{ return mMap == inRHS.mMap && mBucket == inRHS.mBucket && mOffset == inRHS.mOffset; }
+		bool				operator != (const Iterator &inRHS) const	{ return !(*this == inRHS); }
+
+		/// Convert to key value pair
+		KeyValue &			operator * ();
+
+		/// Next item
+		Iterator &			operator ++ ();
+
+		MapType *			mMap;
+		uint32				mBucket;
+		uint32				mOffset;
+	};
+
+	/// Iterate over the map, note that it is not safe to do this in parallel to Clear().
+	/// It is safe to do this while adding elements to the map, but newly added elements may or may not be returned by the iterator.
+	Iterator				begin();
+	Iterator				end();
+
+#ifdef JPH_DEBUG
+	/// Output stats about this map to the log
+	void					TraceStats() const;
+#endif
+
+private:
+	LFHMAllocator &			mAllocator;						///< Allocator used to allocate key value pairs
+
+#ifdef JPH_ENABLE_ASSERTS
+	atomic<uint32>			mNumKeyValues = 0;				///< Number of key value pairs in the store
+#endif // JPH_ENABLE_ASSERTS
+
+	atomic<uint32> *		mBuckets = nullptr;				///< This contains the offset in mObjectStore of the first object with a particular hash
+	uint32					mNumBuckets = 0;				///< Current number of buckets
+	uint32					mMaxBuckets = 0;				///< Maximum number of buckets
+};
+
+JPH_NAMESPACE_END
+
+#include "LockFreeHashMap.inl"
diff --git a/thirdparty/jolt_physics/Jolt/Core/LockFreeHashMap.inl b/thirdparty/jolt_physics/Jolt/Core/LockFreeHashMap.inl
new file mode 100644
index 0000000000..aa4b875e9c
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/LockFreeHashMap.inl
@@ -0,0 +1,351 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+///////////////////////////////////////////////////////////////////////////////////
+// LFHMAllocator
+///////////////////////////////////////////////////////////////////////////////////
+
+inline LFHMAllocator::~LFHMAllocator()
+{
+	AlignedFree(mObjectStore);
+}
+
+inline void LFHMAllocator::Init(uint inObjectStoreSizeBytes)
+{
+	JPH_ASSERT(mObjectStore == nullptr);
+
+	mObjectStoreSizeBytes = inObjectStoreSizeBytes;
+	mObjectStore = reinterpret_cast<uint8 *>(JPH::AlignedAllocate(inObjectStoreSizeBytes, 16));
+}
+
+inline void LFHMAllocator::Clear()
+{
+	mWriteOffset = 0;
+}
+
+inline void LFHMAllocator::Allocate(uint32 inBlockSize, uint32 &ioBegin, uint32 &ioEnd)
+{
+	// If we're already beyond the end of our buffer then don't do an atomic add.
+	// It's possible that many keys are inserted after the allocator is full, making it possible
+	// for mWriteOffset (uint32) to wrap around to zero. When this happens, there will be a memory corruption.
+	// This way, we will be able to progress the write offset beyond the size of the buffer
+	// worst case by max <CPU count> * inBlockSize.
+	if (mWriteOffset.load(memory_order_relaxed) >= mObjectStoreSizeBytes)
+		return;
+
+	// Atomically fetch a block from the pool
+	uint32 begin = mWriteOffset.fetch_add(inBlockSize, memory_order_relaxed);
+	uint32 end = min(begin + inBlockSize, mObjectStoreSizeBytes);
+
+	if (ioEnd == begin)
+	{
+		// Block is allocated straight after our previous block
+		begin = ioBegin;
+	}
+	else
+	{
+		// Block is a new block
+		begin = min(begin, mObjectStoreSizeBytes);
+	}
+
+	// Store the begin and end of the resulting block
+	ioBegin = begin;
+	ioEnd = end;
+}
+
+template <class T>
+inline uint32 LFHMAllocator::ToOffset(const T *inData) const
+{
+	const uint8 *data = reinterpret_cast<const uint8 *>(inData);
+	JPH_ASSERT(data >= mObjectStore && data < mObjectStore + mObjectStoreSizeBytes);
+	return uint32(data - mObjectStore);
+}
+
+template <class T>
+inline T *LFHMAllocator::FromOffset(uint32 inOffset) const
+{
+	JPH_ASSERT(inOffset < mObjectStoreSizeBytes);
+	return reinterpret_cast<T *>(mObjectStore + inOffset);
+}
+
+///////////////////////////////////////////////////////////////////////////////////
+// LFHMAllocatorContext
+///////////////////////////////////////////////////////////////////////////////////
+
+inline LFHMAllocatorContext::LFHMAllocatorContext(LFHMAllocator &inAllocator, uint32 inBlockSize) :
+	mAllocator(inAllocator),
+	mBlockSize(inBlockSize)
+{
+}
+
+inline bool LFHMAllocatorContext::Allocate(uint32 inSize, uint32 inAlignment, uint32 &outWriteOffset)
+{
+	// Calculate needed bytes for alignment
+	JPH_ASSERT(IsPowerOf2(inAlignment));
+	uint32 alignment_mask = inAlignment - 1;
+	uint32 alignment = (inAlignment - (mBegin & alignment_mask)) & alignment_mask;
+
+	// Check if we have space
+	if (mEnd - mBegin < inSize + alignment)
+	{
+		// Allocate a new block
+		mAllocator.Allocate(mBlockSize, mBegin, mEnd);
+
+		// Update alignment
+		alignment = (inAlignment - (mBegin & alignment_mask)) & alignment_mask;
+
+		// Check if we have space again
+		if (mEnd - mBegin < inSize + alignment)
+			return false;
+	}
+
+	// Make the allocation
+	mBegin += alignment;
+	outWriteOffset = mBegin;
+	mBegin += inSize;
+	return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////
+// LockFreeHashMap
+///////////////////////////////////////////////////////////////////////////////////
+
+template <class Key, class Value>
+void LockFreeHashMap<Key, Value>::Init(uint32 inMaxBuckets)
+{
+	JPH_ASSERT(inMaxBuckets >= 4 && IsPowerOf2(inMaxBuckets));
+	JPH_ASSERT(mBuckets == nullptr);
+
+	mNumBuckets = inMaxBuckets;
+	mMaxBuckets = inMaxBuckets;
+
+	mBuckets = reinterpret_cast<atomic<uint32> *>(AlignedAllocate(inMaxBuckets * sizeof(atomic<uint32>), 16));
+
+	Clear();
+}
+
+template <class Key, class Value>
+LockFreeHashMap<Key, Value>::~LockFreeHashMap()
+{
+	AlignedFree(mBuckets);
+}
+
+template <class Key, class Value>
+void LockFreeHashMap<Key, Value>::Clear()
+{
+#ifdef JPH_ENABLE_ASSERTS
+	// Reset number of key value pairs
+	mNumKeyValues = 0;
+#endif // JPH_ENABLE_ASSERTS
+
+	// Reset buckets 4 at a time
+	static_assert(sizeof(atomic<uint32>) == sizeof(uint32));
+	UVec4 invalid_handle = UVec4::sReplicate(cInvalidHandle);
+	uint32 *start = reinterpret_cast<uint32 *>(mBuckets);
+	const uint32 *end = start + mNumBuckets;
+	JPH_ASSERT(IsAligned(start, 16));
+	while (start < end)
+	{
+		invalid_handle.StoreInt4Aligned(start);
+		start += 4;
+	}
+}
+
+template <class Key, class Value>
+void LockFreeHashMap<Key, Value>::SetNumBuckets(uint32 inNumBuckets)
+{
+	JPH_ASSERT(mNumKeyValues == 0);
+	JPH_ASSERT(inNumBuckets <= mMaxBuckets);
+	JPH_ASSERT(inNumBuckets >= 4 && IsPowerOf2(inNumBuckets));
+
+	mNumBuckets = inNumBuckets;
+}
+
+template <class Key, class Value>
+template <class... Params>
+inline typename LockFreeHashMap<Key, Value>::KeyValue *LockFreeHashMap<Key, Value>::Create(LFHMAllocatorContext &ioContext, const Key &inKey, uint64 inKeyHash, int inExtraBytes, Params &&... inConstructorParams)
+{
+	// This is not a multi map, test the key hasn't been inserted yet
+	JPH_ASSERT(Find(inKey, inKeyHash) == nullptr);
+
+	// Calculate total size
+	uint size = sizeof(KeyValue) + inExtraBytes;
+
+	// Get the write offset for this key value pair
+	uint32 write_offset;
+	if (!ioContext.Allocate(size, alignof(KeyValue), write_offset))
+		return nullptr;
+
+#ifdef JPH_ENABLE_ASSERTS
+	// Increment amount of entries in map
+	mNumKeyValues.fetch_add(1, memory_order_relaxed);
+#endif // JPH_ENABLE_ASSERTS
+
+	// Construct the key/value pair
+	KeyValue *kv = mAllocator.template FromOffset<KeyValue>(write_offset);
+	JPH_ASSERT(intptr_t(kv) % alignof(KeyValue) == 0);
+#ifdef JPH_DEBUG
+	memset(kv, 0xcd, size);
+#endif
+	kv->mKey = inKey;
+	new (&kv->mValue) Value(std::forward<Params>(inConstructorParams)...);
+
+	// Get the offset to the first object from the bucket with corresponding hash
+	atomic<uint32> &offset = mBuckets[inKeyHash & (mNumBuckets - 1)];
+
+	// Add this entry as the first element in the linked list
+	uint32 old_offset = offset.load(memory_order_relaxed);
+	for (;;)
+	{
+		kv->mNextOffset = old_offset;
+		if (offset.compare_exchange_weak(old_offset, write_offset, memory_order_release))
+			break;
+	}
+
+	return kv;
+}
+
+template <class Key, class Value>
+inline const typename LockFreeHashMap<Key, Value>::KeyValue *LockFreeHashMap<Key, Value>::Find(const Key &inKey, uint64 inKeyHash) const
+{
+	// Get the offset to the keyvalue object from the bucket with corresponding hash
+	uint32 offset = mBuckets[inKeyHash & (mNumBuckets - 1)].load(memory_order_acquire);
+	while (offset != cInvalidHandle)
+	{
+		// Loop through linked list of values until the right one is found
+		const KeyValue *kv = mAllocator.template FromOffset<const KeyValue>(offset);
+		if (kv->mKey == inKey)
+			return kv;
+		offset = kv->mNextOffset;
+	}
+
+	// Not found
+	return nullptr;
+}
+
+template <class Key, class Value>
+inline uint32 LockFreeHashMap<Key, Value>::ToHandle(const KeyValue *inKeyValue) const
+{
+	return mAllocator.ToOffset(inKeyValue);
+}
+
+template <class Key, class Value>
+inline const typename LockFreeHashMap<Key, Value>::KeyValue *LockFreeHashMap<Key, Value>::FromHandle(uint32 inHandle) const
+{
+	return mAllocator.template FromOffset<const KeyValue>(inHandle);
+}
+
+template <class Key, class Value>
+inline void LockFreeHashMap<Key, Value>::GetAllKeyValues(Array<const KeyValue *> &outAll) const
+{
+	for (const atomic<uint32> *bucket = mBuckets; bucket < mBuckets + mNumBuckets; ++bucket)
+	{
+		uint32 offset = *bucket;
+		while (offset != cInvalidHandle)
+		{
+			const KeyValue *kv = mAllocator.template FromOffset<const KeyValue>(offset);
+			outAll.push_back(kv);
+			offset = kv->mNextOffset;
+		}
+	}
+}
+
+template <class Key, class Value>
+typename LockFreeHashMap<Key, Value>::Iterator LockFreeHashMap<Key, Value>::begin()
+{
+	// Start with the first bucket
+	Iterator it { this, 0, mBuckets[0] };
+
+	// If it doesn't contain a valid entry, use the ++ operator to find the first valid entry
+	if (it.mOffset == cInvalidHandle)
+		++it;
+
+	return it;
+}
+
+template <class Key, class Value>
+typename LockFreeHashMap<Key, Value>::Iterator LockFreeHashMap<Key, Value>::end()
+{
+	return { this, mNumBuckets, cInvalidHandle };
+}
+
+template <class Key, class Value>
+typename LockFreeHashMap<Key, Value>::KeyValue &LockFreeHashMap<Key, Value>::Iterator::operator* ()
+{
+	JPH_ASSERT(mOffset != cInvalidHandle);
+
+	return *mMap->mAllocator.template FromOffset<KeyValue>(mOffset);
+}
+
+template <class Key, class Value>
+typename LockFreeHashMap<Key, Value>::Iterator &LockFreeHashMap<Key, Value>::Iterator::operator++ ()
+{
+	JPH_ASSERT(mBucket < mMap->mNumBuckets);
+
+	// Find the next key value in this bucket
+	if (mOffset != cInvalidHandle)
+	{
+		const KeyValue *kv = mMap->mAllocator.template FromOffset<const KeyValue>(mOffset);
+		mOffset = kv->mNextOffset;
+		if (mOffset != cInvalidHandle)
+			return *this;
+	}
+
+	// Loop over next buckets
+	for (;;)
+	{
+		// Next bucket
+		++mBucket;
+		if (mBucket >= mMap->mNumBuckets)
+			return *this;
+
+		// Fetch the first entry in the bucket
+		mOffset = mMap->mBuckets[mBucket];
+		if (mOffset != cInvalidHandle)
+			return *this;
+	}
+}
+
+#ifdef JPH_DEBUG
+
+template <class Key, class Value>
+void LockFreeHashMap<Key, Value>::TraceStats() const
+{
+	const int cMaxPerBucket = 256;
+
+	int max_objects_per_bucket = 0;
+	int num_objects = 0;
+	int histogram[cMaxPerBucket];
+	for (int i = 0; i < cMaxPerBucket; ++i)
+		histogram[i] = 0;
+
+	for (atomic<uint32> *bucket = mBuckets, *bucket_end = mBuckets + mNumBuckets; bucket < bucket_end; ++bucket)
+	{
+		int objects_in_bucket = 0;
+		uint32 offset = *bucket;
+		while (offset != cInvalidHandle)
+		{
+			const KeyValue *kv = mAllocator.template FromOffset<const KeyValue>(offset);
+			offset = kv->mNextOffset;
+			++objects_in_bucket;
+			++num_objects;
+		}
+		max_objects_per_bucket = max(objects_in_bucket, max_objects_per_bucket);
+		histogram[min(objects_in_bucket, cMaxPerBucket - 1)]++;
+	}
+
+	Trace("max_objects_per_bucket = %d, num_buckets = %u, num_objects = %d", max_objects_per_bucket, mNumBuckets, num_objects);
+
+	for (int i = 0; i < cMaxPerBucket; ++i)
+		if (histogram[i] != 0)
+			Trace("%d: %d", i, histogram[i]);
+}
+
+#endif
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/Memory.cpp b/thirdparty/jolt_physics/Jolt/Core/Memory.cpp
new file mode 100644
index 0000000000..6e642c4ea8
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/Memory.cpp
@@ -0,0 +1,85 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+JPH_SUPPRESS_WARNINGS_STD_BEGIN
+#include <cstdlib>
+JPH_SUPPRESS_WARNINGS_STD_END
+#include <stdlib.h>
+
+JPH_NAMESPACE_BEGIN
+
+#ifdef JPH_DISABLE_CUSTOM_ALLOCATOR
+	#define JPH_ALLOC_FN(x)	x
+	#define JPH_ALLOC_SCOPE
+#else
+	#define JPH_ALLOC_FN(x)	x##Impl
+	#define JPH_ALLOC_SCOPE static
+#endif
+
+JPH_ALLOC_SCOPE void *JPH_ALLOC_FN(Allocate)(size_t inSize)
+{
+	JPH_ASSERT(inSize > 0);
+	return malloc(inSize);
+}
+
+JPH_ALLOC_SCOPE void *JPH_ALLOC_FN(Reallocate)(void *inBlock, [[maybe_unused]] size_t inOldSize, size_t inNewSize)
+{
+	JPH_ASSERT(inNewSize > 0);
+	return realloc(inBlock, inNewSize);
+}
+
+JPH_ALLOC_SCOPE void JPH_ALLOC_FN(Free)(void *inBlock)
+{
+	free(inBlock);
+}
+
+JPH_ALLOC_SCOPE void *JPH_ALLOC_FN(AlignedAllocate)(size_t inSize, size_t inAlignment)
+{
+	JPH_ASSERT(inSize > 0 && inAlignment > 0);
+
+#if defined(JPH_PLATFORM_WINDOWS)
+	// Microsoft doesn't implement posix_memalign
+	return _aligned_malloc(inSize, inAlignment);
+#else
+	void *block = nullptr;
+	JPH_SUPPRESS_WARNING_PUSH
+	JPH_GCC_SUPPRESS_WARNING("-Wunused-result")
+	JPH_CLANG_SUPPRESS_WARNING("-Wunused-result")
+	posix_memalign(&block, inAlignment, inSize);
+	JPH_SUPPRESS_WARNING_POP
+	return block;
+#endif
+}
+
+JPH_ALLOC_SCOPE void JPH_ALLOC_FN(AlignedFree)(void *inBlock)
+{
+#if defined(JPH_PLATFORM_WINDOWS)
+	_aligned_free(inBlock);
+#else
+	free(inBlock);
+#endif
+}
+
+#ifndef JPH_DISABLE_CUSTOM_ALLOCATOR
+
+AllocateFunction Allocate = nullptr;
+ReallocateFunction Reallocate = nullptr;
+FreeFunction Free = nullptr;
+AlignedAllocateFunction AlignedAllocate = nullptr;
+AlignedFreeFunction AlignedFree = nullptr;
+
+void RegisterDefaultAllocator()
+{
+	Allocate = AllocateImpl;
+	Reallocate = ReallocateImpl;
+	Free = FreeImpl;
+	AlignedAllocate = AlignedAllocateImpl;
+	AlignedFree = AlignedFreeImpl;
+}
+
+#endif // JPH_DISABLE_CUSTOM_ALLOCATOR
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/Memory.h b/thirdparty/jolt_physics/Jolt/Core/Memory.h
new file mode 100644
index 0000000000..d5b7c6a308
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/Memory.h
@@ -0,0 +1,58 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+#ifndef JPH_DISABLE_CUSTOM_ALLOCATOR
+
+// Normal memory allocation, must be at least 8 byte aligned on 32 bit platform and 16 byte aligned on 64 bit platform
+using AllocateFunction = void *(*)(size_t inSize);
+using ReallocateFunction = void *(*)(void *inBlock, size_t inOldSize, size_t inNewSize);
+using FreeFunction = void (*)(void *inBlock);
+
+// Aligned memory allocation
+using AlignedAllocateFunction = void *(*)(size_t inSize, size_t inAlignment);
+using AlignedFreeFunction = void (*)(void *inBlock);
+
+// User defined allocation / free functions
+JPH_EXPORT extern AllocateFunction Allocate;
+JPH_EXPORT extern ReallocateFunction Reallocate;
+JPH_EXPORT extern FreeFunction Free;
+JPH_EXPORT extern AlignedAllocateFunction AlignedAllocate;
+JPH_EXPORT extern AlignedFreeFunction AlignedFree;
+
+/// Register platform default allocation / free functions
+JPH_EXPORT void RegisterDefaultAllocator();
+
+/// Macro to override the new and delete functions
+#define JPH_OVERRIDE_NEW_DELETE \
+	JPH_INLINE void *operator new (size_t inCount)												{ return JPH::Allocate(inCount); } \
+	JPH_INLINE void operator delete (void *inPointer) noexcept									{ JPH::Free(inPointer); } \
+	JPH_INLINE void *operator new[] (size_t inCount)											{ return JPH::Allocate(inCount); } \
+	JPH_INLINE void operator delete[] (void *inPointer) noexcept								{ JPH::Free(inPointer); } \
+	JPH_INLINE void *operator new (size_t inCount, std::align_val_t inAlignment)				{ return JPH::AlignedAllocate(inCount, static_cast<size_t>(inAlignment)); } \
+	JPH_INLINE void operator delete (void *inPointer, [[maybe_unused]] std::align_val_t inAlignment) noexcept	{ JPH::AlignedFree(inPointer); } \
+	JPH_INLINE void *operator new[] (size_t inCount, std::align_val_t inAlignment)				{ return JPH::AlignedAllocate(inCount, static_cast<size_t>(inAlignment)); } \
+	JPH_INLINE void operator delete[] (void *inPointer, [[maybe_unused]] std::align_val_t inAlignment) noexcept	{ JPH::AlignedFree(inPointer); }
+
+#else
+
+// Directly define the allocation functions
+JPH_EXPORT void *Allocate(size_t inSize);
+JPH_EXPORT void *Reallocate(void *inBlock, size_t inOldSize, size_t inNewSize);
+JPH_EXPORT void Free(void *inBlock);
+JPH_EXPORT void *AlignedAllocate(size_t inSize, size_t inAlignment);
+JPH_EXPORT void AlignedFree(void *inBlock);
+
+// Don't implement allocator registering
+inline void RegisterDefaultAllocator() { }
+
+// Don't override new/delete
+#define JPH_OVERRIDE_NEW_DELETE
+
+#endif // !JPH_DISABLE_CUSTOM_ALLOCATOR
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/Mutex.h b/thirdparty/jolt_physics/Jolt/Core/Mutex.h
new file mode 100644
index 0000000000..6969aa45d0
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/Mutex.h
@@ -0,0 +1,223 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/Profiler.h>
+#include <Jolt/Core/NonCopyable.h>
+
+JPH_SUPPRESS_WARNINGS_STD_BEGIN
+#include <mutex>
+#include <shared_mutex>
+#include <thread>
+JPH_SUPPRESS_WARNINGS_STD_END
+
+JPH_NAMESPACE_BEGIN
+
+// Things we're using from STL
+using std::mutex;
+using std::shared_mutex;
+using std::thread;
+using std::lock_guard;
+using std::shared_lock;
+using std::unique_lock;
+
+#ifdef JPH_PLATFORM_BLUE
+
+// On Platform Blue the mutex class is not very fast so we implement it using the official APIs
+class MutexBase : public NonCopyable
+{
+public:
+					MutexBase()
+	{
+		JPH_PLATFORM_BLUE_MUTEX_INIT(mMutex);
+	}
+
+					~MutexBase()
+	{
+		JPH_PLATFORM_BLUE_MUTEX_DESTROY(mMutex);
+	}
+
+	inline bool		try_lock()
+	{
+		return JPH_PLATFORM_BLUE_MUTEX_TRYLOCK(mMutex);
+	}
+
+	inline void		lock()
+	{
+		JPH_PLATFORM_BLUE_MUTEX_LOCK(mMutex);
+	}
+
+	inline void		unlock()
+	{
+		JPH_PLATFORM_BLUE_MUTEX_UNLOCK(mMutex);
+	}
+
+private:
+	JPH_PLATFORM_BLUE_MUTEX		mMutex;
+};
+
+// On Platform Blue the shared_mutex class is not very fast so we implement it using the official APIs
+class SharedMutexBase : public NonCopyable
+{
+public:
+					SharedMutexBase()
+	{
+		JPH_PLATFORM_BLUE_RWLOCK_INIT(mRWLock);
+	}
+
+					~SharedMutexBase()
+	{
+		JPH_PLATFORM_BLUE_RWLOCK_DESTROY(mRWLock);
+	}
+
+	inline bool		try_lock()
+	{
+		return JPH_PLATFORM_BLUE_RWLOCK_TRYWLOCK(mRWLock);
+	}
+
+	inline bool		try_lock_shared()
+	{
+		return JPH_PLATFORM_BLUE_RWLOCK_TRYRLOCK(mRWLock);
+	}
+
+	inline void		lock()
+	{
+		JPH_PLATFORM_BLUE_RWLOCK_WLOCK(mRWLock);
+	}
+
+	inline void		unlock()
+	{
+		JPH_PLATFORM_BLUE_RWLOCK_WUNLOCK(mRWLock);
+	}
+
+	inline void		lock_shared()
+	{
+		JPH_PLATFORM_BLUE_RWLOCK_RLOCK(mRWLock);
+	}
+
+	inline void		unlock_shared()
+	{
+		JPH_PLATFORM_BLUE_RWLOCK_RUNLOCK(mRWLock);
+	}
+
+private:
+	JPH_PLATFORM_BLUE_RWLOCK	mRWLock;
+};
+
+#else
+
+// On other platforms just use the STL implementation
+using MutexBase = mutex;
+using SharedMutexBase = shared_mutex;
+
+#endif // JPH_PLATFORM_BLUE
+
+#if defined(JPH_ENABLE_ASSERTS) || defined(JPH_PROFILE_ENABLED) || defined(JPH_EXTERNAL_PROFILE)
+
+/// Very simple wrapper around MutexBase which tracks lock contention in the profiler
+/// and asserts that locks/unlocks take place on the same thread
+class Mutex : public MutexBase
+{
+public:
+	inline bool		try_lock()
+	{
+		JPH_ASSERT(mLockedThreadID != std::this_thread::get_id());
+		if (MutexBase::try_lock())
+		{
+			JPH_IF_ENABLE_ASSERTS(mLockedThreadID = std::this_thread::get_id();)
+			return true;
+		}
+		return false;
+	}
+
+	inline void		lock()
+	{
+		if (!try_lock())
+		{
+			JPH_PROFILE("Lock", 0xff00ffff);
+			MutexBase::lock();
+			JPH_IF_ENABLE_ASSERTS(mLockedThreadID = std::this_thread::get_id();)
+		}
+	}
+
+	inline void		unlock()
+	{
+		JPH_ASSERT(mLockedThreadID == std::this_thread::get_id());
+		JPH_IF_ENABLE_ASSERTS(mLockedThreadID = thread::id();)
+		MutexBase::unlock();
+	}
+
+#ifdef JPH_ENABLE_ASSERTS
+	inline bool		is_locked()
+	{
+		return mLockedThreadID != thread::id();
+	}
+#endif // JPH_ENABLE_ASSERTS
+
+private:
+	JPH_IF_ENABLE_ASSERTS(thread::id mLockedThreadID;)
+};
+
+/// Very simple wrapper around SharedMutexBase which tracks lock contention in the profiler
+/// and asserts that locks/unlocks take place on the same thread
+class SharedMutex : public SharedMutexBase
+{
+public:
+	inline bool		try_lock()
+	{
+		JPH_ASSERT(mLockedThreadID != std::this_thread::get_id());
+		if (SharedMutexBase::try_lock())
+		{
+			JPH_IF_ENABLE_ASSERTS(mLockedThreadID = std::this_thread::get_id();)
+			return true;
+		}
+		return false;
+	}
+
+	inline void		lock()
+	{
+		if (!try_lock())
+		{
+			JPH_PROFILE("WLock", 0xff00ffff);
+			SharedMutexBase::lock();
+			JPH_IF_ENABLE_ASSERTS(mLockedThreadID = std::this_thread::get_id();)
+		}
+	}
+
+	inline void		unlock()
+	{
+		JPH_ASSERT(mLockedThreadID == std::this_thread::get_id());
+		JPH_IF_ENABLE_ASSERTS(mLockedThreadID = thread::id();)
+		SharedMutexBase::unlock();
+	}
+
+#ifdef JPH_ENABLE_ASSERTS
+	inline bool		is_locked()
+	{
+		return mLockedThreadID != thread::id();
+	}
+#endif // JPH_ENABLE_ASSERTS
+
+	inline void		lock_shared()
+	{
+		if (!try_lock_shared())
+		{
+			JPH_PROFILE("RLock", 0xff00ffff);
+			SharedMutexBase::lock_shared();
+		}
+	}
+
+private:
+	JPH_IF_ENABLE_ASSERTS(thread::id mLockedThreadID;)
+};
+
+#else
+
+using Mutex = MutexBase;
+using SharedMutex = SharedMutexBase;
+
+#endif
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/MutexArray.h b/thirdparty/jolt_physics/Jolt/Core/MutexArray.h
new file mode 100644
index 0000000000..3a0b5586ae
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/MutexArray.h
@@ -0,0 +1,98 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/NonCopyable.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// A mutex array protects a number of resources with a limited amount of mutexes.
+/// It uses hashing to find the mutex of a particular object.
+/// The idea is that if the amount of threads is much smaller than the amount of mutexes
+/// that there is a relatively small chance that two different objects map to the same mutex.
+template <class MutexType>
+class MutexArray : public NonCopyable
+{
+public:
+	/// Constructor, constructs an empty mutex array that you need to initialize with Init()
+							MutexArray() = default;
+
+	/// Constructor, constructs an array with inNumMutexes entries
+	explicit				MutexArray(uint inNumMutexes) { Init(inNumMutexes); }
+
+	/// Destructor
+							~MutexArray() { delete [] mMutexStorage; }
+
+	/// Initialization
+	/// @param inNumMutexes The amount of mutexes to allocate
+	void					Init(uint inNumMutexes)
+	{
+		JPH_ASSERT(mMutexStorage == nullptr);
+		JPH_ASSERT(inNumMutexes > 0 && IsPowerOf2(inNumMutexes));
+
+		mMutexStorage = new MutexStorage[inNumMutexes];
+		mNumMutexes = inNumMutexes;
+	}
+
+	/// Get the number of mutexes that were allocated
+	inline uint				GetNumMutexes() const
+	{
+		return mNumMutexes;
+	}
+
+	/// Convert an object index to a mutex index
+	inline uint32			GetMutexIndex(uint32 inObjectIndex) const
+	{
+		Hash<uint32> hasher;
+		return hasher(inObjectIndex) & (mNumMutexes - 1);
+	}
+
+	/// Get the mutex belonging to a certain object by index
+	inline MutexType &		GetMutexByObjectIndex(uint32 inObjectIndex)
+	{
+		return mMutexStorage[GetMutexIndex(inObjectIndex)].mMutex;
+	}
+
+	/// Get a mutex by index in the array
+	inline MutexType &		GetMutexByIndex(uint32 inMutexIndex)
+	{
+		return mMutexStorage[inMutexIndex].mMutex;
+	}
+
+	/// Lock all mutexes
+	void					LockAll()
+	{
+		JPH_PROFILE_FUNCTION();
+
+		MutexStorage *end = mMutexStorage + mNumMutexes;
+		for (MutexStorage *m = mMutexStorage; m < end; ++m)
+			m->mMutex.lock();
+	}
+
+	/// Unlock all mutexes
+	void					UnlockAll()
+	{
+		JPH_PROFILE_FUNCTION();
+
+		MutexStorage *end = mMutexStorage + mNumMutexes;
+		for (MutexStorage *m = mMutexStorage; m < end; ++m)
+			m->mMutex.unlock();
+	}
+
+private:
+	/// Align the mutex to a cache line to ensure there is no false sharing (this is platform dependent, we do this to be safe)
+	struct alignas(JPH_CACHE_LINE_SIZE) MutexStorage
+	{
+		JPH_OVERRIDE_NEW_DELETE
+
+		MutexType			mMutex;
+	};
+
+	MutexStorage *			mMutexStorage = nullptr;
+	uint					mNumMutexes = 0;
+};
+
+JPH_NAMESPACE_END
+
diff --git a/thirdparty/jolt_physics/Jolt/Core/NonCopyable.h b/thirdparty/jolt_physics/Jolt/Core/NonCopyable.h
new file mode 100644
index 0000000000..18b431e925
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/NonCopyable.h
@@ -0,0 +1,18 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that makes another class non-copyable. Usage: Inherit from NonCopyable.
+class JPH_EXPORT NonCopyable
+{
+public:
+			NonCopyable() = default;
+			NonCopyable(const NonCopyable &) = delete;
+	void	operator = (const NonCopyable &) = delete;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/Profiler.cpp b/thirdparty/jolt_physics/Jolt/Core/Profiler.cpp
new file mode 100644
index 0000000000..39152b10b4
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/Profiler.cpp
@@ -0,0 +1,354 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Core/Profiler.h>
+#include <Jolt/Core/Color.h>
+#include <Jolt/Core/StringTools.h>
+#include <Jolt/Core/QuickSort.h>
+
+JPH_SUPPRESS_WARNINGS_STD_BEGIN
+#include <fstream>
+JPH_SUPPRESS_WARNINGS_STD_END
+
+JPH_NAMESPACE_BEGIN
+
+#if defined(JPH_EXTERNAL_PROFILE) && defined(JPH_SHARED_LIBRARY)
+
+ProfileStartMeasurementFunction ProfileStartMeasurement = [](const char *, uint32, uint8 *) { };
+ProfileEndMeasurementFunction ProfileEndMeasurement = [](uint8 *) { };
+
+#elif defined(JPH_PROFILE_ENABLED)
+
+//////////////////////////////////////////////////////////////////////////////////////////
+// Profiler
+//////////////////////////////////////////////////////////////////////////////////////////
+
+Profiler *Profiler::sInstance = nullptr;
+
+#ifdef JPH_SHARED_LIBRARY
+	static thread_local ProfileThread *sInstance = nullptr;
+
+	ProfileThread *ProfileThread::sGetInstance()
+	{
+		return sInstance;
+	}
+
+	void ProfileThread::sSetInstance(ProfileThread *inInstance)
+	{
+		sInstance = inInstance;
+	}
+#else
+	thread_local ProfileThread *ProfileThread::sInstance = nullptr;
+#endif
+
+bool ProfileMeasurement::sOutOfSamplesReported = false;
+
+void Profiler::UpdateReferenceTime()
+{
+	mReferenceTick = GetProcessorTickCount();
+	mReferenceTime = std::chrono::high_resolution_clock::now();
+}
+
+uint64 Profiler::GetProcessorTicksPerSecond() const
+{
+	uint64 ticks = GetProcessorTickCount();
+	std::chrono::high_resolution_clock::time_point time = std::chrono::high_resolution_clock::now();
+
+	return (ticks - mReferenceTick) * 1000000000ULL / std::chrono::duration_cast<std::chrono::nanoseconds>(time - mReferenceTime).count();
+}
+
+// This function assumes that none of the threads are active while we're dumping the profile,
+// otherwise there will be a race condition on mCurrentSample and the profile data.
+JPH_TSAN_NO_SANITIZE
+void Profiler::NextFrame()
+{
+	std::lock_guard lock(mLock);
+
+	if (mDump)
+	{
+		DumpInternal();
+		mDump = false;
+	}
+
+	for (ProfileThread *t : mThreads)
+		t->mCurrentSample = 0;
+
+	UpdateReferenceTime();
+}
+
+void Profiler::Dump(const string_view &inTag)
+{
+	mDump = true;
+	mDumpTag = inTag;
+}
+
+void Profiler::AddThread(ProfileThread *inThread)
+{
+	std::lock_guard lock(mLock);
+
+	mThreads.push_back(inThread);
+}
+
+void Profiler::RemoveThread(ProfileThread *inThread)
+{
+	std::lock_guard lock(mLock);
+
+	Array<ProfileThread *>::iterator i = std::find(mThreads.begin(), mThreads.end(), inThread);
+	JPH_ASSERT(i != mThreads.end());
+	mThreads.erase(i);
+}
+
+void Profiler::sAggregate(int inDepth, uint32 inColor, ProfileSample *&ioSample, const ProfileSample *inEnd, Aggregators &ioAggregators, KeyToAggregator &ioKeyToAggregator)
+{
+	// Store depth
+	ioSample->mDepth = uint8(min(255, inDepth));
+
+	// Update color
+	if (ioSample->mColor == 0)
+		ioSample->mColor = inColor;
+	else
+		inColor = ioSample->mColor;
+
+	// Start accumulating totals
+	uint64 cycles_this_with_children = ioSample->mEndCycle - ioSample->mStartCycle;
+
+	// Loop over following samples until we find a sample that starts on or after our end
+	ProfileSample *sample;
+	for (sample = ioSample + 1; sample < inEnd && sample->mStartCycle < ioSample->mEndCycle; ++sample)
+	{
+		JPH_ASSERT(sample[-1].mStartCycle <= sample->mStartCycle);
+		JPH_ASSERT(sample->mStartCycle >= ioSample->mStartCycle);
+		JPH_ASSERT(sample->mEndCycle <= ioSample->mEndCycle);
+
+		// Recurse and skip over the children of this child
+		sAggregate(inDepth + 1, inColor, sample, inEnd, ioAggregators, ioKeyToAggregator);
+	}
+
+	// Find the aggregator for this name / filename pair
+	Aggregator *aggregator;
+	KeyToAggregator::iterator aggregator_idx = ioKeyToAggregator.find(ioSample->mName);
+	if (aggregator_idx == ioKeyToAggregator.end())
+	{
+		// Not found, add to map and insert in array
+		ioKeyToAggregator.try_emplace(ioSample->mName, ioAggregators.size());
+		ioAggregators.emplace_back(ioSample->mName);
+		aggregator = &ioAggregators.back();
+	}
+	else
+	{
+		// Found
+		aggregator = &ioAggregators[aggregator_idx->second];
+	}
+
+	// Add the measurement to the aggregator
+	aggregator->AccumulateMeasurement(cycles_this_with_children);
+
+	// Update ioSample to the last child of ioSample
+	JPH_ASSERT(sample[-1].mStartCycle <= ioSample->mEndCycle);
+	JPH_ASSERT(sample >= inEnd || sample->mStartCycle >= ioSample->mEndCycle);
+	ioSample = sample - 1;
+}
+
+void Profiler::DumpInternal()
+{
+	// Freeze data from threads
+	// Note that this is not completely thread safe: As a profile sample is added mCurrentSample is incremented
+	// but the data is not written until the sample finishes. So if we dump the profile information while
+	// some other thread is running, we may get some garbage information from the previous frame
+	Threads threads;
+	for (ProfileThread *t : mThreads)
+		threads.push_back({ t->mThreadName, t->mSamples, t->mSamples + t->mCurrentSample });
+
+	// Shift all samples so that the first sample is at zero
+	uint64 min_cycle = 0xffffffffffffffffUL;
+	for (const ThreadSamples &t : threads)
+		if (t.mSamplesBegin < t.mSamplesEnd)
+			min_cycle = min(min_cycle, t.mSamplesBegin[0].mStartCycle);
+	for (const ThreadSamples &t : threads)
+		for (ProfileSample *s = t.mSamplesBegin, *end = t.mSamplesEnd; s < end; ++s)
+		{
+			s->mStartCycle -= min_cycle;
+			s->mEndCycle -= min_cycle;
+		}
+
+	// Determine tag of this profile
+	String tag;
+	if (mDumpTag.empty())
+	{
+		// Next sequence number
+		static int number = 0;
+		++number;
+		tag = ConvertToString(number);
+	}
+	else
+	{
+		// Take provided tag
+		tag = mDumpTag;
+		mDumpTag.clear();
+	}
+
+	// Aggregate data across threads
+	Aggregators aggregators;
+	KeyToAggregator key_to_aggregators;
+	for (const ThreadSamples &t : threads)
+		for (ProfileSample *s = t.mSamplesBegin, *end = t.mSamplesEnd; s < end; ++s)
+			sAggregate(0, Color::sGetDistinctColor(0).GetUInt32(), s, end, aggregators, key_to_aggregators);
+
+	// Dump as chart
+	DumpChart(tag.c_str(), threads, key_to_aggregators, aggregators);
+}
+
+static String sHTMLEncode(const char *inString)
+{
+	String str(inString);
+	StringReplace(str, "<", "&lt;");
+	StringReplace(str, ">", "&gt;");
+	return str;
+}
+
+void Profiler::DumpChart(const char *inTag, const Threads &inThreads, const KeyToAggregator &inKeyToAggregators, const Aggregators &inAggregators)
+{
+	// Open file
+	std::ofstream f;
+	f.open(StringFormat("profile_chart_%s.html", inTag).c_str(), std::ofstream::out | std::ofstream::trunc);
+	if (!f.is_open())
+		return;
+
+	// Write header
+	f << R"(<!DOCTYPE html>
+<html>
+	<head>
+		<title>Profile Chart</title>
+		<link rel="stylesheet" href="WebIncludes/profile_chart.css">
+		<script type="text/javascript" src="WebIncludes/profile_chart.js"></script>
+	</head>
+	<body onload="startChart();">
+	<script type="text/javascript">
+)";
+
+	// Get cycles per second
+	uint64 cycles_per_second = GetProcessorTicksPerSecond();
+	f << "var cycles_per_second = " << cycles_per_second << ";\n";
+
+	// Dump samples
+	f << "var threads = [\n";
+	bool first_thread = true;
+	for (const ThreadSamples &t : inThreads)
+	{
+		if (!first_thread)
+			f << ",\n";
+		first_thread = false;
+
+		f << "{\nthread_name: \"" << t.mThreadName << "\",\naggregator: [";
+		bool first = true;
+		for (const ProfileSample *s = t.mSamplesBegin, *end = t.mSamplesEnd; s < end; ++s)
+		{
+			if (!first)
+				f << ",";
+			first = false;
+			f << inKeyToAggregators.find(s->mName)->second;
+		}
+		f << "],\ncolor: [";
+		first = true;
+		for (const ProfileSample *s = t.mSamplesBegin, *end = t.mSamplesEnd; s < end; ++s)
+		{
+			if (!first)
+				f << ",";
+			first = false;
+			Color c(s->mColor);
+			f << StringFormat("\"#%02x%02x%02x\"", c.r, c.g, c.b);
+		}
+		f << "],\nstart: [";
+		first = true;
+		for (const ProfileSample *s = t.mSamplesBegin, *end = t.mSamplesEnd; s < end; ++s)
+		{
+			if (!first)
+				f << ",";
+			first = false;
+			f << s->mStartCycle;
+		}
+		f << "],\ncycles: [";
+		first = true;
+		for (const ProfileSample *s = t.mSamplesBegin, *end = t.mSamplesEnd; s < end; ++s)
+		{
+			if (!first)
+				f << ",";
+			first = false;
+			f << s->mEndCycle - s->mStartCycle;
+		}
+		f << "],\ndepth: [";
+		first = true;
+		for (const ProfileSample *s = t.mSamplesBegin, *end = t.mSamplesEnd; s < end; ++s)
+		{
+			if (!first)
+				f << ",";
+			first = false;
+			f << int(s->mDepth);
+		}
+		f << "]\n}";
+	}
+
+	// Dump aggregated data
+	f << "];\nvar aggregated = {\nname: [";
+	bool first = true;
+	for (const Aggregator &a : inAggregators)
+	{
+		if (!first)
+			f << ",";
+		first = false;
+		String name = "\"" + sHTMLEncode(a.mName) + "\"";
+		f << name;
+	}
+	f << "],\ncalls: [";
+	first = true;
+	for (const Aggregator &a : inAggregators)
+	{
+		if (!first)
+			f << ",";
+		first = false;
+		f << a.mCallCounter;
+	}
+	f << "],\nmin_cycles: [";
+	first = true;
+	for (const Aggregator &a : inAggregators)
+	{
+		if (!first)
+			f << ",";
+		first = false;
+		f << a.mMinCyclesInCallWithChildren;
+	}
+	f << "],\nmax_cycles: [";
+	first = true;
+	for (const Aggregator &a : inAggregators)
+	{
+		if (!first)
+			f << ",";
+		first = false;
+		f << a.mMaxCyclesInCallWithChildren;
+	}
+	f << "],\ncycles_per_frame: [";
+	first = true;
+	for (const Aggregator &a : inAggregators)
+	{
+		if (!first)
+			f << ",";
+		first = false;
+		f << a.mTotalCyclesInCallWithChildren;
+	}
+
+	// Write footer
+	f << R"(]};
+</script>
+
+<canvas id="canvas"></canvas>
+<div id="tooltip"></div>
+
+</tbody></table></body></html>)";
+}
+
+#endif // JPH_PROFILE_ENABLED
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/Profiler.h b/thirdparty/jolt_physics/Jolt/Core/Profiler.h
new file mode 100644
index 0000000000..878865313b
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/Profiler.h
@@ -0,0 +1,301 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_SUPPRESS_WARNINGS_STD_BEGIN
+#include <mutex>
+#include <chrono>
+JPH_SUPPRESS_WARNINGS_STD_END
+
+#include <Jolt/Core/NonCopyable.h>
+#include <Jolt/Core/TickCounter.h>
+#include <Jolt/Core/UnorderedMap.h>
+
+#if defined(JPH_EXTERNAL_PROFILE)
+
+JPH_NAMESPACE_BEGIN
+
+#ifdef JPH_SHARED_LIBRARY
+/// Functions called when a profiler measurement starts or stops, need to be overridden by the user.
+using ProfileStartMeasurementFunction = void (*)(const char *inName, uint32 inColor, uint8 *ioUserData);
+using ProfileEndMeasurementFunction = void (*)(uint8 *ioUserData);
+
+JPH_EXPORT extern ProfileStartMeasurementFunction ProfileStartMeasurement;
+JPH_EXPORT extern ProfileEndMeasurementFunction ProfileEndMeasurement;
+#endif // JPH_SHARED_LIBRARY
+
+/// Create this class on the stack to start sampling timing information of a particular scope.
+///
+/// For statically linked builds, this is left unimplemented intentionally. Needs to be implemented by the user of the library.
+/// On construction a measurement should start, on destruction it should be stopped.
+/// For dynamically linked builds, the user should override the ProfileStartMeasurement and ProfileEndMeasurement functions.
+class alignas(16) ExternalProfileMeasurement : public NonCopyable
+{
+public:
+	/// Constructor
+#ifdef JPH_SHARED_LIBRARY
+	JPH_INLINE						ExternalProfileMeasurement(const char *inName, uint32 inColor = 0) { ProfileStartMeasurement(inName, inColor, mUserData); }
+	JPH_INLINE						~ExternalProfileMeasurement() { ProfileEndMeasurement(mUserData); }
+#else
+									ExternalProfileMeasurement(const char *inName, uint32 inColor = 0);
+									~ExternalProfileMeasurement();
+#endif
+
+private:
+	uint8							mUserData[64];
+};
+
+JPH_NAMESPACE_END
+
+//////////////////////////////////////////////////////////////////////////////////////////
+// Macros to do the actual profiling
+//////////////////////////////////////////////////////////////////////////////////////////
+
+JPH_SUPPRESS_WARNING_PUSH
+JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat-pedantic")
+
+// Dummy implementations
+#define JPH_PROFILE_START(name)
+#define JPH_PROFILE_END()
+#define JPH_PROFILE_THREAD_START(name)
+#define JPH_PROFILE_THREAD_END()
+#define JPH_PROFILE_NEXTFRAME()
+#define JPH_PROFILE_DUMP(...)
+
+// Scope profiling measurement
+#define JPH_PROFILE_TAG2(line)		profile##line
+#define JPH_PROFILE_TAG(line)		JPH_PROFILE_TAG2(line)
+
+/// Macro to collect profiling information.
+///
+/// Usage:
+///
+///		{
+///			JPH_PROFILE("Operation");
+///			do operation;
+///		}
+///
+#define JPH_PROFILE(...)			ExternalProfileMeasurement JPH_PROFILE_TAG(__LINE__)(__VA_ARGS__)
+
+// Scope profiling for function
+#define JPH_PROFILE_FUNCTION()		JPH_PROFILE(JPH_FUNCTION_NAME)
+
+JPH_SUPPRESS_WARNING_POP
+
+#elif defined(JPH_PROFILE_ENABLED)
+
+JPH_NAMESPACE_BEGIN
+
+class ProfileSample;
+class ProfileThread;
+
+/// Singleton class for managing profiling information
+class JPH_EXPORT Profiler : public NonCopyable
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+								Profiler()															{ UpdateReferenceTime(); }
+
+	/// Increments the frame counter to provide statistics per frame
+	void						NextFrame();
+
+	/// Dump profiling statistics at the start of the next frame
+	/// @param inTag If not empty, this overrides the auto incrementing number in the filename of the dump file
+	void						Dump(const string_view &inTag = string_view());
+
+	/// Add a thread to be instrumented
+	void						AddThread(ProfileThread *inThread);
+
+	/// Remove a thread from being instrumented
+	void						RemoveThread(ProfileThread *inThread);
+
+	/// Singleton instance
+	static Profiler *			sInstance;
+
+private:
+	/// Helper class to freeze ProfileSamples per thread while processing them
+	struct ThreadSamples
+	{
+		String					mThreadName;
+		ProfileSample *			mSamplesBegin;
+		ProfileSample *			mSamplesEnd;
+	};
+
+	/// Helper class to aggregate ProfileSamples
+	class Aggregator
+	{
+	public:
+		/// Constructor
+								Aggregator(const char *inName)										: mName(inName) { }
+
+		/// Accumulate results for a measurement
+		void					AccumulateMeasurement(uint64 inCyclesInCallWithChildren)
+		{
+			mCallCounter++;
+			mTotalCyclesInCallWithChildren += inCyclesInCallWithChildren;
+			mMinCyclesInCallWithChildren = min(inCyclesInCallWithChildren, mMinCyclesInCallWithChildren);
+			mMaxCyclesInCallWithChildren = max(inCyclesInCallWithChildren, mMaxCyclesInCallWithChildren);
+		}
+
+		/// Sort descending by total cycles
+		bool					operator < (const Aggregator &inRHS) const
+		{
+			return mTotalCyclesInCallWithChildren > inRHS.mTotalCyclesInCallWithChildren;
+		}
+
+		/// Identification
+		const char *			mName;																///< User defined name of this item
+
+		/// Statistics
+		uint32					mCallCounter = 0;													///< Number of times AccumulateMeasurement was called
+		uint64					mTotalCyclesInCallWithChildren = 0;									///< Total amount of cycles spent in this scope
+		uint64					mMinCyclesInCallWithChildren = 0xffffffffffffffffUL;				///< Minimum amount of cycles spent per call
+		uint64					mMaxCyclesInCallWithChildren = 0;									///< Maximum amount of cycles spent per call
+	};
+
+	using Threads = Array<ThreadSamples>;
+	using Aggregators = Array<Aggregator>;
+	using KeyToAggregator = UnorderedMap<const char *, size_t>;
+
+	/// Helper function to aggregate profile sample data
+	static void					sAggregate(int inDepth, uint32 inColor, ProfileSample *&ioSample, const ProfileSample *inEnd, Aggregators &ioAggregators, KeyToAggregator &ioKeyToAggregator);
+
+	/// We measure the amount of ticks per second, this function resets the reference time point
+	void						UpdateReferenceTime();
+
+	/// Get the amount of ticks per second, note that this number will never be fully accurate as the amount of ticks per second may vary with CPU load, so this number is only to be used to give an indication of time for profiling purposes
+	uint64						GetProcessorTicksPerSecond() const;
+
+	/// Dump profiling statistics
+	void						DumpInternal();
+	void						DumpChart(const char *inTag, const Threads &inThreads, const KeyToAggregator &inKeyToAggregators, const Aggregators &inAggregators);
+
+	std::mutex					mLock;																///< Lock that protects mThreads
+	uint64						mReferenceTick;														///< Tick count at the start of the frame
+	std::chrono::high_resolution_clock::time_point mReferenceTime;									///< Time at the start of the frame
+	Array<ProfileThread *>		mThreads;															///< List of all active threads
+	bool						mDump = false;														///< When true, the samples are dumped next frame
+	String						mDumpTag;															///< When not empty, this overrides the auto incrementing number of the dump filename
+};
+
+// Class that contains the information of a single scoped measurement
+class alignas(16) JPH_EXPORT_GCC_BUG_WORKAROUND ProfileSample : public NonCopyable
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	const char *				mName;																///< User defined name of this item
+	uint32						mColor;																///< Color to use for this sample
+	uint8						mDepth;																///< Calculated depth
+	uint8						mUnused[3];
+	uint64						mStartCycle;														///< Cycle counter at start of measurement
+	uint64						mEndCycle;															///< Cycle counter at end of measurement
+};
+
+/// Collects all samples of a single thread
+class ProfileThread : public NonCopyable
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+	inline						ProfileThread(const string_view &inThreadName);
+	inline						~ProfileThread();
+
+	static const uint cMaxSamples = 65536;
+
+	String						mThreadName;														///< Name of the thread that we're collecting information for
+	ProfileSample				mSamples[cMaxSamples];												///< Buffer of samples
+	uint						mCurrentSample = 0;													///< Next position to write a sample to
+
+#ifdef JPH_SHARED_LIBRARY
+	JPH_EXPORT static void		sSetInstance(ProfileThread *inInstance);
+	JPH_EXPORT static ProfileThread *sGetInstance();
+#else
+	static inline void			sSetInstance(ProfileThread *inInstance)								{ sInstance = inInstance; }
+	static inline ProfileThread *sGetInstance()														{ return sInstance; }
+
+private:
+	static thread_local ProfileThread *sInstance;
+#endif
+};
+
+/// Create this class on the stack to start sampling timing information of a particular scope
+class JPH_EXPORT ProfileMeasurement : public NonCopyable
+{
+public:
+	/// Constructor
+	inline						ProfileMeasurement(const char *inName, uint32 inColor = 0);
+	inline						~ProfileMeasurement();
+
+private:
+	ProfileSample *				mSample;
+	ProfileSample				mTemp;
+
+	static bool					sOutOfSamplesReported;
+};
+
+JPH_NAMESPACE_END
+
+#include "Profiler.inl"
+
+//////////////////////////////////////////////////////////////////////////////////////////
+// Macros to do the actual profiling
+//////////////////////////////////////////////////////////////////////////////////////////
+
+JPH_SUPPRESS_WARNING_PUSH
+JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat-pedantic")
+
+/// Start instrumenting program
+#define JPH_PROFILE_START(name)			do { Profiler::sInstance = new Profiler; JPH_PROFILE_THREAD_START(name); } while (false)
+
+/// End instrumenting program
+#define JPH_PROFILE_END()				do { JPH_PROFILE_THREAD_END(); delete Profiler::sInstance; Profiler::sInstance = nullptr; } while (false)
+
+/// Start instrumenting a thread
+#define JPH_PROFILE_THREAD_START(name)	do { if (Profiler::sInstance) ProfileThread::sSetInstance(new ProfileThread(name)); } while (false)
+
+/// End instrumenting a thread
+#define JPH_PROFILE_THREAD_END()		do { delete ProfileThread::sGetInstance(); ProfileThread::sSetInstance(nullptr); } while (false)
+
+/// Scope profiling measurement
+#define JPH_PROFILE_TAG2(line)			profile##line
+#define JPH_PROFILE_TAG(line)			JPH_PROFILE_TAG2(line)
+#define JPH_PROFILE(...)				ProfileMeasurement JPH_PROFILE_TAG(__LINE__)(__VA_ARGS__)
+
+/// Scope profiling for function
+#define JPH_PROFILE_FUNCTION()			JPH_PROFILE(JPH_FUNCTION_NAME)
+
+/// Update frame counter
+#define JPH_PROFILE_NEXTFRAME()			Profiler::sInstance->NextFrame()
+
+/// Dump profiling info
+#define JPH_PROFILE_DUMP(...)			Profiler::sInstance->Dump(__VA_ARGS__)
+
+JPH_SUPPRESS_WARNING_POP
+
+#else
+
+//////////////////////////////////////////////////////////////////////////////////////////
+// Dummy profiling instructions
+//////////////////////////////////////////////////////////////////////////////////////////
+
+JPH_SUPPRESS_WARNING_PUSH
+JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat-pedantic")
+
+#define JPH_PROFILE_START(name)
+#define JPH_PROFILE_END()
+#define JPH_PROFILE_THREAD_START(name)
+#define JPH_PROFILE_THREAD_END()
+#define JPH_PROFILE(...)
+#define JPH_PROFILE_FUNCTION()
+#define JPH_PROFILE_NEXTFRAME()
+#define JPH_PROFILE_DUMP(...)
+
+JPH_SUPPRESS_WARNING_POP
+
+#endif
diff --git a/thirdparty/jolt_physics/Jolt/Core/Profiler.inl b/thirdparty/jolt_physics/Jolt/Core/Profiler.inl
new file mode 100644
index 0000000000..912ba83833
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/Profiler.inl
@@ -0,0 +1,90 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+JPH_NAMESPACE_BEGIN
+
+//////////////////////////////////////////////////////////////////////////////////////////
+// ProfileThread
+//////////////////////////////////////////////////////////////////////////////////////////
+
+ProfileThread::ProfileThread(const string_view &inThreadName) :
+	mThreadName(inThreadName)
+{
+	Profiler::sInstance->AddThread(this);
+}
+
+ProfileThread::~ProfileThread()
+{
+	Profiler::sInstance->RemoveThread(this);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////
+// ProfileMeasurement
+//////////////////////////////////////////////////////////////////////////////////////////
+
+JPH_TSAN_NO_SANITIZE // TSAN reports a race on sOutOfSamplesReported, however the worst case is that we report the out of samples message multiple times
+ProfileMeasurement::ProfileMeasurement(const char *inName, uint32 inColor)
+{
+	ProfileThread *current_thread = ProfileThread::sGetInstance();
+	if (current_thread == nullptr)
+	{
+		// Thread not instrumented
+		mSample = nullptr;
+	}
+	else if (current_thread->mCurrentSample < ProfileThread::cMaxSamples)
+	{
+		// Get pointer to write data to
+		mSample = &current_thread->mSamples[current_thread->mCurrentSample++];
+
+		// Start constructing sample (will end up on stack)
+		mTemp.mName = inName;
+		mTemp.mColor = inColor;
+
+		// Collect start sample last
+		mTemp.mStartCycle = GetProcessorTickCount();
+	}
+	else
+	{
+		// Out of samples
+		if (!sOutOfSamplesReported)
+		{
+			sOutOfSamplesReported = true;
+			Trace("ProfileMeasurement: Too many samples, some data will be lost!");
+		}
+		mSample = nullptr;
+	}
+}
+
+ProfileMeasurement::~ProfileMeasurement()
+{
+	if (mSample != nullptr)
+	{
+		// Finalize sample
+		mTemp.mEndCycle = GetProcessorTickCount();
+
+		// Write it to the memory buffer bypassing the cache
+		static_assert(sizeof(ProfileSample) == 32, "Assume 32 bytes");
+		static_assert(alignof(ProfileSample) == 16, "Assume 16 byte alignment");
+	#if defined(JPH_USE_SSE)
+		const __m128i *src = reinterpret_cast<const __m128i *>(&mTemp);
+		__m128i *dst = reinterpret_cast<__m128i *>(mSample);
+		__m128i val = _mm_loadu_si128(src);
+		_mm_stream_si128(dst, val);
+		val = _mm_loadu_si128(src + 1);
+		_mm_stream_si128(dst + 1, val);
+	#elif defined(JPH_USE_NEON)
+		const int *src = reinterpret_cast<const int *>(&mTemp);
+		int *dst = reinterpret_cast<int *>(mSample);
+		int32x4_t val = vld1q_s32(src);
+		vst1q_s32(dst, val);
+		val = vld1q_s32(src + 4);
+		vst1q_s32(dst + 4, val);
+	#else
+		memcpy(mSample, &mTemp, sizeof(ProfileSample));
+	#endif
+		mSample = nullptr;
+	}
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/QuickSort.h b/thirdparty/jolt_physics/Jolt/Core/QuickSort.h
new file mode 100644
index 0000000000..bd12a3b339
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/QuickSort.h
@@ -0,0 +1,137 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/InsertionSort.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Helper function for QuickSort, will move the pivot element to inMiddle.
+template <typename Iterator, typename Compare>
+inline void QuickSortMedianOfThree(Iterator inFirst, Iterator inMiddle, Iterator inLast, Compare inCompare)
+{
+	// This should be guaranteed because we switch over to insertion sort when there's 32 or less elements
+	JPH_ASSERT(inFirst != inMiddle && inMiddle != inLast);
+
+	if (inCompare(*inMiddle, *inFirst))
+		std::swap(*inFirst, *inMiddle);
+
+	if (inCompare(*inLast, *inFirst))
+		std::swap(*inFirst, *inLast);
+
+	if (inCompare(*inLast, *inMiddle))
+		std::swap(*inMiddle, *inLast);
+}
+
+/// Helper function for QuickSort using the Ninther method, will move the pivot element to inMiddle.
+template <typename Iterator, typename Compare>
+inline void QuickSortNinther(Iterator inFirst, Iterator inMiddle, Iterator inLast, Compare inCompare)
+{
+	// Divide the range in 8 equal parts (this means there are 9 points)
+	auto diff = (inLast - inFirst) >> 3;
+	auto two_diff = diff << 1;
+
+	// Median of first 3 points
+	Iterator mid1 = inFirst + diff;
+	QuickSortMedianOfThree(inFirst, mid1, inFirst + two_diff, inCompare);
+
+	// Median of second 3 points
+	QuickSortMedianOfThree(inMiddle - diff, inMiddle, inMiddle + diff, inCompare);
+
+	// Median of third 3 points
+	Iterator mid3 = inLast - diff;
+	QuickSortMedianOfThree(inLast - two_diff, mid3, inLast, inCompare);
+
+	// Determine the median of the 3 medians
+	QuickSortMedianOfThree(mid1, inMiddle, mid3, inCompare);
+}
+
+/// Implementation of the quick sort algorithm. The STL version implementation is not consistent across platforms.
+template <typename Iterator, typename Compare>
+inline void QuickSort(Iterator inBegin, Iterator inEnd, Compare inCompare)
+{
+	// Implementation based on https://en.wikipedia.org/wiki/Quicksort using Hoare's partition scheme
+
+	// Loop so that we only need to do 1 recursive call instead of 2.
+	for (;;)
+	{
+		// If there's less than 2 elements we're done
+		auto num_elements = inEnd - inBegin;
+		if (num_elements < 2)
+			return;
+
+		// Fall back to insertion sort if there are too few elements
+		if (num_elements <= 32)
+		{
+			InsertionSort(inBegin, inEnd, inCompare);
+			return;
+		}
+
+		// Determine pivot
+		Iterator pivot_iterator = inBegin + ((num_elements - 1) >> 1);
+		QuickSortNinther(inBegin, pivot_iterator, inEnd - 1, inCompare);
+		auto pivot = *pivot_iterator;
+
+		// Left and right iterators
+		Iterator i = inBegin;
+		Iterator j = inEnd;
+
+		for (;;)
+		{
+			// Find the first element that is bigger than the pivot
+			while (inCompare(*i, pivot))
+				i++;
+
+			// Find the last element that is smaller than the pivot
+			do
+				--j;
+			while (inCompare(pivot, *j));
+
+			// If the two iterators crossed, we're done
+			if (i >= j)
+				break;
+
+			// Swap the elements
+			std::swap(*i, *j);
+
+			// Note that the first while loop in this function should
+			// have been do i++ while (...) but since we cannot decrement
+			// the iterator from inBegin we left that out, so we need to do
+			// it here.
+			++i;
+		}
+
+		// Include the middle element on the left side
+		j++;
+
+		// Check which partition is smaller
+		if (j - inBegin < inEnd - j)
+		{
+			// Left side is smaller, recurse to left first
+			QuickSort(inBegin, j, inCompare);
+
+			// Loop again with the right side to avoid a call
+			inBegin = j;
+		}
+		else
+		{
+			// Right side is smaller, recurse to right first
+			QuickSort(j, inEnd, inCompare);
+
+			// Loop again with the left side to avoid a call
+			inEnd = j;
+		}
+	}
+}
+
+/// Implementation of quick sort algorithm without comparator.
+template <typename Iterator>
+inline void QuickSort(Iterator inBegin, Iterator inEnd)
+{
+	std::less<> compare;
+	QuickSort(inBegin, inEnd, compare);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/RTTI.cpp b/thirdparty/jolt_physics/Jolt/Core/RTTI.cpp
new file mode 100644
index 0000000000..91235745ad
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/RTTI.cpp
@@ -0,0 +1,149 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Core/RTTI.h>
+#include <Jolt/Core/StringTools.h>
+
+JPH_NAMESPACE_BEGIN
+
+//////////////////////////////////////////////////////////////////////////////////////////
+// RTTI
+//////////////////////////////////////////////////////////////////////////////////////////
+
+RTTI::RTTI(const char *inName, int inSize, pCreateObjectFunction inCreateObject, pDestructObjectFunction inDestructObject) :
+	mName(inName),
+	mSize(inSize),
+	mCreate(inCreateObject),
+	mDestruct(inDestructObject)
+{
+	JPH_ASSERT(inDestructObject != nullptr, "Object cannot be destructed");
+}
+
+RTTI::RTTI(const char *inName, int inSize, pCreateObjectFunction inCreateObject, pDestructObjectFunction inDestructObject, pCreateRTTIFunction inCreateRTTI) :
+	mName(inName),
+	mSize(inSize),
+	mCreate(inCreateObject),
+	mDestruct(inDestructObject)
+{
+	JPH_ASSERT(inDestructObject != nullptr, "Object cannot be destructed");
+
+	inCreateRTTI(*this);
+}
+
+int RTTI::GetBaseClassCount() const
+{
+	return (int)mBaseClasses.size();
+}
+
+const RTTI *RTTI::GetBaseClass(int inIdx) const
+{
+	return mBaseClasses[inIdx].mRTTI;
+}
+
+uint32 RTTI::GetHash() const
+{
+	// Perform diffusion step to get from 64 to 32 bits (see https://en.wikipedia.org/wiki/Fowler%E2%80%93Noll%E2%80%93Vo_hash_function)
+	uint64 hash = HashString(mName);
+	return (uint32)(hash ^ (hash >> 32));
+}
+
+void *RTTI::CreateObject() const
+{
+	return IsAbstract()? nullptr : mCreate();
+}
+
+void RTTI::DestructObject(void *inObject) const
+{
+	mDestruct(inObject);
+}
+
+void RTTI::AddBaseClass(const RTTI *inRTTI, int inOffset)
+{
+	JPH_ASSERT(inOffset >= 0 && inOffset < mSize, "Base class not contained in derived class");
+
+	// Add base class
+	BaseClass base;
+	base.mRTTI = inRTTI;
+	base.mOffset = inOffset;
+	mBaseClasses.push_back(base);
+
+#ifdef JPH_OBJECT_STREAM
+	// Add attributes of base class
+	for (const SerializableAttribute &a : inRTTI->mAttributes)
+		mAttributes.push_back(SerializableAttribute(a, inOffset));
+#endif // JPH_OBJECT_STREAM
+}
+
+bool RTTI::operator == (const RTTI &inRHS) const
+{
+	// Compare addresses
+	if (this == &inRHS)
+		return true;
+
+	// Check that the names differ (if that is the case we probably have two instances
+	// of the same attribute info across the program, probably the second is in a DLL)
+	JPH_ASSERT(strcmp(mName, inRHS.mName) != 0);
+	return false;
+}
+
+bool RTTI::IsKindOf(const RTTI *inRTTI) const
+{
+	// Check if this is the same type
+	if (this == inRTTI)
+		return true;
+
+	// Check all base classes
+	for (const BaseClass &b : mBaseClasses)
+		if (b.mRTTI->IsKindOf(inRTTI))
+			return true;
+
+	return false;
+}
+
+const void *RTTI::CastTo(const void *inObject, const RTTI *inRTTI) const
+{
+	JPH_ASSERT(inObject != nullptr);
+
+	// Check if this is the same type
+	if (this == inRTTI)
+		return inObject;
+
+	// Check all base classes
+	for (const BaseClass &b : mBaseClasses)
+	{
+		// Cast the pointer to the base class
+		const void *casted = (const void *)(((const uint8 *)inObject) + b.mOffset);
+
+		// Test base class
+		const void *rv = b.mRTTI->CastTo(casted, inRTTI);
+		if (rv != nullptr)
+			return rv;
+	}
+
+	// Not possible to cast
+	return nullptr;
+}
+
+#ifdef JPH_OBJECT_STREAM
+
+void RTTI::AddAttribute(const SerializableAttribute &inAttribute)
+{
+	mAttributes.push_back(inAttribute);
+}
+
+int RTTI::GetAttributeCount() const
+{
+	return (int)mAttributes.size();
+}
+
+const SerializableAttribute &RTTI::GetAttribute(int inIdx) const
+{
+	return mAttributes[inIdx];
+}
+
+#endif // JPH_OBJECT_STREAM
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/RTTI.h b/thirdparty/jolt_physics/Jolt/Core/RTTI.h
new file mode 100644
index 0000000000..90f358cbd5
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/RTTI.h
@@ -0,0 +1,436 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/Reference.h>
+#include <Jolt/Core/StaticArray.h>
+#include <Jolt/ObjectStream/SerializableAttribute.h>
+
+JPH_NAMESPACE_BEGIN
+
+//////////////////////////////////////////////////////////////////////////////////////////
+// RTTI
+//////////////////////////////////////////////////////////////////////////////////////////
+
+/// Light weight runtime type information system. This way we don't need to turn
+/// on the default RTTI system of the compiler (introducing a possible overhead for every
+/// class)
+///
+/// Notes:
+///  - An extra virtual member function is added. This adds 8 bytes to the size of
+///    an instance of the class (unless you are already using virtual functions).
+///
+/// To use RTTI on a specific class use:
+///
+/// Header file:
+///
+///		class Foo
+///		{
+///			JPH_DECLARE_RTTI_VIRTUAL_BASE(Foo)
+///		}
+///
+///		class Bar : public Foo
+///		{
+///			JPH_DECLARE_RTTI_VIRTUAL(Bar)
+///		};
+///
+/// Implementation file:
+///
+///		JPH_IMPLEMENT_RTTI_VIRTUAL_BASE(Foo)
+///		{
+///		}
+///
+///		JPH_IMPLEMENT_RTTI_VIRTUAL(Bar)
+///		{
+///			JPH_ADD_BASE_CLASS(Bar, Foo) // Multiple inheritance is allowed, just do JPH_ADD_BASE_CLASS for every base class
+///		}
+///
+/// For abstract classes use:
+///
+/// Header file:
+///
+///		class Foo
+///		{
+///			JPH_DECLARE_RTTI_ABSTRACT_BASE(Foo)
+///
+///		public:
+///			virtual void AbstractFunction() = 0;
+///		}
+///
+///		class Bar : public Foo
+///		{
+///			JPH_DECLARE_RTTI_VIRTUAL(Bar)
+///
+///		public:
+///			virtual void AbstractFunction() { } // Function is now implemented so this class is no longer abstract
+///		};
+///
+/// Implementation file:
+///
+///		JPH_IMPLEMENT_RTTI_ABSTRACT_BASE(Foo)
+///		{
+///		}
+///
+///		JPH_IMPLEMENT_RTTI_VIRTUAL(Bar)
+///		{
+///			JPH_ADD_BASE_CLASS(Bar, Foo)
+///		}
+///
+/// Example of usage in a program:
+///
+///		Foo *foo_ptr = new Foo;
+///		Foo *bar_ptr = new Bar;
+///
+///		IsType(foo_ptr, RTTI(Bar)) returns false
+///		IsType(bar_ptr, RTTI(Bar)) returns true
+///
+///		IsKindOf(foo_ptr, RTTI(Bar)) returns false
+///		IsKindOf(bar_ptr, RTTI(Foo)) returns true
+///		IsKindOf(bar_ptr, RTTI(Bar)) returns true
+///
+///		StaticCast<Bar>(foo_ptr) asserts and returns foo_ptr casted to Bar *
+///		StaticCast<Bar>(bar_ptr) returns bar_ptr casted to Bar *
+///
+///		DynamicCast<Bar>(foo_ptr) returns nullptr
+///		DynamicCast<Bar>(bar_ptr) returns bar_ptr casted to Bar *
+///
+/// Other feature of DynamicCast:
+///
+///		class A { int data[5]; };
+///		class B { int data[7]; };
+///		class C : public A, public B { int data[9]; };
+///
+///		C *c = new C;
+///		A *a = c;
+///
+/// Note that:
+///
+///		B *b = (B *)a;
+///
+/// generates an invalid pointer,
+///
+///		B *b = StaticCast<B>(a);
+///
+/// doesn't compile, and
+///
+///		B *b = DynamicCast<B>(a);
+///
+/// does the correct cast
+class JPH_EXPORT RTTI
+{
+public:
+	/// Function to create an object
+	using pCreateObjectFunction = void *(*)();
+
+	/// Function to destroy an object
+	using pDestructObjectFunction = void (*)(void *inObject);
+
+	/// Function to initialize the runtime type info structure
+	using pCreateRTTIFunction = void (*)(RTTI &inRTTI);
+
+	/// Constructor
+								RTTI(const char *inName, int inSize, pCreateObjectFunction inCreateObject, pDestructObjectFunction inDestructObject);
+								RTTI(const char *inName, int inSize, pCreateObjectFunction inCreateObject, pDestructObjectFunction inDestructObject, pCreateRTTIFunction inCreateRTTI);
+
+	// Properties
+	inline const char *			GetName() const												{ return mName; }
+	void						SetName(const char *inName)									{ mName = inName; }
+	inline int					GetSize() const												{ return mSize; }
+	bool						IsAbstract() const											{ return mCreate == nullptr || mDestruct == nullptr; }
+	int							GetBaseClassCount() const;
+	const RTTI *				GetBaseClass(int inIdx) const;
+	uint32						GetHash() const;
+
+	/// Create an object of this type (returns nullptr if the object is abstract)
+	void *						CreateObject() const;
+
+	/// Destruct object of this type (does nothing if the object is abstract)
+	void						DestructObject(void *inObject) const;
+
+	/// Add base class
+	void						AddBaseClass(const RTTI *inRTTI, int inOffset);
+
+	/// Equality operators
+	bool						operator == (const RTTI &inRHS) const;
+	bool						operator != (const RTTI &inRHS) const						{ return !(*this == inRHS); }
+
+	/// Test if this class is derived from class of type inRTTI
+	bool						IsKindOf(const RTTI *inRTTI) const;
+
+	/// Cast inObject of this type to object of type inRTTI, returns nullptr if the cast is unsuccessful
+	const void *				CastTo(const void *inObject, const RTTI *inRTTI) const;
+
+#ifdef JPH_OBJECT_STREAM
+	/// Attribute access
+	void						AddAttribute(const SerializableAttribute &inAttribute);
+	int							GetAttributeCount() const;
+	const SerializableAttribute & GetAttribute(int inIdx) const;
+#endif // JPH_OBJECT_STREAM
+
+protected:
+	/// Base class information
+	struct BaseClass
+	{
+		const RTTI *			mRTTI;
+		int						mOffset;
+	};
+
+	const char *				mName;														///< Class name
+	int							mSize;														///< Class size
+	StaticArray<BaseClass, 4>	mBaseClasses;												///< Names of base classes
+	pCreateObjectFunction		mCreate;													///< Pointer to a function that will create a new instance of this class
+	pDestructObjectFunction		mDestruct;													///< Pointer to a function that will destruct an object of this class
+#ifdef JPH_OBJECT_STREAM
+	StaticArray<SerializableAttribute, 32> mAttributes;										///< All attributes of this class
+#endif // JPH_OBJECT_STREAM
+};
+
+//////////////////////////////////////////////////////////////////////////////////////////
+// Add run time type info to types that don't have virtual functions
+//////////////////////////////////////////////////////////////////////////////////////////
+
+// JPH_DECLARE_RTTI_NON_VIRTUAL
+#define JPH_DECLARE_RTTI_NON_VIRTUAL(linkage, class_name)															\
+public:																												\
+	JPH_OVERRIDE_NEW_DELETE																							\
+	friend linkage RTTI *		GetRTTIOfType(class_name *);														\
+	friend inline const RTTI *	GetRTTI([[maybe_unused]] const class_name *inObject) { return GetRTTIOfType(static_cast<class_name *>(nullptr)); }\
+	static void					sCreateRTTI(RTTI &inRTTI);															\
+
+// JPH_IMPLEMENT_RTTI_NON_VIRTUAL
+#define JPH_IMPLEMENT_RTTI_NON_VIRTUAL(class_name)																	\
+	RTTI *						GetRTTIOfType(class_name *)															\
+	{																												\
+		static RTTI rtti(#class_name, sizeof(class_name), []() -> void * { return new class_name; }, [](void *inObject) { delete (class_name *)inObject; }, &class_name::sCreateRTTI); \
+		return &rtti;																								\
+	}																												\
+	void						class_name::sCreateRTTI(RTTI &inRTTI)												\
+
+//////////////////////////////////////////////////////////////////////////////////////////
+// Same as above, but when you cannot insert the declaration in the class
+// itself, for example for templates and third party classes
+//////////////////////////////////////////////////////////////////////////////////////////
+
+// JPH_DECLARE_RTTI_OUTSIDE_CLASS
+#define JPH_DECLARE_RTTI_OUTSIDE_CLASS(linkage, class_name)															\
+	linkage RTTI *				GetRTTIOfType(class_name *);														\
+	inline const RTTI *			GetRTTI(const class_name *inObject) { return GetRTTIOfType((class_name *)nullptr); }\
+	void						CreateRTTI##class_name(RTTI &inRTTI);												\
+
+// JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS
+#define JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(class_name)																\
+	RTTI *						GetRTTIOfType(class_name *)															\
+	{																												\
+		static RTTI rtti((const char *)#class_name, sizeof(class_name), []() -> void * { return new class_name; }, [](void *inObject) { delete (class_name *)inObject; }, &CreateRTTI##class_name); \
+		return &rtti;																								\
+	}																												\
+	void						CreateRTTI##class_name(RTTI &inRTTI)
+
+//////////////////////////////////////////////////////////////////////////////////////////
+// Same as above, but for classes that have virtual functions
+//////////////////////////////////////////////////////////////////////////////////////////
+
+#define JPH_DECLARE_RTTI_HELPER(linkage, class_name, modifier)														\
+public:																												\
+	JPH_OVERRIDE_NEW_DELETE																							\
+	friend linkage RTTI *		GetRTTIOfType(class_name *);														\
+	friend inline const RTTI *	GetRTTI(const class_name *inObject) { return inObject->GetRTTI(); }					\
+	virtual const RTTI *		GetRTTI() const modifier;															\
+	virtual const void *		CastTo(const RTTI *inRTTI) const modifier;											\
+	static void					sCreateRTTI(RTTI &inRTTI);															\
+
+// JPH_DECLARE_RTTI_VIRTUAL - for derived classes with RTTI
+#define JPH_DECLARE_RTTI_VIRTUAL(linkage, class_name)																\
+	JPH_DECLARE_RTTI_HELPER(linkage, class_name, override)
+
+// JPH_IMPLEMENT_RTTI_VIRTUAL
+#define JPH_IMPLEMENT_RTTI_VIRTUAL(class_name)																		\
+	RTTI *			GetRTTIOfType(class_name *)																		\
+	{																												\
+		static RTTI rtti(#class_name, sizeof(class_name), []() -> void * { return new class_name; }, [](void *inObject) { delete (class_name *)inObject; }, &class_name::sCreateRTTI); \
+		return &rtti;																								\
+	}																												\
+	const RTTI *				class_name::GetRTTI() const															\
+	{																												\
+		return JPH_RTTI(class_name);																				\
+	}																												\
+	const void *				class_name::CastTo(const RTTI *inRTTI) const										\
+	{																												\
+		return JPH_RTTI(class_name)->CastTo((const void *)this, inRTTI);											\
+	}																												\
+	void						class_name::sCreateRTTI(RTTI &inRTTI)												\
+
+// JPH_DECLARE_RTTI_VIRTUAL_BASE - for concrete base class that has RTTI
+#define JPH_DECLARE_RTTI_VIRTUAL_BASE(linkage, class_name)															\
+	JPH_DECLARE_RTTI_HELPER(linkage, class_name, )
+
+// JPH_IMPLEMENT_RTTI_VIRTUAL_BASE
+#define JPH_IMPLEMENT_RTTI_VIRTUAL_BASE(class_name)																	\
+	JPH_IMPLEMENT_RTTI_VIRTUAL(class_name)
+
+// JPH_DECLARE_RTTI_ABSTRACT - for derived abstract class that have RTTI
+#define JPH_DECLARE_RTTI_ABSTRACT(linkage, class_name)																\
+	JPH_DECLARE_RTTI_HELPER(linkage, class_name, override)
+
+// JPH_IMPLEMENT_RTTI_ABSTRACT
+#define JPH_IMPLEMENT_RTTI_ABSTRACT(class_name)																		\
+	RTTI *						GetRTTIOfType(class_name *)															\
+	{																												\
+		static RTTI rtti(#class_name, sizeof(class_name), nullptr, [](void *inObject) { delete (class_name *)inObject; }, &class_name::sCreateRTTI); \
+		return &rtti;																								\
+	}																												\
+	const RTTI *				class_name::GetRTTI() const															\
+	{																												\
+		return JPH_RTTI(class_name);																				\
+	}																												\
+	const void *				class_name::CastTo(const RTTI *inRTTI) const										\
+	{																												\
+		return JPH_RTTI(class_name)->CastTo((const void *)this, inRTTI);											\
+	}																												\
+	void						class_name::sCreateRTTI(RTTI &inRTTI)												\
+
+// JPH_DECLARE_RTTI_ABSTRACT_BASE - for abstract base class that has RTTI
+#define JPH_DECLARE_RTTI_ABSTRACT_BASE(linkage, class_name)															\
+	JPH_DECLARE_RTTI_HELPER(linkage, class_name, )
+
+// JPH_IMPLEMENT_RTTI_ABSTRACT_BASE
+#define JPH_IMPLEMENT_RTTI_ABSTRACT_BASE(class_name)																\
+	JPH_IMPLEMENT_RTTI_ABSTRACT(class_name)
+
+//////////////////////////////////////////////////////////////////////////////////////////
+// Declare an RTTI class for registering with the factory
+//////////////////////////////////////////////////////////////////////////////////////////
+
+#define JPH_DECLARE_RTTI_FOR_FACTORY(linkage, class_name)															\
+	linkage RTTI *				GetRTTIOfType(class class_name *);
+
+#define JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(linkage, name_space, class_name)								\
+	namespace name_space {																							\
+		class class_name;																							\
+		linkage RTTI *			GetRTTIOfType(class class_name *);													\
+	}
+
+//////////////////////////////////////////////////////////////////////////////////////////
+// Find the RTTI of a class
+//////////////////////////////////////////////////////////////////////////////////////////
+
+#define JPH_RTTI(class_name)	GetRTTIOfType(static_cast<class_name *>(nullptr))
+
+//////////////////////////////////////////////////////////////////////////////////////////
+// Macro to rename a class, useful for embedded classes:
+//
+// class A { class B { }; }
+//
+// Now use JPH_RENAME_CLASS(B, A::B) to avoid conflicts with other classes named B
+//////////////////////////////////////////////////////////////////////////////////////////
+
+// JPH_RENAME_CLASS
+#define JPH_RENAME_CLASS(class_name, new_name)																		\
+								inRTTI.SetName(#new_name);
+
+//////////////////////////////////////////////////////////////////////////////////////////
+// Macro to add base classes
+//////////////////////////////////////////////////////////////////////////////////////////
+
+/// Define very dirty macro to get the offset of a baseclass into a class
+#define JPH_BASE_CLASS_OFFSET(inClass, inBaseClass)	((int(uint64((inBaseClass *)((inClass *)0x10000))))-0x10000)
+
+// JPH_ADD_BASE_CLASS
+#define JPH_ADD_BASE_CLASS(class_name, base_class_name)																\
+								inRTTI.AddBaseClass(JPH_RTTI(base_class_name), JPH_BASE_CLASS_OFFSET(class_name, base_class_name));
+
+//////////////////////////////////////////////////////////////////////////////////////////
+// Macros and templates to identify a class
+//////////////////////////////////////////////////////////////////////////////////////////
+
+/// Check if inObject is of DstType
+template <class Type>
+inline bool IsType(const Type *inObject, const RTTI *inRTTI)
+{
+	return inObject == nullptr || *inObject->GetRTTI() == *inRTTI;
+}
+
+template <class Type>
+inline bool IsType(const RefConst<Type> &inObject, const RTTI *inRTTI)
+{
+	return inObject == nullptr || *inObject->GetRTTI() == *inRTTI;
+}
+
+template <class Type>
+inline bool IsType(const Ref<Type> &inObject, const RTTI *inRTTI)
+{
+	return inObject == nullptr || *inObject->GetRTTI() == *inRTTI;
+}
+
+/// Check if inObject is or is derived from DstType
+template <class Type>
+inline bool IsKindOf(const Type *inObject, const RTTI *inRTTI)
+{
+	return inObject == nullptr || inObject->GetRTTI()->IsKindOf(inRTTI);
+}
+
+template <class Type>
+inline bool IsKindOf(const RefConst<Type> &inObject, const RTTI *inRTTI)
+{
+	return inObject == nullptr || inObject->GetRTTI()->IsKindOf(inRTTI);
+}
+
+template <class Type>
+inline bool IsKindOf(const Ref<Type> &inObject, const RTTI *inRTTI)
+{
+	return inObject == nullptr || inObject->GetRTTI()->IsKindOf(inRTTI);
+}
+
+/// Cast inObject to DstType, asserts on failure
+template <class DstType, class SrcType, std::enable_if_t<std::is_base_of_v<DstType, SrcType> || std::is_base_of_v<SrcType, DstType>, bool> = true>
+inline const DstType *StaticCast(const SrcType *inObject)
+{
+	return static_cast<const DstType *>(inObject);
+}
+
+template <class DstType, class SrcType, std::enable_if_t<std::is_base_of_v<DstType, SrcType> || std::is_base_of_v<SrcType, DstType>, bool> = true>
+inline DstType *StaticCast(SrcType *inObject)
+{
+	return static_cast<DstType *>(inObject);
+}
+
+template <class DstType, class SrcType, std::enable_if_t<std::is_base_of_v<DstType, SrcType> || std::is_base_of_v<SrcType, DstType>, bool> = true>
+inline const DstType *StaticCast(const RefConst<SrcType> &inObject)
+{
+	return static_cast<const DstType *>(inObject.GetPtr());
+}
+
+template <class DstType, class SrcType, std::enable_if_t<std::is_base_of_v<DstType, SrcType> || std::is_base_of_v<SrcType, DstType>, bool> = true>
+inline DstType *StaticCast(const Ref<SrcType> &inObject)
+{
+	return static_cast<DstType *>(inObject.GetPtr());
+}
+
+/// Cast inObject to DstType, returns nullptr on failure
+template <class DstType, class SrcType>
+inline const DstType *DynamicCast(const SrcType *inObject)
+{
+	return inObject != nullptr? reinterpret_cast<const DstType *>(inObject->CastTo(JPH_RTTI(DstType))) : nullptr;
+}
+
+template <class DstType, class SrcType>
+inline DstType *DynamicCast(SrcType *inObject)
+{
+	return inObject != nullptr? const_cast<DstType *>(reinterpret_cast<const DstType *>(inObject->CastTo(JPH_RTTI(DstType)))) : nullptr;
+}
+
+template <class DstType, class SrcType>
+inline const DstType *DynamicCast(const RefConst<SrcType> &inObject)
+{
+	return inObject != nullptr? reinterpret_cast<const DstType *>(inObject->CastTo(JPH_RTTI(DstType))) : nullptr;
+}
+
+template <class DstType, class SrcType>
+inline DstType *DynamicCast(const Ref<SrcType> &inObject)
+{
+	return inObject != nullptr? const_cast<DstType *>(reinterpret_cast<const DstType *>(inObject->CastTo(JPH_RTTI(DstType)))) : nullptr;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/Reference.h b/thirdparty/jolt_physics/Jolt/Core/Reference.h
new file mode 100644
index 0000000000..8a2de69686
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/Reference.h
@@ -0,0 +1,244 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/Atomics.h>
+
+JPH_NAMESPACE_BEGIN
+
+// Forward declares
+template <class T> class Ref;
+template <class T> class RefConst;
+
+/// Simple class to facilitate reference counting / releasing
+/// Derive your class from RefTarget and you can reference it by using Ref<classname> or RefConst<classname>
+///
+/// Reference counting classes keep an integer which indicates how many references
+/// to the object are active. Reference counting objects are derived from RefTarget
+/// and staT & their life with a reference count of zero. They can then be assigned
+/// to equivalents of pointers (Ref) which will increase the reference count immediately.
+/// If the destructor of Ref is called or another object is assigned to the reference
+/// counting pointer it will decrease the reference count of the object again. If this
+/// reference count becomes zero, the object is destroyed.
+///
+/// This provides a very powerful mechanism to prevent memory leaks, but also gives
+/// some responsibility to the programmer. The most notable point is that you cannot
+/// have one object reference another and have the other reference the first one
+/// back, because this way the reference count of both objects will never become
+/// lower than 1, resulting in a memory leak. By carefully designing your classes
+/// (and particularly identifying who owns who in the class hierarchy) you can avoid
+/// these problems.
+template <class T>
+class RefTarget
+{
+public:
+	/// Constructor
+	inline					RefTarget() = default;
+	inline					RefTarget(const RefTarget &)					{ /* Do not copy refcount */ }
+	inline					~RefTarget()									{ JPH_IF_ENABLE_ASSERTS(uint32 value = mRefCount.load(memory_order_relaxed);) JPH_ASSERT(value == 0 || value == cEmbedded); } ///< assert no one is referencing us
+
+	/// Mark this class as embedded, this means the type can be used in a compound or constructed on the stack.
+	/// The Release function will never destruct the object, it is assumed the destructor will be called by whoever allocated
+	/// the object and at that point in time it is checked that no references are left to the structure.
+	inline void				SetEmbedded() const								{ JPH_IF_ENABLE_ASSERTS(uint32 old = ) mRefCount.fetch_add(cEmbedded, memory_order_relaxed); JPH_ASSERT(old < cEmbedded); }
+
+	/// Assignment operator
+	inline RefTarget &		operator = (const RefTarget &)					{ /* Don't copy refcount */ return *this; }
+
+	/// Get current refcount of this object
+	uint32					GetRefCount() const								{ return mRefCount.load(memory_order_relaxed); }
+
+	/// Add or release a reference to this object
+	inline void				AddRef() const
+	{
+		// Adding a reference can use relaxed memory ordering
+		mRefCount.fetch_add(1, memory_order_relaxed);
+	}
+
+	inline void				Release() const
+	{
+	#ifndef JPH_TSAN_ENABLED
+		// Releasing a reference must use release semantics...
+		if (mRefCount.fetch_sub(1, memory_order_release) == 1)
+		{
+			// ... so that we can use acquire to ensure that we see any updates from other threads that released a ref before deleting the object
+			atomic_thread_fence(memory_order_acquire);
+			delete static_cast<const T *>(this);
+		}
+	#else
+		// But under TSAN, we cannot use atomic_thread_fence, so we use an acq_rel operation unconditionally instead
+		if (mRefCount.fetch_sub(1, memory_order_acq_rel) == 1)
+			delete static_cast<const T *>(this);
+	#endif
+	}
+
+	/// INTERNAL HELPER FUNCTION USED BY SERIALIZATION
+	static int				sInternalGetRefCountOffset()					{ return offsetof(T, mRefCount); }
+
+protected:
+	static constexpr uint32 cEmbedded = 0x0ebedded;							///< A large value that gets added to the refcount to mark the object as embedded
+
+	mutable atomic<uint32>	mRefCount = 0;									///< Current reference count
+};
+
+/// Pure virtual version of RefTarget
+class JPH_EXPORT RefTargetVirtual
+{
+public:
+	/// Virtual destructor
+	virtual					~RefTargetVirtual() = default;
+
+	/// Virtual add reference
+	virtual void			AddRef() = 0;
+
+	/// Virtual release reference
+	virtual void			Release() = 0;
+};
+
+/// Class for automatic referencing, this is the equivalent of a pointer to type T
+/// if you assign a value to this class it will increment the reference count by one
+/// of this object, and if you assign something else it will decrease the reference
+/// count of the first object again. If it reaches a reference count of zero it will
+/// be deleted
+template <class T>
+class Ref
+{
+public:
+	/// Constructor
+	inline					Ref()											: mPtr(nullptr) { }
+	inline					Ref(T *inRHS)									: mPtr(inRHS) { AddRef(); }
+	inline					Ref(const Ref<T> &inRHS)						: mPtr(inRHS.mPtr) { AddRef(); }
+	inline					Ref(Ref<T> &&inRHS) noexcept					: mPtr(inRHS.mPtr) { inRHS.mPtr = nullptr; }
+	inline					~Ref()											{ Release(); }
+
+	/// Assignment operators
+	inline Ref<T> &			operator = (T *inRHS)							{ if (mPtr != inRHS) { Release(); mPtr = inRHS; AddRef(); } return *this; }
+	inline Ref<T> &			operator = (const Ref<T> &inRHS)				{ if (mPtr != inRHS.mPtr) { Release(); mPtr = inRHS.mPtr; AddRef(); } return *this; }
+	inline Ref<T> &			operator = (Ref<T> &&inRHS) noexcept			{ if (mPtr != inRHS.mPtr) { Release(); mPtr = inRHS.mPtr; inRHS.mPtr = nullptr; } return *this; }
+
+	/// Casting operators
+	inline					operator T *() const							{ return mPtr; }
+
+	/// Access like a normal pointer
+	inline T *				operator -> () const							{ return mPtr; }
+	inline T &				operator * () const								{ return *mPtr; }
+
+	/// Comparison
+	inline bool				operator == (const T * inRHS) const				{ return mPtr == inRHS; }
+	inline bool				operator == (const Ref<T> &inRHS) const			{ return mPtr == inRHS.mPtr; }
+	inline bool				operator != (const T * inRHS) const				{ return mPtr != inRHS; }
+	inline bool				operator != (const Ref<T> &inRHS) const			{ return mPtr != inRHS.mPtr; }
+
+	/// Get pointer
+	inline T *				GetPtr() const									{ return mPtr; }
+
+	/// Get hash for this object
+	uint64					GetHash() const
+	{
+		return Hash<T *> { } (mPtr);
+	}
+
+	/// INTERNAL HELPER FUNCTION USED BY SERIALIZATION
+	void **					InternalGetPointer()							{ return reinterpret_cast<void **>(&mPtr); }
+
+private:
+	template <class T2> friend class RefConst;
+
+	/// Use "variable = nullptr;" to release an object, do not call these functions
+	inline void				AddRef()										{ if (mPtr != nullptr) mPtr->AddRef(); }
+	inline void				Release()										{ if (mPtr != nullptr) mPtr->Release(); }
+
+	T *						mPtr;											///< Pointer to object that we are reference counting
+};
+
+/// Class for automatic referencing, this is the equivalent of a CONST pointer to type T
+/// if you assign a value to this class it will increment the reference count by one
+/// of this object, and if you assign something else it will decrease the reference
+/// count of the first object again. If it reaches a reference count of zero it will
+/// be deleted
+template <class T>
+class RefConst
+{
+public:
+	/// Constructor
+	inline					RefConst()										: mPtr(nullptr) { }
+	inline					RefConst(const T * inRHS)						: mPtr(inRHS) { AddRef(); }
+	inline					RefConst(const RefConst<T> &inRHS)				: mPtr(inRHS.mPtr) { AddRef(); }
+	inline					RefConst(RefConst<T> &&inRHS) noexcept			: mPtr(inRHS.mPtr) { inRHS.mPtr = nullptr; }
+	inline					RefConst(const Ref<T> &inRHS)					: mPtr(inRHS.mPtr) { AddRef(); }
+	inline					RefConst(Ref<T> &&inRHS) noexcept				: mPtr(inRHS.mPtr) { inRHS.mPtr = nullptr; }
+	inline					~RefConst()										{ Release(); }
+
+	/// Assignment operators
+	inline RefConst<T> &	operator = (const T * inRHS)					{ if (mPtr != inRHS) { Release(); mPtr = inRHS; AddRef(); } return *this; }
+	inline RefConst<T> &	operator = (const RefConst<T> &inRHS)			{ if (mPtr != inRHS.mPtr) { Release(); mPtr = inRHS.mPtr; AddRef(); } return *this; }
+	inline RefConst<T> &	operator = (RefConst<T> &&inRHS) noexcept		{ if (mPtr != inRHS.mPtr) { Release(); mPtr = inRHS.mPtr; inRHS.mPtr = nullptr; } return *this; }
+	inline RefConst<T> &	operator = (const Ref<T> &inRHS)				{ if (mPtr != inRHS.mPtr) { Release(); mPtr = inRHS.mPtr; AddRef(); } return *this; }
+	inline RefConst<T> &	operator = (Ref<T> &&inRHS) noexcept			{ if (mPtr != inRHS.mPtr) { Release(); mPtr = inRHS.mPtr; inRHS.mPtr = nullptr; } return *this; }
+
+	/// Casting operators
+	inline					operator const T * () const						{ return mPtr; }
+
+	/// Access like a normal pointer
+	inline const T *		operator -> () const							{ return mPtr; }
+	inline const T &		operator * () const								{ return *mPtr; }
+
+	/// Comparison
+	inline bool				operator == (const T * inRHS) const				{ return mPtr == inRHS; }
+	inline bool				operator == (const RefConst<T> &inRHS) const	{ return mPtr == inRHS.mPtr; }
+	inline bool				operator == (const Ref<T> &inRHS) const			{ return mPtr == inRHS.mPtr; }
+	inline bool				operator != (const T * inRHS) const				{ return mPtr != inRHS; }
+	inline bool				operator != (const RefConst<T> &inRHS) const	{ return mPtr != inRHS.mPtr; }
+	inline bool				operator != (const Ref<T> &inRHS) const			{ return mPtr != inRHS.mPtr; }
+
+	/// Get pointer
+	inline const T *		GetPtr() const									{ return mPtr; }
+
+	/// Get hash for this object
+	uint64					GetHash() const
+	{
+		return Hash<const T *> { } (mPtr);
+	}
+
+	/// INTERNAL HELPER FUNCTION USED BY SERIALIZATION
+	void **					InternalGetPointer()							{ return const_cast<void **>(reinterpret_cast<const void **>(&mPtr)); }
+
+private:
+	/// Use "variable = nullptr;" to release an object, do not call these functions
+	inline void				AddRef()										{ if (mPtr != nullptr) mPtr->AddRef(); }
+	inline void				Release()										{ if (mPtr != nullptr) mPtr->Release(); }
+
+	const T *				mPtr;											///< Pointer to object that we are reference counting
+};
+
+JPH_NAMESPACE_END
+
+JPH_SUPPRESS_WARNING_PUSH
+JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat")
+
+namespace std
+{
+	/// Declare std::hash for Ref
+	template <class T>
+	struct hash<JPH::Ref<T>>
+	{
+		size_t operator () (const JPH::Ref<T> &inRHS) const
+		{
+			return size_t(inRHS.GetHash());
+		}
+	};
+
+	/// Declare std::hash for RefConst
+	template <class T>
+	struct hash<JPH::RefConst<T>>
+	{
+		size_t operator () (const JPH::RefConst<T> &inRHS) const
+		{
+			return size_t(inRHS.GetHash());
+		}
+	};
+}
+
+JPH_SUPPRESS_WARNING_POP
diff --git a/thirdparty/jolt_physics/Jolt/Core/Result.h b/thirdparty/jolt_physics/Jolt/Core/Result.h
new file mode 100644
index 0000000000..ad2eab164b
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/Result.h
@@ -0,0 +1,174 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Helper class that either contains a valid result or an error
+template <class Type>
+class Result
+{
+public:
+	/// Default constructor
+						Result()									{ }
+
+	/// Copy constructor
+						Result(const Result<Type> &inRHS) :
+		mState(inRHS.mState)
+	{
+		switch (inRHS.mState)
+		{
+		case EState::Valid:
+			::new (&mResult) Type (inRHS.mResult);
+			break;
+
+		case EState::Error:
+			::new (&mError) String(inRHS.mError);
+			break;
+
+		case EState::Invalid:
+			break;
+		}
+	}
+
+	/// Move constructor
+						Result(Result<Type> &&inRHS) noexcept :
+		mState(inRHS.mState)
+	{
+		switch (inRHS.mState)
+		{
+		case EState::Valid:
+			::new (&mResult) Type (std::move(inRHS.mResult));
+			break;
+
+		case EState::Error:
+			::new (&mError) String(std::move(inRHS.mError));
+			break;
+
+		case EState::Invalid:
+			break;
+		}
+
+		// Don't reset the state of inRHS, the destructors still need to be called after a move operation
+	}
+
+	/// Destructor
+						~Result()									{ Clear(); }
+
+	/// Copy assignment
+	Result<Type> &		operator = (const Result<Type> &inRHS)
+	{
+		Clear();
+
+		mState = inRHS.mState;
+
+		switch (inRHS.mState)
+		{
+		case EState::Valid:
+			::new (&mResult) Type (inRHS.mResult);
+			break;
+
+		case EState::Error:
+			::new (&mError) String(inRHS.mError);
+			break;
+
+		case EState::Invalid:
+			break;
+		}
+
+		return *this;
+	}
+
+	/// Move assignment
+	Result<Type> &		operator = (Result<Type> &&inRHS) noexcept
+	{
+		Clear();
+
+		mState = inRHS.mState;
+
+		switch (inRHS.mState)
+		{
+		case EState::Valid:
+			::new (&mResult) Type (std::move(inRHS.mResult));
+			break;
+
+		case EState::Error:
+			::new (&mError) String(std::move(inRHS.mError));
+			break;
+
+		case EState::Invalid:
+			break;
+		}
+
+		// Don't reset the state of inRHS, the destructors still need to be called after a move operation
+
+		return *this;
+	}
+
+	/// Clear result or error
+	void				Clear()
+	{
+		switch (mState)
+		{
+		case EState::Valid:
+			mResult.~Type();
+			break;
+
+		case EState::Error:
+			mError.~String();
+			break;
+
+		case EState::Invalid:
+			break;
+		}
+
+		mState = EState::Invalid;
+	}
+
+	/// Checks if the result is still uninitialized
+	bool				IsEmpty() const								{ return mState == EState::Invalid; }
+
+	/// Checks if the result is valid
+	bool				IsValid() const								{ return mState == EState::Valid; }
+
+	/// Get the result value
+	const Type &		Get() const									{ JPH_ASSERT(IsValid()); return mResult; }
+
+	/// Set the result value
+	void				Set(const Type &inResult)					{ Clear(); ::new (&mResult) Type(inResult); mState = EState::Valid; }
+
+	/// Set the result value (move value)
+	void				Set(Type &&inResult)						{ Clear(); ::new (&mResult) Type(std::move(inResult)); mState = EState::Valid; }
+
+	/// Check if we had an error
+	bool				HasError() const							{ return mState == EState::Error; }
+
+	/// Get the error value
+	const String &		GetError() const							{ JPH_ASSERT(HasError()); return mError; }
+
+	/// Set an error value
+	void				SetError(const char *inError)				{ Clear(); ::new (&mError) String(inError); mState = EState::Error; }
+	void				SetError(const string_view &inError)		{ Clear(); ::new (&mError) String(inError); mState = EState::Error; }
+	void				SetError(String &&inError)					{ Clear(); ::new (&mError) String(std::move(inError)); mState = EState::Error; }
+
+private:
+	union
+	{
+		Type			mResult;									///< The actual result object
+		String			mError;										///< The error description if the result failed
+	};
+
+	/// State of the result
+	enum class EState : uint8
+	{
+		Invalid,
+		Valid,
+		Error
+	};
+
+	EState				mState = EState::Invalid;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/STLAlignedAllocator.h b/thirdparty/jolt_physics/Jolt/Core/STLAlignedAllocator.h
new file mode 100644
index 0000000000..60e3ad4873
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/STLAlignedAllocator.h
@@ -0,0 +1,72 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// STL allocator that takes care that memory is aligned to N bytes
+template <typename T, size_t N>
+class STLAlignedAllocator
+{
+public:
+	using value_type = T;
+
+	/// Pointer to type
+	using pointer = T *;
+	using const_pointer = const T *;
+
+	/// Reference to type.
+	/// Can be removed in C++20.
+	using reference = T &;
+	using const_reference = const T &;
+
+	using size_type = size_t;
+	using difference_type = ptrdiff_t;
+
+	/// The allocator is stateless
+	using is_always_equal = std::true_type;
+
+	/// Allocator supports moving
+	using propagate_on_container_move_assignment = std::true_type;
+
+	/// Constructor
+	inline					STLAlignedAllocator() = default;
+
+	/// Constructor from other allocator
+	template <typename T2>
+	inline explicit			STLAlignedAllocator(const STLAlignedAllocator<T2, N> &) { }
+
+	/// Allocate memory
+	inline pointer			allocate(size_type inN)
+	{
+		return (pointer)AlignedAllocate(inN * sizeof(value_type), N);
+	}
+
+	/// Free memory
+	inline void				deallocate(pointer inPointer, size_type)
+	{
+		AlignedFree(inPointer);
+	}
+
+	/// Allocators are stateless so assumed to be equal
+	inline bool				operator == (const STLAlignedAllocator<T, N> &) const
+	{
+		return true;
+	}
+
+	inline bool				operator != (const STLAlignedAllocator<T, N> &) const
+	{
+		return false;
+	}
+
+	/// Converting to allocator for other type
+	template <typename T2>
+	struct rebind
+	{
+		using other = STLAlignedAllocator<T2, N>;
+	};
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/STLAllocator.h b/thirdparty/jolt_physics/Jolt/Core/STLAllocator.h
new file mode 100644
index 0000000000..f9573279ad
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/STLAllocator.h
@@ -0,0 +1,127 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Default implementation of AllocatorHasReallocate which tells if an allocator has a reallocate function
+template <class T> struct AllocatorHasReallocate { static constexpr bool sValue = false; };
+
+#ifndef JPH_DISABLE_CUSTOM_ALLOCATOR
+
+/// STL allocator that forwards to our allocation functions
+template <typename T>
+class STLAllocator
+{
+public:
+	using value_type = T;
+
+	/// Pointer to type
+	using pointer = T *;
+	using const_pointer = const T *;
+
+	/// Reference to type.
+	/// Can be removed in C++20.
+	using reference = T &;
+	using const_reference = const T &;
+
+	using size_type = size_t;
+	using difference_type = ptrdiff_t;
+
+	/// The allocator is stateless
+	using is_always_equal = std::true_type;
+
+	/// Allocator supports moving
+	using propagate_on_container_move_assignment = std::true_type;
+
+	/// Constructor
+	inline					STLAllocator() = default;
+
+	/// Constructor from other allocator
+	template <typename T2>
+	inline					STLAllocator(const STLAllocator<T2> &) { }
+
+	/// If this allocator needs to fall back to aligned allocations because the type requires it
+	static constexpr bool	needs_aligned_allocate = alignof(T) > (JPH_CPU_ADDRESS_BITS == 32? 8 : 16);
+
+	/// Allocate memory
+	inline pointer			allocate(size_type inN)
+	{
+		if constexpr (needs_aligned_allocate)
+			return pointer(AlignedAllocate(inN * sizeof(value_type), alignof(T)));
+		else
+			return pointer(Allocate(inN * sizeof(value_type)));
+	}
+
+	/// Should we expose a reallocate function?
+	static constexpr bool	has_reallocate = std::is_trivially_copyable<T>() && !needs_aligned_allocate;
+
+	/// Reallocate memory
+	template <bool has_reallocate_v = has_reallocate, typename = std::enable_if_t<has_reallocate_v>>
+	inline pointer			reallocate(pointer inOldPointer, size_type inOldSize, size_type inNewSize)
+	{
+		JPH_ASSERT(inNewSize > 0); // Reallocating to zero size is implementation dependent, so we don't allow it
+		return pointer(Reallocate(inOldPointer, inOldSize * sizeof(value_type), inNewSize * sizeof(value_type)));
+	}
+
+	/// Free memory
+	inline void				deallocate(pointer inPointer, size_type)
+	{
+		if constexpr (needs_aligned_allocate)
+			AlignedFree(inPointer);
+		else
+			Free(inPointer);
+	}
+
+	/// Allocators are stateless so assumed to be equal
+	inline bool				operator == (const STLAllocator<T> &) const
+	{
+		return true;
+	}
+
+	inline bool				operator != (const STLAllocator<T> &) const
+	{
+		return false;
+	}
+
+	/// Converting to allocator for other type
+	template <typename T2>
+	struct rebind
+	{
+		using other = STLAllocator<T2>;
+	};
+};
+
+/// The STLAllocator implements the reallocate function if the alignment of the class is smaller or equal to the default alignment for the platform
+template <class T> struct AllocatorHasReallocate<STLAllocator<T>> { static constexpr bool sValue = STLAllocator<T>::has_reallocate; };
+
+#else
+
+template <typename T> using STLAllocator = std::allocator<T>;
+
+#endif // !JPH_DISABLE_CUSTOM_ALLOCATOR
+
+// Declare STL containers that use our allocator
+using String = std::basic_string<char, std::char_traits<char>, STLAllocator<char>>;
+using IStringStream = std::basic_istringstream<char, std::char_traits<char>, STLAllocator<char>>;
+
+JPH_NAMESPACE_END
+
+#if (!defined(JPH_PLATFORM_WINDOWS) || defined(JPH_COMPILER_MINGW)) && !defined(JPH_DISABLE_CUSTOM_ALLOCATOR)
+
+namespace std
+{
+	/// Declare std::hash for String, for some reason on Linux based platforms template deduction takes the wrong variant
+	template <>
+	struct hash<JPH::String>
+	{
+		inline size_t operator () (const JPH::String &inRHS) const
+		{
+			return hash<string_view> { } (inRHS);
+		}
+	};
+}
+
+#endif // (!JPH_PLATFORM_WINDOWS || JPH_COMPILER_MINGW) && !JPH_DISABLE_CUSTOM_ALLOCATOR
diff --git a/thirdparty/jolt_physics/Jolt/Core/STLTempAllocator.h b/thirdparty/jolt_physics/Jolt/Core/STLTempAllocator.h
new file mode 100644
index 0000000000..cf7c39d455
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/STLTempAllocator.h
@@ -0,0 +1,80 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/TempAllocator.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// STL allocator that wraps around TempAllocator
+template <typename T>
+class STLTempAllocator
+{
+public:
+	using value_type = T;
+
+	/// Pointer to type
+	using pointer = T *;
+	using const_pointer = const T *;
+
+	/// Reference to type.
+	/// Can be removed in C++20.
+	using reference = T &;
+	using const_reference = const T &;
+
+	using size_type = size_t;
+	using difference_type = ptrdiff_t;
+
+	/// The allocator is not stateless (depends on the temp allocator)
+	using is_always_equal = std::false_type;
+
+	/// Constructor
+	inline					STLTempAllocator(TempAllocator &inAllocator) : mAllocator(inAllocator) { }
+
+	/// Constructor from other allocator
+	template <typename T2>
+	inline explicit			STLTempAllocator(const STLTempAllocator<T2> &inRHS) : mAllocator(inRHS.GetAllocator()) { }
+
+	/// Allocate memory
+	inline pointer			allocate(size_type inN)
+	{
+		return pointer(mAllocator.Allocate(uint(inN * sizeof(value_type))));
+	}
+
+	/// Free memory
+	inline void				deallocate(pointer inPointer, size_type inN)
+	{
+		mAllocator.Free(inPointer, uint(inN * sizeof(value_type)));
+	}
+
+	/// Allocators are not-stateless, assume if allocator address matches that the allocators are the same
+	inline bool				operator == (const STLTempAllocator<T> &inRHS) const
+	{
+		return &mAllocator == &inRHS.mAllocator;
+	}
+
+	inline bool				operator != (const STLTempAllocator<T> &inRHS) const
+	{
+		return &mAllocator != &inRHS.mAllocator;
+	}
+
+	/// Converting to allocator for other type
+	template <typename T2>
+	struct rebind
+	{
+		using other = STLTempAllocator<T2>;
+	};
+
+	/// Get our temp allocator
+	TempAllocator &			GetAllocator() const
+	{
+		return mAllocator;
+	}
+
+private:
+	TempAllocator &			mAllocator;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/ScopeExit.h b/thirdparty/jolt_physics/Jolt/Core/ScopeExit.h
new file mode 100644
index 0000000000..6138384213
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/ScopeExit.h
@@ -0,0 +1,49 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/NonCopyable.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that calls a function when it goes out of scope
+template <class F>
+class ScopeExit : public NonCopyable
+{
+public:
+	/// Constructor specifies the exit function
+	JPH_INLINE explicit	ScopeExit(F &&inFunction) : mFunction(std::move(inFunction)) { }
+
+	/// Destructor calls the exit function
+	JPH_INLINE			~ScopeExit() { if (!mInvoked) mFunction(); }
+
+	/// Call the exit function now instead of when going out of scope
+	JPH_INLINE void		Invoke()
+	{
+		if (!mInvoked)
+		{
+			mFunction();
+			mInvoked = true;
+		}
+	}
+
+	/// No longer call the exit function when going out of scope
+	JPH_INLINE void		Release()
+	{
+		mInvoked = true;
+	}
+
+private:
+	F					mFunction;
+	bool				mInvoked = false;
+};
+
+#define JPH_SCOPE_EXIT_TAG2(line)			scope_exit##line
+#define JPH_SCOPE_EXIT_TAG(line)			JPH_SCOPE_EXIT_TAG2(line)
+
+/// Usage: JPH_SCOPE_EXIT([]{ code to call on scope exit });
+#define JPH_SCOPE_EXIT(...) ScopeExit JPH_SCOPE_EXIT_TAG(__LINE__)(__VA_ARGS__)
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/Semaphore.cpp b/thirdparty/jolt_physics/Jolt/Core/Semaphore.cpp
new file mode 100644
index 0000000000..27bb591bce
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/Semaphore.cpp
@@ -0,0 +1,82 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Core/Semaphore.h>
+
+#ifdef JPH_PLATFORM_WINDOWS
+	JPH_SUPPRESS_WARNING_PUSH
+	JPH_MSVC_SUPPRESS_WARNING(5039) // winbase.h(13179): warning C5039: 'TpSetCallbackCleanupGroup': pointer or reference to potentially throwing function passed to 'extern "C"' function under -EHc. Undefined behavior may occur if this function throws an exception.
+	#ifndef WIN32_LEAN_AND_MEAN
+		#define WIN32_LEAN_AND_MEAN
+	#endif
+#ifndef JPH_COMPILER_MINGW
+	#include <Windows.h>
+#else
+	#include <windows.h>
+#endif
+
+	JPH_SUPPRESS_WARNING_POP
+#endif
+
+JPH_NAMESPACE_BEGIN
+
+Semaphore::Semaphore()
+{
+#ifdef JPH_PLATFORM_WINDOWS
+	mSemaphore = CreateSemaphore(nullptr, 0, INT_MAX, nullptr);
+#endif
+}
+
+Semaphore::~Semaphore()
+{
+#ifdef JPH_PLATFORM_WINDOWS
+	CloseHandle(mSemaphore);
+#endif
+}
+
+void Semaphore::Release(uint inNumber)
+{
+	JPH_ASSERT(inNumber > 0);
+
+#ifdef JPH_PLATFORM_WINDOWS
+	int old_value = mCount.fetch_add(inNumber);
+	if (old_value < 0)
+	{
+		int new_value = old_value + (int)inNumber;
+		int num_to_release = min(new_value, 0) - old_value;
+		::ReleaseSemaphore(mSemaphore, num_to_release, nullptr);
+	}
+#else
+	std::lock_guard lock(mLock);
+	mCount.fetch_add(inNumber, std::memory_order_relaxed);
+	if (inNumber > 1)
+		mWaitVariable.notify_all();
+	else
+		mWaitVariable.notify_one();
+#endif
+}
+
+void Semaphore::Acquire(uint inNumber)
+{
+	JPH_ASSERT(inNumber > 0);
+
+#ifdef JPH_PLATFORM_WINDOWS
+	int old_value = mCount.fetch_sub(inNumber);
+	int new_value = old_value - (int)inNumber;
+	if (new_value < 0)
+	{
+		int num_to_acquire = min(old_value, 0) - new_value;
+		for (int i = 0; i < num_to_acquire; ++i)
+			WaitForSingleObject(mSemaphore, INFINITE);
+	}
+#else
+	std::unique_lock lock(mLock);
+	mCount.fetch_sub(inNumber, std::memory_order_relaxed);
+	mWaitVariable.wait(lock, [this]() { return mCount >= 0; });
+#endif
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/Semaphore.h b/thirdparty/jolt_physics/Jolt/Core/Semaphore.h
new file mode 100644
index 0000000000..091d7d5b0a
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/Semaphore.h
@@ -0,0 +1,51 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_SUPPRESS_WARNINGS_STD_BEGIN
+#include <atomic>
+#include <mutex>
+#include <condition_variable>
+JPH_SUPPRESS_WARNINGS_STD_END
+
+JPH_NAMESPACE_BEGIN
+
+// Things we're using from STL
+using std::atomic;
+using std::mutex;
+using std::condition_variable;
+
+/// Implements a semaphore
+/// When we switch to C++20 we can use counting_semaphore to unify this
+class JPH_EXPORT Semaphore
+{
+public:
+	/// Constructor
+						Semaphore();
+						~Semaphore();
+
+	/// Release the semaphore, signaling the thread waiting on the barrier that there may be work
+	void				Release(uint inNumber = 1);
+
+	/// Acquire the semaphore inNumber times
+	void				Acquire(uint inNumber = 1);
+
+	/// Get the current value of the semaphore
+	inline int			GetValue() const								{ return mCount.load(std::memory_order_relaxed); }
+
+private:
+#ifdef JPH_PLATFORM_WINDOWS
+	// On windows we use a semaphore object since it is more efficient than a lock and a condition variable
+	alignas(JPH_CACHE_LINE_SIZE) atomic<int> mCount { 0 };				///< We increment mCount for every release, to acquire we decrement the count. If the count is negative we know that we are waiting on the actual semaphore.
+	void *				mSemaphore;										///< The semaphore is an expensive construct so we only acquire/release it if we know that we need to wait/have waiting threads
+#else
+	// Other platforms: Emulate a semaphore using a mutex, condition variable and count
+	mutex				mLock;
+	condition_variable	mWaitVariable;
+	atomic<int>			mCount { 0 };
+#endif
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/StaticArray.h b/thirdparty/jolt_physics/Jolt/Core/StaticArray.h
new file mode 100644
index 0000000000..fddaaa37a2
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/StaticArray.h
@@ -0,0 +1,329 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/HashCombine.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Simple variable length array backed by a fixed size buffer
+template <class T, uint N>
+class [[nodiscard]] StaticArray
+{
+public:
+	using value_type = T;
+
+	using size_type = uint;
+
+	static constexpr uint Capacity = N;
+
+	/// Default constructor
+						StaticArray() = default;
+
+	/// Constructor from initializer list
+	explicit			StaticArray(std::initializer_list<T> inList)
+	{
+		JPH_ASSERT(inList.size() <= N);
+		for (const T &v : inList)
+			::new (reinterpret_cast<T *>(&mElements[mSize++])) T(v);
+	}
+
+	/// Copy constructor
+						StaticArray(const StaticArray<T, N> &inRHS)
+	{
+		while (mSize < inRHS.mSize)
+		{
+			::new (&mElements[mSize]) T(inRHS[mSize]);
+			++mSize;
+		}
+	}
+
+	/// Destruct all elements
+						~StaticArray()
+	{
+		if constexpr (!std::is_trivially_destructible<T>())
+			for (T *e = reinterpret_cast<T *>(mElements), *end = e + mSize; e < end; ++e)
+				e->~T();
+	}
+
+	/// Destruct all elements and set length to zero
+	void				clear()
+	{
+		if constexpr (!std::is_trivially_destructible<T>())
+			for (T *e = reinterpret_cast<T *>(mElements), *end = e + mSize; e < end; ++e)
+				e->~T();
+		mSize = 0;
+	}
+
+	/// Add element to the back of the array
+	void				push_back(const T &inElement)
+	{
+		JPH_ASSERT(mSize < N);
+		::new (&mElements[mSize++]) T(inElement);
+	}
+
+	/// Construct element at the back of the array
+	template <class... A>
+	void				emplace_back(A &&... inElement)
+	{
+		JPH_ASSERT(mSize < N);
+		::new (&mElements[mSize++]) T(std::forward<A>(inElement)...);
+	}
+
+	/// Remove element from the back of the array
+	void				pop_back()
+	{
+		JPH_ASSERT(mSize > 0);
+		reinterpret_cast<T &>(mElements[--mSize]).~T();
+	}
+
+	/// Returns true if there are no elements in the array
+	bool				empty() const
+	{
+		return mSize == 0;
+	}
+
+	/// Returns amount of elements in the array
+	size_type			size() const
+	{
+		return mSize;
+	}
+
+	/// Returns maximum amount of elements the array can hold
+	size_type			capacity() const
+	{
+		return N;
+	}
+
+	/// Resize array to new length
+	void				resize(size_type inNewSize)
+	{
+		JPH_ASSERT(inNewSize <= N);
+		if constexpr (!std::is_trivially_constructible<T>())
+			for (T *element = reinterpret_cast<T *>(mElements) + mSize, *element_end = reinterpret_cast<T *>(mElements) + inNewSize; element < element_end; ++element)
+				::new (element) T;
+		if constexpr (!std::is_trivially_destructible<T>())
+			for (T *element = reinterpret_cast<T *>(mElements) + inNewSize, *element_end = reinterpret_cast<T *>(mElements) + mSize; element < element_end; ++element)
+				element->~T();
+		mSize = inNewSize;
+	}
+
+	using const_iterator = const T *;
+
+	/// Iterators
+	const_iterator		begin() const
+	{
+		return reinterpret_cast<const T *>(mElements);
+	}
+
+	const_iterator		end() const
+	{
+		return reinterpret_cast<const T *>(mElements + mSize);
+	}
+
+	using iterator = T *;
+
+	iterator			begin()
+	{
+		return reinterpret_cast<T *>(mElements);
+	}
+
+	iterator			end()
+	{
+		return reinterpret_cast<T *>(mElements + mSize);
+	}
+
+	const T *			data() const
+	{
+		return reinterpret_cast<const T *>(mElements);
+	}
+
+	T *					data()
+	{
+		return reinterpret_cast<T *>(mElements);
+	}
+
+	/// Access element
+	T &					operator [] (size_type inIdx)
+	{
+		JPH_ASSERT(inIdx < mSize);
+		return reinterpret_cast<T &>(mElements[inIdx]);
+	}
+
+	const T &			operator [] (size_type inIdx) const
+	{
+		JPH_ASSERT(inIdx < mSize);
+		return reinterpret_cast<const T &>(mElements[inIdx]);
+	}
+
+	/// Access element
+	T &					at(size_type inIdx)
+	{
+		JPH_ASSERT(inIdx < mSize);
+		return reinterpret_cast<T &>(mElements[inIdx]);
+	}
+
+	const T &			at(size_type inIdx) const
+	{
+		JPH_ASSERT(inIdx < mSize);
+		return reinterpret_cast<const T &>(mElements[inIdx]);
+	}
+
+	/// First element in the array
+	const T &			front() const
+	{
+		JPH_ASSERT(mSize > 0);
+		return reinterpret_cast<const T &>(mElements[0]);
+	}
+
+	T &					front()
+	{
+		JPH_ASSERT(mSize > 0);
+		return reinterpret_cast<T &>(mElements[0]);
+	}
+
+	/// Last element in the array
+	const T &			back() const
+	{
+		JPH_ASSERT(mSize > 0);
+		return reinterpret_cast<const T &>(mElements[mSize - 1]);
+	}
+
+	T &					back()
+	{
+		JPH_ASSERT(mSize > 0);
+		return reinterpret_cast<T &>(mElements[mSize - 1]);
+	}
+
+	/// Remove one element from the array
+	void				erase(const_iterator inIter)
+	{
+		size_type p = size_type(inIter - begin());
+		JPH_ASSERT(p < mSize);
+		reinterpret_cast<T &>(mElements[p]).~T();
+		if (p + 1 < mSize)
+			memmove(mElements + p, mElements + p + 1, (mSize - p - 1) * sizeof(T));
+		--mSize;
+	}
+
+	/// Remove multiple element from the array
+	void				erase(const_iterator inBegin, const_iterator inEnd)
+	{
+		size_type p = size_type(inBegin - begin());
+		size_type n = size_type(inEnd - inBegin);
+		JPH_ASSERT(inEnd <= end());
+		for (size_type i = 0; i < n; ++i)
+			reinterpret_cast<T &>(mElements[p + i]).~T();
+		if (p + n < mSize)
+			memmove(mElements + p, mElements + p + n, (mSize - p - n) * sizeof(T));
+		mSize -= n;
+	}
+
+	/// Assignment operator
+	StaticArray<T, N> &	operator = (const StaticArray<T, N> &inRHS)
+	{
+		size_type rhs_size = inRHS.size();
+
+		if (static_cast<const void *>(this) != static_cast<const void *>(&inRHS))
+		{
+			clear();
+
+			while (mSize < rhs_size)
+			{
+				::new (&mElements[mSize]) T(inRHS[mSize]);
+				++mSize;
+			}
+		}
+
+		return *this;
+	}
+
+	/// Assignment operator with static array of different max length
+	template <uint M>
+	StaticArray<T, N> &	operator = (const StaticArray<T, M> &inRHS)
+	{
+		size_type rhs_size = inRHS.size();
+		JPH_ASSERT(rhs_size <= N);
+
+		if (static_cast<const void *>(this) != static_cast<const void *>(&inRHS))
+		{
+			clear();
+
+			while (mSize < rhs_size)
+			{
+				::new (&mElements[mSize]) T(inRHS[mSize]);
+				++mSize;
+			}
+		}
+
+		return *this;
+	}
+
+	/// Comparing arrays
+	bool				operator == (const StaticArray<T, N> &inRHS) const
+	{
+		if (mSize != inRHS.mSize)
+			return false;
+		for (size_type i = 0; i < mSize; ++i)
+			if (!(reinterpret_cast<const T &>(mElements[i]) == reinterpret_cast<const T &>(inRHS.mElements[i])))
+				return false;
+		return true;
+	}
+
+	bool				operator != (const StaticArray<T, N> &inRHS) const
+	{
+		if (mSize != inRHS.mSize)
+			return true;
+		for (size_type i = 0; i < mSize; ++i)
+			if (reinterpret_cast<const T &>(mElements[i]) != reinterpret_cast<const T &>(inRHS.mElements[i]))
+				return true;
+		return false;
+	}
+
+	/// Get hash for this array
+	uint64					GetHash() const
+	{
+		// Hash length first
+		uint64 ret = Hash<uint32> { } (uint32(size()));
+
+		// Then hash elements
+		for (const T *element = reinterpret_cast<const T *>(mElements), *element_end = reinterpret_cast<const T *>(mElements) + mSize; element < element_end; ++element)
+			HashCombine(ret, *element);
+
+		return ret;
+	}
+
+protected:
+	struct alignas(T) Storage
+	{
+		uint8			mData[sizeof(T)];
+	};
+
+	static_assert(sizeof(T) == sizeof(Storage), "Mismatch in size");
+	static_assert(alignof(T) == alignof(Storage), "Mismatch in alignment");
+
+	size_type			mSize = 0;
+	Storage				mElements[N];
+};
+
+JPH_NAMESPACE_END
+
+JPH_SUPPRESS_WARNING_PUSH
+JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat")
+
+namespace std
+{
+	/// Declare std::hash for StaticArray
+	template <class T, JPH::uint N>
+	struct hash<JPH::StaticArray<T, N>>
+	{
+		size_t operator () (const JPH::StaticArray<T, N> &inRHS) const
+		{
+			return std::size_t(inRHS.GetHash());
+		}
+	};
+}
+
+JPH_SUPPRESS_WARNING_POP
diff --git a/thirdparty/jolt_physics/Jolt/Core/StreamIn.h b/thirdparty/jolt_physics/Jolt/Core/StreamIn.h
new file mode 100644
index 0000000000..4de299e6d1
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/StreamIn.h
@@ -0,0 +1,119 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/NonCopyable.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Simple binary input stream
+class JPH_EXPORT StreamIn : public NonCopyable
+{
+public:
+	/// Virtual destructor
+	virtual				~StreamIn() = default;
+
+	/// Read a string of bytes from the binary stream
+	virtual void		ReadBytes(void *outData, size_t inNumBytes) = 0;
+
+	/// Returns true when an attempt has been made to read past the end of the file
+	virtual bool		IsEOF() const = 0;
+
+	/// Returns true if there was an IO failure
+	virtual bool		IsFailed() const = 0;
+
+	/// Read a primitive (e.g. float, int, etc.) from the binary stream
+	template <class T, std::enable_if_t<std::is_trivially_copyable_v<T>, bool> = true>
+	void				Read(T &outT)
+	{
+		ReadBytes(&outT, sizeof(outT));
+	}
+
+	/// Read a vector of primitives from the binary stream
+	template <class T, class A, std::enable_if_t<std::is_trivially_copyable_v<T>, bool> = true>
+	void				Read(Array<T, A> &outT)
+	{
+		uint32 len = uint32(outT.size()); // Initialize to previous array size, this is used for validation in the StateRecorder class
+		Read(len);
+		if (!IsEOF() && !IsFailed())
+		{
+			outT.resize(len);
+			if constexpr (std::is_same_v<T, Vec3> || std::is_same_v<T, DVec3> || std::is_same_v<T, DMat44>)
+			{
+				// These types have unused components that we don't want to read
+				for (typename Array<T, A>::size_type i = 0; i < len; ++i)
+					Read(outT[i]);
+			}
+			else
+			{
+				// Read all elements at once
+				ReadBytes(outT.data(), len * sizeof(T));
+			}
+		}
+		else
+			outT.clear();
+	}
+
+	/// Read a string from the binary stream (reads the number of characters and then the characters)
+	template <class Type, class Traits, class Allocator>
+	void				Read(std::basic_string<Type, Traits, Allocator> &outString)
+	{
+		uint32 len = 0;
+		Read(len);
+		if (!IsEOF() && !IsFailed())
+		{
+			outString.resize(len);
+			ReadBytes(outString.data(), len * sizeof(Type));
+		}
+		else
+			outString.clear();
+	}
+
+	/// Read a vector of primitives from the binary stream using a custom function to read the elements
+	template <class T, class A, typename F>
+	void				Read(Array<T, A> &outT, const F &inReadElement)
+	{
+		uint32 len = uint32(outT.size()); // Initialize to previous array size, this is used for validation in the StateRecorder class
+		Read(len);
+		if (!IsEOF() && !IsFailed())
+		{
+			outT.resize(len);
+			for (typename Array<T, A>::size_type i = 0; i < len; ++i)
+				inReadElement(*this, outT[i]);
+		}
+		else
+			outT.clear();
+	}
+
+	/// Read a Vec3 (don't read W)
+	void				Read(Vec3 &outVec)
+	{
+		ReadBytes(&outVec, 3 * sizeof(float));
+		outVec = Vec3::sFixW(outVec.mValue);
+	}
+
+	/// Read a DVec3 (don't read W)
+	void				Read(DVec3 &outVec)
+	{
+		ReadBytes(&outVec, 3 * sizeof(double));
+		outVec = DVec3::sFixW(outVec.mValue);
+	}
+
+	/// Read a DMat44 (don't read W component of translation)
+	void				Read(DMat44 &outVec)
+	{
+		Vec4 x, y, z;
+		Read(x);
+		Read(y);
+		Read(z);
+
+		DVec3 t;
+		Read(t);
+
+		outVec = DMat44(x, y, z, t);
+	}
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/StreamOut.h b/thirdparty/jolt_physics/Jolt/Core/StreamOut.h
new file mode 100644
index 0000000000..566f8ec8da
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/StreamOut.h
@@ -0,0 +1,97 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/NonCopyable.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Simple binary output stream
+class JPH_EXPORT StreamOut : public NonCopyable
+{
+public:
+	/// Virtual destructor
+	virtual				~StreamOut() = default;
+
+	/// Write a string of bytes to the binary stream
+	virtual void		WriteBytes(const void *inData, size_t inNumBytes) = 0;
+
+	/// Returns true if there was an IO failure
+	virtual bool		IsFailed() const = 0;
+
+	/// Write a primitive (e.g. float, int, etc.) to the binary stream
+	template <class T, std::enable_if_t<std::is_trivially_copyable_v<T>, bool> = true>
+	void				Write(const T &inT)
+	{
+		WriteBytes(&inT, sizeof(inT));
+	}
+
+	/// Write a vector of primitives to the binary stream
+	template <class T, class A, std::enable_if_t<std::is_trivially_copyable_v<T>, bool> = true>
+	void				Write(const Array<T, A> &inT)
+	{
+		uint32 len = uint32(inT.size());
+		Write(len);
+		if (!IsFailed())
+		{
+			if constexpr (std::is_same_v<T, Vec3> || std::is_same_v<T, DVec3> || std::is_same_v<T, DMat44>)
+			{
+				// These types have unused components that we don't want to write
+				for (typename Array<T, A>::size_type i = 0; i < len; ++i)
+					Write(inT[i]);
+			}
+			else
+			{
+				// Write all elements at once
+				WriteBytes(inT.data(), len * sizeof(T));
+			}
+		}
+	}
+
+	/// Write a string to the binary stream (writes the number of characters and then the characters)
+	template <class Type, class Traits, class Allocator>
+	void				Write(const std::basic_string<Type, Traits, Allocator> &inString)
+	{
+		uint32 len = uint32(inString.size());
+		Write(len);
+		if (!IsFailed())
+			WriteBytes(inString.data(), len * sizeof(Type));
+	}
+
+	/// Write a vector of primitives to the binary stream using a custom write function
+	template <class T, class A, typename F>
+	void				Write(const Array<T, A> &inT, const F &inWriteElement)
+	{
+		uint32 len = uint32(inT.size());
+		Write(len);
+		if (!IsFailed())
+			for (typename Array<T, A>::size_type i = 0; i < len; ++i)
+				inWriteElement(inT[i], *this);
+	}
+
+	/// Write a Vec3 (don't write W)
+	void				Write(const Vec3 &inVec)
+	{
+		WriteBytes(&inVec, 3 * sizeof(float));
+	}
+
+	/// Write a DVec3 (don't write W)
+	void				Write(const DVec3 &inVec)
+	{
+		WriteBytes(&inVec, 3 * sizeof(double));
+	}
+
+	/// Write a DMat44 (don't write W component of translation)
+	void				Write(const DMat44 &inVec)
+	{
+		Write(inVec.GetColumn4(0));
+		Write(inVec.GetColumn4(1));
+		Write(inVec.GetColumn4(2));
+
+		Write(inVec.GetTranslation());
+	}
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/StreamUtils.h b/thirdparty/jolt_physics/Jolt/Core/StreamUtils.h
new file mode 100644
index 0000000000..1feb232f35
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/StreamUtils.h
@@ -0,0 +1,168 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/Result.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/Core/UnorderedMap.h>
+#include <Jolt/Core/Factory.h>
+
+JPH_NAMESPACE_BEGIN
+
+namespace StreamUtils {
+
+template <class Type>
+using ObjectToIDMap = UnorderedMap<const Type *, uint32>;
+
+template <class Type>
+using IDToObjectMap = Array<Ref<Type>>;
+
+// Restore a single object by reading the hash of the type, constructing it and then calling the restore function
+template <class Type>
+Result<Ref<Type>>	RestoreObject(StreamIn &inStream, void (Type::*inRestoreBinaryStateFunction)(StreamIn &))
+{
+	Result<Ref<Type>> result;
+
+	// Read the hash of the type
+	uint32 hash;
+	inStream.Read(hash);
+	if (inStream.IsEOF() || inStream.IsFailed())
+	{
+		result.SetError("Failed to read type hash");
+		return result;
+	}
+
+	// Get the RTTI for the type
+	const RTTI *rtti = Factory::sInstance->Find(hash);
+	if (rtti == nullptr)
+	{
+		result.SetError("Failed to create instance of type");
+		return result;
+	}
+
+	// Construct and read the data of the type
+	Ref<Type> object = reinterpret_cast<Type *>(rtti->CreateObject());
+	(object->*inRestoreBinaryStateFunction)(inStream);
+	if (inStream.IsEOF() || inStream.IsFailed())
+	{
+		result.SetError("Failed to restore object");
+		return result;
+	}
+
+	result.Set(object);
+	return result;
+}
+
+/// Save an object reference to a stream. Uses a map to map objects to IDs which is also used to prevent writing duplicates.
+template <class Type>
+void				SaveObjectReference(StreamOut &inStream, const Type *inObject, ObjectToIDMap<Type> *ioObjectToIDMap)
+{
+	if (ioObjectToIDMap == nullptr || inObject == nullptr)
+	{
+		// Write null ID
+		inStream.Write(~uint32(0));
+	}
+	else
+	{
+		typename ObjectToIDMap<Type>::const_iterator id = ioObjectToIDMap->find(inObject);
+		if (id != ioObjectToIDMap->end())
+		{
+			// Existing object, write ID
+			inStream.Write(id->second);
+		}
+		else
+		{
+			// New object, write the ID
+			uint32 new_id = uint32(ioObjectToIDMap->size());
+			(*ioObjectToIDMap)[inObject] = new_id;
+			inStream.Write(new_id);
+
+			// Write the object
+			inObject->SaveBinaryState(inStream);
+		}
+	}
+}
+
+/// Restore an object reference from stream.
+template <class Type>
+Result<Ref<Type>>	RestoreObjectReference(StreamIn &inStream, IDToObjectMap<Type> &ioIDToObjectMap)
+{
+	Result<Ref<Type>> result;
+
+	// Read id
+	uint32 id = ~uint32(0);
+	inStream.Read(id);
+
+	// Check null
+	if (id == ~uint32(0))
+	{
+		result.Set(nullptr);
+		return result;
+	}
+
+	// Check if it already exists
+	if (id >= ioIDToObjectMap.size())
+	{
+		// New object, restore it
+		result = Type::sRestoreFromBinaryState(inStream);
+		if (result.HasError())
+			return result;
+		JPH_ASSERT(id == ioIDToObjectMap.size());
+		ioIDToObjectMap.push_back(result.Get());
+	}
+	else
+	{
+		// Existing object filter
+		result.Set(ioIDToObjectMap[id].GetPtr());
+	}
+
+	return result;
+}
+
+// Save an array of objects to a stream.
+template <class ArrayType, class ValueType>
+void				SaveObjectArray(StreamOut &inStream, const ArrayType &inArray, ObjectToIDMap<ValueType> *ioObjectToIDMap)
+{
+	uint32 len = uint32(inArray.size());
+	inStream.Write(len);
+	for (const ValueType *value: inArray)
+		SaveObjectReference(inStream, value, ioObjectToIDMap);
+}
+
+// Restore an array of objects from a stream.
+template <class ArrayType, class ValueType>
+Result<ArrayType>	RestoreObjectArray(StreamIn &inStream, IDToObjectMap<ValueType> &ioIDToObjectMap)
+{
+	Result<ArrayType> result;
+
+	uint32 len;
+	inStream.Read(len);
+	if (inStream.IsEOF() || inStream.IsFailed())
+	{
+		result.SetError("Failed to read stream");
+		return result;
+	}
+
+	ArrayType values;
+	values.reserve(len);
+	for (size_t i = 0; i < len; ++i)
+	{
+		Result value = RestoreObjectReference(inStream, ioIDToObjectMap);
+		if (value.HasError())
+		{
+			result.SetError(value.GetError());
+			return result;
+		}
+		values.push_back(std::move(value.Get()));
+	}
+
+	result.Set(values);
+	return result;
+}
+
+} // StreamUtils
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/StreamWrapper.h b/thirdparty/jolt_physics/Jolt/Core/StreamWrapper.h
new file mode 100644
index 0000000000..66a36b0571
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/StreamWrapper.h
@@ -0,0 +1,53 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_SUPPRESS_WARNINGS_STD_BEGIN
+#include <ostream>
+JPH_SUPPRESS_WARNINGS_STD_END
+
+JPH_NAMESPACE_BEGIN
+
+/// Wrapper around std::ostream
+class StreamOutWrapper : public StreamOut
+{
+public:
+	/// Constructor
+						StreamOutWrapper(ostream &ioWrapped)						: mWrapped(ioWrapped) { }
+
+	/// Write a string of bytes to the binary stream
+	virtual void		WriteBytes(const void *inData, size_t inNumBytes) override	{ mWrapped.write((const char *)inData, inNumBytes); }
+
+	/// Returns true if there was an IO failure
+	virtual bool		IsFailed() const override									{ return mWrapped.fail(); }
+
+private:
+	ostream &			mWrapped;
+};
+
+/// Wrapper around std::istream
+class StreamInWrapper : public StreamIn
+{
+public:
+	/// Constructor
+						StreamInWrapper(istream &ioWrapped)							: mWrapped(ioWrapped) { }
+
+	/// Write a string of bytes to the binary stream
+	virtual void		ReadBytes(void *outData, size_t inNumBytes) override		{ mWrapped.read((char *)outData, inNumBytes); }
+
+	/// Returns true when an attempt has been made to read past the end of the file
+	virtual bool		IsEOF() const override										{ return mWrapped.eof(); }
+
+	/// Returns true if there was an IO failure
+	virtual bool		IsFailed() const override									{ return mWrapped.fail(); }
+
+private:
+	istream &			mWrapped;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/StridedPtr.h b/thirdparty/jolt_physics/Jolt/Core/StridedPtr.h
new file mode 100644
index 0000000000..1cf97fa678
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/StridedPtr.h
@@ -0,0 +1,63 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// A strided pointer behaves exactly like a normal pointer except that the
+/// elements that the pointer points to can be part of a larger structure.
+/// The stride gives the number of bytes from one element to the next.
+template <class T>
+class JPH_EXPORT StridedPtr
+{
+public:
+	using value_type = T;
+
+	/// Constructors
+							StridedPtr() = default;
+							StridedPtr(const StridedPtr &inRHS) = default;
+							StridedPtr(T *inPtr, int inStride = sizeof(T))			: mPtr(const_cast<uint8 *>(reinterpret_cast<const uint8 *>(inPtr))), mStride(inStride) { }
+
+	/// Assignment
+	inline StridedPtr &		operator = (const StridedPtr &inRHS) = default;
+
+	/// Incrementing / decrementing
+	inline StridedPtr &		operator ++ ()											{ mPtr += mStride; return *this; }
+	inline StridedPtr &		operator -- ()											{ mPtr -= mStride; return *this; }
+	inline StridedPtr		operator ++ (int)										{ StridedPtr old_ptr(*this); mPtr += mStride; return old_ptr; }
+	inline StridedPtr		operator -- (int)										{ StridedPtr old_ptr(*this); mPtr -= mStride; return old_ptr; }
+	inline StridedPtr		operator + (int inOffset) const							{ StridedPtr new_ptr(*this); new_ptr.mPtr += inOffset * mStride; return new_ptr; }
+	inline StridedPtr		operator - (int inOffset) const							{ StridedPtr new_ptr(*this); new_ptr.mPtr -= inOffset * mStride; return new_ptr; }
+	inline void				operator += (int inOffset)								{ mPtr += inOffset * mStride; }
+	inline void				operator -= (int inOffset)								{ mPtr -= inOffset * mStride; }
+
+	/// Distance between two pointers in elements
+	inline int				operator - (const StridedPtr &inRHS) const				{ JPH_ASSERT(inRHS.mStride == mStride); return (mPtr - inRHS.mPtr) / mStride; }
+
+	/// Comparison operators
+	inline bool				operator == (const StridedPtr &inRHS) const				{ return mPtr == inRHS.mPtr; }
+	inline bool				operator != (const StridedPtr &inRHS) const				{ return mPtr != inRHS.mPtr; }
+	inline bool				operator <= (const StridedPtr &inRHS) const				{ return mPtr <= inRHS.mPtr; }
+	inline bool				operator >= (const StridedPtr &inRHS) const				{ return mPtr >= inRHS.mPtr; }
+	inline bool				operator <  (const StridedPtr &inRHS) const				{ return mPtr <  inRHS.mPtr; }
+	inline bool				operator >  (const StridedPtr &inRHS) const				{ return mPtr >  inRHS.mPtr; }
+
+	/// Access value
+	inline T &				operator * () const										{ return *reinterpret_cast<T *>(mPtr); }
+	inline T *				operator -> () const									{ return reinterpret_cast<T *>(mPtr); }
+	inline T &				operator [] (int inOffset) const						{ uint8 *ptr = mPtr + inOffset * mStride; return *reinterpret_cast<T *>(ptr); }
+
+	/// Explicit conversion
+	inline T *				GetPtr() const											{ return reinterpret_cast<T *>(mPtr); }
+
+	/// Get stride in bytes
+	inline int				GetStride() const										{ return mStride; }
+
+private:
+	uint8 *					mPtr = nullptr;											/// Pointer to element
+	int						mStride = 0;											/// Stride (number of bytes) between elements
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/StringTools.cpp b/thirdparty/jolt_physics/Jolt/Core/StringTools.cpp
new file mode 100644
index 0000000000..6eae982cc6
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/StringTools.cpp
@@ -0,0 +1,101 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Core/StringTools.h>
+
+JPH_SUPPRESS_WARNINGS_STD_BEGIN
+#include <cstdarg>
+JPH_SUPPRESS_WARNINGS_STD_END
+
+JPH_NAMESPACE_BEGIN
+
+String StringFormat(const char *inFMT, ...)
+{
+	char buffer[1024];
+
+	// Format the string
+	va_list list;
+	va_start(list, inFMT);
+	vsnprintf(buffer, sizeof(buffer), inFMT, list);
+	va_end(list);
+
+	return String(buffer);
+}
+
+void StringReplace(String &ioString, const string_view &inSearch, const string_view &inReplace)
+{
+	size_t index = 0;
+	for (;;)
+	{
+		 index = ioString.find(inSearch, index);
+		 if (index == String::npos)
+			 break;
+
+		 ioString.replace(index, inSearch.size(), inReplace);
+
+		 index += inReplace.size();
+	}
+}
+
+void StringToVector(const string_view &inString, Array<String> &outVector, const string_view &inDelimiter, bool inClearVector)
+{
+	JPH_ASSERT(inDelimiter.size() > 0);
+
+	// Ensure vector empty
+	if (inClearVector)
+		outVector.clear();
+
+	// No string? no elements
+	if (inString.empty())
+		return;
+
+	// Start with initial string
+	String s(inString);
+
+	// Add to vector while we have a delimiter
+	size_t i;
+	while (!s.empty() && (i = s.find(inDelimiter)) != String::npos)
+	{
+		outVector.push_back(s.substr(0, i));
+		s.erase(0, i + inDelimiter.length());
+	}
+
+	// Add final element
+	outVector.push_back(s);
+}
+
+void VectorToString(const Array<String> &inVector, String &outString, const string_view &inDelimiter)
+{
+	// Ensure string empty
+	outString.clear();
+
+	for (const String &s : inVector)
+	{
+		// Add delimiter if not first element
+		if (!outString.empty())
+			outString.append(inDelimiter);
+
+		// Add element
+		outString.append(s);
+	}
+}
+
+String ToLower(const string_view &inString)
+{
+	String out;
+	out.reserve(inString.length());
+	for (char c : inString)
+		out.push_back((char)tolower(c));
+	return out;
+}
+
+const char *NibbleToBinary(uint32 inNibble)
+{
+	static const char *nibbles[] = { "0000", "0001", "0010", "0011", "0100", "0101", "0110", "0111", "1000", "1001", "1010", "1011", "1100", "1101", "1110", "1111" };
+	return nibbles[inNibble & 0xf];
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/StringTools.h b/thirdparty/jolt_physics/Jolt/Core/StringTools.h
new file mode 100644
index 0000000000..378278b15d
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/StringTools.h
@@ -0,0 +1,38 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Create a formatted text string for debugging purposes.
+/// Note that this function has an internal buffer of 1024 characters, so long strings will be trimmed.
+JPH_EXPORT String StringFormat(const char *inFMT, ...);
+
+/// Convert type to string
+template<typename T>
+String ConvertToString(const T &inValue)
+{
+	using OStringStream = std::basic_ostringstream<char, std::char_traits<char>, STLAllocator<char>>;
+	OStringStream oss;
+	oss << inValue;
+	return oss.str();
+}
+
+/// Replace substring with other string
+JPH_EXPORT void StringReplace(String &ioString, const string_view &inSearch, const string_view &inReplace);
+
+/// Convert a delimited string to an array of strings
+JPH_EXPORT void StringToVector(const string_view &inString, Array<String> &outVector, const string_view &inDelimiter = ",", bool inClearVector = true);
+
+/// Convert an array strings to a delimited string
+JPH_EXPORT void VectorToString(const Array<String> &inVector, String &outString, const string_view &inDelimiter = ",");
+
+/// Convert a string to lower case
+JPH_EXPORT String ToLower(const string_view &inString);
+
+/// Converts the lower 4 bits of inNibble to a string that represents the number in binary format
+JPH_EXPORT const char *NibbleToBinary(uint32 inNibble);
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/TempAllocator.h b/thirdparty/jolt_physics/Jolt/Core/TempAllocator.h
new file mode 100644
index 0000000000..99586bde64
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/TempAllocator.h
@@ -0,0 +1,188 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/NonCopyable.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Allocator for temporary allocations.
+/// This allocator works as a stack: The blocks must always be freed in the reverse order as they are allocated.
+/// Note that allocations and frees can take place from different threads, but the order is guaranteed though
+/// job dependencies, so it is not needed to use any form of locking.
+class JPH_EXPORT TempAllocator : public NonCopyable
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Destructor
+	virtual							~TempAllocator() = default;
+
+	/// Allocates inSize bytes of memory, returned memory address must be JPH_RVECTOR_ALIGNMENT byte aligned
+	virtual void *					Allocate(uint inSize) = 0;
+
+	/// Frees inSize bytes of memory located at inAddress
+	virtual void					Free(void *inAddress, uint inSize) = 0;
+};
+
+/// Default implementation of the temp allocator that allocates a large block through malloc upfront
+class JPH_EXPORT TempAllocatorImpl final : public TempAllocator
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructs the allocator with a maximum allocatable size of inSize
+	explicit						TempAllocatorImpl(uint inSize) :
+		mBase(static_cast<uint8 *>(AlignedAllocate(inSize, JPH_RVECTOR_ALIGNMENT))),
+		mSize(inSize)
+	{
+	}
+
+	/// Destructor, frees the block
+	virtual							~TempAllocatorImpl() override
+	{
+		JPH_ASSERT(mTop == 0);
+		AlignedFree(mBase);
+	}
+
+	// See: TempAllocator
+	virtual void *					Allocate(uint inSize) override
+	{
+		if (inSize == 0)
+		{
+			return nullptr;
+		}
+		else
+		{
+			uint new_top = mTop + AlignUp(inSize, JPH_RVECTOR_ALIGNMENT);
+			if (new_top > mSize)
+			{
+				Trace("TempAllocator: Out of memory");
+				std::abort();
+			}
+			void *address = mBase + mTop;
+			mTop = new_top;
+			return address;
+		}
+	}
+
+	// See: TempAllocator
+	virtual void					Free(void *inAddress, uint inSize) override
+	{
+		if (inAddress == nullptr)
+		{
+			JPH_ASSERT(inSize == 0);
+		}
+		else
+		{
+			mTop -= AlignUp(inSize, JPH_RVECTOR_ALIGNMENT);
+			if (mBase + mTop != inAddress)
+			{
+				Trace("TempAllocator: Freeing in the wrong order");
+				std::abort();
+			}
+		}
+	}
+
+	/// Check if no allocations have been made
+	bool							IsEmpty() const
+	{
+		return mTop == 0;
+	}
+
+	/// Get the total size of the fixed buffer
+	uint							GetSize() const
+	{
+		return mSize;
+	}
+
+	/// Get current usage in bytes of the buffer
+	uint							GetUsage() const
+	{
+		return mTop;
+	}
+
+	/// Check if an allocation of inSize can be made in this fixed buffer allocator
+	bool							CanAllocate(uint inSize) const
+	{
+		return mTop + AlignUp(inSize, JPH_RVECTOR_ALIGNMENT) <= mSize;
+	}
+
+	/// Check if memory block at inAddress is owned by this allocator
+	bool							OwnsMemory(const void *inAddress) const
+	{
+		return inAddress >= mBase && inAddress < mBase + mSize;
+	}
+
+private:
+	uint8 *							mBase;							///< Base address of the memory block
+	uint							mSize;							///< Size of the memory block
+	uint							mTop = 0;						///< End of currently allocated area
+};
+
+/// Implementation of the TempAllocator that just falls back to malloc/free
+/// Note: This can be quite slow when running in the debugger as large memory blocks need to be initialized with 0xcd
+class JPH_EXPORT TempAllocatorMalloc final : public TempAllocator
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	// See: TempAllocator
+	virtual void *					Allocate(uint inSize) override
+	{
+		return inSize > 0? AlignedAllocate(inSize, JPH_RVECTOR_ALIGNMENT) : nullptr;
+	}
+
+	// See: TempAllocator
+	virtual void					Free(void *inAddress, [[maybe_unused]] uint inSize) override
+	{
+		if (inAddress != nullptr)
+			AlignedFree(inAddress);
+	}
+};
+
+/// Implementation of the TempAllocator that tries to allocate from a large preallocated block, but falls back to malloc when it is exhausted
+class JPH_EXPORT TempAllocatorImplWithMallocFallback final : public TempAllocator
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructs the allocator with an initial fixed block if inSize
+	explicit						TempAllocatorImplWithMallocFallback(uint inSize) :
+		mAllocator(inSize)
+	{
+	}
+
+	// See: TempAllocator
+	virtual void *					Allocate(uint inSize) override
+	{
+		if (mAllocator.CanAllocate(inSize))
+			return mAllocator.Allocate(inSize);
+		else
+			return mFallbackAllocator.Allocate(inSize);
+	}
+
+	// See: TempAllocator
+	virtual void					Free(void *inAddress, uint inSize) override
+	{
+		if (inAddress == nullptr)
+		{
+			JPH_ASSERT(inSize == 0);
+		}
+		else
+		{
+			if (mAllocator.OwnsMemory(inAddress))
+				mAllocator.Free(inAddress, inSize);
+			else
+				mFallbackAllocator.Free(inAddress, inSize);
+		}
+	}
+
+private:
+	TempAllocatorImpl				mAllocator;
+	TempAllocatorMalloc				mFallbackAllocator;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/TickCounter.cpp b/thirdparty/jolt_physics/Jolt/Core/TickCounter.cpp
new file mode 100644
index 0000000000..d08140091e
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/TickCounter.cpp
@@ -0,0 +1,36 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Core/TickCounter.h>
+
+#if defined(JPH_PLATFORM_WINDOWS)
+	JPH_SUPPRESS_WARNING_PUSH
+	JPH_MSVC_SUPPRESS_WARNING(5039) // winbase.h(13179): warning C5039: 'TpSetCallbackCleanupGroup': pointer or reference to potentially throwing function passed to 'extern "C"' function under -EHc. Undefined behavior may occur if this function throws an exception.
+	#ifndef WIN32_LEAN_AND_MEAN
+		#define WIN32_LEAN_AND_MEAN
+	#endif
+#ifndef JPH_COMPILER_MINGW
+	#include <Windows.h>
+#else
+	#include <windows.h>
+#endif
+	JPH_SUPPRESS_WARNING_POP
+#endif
+
+JPH_NAMESPACE_BEGIN
+
+#if defined(JPH_PLATFORM_WINDOWS_UWP) || (defined(JPH_PLATFORM_WINDOWS) && defined(JPH_CPU_ARM))
+
+uint64 GetProcessorTickCount()
+{
+	LARGE_INTEGER count;
+	QueryPerformanceCounter(&count);
+	return uint64(count.QuadPart);
+}
+
+#endif // JPH_PLATFORM_WINDOWS_UWP || (JPH_PLATFORM_WINDOWS && JPH_CPU_ARM)
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/TickCounter.h b/thirdparty/jolt_physics/Jolt/Core/TickCounter.h
new file mode 100644
index 0000000000..2b5410e3d9
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/TickCounter.h
@@ -0,0 +1,49 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+// Include for __rdtsc
+#if defined(JPH_PLATFORM_WINDOWS)
+	#include <intrin.h>
+#elif defined(JPH_CPU_X86) && defined(JPH_COMPILER_GCC)
+	#include <x86intrin.h>
+#elif defined(JPH_CPU_E2K)
+	#include <x86intrin.h>
+#endif
+
+JPH_NAMESPACE_BEGIN
+
+#if defined(JPH_PLATFORM_WINDOWS_UWP) || (defined(JPH_PLATFORM_WINDOWS) && defined(JPH_CPU_ARM))
+
+/// Functionality to get the processors cycle counter
+uint64 GetProcessorTickCount(); // Not inline to avoid having to include Windows.h
+
+#else
+
+/// Functionality to get the processors cycle counter
+JPH_INLINE uint64 GetProcessorTickCount()
+{
+#if defined(JPH_PLATFORM_BLUE)
+	return JPH_PLATFORM_BLUE_GET_TICKS();
+#elif defined(JPH_CPU_X86)
+	return __rdtsc();
+#elif defined(JPH_CPU_E2K)
+	return __rdtsc();
+#elif defined(JPH_CPU_ARM) && defined(JPH_USE_NEON)
+	uint64 val;
+	asm volatile("mrs %0, cntvct_el0" : "=r" (val));
+	return val;
+#elif defined(JPH_CPU_ARM)
+	return 0; // Not supported
+#elif defined(JPH_CPU_WASM)
+	return 0; // Not supported
+#else
+	#error Undefined
+#endif
+}
+
+#endif // JPH_PLATFORM_WINDOWS_UWP || (JPH_PLATFORM_WINDOWS && JPH_CPU_ARM)
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/UnorderedMap.h b/thirdparty/jolt_physics/Jolt/Core/UnorderedMap.h
new file mode 100644
index 0000000000..f876e2849e
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/UnorderedMap.h
@@ -0,0 +1,80 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/HashTable.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Internal helper class to provide context for UnorderedMap
+template <class Key, class Value>
+class UnorderedMapDetail
+{
+public:
+	/// Get key from key value pair
+	static const Key &			sGetKey(const std::pair<Key, Value> &inKeyValue)
+	{
+		return inKeyValue.first;
+	}
+};
+
+/// Hash Map class
+/// @tparam Key Key type
+/// @tparam Value Value type
+/// @tparam Hash Hash function (note should be 64-bits)
+/// @tparam KeyEqual Equality comparison function
+template <class Key, class Value, class Hash = JPH::Hash<Key>, class KeyEqual = std::equal_to<Key>>
+class UnorderedMap : public HashTable<Key, std::pair<Key, Value>, UnorderedMapDetail<Key, Value>, Hash, KeyEqual>
+{
+	using Base = HashTable<Key, std::pair<Key, Value>, UnorderedMapDetail<Key, Value>, Hash, KeyEqual>;
+
+public:
+	using size_type = typename Base::size_type;
+	using iterator = typename Base::iterator;
+	using const_iterator = typename Base::const_iterator;
+	using value_type = typename Base::value_type;
+
+	Value &						operator [] (const Key &inKey)
+	{
+		size_type index;
+		bool inserted = this->InsertKey(inKey, index);
+		value_type &key_value = this->GetElement(index);
+		if (inserted)
+			::new (&key_value) value_type(inKey, Value());
+		return key_value.second;
+	}
+
+	template<class... Args>
+	std::pair<iterator, bool>	try_emplace(const Key &inKey, Args &&...inArgs)
+	{
+		size_type index;
+		bool inserted = this->InsertKey(inKey, index);
+		if (inserted)
+			::new (&this->GetElement(index)) value_type(std::piecewise_construct, std::forward_as_tuple(inKey), std::forward_as_tuple(std::forward<Args>(inArgs)...));
+		return std::make_pair(iterator(this, index), inserted);
+	}
+
+	template<class... Args>
+	std::pair<iterator, bool>	try_emplace(Key &&inKey, Args &&...inArgs)
+	{
+		size_type index;
+		bool inserted = this->InsertKey(inKey, index);
+		if (inserted)
+			::new (&this->GetElement(index)) value_type(std::piecewise_construct, std::forward_as_tuple(std::move(inKey)), std::forward_as_tuple(std::forward<Args>(inArgs)...));
+		return std::make_pair(iterator(this, index), inserted);
+	}
+
+	/// Const version of find
+	using Base::find;
+
+	/// Non-const version of find
+	iterator					find(const Key &inKey)
+	{
+		const_iterator it = Base::find(inKey);
+		return iterator(this, it.mIndex);
+	}
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Core/UnorderedSet.h b/thirdparty/jolt_physics/Jolt/Core/UnorderedSet.h
new file mode 100644
index 0000000000..a320255443
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Core/UnorderedSet.h
@@ -0,0 +1,32 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/HashTable.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Internal helper class to provide context for UnorderedSet
+template <class Key>
+class UnorderedSetDetail
+{
+public:
+	/// The key is the key, just return it
+	static const Key &		sGetKey(const Key &inKey)
+	{
+		return inKey;
+	}
+};
+
+/// Hash Set class
+/// @tparam Key Key type
+/// @tparam Hash Hash function (note should be 64-bits)
+/// @tparam KeyEqual Equality comparison function
+template <class Key, class Hash = JPH::Hash<Key>, class KeyEqual = std::equal_to<Key>>
+class UnorderedSet : public HashTable<Key, Key, UnorderedSetDetail<Key>, Hash, KeyEqual>
+{
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Geometry/AABox.h b/thirdparty/jolt_physics/Jolt/Geometry/AABox.h
new file mode 100644
index 0000000000..65e353aff9
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Geometry/AABox.h
@@ -0,0 +1,312 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/Triangle.h>
+#include <Jolt/Geometry/IndexedTriangle.h>
+#include <Jolt/Geometry/Plane.h>
+#include <Jolt/Math/Mat44.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Axis aligned box
+class [[nodiscard]] AABox
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+					AABox()												: mMin(Vec3::sReplicate(FLT_MAX)), mMax(Vec3::sReplicate(-FLT_MAX)) { }
+					AABox(Vec3Arg inMin, Vec3Arg inMax)					: mMin(inMin), mMax(inMax) { }
+					AABox(DVec3Arg inMin, DVec3Arg inMax)				: mMin(inMin.ToVec3RoundDown()), mMax(inMax.ToVec3RoundUp()) { }
+					AABox(Vec3Arg inCenter, float inRadius)				: mMin(inCenter - Vec3::sReplicate(inRadius)), mMax(inCenter + Vec3::sReplicate(inRadius)) { }
+
+	/// Create box from 2 points
+	static AABox	sFromTwoPoints(Vec3Arg inP1, Vec3Arg inP2)			{ return AABox(Vec3::sMin(inP1, inP2), Vec3::sMax(inP1, inP2)); }
+
+	/// Create box from indexed triangle
+	static AABox	sFromTriangle(const VertexList &inVertices, const IndexedTriangle &inTriangle)
+	{
+		AABox box = sFromTwoPoints(Vec3(inVertices[inTriangle.mIdx[0]]), Vec3(inVertices[inTriangle.mIdx[1]]));
+		box.Encapsulate(Vec3(inVertices[inTriangle.mIdx[2]]));
+		return box;
+	}
+
+	/// Get bounding box of size 2 * FLT_MAX
+	static AABox	sBiggest()
+	{
+		return AABox(Vec3::sReplicate(-FLT_MAX), Vec3::sReplicate(FLT_MAX));
+	}
+
+	/// Comparison operators
+	bool			operator == (const AABox &inRHS) const				{ return mMin == inRHS.mMin && mMax == inRHS.mMax; }
+	bool			operator != (const AABox &inRHS) const				{ return mMin != inRHS.mMin || mMax != inRHS.mMax; }
+
+	/// Reset the bounding box to an empty bounding box
+	void			SetEmpty()
+	{
+		mMin = Vec3::sReplicate(FLT_MAX);
+		mMax = Vec3::sReplicate(-FLT_MAX);
+	}
+
+	/// Check if the bounding box is valid (max >= min)
+	bool			IsValid() const
+	{
+		return mMin.GetX() <= mMax.GetX() && mMin.GetY() <= mMax.GetY() && mMin.GetZ() <= mMax.GetZ();
+	}
+
+	/// Encapsulate point in bounding box
+	void			Encapsulate(Vec3Arg inPos)
+	{
+		mMin = Vec3::sMin(mMin, inPos);
+		mMax = Vec3::sMax(mMax, inPos);
+	}
+
+	/// Encapsulate bounding box in bounding box
+	void			Encapsulate(const AABox &inRHS)
+	{
+		mMin = Vec3::sMin(mMin, inRHS.mMin);
+		mMax = Vec3::sMax(mMax, inRHS.mMax);
+	}
+
+	/// Encapsulate triangle in bounding box
+	void			Encapsulate(const Triangle &inRHS)
+	{
+		Vec3 v = Vec3::sLoadFloat3Unsafe(inRHS.mV[0]);
+		Encapsulate(v);
+		v = Vec3::sLoadFloat3Unsafe(inRHS.mV[1]);
+		Encapsulate(v);
+		v = Vec3::sLoadFloat3Unsafe(inRHS.mV[2]);
+		Encapsulate(v);
+	}
+
+	/// Encapsulate triangle in bounding box
+	void			Encapsulate(const VertexList &inVertices, const IndexedTriangle &inTriangle)
+	{
+		for (uint32 idx : inTriangle.mIdx)
+			Encapsulate(Vec3(inVertices[idx]));
+	}
+
+	/// Intersect this bounding box with inOther, returns the intersection
+	AABox			Intersect(const AABox &inOther) const
+	{
+		return AABox(Vec3::sMax(mMin, inOther.mMin), Vec3::sMin(mMax, inOther.mMax));
+	}
+
+	/// Make sure that each edge of the bounding box has a minimal length
+	void			EnsureMinimalEdgeLength(float inMinEdgeLength)
+	{
+		Vec3 min_length = Vec3::sReplicate(inMinEdgeLength);
+		mMax = Vec3::sSelect(mMax, mMin + min_length, Vec3::sLess(mMax - mMin, min_length));
+	}
+
+	/// Widen the box on both sides by inVector
+	void			ExpandBy(Vec3Arg inVector)
+	{
+		mMin -= inVector;
+		mMax += inVector;
+	}
+
+	/// Get center of bounding box
+	Vec3			GetCenter() const
+	{
+		return 0.5f * (mMin + mMax);
+	}
+
+	/// Get extent of bounding box (half of the size)
+	Vec3			GetExtent() const
+	{
+		return 0.5f * (mMax - mMin);
+	}
+
+	/// Get size of bounding box
+	Vec3			GetSize() const
+	{
+		return mMax - mMin;
+	}
+
+	/// Get surface area of bounding box
+	float			GetSurfaceArea() const
+	{
+		Vec3 extent = mMax - mMin;
+		return 2.0f * (extent.GetX() * extent.GetY() + extent.GetX() * extent.GetZ() + extent.GetY() * extent.GetZ());
+	}
+
+	/// Get volume of bounding box
+	float			GetVolume() const
+	{
+		Vec3 extent = mMax - mMin;
+		return extent.GetX() * extent.GetY() * extent.GetZ();
+	}
+
+	/// Check if this box contains another box
+	bool			Contains(const AABox &inOther) const
+	{
+		return UVec4::sAnd(Vec3::sLessOrEqual(mMin, inOther.mMin), Vec3::sGreaterOrEqual(mMax, inOther.mMax)).TestAllXYZTrue();
+	}
+
+	/// Check if this box contains a point
+	bool			Contains(Vec3Arg inOther) const
+	{
+		return UVec4::sAnd(Vec3::sLessOrEqual(mMin, inOther), Vec3::sGreaterOrEqual(mMax, inOther)).TestAllXYZTrue();
+	}
+
+	/// Check if this box contains a point
+	bool			Contains(DVec3Arg inOther) const
+	{
+		return Contains(Vec3(inOther));
+	}
+
+	/// Check if this box overlaps with another box
+	bool			Overlaps(const AABox &inOther) const
+	{
+		return !UVec4::sOr(Vec3::sGreater(mMin, inOther.mMax), Vec3::sLess(mMax, inOther.mMin)).TestAnyXYZTrue();
+	}
+
+	/// Check if this box overlaps with a plane
+	bool			Overlaps(const Plane &inPlane) const
+	{
+		Vec3 normal = inPlane.GetNormal();
+		float dist_normal = inPlane.SignedDistance(GetSupport(normal));
+		float dist_min_normal = inPlane.SignedDistance(GetSupport(-normal));
+		return dist_normal * dist_min_normal <= 0.0f; // If both support points are on the same side of the plane we don't overlap
+	}
+
+	/// Translate bounding box
+	void			Translate(Vec3Arg inTranslation)
+	{
+		mMin += inTranslation;
+		mMax += inTranslation;
+	}
+
+	/// Translate bounding box
+	void			Translate(DVec3Arg inTranslation)
+	{
+		mMin = (DVec3(mMin) + inTranslation).ToVec3RoundDown();
+		mMax = (DVec3(mMax) + inTranslation).ToVec3RoundUp();
+	}
+
+	/// Transform bounding box
+	AABox			Transformed(Mat44Arg inMatrix) const
+	{
+		// Start with the translation of the matrix
+		Vec3 new_min, new_max;
+		new_min = new_max = inMatrix.GetTranslation();
+
+		// Now find the extreme points by considering the product of the min and max with each column of inMatrix
+		for (int c = 0; c < 3; ++c)
+		{
+			Vec3 col = inMatrix.GetColumn3(c);
+
+			Vec3 a = col * mMin[c];
+			Vec3 b = col * mMax[c];
+
+			new_min += Vec3::sMin(a, b);
+			new_max += Vec3::sMax(a, b);
+		}
+
+		// Return the new bounding box
+		return AABox(new_min, new_max);
+	}
+
+	/// Transform bounding box
+	AABox			Transformed(DMat44Arg inMatrix) const
+	{
+		AABox transformed = Transformed(inMatrix.GetRotation());
+		transformed.Translate(inMatrix.GetTranslation());
+		return transformed;
+	}
+
+	/// Scale this bounding box, can handle non-uniform and negative scaling
+	AABox			Scaled(Vec3Arg inScale) const
+	{
+		return AABox::sFromTwoPoints(mMin * inScale, mMax * inScale);
+	}
+
+	/// Calculate the support vector for this convex shape.
+	Vec3			GetSupport(Vec3Arg inDirection) const
+	{
+		return Vec3::sSelect(mMax, mMin, Vec3::sLess(inDirection, Vec3::sZero()));
+	}
+
+	/// Get the vertices of the face that faces inDirection the most
+	template <class VERTEX_ARRAY>
+	void			GetSupportingFace(Vec3Arg inDirection, VERTEX_ARRAY &outVertices) const
+	{
+		outVertices.resize(4);
+
+		int axis = inDirection.Abs().GetHighestComponentIndex();
+		if (inDirection[axis] < 0.0f)
+		{
+			switch (axis)
+			{
+			case 0:
+				outVertices[0] = Vec3(mMax.GetX(), mMin.GetY(), mMin.GetZ());
+				outVertices[1] = Vec3(mMax.GetX(), mMax.GetY(), mMin.GetZ());
+				outVertices[2] = Vec3(mMax.GetX(), mMax.GetY(), mMax.GetZ());
+				outVertices[3] = Vec3(mMax.GetX(), mMin.GetY(), mMax.GetZ());
+				break;
+
+			case 1:
+				outVertices[0] = Vec3(mMin.GetX(), mMax.GetY(), mMin.GetZ());
+				outVertices[1] = Vec3(mMin.GetX(), mMax.GetY(), mMax.GetZ());
+				outVertices[2] = Vec3(mMax.GetX(), mMax.GetY(), mMax.GetZ());
+				outVertices[3] = Vec3(mMax.GetX(), mMax.GetY(), mMin.GetZ());
+				break;
+
+			case 2:
+				outVertices[0] = Vec3(mMin.GetX(), mMin.GetY(), mMax.GetZ());
+				outVertices[1] = Vec3(mMax.GetX(), mMin.GetY(), mMax.GetZ());
+				outVertices[2] = Vec3(mMax.GetX(), mMax.GetY(), mMax.GetZ());
+				outVertices[3] = Vec3(mMin.GetX(), mMax.GetY(), mMax.GetZ());
+				break;
+			}
+		}
+		else
+		{
+			switch (axis)
+			{
+			case 0:
+				outVertices[0] = Vec3(mMin.GetX(), mMin.GetY(), mMin.GetZ());
+				outVertices[1] = Vec3(mMin.GetX(), mMin.GetY(), mMax.GetZ());
+				outVertices[2] = Vec3(mMin.GetX(), mMax.GetY(), mMax.GetZ());
+				outVertices[3] = Vec3(mMin.GetX(), mMax.GetY(), mMin.GetZ());
+				break;
+
+			case 1:
+				outVertices[0] = Vec3(mMin.GetX(), mMin.GetY(), mMin.GetZ());
+				outVertices[1] = Vec3(mMax.GetX(), mMin.GetY(), mMin.GetZ());
+				outVertices[2] = Vec3(mMax.GetX(), mMin.GetY(), mMax.GetZ());
+				outVertices[3] = Vec3(mMin.GetX(), mMin.GetY(), mMax.GetZ());
+				break;
+
+			case 2:
+				outVertices[0] = Vec3(mMin.GetX(), mMin.GetY(), mMin.GetZ());
+				outVertices[1] = Vec3(mMin.GetX(), mMax.GetY(), mMin.GetZ());
+				outVertices[2] = Vec3(mMax.GetX(), mMax.GetY(), mMin.GetZ());
+				outVertices[3] = Vec3(mMax.GetX(), mMin.GetY(), mMin.GetZ());
+				break;
+			}
+		}
+	}
+
+	/// Get the closest point on or in this box to inPoint
+	Vec3			GetClosestPoint(Vec3Arg inPoint) const
+	{
+		return Vec3::sMin(Vec3::sMax(inPoint, mMin), mMax);
+	}
+
+	/// Get the squared distance between inPoint and this box (will be 0 if in Point is inside the box)
+	inline float	GetSqDistanceTo(Vec3Arg inPoint) const
+	{
+		return (GetClosestPoint(inPoint) - inPoint).LengthSq();
+	}
+
+	/// Bounding box min and max
+	Vec3			mMin;
+	Vec3			mMax;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Geometry/AABox4.h b/thirdparty/jolt_physics/Jolt/Geometry/AABox4.h
new file mode 100644
index 0000000000..4465d4dabe
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Geometry/AABox4.h
@@ -0,0 +1,224 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/OrientedBox.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Helper functions that process 4 axis aligned boxes at the same time using SIMD
+/// Test if 4 bounding boxes overlap with 1 bounding box, splat 1 box
+JPH_INLINE UVec4 AABox4VsBox(const AABox &inBox1, Vec4Arg inBox2MinX, Vec4Arg inBox2MinY, Vec4Arg inBox2MinZ, Vec4Arg inBox2MaxX, Vec4Arg inBox2MaxY, Vec4Arg inBox2MaxZ)
+{
+	// Splat values of box 1
+	Vec4 box1_minx = inBox1.mMin.SplatX();
+	Vec4 box1_miny = inBox1.mMin.SplatY();
+	Vec4 box1_minz = inBox1.mMin.SplatZ();
+	Vec4 box1_maxx = inBox1.mMax.SplatX();
+	Vec4 box1_maxy = inBox1.mMax.SplatY();
+	Vec4 box1_maxz = inBox1.mMax.SplatZ();
+
+	// Test separation over each axis
+	UVec4 nooverlapx = UVec4::sOr(Vec4::sGreater(box1_minx, inBox2MaxX), Vec4::sGreater(inBox2MinX, box1_maxx));
+	UVec4 nooverlapy = UVec4::sOr(Vec4::sGreater(box1_miny, inBox2MaxY), Vec4::sGreater(inBox2MinY, box1_maxy));
+	UVec4 nooverlapz = UVec4::sOr(Vec4::sGreater(box1_minz, inBox2MaxZ), Vec4::sGreater(inBox2MinZ, box1_maxz));
+
+	// Return overlap
+	return UVec4::sNot(UVec4::sOr(UVec4::sOr(nooverlapx, nooverlapy), nooverlapz));
+}
+
+/// Scale 4 axis aligned boxes
+JPH_INLINE void AABox4Scale(Vec3Arg inScale, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ, Vec4 &outBoundsMinX, Vec4 &outBoundsMinY, Vec4 &outBoundsMinZ, Vec4 &outBoundsMaxX, Vec4 &outBoundsMaxY, Vec4 &outBoundsMaxZ)
+{
+	Vec4 scale_x = inScale.SplatX();
+	Vec4 scaled_min_x = scale_x * inBoxMinX;
+	Vec4 scaled_max_x = scale_x * inBoxMaxX;
+	outBoundsMinX = Vec4::sMin(scaled_min_x, scaled_max_x); // Negative scale can flip min and max
+	outBoundsMaxX = Vec4::sMax(scaled_min_x, scaled_max_x);
+
+	Vec4 scale_y = inScale.SplatY();
+	Vec4 scaled_min_y = scale_y * inBoxMinY;
+	Vec4 scaled_max_y = scale_y * inBoxMaxY;
+	outBoundsMinY = Vec4::sMin(scaled_min_y, scaled_max_y);
+	outBoundsMaxY = Vec4::sMax(scaled_min_y, scaled_max_y);
+
+	Vec4 scale_z = inScale.SplatZ();
+	Vec4 scaled_min_z = scale_z * inBoxMinZ;
+	Vec4 scaled_max_z = scale_z * inBoxMaxZ;
+	outBoundsMinZ = Vec4::sMin(scaled_min_z, scaled_max_z);
+	outBoundsMaxZ = Vec4::sMax(scaled_min_z, scaled_max_z);
+}
+
+/// Enlarge 4 bounding boxes with extent (add to both sides)
+JPH_INLINE void AABox4EnlargeWithExtent(Vec3Arg inExtent, Vec4 &ioBoundsMinX, Vec4 &ioBoundsMinY, Vec4 &ioBoundsMinZ, Vec4 &ioBoundsMaxX, Vec4 &ioBoundsMaxY, Vec4 &ioBoundsMaxZ)
+{
+	Vec4 extent_x = inExtent.SplatX();
+	ioBoundsMinX -= extent_x;
+	ioBoundsMaxX += extent_x;
+
+	Vec4 extent_y = inExtent.SplatY();
+	ioBoundsMinY -= extent_y;
+	ioBoundsMaxY += extent_y;
+
+	Vec4 extent_z = inExtent.SplatZ();
+	ioBoundsMinZ -= extent_z;
+	ioBoundsMaxZ += extent_z;
+}
+
+/// Test if 4 bounding boxes overlap with a point
+JPH_INLINE UVec4 AABox4VsPoint(Vec3Arg inPoint, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ)
+{
+	// Splat point to 4 component vectors
+	Vec4 point_x = Vec4(inPoint).SplatX();
+	Vec4 point_y = Vec4(inPoint).SplatY();
+	Vec4 point_z = Vec4(inPoint).SplatZ();
+
+	// Test if point overlaps with box
+	UVec4 overlapx = UVec4::sAnd(Vec4::sGreaterOrEqual(point_x, inBoxMinX), Vec4::sLessOrEqual(point_x, inBoxMaxX));
+	UVec4 overlapy = UVec4::sAnd(Vec4::sGreaterOrEqual(point_y, inBoxMinY), Vec4::sLessOrEqual(point_y, inBoxMaxY));
+	UVec4 overlapz = UVec4::sAnd(Vec4::sGreaterOrEqual(point_z, inBoxMinZ), Vec4::sLessOrEqual(point_z, inBoxMaxZ));
+
+	// Test if all are overlapping
+	return UVec4::sAnd(UVec4::sAnd(overlapx, overlapy), overlapz);
+}
+
+/// Test if 4 bounding boxes overlap with an oriented box
+JPH_INLINE UVec4 AABox4VsBox(Mat44Arg inOrientation, Vec3Arg inHalfExtents, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ, float inEpsilon = 1.0e-6f)
+{
+	// Taken from: Real Time Collision Detection - Christer Ericson
+	// Chapter 4.4.1, page 103-105.
+	// Note that the code is swapped around: A is the aabox and B is the oriented box (this saves us from having to invert the orientation of the oriented box)
+
+	// Compute translation vector t (the translation of B in the space of A)
+	Vec4 t[3] {
+		inOrientation.GetTranslation().SplatX() - 0.5f * (inBoxMinX + inBoxMaxX),
+		inOrientation.GetTranslation().SplatY() - 0.5f * (inBoxMinY + inBoxMaxY),
+		inOrientation.GetTranslation().SplatZ() - 0.5f * (inBoxMinZ + inBoxMaxZ) };
+
+	// Compute common subexpressions. Add in an epsilon term to
+	// counteract arithmetic errors when two edges are parallel and
+	// their cross product is (near) null (see text for details)
+	Vec3 epsilon = Vec3::sReplicate(inEpsilon);
+	Vec3 abs_r[3] { inOrientation.GetAxisX().Abs() + epsilon, inOrientation.GetAxisY().Abs() + epsilon, inOrientation.GetAxisZ().Abs() + epsilon };
+
+	// Half extents for a
+	Vec4 a_half_extents[3] {
+		0.5f * (inBoxMaxX - inBoxMinX),
+		0.5f * (inBoxMaxY - inBoxMinY),
+		0.5f * (inBoxMaxZ - inBoxMinZ) };
+
+	// Half extents of b
+	Vec4 b_half_extents_x = inHalfExtents.SplatX();
+	Vec4 b_half_extents_y = inHalfExtents.SplatY();
+	Vec4 b_half_extents_z = inHalfExtents.SplatZ();
+
+	// Each component corresponds to 1 overlapping OBB vs ABB
+	UVec4 overlaps = UVec4(0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff);
+
+	// Test axes L = A0, L = A1, L = A2
+	Vec4 ra, rb;
+	for (int i = 0; i < 3; i++)
+	{
+		ra = a_half_extents[i];
+		rb = b_half_extents_x * abs_r[0][i] + b_half_extents_y * abs_r[1][i] + b_half_extents_z * abs_r[2][i];
+		overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual(t[i].Abs(), ra + rb));
+	}
+
+	// Test axes L = B0, L = B1, L = B2
+	for (int i = 0; i < 3; i++)
+	{
+		ra = a_half_extents[0] * abs_r[i][0] + a_half_extents[1] * abs_r[i][1] + a_half_extents[2] * abs_r[i][2];
+		rb = Vec4::sReplicate(inHalfExtents[i]);
+		overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[0] * inOrientation(0, i) + t[1] * inOrientation(1, i) + t[2] * inOrientation(2, i)).Abs(), ra + rb));
+	}
+
+	// Test axis L = A0 x B0
+	ra = a_half_extents[1] * abs_r[0][2] + a_half_extents[2] * abs_r[0][1];
+	rb = b_half_extents_y * abs_r[2][0] + b_half_extents_z * abs_r[1][0];
+	overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[2] * inOrientation(1, 0) - t[1] * inOrientation(2, 0)).Abs(), ra + rb));
+
+	// Test axis L = A0 x B1
+	ra = a_half_extents[1] * abs_r[1][2] + a_half_extents[2] * abs_r[1][1];
+	rb = b_half_extents_x * abs_r[2][0] + b_half_extents_z * abs_r[0][0];
+	overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[2] * inOrientation(1, 1) - t[1] * inOrientation(2, 1)).Abs(), ra + rb));
+
+	// Test axis L = A0 x B2
+	ra = a_half_extents[1] * abs_r[2][2] + a_half_extents[2] * abs_r[2][1];
+	rb = b_half_extents_x * abs_r[1][0] + b_half_extents_y * abs_r[0][0];
+	overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[2] * inOrientation(1, 2) - t[1] * inOrientation(2, 2)).Abs(), ra + rb));
+
+	// Test axis L = A1 x B0
+	ra = a_half_extents[0] * abs_r[0][2] + a_half_extents[2] * abs_r[0][0];
+	rb = b_half_extents_y * abs_r[2][1] + b_half_extents_z * abs_r[1][1];
+	overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[0] * inOrientation(2, 0) - t[2] * inOrientation(0, 0)).Abs(), ra + rb));
+
+	// Test axis L = A1 x B1
+	ra = a_half_extents[0] * abs_r[1][2] + a_half_extents[2] * abs_r[1][0];
+	rb = b_half_extents_x * abs_r[2][1] + b_half_extents_z * abs_r[0][1];
+	overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[0] * inOrientation(2, 1) - t[2] * inOrientation(0, 1)).Abs(), ra + rb));
+
+	// Test axis L = A1 x B2
+	ra = a_half_extents[0] * abs_r[2][2] + a_half_extents[2] * abs_r[2][0];
+	rb = b_half_extents_x * abs_r[1][1] + b_half_extents_y * abs_r[0][1];
+	overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[0] * inOrientation(2, 2) - t[2] * inOrientation(0, 2)).Abs(), ra + rb));
+
+	// Test axis L = A2 x B0
+	ra = a_half_extents[0] * abs_r[0][1] + a_half_extents[1] * abs_r[0][0];
+	rb = b_half_extents_y * abs_r[2][2] + b_half_extents_z * abs_r[1][2];
+	overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[1] * inOrientation(0, 0) - t[0] * inOrientation(1, 0)).Abs(), ra + rb));
+
+	// Test axis L = A2 x B1
+	ra = a_half_extents[0] * abs_r[1][1] + a_half_extents[1] * abs_r[1][0];
+	rb = b_half_extents_x * abs_r[2][2] + b_half_extents_z * abs_r[0][2];
+	overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[1] * inOrientation(0, 1) - t[0] * inOrientation(1, 1)).Abs(), ra + rb));
+
+	// Test axis L = A2 x B2
+	ra = a_half_extents[0] * abs_r[2][1] + a_half_extents[1] * abs_r[2][0];
+	rb = b_half_extents_x * abs_r[1][2] + b_half_extents_y * abs_r[0][2];
+	overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[1] * inOrientation(0, 2) - t[0] * inOrientation(1, 2)).Abs(), ra + rb));
+
+	// Return if the OBB vs AABBs are intersecting
+	return overlaps;
+}
+
+/// Convenience function that tests 4 AABoxes vs OrientedBox
+JPH_INLINE UVec4 AABox4VsBox(const OrientedBox &inBox, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ, float inEpsilon = 1.0e-6f)
+{
+	return AABox4VsBox(inBox.mOrientation, inBox.mHalfExtents, inBoxMinX, inBoxMinY, inBoxMinZ, inBoxMaxX, inBoxMaxY, inBoxMaxZ, inEpsilon);
+}
+
+/// Get the squared distance between 4 AABoxes and a point
+JPH_INLINE Vec4 AABox4DistanceSqToPoint(Vec4Arg inPointX, Vec4Arg inPointY, Vec4Arg inPointZ, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ)
+{
+	// Get closest point on box
+	Vec4 closest_x = Vec4::sMin(Vec4::sMax(inPointX, inBoxMinX), inBoxMaxX);
+	Vec4 closest_y = Vec4::sMin(Vec4::sMax(inPointY, inBoxMinY), inBoxMaxY);
+	Vec4 closest_z = Vec4::sMin(Vec4::sMax(inPointZ, inBoxMinZ), inBoxMaxZ);
+
+	// Return the squared distance between the box and point
+	return Square(closest_x - inPointX) + Square(closest_y - inPointY) + Square(closest_z - inPointZ);
+}
+
+/// Get the squared distance between 4 AABoxes and a point
+JPH_INLINE Vec4 AABox4DistanceSqToPoint(Vec3 inPoint, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ)
+{
+	return AABox4DistanceSqToPoint(inPoint.SplatX(), inPoint.SplatY(), inPoint.SplatZ(), inBoxMinX, inBoxMinY, inBoxMinZ, inBoxMaxX, inBoxMaxY, inBoxMaxZ);
+}
+
+/// Test 4 AABoxes vs a sphere
+JPH_INLINE UVec4 AABox4VsSphere(Vec4Arg inCenterX, Vec4Arg inCenterY, Vec4Arg inCenterZ, Vec4Arg inRadiusSq, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ)
+{
+	// Test the distance from the center of the sphere to the box is smaller than the radius
+	Vec4 distance_sq = AABox4DistanceSqToPoint(inCenterX, inCenterY, inCenterZ, inBoxMinX, inBoxMinY, inBoxMinZ, inBoxMaxX, inBoxMaxY, inBoxMaxZ);
+	return Vec4::sLessOrEqual(distance_sq, inRadiusSq);
+}
+
+/// Test 4 AABoxes vs a sphere
+JPH_INLINE UVec4 AABox4VsSphere(Vec3Arg inCenter, float inRadiusSq, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ)
+{
+	return AABox4VsSphere(inCenter.SplatX(), inCenter.SplatY(), inCenter.SplatZ(), Vec4::sReplicate(inRadiusSq), inBoxMinX, inBoxMinY, inBoxMinZ, inBoxMaxX, inBoxMaxY, inBoxMaxZ);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Geometry/ClipPoly.h b/thirdparty/jolt_physics/Jolt/Geometry/ClipPoly.h
new file mode 100644
index 0000000000..7301d8e6f0
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Geometry/ClipPoly.h
@@ -0,0 +1,200 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/AABox.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Clip inPolygonToClip against the positive halfspace of plane defined by inPlaneOrigin and inPlaneNormal.
+/// inPlaneNormal does not need to be normalized.
+template <class VERTEX_ARRAY>
+void ClipPolyVsPlane(const VERTEX_ARRAY &inPolygonToClip, Vec3Arg inPlaneOrigin, Vec3Arg inPlaneNormal, VERTEX_ARRAY &outClippedPolygon)
+{
+	JPH_ASSERT(inPolygonToClip.size() >= 2);
+	JPH_ASSERT(outClippedPolygon.empty());
+
+	// Determine state of last point
+	Vec3 e1 = inPolygonToClip[inPolygonToClip.size() - 1];
+	float prev_num = (inPlaneOrigin - e1).Dot(inPlaneNormal);
+	bool prev_inside = prev_num < 0.0f;
+
+	// Loop through all vertices
+	for (typename VERTEX_ARRAY::size_type j = 0; j < inPolygonToClip.size(); ++j)
+	{
+		// Check if second point is inside
+		Vec3Arg e2 = inPolygonToClip[j];
+		float num = (inPlaneOrigin - e2).Dot(inPlaneNormal);
+		bool cur_inside = num < 0.0f;
+
+		// In -> Out or Out -> In: Add point on clipping plane
+		if (cur_inside != prev_inside)
+		{
+			// Solve: (X - inPlaneOrigin) . inPlaneNormal = 0 and X = e1 + t * (e2 - e1) for X
+			Vec3 e12 = e2 - e1;
+			float denom = e12.Dot(inPlaneNormal);
+			if (denom != 0.0f)
+				outClippedPolygon.push_back(e1 + (prev_num / denom) * e12);
+			else
+				cur_inside = prev_inside; // Edge is parallel to plane, treat point as if it were on the same side as the last point
+		}
+
+		// Point inside, add it
+		if (cur_inside)
+			outClippedPolygon.push_back(e2);
+
+		// Update previous state
+		prev_num = num;
+		prev_inside = cur_inside;
+		e1 = e2;
+	}
+}
+
+/// Clip polygon versus polygon.
+/// Both polygons are assumed to be in counter clockwise order.
+/// @param inClippingPolygonNormal is used to create planes of all edges in inClippingPolygon against which inPolygonToClip is clipped, inClippingPolygonNormal does not need to be normalized
+/// @param inClippingPolygon is the polygon which inClippedPolygon is clipped against
+/// @param inPolygonToClip is the polygon that is clipped
+/// @param outClippedPolygon will contain clipped polygon when function returns
+template <class VERTEX_ARRAY>
+void ClipPolyVsPoly(const VERTEX_ARRAY &inPolygonToClip, const VERTEX_ARRAY &inClippingPolygon, Vec3Arg inClippingPolygonNormal, VERTEX_ARRAY &outClippedPolygon)
+{
+	JPH_ASSERT(inPolygonToClip.size() >= 2);
+	JPH_ASSERT(inClippingPolygon.size() >= 3);
+
+	VERTEX_ARRAY tmp_vertices[2];
+	int tmp_vertices_idx = 0;
+
+	for (typename VERTEX_ARRAY::size_type i = 0; i < inClippingPolygon.size(); ++i)
+	{
+		// Get edge to clip against
+		Vec3 clip_e1 = inClippingPolygon[i];
+		Vec3 clip_e2 = inClippingPolygon[(i + 1) % inClippingPolygon.size()];
+		Vec3 clip_normal = inClippingPolygonNormal.Cross(clip_e2 - clip_e1); // Pointing inward to the clipping polygon
+
+		// Get source and target polygon
+		const VERTEX_ARRAY &src_polygon = (i == 0)? inPolygonToClip : tmp_vertices[tmp_vertices_idx];
+		tmp_vertices_idx ^= 1;
+		VERTEX_ARRAY &tgt_polygon = (i == inClippingPolygon.size() - 1)? outClippedPolygon : tmp_vertices[tmp_vertices_idx];
+		tgt_polygon.clear();
+
+		// Clip against the edge
+		ClipPolyVsPlane(src_polygon, clip_e1, clip_normal, tgt_polygon);
+
+		// Break out if no polygon left
+		if (tgt_polygon.size() < 3)
+		{
+			outClippedPolygon.clear();
+			break;
+		}
+	}
+}
+
+/// Clip inPolygonToClip against an edge, the edge is projected on inPolygonToClip using inClippingEdgeNormal.
+/// The positive half space (the side on the edge in the direction of inClippingEdgeNormal) is cut away.
+template <class VERTEX_ARRAY>
+void ClipPolyVsEdge(const VERTEX_ARRAY &inPolygonToClip, Vec3Arg inEdgeVertex1, Vec3Arg inEdgeVertex2, Vec3Arg inClippingEdgeNormal, VERTEX_ARRAY &outClippedPolygon)
+{
+	JPH_ASSERT(inPolygonToClip.size() >= 3);
+	JPH_ASSERT(outClippedPolygon.empty());
+
+	// Get normal that is perpendicular to the edge and the clipping edge normal
+	Vec3 edge = inEdgeVertex2 - inEdgeVertex1;
+	Vec3 edge_normal = inClippingEdgeNormal.Cross(edge);
+
+	// Project vertices of edge on inPolygonToClip
+	Vec3 polygon_normal = (inPolygonToClip[2] - inPolygonToClip[0]).Cross(inPolygonToClip[1] - inPolygonToClip[0]);
+	float polygon_normal_len_sq = polygon_normal.LengthSq();
+	Vec3 v1 = inEdgeVertex1 + polygon_normal.Dot(inPolygonToClip[0] - inEdgeVertex1) * polygon_normal / polygon_normal_len_sq;
+	Vec3 v2 = inEdgeVertex2 + polygon_normal.Dot(inPolygonToClip[0] - inEdgeVertex2) * polygon_normal / polygon_normal_len_sq;
+	Vec3 v12 = v2 - v1;
+	float v12_len_sq = v12.LengthSq();
+
+	// Determine state of last point
+	Vec3 e1 = inPolygonToClip[inPolygonToClip.size() - 1];
+	float prev_num = (inEdgeVertex1 - e1).Dot(edge_normal);
+	bool prev_inside = prev_num < 0.0f;
+
+	// Loop through all vertices
+	for (typename VERTEX_ARRAY::size_type j = 0; j < inPolygonToClip.size(); ++j)
+	{
+		// Check if second point is inside
+		Vec3 e2 = inPolygonToClip[j];
+		float num = (inEdgeVertex1 - e2).Dot(edge_normal);
+		bool cur_inside = num < 0.0f;
+
+		// In -> Out or Out -> In: Add point on clipping plane
+		if (cur_inside != prev_inside)
+		{
+			// Solve: (inEdgeVertex1 - X) . edge_normal = 0 and X = e1 + t * (e2 - e1) for X
+			Vec3 e12 = e2 - e1;
+			float denom = e12.Dot(edge_normal);
+			Vec3 clipped_point = denom != 0.0f? e1 + (prev_num / denom) * e12 : e1;
+
+			// Project point on line segment v1, v2 so see if it falls outside if the edge
+			float projection = (clipped_point - v1).Dot(v12);
+			if (projection < 0.0f)
+				outClippedPolygon.push_back(v1);
+			else if (projection > v12_len_sq)
+				outClippedPolygon.push_back(v2);
+			else
+				outClippedPolygon.push_back(clipped_point);
+		}
+
+		// Update previous state
+		prev_num = num;
+		prev_inside = cur_inside;
+		e1 = e2;
+	}
+}
+
+/// Clip polygon vs axis aligned box, inPolygonToClip is assume to be in counter clockwise order.
+/// Output will be stored in outClippedPolygon. Everything inside inAABox will be kept.
+template <class VERTEX_ARRAY>
+void ClipPolyVsAABox(const VERTEX_ARRAY &inPolygonToClip, const AABox &inAABox, VERTEX_ARRAY &outClippedPolygon)
+{
+	JPH_ASSERT(inPolygonToClip.size() >= 2);
+
+	VERTEX_ARRAY tmp_vertices[2];
+	int tmp_vertices_idx = 0;
+
+	for (int coord = 0; coord < 3; ++coord)
+		for (int side = 0; side < 2; ++side)
+		{
+			// Get plane to clip against
+			Vec3 origin = Vec3::sZero(), normal = Vec3::sZero();
+			if (side == 0)
+			{
+				normal.SetComponent(coord, 1.0f);
+				origin.SetComponent(coord, inAABox.mMin[coord]);
+			}
+			else
+			{
+				normal.SetComponent(coord, -1.0f);
+				origin.SetComponent(coord, inAABox.mMax[coord]);
+			}
+
+			// Get source and target polygon
+			const VERTEX_ARRAY &src_polygon = tmp_vertices_idx == 0? inPolygonToClip : tmp_vertices[tmp_vertices_idx & 1];
+			tmp_vertices_idx++;
+			VERTEX_ARRAY &tgt_polygon = tmp_vertices_idx == 6? outClippedPolygon : tmp_vertices[tmp_vertices_idx & 1];
+			tgt_polygon.clear();
+
+			// Clip against the edge
+			ClipPolyVsPlane(src_polygon, origin, normal, tgt_polygon);
+
+			// Break out if no polygon left
+			if (tgt_polygon.size() < 3)
+			{
+				outClippedPolygon.clear();
+				return;
+			}
+
+			// Flip normal
+			normal = -normal;
+		}
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Geometry/ClosestPoint.h b/thirdparty/jolt_physics/Jolt/Geometry/ClosestPoint.h
new file mode 100644
index 0000000000..a437763f57
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Geometry/ClosestPoint.h
@@ -0,0 +1,498 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+// Turn off fused multiply add instruction because it makes the equations of the form a * b - c * d inaccurate below
+JPH_PRECISE_MATH_ON
+
+/// Helper utils to find the closest point to a line segment, triangle or tetrahedron
+namespace ClosestPoint
+{
+	/// Compute barycentric coordinates of closest point to origin for infinite line defined by (inA, inB)
+	/// Point can then be computed as inA * outU + inB * outV
+	/// Returns false if the points inA, inB do not form a line (are at the same point)
+	inline bool GetBaryCentricCoordinates(Vec3Arg inA, Vec3Arg inB, float &outU, float &outV)
+	{
+		Vec3 ab = inB - inA;
+		float denominator = ab.LengthSq();
+		if (denominator < Square(FLT_EPSILON))
+		{
+			// Degenerate line segment, fallback to points
+			if (inA.LengthSq() < inB.LengthSq())
+			{
+				// A closest
+				outU = 1.0f;
+				outV = 0.0f;
+			}
+			else
+			{
+				// B closest
+				outU = 0.0f;
+				outV = 1.0f;
+			}
+			return false;
+		}
+		else
+		{
+			outV = -inA.Dot(ab) / denominator;
+			outU = 1.0f - outV;
+		}
+		return true;
+	}
+
+	/// Compute barycentric coordinates of closest point to origin for plane defined by (inA, inB, inC)
+	/// Point can then be computed as inA * outU + inB * outV + inC * outW
+	/// Returns false if the points inA, inB, inC do not form a plane (are on the same line or at the same point)
+	inline bool GetBaryCentricCoordinates(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, float &outU, float &outV, float &outW)
+	{
+		// Taken from: Real-Time Collision Detection - Christer Ericson (Section: Barycentric Coordinates)
+		// With p = 0
+		// Adjusted to always include the shortest edge of the triangle in the calculation to improve numerical accuracy
+
+		// First calculate the three edges
+		Vec3 v0 = inB - inA;
+		Vec3 v1 = inC - inA;
+		Vec3 v2 = inC - inB;
+
+		// Make sure that the shortest edge is included in the calculation to keep the products a * b - c * d as small as possible to preserve accuracy
+		float d00 = v0.LengthSq();
+		float d11 = v1.LengthSq();
+		float d22 = v2.LengthSq();
+		if (d00 <= d22)
+		{
+			// Use v0 and v1 to calculate barycentric coordinates
+			float d01 = v0.Dot(v1);
+
+			// Denominator must be positive:
+			// |v0|^2 * |v1|^2 - (v0 . v1)^2 = |v0|^2 * |v1|^2 * (1 - cos(angle)^2) >= 0
+			float denominator = d00 * d11 - d01 * d01;
+			if (denominator < 1.0e-12f)
+			{
+				// Degenerate triangle, return coordinates along longest edge
+				if (d00 > d11)
+				{
+					GetBaryCentricCoordinates(inA, inB, outU, outV);
+					outW = 0.0f;
+				}
+				else
+				{
+					GetBaryCentricCoordinates(inA, inC, outU, outW);
+					outV = 0.0f;
+				}
+				return false;
+			}
+			else
+			{
+				float a0 = inA.Dot(v0);
+				float a1 = inA.Dot(v1);
+				outV = (d01 * a1 - d11 * a0) / denominator;
+				outW = (d01 * a0 - d00 * a1) / denominator;
+				outU = 1.0f - outV - outW;
+			}
+		}
+		else
+		{
+			// Use v1 and v2 to calculate barycentric coordinates
+			float d12 = v1.Dot(v2);
+
+			float denominator = d11 * d22 - d12 * d12;
+			if (denominator < 1.0e-12f)
+			{
+				// Degenerate triangle, return coordinates along longest edge
+				if (d11 > d22)
+				{
+					GetBaryCentricCoordinates(inA, inC, outU, outW);
+					outV = 0.0f;
+				}
+				else
+				{
+					GetBaryCentricCoordinates(inB, inC, outV, outW);
+					outU = 0.0f;
+				}
+				return false;
+			}
+			else
+			{
+				float c1 = inC.Dot(v1);
+				float c2 = inC.Dot(v2);
+				outU = (d22 * c1 - d12 * c2) / denominator;
+				outV = (d11 * c2 - d12 * c1) / denominator;
+				outW = 1.0f - outU - outV;
+			}
+		}
+		return true;
+	}
+
+	/// Get the closest point to the origin of line (inA, inB)
+	/// outSet describes which features are closest: 1 = a, 2 = b, 3 = line segment ab
+	inline Vec3	GetClosestPointOnLine(Vec3Arg inA, Vec3Arg inB, uint32 &outSet)
+	{
+		float u, v;
+		GetBaryCentricCoordinates(inA, inB, u, v);
+		if (v <= 0.0f)
+		{
+			// inA is closest point
+			outSet = 0b0001;
+			return inA;
+		}
+		else if (u <= 0.0f)
+		{
+			// inB is closest point
+			outSet = 0b0010;
+			return inB;
+		}
+		else
+		{
+			// Closest point lies on line inA inB
+			outSet = 0b0011;
+			return u * inA + v * inB;
+		}
+	}
+
+	/// Get the closest point to the origin of triangle (inA, inB, inC)
+	/// outSet describes which features are closest: 1 = a, 2 = b, 4 = c, 5 = line segment ac, 7 = triangle interior etc.
+	/// If MustIncludeC is true, the function assumes that C is part of the closest feature (vertex, edge, face) and does less work, if the assumption is not true then a closest point to the other features is returned.
+	template <bool MustIncludeC = false>
+	inline Vec3	GetClosestPointOnTriangle(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, uint32 &outSet)
+	{
+		// Taken from: Real-Time Collision Detection - Christer Ericson (Section: Closest Point on Triangle to Point)
+		// With p = 0
+
+		// The most accurate normal is calculated by using the two shortest edges
+		// See: https://box2d.org/posts/2014/01/troublesome-triangle/
+		// The difference in normals is most pronounced when one edge is much smaller than the others (in which case the other 2 must have roughly the same length).
+		// Therefore we can suffice by just picking the shortest from 2 edges and use that with the 3rd edge to calculate the normal.
+		// We first check which of the edges is shorter and if bc is shorter than ac then we swap a with c to a is always on the shortest edge
+		UVec4 swap_ac;
+		{
+			Vec3 ac = inC - inA;
+			Vec3 bc = inC - inB;
+			swap_ac = Vec4::sLess(bc.DotV4(bc), ac.DotV4(ac));
+		}
+		Vec3 a = Vec3::sSelect(inA, inC, swap_ac);
+		Vec3 c = Vec3::sSelect(inC, inA, swap_ac);
+
+		// Calculate normal
+		Vec3 ab = inB - a;
+		Vec3 ac = c - a;
+		Vec3 n = ab.Cross(ac);
+		float n_len_sq = n.LengthSq();
+
+		// Check degenerate
+		if (n_len_sq < 1.0e-10f) // Square(FLT_EPSILON) was too small and caused numerical problems, see test case TestCollideParallelTriangleVsCapsule
+		{
+			// Degenerate, fallback to vertices and edges
+
+			// Start with vertex C being the closest
+			uint32 closest_set = 0b0100;
+			Vec3 closest_point = inC;
+			float best_dist_sq = inC.LengthSq();
+
+			// If the closest point must include C then A or B cannot be closest
+			// Note that we test vertices first because we want to prefer a closest vertex over a closest edge (this results in an outSet with fewer bits set)
+			if constexpr (!MustIncludeC)
+			{
+				// Try vertex A
+				float a_len_sq = inA.LengthSq();
+				if (a_len_sq < best_dist_sq)
+				{
+					closest_set = 0b0001;
+					closest_point = inA;
+					best_dist_sq = a_len_sq;
+				}
+
+				// Try vertex B
+				float b_len_sq = inB.LengthSq();
+				if (b_len_sq < best_dist_sq)
+				{
+					closest_set = 0b0010;
+					closest_point = inB;
+					best_dist_sq = b_len_sq;
+				}
+			}
+
+			// Edge AC
+			float ac_len_sq = ac.LengthSq();
+			if (ac_len_sq > Square(FLT_EPSILON))
+			{
+				float v = Clamp(-a.Dot(ac) / ac_len_sq, 0.0f, 1.0f);
+				Vec3 q = a + v * ac;
+				float dist_sq = q.LengthSq();
+				if (dist_sq < best_dist_sq)
+				{
+					closest_set = 0b0101;
+					closest_point = q;
+					best_dist_sq = dist_sq;
+				}
+			}
+
+			// Edge BC
+			Vec3 bc = inC - inB;
+			float bc_len_sq = bc.LengthSq();
+			if (bc_len_sq > Square(FLT_EPSILON))
+			{
+				float v = Clamp(-inB.Dot(bc) / bc_len_sq, 0.0f, 1.0f);
+				Vec3 q = inB + v * bc;
+				float dist_sq = q.LengthSq();
+				if (dist_sq < best_dist_sq)
+				{
+					closest_set = 0b0110;
+					closest_point = q;
+					best_dist_sq = dist_sq;
+				}
+			}
+
+			// If the closest point must include C then AB cannot be closest
+			if constexpr (!MustIncludeC)
+			{
+				// Edge AB
+				ab = inB - inA;
+				float ab_len_sq = ab.LengthSq();
+				if (ab_len_sq > Square(FLT_EPSILON))
+				{
+					float v = Clamp(-inA.Dot(ab) / ab_len_sq, 0.0f, 1.0f);
+					Vec3 q = inA + v * ab;
+					float dist_sq = q.LengthSq();
+					if (dist_sq < best_dist_sq)
+					{
+						closest_set = 0b0011;
+						closest_point = q;
+						best_dist_sq = dist_sq;
+					}
+				}
+			}
+
+			outSet = closest_set;
+			return closest_point;
+		}
+
+		// Check if P in vertex region outside A
+		Vec3 ap = -a;
+		float d1 = ab.Dot(ap);
+		float d2 = ac.Dot(ap);
+		if (d1 <= 0.0f && d2 <= 0.0f)
+		{
+			outSet = swap_ac.GetX()? 0b0100 : 0b0001;
+			return a; // barycentric coordinates (1,0,0)
+		}
+
+		// Check if P in vertex region outside B
+		Vec3 bp = -inB;
+		float d3 = ab.Dot(bp);
+		float d4 = ac.Dot(bp);
+		if (d3 >= 0.0f && d4 <= d3)
+		{
+			outSet = 0b0010;
+			return inB; // barycentric coordinates (0,1,0)
+		}
+
+		// Check if P in edge region of AB, if so return projection of P onto AB
+		if (d1 * d4 <= d3 * d2 && d1 >= 0.0f && d3 <= 0.0f)
+		{
+			float v = d1 / (d1 - d3);
+			outSet = swap_ac.GetX()? 0b0110 : 0b0011;
+			return a + v * ab; // barycentric coordinates (1-v,v,0)
+		}
+
+		// Check if P in vertex region outside C
+		Vec3 cp = -c;
+		float d5 = ab.Dot(cp);
+		float d6 = ac.Dot(cp);
+		if (d6 >= 0.0f && d5 <= d6)
+		{
+			outSet = swap_ac.GetX()? 0b0001 : 0b0100;
+			return c; // barycentric coordinates (0,0,1)
+		}
+
+		// Check if P in edge region of AC, if so return projection of P onto AC
+		if (d5 * d2 <= d1 * d6 && d2 >= 0.0f && d6 <= 0.0f)
+		{
+			float w = d2 / (d2 - d6);
+			outSet = 0b0101;
+			return a + w * ac; // barycentric coordinates (1-w,0,w)
+		}
+
+		// Check if P in edge region of BC, if so return projection of P onto BC
+		float d4_d3 = d4 - d3;
+		float d5_d6 = d5 - d6;
+		if (d3 * d6 <= d5 * d4 && d4_d3 >= 0.0f && d5_d6 >= 0.0f)
+		{
+			float w = d4_d3 / (d4_d3 + d5_d6);
+			outSet = swap_ac.GetX()? 0b0011 : 0b0110;
+			return inB + w * (c - inB); // barycentric coordinates (0,1-w,w)
+		}
+
+		// P inside face region.
+		// Here we deviate from Christer Ericson's article to improve accuracy.
+		// Determine distance between triangle and origin: distance = (centroid - origin) . normal / |normal|
+		// Closest point to origin is then: distance . normal / |normal|
+		// Note that this way of calculating the closest point is much more accurate than first calculating barycentric coordinates
+		// and then calculating the closest point based on those coordinates.
+		outSet = 0b0111;
+		return n * (a + inB + c).Dot(n) / (3.0f * n_len_sq);
+	}
+
+	/// Check if the origin is outside the plane of triangle (inA, inB, inC). inD specifies the front side of the plane.
+	inline bool OriginOutsideOfPlane(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, Vec3Arg inD)
+	{
+		// Taken from: Real-Time Collision Detection - Christer Ericson (Section: Closest Point on Tetrahedron to Point)
+		// With p = 0
+
+		// Test if point p and d lie on opposite sides of plane through abc
+		Vec3 n = (inB - inA).Cross(inC - inA);
+		float signp = inA.Dot(n); // [AP AB AC]
+		float signd = (inD - inA).Dot(n); // [AD AB AC]
+
+		// Points on opposite sides if expression signs are the same
+		// Note that we left out the minus sign in signp so we need to check > 0 instead of < 0 as in Christer's book
+		// We compare against a small negative value to allow for a little bit of slop in the calculations
+		return signp * signd > -FLT_EPSILON;
+	}
+
+	/// Returns for each of the planes of the tetrahedron if the origin is inside it
+	/// Roughly equivalent to:
+	///	[OriginOutsideOfPlane(inA, inB, inC, inD),
+	///	 OriginOutsideOfPlane(inA, inC, inD, inB),
+	///	 OriginOutsideOfPlane(inA, inD, inB, inC),
+	///	 OriginOutsideOfPlane(inB, inD, inC, inA)]
+	inline UVec4 OriginOutsideOfTetrahedronPlanes(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, Vec3Arg inD)
+	{
+		Vec3 ab = inB - inA;
+		Vec3 ac = inC - inA;
+		Vec3 ad = inD - inA;
+		Vec3 bd = inD - inB;
+		Vec3 bc = inC - inB;
+
+		Vec3 ab_cross_ac = ab.Cross(ac);
+		Vec3 ac_cross_ad = ac.Cross(ad);
+		Vec3 ad_cross_ab = ad.Cross(ab);
+		Vec3 bd_cross_bc = bd.Cross(bc);
+
+		// For each plane get the side on which the origin is
+		float signp0 = inA.Dot(ab_cross_ac); // ABC
+		float signp1 = inA.Dot(ac_cross_ad); // ACD
+		float signp2 = inA.Dot(ad_cross_ab); // ADB
+		float signp3 = inB.Dot(bd_cross_bc); // BDC
+		Vec4 signp(signp0, signp1, signp2, signp3);
+
+		// For each plane get the side that is outside (determined by the 4th point)
+		float signd0 = ad.Dot(ab_cross_ac);  // D
+		float signd1 = ab.Dot(ac_cross_ad);  // B
+		float signd2 = ac.Dot(ad_cross_ab);  // C
+		float signd3 = -ab.Dot(bd_cross_bc); // A
+		Vec4 signd(signd0, signd1, signd2, signd3);
+
+		// The winding of all triangles has been chosen so that signd should have the
+		// same sign for all components. If this is not the case the tetrahedron
+		// is degenerate and we return that the origin is in front of all sides
+		int sign_bits = signd.GetSignBits();
+		switch (sign_bits)
+		{
+		case 0:
+			// All positive
+			return Vec4::sGreaterOrEqual(signp, Vec4::sReplicate(-FLT_EPSILON));
+
+		case 0xf:
+			// All negative
+			return Vec4::sLessOrEqual(signp, Vec4::sReplicate(FLT_EPSILON));
+
+		default:
+			// Mixed signs, degenerate tetrahedron
+			return UVec4::sReplicate(0xffffffff);
+		}
+	}
+
+	/// Get the closest point between tetrahedron (inA, inB, inC, inD) to the origin
+	/// outSet specifies which feature was closest, 1 = a, 2 = b, 4 = c, 8 = d. Edges have 2 bits set, triangles 3 and if the point is in the interior 4 bits are set.
+	/// If MustIncludeD is true, the function assumes that D is part of the closest feature (vertex, edge, face, tetrahedron) and does less work, if the assumption is not true then a closest point to the other features is returned.
+	template <bool MustIncludeD = false>
+	inline Vec3	GetClosestPointOnTetrahedron(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, Vec3Arg inD, uint32 &outSet)
+	{
+		// Taken from: Real-Time Collision Detection - Christer Ericson (Section: Closest Point on Tetrahedron to Point)
+		// With p = 0
+
+		// Start out assuming point inside all halfspaces, so closest to itself
+		uint32 closest_set = 0b1111;
+		Vec3 closest_point = Vec3::sZero();
+		float best_dist_sq = FLT_MAX;
+
+		// Determine for each of the faces of the tetrahedron if the origin is in front of the plane
+		UVec4 origin_out_of_planes = OriginOutsideOfTetrahedronPlanes(inA, inB, inC, inD);
+
+		// If point outside face abc then compute closest point on abc
+		if (origin_out_of_planes.GetX()) // OriginOutsideOfPlane(inA, inB, inC, inD)
+		{
+			if constexpr (MustIncludeD)
+			{
+				// If the closest point must include D then ABC cannot be closest but the closest point
+				// cannot be an interior point either so we return A as closest point
+				closest_set = 0b0001;
+				closest_point = inA;
+			}
+			else
+			{
+				// Test the face normally
+				closest_point = GetClosestPointOnTriangle<false>(inA, inB, inC, closest_set);
+			}
+			best_dist_sq = closest_point.LengthSq();
+		}
+
+		// Repeat test for face acd
+		if (origin_out_of_planes.GetY()) // OriginOutsideOfPlane(inA, inC, inD, inB)
+		{
+			uint32 set;
+			Vec3 q = GetClosestPointOnTriangle<MustIncludeD>(inA, inC, inD, set);
+			float dist_sq = q.LengthSq();
+			if (dist_sq < best_dist_sq)
+			{
+				best_dist_sq = dist_sq;
+				closest_point = q;
+				closest_set = (set & 0b0001) + ((set & 0b0110) << 1);
+			}
+		}
+
+		// Repeat test for face adb
+		if (origin_out_of_planes.GetZ()) // OriginOutsideOfPlane(inA, inD, inB, inC)
+		{
+			// Keep original vertex order, it doesn't matter if the triangle is facing inward or outward
+			// and it improves consistency for GJK which will always add a new vertex D and keep the closest
+			// feature from the previous iteration in ABC
+			uint32 set;
+			Vec3 q = GetClosestPointOnTriangle<MustIncludeD>(inA, inB, inD, set);
+			float dist_sq = q.LengthSq();
+			if (dist_sq < best_dist_sq)
+			{
+				best_dist_sq = dist_sq;
+				closest_point = q;
+				closest_set = (set & 0b0011) + ((set & 0b0100) << 1);
+			}
+		}
+
+		// Repeat test for face bdc
+		if (origin_out_of_planes.GetW()) // OriginOutsideOfPlane(inB, inD, inC, inA)
+		{
+			// Keep original vertex order, it doesn't matter if the triangle is facing inward or outward
+			// and it improves consistency for GJK which will always add a new vertex D and keep the closest
+			// feature from the previous iteration in ABC
+			uint32 set;
+			Vec3 q = GetClosestPointOnTriangle<MustIncludeD>(inB, inC, inD, set);
+			float dist_sq = q.LengthSq();
+			if (dist_sq < best_dist_sq)
+			{
+				closest_point = q;
+				closest_set = set << 1;
+			}
+		}
+
+		outSet = closest_set;
+		return closest_point;
+	}
+};
+
+JPH_PRECISE_MATH_OFF
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder.cpp b/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder.cpp
new file mode 100644
index 0000000000..e15c0912ff
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder.cpp
@@ -0,0 +1,1467 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Geometry/ConvexHullBuilder.h>
+#include <Jolt/Geometry/ConvexHullBuilder2D.h>
+#include <Jolt/Geometry/ClosestPoint.h>
+#include <Jolt/Core/StringTools.h>
+#include <Jolt/Core/UnorderedSet.h>
+
+#ifdef JPH_CONVEX_BUILDER_DUMP_SHAPE
+JPH_SUPPRESS_WARNINGS_STD_BEGIN
+#include <fstream>
+JPH_SUPPRESS_WARNINGS_STD_END
+#endif // JPH_CONVEX_BUILDER_DUMP_SHAPE
+
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif
+
+JPH_NAMESPACE_BEGIN
+
+ConvexHullBuilder::Face::~Face()
+{
+	// Free all edges
+	Edge *e = mFirstEdge;
+	if (e != nullptr)
+	{
+		do
+		{
+			Edge *next = e->mNextEdge;
+			delete e;
+			e = next;
+		} while (e != mFirstEdge);
+	}
+}
+
+void ConvexHullBuilder::Face::CalculateNormalAndCentroid(const Vec3 *inPositions)
+{
+	// Get point that we use to construct a triangle fan
+	Edge *e = mFirstEdge;
+	Vec3 y0 = inPositions[e->mStartIdx];
+
+	// Get the 2nd point
+	e = e->mNextEdge;
+	Vec3 y1 = inPositions[e->mStartIdx];
+
+	// Start accumulating the centroid
+	mCentroid = y0 + y1;
+	int n = 2;
+
+	// Start accumulating the normal
+	mNormal = Vec3::sZero();
+
+	// Loop over remaining edges accumulating normals in a triangle fan fashion
+	for (e = e->mNextEdge; e != mFirstEdge; e = e->mNextEdge)
+	{
+		// Get the 3rd point
+		Vec3 y2 = inPositions[e->mStartIdx];
+
+		// Calculate edges (counter clockwise)
+		Vec3 e0 = y1 - y0;
+		Vec3 e1 = y2 - y1;
+		Vec3 e2 = y0 - y2;
+
+		// The best normal is calculated by using the two shortest edges
+		// See: https://box2d.org/posts/2014/01/troublesome-triangle/
+		// The difference in normals is most pronounced when one edge is much smaller than the others (in which case the others must have roughly the same length).
+		// Therefore we can suffice by just picking the shortest from 2 edges and use that with the 3rd edge to calculate the normal.
+		// We first check which of the edges is shorter: e1 or e2
+		UVec4 e1_shorter_than_e2 = Vec4::sLess(e1.DotV4(e1), e2.DotV4(e2));
+
+		// We calculate both normals and then select the one that had the shortest edge for our normal (this avoids branching)
+		Vec3 normal_e01 = e0.Cross(e1);
+		Vec3 normal_e02 = e2.Cross(e0);
+		mNormal += Vec3::sSelect(normal_e02, normal_e01, e1_shorter_than_e2);
+
+		// Accumulate centroid
+		mCentroid += y2;
+		n++;
+
+		// Update y1 for next triangle
+		y1 = y2;
+	}
+
+	// Finalize centroid
+	mCentroid /= float(n);
+}
+
+void ConvexHullBuilder::Face::Initialize(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions)
+{
+	JPH_ASSERT(mFirstEdge == nullptr);
+	JPH_ASSERT(inIdx0 != inIdx1 && inIdx0 != inIdx2 && inIdx1 != inIdx2);
+
+	// Create 3 edges
+	Edge *e0 = new Edge(this, inIdx0);
+	Edge *e1 = new Edge(this, inIdx1);
+	Edge *e2 = new Edge(this, inIdx2);
+
+	// Link edges
+	e0->mNextEdge = e1;
+	e1->mNextEdge = e2;
+	e2->mNextEdge = e0;
+	mFirstEdge = e0;
+
+	CalculateNormalAndCentroid(inPositions);
+}
+
+ConvexHullBuilder::ConvexHullBuilder(const Positions &inPositions) :
+	mPositions(inPositions)
+{
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+	mIteration = 0;
+
+	// Center the drawing of the first hull around the origin and calculate the delta offset between states
+	mOffset = RVec3::sZero();
+	if (mPositions.empty())
+	{
+		// No hull will be generated
+		mDelta = Vec3::sZero();
+	}
+	else
+	{
+		Vec3 maxv = Vec3::sReplicate(-FLT_MAX), minv = Vec3::sReplicate(FLT_MAX);
+		for (Vec3 v : mPositions)
+		{
+			minv = Vec3::sMin(minv, v);
+			maxv = Vec3::sMax(maxv, v);
+			mOffset -= v;
+		}
+		mOffset /= Real(mPositions.size());
+		mDelta = Vec3((maxv - minv).GetX() + 0.5f, 0, 0);
+		mOffset += mDelta; // Don't start at origin, we're already drawing the final hull there
+	}
+#endif
+}
+
+void ConvexHullBuilder::FreeFaces()
+{
+	for (Face *f : mFaces)
+		delete f;
+	mFaces.clear();
+}
+
+void ConvexHullBuilder::GetFaceForPoint(Vec3Arg inPoint, const Faces &inFaces, Face *&outFace, float &outDistSq) const
+{
+	outFace = nullptr;
+	outDistSq = 0.0f;
+
+	for (Face *f : inFaces)
+		if (!f->mRemoved)
+		{
+			// Determine distance to face
+			float dot = f->mNormal.Dot(inPoint - f->mCentroid);
+			if (dot > 0.0f)
+			{
+				float dist_sq = dot * dot / f->mNormal.LengthSq();
+				if (dist_sq > outDistSq)
+				{
+					outFace = f;
+					outDistSq = dist_sq;
+				}
+			}
+		}
+}
+
+float ConvexHullBuilder::GetDistanceToEdgeSq(Vec3Arg inPoint, const Face *inFace) const
+{
+	bool all_inside = true;
+	float edge_dist_sq = FLT_MAX;
+
+	// Test if it is inside the edges of the polygon
+	Edge *edge = inFace->mFirstEdge;
+	Vec3 p1 = mPositions[edge->GetPreviousEdge()->mStartIdx];
+	do
+	{
+		Vec3 p2 = mPositions[edge->mStartIdx];
+		if ((p2 - p1).Cross(inPoint - p1).Dot(inFace->mNormal) < 0.0f)
+		{
+			// It is outside
+			all_inside = false;
+
+			// Measure distance to this edge
+			uint32 s;
+			edge_dist_sq = min(edge_dist_sq, ClosestPoint::GetClosestPointOnLine(p1 - inPoint, p2 - inPoint, s).LengthSq());
+		}
+		p1 = p2;
+		edge = edge->mNextEdge;
+	} while (edge != inFace->mFirstEdge);
+
+	return all_inside? 0.0f : edge_dist_sq;
+}
+
+bool ConvexHullBuilder::AssignPointToFace(int inPositionIdx, const Faces &inFaces, float inToleranceSq)
+{
+	Vec3 point = mPositions[inPositionIdx];
+
+	// Find the face for which the point is furthest away
+	Face *best_face;
+	float best_dist_sq;
+	GetFaceForPoint(point, inFaces, best_face, best_dist_sq);
+
+	if (best_face != nullptr)
+	{
+		// Check if this point is within the tolerance margin to the plane
+		if (best_dist_sq <= inToleranceSq)
+		{
+			// Check distance to edges
+			float dist_to_edge_sq = GetDistanceToEdgeSq(point, best_face);
+			if (dist_to_edge_sq > inToleranceSq)
+			{
+				// Point is outside of the face and too far away to discard
+				mCoplanarList.push_back({ inPositionIdx, dist_to_edge_sq });
+			}
+		}
+		else
+		{
+			// This point is in front of the face, add it to the conflict list
+			if (best_dist_sq > best_face->mFurthestPointDistanceSq)
+			{
+				// This point is further away than any others, update the distance and add point as last point
+				best_face->mFurthestPointDistanceSq = best_dist_sq;
+				best_face->mConflictList.push_back(inPositionIdx);
+			}
+			else
+			{
+				// Not the furthest point, add it as the before last point
+				best_face->mConflictList.insert(best_face->mConflictList.begin() + best_face->mConflictList.size() - 1, inPositionIdx);
+			}
+
+			return true;
+		}
+	}
+
+	return false;
+}
+
+float ConvexHullBuilder::DetermineCoplanarDistance() const
+{
+	// Formula as per: Implementing Quickhull - Dirk Gregorius.
+	Vec3 vmax = Vec3::sZero();
+	for (Vec3 v : mPositions)
+		vmax = Vec3::sMax(vmax, v.Abs());
+	return 3.0f * FLT_EPSILON * (vmax.GetX() + vmax.GetY() + vmax.GetZ());
+}
+
+int ConvexHullBuilder::GetNumVerticesUsed() const
+{
+	UnorderedSet<int> used_verts;
+	used_verts.reserve(UnorderedSet<int>::size_type(mPositions.size()));
+	for (Face *f : mFaces)
+	{
+		Edge *e = f->mFirstEdge;
+		do
+		{
+			used_verts.insert(e->mStartIdx);
+			e = e->mNextEdge;
+		} while (e != f->mFirstEdge);
+	}
+	return (int)used_verts.size();
+}
+
+bool ConvexHullBuilder::ContainsFace(const Array<int> &inIndices) const
+{
+	for (Face *f : mFaces)
+	{
+		Edge *e = f->mFirstEdge;
+		Array<int>::const_iterator index = std::find(inIndices.begin(), inIndices.end(), e->mStartIdx);
+		if (index != inIndices.end())
+		{
+			size_t matches = 0;
+
+			do
+			{
+				// Check if index matches
+				if (*index != e->mStartIdx)
+					break;
+
+				// Increment number of matches
+				matches++;
+
+				// Next index in list of inIndices
+				index++;
+				if (index == inIndices.end())
+					index = inIndices.begin();
+
+				// Next edge
+				e = e->mNextEdge;
+			} while (e != f->mFirstEdge);
+
+			if (matches == inIndices.size())
+				return true;
+		}
+	}
+
+	return false;
+}
+
+ConvexHullBuilder::EResult ConvexHullBuilder::Initialize(int inMaxVertices, float inTolerance, const char *&outError)
+{
+	// Free the faces possibly left over from an earlier hull
+	FreeFaces();
+
+	// Test that we have at least 3 points
+	if (mPositions.size() < 3)
+	{
+		outError = "Need at least 3 points to make a hull";
+		return EResult::TooFewPoints;
+	}
+
+	// Determine a suitable tolerance for detecting that points are coplanar
+	float coplanar_tolerance_sq = Square(DetermineCoplanarDistance());
+
+	// Increase desired tolerance if accuracy doesn't allow it
+	float tolerance_sq = max(coplanar_tolerance_sq, Square(inTolerance));
+
+	// Find point furthest from the origin
+	int idx1 = -1;
+	float max_dist_sq = -1.0f;
+	for (int i = 0; i < (int)mPositions.size(); ++i)
+	{
+		float dist_sq = mPositions[i].LengthSq();
+		if (dist_sq > max_dist_sq)
+		{
+			max_dist_sq = dist_sq;
+			idx1 = i;
+		}
+	}
+	JPH_ASSERT(idx1 >= 0);
+
+	// Find point that is furthest away from this point
+	int idx2 = -1;
+	max_dist_sq = -1.0f;
+	for (int i = 0; i < (int)mPositions.size(); ++i)
+		if (i != idx1)
+		{
+			float dist_sq = (mPositions[i] - mPositions[idx1]).LengthSq();
+			if (dist_sq > max_dist_sq)
+			{
+				max_dist_sq = dist_sq;
+				idx2 = i;
+			}
+		}
+	JPH_ASSERT(idx2 >= 0);
+
+	// Find point that forms the biggest triangle
+	int idx3 = -1;
+	float best_triangle_area_sq = -1.0f;
+	for (int i = 0; i < (int)mPositions.size(); ++i)
+		if (i != idx1 && i != idx2)
+		{
+			float triangle_area_sq = (mPositions[idx1] - mPositions[i]).Cross(mPositions[idx2] - mPositions[i]).LengthSq();
+			if (triangle_area_sq > best_triangle_area_sq)
+			{
+				best_triangle_area_sq = triangle_area_sq;
+				idx3 = i;
+			}
+		}
+	JPH_ASSERT(idx3 >= 0);
+	if (best_triangle_area_sq < cMinTriangleAreaSq)
+	{
+		outError = "Could not find a suitable initial triangle because its area was too small";
+		return EResult::Degenerate;
+	}
+
+	// Check if we have only 3 vertices
+	if (mPositions.size() == 3)
+	{
+		// Create two triangles (back to back)
+		Face *t1 = CreateTriangle(idx1, idx2, idx3);
+		Face *t2 = CreateTriangle(idx1, idx3, idx2);
+
+		// Link faces edges
+		sLinkFace(t1->mFirstEdge, t2->mFirstEdge->mNextEdge->mNextEdge);
+		sLinkFace(t1->mFirstEdge->mNextEdge, t2->mFirstEdge->mNextEdge);
+		sLinkFace(t1->mFirstEdge->mNextEdge->mNextEdge, t2->mFirstEdge);
+
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+		// Draw current state
+		DrawState();
+#endif
+
+		return EResult::Success;
+	}
+
+	// Find point that forms the biggest tetrahedron
+	Vec3 initial_plane_normal = (mPositions[idx2] - mPositions[idx1]).Cross(mPositions[idx3] - mPositions[idx1]).Normalized();
+	Vec3 initial_plane_centroid = (mPositions[idx1] + mPositions[idx2] + mPositions[idx3]) / 3.0f;
+	int idx4 = -1;
+	float max_dist = 0.0f;
+	for (int i = 0; i < (int)mPositions.size(); ++i)
+		if (i != idx1 && i != idx2 && i != idx3)
+		{
+			float dist = (mPositions[i] - initial_plane_centroid).Dot(initial_plane_normal);
+			if (abs(dist) > abs(max_dist))
+			{
+				max_dist = dist;
+				idx4 = i;
+			}
+		}
+
+	// Check if the hull is coplanar
+	if (Square(max_dist) <= 25.0f * coplanar_tolerance_sq)
+	{
+		// First project all points in 2D space
+		Vec3 base1 = initial_plane_normal.GetNormalizedPerpendicular();
+		Vec3 base2 = initial_plane_normal.Cross(base1);
+		Array<Vec3> positions_2d;
+		positions_2d.reserve(mPositions.size());
+		for (Vec3 v : mPositions)
+			positions_2d.emplace_back(base1.Dot(v), base2.Dot(v), 0.0f);
+
+		// Build hull
+		Array<int> edges_2d;
+		ConvexHullBuilder2D builder_2d(positions_2d);
+		ConvexHullBuilder2D::EResult result = builder_2d.Initialize(idx1, idx2, idx3, inMaxVertices, inTolerance, edges_2d);
+
+		// Create faces (back to back)
+		Face *f1 = CreateFace();
+		Face *f2 = CreateFace();
+
+		// Create edges for face 1
+		Array<Edge *> edges_f1;
+		edges_f1.reserve(edges_2d.size());
+		for (int start_idx : edges_2d)
+		{
+			Edge *edge = new Edge(f1, start_idx);
+			if (edges_f1.empty())
+				f1->mFirstEdge = edge;
+			else
+				edges_f1.back()->mNextEdge = edge;
+			edges_f1.push_back(edge);
+		}
+		edges_f1.back()->mNextEdge = f1->mFirstEdge;
+
+		// Create edges for face 2
+		Array<Edge *> edges_f2;
+		edges_f2.reserve(edges_2d.size());
+		for (int i = (int)edges_2d.size() - 1; i >= 0; --i)
+		{
+			Edge *edge = new Edge(f2, edges_2d[i]);
+			if (edges_f2.empty())
+				f2->mFirstEdge = edge;
+			else
+				edges_f2.back()->mNextEdge = edge;
+			edges_f2.push_back(edge);
+		}
+		edges_f2.back()->mNextEdge = f2->mFirstEdge;
+
+		// Link edges
+		for (size_t i = 0; i < edges_2d.size(); ++i)
+			sLinkFace(edges_f1[i], edges_f2[(2 * edges_2d.size() - 2 - i) % edges_2d.size()]);
+
+		// Calculate the plane for both faces
+		f1->CalculateNormalAndCentroid(mPositions.data());
+		f2->mNormal = -f1->mNormal;
+		f2->mCentroid = f1->mCentroid;
+
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+		// Draw current state
+		DrawState();
+#endif
+
+		return result == ConvexHullBuilder2D::EResult::MaxVerticesReached? EResult::MaxVerticesReached : EResult::Success;
+	}
+
+	// Ensure the planes are facing outwards
+	if (max_dist < 0.0f)
+		std::swap(idx2, idx3);
+
+	// Create tetrahedron
+	Face *t1 = CreateTriangle(idx1, idx2, idx4);
+	Face *t2 = CreateTriangle(idx2, idx3, idx4);
+	Face *t3 = CreateTriangle(idx3, idx1, idx4);
+	Face *t4 = CreateTriangle(idx1, idx3, idx2);
+
+	// Link face edges
+	sLinkFace(t1->mFirstEdge, t4->mFirstEdge->mNextEdge->mNextEdge);
+	sLinkFace(t1->mFirstEdge->mNextEdge, t2->mFirstEdge->mNextEdge->mNextEdge);
+	sLinkFace(t1->mFirstEdge->mNextEdge->mNextEdge, t3->mFirstEdge->mNextEdge);
+	sLinkFace(t2->mFirstEdge, t4->mFirstEdge->mNextEdge);
+	sLinkFace(t2->mFirstEdge->mNextEdge, t3->mFirstEdge->mNextEdge->mNextEdge);
+	sLinkFace(t3->mFirstEdge, t4->mFirstEdge);
+
+	// Build the initial conflict lists
+	Faces faces { t1, t2, t3, t4 };
+	for (int idx = 0; idx < (int)mPositions.size(); ++idx)
+		if (idx != idx1 && idx != idx2 && idx != idx3 && idx != idx4)
+			AssignPointToFace(idx, faces, tolerance_sq);
+
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+	// Draw current state including conflict list
+	DrawState(true);
+
+	// Increment iteration counter
+	++mIteration;
+#endif
+
+	// Overestimate of the actual amount of vertices we use, for limiting the amount of vertices in the hull
+	int num_vertices_used = 4;
+
+	// Loop through the remainder of the points and add them
+	for (;;)
+	{
+		// Find the face with the furthest point on it
+		Face *face_with_furthest_point = nullptr;
+		float furthest_dist_sq = 0.0f;
+		for (Face *f : mFaces)
+			if (f->mFurthestPointDistanceSq > furthest_dist_sq)
+			{
+				furthest_dist_sq = f->mFurthestPointDistanceSq;
+				face_with_furthest_point = f;
+			}
+
+		int furthest_point_idx;
+		if (face_with_furthest_point != nullptr)
+		{
+			// Take the furthest point
+			furthest_point_idx = face_with_furthest_point->mConflictList.back();
+			face_with_furthest_point->mConflictList.pop_back();
+		}
+		else if (!mCoplanarList.empty())
+		{
+			// Try to assign points to faces (this also recalculates the distance to the hull for the coplanar vertices)
+			CoplanarList coplanar;
+			mCoplanarList.swap(coplanar);
+			bool added = false;
+			for (const Coplanar &c : coplanar)
+				added |= AssignPointToFace(c.mPositionIdx, mFaces, tolerance_sq);
+
+			// If we were able to assign a point, loop again to pick it up
+			if (added)
+				continue;
+
+			// If the coplanar list is empty, there are no points left and we're done
+			if (mCoplanarList.empty())
+				break;
+
+			do
+			{
+				// Find the vertex that is furthest from the hull
+				CoplanarList::size_type best_idx = 0;
+				float best_dist_sq = mCoplanarList.front().mDistanceSq;
+				for (CoplanarList::size_type idx = 1; idx < mCoplanarList.size(); ++idx)
+				{
+					const Coplanar &c = mCoplanarList[idx];
+					if (c.mDistanceSq > best_dist_sq)
+					{
+						best_idx = idx;
+						best_dist_sq = c.mDistanceSq;
+					}
+				}
+
+				// Swap it to the end
+				std::swap(mCoplanarList[best_idx], mCoplanarList.back());
+
+				// Remove it
+				furthest_point_idx = mCoplanarList.back().mPositionIdx;
+				mCoplanarList.pop_back();
+
+				// Find the face for which the point is furthest away
+				GetFaceForPoint(mPositions[furthest_point_idx], mFaces, face_with_furthest_point, best_dist_sq);
+			} while (!mCoplanarList.empty() && face_with_furthest_point == nullptr);
+
+			if (face_with_furthest_point == nullptr)
+				break;
+		}
+		else
+		{
+			// If there are no more vertices, we're done
+			break;
+		}
+
+		// Check if we have a limit on the max vertices that we should produce
+		if (num_vertices_used >= inMaxVertices)
+		{
+			// Count the actual amount of used vertices (we did not take the removal of any vertices into account)
+			num_vertices_used = GetNumVerticesUsed();
+
+			// Check if there are too many
+			if (num_vertices_used >= inMaxVertices)
+				return EResult::MaxVerticesReached;
+		}
+
+		// We're about to add another vertex
+		++num_vertices_used;
+
+		// Add the point to the hull
+		Faces new_faces;
+		AddPoint(face_with_furthest_point, furthest_point_idx, coplanar_tolerance_sq, new_faces);
+
+		// Redistribute points on conflict lists belonging to removed faces
+		for (const Face *face : mFaces)
+			if (face->mRemoved)
+				for (int idx : face->mConflictList)
+					AssignPointToFace(idx, new_faces, tolerance_sq);
+
+		// Permanently delete faces that we removed in AddPoint()
+		GarbageCollectFaces();
+
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+		// Draw state at the end of this step including conflict list
+		DrawState(true);
+
+		// Increment iteration counter
+		++mIteration;
+#endif
+	}
+
+	// Check if we are left with a hull. It is possible that hull building fails if the points are nearly coplanar.
+	if (mFaces.size() < 2)
+	{
+		outError = "Too few faces in hull";
+		return EResult::TooFewFaces;
+	}
+
+	return EResult::Success;
+}
+
+void ConvexHullBuilder::AddPoint(Face *inFacingFace, int inIdx, float inCoplanarToleranceSq, Faces &outNewFaces)
+{
+	// Get position
+	Vec3 pos = mPositions[inIdx];
+
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+	// Draw point to be added
+	DebugRenderer::sInstance->DrawMarker(cDrawScale * (mOffset + pos), Color::sYellow, 0.1f);
+	DebugRenderer::sInstance->DrawText3D(cDrawScale * (mOffset + pos), ConvertToString(inIdx), Color::sWhite);
+#endif
+
+#ifdef JPH_ENABLE_ASSERTS
+	// Check if structure is intact
+	ValidateFaces();
+#endif
+
+	// Find edge of convex hull of faces that are not facing the new vertex
+	FullEdges edges;
+	FindEdge(inFacingFace, pos, edges);
+	JPH_ASSERT(edges.size() >= 3);
+
+	// Create new faces
+	outNewFaces.reserve(edges.size());
+	for (const FullEdge &e : edges)
+	{
+		JPH_ASSERT(e.mStartIdx != e.mEndIdx);
+		Face *f = CreateTriangle(e.mStartIdx, e.mEndIdx, inIdx);
+		outNewFaces.push_back(f);
+	}
+
+	// Link edges
+	for (Faces::size_type i = 0; i < outNewFaces.size(); ++i)
+	{
+		sLinkFace(outNewFaces[i]->mFirstEdge, edges[i].mNeighbourEdge);
+		sLinkFace(outNewFaces[i]->mFirstEdge->mNextEdge, outNewFaces[(i + 1) % outNewFaces.size()]->mFirstEdge->mNextEdge->mNextEdge);
+	}
+
+	// Loop on faces that were modified until nothing needs to be checked anymore
+	Faces affected_faces = outNewFaces;
+	while (!affected_faces.empty())
+	{
+		// Take the next face
+		Face *face = affected_faces.back();
+		affected_faces.pop_back();
+
+		if (!face->mRemoved)
+		{
+			// Merge with neighbour if this is a degenerate face
+			MergeDegenerateFace(face, affected_faces);
+
+			// Merge with coplanar neighbours (or when the neighbour forms a concave edge)
+			if (!face->mRemoved)
+				MergeCoplanarOrConcaveFaces(face, inCoplanarToleranceSq, affected_faces);
+		}
+	}
+
+#ifdef JPH_ENABLE_ASSERTS
+	// Check if structure is intact
+	ValidateFaces();
+#endif
+}
+
+void ConvexHullBuilder::GarbageCollectFaces()
+{
+	for (int i = (int)mFaces.size() - 1; i >= 0; --i)
+	{
+		Face *f = mFaces[i];
+		if (f->mRemoved)
+		{
+			FreeFace(f);
+			mFaces.erase(mFaces.begin() + i);
+		}
+	}
+}
+
+ConvexHullBuilder::Face *ConvexHullBuilder::CreateFace()
+{
+	// Call provider to create face
+	Face *f = new Face();
+
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+	// Remember iteration counter
+	f->mIteration = mIteration;
+#endif
+
+	// Add to list
+	mFaces.push_back(f);
+	return f;
+}
+
+ConvexHullBuilder::Face *ConvexHullBuilder::CreateTriangle(int inIdx1, int inIdx2, int inIdx3)
+{
+	Face *f = CreateFace();
+	f->Initialize(inIdx1, inIdx2, inIdx3, mPositions.data());
+	return f;
+}
+
+void ConvexHullBuilder::FreeFace(Face *inFace)
+{
+	JPH_ASSERT(inFace->mRemoved);
+
+#ifdef JPH_ENABLE_ASSERTS
+	// Make sure that this face is not connected
+	Edge *e = inFace->mFirstEdge;
+	if (e != nullptr)
+		do
+		{
+			JPH_ASSERT(e->mNeighbourEdge == nullptr);
+			e = e->mNextEdge;
+		} while (e != inFace->mFirstEdge);
+#endif
+
+	// Free the face
+	delete inFace;
+}
+
+void ConvexHullBuilder::sLinkFace(Edge *inEdge1, Edge *inEdge2)
+{
+	// Check not connected yet
+	JPH_ASSERT(inEdge1->mNeighbourEdge == nullptr);
+	JPH_ASSERT(inEdge2->mNeighbourEdge == nullptr);
+	JPH_ASSERT(inEdge1->mFace != inEdge2->mFace);
+
+	// Check vertices match
+	JPH_ASSERT(inEdge1->mStartIdx == inEdge2->mNextEdge->mStartIdx);
+	JPH_ASSERT(inEdge2->mStartIdx == inEdge1->mNextEdge->mStartIdx);
+
+	// Link up
+	inEdge1->mNeighbourEdge = inEdge2;
+	inEdge2->mNeighbourEdge = inEdge1;
+}
+
+void ConvexHullBuilder::sUnlinkFace(Face *inFace)
+{
+	// Unlink from neighbours
+	Edge *e = inFace->mFirstEdge;
+	do
+	{
+		if (e->mNeighbourEdge != nullptr)
+		{
+			// Validate that neighbour points to us
+			JPH_ASSERT(e->mNeighbourEdge->mNeighbourEdge == e);
+
+			// Unlink
+			e->mNeighbourEdge->mNeighbourEdge = nullptr;
+			e->mNeighbourEdge = nullptr;
+		}
+		e = e->mNextEdge;
+	} while (e != inFace->mFirstEdge);
+}
+
+void ConvexHullBuilder::FindEdge(Face *inFacingFace, Vec3Arg inVertex, FullEdges &outEdges) const
+{
+	// Assert that we were given an empty array
+	JPH_ASSERT(outEdges.empty());
+
+	// Should start with a facing face
+	JPH_ASSERT(inFacingFace->IsFacing(inVertex));
+
+	// Flag as removed
+	inFacingFace->mRemoved = true;
+
+	// Instead of recursing, we build our own stack with the information we need
+	struct StackEntry
+	{
+		Edge *		mFirstEdge;
+		Edge *		mCurrentEdge;
+	};
+	constexpr int cMaxEdgeLength = 128;
+	StackEntry stack[cMaxEdgeLength];
+	int cur_stack_pos = 0;
+
+	static_assert(alignof(Edge) >= 2, "Need lowest bit to indicate to tell if we completed the loop");
+
+	// Start with the face / edge provided
+	stack[0].mFirstEdge = inFacingFace->mFirstEdge;
+	stack[0].mCurrentEdge = reinterpret_cast<Edge *>(reinterpret_cast<uintptr_t>(inFacingFace->mFirstEdge) | 1); // Set lowest bit of pointer to make it different from the first edge
+
+	for (;;)
+	{
+		StackEntry &cur_entry = stack[cur_stack_pos];
+
+		// Next edge
+		Edge *raw_e = cur_entry.mCurrentEdge;
+		Edge *e = reinterpret_cast<Edge *>(reinterpret_cast<uintptr_t>(raw_e) & ~uintptr_t(1)); // Remove the lowest bit which was used to indicate that this is the first edge we're testing
+		cur_entry.mCurrentEdge = e->mNextEdge;
+
+		// If we're back at the first edge we've completed the face and we're done
+		if (raw_e == cur_entry.mFirstEdge)
+		{
+			// This face needs to be removed, unlink it now, caller will free
+			sUnlinkFace(e->mFace);
+
+			// Pop from stack
+			if (--cur_stack_pos < 0)
+				break;
+		}
+		else
+		{
+			// Visit neighbour face
+			Edge *ne = e->mNeighbourEdge;
+			if (ne != nullptr)
+			{
+				Face *n = ne->mFace;
+				if (!n->mRemoved)
+				{
+					// Check if vertex is on the front side of this face
+					if (n->IsFacing(inVertex))
+					{
+						// Vertex on front, this face needs to be removed
+						n->mRemoved = true;
+
+						// Add element to the stack of elements to visit
+						cur_stack_pos++;
+						JPH_ASSERT(cur_stack_pos < cMaxEdgeLength);
+						StackEntry &new_entry = stack[cur_stack_pos];
+						new_entry.mFirstEdge = ne;
+						new_entry.mCurrentEdge = ne->mNextEdge; // We don't need to test this edge again since we came from it
+					}
+					else
+					{
+						// Vertex behind, keep edge
+						FullEdge full;
+						full.mNeighbourEdge = ne;
+						full.mStartIdx = e->mStartIdx;
+						full.mEndIdx = ne->mStartIdx;
+						outEdges.push_back(full);
+					}
+				}
+			}
+		}
+	}
+
+	// Assert that we have a fully connected loop
+#ifdef JPH_ENABLE_ASSERTS
+	for (int i = 0; i < (int)outEdges.size(); ++i)
+		JPH_ASSERT(outEdges[i].mEndIdx == outEdges[(i + 1) % outEdges.size()].mStartIdx);
+#endif
+
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+	// Draw edge of facing faces
+	for (int i = 0; i < (int)outEdges.size(); ++i)
+		DebugRenderer::sInstance->DrawArrow(cDrawScale * (mOffset + mPositions[outEdges[i].mStartIdx]), cDrawScale * (mOffset + mPositions[outEdges[i].mEndIdx]), Color::sWhite, 0.01f);
+	DrawState();
+#endif
+}
+
+void ConvexHullBuilder::MergeFaces(Edge *inEdge)
+{
+	// Get the face
+	Face *face = inEdge->mFace;
+
+	// Find the previous and next edge
+	Edge *next_edge = inEdge->mNextEdge;
+	Edge *prev_edge = inEdge->GetPreviousEdge();
+
+	// Get the other face
+	Edge *other_edge = inEdge->mNeighbourEdge;
+	Face *other_face = other_edge->mFace;
+
+	// Check if attempting to merge with self
+	JPH_ASSERT(face != other_face);
+
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+	DrawWireFace(face, Color::sGreen);
+	DrawWireFace(other_face, Color::sRed);
+	DrawState();
+#endif
+
+	// Loop over the edges of the other face and make them belong to inFace
+	Edge *edge = other_edge->mNextEdge;
+	prev_edge->mNextEdge = edge;
+	for (;;)
+	{
+		edge->mFace = face;
+		if (edge->mNextEdge == other_edge)
+		{
+			// Terminate when we are back at other_edge
+			edge->mNextEdge = next_edge;
+			break;
+		}
+		edge = edge->mNextEdge;
+	}
+
+	// If the first edge happens to be inEdge we need to fix it because this edge is no longer part of the face.
+	// Note that we replace it with the first edge of the merged face so that if the MergeFace function is called
+	// from a loop that loops around the face that it will still terminate after visiting all edges once.
+	if (face->mFirstEdge == inEdge)
+		face->mFirstEdge = prev_edge->mNextEdge;
+
+	// Free the edges
+	delete inEdge;
+	delete other_edge;
+
+	// Mark the other face as removed
+	other_face->mFirstEdge = nullptr;
+	other_face->mRemoved = true;
+
+	// Recalculate plane
+	face->CalculateNormalAndCentroid(mPositions.data());
+
+	// Merge conflict lists
+	if (face->mFurthestPointDistanceSq > other_face->mFurthestPointDistanceSq)
+	{
+		// This face has a point that's further away, make sure it remains the last one as we add the other points to this faces list
+		face->mConflictList.insert(face->mConflictList.end() - 1, other_face->mConflictList.begin(), other_face->mConflictList.end());
+	}
+	else
+	{
+		// The other face has a point that's furthest away, add that list at the end.
+		face->mConflictList.insert(face->mConflictList.end(), other_face->mConflictList.begin(), other_face->mConflictList.end());
+		face->mFurthestPointDistanceSq = other_face->mFurthestPointDistanceSq;
+	}
+	other_face->mConflictList.clear();
+
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+	DrawWireFace(face, Color::sWhite);
+	DrawState();
+#endif
+}
+
+void ConvexHullBuilder::MergeDegenerateFace(Face *inFace, Faces &ioAffectedFaces)
+{
+	// Check area of face
+	if (inFace->mNormal.LengthSq() < cMinTriangleAreaSq)
+	{
+		// Find longest edge, since this face is a sliver this should keep the face convex
+		float max_length_sq = 0.0f;
+		Edge *longest_edge = nullptr;
+		Edge *e = inFace->mFirstEdge;
+		Vec3 p1 = mPositions[e->mStartIdx];
+		do
+		{
+			Edge *next = e->mNextEdge;
+			Vec3 p2 = mPositions[next->mStartIdx];
+			float length_sq = (p2 - p1).LengthSq();
+			if (length_sq >= max_length_sq)
+			{
+				max_length_sq = length_sq;
+				longest_edge = e;
+			}
+			p1 = p2;
+			e = next;
+		} while (e != inFace->mFirstEdge);
+
+		// Merge with face on longest edge
+		MergeFaces(longest_edge);
+
+		// Remove any invalid edges
+		RemoveInvalidEdges(inFace, ioAffectedFaces);
+	}
+}
+
+void ConvexHullBuilder::MergeCoplanarOrConcaveFaces(Face *inFace, float inCoplanarToleranceSq, Faces &ioAffectedFaces)
+{
+	bool merged = false;
+
+	Edge *edge = inFace->mFirstEdge;
+	do
+	{
+		// Store next edge since this edge can be removed
+		Edge *next_edge = edge->mNextEdge;
+
+		// Test if centroid of one face is above plane of the other face by inCoplanarToleranceSq.
+		// If so we need to merge other face into inFace.
+		const Face *other_face = edge->mNeighbourEdge->mFace;
+		Vec3 delta_centroid = other_face->mCentroid - inFace->mCentroid;
+		float dist_other_face_centroid = inFace->mNormal.Dot(delta_centroid);
+		float signed_dist_other_face_centroid_sq = abs(dist_other_face_centroid) * dist_other_face_centroid;
+		float dist_face_centroid = -other_face->mNormal.Dot(delta_centroid);
+		float signed_dist_face_centroid_sq = abs(dist_face_centroid) * dist_face_centroid;
+		float face_normal_len_sq = inFace->mNormal.LengthSq();
+		float other_face_normal_len_sq = other_face->mNormal.LengthSq();
+		if ((signed_dist_other_face_centroid_sq > -inCoplanarToleranceSq * face_normal_len_sq
+			|| signed_dist_face_centroid_sq > -inCoplanarToleranceSq * other_face_normal_len_sq)
+			&& inFace->mNormal.Dot(other_face->mNormal) > 0.0f) // Never merge faces that are back to back
+		{
+			MergeFaces(edge);
+			merged = true;
+		}
+
+		edge = next_edge;
+	} while (edge != inFace->mFirstEdge);
+
+	if (merged)
+		RemoveInvalidEdges(inFace, ioAffectedFaces);
+}
+
+void ConvexHullBuilder::sMarkAffected(Face *inFace, Faces &ioAffectedFaces)
+{
+	if (std::find(ioAffectedFaces.begin(), ioAffectedFaces.end(), inFace) == ioAffectedFaces.end())
+		ioAffectedFaces.push_back(inFace);
+}
+
+void ConvexHullBuilder::RemoveInvalidEdges(Face *inFace, Faces &ioAffectedFaces)
+{
+	// This marks that the plane needs to be recalculated (we delay this until the end of the
+	// function since we don't use the plane and we want to avoid calculating it multiple times)
+	bool recalculate_plane = false;
+
+	// We keep going through this loop until no more edges were removed
+	bool removed;
+	do
+	{
+		removed = false;
+
+		// Loop over all edges in this face
+		Edge *edge = inFace->mFirstEdge;
+		Face *neighbour_face = edge->mNeighbourEdge->mFace;
+		do
+		{
+			Edge *next_edge = edge->mNextEdge;
+			Face *next_neighbour_face = next_edge->mNeighbourEdge->mFace;
+
+			if (neighbour_face == inFace)
+			{
+				// We only remove 1 edge at a time, check if this edge's next edge is our neighbour.
+				// If this check fails, we will continue to scan along the edge until we find an edge where this is the case.
+				if (edge->mNeighbourEdge == next_edge)
+				{
+					// This edge leads back to the starting point, this means the edge is interior and needs to be removed
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+					DrawWireFace(inFace, Color::sBlue);
+					DrawState();
+#endif
+
+					// Remove edge
+					Edge *prev_edge = edge->GetPreviousEdge();
+					prev_edge->mNextEdge = next_edge->mNextEdge;
+					if (inFace->mFirstEdge == edge || inFace->mFirstEdge == next_edge)
+						inFace->mFirstEdge = prev_edge;
+					delete edge;
+					delete next_edge;
+
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+					DrawWireFace(inFace, Color::sGreen);
+					DrawState();
+#endif
+
+					// Check if inFace now has only 2 edges left
+					if (RemoveTwoEdgeFace(inFace, ioAffectedFaces))
+						return; // Bail if face no longer exists
+
+					// Restart the loop
+					recalculate_plane = true;
+					removed = true;
+					break;
+				}
+			}
+			else if (neighbour_face == next_neighbour_face)
+			{
+				// There are two edges that connect to the same face, we will remove the second one
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+				DrawWireFace(inFace, Color::sYellow);
+				DrawWireFace(neighbour_face, Color::sRed);
+				DrawState();
+#endif
+
+				// First merge the neighbours edges
+				Edge *neighbour_edge = next_edge->mNeighbourEdge;
+				Edge *next_neighbour_edge = neighbour_edge->mNextEdge;
+				if (neighbour_face->mFirstEdge == next_neighbour_edge)
+					neighbour_face->mFirstEdge = neighbour_edge;
+				neighbour_edge->mNextEdge = next_neighbour_edge->mNextEdge;
+				neighbour_edge->mNeighbourEdge = edge;
+				delete next_neighbour_edge;
+
+				// Then merge my own edges
+				if (inFace->mFirstEdge == next_edge)
+					inFace->mFirstEdge = edge;
+				edge->mNextEdge = next_edge->mNextEdge;
+				edge->mNeighbourEdge = neighbour_edge;
+				delete next_edge;
+
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+				DrawWireFace(inFace, Color::sYellow);
+				DrawWireFace(neighbour_face, Color::sGreen);
+				DrawState();
+#endif
+
+				// Check if neighbour has only 2 edges left
+				if (!RemoveTwoEdgeFace(neighbour_face, ioAffectedFaces))
+				{
+					// No, we need to recalculate its plane
+					neighbour_face->CalculateNormalAndCentroid(mPositions.data());
+
+					// Mark neighbour face as affected
+					sMarkAffected(neighbour_face, ioAffectedFaces);
+				}
+
+				// Check if inFace now has only 2 edges left
+				if (RemoveTwoEdgeFace(inFace, ioAffectedFaces))
+					return; // Bail if face no longer exists
+
+				// Restart loop
+				recalculate_plane = true;
+				removed = true;
+				break;
+			}
+
+			// This edge is ok, go to the next edge
+			edge = next_edge;
+			neighbour_face = next_neighbour_face;
+
+		} while (edge != inFace->mFirstEdge);
+	} while (removed);
+
+	// Recalculate plane?
+	if (recalculate_plane)
+		inFace->CalculateNormalAndCentroid(mPositions.data());
+}
+
+bool ConvexHullBuilder::RemoveTwoEdgeFace(Face *inFace, Faces &ioAffectedFaces) const
+{
+	// Check if this face contains only 2 edges
+	Edge *edge = inFace->mFirstEdge;
+	Edge *next_edge = edge->mNextEdge;
+	JPH_ASSERT(edge != next_edge); // 1 edge faces should not exist
+	if (next_edge->mNextEdge == edge)
+	{
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+		DrawWireFace(inFace, Color::sRed);
+		DrawState();
+#endif
+
+		// Schedule both neighbours for re-checking
+		Edge *neighbour_edge = edge->mNeighbourEdge;
+		Face *neighbour_face = neighbour_edge->mFace;
+		Edge *next_neighbour_edge = next_edge->mNeighbourEdge;
+		Face *next_neighbour_face = next_neighbour_edge->mFace;
+		sMarkAffected(neighbour_face, ioAffectedFaces);
+		sMarkAffected(next_neighbour_face, ioAffectedFaces);
+
+		// Link my neighbours to each other
+		neighbour_edge->mNeighbourEdge = next_neighbour_edge;
+		next_neighbour_edge->mNeighbourEdge = neighbour_edge;
+
+		// Unlink my edges
+		edge->mNeighbourEdge = nullptr;
+		next_edge->mNeighbourEdge = nullptr;
+
+		// Mark this face as removed
+		inFace->mRemoved = true;
+
+		return true;
+	}
+
+	return false;
+}
+
+#ifdef JPH_ENABLE_ASSERTS
+
+void ConvexHullBuilder::DumpFace(const Face *inFace) const
+{
+	Trace("f:0x%p", inFace);
+
+	const Edge *e = inFace->mFirstEdge;
+	do
+	{
+		Trace("\te:0x%p { i:%d e:0x%p f:0x%p }", e, e->mStartIdx, e->mNeighbourEdge, e->mNeighbourEdge->mFace);
+		e = e->mNextEdge;
+	} while (e != inFace->mFirstEdge);
+}
+
+void ConvexHullBuilder::DumpFaces() const
+{
+	Trace("Dump Faces:");
+
+	for (const Face *f : mFaces)
+		if (!f->mRemoved)
+			DumpFace(f);
+}
+
+void ConvexHullBuilder::ValidateFace(const Face *inFace) const
+{
+	if (inFace->mRemoved)
+	{
+		const Edge *e = inFace->mFirstEdge;
+		if (e != nullptr)
+			do
+			{
+				JPH_ASSERT(e->mNeighbourEdge == nullptr);
+				e = e->mNextEdge;
+			} while (e != inFace->mFirstEdge);
+	}
+	else
+	{
+		int edge_count = 0;
+
+		const Edge *e = inFace->mFirstEdge;
+		do
+		{
+			// Count edge
+			++edge_count;
+
+			// Validate that adjacent faces are all different
+			if (mFaces.size() > 2)
+				for (const Edge *other_edge = e->mNextEdge; other_edge != inFace->mFirstEdge; other_edge = other_edge->mNextEdge)
+					JPH_ASSERT(e->mNeighbourEdge->mFace != other_edge->mNeighbourEdge->mFace);
+
+			// Assert that the face is correct
+			JPH_ASSERT(e->mFace == inFace);
+
+			// Assert that we have a neighbour
+			const Edge *nb_edge = e->mNeighbourEdge;
+			JPH_ASSERT(nb_edge != nullptr);
+			if (nb_edge != nullptr)
+			{
+				// Assert that our neighbours edge points to us
+				JPH_ASSERT(nb_edge->mNeighbourEdge == e);
+
+				// Assert that it belongs to a different face
+				JPH_ASSERT(nb_edge->mFace != inFace);
+
+				// Assert that the next edge of the neighbour points to the same vertex as this edge's vertex
+				JPH_ASSERT(nb_edge->mNextEdge->mStartIdx == e->mStartIdx);
+
+				// Assert that my next edge points to the same vertex as my neighbours vertex
+				JPH_ASSERT(e->mNextEdge->mStartIdx == nb_edge->mStartIdx);
+			}
+			e = e->mNextEdge;
+		} while (e != inFace->mFirstEdge);
+
+		// Assert that we have 3 or more edges
+		JPH_ASSERT(edge_count >= 3);
+	}
+}
+
+void ConvexHullBuilder::ValidateFaces() const
+{
+	for (const Face *f : mFaces)
+		ValidateFace(f);
+}
+
+#endif // JPH_ENABLE_ASSERTS
+
+void ConvexHullBuilder::GetCenterOfMassAndVolume(Vec3 &outCenterOfMass, float &outVolume) const
+{
+	// Fourth point is the average of all face centroids
+	Vec3 v4 = Vec3::sZero();
+	for (const Face *f : mFaces)
+		v4 += f->mCentroid;
+	v4 /= float(mFaces.size());
+
+	// Calculate mass and center of mass of this convex hull by summing all tetrahedrons
+	outVolume = 0.0f;
+	outCenterOfMass = Vec3::sZero();
+	for (const Face *f : mFaces)
+	{
+		// Get the first vertex that we'll use to create a triangle fan
+		Edge *e = f->mFirstEdge;
+		Vec3 v1 = mPositions[e->mStartIdx];
+
+		// Get the second vertex
+		e = e->mNextEdge;
+		Vec3 v2 = mPositions[e->mStartIdx];
+
+		for (e = e->mNextEdge; e != f->mFirstEdge; e = e->mNextEdge)
+		{
+			// Fetch the last point of the triangle
+			Vec3 v3 = mPositions[e->mStartIdx];
+
+			// Calculate center of mass and mass of this tetrahedron,
+			// see: https://en.wikipedia.org/wiki/Tetrahedron#Volume
+			float volume_tetrahedron = (v1 - v4).Dot((v2 - v4).Cross(v3 - v4)); // Needs to be divided by 6, postpone this until the end of the loop
+			Vec3 center_of_mass_tetrahedron = v1 + v2 + v3 + v4; // Needs to be divided by 4, postpone this until the end of the loop
+
+			// Accumulate results
+			outVolume += volume_tetrahedron;
+			outCenterOfMass += volume_tetrahedron * center_of_mass_tetrahedron;
+
+			// Update v2 for next triangle
+			v2 = v3;
+		}
+	}
+
+	// Calculate center of mass, fall back to average point in case there is no volume (everything is on a plane in this case)
+	if (outVolume > FLT_EPSILON)
+		outCenterOfMass /= 4.0f * outVolume;
+	else
+		outCenterOfMass = v4;
+
+	outVolume /= 6.0f;
+}
+
+void ConvexHullBuilder::DetermineMaxError(Face *&outFaceWithMaxError, float &outMaxError, int &outMaxErrorPositionIdx, float &outCoplanarDistance) const
+{
+	outCoplanarDistance = DetermineCoplanarDistance();
+
+	// This measures the distance from a polygon to the furthest point outside of the hull
+	float max_error = 0.0f;
+	Face *max_error_face = nullptr;
+	int max_error_point = -1;
+
+	for (int i = 0; i < (int)mPositions.size(); ++i)
+	{
+		Vec3 v = mPositions[i];
+
+		// This measures the closest edge from all faces to point v
+		// Note that we take the min of all faces since there may be multiple near coplanar faces so if we were to test this per face
+		// we may find that a point is outside of a polygon and mark it as an error, while it is actually inside a nearly coplanar
+		// polygon.
+		float min_edge_dist_sq = FLT_MAX;
+		Face *min_edge_dist_face = nullptr;
+
+		for (Face *f : mFaces)
+		{
+			// Check if point is on or in front of plane
+			float normal_len = f->mNormal.Length();
+			JPH_ASSERT(normal_len > 0.0f);
+			float plane_dist = f->mNormal.Dot(v - f->mCentroid) / normal_len;
+			if (plane_dist > -outCoplanarDistance)
+			{
+				// Check distance to the edges of this face
+				float edge_dist_sq = GetDistanceToEdgeSq(v, f);
+				if (edge_dist_sq < min_edge_dist_sq)
+				{
+					min_edge_dist_sq = edge_dist_sq;
+					min_edge_dist_face = f;
+				}
+
+				// If the point is inside the polygon and the point is in front of the plane, measure the distance
+				if (edge_dist_sq == 0.0f && plane_dist > max_error)
+				{
+					max_error = plane_dist;
+					max_error_face = f;
+					max_error_point = i;
+				}
+			}
+		}
+
+		// If the minimum distance to an edge is further than our current max error, we use that as max error
+		float min_edge_dist = sqrt(min_edge_dist_sq);
+		if (min_edge_dist_face != nullptr && min_edge_dist > max_error)
+		{
+			max_error = min_edge_dist;
+			max_error_face = min_edge_dist_face;
+			max_error_point = i;
+		}
+	}
+
+	outFaceWithMaxError = max_error_face;
+	outMaxError = max_error;
+	outMaxErrorPositionIdx = max_error_point;
+}
+
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+
+void ConvexHullBuilder::DrawState(bool inDrawConflictList) const
+{
+	// Draw origin
+	DebugRenderer::sInstance->DrawMarker(cDrawScale * mOffset, Color::sRed, 0.2f);
+
+	int face_idx = 0;
+
+	// Draw faces
+	for (const Face *f : mFaces)
+		if (!f->mRemoved)
+		{
+			Color iteration_color = Color::sGetDistinctColor(f->mIteration);
+			Color face_color = Color::sGetDistinctColor(face_idx++);
+
+			// First point
+			const Edge *e = f->mFirstEdge;
+			RVec3 p1 = cDrawScale * (mOffset + mPositions[e->mStartIdx]);
+
+			// Second point
+			e = e->mNextEdge;
+			RVec3 p2 = cDrawScale * (mOffset + mPositions[e->mStartIdx]);
+
+			// First line
+			DebugRenderer::sInstance->DrawLine(p1, p2, Color::sGrey);
+
+			do
+			{
+				// Third point
+				e = e->mNextEdge;
+				RVec3 p3 = cDrawScale * (mOffset + mPositions[e->mStartIdx]);
+
+				DebugRenderer::sInstance->DrawTriangle(p1, p2, p3, iteration_color);
+
+				DebugRenderer::sInstance->DrawLine(p2, p3, Color::sGrey);
+
+				p2 = p3;
+			}
+			while (e != f->mFirstEdge);
+
+			// Draw normal
+			RVec3 centroid = cDrawScale * (mOffset + f->mCentroid);
+			DebugRenderer::sInstance->DrawArrow(centroid, centroid + f->mNormal.NormalizedOr(Vec3::sZero()), face_color, 0.01f);
+
+			// Draw conflict list
+			if (inDrawConflictList)
+				for (int idx : f->mConflictList)
+					DebugRenderer::sInstance->DrawMarker(cDrawScale * (mOffset + mPositions[idx]), face_color, 0.05f);
+		}
+
+	// Offset to the right
+	mOffset += mDelta;
+}
+
+void ConvexHullBuilder::DrawWireFace(const Face *inFace, ColorArg inColor) const
+{
+	const Edge *e = inFace->mFirstEdge;
+	RVec3 prev = cDrawScale * (mOffset + mPositions[e->mStartIdx]);
+	do
+	{
+		const Edge *next = e->mNextEdge;
+		RVec3 cur = cDrawScale * (mOffset + mPositions[next->mStartIdx]);
+		DebugRenderer::sInstance->DrawArrow(prev, cur, inColor, 0.01f);
+		DebugRenderer::sInstance->DrawText3D(prev, ConvertToString(e->mStartIdx), inColor);
+		e = next;
+		prev = cur;
+	} while (e != inFace->mFirstEdge);
+}
+
+void ConvexHullBuilder::DrawEdge(const Edge *inEdge, ColorArg inColor) const
+{
+	RVec3 p1 = cDrawScale * (mOffset + mPositions[inEdge->mStartIdx]);
+	RVec3 p2 = cDrawScale * (mOffset + mPositions[inEdge->mNextEdge->mStartIdx]);
+	DebugRenderer::sInstance->DrawArrow(p1, p2, inColor, 0.01f);
+}
+
+#endif // JPH_CONVEX_BUILDER_DEBUG
+
+#ifdef JPH_CONVEX_BUILDER_DUMP_SHAPE
+
+void ConvexHullBuilder::DumpShape() const
+{
+	static atomic<int> sShapeNo = 1;
+	int shape_no = sShapeNo++;
+
+	std::ofstream f;
+	f.open(StringFormat("dumped_shape%d.cpp", shape_no).c_str(), std::ofstream::out | std::ofstream::trunc);
+	if (!f.is_open())
+		return;
+
+	f << "{\n";
+	for (Vec3 v : mPositions)
+		f << StringFormat("\tVec3(%.9gf, %.9gf, %.9gf),\n", (double)v.GetX(), (double)v.GetY(), (double)v.GetZ());
+	f << "},\n";
+}
+
+#endif // JPH_CONVEX_BUILDER_DUMP_SHAPE
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder.h b/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder.h
new file mode 100644
index 0000000000..db1ef358d1
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder.h
@@ -0,0 +1,276 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+//#define JPH_CONVEX_BUILDER_DEBUG
+//#define JPH_CONVEX_BUILDER_DUMP_SHAPE
+
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+	#include <Jolt/Core/Color.h>
+#endif
+
+#include <Jolt/Core/StaticArray.h>
+#include <Jolt/Core/NonCopyable.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// A convex hull builder that tries to create hulls as accurately as possible. Used for offline processing.
+class JPH_EXPORT ConvexHullBuilder : public NonCopyable
+{
+public:
+	// Forward declare
+	class Face;
+
+	/// Class that holds the information of an edge
+	class Edge : public NonCopyable
+	{
+	public:
+		JPH_OVERRIDE_NEW_DELETE
+
+		/// Constructor
+						Edge(Face *inFace, int inStartIdx)	: mFace(inFace), mStartIdx(inStartIdx) { }
+
+		/// Get the previous edge
+		inline Edge *	GetPreviousEdge()
+		{
+			Edge *prev_edge = this;
+			while (prev_edge->mNextEdge != this)
+				prev_edge = prev_edge->mNextEdge;
+			return prev_edge;
+		}
+
+		Face *			mFace;								///< Face that this edge belongs to
+		Edge *			mNextEdge = nullptr;				///< Next edge of this face
+		Edge *			mNeighbourEdge = nullptr;			///< Edge that this edge is connected to
+		int				mStartIdx;							///< Vertex index in mPositions that indicates the start vertex of this edge
+	};
+
+	using ConflictList = Array<int>;
+
+	/// Class that holds the information of one face
+	class Face : public NonCopyable
+	{
+	public:
+		JPH_OVERRIDE_NEW_DELETE
+
+		/// Destructor
+						~Face();
+
+		/// Initialize a face with three indices
+		void			Initialize(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions);
+
+		/// Calculates the centroid and normal for this face
+		void			CalculateNormalAndCentroid(const Vec3 *inPositions);
+
+		/// Check if face inFace is facing inPosition
+		inline bool		IsFacing(Vec3Arg inPosition) const
+		{
+			JPH_ASSERT(!mRemoved);
+			return mNormal.Dot(inPosition - mCentroid) > 0.0f;
+		}
+
+		Vec3			mNormal;							///< Normal of this face, length is 2 times area of face
+		Vec3			mCentroid;							///< Center of the face
+		ConflictList	mConflictList;						///< Positions associated with this edge (that are closest to this edge). The last position in the list is the point that is furthest away from the face.
+		Edge *			mFirstEdge = nullptr;				///< First edge of this face
+		float			mFurthestPointDistanceSq = 0.0f;	///< Squared distance of furthest point from the conflict list to the face
+		bool			mRemoved = false;					///< Flag that indicates that face has been removed (face will be freed later)
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+		int				mIteration;							///< Iteration that this face was created
+#endif
+	};
+
+	// Typedefs
+	using Positions = Array<Vec3>;
+	using Faces = Array<Face *>;
+
+	/// Constructor
+	explicit			ConvexHullBuilder(const Positions &inPositions);
+
+	/// Destructor
+						~ConvexHullBuilder()				{ FreeFaces(); }
+
+	/// Result enum that indicates how the hull got created
+	enum class EResult
+	{
+		Success,											///< Hull building finished successfully
+		MaxVerticesReached,									///< Hull building finished successfully, but the desired accuracy was not reached because the max vertices limit was reached
+		TooFewPoints,										///< Too few points to create a hull
+		TooFewFaces,										///< Too few faces in the created hull (signifies precision errors during building)
+		Degenerate,											///< Degenerate hull detected
+	};
+
+	/// Takes all positions as provided by the constructor and use them to build a hull
+	/// Any points that are closer to the hull than inTolerance will be discarded
+	/// @param inMaxVertices Max vertices to allow in the hull. Specify INT_MAX if there is no limit.
+	/// @param inTolerance Max distance that a point is allowed to be outside of the hull
+	/// @param outError Error message when building fails
+	/// @return Status code that reports if the hull was created or not
+	EResult				Initialize(int inMaxVertices, float inTolerance, const char *&outError);
+
+	/// Returns the amount of vertices that are currently used by the hull
+	int					GetNumVerticesUsed() const;
+
+	/// Returns true if the hull contains a polygon with inIndices (counter clockwise indices in mPositions)
+	bool				ContainsFace(const Array<int> &inIndices) const;
+
+	/// Calculate the center of mass and the volume of the current convex hull
+	void				GetCenterOfMassAndVolume(Vec3 &outCenterOfMass, float &outVolume) const;
+
+	/// Determines the point that is furthest outside of the hull and reports how far it is outside of the hull (which indicates a failure during hull building)
+	/// @param outFaceWithMaxError The face that caused the error
+	/// @param outMaxError The maximum distance of a point to the hull
+	/// @param outMaxErrorPositionIdx The index of the point that had this distance
+	/// @param outCoplanarDistance Points that are less than this distance from the hull are considered on the hull. This should be used as a lowerbound for the allowed error.
+	void				DetermineMaxError(Face *&outFaceWithMaxError, float &outMaxError, int &outMaxErrorPositionIdx, float &outCoplanarDistance) const;
+
+	/// Access to the created faces. Memory is owned by the convex hull builder.
+	const Faces &		GetFaces() const					{ return mFaces; }
+
+private:
+	/// Minimal square area of a triangle (used for merging and checking if a triangle is degenerate)
+	static constexpr float cMinTriangleAreaSq = 1.0e-12f;
+
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+	/// Factor to scale convex hull when debug drawing the construction process
+	static constexpr Real cDrawScale = 10;
+#endif
+
+	/// Class that holds an edge including start and end index
+	class FullEdge
+	{
+	public:
+		Edge *			mNeighbourEdge;						///< Edge that this edge is connected to
+		int				mStartIdx;							///< Vertex index in mPositions that indicates the start vertex of this edge
+		int				mEndIdx;							///< Vertex index in mPosition that indicates the end vertex of this edge
+	};
+
+	// Private typedefs
+	using FullEdges = Array<FullEdge>;
+
+	// Determine a suitable tolerance for detecting that points are coplanar
+	float				DetermineCoplanarDistance() const;
+
+	/// Find the face for which inPoint is furthest to the front
+	/// @param inPoint Point to test
+	/// @param inFaces List of faces to test
+	/// @param outFace Returns the best face
+	/// @param outDistSq Returns the squared distance how much inPoint is in front of the plane of the face
+	void				GetFaceForPoint(Vec3Arg inPoint, const Faces &inFaces, Face *&outFace, float &outDistSq) const;
+
+	/// @brief Calculates the distance between inPoint and inFace
+	/// @param inFace Face to test
+	/// @param inPoint Point to test
+	/// @return If the projection of the point on the plane is interior to the face 0, otherwise the squared distance to the closest edge
+	float				GetDistanceToEdgeSq(Vec3Arg inPoint, const Face *inFace) const;
+
+	/// Assigns a position to one of the supplied faces based on which face is closest.
+	/// @param inPositionIdx Index of the position to add
+	/// @param inFaces List of faces to consider
+	/// @param inToleranceSq Tolerance of the hull, if the point is closer to the face than this, we ignore it
+	/// @return True if point was assigned, false if it was discarded or added to the coplanar list
+	bool				AssignPointToFace(int inPositionIdx, const Faces &inFaces, float inToleranceSq);
+
+	/// Add a new point to the convex hull
+	void				AddPoint(Face *inFacingFace, int inIdx, float inToleranceSq, Faces &outNewFaces);
+
+	/// Remove all faces that have been marked 'removed' from mFaces list
+	void				GarbageCollectFaces();
+
+	/// Create a new face
+	Face *				CreateFace();
+
+	/// Create a new triangle
+	Face *				CreateTriangle(int inIdx1, int inIdx2, int inIdx3);
+
+	/// Delete a face (checking that it is not connected to any other faces)
+	void				FreeFace(Face *inFace);
+
+	/// Release all faces and edges
+	void				FreeFaces();
+
+	/// Link face edge to other face edge
+	static void			sLinkFace(Edge *inEdge1, Edge *inEdge2);
+
+	/// Unlink this face from all of its neighbours
+	static void			sUnlinkFace(Face *inFace);
+
+	/// Given one face that faces inVertex, find the edges of the faces that are not facing inVertex.
+	/// Will flag all those faces for removal.
+	void				FindEdge(Face *inFacingFace, Vec3Arg inVertex, FullEdges &outEdges) const;
+
+	/// Merges the two faces that share inEdge into the face inEdge->mFace
+	void				MergeFaces(Edge *inEdge);
+
+	/// Merges inFace with a neighbour if it is degenerate (a sliver)
+	void				MergeDegenerateFace(Face *inFace, Faces &ioAffectedFaces);
+
+	/// Merges any coplanar as well as neighbours that form a non-convex edge into inFace.
+	/// Faces are considered coplanar if the distance^2 of the other face's centroid is smaller than inToleranceSq.
+	void				MergeCoplanarOrConcaveFaces(Face *inFace, float inToleranceSq, Faces &ioAffectedFaces);
+
+	/// Mark face as affected if it is not already in the list
+	static void			sMarkAffected(Face *inFace, Faces &ioAffectedFaces);
+
+	/// Removes all invalid edges.
+	/// 1. Merges inFace with faces that share two edges with it since this means inFace or the other face cannot be convex or the edge is colinear.
+	/// 2. Removes edges that are interior to inFace (that have inFace on both sides)
+	/// Any faces that need to be checked for validity will be added to ioAffectedFaces.
+	void				RemoveInvalidEdges(Face *inFace, Faces &ioAffectedFaces);
+
+	/// Removes inFace if it consists of only 2 edges, linking its neighbouring faces together
+	/// Any faces that need to be checked for validity will be added to ioAffectedFaces.
+	/// @return True if face was removed.
+	bool				RemoveTwoEdgeFace(Face *inFace, Faces &ioAffectedFaces) const;
+
+#ifdef JPH_ENABLE_ASSERTS
+	/// Dumps the text representation of a face to the TTY
+	void				DumpFace(const Face *inFace) const;
+
+	/// Dumps the text representation of all faces to the TTY
+	void				DumpFaces() const;
+
+	/// Check consistency of 1 face
+	void				ValidateFace(const Face *inFace) const;
+
+	/// Check consistency of all faces
+	void				ValidateFaces() const;
+#endif
+
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+	/// Draw state of algorithm
+	void				DrawState(bool inDrawConflictList = false) const;
+
+	/// Draw a face for debugging purposes
+	void				DrawWireFace(const Face *inFace, ColorArg inColor) const;
+
+	/// Draw an edge for debugging purposes
+	void				DrawEdge(const Edge *inEdge, ColorArg inColor) const;
+#endif
+
+#ifdef JPH_CONVEX_BUILDER_DUMP_SHAPE
+	void				DumpShape() const;
+#endif
+
+	const Positions &	mPositions;							///< List of positions (some of them are part of the hull)
+	Faces				mFaces;								///< List of faces that are part of the hull (if !mRemoved)
+
+	struct Coplanar
+	{
+		int				mPositionIdx;						///< Index in mPositions
+		float			mDistanceSq;						///< Distance to the edge of closest face (should be > 0)
+	};
+	using CoplanarList = Array<Coplanar>;
+
+	CoplanarList		mCoplanarList;						///< List of positions that are coplanar to a face but outside of the face, these are added to the hull at the end
+
+#ifdef JPH_CONVEX_BUILDER_DEBUG
+	int					mIteration;							///< Number of iterations we've had so far (for debug purposes)
+	mutable RVec3		mOffset;							///< Offset to use for state drawing
+	Vec3				mDelta;								///< Delta offset between next states
+#endif
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder2D.cpp b/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder2D.cpp
new file mode 100644
index 0000000000..c821f92eef
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder2D.cpp
@@ -0,0 +1,335 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Geometry/ConvexHullBuilder2D.h>
+
+#ifdef JPH_CONVEX_BUILDER_2D_DEBUG
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif
+
+JPH_NAMESPACE_BEGIN
+
+void ConvexHullBuilder2D::Edge::CalculateNormalAndCenter(const Vec3 *inPositions)
+{
+	Vec3 p1 = inPositions[mStartIdx];
+	Vec3 p2 = inPositions[mNextEdge->mStartIdx];
+
+	// Center of edge
+	mCenter = 0.5f * (p1 + p2);
+
+	// Create outward pointing normal.
+	// We have two choices for the normal (which satisfies normal . edge = 0):
+	// normal1 = (-edge.y, edge.x, 0)
+	// normal2 = (edge.y, -edge.x, 0)
+	// We want (normal x edge).z > 0 so that the normal points out of the polygon. Only normal2 satisfies this condition.
+	Vec3 edge = p2 - p1;
+	mNormal = Vec3(edge.GetY(), -edge.GetX(), 0);
+}
+
+ConvexHullBuilder2D::ConvexHullBuilder2D(const Positions &inPositions) :
+	mPositions(inPositions)
+{
+#ifdef JPH_CONVEX_BUILDER_2D_DEBUG
+	// Center the drawing of the first hull around the origin and calculate the delta offset between states
+	mOffset = RVec3::sZero();
+	if (mPositions.empty())
+	{
+		// No hull will be generated
+		mDelta = Vec3::sZero();
+	}
+	else
+	{
+		Vec3 maxv = Vec3::sReplicate(-FLT_MAX), minv = Vec3::sReplicate(FLT_MAX);
+		for (Vec3 v : mPositions)
+		{
+			minv = Vec3::sMin(minv, v);
+			maxv = Vec3::sMax(maxv, v);
+			mOffset -= v;
+		}
+		mOffset /= Real(mPositions.size());
+		mDelta = Vec3((maxv - minv).GetX() + 0.5f, 0, 0);
+		mOffset += mDelta; // Don't start at origin, we're already drawing the final hull there
+	}
+#endif
+}
+
+ConvexHullBuilder2D::~ConvexHullBuilder2D()
+{
+	FreeEdges();
+}
+
+void ConvexHullBuilder2D::FreeEdges()
+{
+	if (mFirstEdge == nullptr)
+		return;
+
+	Edge *edge = mFirstEdge;
+	do
+	{
+		Edge *next = edge->mNextEdge;
+		delete edge;
+		edge = next;
+	} while (edge != mFirstEdge);
+
+	mFirstEdge = nullptr;
+	mNumEdges = 0;
+}
+
+#ifdef JPH_ENABLE_ASSERTS
+
+void ConvexHullBuilder2D::ValidateEdges() const
+{
+	if (mFirstEdge == nullptr)
+	{
+		JPH_ASSERT(mNumEdges == 0);
+		return;
+	}
+
+	int count = 0;
+
+	Edge *edge = mFirstEdge;
+	do
+	{
+		// Validate connectivity
+		JPH_ASSERT(edge->mNextEdge->mPrevEdge == edge);
+		JPH_ASSERT(edge->mPrevEdge->mNextEdge == edge);
+
+		++count;
+		edge = edge->mNextEdge;
+	} while (edge != mFirstEdge);
+
+	// Validate that count matches
+	JPH_ASSERT(count == mNumEdges);
+}
+
+#endif // JPH_ENABLE_ASSERTS
+
+void ConvexHullBuilder2D::AssignPointToEdge(int inPositionIdx, const Array<Edge *> &inEdges) const
+{
+	Vec3 point = mPositions[inPositionIdx];
+
+	Edge *best_edge = nullptr;
+	float best_dist_sq = 0.0f;
+
+	// Test against all edges
+	for (Edge *edge : inEdges)
+	{
+		// Determine distance to edge
+		float dot = edge->mNormal.Dot(point - edge->mCenter);
+		if (dot > 0.0f)
+		{
+			float dist_sq = dot * dot / edge->mNormal.LengthSq();
+			if (dist_sq > best_dist_sq)
+			{
+				best_edge = edge;
+				best_dist_sq = dist_sq;
+			}
+		}
+	}
+
+	// If this point is in front of the edge, add it to the conflict list
+	if (best_edge != nullptr)
+	{
+		if (best_dist_sq > best_edge->mFurthestPointDistanceSq)
+		{
+			// This point is further away than any others, update the distance and add point as last point
+			best_edge->mFurthestPointDistanceSq = best_dist_sq;
+			best_edge->mConflictList.push_back(inPositionIdx);
+		}
+		else
+		{
+			// Not the furthest point, add it as the before last point
+			best_edge->mConflictList.insert(best_edge->mConflictList.begin() + best_edge->mConflictList.size() - 1, inPositionIdx);
+		}
+	}
+}
+
+ConvexHullBuilder2D::EResult ConvexHullBuilder2D::Initialize(int inIdx1, int inIdx2, int inIdx3, int inMaxVertices, float inTolerance, Edges &outEdges)
+{
+	// Clear any leftovers
+	FreeEdges();
+	outEdges.clear();
+
+	// Reset flag
+	EResult result = EResult::Success;
+
+	// Determine a suitable tolerance for detecting that points are colinear
+	// Formula as per: Implementing Quickhull - Dirk Gregorius.
+	Vec3 vmax = Vec3::sZero();
+	for (Vec3 v : mPositions)
+		vmax = Vec3::sMax(vmax, v.Abs());
+	float colinear_tolerance_sq = Square(2.0f * FLT_EPSILON * (vmax.GetX() + vmax.GetY()));
+
+	// Increase desired tolerance if accuracy doesn't allow it
+	float tolerance_sq = max(colinear_tolerance_sq, Square(inTolerance));
+
+	// Start with the initial indices in counter clockwise order
+	float z = (mPositions[inIdx2] - mPositions[inIdx1]).Cross(mPositions[inIdx3] - mPositions[inIdx1]).GetZ();
+	if (z < 0.0f)
+		std::swap(inIdx1, inIdx2);
+
+	// Create and link edges
+	Edge *e1 = new Edge(inIdx1);
+	Edge *e2 = new Edge(inIdx2);
+	Edge *e3 = new Edge(inIdx3);
+	e1->mNextEdge = e2;
+	e1->mPrevEdge = e3;
+	e2->mNextEdge = e3;
+	e2->mPrevEdge = e1;
+	e3->mNextEdge = e1;
+	e3->mPrevEdge = e2;
+	mFirstEdge = e1;
+	mNumEdges = 3;
+
+	// Build the initial conflict lists
+	Array<Edge *> edges { e1, e2, e3 };
+	for (Edge *edge : edges)
+		edge->CalculateNormalAndCenter(mPositions.data());
+	for (int idx = 0; idx < (int)mPositions.size(); ++idx)
+		if (idx != inIdx1 && idx != inIdx2 && idx != inIdx3)
+			AssignPointToEdge(idx, edges);
+
+	JPH_IF_ENABLE_ASSERTS(ValidateEdges();)
+#ifdef JPH_CONVEX_BUILDER_2D_DEBUG
+	DrawState();
+#endif
+
+	// Add the remaining points to the hull
+	for (;;)
+	{
+		// Check if we've reached the max amount of vertices that are allowed
+		if (mNumEdges >= inMaxVertices)
+		{
+			result = EResult::MaxVerticesReached;
+			break;
+		}
+
+		// Find the edge with the furthest point on it
+		Edge *edge_with_furthest_point = nullptr;
+		float furthest_dist_sq = 0.0f;
+		Edge *edge = mFirstEdge;
+		do
+		{
+			if (edge->mFurthestPointDistanceSq > furthest_dist_sq)
+			{
+				furthest_dist_sq = edge->mFurthestPointDistanceSq;
+				edge_with_furthest_point = edge;
+			}
+			edge = edge->mNextEdge;
+		} while (edge != mFirstEdge);
+
+		// If there is none closer than our tolerance value, we're done
+		if (edge_with_furthest_point == nullptr || furthest_dist_sq < tolerance_sq)
+			break;
+
+		// Take the furthest point
+		int furthest_point_idx = edge_with_furthest_point->mConflictList.back();
+		edge_with_furthest_point->mConflictList.pop_back();
+		Vec3 furthest_point = mPositions[furthest_point_idx];
+
+		// Find the horizon of edges that need to be removed
+		Edge *first_edge = edge_with_furthest_point;
+		do
+		{
+			Edge *prev = first_edge->mPrevEdge;
+			if (!prev->IsFacing(furthest_point))
+				break;
+			first_edge = prev;
+		} while (first_edge != edge_with_furthest_point);
+
+		Edge *last_edge = edge_with_furthest_point;
+		do
+		{
+			Edge *next = last_edge->mNextEdge;
+			if (!next->IsFacing(furthest_point))
+				break;
+			last_edge = next;
+		} while (last_edge != edge_with_furthest_point);
+
+		// Create new edges
+		e1 = new Edge(first_edge->mStartIdx);
+		e2 = new Edge(furthest_point_idx);
+		e1->mNextEdge = e2;
+		e1->mPrevEdge = first_edge->mPrevEdge;
+		e2->mPrevEdge = e1;
+		e2->mNextEdge = last_edge->mNextEdge;
+		e1->mPrevEdge->mNextEdge = e1;
+		e2->mNextEdge->mPrevEdge = e2;
+		mFirstEdge = e1; // We could delete mFirstEdge so just update it to the newly created edge
+		mNumEdges += 2;
+
+		// Calculate normals
+		Array<Edge *> new_edges { e1, e2 };
+		for (Edge *new_edge : new_edges)
+			new_edge->CalculateNormalAndCenter(mPositions.data());
+
+		// Delete the old edges
+		for (;;)
+		{
+			Edge *next = first_edge->mNextEdge;
+
+			// Redistribute points in conflict list
+			for (int idx : first_edge->mConflictList)
+				AssignPointToEdge(idx, new_edges);
+
+			// Delete the old edge
+			delete first_edge;
+			--mNumEdges;
+
+			if (first_edge == last_edge)
+				break;
+			first_edge = next;
+		}
+
+		JPH_IF_ENABLE_ASSERTS(ValidateEdges();)
+	#ifdef JPH_CONVEX_BUILDER_2D_DEBUG
+		DrawState();
+	#endif
+	}
+
+	// Convert the edge list to a list of indices
+	outEdges.reserve(mNumEdges);
+	Edge *edge = mFirstEdge;
+	do
+	{
+		outEdges.push_back(edge->mStartIdx);
+		edge = edge->mNextEdge;
+	} while (edge != mFirstEdge);
+
+	return result;
+}
+
+#ifdef JPH_CONVEX_BUILDER_2D_DEBUG
+
+void ConvexHullBuilder2D::DrawState()
+{
+	int color_idx = 0;
+
+	const Edge *edge = mFirstEdge;
+	do
+	{
+		const Edge *next = edge->mNextEdge;
+
+		// Get unique color per edge
+		Color color = Color::sGetDistinctColor(color_idx++);
+
+		// Draw edge and normal
+		DebugRenderer::sInstance->DrawArrow(cDrawScale * (mOffset + mPositions[edge->mStartIdx]), cDrawScale * (mOffset + mPositions[next->mStartIdx]), color, 0.1f);
+		DebugRenderer::sInstance->DrawArrow(cDrawScale * (mOffset + edge->mCenter), cDrawScale * (mOffset + edge->mCenter) + edge->mNormal.NormalizedOr(Vec3::sZero()), Color::sGreen, 0.1f);
+
+		// Draw points that belong to this edge in the same color
+		for (int idx : edge->mConflictList)
+			DebugRenderer::sInstance->DrawMarker(cDrawScale * (mOffset + mPositions[idx]), color, 0.05f);
+
+		edge = next;
+	} while (edge != mFirstEdge);
+
+	mOffset += mDelta;
+}
+
+#endif
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder2D.h b/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder2D.h
new file mode 100644
index 0000000000..ff06a3403e
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Geometry/ConvexHullBuilder2D.h
@@ -0,0 +1,105 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/NonCopyable.h>
+
+//#define JPH_CONVEX_BUILDER_2D_DEBUG
+
+JPH_NAMESPACE_BEGIN
+
+/// A convex hull builder that tries to create 2D hulls as accurately as possible. Used for offline processing.
+class JPH_EXPORT ConvexHullBuilder2D : public NonCopyable
+{
+public:
+	using Positions = Array<Vec3>;
+	using Edges = Array<int>;
+
+	/// Constructor
+	/// @param inPositions Positions used to make the hull. Uses X and Y component of Vec3 only!
+	explicit			ConvexHullBuilder2D(const Positions &inPositions);
+
+	/// Destructor
+						~ConvexHullBuilder2D();
+
+	/// Result enum that indicates how the hull got created
+	enum class EResult
+	{
+		Success,													///< Hull building finished successfully
+		MaxVerticesReached,											///< Hull building finished successfully, but the desired accuracy was not reached because the max vertices limit was reached
+	};
+
+	/// Takes all positions as provided by the constructor and use them to build a hull
+	/// Any points that are closer to the hull than inTolerance will be discarded
+	/// @param inIdx1 , inIdx2 , inIdx3 The indices to use as initial hull (in any order)
+	/// @param inMaxVertices Max vertices to allow in the hull. Specify INT_MAX if there is no limit.
+	/// @param inTolerance Max distance that a point is allowed to be outside of the hull
+	/// @param outEdges On success this will contain the list of indices that form the hull (counter clockwise)
+	/// @return Status code that reports if the hull was created or not
+	EResult				Initialize(int inIdx1, int inIdx2, int inIdx3, int inMaxVertices, float inTolerance, Edges &outEdges);
+
+private:
+#ifdef JPH_CONVEX_BUILDER_2D_DEBUG
+	/// Factor to scale convex hull when debug drawing the construction process
+	static constexpr Real cDrawScale = 10;
+#endif
+
+	class Edge;
+
+	/// Frees all edges
+	void				FreeEdges();
+
+	/// Assigns a position to one of the supplied edges based on which edge is closest.
+	/// @param inPositionIdx Index of the position to add
+	/// @param inEdges List of edges to consider
+	void				AssignPointToEdge(int inPositionIdx, const Array<Edge *> &inEdges) const;
+
+#ifdef JPH_CONVEX_BUILDER_2D_DEBUG
+	/// Draw state of algorithm
+	void				DrawState();
+#endif
+
+#ifdef JPH_ENABLE_ASSERTS
+	/// Validate that the edge structure is intact
+	void				ValidateEdges() const;
+#endif
+
+	using ConflictList = Array<int>;
+
+	/// Linked list of edges
+	class Edge
+	{
+	public:
+		JPH_OVERRIDE_NEW_DELETE
+
+		/// Constructor
+		explicit		Edge(int inStartIdx)						: mStartIdx(inStartIdx) { }
+
+		/// Calculate the center of the edge and the edge normal
+		void			CalculateNormalAndCenter(const Vec3 *inPositions);
+
+		/// Check if this edge is facing inPosition
+		inline bool		IsFacing(Vec3Arg inPosition) const			{ return mNormal.Dot(inPosition - mCenter) > 0.0f; }
+
+		Vec3			mNormal;									///< Normal of the edge (not normalized)
+		Vec3			mCenter;									///< Center of the edge
+		ConflictList	mConflictList;								///< Positions associated with this edge (that are closest to this edge). Last entry is the one furthest away from the edge, remainder is unsorted.
+		Edge *			mPrevEdge = nullptr;						///< Previous edge in circular list
+		Edge *			mNextEdge = nullptr;						///< Next edge in circular list
+		int				mStartIdx;									///< Position index of start of this edge
+		float			mFurthestPointDistanceSq = 0.0f;			///< Squared distance of furthest point from the conflict list to the edge
+	};
+
+	const Positions &	mPositions;									///< List of positions (some of them are part of the hull)
+	Edge *				mFirstEdge = nullptr;						///< First edge of the hull
+	int					mNumEdges = 0;								///< Number of edges in hull
+
+#ifdef JPH_CONVEX_BUILDER_2D_DEBUG
+	RVec3				mOffset;									///< Offset to use for state drawing
+	Vec3				mDelta;										///< Delta offset between next states
+#endif
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Geometry/ConvexSupport.h b/thirdparty/jolt_physics/Jolt/Geometry/ConvexSupport.h
new file mode 100644
index 0000000000..3ba2c935db
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Geometry/ConvexSupport.h
@@ -0,0 +1,188 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Math/Mat44.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Helper functions to get the support point for a convex object
+/// Structure that transforms a convex object (supports only uniform scaling)
+template <typename ConvexObject>
+struct TransformedConvexObject
+{
+	/// Create transformed convex object.
+	TransformedConvexObject(Mat44Arg inTransform, const ConvexObject &inObject) :
+		mTransform(inTransform),
+		mObject(inObject)
+	{
+	}
+
+	/// Calculate the support vector for this convex shape.
+	Vec3					GetSupport(Vec3Arg inDirection) const
+	{
+		return mTransform * mObject.GetSupport(mTransform.Multiply3x3Transposed(inDirection));
+	}
+
+	/// Get the vertices of the face that faces inDirection the most
+	template <class VERTEX_ARRAY>
+	void					GetSupportingFace(Vec3Arg inDirection, VERTEX_ARRAY &outVertices) const
+	{
+		mObject.GetSupportingFace(mTransform.Multiply3x3Transposed(inDirection), outVertices);
+
+		for (Vec3 &v : outVertices)
+			v = mTransform * v;
+	}
+
+	Mat44					mTransform;
+	const ConvexObject &	mObject;
+};
+
+/// Structure that adds a convex radius
+template <typename ConvexObject>
+struct AddConvexRadius
+{
+	AddConvexRadius(const ConvexObject &inObject, float inRadius) :
+		mObject(inObject),
+		mRadius(inRadius)
+	{
+	}
+
+	/// Calculate the support vector for this convex shape.
+	Vec3					GetSupport(Vec3Arg inDirection) const
+	{
+		float length = inDirection.Length();
+		return length > 0.0f ? mObject.GetSupport(inDirection) + (mRadius / length) * inDirection : mObject.GetSupport(inDirection);
+	}
+
+	const ConvexObject &	mObject;
+	float					mRadius;
+};
+
+/// Structure that performs a Minkowski difference A - B
+template <typename ConvexObjectA, typename ConvexObjectB>
+struct MinkowskiDifference
+{
+	MinkowskiDifference(const ConvexObjectA &inObjectA, const ConvexObjectB &inObjectB) :
+		mObjectA(inObjectA),
+		mObjectB(inObjectB)
+	{
+	}
+
+	/// Calculate the support vector for this convex shape.
+	Vec3					GetSupport(Vec3Arg inDirection) const
+	{
+		return mObjectA.GetSupport(inDirection) - mObjectB.GetSupport(-inDirection);
+	}
+
+	const ConvexObjectA &	mObjectA;
+	const ConvexObjectB &	mObjectB;
+};
+
+/// Class that wraps a point so that it can be used with convex collision detection
+struct PointConvexSupport
+{
+	/// Calculate the support vector for this convex shape.
+	Vec3					GetSupport([[maybe_unused]] Vec3Arg inDirection) const
+	{
+		return mPoint;
+	}
+
+	Vec3					mPoint;
+};
+
+/// Class that wraps a triangle so that it can used with convex collision detection
+struct TriangleConvexSupport
+{
+	/// Constructor
+							TriangleConvexSupport(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3) :
+		mV1(inV1),
+		mV2(inV2),
+		mV3(inV3)
+	{
+	}
+
+	/// Calculate the support vector for this convex shape.
+	Vec3					GetSupport(Vec3Arg inDirection) const
+	{
+		// Project vertices on inDirection
+		float d1 = mV1.Dot(inDirection);
+		float d2 = mV2.Dot(inDirection);
+		float d3 = mV3.Dot(inDirection);
+
+		// Return vertex with biggest projection
+		if (d1 > d2)
+		{
+			if (d1 > d3)
+				return mV1;
+			else
+				return mV3;
+		}
+		else
+		{
+			if (d2 > d3)
+				return mV2;
+			else
+				return mV3;
+		}
+	}
+
+	/// Get the vertices of the face that faces inDirection the most
+	template <class VERTEX_ARRAY>
+	void					GetSupportingFace([[maybe_unused]] Vec3Arg inDirection, VERTEX_ARRAY &outVertices) const
+	{
+		outVertices.push_back(mV1);
+		outVertices.push_back(mV2);
+		outVertices.push_back(mV3);
+	}
+
+	/// The three vertices of the triangle
+	Vec3					mV1;
+	Vec3					mV2;
+	Vec3					mV3;
+};
+
+/// Class that wraps a polygon so that it can used with convex collision detection
+template <class VERTEX_ARRAY>
+struct PolygonConvexSupport
+{
+	/// Constructor
+	explicit				PolygonConvexSupport(const VERTEX_ARRAY &inVertices) :
+		mVertices(inVertices)
+	{
+	}
+
+	/// Calculate the support vector for this convex shape.
+	Vec3					GetSupport(Vec3Arg inDirection) const
+	{
+		Vec3 support_point = mVertices[0];
+		float best_dot = mVertices[0].Dot(inDirection);
+
+		for (typename VERTEX_ARRAY::const_iterator v = mVertices.begin() + 1; v < mVertices.end(); ++v)
+		{
+			float dot = v->Dot(inDirection);
+			if (dot > best_dot)
+			{
+				best_dot = dot;
+				support_point = *v;
+			}
+		}
+
+		return support_point;
+	}
+
+	/// Get the vertices of the face that faces inDirection the most
+	template <class VERTEX_ARRAY_ARG>
+	void					GetSupportingFace([[maybe_unused]] Vec3Arg inDirection, VERTEX_ARRAY_ARG &outVertices) const
+	{
+		for (Vec3 v : mVertices)
+			outVertices.push_back(v);
+	}
+
+	/// The vertices of the polygon
+	const VERTEX_ARRAY &	mVertices;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Geometry/EPAConvexHullBuilder.h b/thirdparty/jolt_physics/Jolt/Geometry/EPAConvexHullBuilder.h
new file mode 100644
index 0000000000..15d61b068b
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Geometry/EPAConvexHullBuilder.h
@@ -0,0 +1,845 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+// Define to validate the integrity of the hull structure
+//#define JPH_EPA_CONVEX_BUILDER_VALIDATE
+
+// Define to draw the building of the hull for debugging purposes
+//#define JPH_EPA_CONVEX_BUILDER_DRAW
+
+#include <Jolt/Core/NonCopyable.h>
+#include <Jolt/Core/BinaryHeap.h>
+
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+	#include <Jolt/Renderer/DebugRenderer.h>
+	#include <Jolt/Core/StringTools.h>
+#endif
+
+JPH_NAMESPACE_BEGIN
+
+/// A convex hull builder specifically made for the EPA penetration depth calculation. It trades accuracy for speed and will simply abort of the hull forms defects due to numerical precision problems.
+class EPAConvexHullBuilder : public NonCopyable
+{
+private:
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+	/// Factor to scale convex hull when debug drawing the construction process
+	static constexpr Real cDrawScale = 10;
+#endif
+
+public:
+	// Due to the Euler characteristic (https://en.wikipedia.org/wiki/Euler_characteristic) we know that Vertices - Edges + Faces = 2
+	// In our case we only have triangles and they are always fully connected, so each edge is shared exactly between 2 faces: Edges = Faces * 3 / 2
+	// Substituting: Vertices = Faces / 2 + 2 which is approximately Faces / 2.
+	static constexpr int cMaxTriangles = 256;				///< Max triangles in hull
+	static constexpr int cMaxPoints = cMaxTriangles / 2;	///< Max number of points in hull
+
+	// Constants
+	static constexpr int cMaxEdgeLength = 128;				///< Max number of edges in FindEdge
+	static constexpr float cMinTriangleArea = 1.0e-10f;		///< Minimum area of a triangle before, if smaller than this it will not be added to the priority queue
+	static constexpr float cBarycentricEpsilon = 1.0e-3f;	///< Epsilon value used to determine if a point is in the interior of a triangle
+
+	// Forward declare
+	class Triangle;
+
+	/// Class that holds the information of an edge
+	class Edge
+	{
+	public:
+		/// Information about neighbouring triangle
+		Triangle *		mNeighbourTriangle;					///< Triangle that neighbours this triangle
+		int				mNeighbourEdge;						///< Index in mEdge that specifies edge that this Edge is connected to
+
+		int				mStartIdx;							///< Vertex index in mPositions that indicates the start vertex of this edge
+	};
+
+	using Edges = StaticArray<Edge, cMaxEdgeLength>;
+	using NewTriangles = StaticArray<Triangle *, cMaxEdgeLength>;
+
+	/// Class that holds the information of one triangle
+	class Triangle : public NonCopyable
+	{
+	public:
+		/// Constructor
+		inline			Triangle(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions);
+
+		/// Check if triangle is facing inPosition
+		inline bool		IsFacing(Vec3Arg inPosition) const
+		{
+			JPH_ASSERT(!mRemoved);
+			return mNormal.Dot(inPosition - mCentroid) > 0.0f;
+		}
+
+		/// Check if triangle is facing the origin
+		inline bool		IsFacingOrigin() const
+		{
+			JPH_ASSERT(!mRemoved);
+			return mNormal.Dot(mCentroid) < 0.0f;
+		}
+
+		/// Get the next edge of edge inIndex
+		inline const Edge & GetNextEdge(int inIndex) const
+		{
+			return mEdge[(inIndex + 1) % 3];
+		}
+
+		Edge			mEdge[3];							///< 3 edges of this triangle
+		Vec3			mNormal;							///< Normal of this triangle, length is 2 times area of triangle
+		Vec3			mCentroid;							///< Center of the triangle
+		float			mClosestLenSq = FLT_MAX;			///< Closest distance^2 from origin to triangle
+		float			mLambda[2];							///< Barycentric coordinates of closest point to origin on triangle
+		bool			mLambdaRelativeTo0;					///< How to calculate the closest point, true: y0 + l0 * (y1 - y0) + l1 * (y2 - y0), false: y1 + l0 * (y0 - y1) + l1 * (y2 - y1)
+		bool			mClosestPointInterior = false;		///< Flag that indicates that the closest point from this triangle to the origin is an interior point
+		bool			mRemoved = false;					///< Flag that indicates that triangle has been removed
+		bool			mInQueue = false;					///< Flag that indicates that this triangle was placed in the sorted heap (stays true after it is popped because the triangle is freed by the main EPA algorithm loop)
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+		int				mIteration;							///< Iteration that this triangle was created
+#endif
+	};
+
+	/// Factory that creates triangles in a fixed size buffer
+	class TriangleFactory : public NonCopyable
+	{
+	private:
+		/// Struct that stores both a triangle or a next pointer in case the triangle is unused
+		union alignas(Triangle) Block
+		{
+			uint8		mTriangle[sizeof(Triangle)];
+			Block *		mNextFree;
+		};
+
+		/// Storage for triangle data
+		Block			mTriangles[cMaxTriangles];			///< Storage for triangles
+		Block *			mNextFree = nullptr;				///< List of free triangles
+		int				mHighWatermark = 0;					///< High water mark for used triangles (if mNextFree == nullptr we can take one from here)
+
+	public:
+		/// Return all triangles to the free pool
+		void			Clear()
+		{
+			mNextFree = nullptr;
+			mHighWatermark = 0;
+		}
+
+		/// Allocate a new triangle with 3 indexes
+		Triangle *		CreateTriangle(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions)
+		{
+			Triangle *t;
+			if (mNextFree != nullptr)
+			{
+				// Entry available from the free list
+				t = reinterpret_cast<Triangle *>(&mNextFree->mTriangle);
+				mNextFree = mNextFree->mNextFree;
+			}
+			else
+			{
+				// Allocate from never used before triangle store
+				if (mHighWatermark >= cMaxTriangles)
+					return nullptr; // Buffer full
+				t = reinterpret_cast<Triangle *>(&mTriangles[mHighWatermark].mTriangle);
+				++mHighWatermark;
+			}
+
+			// Call constructor
+			new (t) Triangle(inIdx0, inIdx1, inIdx2, inPositions);
+
+			return t;
+		}
+
+		/// Free a triangle
+		void			FreeTriangle(Triangle *inT)
+		{
+			// Destruct triangle
+			inT->~Triangle();
+#ifdef JPH_DEBUG
+			memset(inT, 0xcd, sizeof(Triangle));
+#endif
+
+			// Add triangle to the free list
+			Block *tu = reinterpret_cast<Block *>(inT);
+			tu->mNextFree = mNextFree;
+			mNextFree = tu;
+		}
+	};
+
+	// Typedefs
+	using PointsBase = StaticArray<Vec3, cMaxPoints>;
+	using Triangles = StaticArray<Triangle *, cMaxTriangles>;
+
+	/// Specialized points list that allows direct access to the size
+	class Points : public PointsBase
+	{
+	public:
+		size_type &		GetSizeRef()
+		{
+			return mSize;
+		}
+	};
+
+	/// Specialized triangles list that keeps them sorted on closest distance to origin
+	class TriangleQueue : public Triangles
+	{
+	public:
+		/// Function to sort triangles on closest distance to origin
+		static bool		sTriangleSorter(const Triangle *inT1, const Triangle *inT2)
+		{
+			return inT1->mClosestLenSq > inT2->mClosestLenSq;
+		}
+
+		/// Add triangle to the list
+		void			push_back(Triangle *inT)
+		{
+			// Add to base
+			Triangles::push_back(inT);
+
+			// Mark in queue
+			inT->mInQueue = true;
+
+			// Resort heap
+			BinaryHeapPush(begin(), end(), sTriangleSorter);
+		}
+
+		/// Peek the next closest triangle without removing it
+		Triangle *		PeekClosest()
+		{
+			return front();
+		}
+
+		/// Get next closest triangle
+		Triangle *		PopClosest()
+		{
+			// Move closest to end
+			BinaryHeapPop(begin(), end(), sTriangleSorter);
+
+			// Remove last triangle
+			Triangle *t = back();
+			pop_back();
+			return t;
+		}
+	};
+
+	/// Constructor
+	explicit			EPAConvexHullBuilder(const Points &inPositions) :
+		mPositions(inPositions)
+	{
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+		mIteration = 0;
+		mOffset = RVec3::sZero();
+#endif
+	}
+
+	/// Initialize the hull with 3 points
+	void				Initialize(int inIdx1, int inIdx2, int inIdx3)
+	{
+		// Release triangles
+		mFactory.Clear();
+
+		// Create triangles (back to back)
+		Triangle *t1 = CreateTriangle(inIdx1, inIdx2, inIdx3);
+		Triangle *t2 = CreateTriangle(inIdx1, inIdx3, inIdx2);
+
+		// Link triangles edges
+		sLinkTriangle(t1, 0, t2, 2);
+		sLinkTriangle(t1, 1, t2, 1);
+		sLinkTriangle(t1, 2, t2, 0);
+
+		// Always add both triangles to the priority queue
+		mTriangleQueue.push_back(t1);
+		mTriangleQueue.push_back(t2);
+
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+		// Draw current state
+		DrawState();
+
+		// Increment iteration counter
+		++mIteration;
+#endif
+	}
+
+	/// Check if there's another triangle to process from the queue
+	bool				HasNextTriangle() const
+	{
+		return !mTriangleQueue.empty();
+	}
+
+	/// Access to the next closest triangle to the origin (won't remove it from the queue).
+	Triangle *			PeekClosestTriangleInQueue()
+	{
+		return mTriangleQueue.PeekClosest();
+	}
+
+	/// Access to the next closest triangle to the origin and remove it from the queue.
+	Triangle *			PopClosestTriangleFromQueue()
+	{
+		return mTriangleQueue.PopClosest();
+	}
+
+	/// Find the triangle on which inPosition is the furthest to the front
+	/// Note this function works as long as all points added have been added with AddPoint(..., FLT_MAX).
+	Triangle *			FindFacingTriangle(Vec3Arg inPosition, float &outBestDistSq)
+	{
+		Triangle *best = nullptr;
+		float best_dist_sq = 0.0f;
+
+		for (Triangle *t : mTriangleQueue)
+			if (!t->mRemoved)
+			{
+				float dot = t->mNormal.Dot(inPosition - t->mCentroid);
+				if (dot > 0.0f)
+				{
+					float dist_sq = dot * dot / t->mNormal.LengthSq();
+					if (dist_sq > best_dist_sq)
+					{
+						best = t;
+						best_dist_sq = dist_sq;
+					}
+				}
+			}
+
+		outBestDistSq = best_dist_sq;
+		return best;
+	}
+
+	/// Add a new point to the convex hull
+	bool				AddPoint(Triangle *inFacingTriangle, int inIdx, float inClosestDistSq, NewTriangles &outTriangles)
+	{
+		// Get position
+		Vec3 pos = mPositions[inIdx];
+
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+		// Draw new support point
+		DrawMarker(pos, Color::sYellow, 1.0f);
+#endif
+
+#ifdef JPH_EPA_CONVEX_BUILDER_VALIDATE
+		// Check if structure is intact
+		ValidateTriangles();
+#endif
+
+		// Find edge of convex hull of triangles that are not facing the new vertex w
+		Edges edges;
+		if (!FindEdge(inFacingTriangle, pos, edges))
+			return false;
+
+		// Create new triangles
+		int num_edges = edges.size();
+		for (int i = 0; i < num_edges; ++i)
+		{
+			// Create new triangle
+			Triangle *nt = CreateTriangle(edges[i].mStartIdx, edges[(i + 1) % num_edges].mStartIdx, inIdx);
+			if (nt == nullptr)
+				return false;
+			outTriangles.push_back(nt);
+
+			// Check if we need to put this triangle in the priority queue
+			if ((nt->mClosestPointInterior && nt->mClosestLenSq < inClosestDistSq)	// For the main algorithm
+				|| nt->mClosestLenSq < 0.0f)										// For when the origin is not inside the hull yet
+				mTriangleQueue.push_back(nt);
+		}
+
+		// Link edges
+		for (int i = 0; i < num_edges; ++i)
+		{
+			sLinkTriangle(outTriangles[i], 0, edges[i].mNeighbourTriangle, edges[i].mNeighbourEdge);
+			sLinkTriangle(outTriangles[i], 1, outTriangles[(i + 1) % num_edges], 2);
+		}
+
+#ifdef JPH_EPA_CONVEX_BUILDER_VALIDATE
+		// Check if structure is intact
+		ValidateTriangles();
+#endif
+
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+		// Draw state of the hull
+		DrawState();
+
+		// Increment iteration counter
+		++mIteration;
+#endif
+
+		return true;
+	}
+
+	/// Free a triangle
+	void				FreeTriangle(Triangle *inT)
+	{
+#ifdef JPH_ENABLE_ASSERTS
+		// Make sure that this triangle is not connected
+		JPH_ASSERT(inT->mRemoved);
+		for (const Edge &e : inT->mEdge)
+			JPH_ASSERT(e.mNeighbourTriangle == nullptr);
+#endif
+
+#if defined(JPH_EPA_CONVEX_BUILDER_VALIDATE) || defined(JPH_EPA_CONVEX_BUILDER_DRAW)
+		// Remove from list of all triangles
+		Triangles::iterator i = std::find(mTriangles.begin(), mTriangles.end(), inT);
+		JPH_ASSERT(i != mTriangles.end());
+		mTriangles.erase(i);
+#endif
+
+		mFactory.FreeTriangle(inT);
+	}
+
+private:
+	/// Create a new triangle
+	Triangle *			CreateTriangle(int inIdx1, int inIdx2, int inIdx3)
+	{
+		// Call provider to create triangle
+		Triangle *t = mFactory.CreateTriangle(inIdx1, inIdx2, inIdx3, mPositions.data());
+		if (t == nullptr)
+			return nullptr;
+
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+		// Remember iteration counter
+		t->mIteration = mIteration;
+#endif
+
+#if defined(JPH_EPA_CONVEX_BUILDER_VALIDATE) || defined(JPH_EPA_CONVEX_BUILDER_DRAW)
+		// Add to list of triangles for debugging purposes
+		mTriangles.push_back(t);
+#endif
+
+		return t;
+	}
+
+	/// Link triangle edge to other triangle edge
+	static void			sLinkTriangle(Triangle *inT1, int inEdge1, Triangle *inT2, int inEdge2)
+	{
+		JPH_ASSERT(inEdge1 >= 0 && inEdge1 < 3);
+		JPH_ASSERT(inEdge2 >= 0 && inEdge2 < 3);
+		Edge &e1 = inT1->mEdge[inEdge1];
+		Edge &e2 = inT2->mEdge[inEdge2];
+
+		// Check not connected yet
+		JPH_ASSERT(e1.mNeighbourTriangle == nullptr);
+		JPH_ASSERT(e2.mNeighbourTriangle == nullptr);
+
+		// Check vertices match
+		JPH_ASSERT(e1.mStartIdx == inT2->GetNextEdge(inEdge2).mStartIdx);
+		JPH_ASSERT(e2.mStartIdx == inT1->GetNextEdge(inEdge1).mStartIdx);
+
+		// Link up
+		e1.mNeighbourTriangle = inT2;
+		e1.mNeighbourEdge = inEdge2;
+		e2.mNeighbourTriangle = inT1;
+		e2.mNeighbourEdge = inEdge1;
+	}
+
+	/// Unlink this triangle
+	void				UnlinkTriangle(Triangle *inT)
+	{
+		// Unlink from neighbours
+		for (int i = 0; i < 3; ++i)
+		{
+			Edge &edge = inT->mEdge[i];
+			if (edge.mNeighbourTriangle != nullptr)
+			{
+				Edge &neighbour_edge = edge.mNeighbourTriangle->mEdge[edge.mNeighbourEdge];
+
+				// Validate that neighbour points to us
+				JPH_ASSERT(neighbour_edge.mNeighbourTriangle == inT);
+				JPH_ASSERT(neighbour_edge.mNeighbourEdge == i);
+
+				// Unlink
+				neighbour_edge.mNeighbourTriangle = nullptr;
+				edge.mNeighbourTriangle = nullptr;
+			}
+		}
+
+		// If this triangle is not in the priority queue, we can delete it now
+		if (!inT->mInQueue)
+			FreeTriangle(inT);
+	}
+
+	/// Given one triangle that faces inVertex, find the edges of the triangles that are not facing inVertex.
+	/// Will flag all those triangles for removal.
+	bool				FindEdge(Triangle *inFacingTriangle, Vec3Arg inVertex, Edges &outEdges)
+	{
+		// Assert that we were given an empty array
+		JPH_ASSERT(outEdges.empty());
+
+		// Should start with a facing triangle
+		JPH_ASSERT(inFacingTriangle->IsFacing(inVertex));
+
+		// Flag as removed
+		inFacingTriangle->mRemoved = true;
+
+		// Instead of recursing, we build our own stack with the information we need
+		struct StackEntry
+		{
+			Triangle *	mTriangle;
+			int			mEdge;
+			int			mIter;
+		};
+		StackEntry stack[cMaxEdgeLength];
+		int cur_stack_pos = 0;
+
+		// Start with the triangle / edge provided
+		stack[0].mTriangle = inFacingTriangle;
+		stack[0].mEdge = 0;
+		stack[0].mIter = -1; // Start with edge 0 (is incremented below before use)
+
+		// Next index that we expect to find, if we don't then there are 'islands'
+		int next_expected_start_idx = -1;
+
+		for (;;)
+		{
+			StackEntry &cur_entry = stack[cur_stack_pos];
+
+			// Next iteration
+			if (++cur_entry.mIter >= 3)
+			{
+				// This triangle needs to be removed, unlink it now
+				UnlinkTriangle(cur_entry.mTriangle);
+
+				// Pop from stack
+				if (--cur_stack_pos < 0)
+					break;
+			}
+			else
+			{
+				// Visit neighbour
+				Edge &e = cur_entry.mTriangle->mEdge[(cur_entry.mEdge + cur_entry.mIter) % 3];
+				Triangle *n = e.mNeighbourTriangle;
+				if (n != nullptr && !n->mRemoved)
+				{
+					// Check if vertex is on the front side of this triangle
+					if (n->IsFacing(inVertex))
+					{
+						// Vertex on front, this triangle needs to be removed
+						n->mRemoved = true;
+
+						// Add element to the stack of elements to visit
+						cur_stack_pos++;
+						JPH_ASSERT(cur_stack_pos < cMaxEdgeLength);
+						StackEntry &new_entry = stack[cur_stack_pos];
+						new_entry.mTriangle = n;
+						new_entry.mEdge = e.mNeighbourEdge;
+						new_entry.mIter = 0; // Is incremented before use, we don't need to test this edge again since we came from it
+					}
+					else
+					{
+						// Detect if edge doesn't connect to previous edge, if this happens we have found and 'island' which means
+						// the newly added point is so close to the triangles of the hull that we classified some (nearly) coplanar
+						// triangles as before and some behind the point. At this point we just abort adding the point because
+						// we've reached numerical precision.
+						// Note that we do not need to test if the first and last edge connect, since when there are islands
+						// there should be at least 2 disconnects.
+						if (e.mStartIdx != next_expected_start_idx && next_expected_start_idx != -1)
+							return false;
+
+						// Next expected index is the start index of our neighbour's edge
+						next_expected_start_idx = n->mEdge[e.mNeighbourEdge].mStartIdx;
+
+						// Vertex behind, keep edge
+						outEdges.push_back(e);
+					}
+				}
+			}
+		}
+
+		// Assert that we have a fully connected loop
+		JPH_ASSERT(outEdges.empty() || outEdges[0].mStartIdx == next_expected_start_idx);
+
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+		// Draw edge of facing triangles
+		for (int i = 0; i < (int)outEdges.size(); ++i)
+		{
+			RVec3 edge_start = cDrawScale * (mOffset + mPositions[outEdges[i].mStartIdx]);
+			DebugRenderer::sInstance->DrawArrow(edge_start, cDrawScale * (mOffset + mPositions[outEdges[(i + 1) % outEdges.size()].mStartIdx]), Color::sYellow, 0.01f);
+			DebugRenderer::sInstance->DrawText3D(edge_start, ConvertToString(outEdges[i].mStartIdx), Color::sWhite);
+		}
+
+		// Draw the state with the facing triangles removed
+		DrawState();
+#endif
+
+		// When we start with two triangles facing away from each other and adding a point that is on the plane,
+		// sometimes we consider the point in front of both causing both triangles to be removed resulting in an empty edge list.
+		// In this case we fail to add the point which will result in no collision reported (the shapes are contacting in 1 point so there's 0 penetration)
+		return outEdges.size() >= 3;
+	}
+
+#ifdef JPH_EPA_CONVEX_BUILDER_VALIDATE
+	/// Check consistency of 1 triangle
+	void				ValidateTriangle(const Triangle *inT) const
+	{
+		if (inT->mRemoved)
+		{
+			// Validate that removed triangles are not connected to anything
+			for (const Edge &my_edge : inT->mEdge)
+				JPH_ASSERT(my_edge.mNeighbourTriangle == nullptr);
+		}
+		else
+		{
+			for (int i = 0; i < 3; ++i)
+			{
+				const Edge &my_edge = inT->mEdge[i];
+
+				// Assert that we have a neighbour
+				const Triangle *nb = my_edge.mNeighbourTriangle;
+				JPH_ASSERT(nb != nullptr);
+
+				if (nb != nullptr)
+				{
+					// Assert that our neighbours edge points to us
+					const Edge &nb_edge = nb->mEdge[my_edge.mNeighbourEdge];
+					JPH_ASSERT(nb_edge.mNeighbourTriangle == inT);
+					JPH_ASSERT(nb_edge.mNeighbourEdge == i);
+
+					// Assert that the next edge of the neighbour points to the same vertex as this edge's vertex
+					const Edge &nb_next_edge = nb->GetNextEdge(my_edge.mNeighbourEdge);
+					JPH_ASSERT(nb_next_edge.mStartIdx == my_edge.mStartIdx);
+
+					// Assert that my next edge points to the same vertex as my neighbours vertex
+					const Edge &my_next_edge = inT->GetNextEdge(i);
+					JPH_ASSERT(my_next_edge.mStartIdx == nb_edge.mStartIdx);
+				}
+			}
+		}
+	}
+
+	/// Check consistency of all triangles
+	void				ValidateTriangles() const
+	{
+		for (const Triangle *t : mTriangles)
+			ValidateTriangle(t);
+	}
+#endif
+
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+public:
+	/// Draw state of algorithm
+	void				DrawState()
+	{
+		// Draw origin
+		DebugRenderer::sInstance->DrawCoordinateSystem(RMat44::sTranslation(cDrawScale * mOffset), 1.0f);
+
+		// Draw triangles
+		for (const Triangle *t : mTriangles)
+			if (!t->mRemoved)
+			{
+				// Calculate the triangle vertices
+				RVec3 p1 = cDrawScale * (mOffset + mPositions[t->mEdge[0].mStartIdx]);
+				RVec3 p2 = cDrawScale * (mOffset + mPositions[t->mEdge[1].mStartIdx]);
+				RVec3 p3 = cDrawScale * (mOffset + mPositions[t->mEdge[2].mStartIdx]);
+
+				// Draw triangle
+				DebugRenderer::sInstance->DrawTriangle(p1, p2, p3, Color::sGetDistinctColor(t->mIteration));
+				DebugRenderer::sInstance->DrawWireTriangle(p1, p2, p3, Color::sGrey);
+
+				// Draw normal
+				RVec3 centroid = cDrawScale * (mOffset + t->mCentroid);
+				float len = t->mNormal.Length();
+				if (len > 0.0f)
+					DebugRenderer::sInstance->DrawArrow(centroid, centroid + t->mNormal / len, Color::sDarkGreen, 0.01f);
+			}
+
+		// Determine max position
+		float min_x = FLT_MAX;
+		float max_x = -FLT_MAX;
+		for (Vec3 p : mPositions)
+		{
+			min_x = min(min_x, p.GetX());
+			max_x = max(max_x, p.GetX());
+		}
+
+		// Offset to the right
+		mOffset += Vec3(max_x - min_x + 0.5f, 0.0f, 0.0f);
+	}
+
+	/// Draw a label to indicate the next stage in the algorithm
+	void				DrawLabel(const string_view &inText)
+	{
+		DebugRenderer::sInstance->DrawText3D(cDrawScale * mOffset, inText, Color::sWhite, 0.1f * cDrawScale);
+
+		mOffset += Vec3(5.0f, 0.0f, 0.0f);
+	}
+
+	/// Draw geometry for debugging purposes
+	void				DrawGeometry(const DebugRenderer::GeometryRef &inGeometry, ColorArg inColor)
+	{
+		RMat44 origin = RMat44::sScale(Vec3::sReplicate(cDrawScale)) * RMat44::sTranslation(mOffset);
+		DebugRenderer::sInstance->DrawGeometry(origin, inGeometry->mBounds.Transformed(origin), inGeometry->mBounds.GetExtent().LengthSq(), inColor, inGeometry);
+
+		mOffset += Vec3(inGeometry->mBounds.GetSize().GetX(), 0, 0);
+	}
+
+	/// Draw a triangle for debugging purposes
+	void				DrawWireTriangle(const Triangle &inTriangle, ColorArg inColor)
+	{
+		RVec3 prev = cDrawScale * (mOffset + mPositions[inTriangle.mEdge[2].mStartIdx]);
+		for (const Edge &edge : inTriangle.mEdge)
+		{
+			RVec3 cur = cDrawScale * (mOffset + mPositions[edge.mStartIdx]);
+			DebugRenderer::sInstance->DrawArrow(prev, cur, inColor, 0.01f);
+			prev = cur;
+		}
+	}
+
+	/// Draw a marker for debugging purposes
+	void				DrawMarker(Vec3Arg inPosition, ColorArg inColor, float inSize)
+	{
+		DebugRenderer::sInstance->DrawMarker(cDrawScale * (mOffset + inPosition), inColor, inSize);
+	}
+
+	/// Draw an arrow for debugging purposes
+	void				DrawArrow(Vec3Arg inFrom, Vec3Arg inTo, ColorArg inColor, float inArrowSize)
+	{
+		DebugRenderer::sInstance->DrawArrow(cDrawScale * (mOffset + inFrom), cDrawScale * (mOffset + inTo), inColor, inArrowSize);
+	}
+#endif
+
+private:
+	TriangleFactory		mFactory;							///< Factory to create new triangles and remove old ones
+	const Points &		mPositions;							///< List of positions (some of them are part of the hull)
+	TriangleQueue		mTriangleQueue;						///< List of triangles that are part of the hull that still need to be checked (if !mRemoved)
+
+#if defined(JPH_EPA_CONVEX_BUILDER_VALIDATE) || defined(JPH_EPA_CONVEX_BUILDER_DRAW)
+	Triangles			mTriangles;							///< The list of all triangles in this hull (for debug purposes)
+#endif
+
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+	int					mIteration;							///< Number of iterations we've had so far (for debug purposes)
+	RVec3				mOffset;							///< Offset to use for state drawing
+#endif
+};
+
+// The determinant that is calculated in the Triangle constructor is really sensitive
+// to numerical round off, disable the fmadd instructions to maintain precision.
+JPH_PRECISE_MATH_ON
+
+EPAConvexHullBuilder::Triangle::Triangle(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions)
+{
+	// Fill in indexes
+	JPH_ASSERT(inIdx0 != inIdx1 && inIdx0 != inIdx2 && inIdx1 != inIdx2);
+	mEdge[0].mStartIdx = inIdx0;
+	mEdge[1].mStartIdx = inIdx1;
+	mEdge[2].mStartIdx = inIdx2;
+
+	// Clear links
+	mEdge[0].mNeighbourTriangle = nullptr;
+	mEdge[1].mNeighbourTriangle = nullptr;
+	mEdge[2].mNeighbourTriangle = nullptr;
+
+	// Get vertex positions
+	Vec3 y0 = inPositions[inIdx0];
+	Vec3 y1 = inPositions[inIdx1];
+	Vec3 y2 = inPositions[inIdx2];
+
+	// Calculate centroid
+	mCentroid = (y0 + y1 + y2) / 3.0f;
+
+	// Calculate edges
+	Vec3 y10 = y1 - y0;
+	Vec3 y20 = y2 - y0;
+	Vec3 y21 = y2 - y1;
+
+	// The most accurate normal is calculated by using the two shortest edges
+	// See: https://box2d.org/posts/2014/01/troublesome-triangle/
+	// The difference in normals is most pronounced when one edge is much smaller than the others (in which case the other 2 must have roughly the same length).
+	// Therefore we can suffice by just picking the shortest from 2 edges and use that with the 3rd edge to calculate the normal.
+	// We first check which of the edges is shorter.
+	float y20_dot_y20 = y20.Dot(y20);
+	float y21_dot_y21 = y21.Dot(y21);
+	if (y20_dot_y20 < y21_dot_y21)
+	{
+		// We select the edges y10 and y20
+		mNormal = y10.Cross(y20);
+
+		// Check if triangle is degenerate
+		float normal_len_sq = mNormal.LengthSq();
+		if (normal_len_sq > cMinTriangleArea)
+		{
+			// Determine distance between triangle and origin: distance = (centroid - origin) . normal / |normal|
+			// Note that this way of calculating the closest point is much more accurate than first calculating barycentric coordinates and then calculating the closest
+			// point based on those coordinates. Note that we preserve the sign of the distance to check on which side the origin is.
+			float c_dot_n = mCentroid.Dot(mNormal);
+			mClosestLenSq = abs(c_dot_n) * c_dot_n / normal_len_sq;
+
+			// Calculate closest point to origin using barycentric coordinates:
+			//
+			// v = y0 + l0 * (y1 - y0) + l1 * (y2 - y0)
+			// v . (y1 - y0) = 0
+			// v . (y2 - y0) = 0
+			//
+			// Written in matrix form:
+			//
+			// | y10.y10  y20.y10 | | l0 | = | -y0.y10 |
+			// | y10.y20  y20.y20 | | l1 |   | -y0.y20 |
+			//
+			// (y10 = y1 - y0 etc.)
+			//
+			// Cramers rule to invert matrix:
+			float y10_dot_y10 = y10.LengthSq();
+			float y10_dot_y20 = y10.Dot(y20);
+			float determinant = y10_dot_y10 * y20_dot_y20 - y10_dot_y20 * y10_dot_y20;
+			if (determinant > 0.0f) // If determinant == 0 then the system is linearly dependent and the triangle is degenerate, since y10.10 * y20.y20 > y10.y20^2 it should also be > 0
+			{
+				float y0_dot_y10 = y0.Dot(y10);
+				float y0_dot_y20 = y0.Dot(y20);
+				float l0 = (y10_dot_y20 * y0_dot_y20 - y20_dot_y20 * y0_dot_y10) / determinant;
+				float l1 = (y10_dot_y20 * y0_dot_y10 - y10_dot_y10 * y0_dot_y20) / determinant;
+				mLambda[0] = l0;
+				mLambda[1] = l1;
+				mLambdaRelativeTo0 = true;
+
+				// Check if closest point is interior to the triangle. For a convex hull which contains the origin each face must contain the origin, but because
+				// our faces are triangles, we can have multiple coplanar triangles and only 1 will have the origin as an interior point. We want to use this triangle
+				// to calculate the contact points because it gives the most accurate results, so we will only add these triangles to the priority queue.
+				if (l0 > -cBarycentricEpsilon && l1 > -cBarycentricEpsilon && l0 + l1 < 1.0f + cBarycentricEpsilon)
+					mClosestPointInterior = true;
+			}
+		}
+	}
+	else
+	{
+		// We select the edges y10 and y21
+		mNormal = y10.Cross(y21);
+
+		// Check if triangle is degenerate
+		float normal_len_sq = mNormal.LengthSq();
+		if (normal_len_sq > cMinTriangleArea)
+		{
+			// Again calculate distance between triangle and origin
+			float c_dot_n = mCentroid.Dot(mNormal);
+			mClosestLenSq = abs(c_dot_n) * c_dot_n / normal_len_sq;
+
+			// Calculate closest point to origin using barycentric coordinates but this time using y1 as the reference vertex
+			//
+			// v = y1 + l0 * (y0 - y1) + l1 * (y2 - y1)
+			// v . (y0 - y1) = 0
+			// v . (y2 - y1) = 0
+			//
+			// Written in matrix form:
+			//
+			// | y10.y10  -y21.y10 | | l0 | = | y1.y10 |
+			// | -y10.y21  y21.y21 | | l1 |   | -y1.y21 |
+			//
+			// Cramers rule to invert matrix:
+			float y10_dot_y10 = y10.LengthSq();
+			float y10_dot_y21 = y10.Dot(y21);
+			float determinant = y10_dot_y10 * y21_dot_y21 - y10_dot_y21 * y10_dot_y21;
+			if (determinant > 0.0f)
+			{
+				float y1_dot_y10 = y1.Dot(y10);
+				float y1_dot_y21 = y1.Dot(y21);
+				float l0 = (y21_dot_y21 * y1_dot_y10 - y10_dot_y21 * y1_dot_y21) / determinant;
+				float l1 = (y10_dot_y21 * y1_dot_y10 - y10_dot_y10 * y1_dot_y21) / determinant;
+				mLambda[0] = l0;
+				mLambda[1] = l1;
+				mLambdaRelativeTo0 = false;
+
+				// Again check if the closest point is inside the triangle
+				if (l0 > -cBarycentricEpsilon && l1 > -cBarycentricEpsilon && l0 + l1 < 1.0f + cBarycentricEpsilon)
+					mClosestPointInterior = true;
+			}
+		}
+	}
+}
+
+JPH_PRECISE_MATH_OFF
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Geometry/EPAPenetrationDepth.h b/thirdparty/jolt_physics/Jolt/Geometry/EPAPenetrationDepth.h
new file mode 100644
index 0000000000..ffa0c39127
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Geometry/EPAPenetrationDepth.h
@@ -0,0 +1,559 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/StaticArray.h>
+#include <Jolt/Core/Profiler.h>
+#include <Jolt/Geometry/GJKClosestPoint.h>
+#include <Jolt/Geometry/EPAConvexHullBuilder.h>
+
+//#define JPH_EPA_PENETRATION_DEPTH_DEBUG
+
+JPH_NAMESPACE_BEGIN
+
+/// Implementation of Expanding Polytope Algorithm as described in:
+///
+/// Proximity Queries and Penetration Depth Computation on 3D Game Objects - Gino van den Bergen
+///
+/// The implementation of this algorithm does not completely follow the article, instead of splitting
+/// triangles at each edge as in fig. 7 in the article, we build a convex hull (removing any triangles that
+/// are facing the new point, thereby avoiding the problem of getting really oblong triangles as mentioned in
+/// the article).
+///
+/// The algorithm roughly works like:
+///
+/// - Start with a simplex of the Minkowski sum (difference) of two objects that was calculated by GJK
+/// - This simplex should contain the origin (or else GJK would have reported: no collision)
+/// - In cases where the simplex consists of 1 - 3 points, find some extra support points (of the Minkowski sum) to get to at least 4 points
+/// - Convert this into a convex hull with non-zero volume (which includes the origin)
+/// - A: Calculate the closest point to the origin for all triangles of the hull and take the closest one
+/// - Calculate a new support point (of the Minkowski sum) in this direction and add this point to the convex hull
+/// - This will remove all faces that are facing the new point and will create new triangles to fill up the hole
+/// - Loop to A until no closer point found
+/// - The closest point indicates the position / direction of least penetration
+class EPAPenetrationDepth
+{
+private:
+	// Typedefs
+	static constexpr int cMaxPoints = EPAConvexHullBuilder::cMaxPoints;
+	static constexpr int cMaxPointsToIncludeOriginInHull = 32;
+	static_assert(cMaxPointsToIncludeOriginInHull < cMaxPoints);
+
+	using Triangle = EPAConvexHullBuilder::Triangle;
+	using Points = EPAConvexHullBuilder::Points;
+
+	/// The GJK algorithm, used to start the EPA algorithm
+	GJKClosestPoint		mGJK;
+
+#ifdef JPH_ENABLE_ASSERTS
+	/// Tolerance as passed to the GJK algorithm, used for asserting.
+	float				mGJKTolerance = 0.0f;
+#endif // JPH_ENABLE_ASSERTS
+
+	/// A list of support points for the EPA algorithm
+	class SupportPoints
+	{
+	public:
+		/// List of support points
+		Points			mY;
+		Vec3			mP[cMaxPoints];
+		Vec3			mQ[cMaxPoints];
+
+		/// Calculate and add new support point to the list of points
+		template <typename A, typename B>
+		Vec3			Add(const A &inA, const B &inB, Vec3Arg inDirection, int &outIndex)
+		{
+			// Get support point of the minkowski sum A - B
+			Vec3 p = inA.GetSupport(inDirection);
+			Vec3 q = inB.GetSupport(-inDirection);
+			Vec3 w = p - q;
+
+			// Store new point
+			outIndex = mY.size();
+			mY.push_back(w);
+			mP[outIndex] = p;
+			mQ[outIndex] = q;
+
+			return w;
+		}
+	};
+
+public:
+	/// Return code for GetPenetrationDepthStepGJK
+	enum class EStatus
+	{
+		NotColliding,		///< Returned if the objects don't collide, in this case outPointA/outPointB are invalid
+		Colliding,			///< Returned if the objects penetrate
+		Indeterminate		///< Returned if the objects penetrate further than the convex radius. In this case you need to call GetPenetrationDepthStepEPA to get the actual penetration depth.
+	};
+
+	/// Calculates penetration depth between two objects, first step of two (the GJK step)
+	///
+	/// @param inAExcludingConvexRadius Object A without convex radius.
+	/// @param inBExcludingConvexRadius Object B without convex radius.
+	/// @param inConvexRadiusA Convex radius for A.
+	/// @param inConvexRadiusB Convex radius for B.
+	/// @param ioV Pass in previously returned value or (1, 0, 0). On return this value is changed to direction to move B out of collision along the shortest path (magnitude is meaningless).
+	/// @param inTolerance Minimal distance before A and B are considered colliding.
+	/// @param outPointA Position on A that has the least amount of penetration.
+	/// @param outPointB Position on B that has the least amount of penetration.
+	/// Use |outPointB - outPointA| to get the distance of penetration.
+	template <typename AE, typename BE>
+	EStatus				GetPenetrationDepthStepGJK(const AE &inAExcludingConvexRadius, float inConvexRadiusA, const BE &inBExcludingConvexRadius, float inConvexRadiusB, float inTolerance, Vec3 &ioV, Vec3 &outPointA, Vec3 &outPointB)
+	{
+		JPH_PROFILE_FUNCTION();
+
+		JPH_IF_ENABLE_ASSERTS(mGJKTolerance = inTolerance;)
+
+		// Don't supply a zero ioV, we only want to get points on the hull of the Minkowsky sum and not internal points.
+		//
+		// Note that if the assert below triggers, it is very likely that you have a MeshShape that contains a degenerate triangle (e.g. a sliver).
+		// Go up a couple of levels in the call stack to see if we're indeed testing a triangle and if it is degenerate.
+		// If this is the case then fix the triangles you supply to the MeshShape.
+		JPH_ASSERT(!ioV.IsNearZero());
+
+		// Get closest points
+		float combined_radius = inConvexRadiusA + inConvexRadiusB;
+		float combined_radius_sq = combined_radius * combined_radius;
+		float closest_points_dist_sq = mGJK.GetClosestPoints(inAExcludingConvexRadius, inBExcludingConvexRadius, inTolerance, combined_radius_sq, ioV, outPointA, outPointB);
+		if (closest_points_dist_sq > combined_radius_sq)
+		{
+			// No collision
+			return EStatus::NotColliding;
+		}
+		if (closest_points_dist_sq > 0.0f)
+		{
+			// Collision within convex radius, adjust points for convex radius
+			float v_len = sqrt(closest_points_dist_sq); // GetClosestPoints function returns |ioV|^2 when return value < FLT_MAX
+			outPointA += ioV * (inConvexRadiusA / v_len);
+			outPointB -= ioV * (inConvexRadiusB / v_len);
+			return EStatus::Colliding;
+		}
+
+		return EStatus::Indeterminate;
+	}
+
+	/// Calculates penetration depth between two objects, second step (the EPA step)
+	///
+	/// @param inAIncludingConvexRadius Object A with convex radius
+	/// @param inBIncludingConvexRadius Object B with convex radius
+	/// @param inTolerance A factor that determines the accuracy of the result. If the change of the squared distance is less than inTolerance * current_penetration_depth^2 the algorithm will terminate. Should be bigger or equal to FLT_EPSILON.
+	/// @param outV Direction to move B out of collision along the shortest path (magnitude is meaningless)
+	/// @param outPointA Position on A that has the least amount of penetration
+	/// @param outPointB Position on B that has the least amount of penetration
+	/// Use |outPointB - outPointA| to get the distance of penetration
+	///
+	/// @return False if the objects don't collide, in this case outPointA/outPointB are invalid.
+	/// True if the objects penetrate
+	template <typename AI, typename BI>
+	bool				GetPenetrationDepthStepEPA(const AI &inAIncludingConvexRadius, const BI &inBIncludingConvexRadius, float inTolerance, Vec3 &outV, Vec3 &outPointA, Vec3 &outPointB)
+	{
+		JPH_PROFILE_FUNCTION();
+
+		// Check that the tolerance makes sense (smaller value than this will just result in needless iterations)
+		JPH_ASSERT(inTolerance >= FLT_EPSILON);
+
+		// Fetch the simplex from GJK algorithm
+		SupportPoints support_points;
+		mGJK.GetClosestPointsSimplex(support_points.mY.data(), support_points.mP, support_points.mQ, support_points.mY.GetSizeRef());
+
+		// Fill up the amount of support points to 4
+		switch (support_points.mY.size())
+		{
+		case 1:
+			{
+				// 1 vertex, which must be at the origin, which is useless for our purpose
+				JPH_ASSERT(support_points.mY[0].IsNearZero(Square(mGJKTolerance)));
+				support_points.mY.pop_back();
+
+				// Add support points in 4 directions to form a tetrahedron around the origin
+				int p1, p2, p3, p4;
+				(void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, Vec3(0, 1, 0), p1);
+				(void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, Vec3(-1, -1, -1), p2);
+				(void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, Vec3(1, -1, -1), p3);
+				(void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, Vec3(0, -1, 1), p4);
+				JPH_ASSERT(p1 == 0);
+				JPH_ASSERT(p2 == 1);
+				JPH_ASSERT(p3 == 2);
+				JPH_ASSERT(p4 == 3);
+				break;
+			}
+
+		case 2:
+			{
+				// Two vertices, create 3 extra by taking perpendicular axis and rotating it around in 120 degree increments
+				Vec3 axis = (support_points.mY[1] - support_points.mY[0]).Normalized();
+				Mat44 rotation = Mat44::sRotation(axis, DegreesToRadians(120.0f));
+				Vec3 dir1 = axis.GetNormalizedPerpendicular();
+				Vec3 dir2 = rotation * dir1;
+				Vec3 dir3 = rotation * dir2;
+				int p1, p2, p3;
+				(void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, dir1, p1);
+				(void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, dir2, p2);
+				(void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, dir3, p3);
+				JPH_ASSERT(p1 == 2);
+				JPH_ASSERT(p2 == 3);
+				JPH_ASSERT(p3 == 4);
+				break;
+			}
+
+		case 3:
+		case 4:
+			// We already have enough points
+			break;
+		}
+
+		// Create hull out of the initial points
+		JPH_ASSERT(support_points.mY.size() >= 3);
+		EPAConvexHullBuilder hull(support_points.mY);
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+		hull.DrawLabel("Build initial hull");
+#endif
+#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG
+		Trace("Init: num_points = %u", (uint)support_points.mY.size());
+#endif
+		hull.Initialize(0, 1, 2);
+		for (typename Points::size_type i = 3; i < support_points.mY.size(); ++i)
+		{
+			float dist_sq;
+			Triangle *t = hull.FindFacingTriangle(support_points.mY[i], dist_sq);
+			if (t != nullptr)
+			{
+				EPAConvexHullBuilder::NewTriangles new_triangles;
+				if (!hull.AddPoint(t, i, FLT_MAX, new_triangles))
+				{
+					// We can't recover from a failure to add a point to the hull because the old triangles have been unlinked already.
+					// Assume no collision. This can happen if the shapes touch in 1 point (or plane) in which case the hull is degenerate.
+					return false;
+				}
+			}
+		}
+
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+		hull.DrawLabel("Complete hull");
+
+		// Generate the hull of the Minkowski difference for visualization
+		MinkowskiDifference diff(inAIncludingConvexRadius, inBIncludingConvexRadius);
+		DebugRenderer::GeometryRef geometry = DebugRenderer::sInstance->CreateTriangleGeometryForConvex([&diff](Vec3Arg inDirection) { return diff.GetSupport(inDirection); });
+		hull.DrawGeometry(geometry, Color::sYellow);
+
+		hull.DrawLabel("Ensure origin in hull");
+#endif
+
+		// Loop until we are sure that the origin is inside the hull
+		for (;;)
+		{
+			// Get the next closest triangle
+			Triangle *t = hull.PeekClosestTriangleInQueue();
+
+			// Don't process removed triangles, just free them (because they're in a heap we don't remove them earlier since we would have to rebuild the sorted heap)
+			if (t->mRemoved)
+			{
+				hull.PopClosestTriangleFromQueue();
+
+				// If we run out of triangles, we couldn't include the origin in the hull so there must be very little penetration and we report no collision.
+				if (!hull.HasNextTriangle())
+					return false;
+
+				hull.FreeTriangle(t);
+				continue;
+			}
+
+			// If the closest to the triangle is zero or positive, the origin is in the hull and we can proceed to the main algorithm
+			if (t->mClosestLenSq >= 0.0f)
+				break;
+
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+			hull.DrawLabel("Next iteration");
+#endif
+#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG
+			Trace("EncapsulateOrigin: verts = (%d, %d, %d), closest_dist_sq = %g, centroid = (%g, %g, %g), normal = (%g, %g, %g)",
+				t->mEdge[0].mStartIdx, t->mEdge[1].mStartIdx, t->mEdge[2].mStartIdx,
+				t->mClosestLenSq,
+				t->mCentroid.GetX(), t->mCentroid.GetY(), t->mCentroid.GetZ(),
+				t->mNormal.GetX(), t->mNormal.GetY(), t->mNormal.GetZ());
+#endif
+
+			// Remove the triangle from the queue before we start adding new ones (which may result in a new closest triangle at the front of the queue)
+			hull.PopClosestTriangleFromQueue();
+
+			// Add a support point to get the origin inside the hull
+			int new_index;
+			Vec3 w = support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, t->mNormal, new_index);
+
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+			// Draw the point that we're adding
+			hull.DrawMarker(w, Color::sRed, 1.0f);
+			hull.DrawWireTriangle(*t, Color::sRed);
+			hull.DrawState();
+#endif
+
+			// Add the point to the hull, if we fail we terminate and report no collision
+			EPAConvexHullBuilder::NewTriangles new_triangles;
+			if (!t->IsFacing(w) || !hull.AddPoint(t, new_index, FLT_MAX, new_triangles))
+				return false;
+
+			// The triangle is facing the support point "w" and can now be safely removed
+			JPH_ASSERT(t->mRemoved);
+			hull.FreeTriangle(t);
+
+			// If we run out of triangles or points, we couldn't include the origin in the hull so there must be very little penetration and we report no collision.
+			if (!hull.HasNextTriangle() || support_points.mY.size() >= cMaxPointsToIncludeOriginInHull)
+				return false;
+		}
+
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+		hull.DrawLabel("Main algorithm");
+#endif
+
+		// Current closest distance to origin
+		float closest_dist_sq = FLT_MAX;
+
+		// Remember last good triangle
+		Triangle *last = nullptr;
+
+		// If we want to flip the penetration depth
+		bool flip_v_sign = false;
+
+		// Loop until closest point found
+		do
+		{
+			// Get closest triangle to the origin
+			Triangle *t = hull.PopClosestTriangleFromQueue();
+
+			// Don't process removed triangles, just free them (because they're in a heap we don't remove them earlier since we would have to rebuild the sorted heap)
+			if (t->mRemoved)
+			{
+				hull.FreeTriangle(t);
+				continue;
+			}
+
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+			hull.DrawLabel("Next iteration");
+#endif
+#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG
+			Trace("FindClosest: verts = (%d, %d, %d), closest_len_sq = %g, centroid = (%g, %g, %g), normal = (%g, %g, %g)",
+				t->mEdge[0].mStartIdx, t->mEdge[1].mStartIdx, t->mEdge[2].mStartIdx,
+				t->mClosestLenSq,
+				t->mCentroid.GetX(), t->mCentroid.GetY(), t->mCentroid.GetZ(),
+				t->mNormal.GetX(), t->mNormal.GetY(), t->mNormal.GetZ());
+#endif
+			// Check if next triangle is further away than closest point, we've found the closest point
+			if (t->mClosestLenSq >= closest_dist_sq)
+				break;
+
+			// Replace last good with this triangle
+			if (last != nullptr)
+				hull.FreeTriangle(last);
+			last = t;
+
+			// Add support point in direction of normal of the plane
+			// Note that the article uses the closest point between the origin and plane, but this always has the exact same direction as the normal (if the origin is behind the plane)
+			// and this way we do less calculations and lose less precision
+			int new_index;
+			Vec3 w = support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, t->mNormal, new_index);
+
+			// Project w onto the triangle normal
+			float dot = t->mNormal.Dot(w);
+
+			// Check if we just found a separating axis. This can happen if the shape shrunk by convex radius and then expanded by
+			// convex radius is bigger then the original shape due to inaccuracies in the shrinking process.
+			if (dot < 0.0f)
+				return false;
+
+			// Get the distance squared (along normal) to the support point
+			float dist_sq = Square(dot) / t->mNormal.LengthSq();
+
+#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG
+			Trace("FindClosest: w = (%g, %g, %g), dot = %g, dist_sq = %g",
+				w.GetX(), w.GetY(), w.GetZ(),
+				dot, dist_sq);
+#endif
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+			// Draw the point that we're adding
+			hull.DrawMarker(w, Color::sPurple, 1.0f);
+			hull.DrawWireTriangle(*t, Color::sPurple);
+			hull.DrawState();
+#endif
+
+			// If the error became small enough, we've converged
+			if (dist_sq - t->mClosestLenSq < t->mClosestLenSq * inTolerance)
+			{
+#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG
+				Trace("Converged");
+#endif // JPH_EPA_PENETRATION_DEPTH_DEBUG
+				break;
+			}
+
+			// Keep track of the minimum distance
+			closest_dist_sq = min(closest_dist_sq, dist_sq);
+
+			// If the triangle thinks this point is not front facing, we've reached numerical precision and we're done
+			if (!t->IsFacing(w))
+			{
+#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG
+				Trace("Not facing triangle");
+#endif // JPH_EPA_PENETRATION_DEPTH_DEBUG
+				break;
+			}
+
+			// Add point to hull
+			EPAConvexHullBuilder::NewTriangles new_triangles;
+			if (!hull.AddPoint(t, new_index, closest_dist_sq, new_triangles))
+			{
+#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG
+				Trace("Could not add point");
+#endif // JPH_EPA_PENETRATION_DEPTH_DEBUG
+				break;
+			}
+
+			// If the hull is starting to form defects then we're reaching numerical precision and we have to stop
+			bool has_defect = false;
+			for (const Triangle *nt : new_triangles)
+				if (nt->IsFacingOrigin())
+				{
+					has_defect = true;
+					break;
+				}
+			if (has_defect)
+			{
+#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG
+				Trace("Has defect");
+#endif // JPH_EPA_PENETRATION_DEPTH_DEBUG
+				// When the hull has defects it is possible that the origin has been classified on the wrong side of the triangle
+				// so we do an additional check to see if the penetration in the -triangle normal direction is smaller than
+				// the penetration in the triangle normal direction. If so we must flip the sign of the penetration depth.
+				Vec3 w2 = inAIncludingConvexRadius.GetSupport(-t->mNormal) - inBIncludingConvexRadius.GetSupport(t->mNormal);
+				float dot2 = -t->mNormal.Dot(w2);
+				if (dot2 < dot)
+					flip_v_sign = true;
+				break;
+			}
+		}
+		while (hull.HasNextTriangle() && support_points.mY.size() < cMaxPoints);
+
+		// Determine closest points, if last == null it means the hull was a plane so there's no penetration
+		if (last == nullptr)
+			return false;
+
+#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
+		hull.DrawLabel("Closest found");
+		hull.DrawWireTriangle(*last, Color::sWhite);
+		hull.DrawArrow(last->mCentroid, last->mCentroid + last->mNormal.NormalizedOr(Vec3::sZero()), Color::sWhite, 0.1f);
+		hull.DrawState();
+#endif
+
+		// Calculate penetration by getting the vector from the origin to the closest point on the triangle:
+		// distance = (centroid - origin) . normal / |normal|, closest = origin + distance * normal / |normal|
+		outV = (last->mCentroid.Dot(last->mNormal) / last->mNormal.LengthSq()) * last->mNormal;
+
+		// If penetration is near zero, treat this as a non collision since we cannot find a good normal
+		if (outV.IsNearZero())
+			return false;
+
+		// Check if we have to flip the sign of the penetration depth
+		if (flip_v_sign)
+			outV = -outV;
+
+		// Use the barycentric coordinates for the closest point to the origin to find the contact points on A and B
+		Vec3 p0 = support_points.mP[last->mEdge[0].mStartIdx];
+		Vec3 p1 = support_points.mP[last->mEdge[1].mStartIdx];
+		Vec3 p2 = support_points.mP[last->mEdge[2].mStartIdx];
+
+		Vec3 q0 = support_points.mQ[last->mEdge[0].mStartIdx];
+		Vec3 q1 = support_points.mQ[last->mEdge[1].mStartIdx];
+		Vec3 q2 = support_points.mQ[last->mEdge[2].mStartIdx];
+
+		if (last->mLambdaRelativeTo0)
+		{
+			// y0 was the reference vertex
+			outPointA = p0 + last->mLambda[0] * (p1 - p0) + last->mLambda[1] * (p2 - p0);
+			outPointB = q0 + last->mLambda[0] * (q1 - q0) + last->mLambda[1] * (q2 - q0);
+		}
+		else
+		{
+			// y1 was the reference vertex
+			outPointA = p1 + last->mLambda[0] * (p0 - p1) + last->mLambda[1] * (p2 - p1);
+			outPointB = q1 + last->mLambda[0] * (q0 - q1) + last->mLambda[1] * (q2 - q1);
+		}
+
+		return true;
+	}
+
+	/// This function combines the GJK and EPA steps and is provided as a convenience function.
+	/// Note: less performant since you're providing all support functions in one go
+	/// Note 2: You need to initialize ioV, see documentation at GetPenetrationDepthStepGJK!
+	template <typename AE, typename AI, typename BE, typename BI>
+	bool				GetPenetrationDepth(const AE &inAExcludingConvexRadius, const AI &inAIncludingConvexRadius, float inConvexRadiusA, const BE &inBExcludingConvexRadius, const BI &inBIncludingConvexRadius, float inConvexRadiusB, float inCollisionToleranceSq, float inPenetrationTolerance, Vec3 &ioV, Vec3 &outPointA, Vec3 &outPointB)
+	{
+		// Check result of collision detection
+		switch (GetPenetrationDepthStepGJK(inAExcludingConvexRadius, inConvexRadiusA, inBExcludingConvexRadius, inConvexRadiusB, inCollisionToleranceSq, ioV, outPointA, outPointB))
+		{
+		case EPAPenetrationDepth::EStatus::Colliding:
+			return true;
+
+		case EPAPenetrationDepth::EStatus::NotColliding:
+			return false;
+
+		case EPAPenetrationDepth::EStatus::Indeterminate:
+			return GetPenetrationDepthStepEPA(inAIncludingConvexRadius, inBIncludingConvexRadius, inPenetrationTolerance, ioV, outPointA, outPointB);
+		}
+
+		JPH_ASSERT(false);
+		return false;
+	}
+
+	/// Test if a cast shape inA moving from inStart to lambda * inStart.GetTranslation() + inDirection where lambda e [0, ioLambda> intersects inB
+	///
+	/// @param inStart Start position and orientation of the convex object
+	/// @param inDirection Direction of the sweep (ioLambda * inDirection determines length)
+	///	@param inCollisionTolerance The minimal distance between A and B before they are considered colliding
+	/// @param inPenetrationTolerance A factor that determines the accuracy of the result. If the change of the squared distance is less than inTolerance * current_penetration_depth^2 the algorithm will terminate. Should be bigger or equal to FLT_EPSILON.
+	/// @param inA The convex object A, must support the GetSupport(Vec3) function.
+	/// @param inB The convex object B, must support the GetSupport(Vec3) function.
+	/// @param inConvexRadiusA The convex radius of A, this will be added on all sides to pad A.
+	/// @param inConvexRadiusB The convex radius of B, this will be added on all sides to pad B.
+	/// @param inReturnDeepestPoint If the shapes are initially intersecting this determines if the EPA algorithm will run to find the deepest point
+	/// @param ioLambda The max fraction along the sweep, on output updated with the actual collision fraction.
+	///	@param outPointA is the contact point on A
+	///	@param outPointB is the contact point on B
+	/// @param outContactNormal is either the contact normal when the objects are touching or the penetration axis when the objects are penetrating at the start of the sweep (pointing from A to B, length will not be 1)
+	///
+	/// @return true if the a hit was found, in which case ioLambda, outPointA, outPointB and outSurfaceNormal are updated.
+	template <typename A, typename B>
+	bool				CastShape(Mat44Arg inStart, Vec3Arg inDirection, float inCollisionTolerance, float inPenetrationTolerance, const A &inA, const B &inB, float inConvexRadiusA, float inConvexRadiusB, bool inReturnDeepestPoint, float &ioLambda, Vec3 &outPointA, Vec3 &outPointB, Vec3 &outContactNormal)
+	{
+		JPH_IF_ENABLE_ASSERTS(mGJKTolerance = inCollisionTolerance;)
+
+		// First determine if there's a collision at all
+		if (!mGJK.CastShape(inStart, inDirection, inCollisionTolerance, inA, inB, inConvexRadiusA, inConvexRadiusB, ioLambda, outPointA, outPointB, outContactNormal))
+			return false;
+
+		// When our contact normal is too small, we don't have an accurate result
+		bool contact_normal_invalid = outContactNormal.IsNearZero(Square(inCollisionTolerance));
+
+		if (inReturnDeepestPoint
+			&& ioLambda == 0.0f // Only when lambda = 0 we can have the bodies overlap
+			&& (inConvexRadiusA + inConvexRadiusB == 0.0f // When no convex radius was provided we can never trust contact points at lambda = 0
+				|| contact_normal_invalid))
+		{
+			// If we're initially intersecting, we need to run the EPA algorithm in order to find the deepest contact point
+			AddConvexRadius<A> add_convex_a(inA, inConvexRadiusA);
+			AddConvexRadius<B> add_convex_b(inB, inConvexRadiusB);
+			TransformedConvexObject<AddConvexRadius<A>> transformed_a(inStart, add_convex_a);
+			if (!GetPenetrationDepthStepEPA(transformed_a, add_convex_b, inPenetrationTolerance, outContactNormal, outPointA, outPointB))
+				return false;
+		}
+		else if (contact_normal_invalid)
+		{
+			// If we weren't able to calculate a contact normal, use the cast direction instead
+			outContactNormal = inDirection;
+		}
+
+		return true;
+	}
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Geometry/Ellipse.h b/thirdparty/jolt_physics/Jolt/Geometry/Ellipse.h
new file mode 100644
index 0000000000..bfa508faae
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Geometry/Ellipse.h
@@ -0,0 +1,77 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Math/Float2.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Ellipse centered around the origin
+/// @see https://en.wikipedia.org/wiki/Ellipse
+class Ellipse
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct ellipse with radius A along the X-axis and B along the Y-axis
+					Ellipse(float inA, float inB) : mA(inA), mB(inB) { JPH_ASSERT(inA > 0.0f); JPH_ASSERT(inB > 0.0f); }
+
+	/// Check if inPoint is inside the ellipse
+	bool			IsInside(const Float2 &inPoint) const
+	{
+		return Square(inPoint.x / mA) + Square(inPoint.y / mB) <= 1.0f;
+	}
+
+	/// Get the closest point on the ellipse to inPoint
+	/// Assumes inPoint is outside the ellipse
+	/// @see Rotation Joint Limits in Quaternion Space by Gino van den Bergen, section 10.1 in Game Engine Gems 3.
+	Float2			GetClosestPoint(const Float2 &inPoint) const
+	{
+		float a_sq = Square(mA);
+		float b_sq = Square(mB);
+
+		// Equation of ellipse: f(x, y) = (x/a)^2 + (y/b)^2 - 1 = 0											[1]
+		// Normal on surface: (df/dx, df/dy) = (2 x / a^2, 2 y / b^2)
+		// Closest point (x', y') on ellipse to point (x, y): (x', y') + t (x / a^2, y / b^2) = (x, y)
+		// <=> (x', y') = (a^2 x / (t + a^2), b^2 y / (t + b^2))
+		// Requiring point to be on ellipse (substituting into [1]): g(t) = (a x / (t + a^2))^2 + (b y / (t + b^2))^2 - 1 = 0
+
+		// Newton raphson iteration, starting at t = 0
+		float t = 0.0f;
+		for (;;)
+		{
+			// Calculate g(t)
+			float t_plus_a_sq = t + a_sq;
+			float t_plus_b_sq = t + b_sq;
+			float gt = Square(mA * inPoint.x / t_plus_a_sq) + Square(mB * inPoint.y / t_plus_b_sq) - 1.0f;
+
+			// Check if g(t) it is close enough to zero
+			if (abs(gt) < 1.0e-6f)
+				return Float2(a_sq * inPoint.x / t_plus_a_sq, b_sq * inPoint.y / t_plus_b_sq);
+
+			// Get derivative dg/dt = g'(t) = -2 (b^2 y^2 / (t + b^2)^3 + a^2 x^2 / (t + a^2)^3)
+			float gt_accent = -2.0f *
+				(a_sq * Square(inPoint.x) / Cubed(t_plus_a_sq)
+				+ b_sq * Square(inPoint.y) / Cubed(t_plus_b_sq));
+
+			// Calculate t for next iteration: tn+1 = tn - g(t) / g'(t)
+			float tn = t - gt / gt_accent;
+			t = tn;
+		}
+	}
+
+	/// Get normal at point inPoint (non-normalized vector)
+	Float2			GetNormal(const Float2 &inPoint) const
+	{
+		// Calculated by [d/dx f(x, y), d/dy f(x, y)], where f(x, y) is the ellipse equation from above
+		return Float2(inPoint.x / Square(mA), inPoint.y / Square(mB));
+	}
+
+private:
+	float			mA;				///< Radius along X-axis
+	float			mB;				///< Radius along Y-axis
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Geometry/GJKClosestPoint.h b/thirdparty/jolt_physics/Jolt/Geometry/GJKClosestPoint.h
new file mode 100644
index 0000000000..e5f49b65a0
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Geometry/GJKClosestPoint.h
@@ -0,0 +1,946 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/NonCopyable.h>
+#include <Jolt/Core/FPException.h>
+#include <Jolt/Geometry/ClosestPoint.h>
+#include <Jolt/Geometry/ConvexSupport.h>
+
+//#define JPH_GJK_DEBUG
+#ifdef JPH_GJK_DEBUG
+	#include <Jolt/Core/StringTools.h>
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif
+
+JPH_NAMESPACE_BEGIN
+
+/// Convex vs convex collision detection
+/// Based on: A Fast and Robust GJK Implementation for Collision Detection of Convex Objects - Gino van den Bergen
+class GJKClosestPoint : public NonCopyable
+{
+private:
+	/// Get new closest point to origin given simplex mY of mNumPoints points
+	///
+	/// @param inPrevVLenSq Length of |outV|^2 from the previous iteration, used as a maximum value when selecting a new closest point.
+	/// @param outV Closest point
+	/// @param outVLenSq |outV|^2
+	/// @param outSet Set of points that form the new simplex closest to the origin (bit 1 = mY[0], bit 2 = mY[1], ...)
+	///
+	/// If LastPointPartOfClosestFeature is true then the last point added will be assumed to be part of the closest feature and the function will do less work.
+	///
+	/// @return True if new closest point was found.
+	/// False if the function failed, in this case the output variables are not modified
+	template <bool LastPointPartOfClosestFeature>
+	bool		GetClosest(float inPrevVLenSq, Vec3 &outV, float &outVLenSq, uint32 &outSet) const
+	{
+#ifdef JPH_GJK_DEBUG
+		for (int i = 0; i < mNumPoints; ++i)
+			Trace("y[%d] = [%s], |y[%d]| = %g", i, ConvertToString(mY[i]).c_str(), i, (double)mY[i].Length());
+#endif
+
+		uint32 set;
+		Vec3 v;
+
+		switch (mNumPoints)
+		{
+		case 1:
+			// Single point
+			set = 0b0001;
+			v = mY[0];
+			break;
+
+		case 2:
+			// Line segment
+			v = ClosestPoint::GetClosestPointOnLine(mY[0], mY[1], set);
+			break;
+
+		case 3:
+			// Triangle
+			v = ClosestPoint::GetClosestPointOnTriangle<LastPointPartOfClosestFeature>(mY[0], mY[1], mY[2], set);
+			break;
+
+		case 4:
+			// Tetrahedron
+			v = ClosestPoint::GetClosestPointOnTetrahedron<LastPointPartOfClosestFeature>(mY[0], mY[1], mY[2], mY[3], set);
+			break;
+
+		default:
+			JPH_ASSERT(false);
+			return false;
+		}
+
+#ifdef JPH_GJK_DEBUG
+		Trace("GetClosest: set = 0b%s, v = [%s], |v| = %g", NibbleToBinary(set), ConvertToString(v).c_str(), (double)v.Length());
+#endif
+
+		float v_len_sq = v.LengthSq();
+		if (v_len_sq < inPrevVLenSq) // Note, comparison order important: If v_len_sq is NaN then this expression will be false so we will return false
+		{
+			// Return closest point
+			outV = v;
+			outVLenSq = v_len_sq;
+			outSet = set;
+			return true;
+		}
+
+		// No better match found
+#ifdef JPH_GJK_DEBUG
+		Trace("New closer point is further away, failed to converge");
+#endif
+		return false;
+	}
+
+	// Get max(|Y_0|^2 .. |Y_n|^2)
+	float		GetMaxYLengthSq() const
+	{
+		float y_len_sq = mY[0].LengthSq();
+		for (int i = 1; i < mNumPoints; ++i)
+			y_len_sq = max(y_len_sq, mY[i].LengthSq());
+		return y_len_sq;
+	}
+
+	// Remove points that are not in the set, only updates mY
+	void		UpdatePointSetY(uint32 inSet)
+	{
+		int num_points = 0;
+		for (int i = 0; i < mNumPoints; ++i)
+			if ((inSet & (1 << i)) != 0)
+			{
+				mY[num_points] = mY[i];
+				++num_points;
+			}
+		mNumPoints = num_points;
+	}
+
+	// Remove points that are not in the set, only updates mP
+	void		UpdatePointSetP(uint32 inSet)
+	{
+		int num_points = 0;
+		for (int i = 0; i < mNumPoints; ++i)
+			if ((inSet & (1 << i)) != 0)
+			{
+				mP[num_points] = mP[i];
+				++num_points;
+			}
+		mNumPoints = num_points;
+	}
+
+	// Remove points that are not in the set, only updates mP and mQ
+	void		UpdatePointSetPQ(uint32 inSet)
+	{
+		int num_points = 0;
+		for (int i = 0; i < mNumPoints; ++i)
+			if ((inSet & (1 << i)) != 0)
+			{
+				mP[num_points] = mP[i];
+				mQ[num_points] = mQ[i];
+				++num_points;
+			}
+		mNumPoints = num_points;
+	}
+
+	// Remove points that are not in the set, updates mY, mP and mQ
+	void		UpdatePointSetYPQ(uint32 inSet)
+	{
+		int num_points = 0;
+		for (int i = 0; i < mNumPoints; ++i)
+			if ((inSet & (1 << i)) != 0)
+			{
+				mY[num_points] = mY[i];
+				mP[num_points] = mP[i];
+				mQ[num_points] = mQ[i];
+				++num_points;
+			}
+		mNumPoints = num_points;
+	}
+
+	// Calculate closest points on A and B
+	void		CalculatePointAAndB(Vec3 &outPointA, Vec3 &outPointB) const
+	{
+		switch (mNumPoints)
+		{
+		case 1:
+			outPointA = mP[0];
+			outPointB = mQ[0];
+			break;
+
+		case 2:
+			{
+				float u, v;
+				ClosestPoint::GetBaryCentricCoordinates(mY[0], mY[1], u, v);
+				outPointA = u * mP[0] + v * mP[1];
+				outPointB = u * mQ[0] + v * mQ[1];
+			}
+			break;
+
+		case 3:
+			{
+				float u, v, w;
+				ClosestPoint::GetBaryCentricCoordinates(mY[0], mY[1], mY[2], u, v, w);
+				outPointA = u * mP[0] + v * mP[1] + w * mP[2];
+				outPointB = u * mQ[0] + v * mQ[1] + w * mQ[2];
+			}
+			break;
+
+		case 4:
+		#ifdef JPH_DEBUG
+			memset(&outPointA, 0xcd, sizeof(outPointA));
+			memset(&outPointB, 0xcd, sizeof(outPointB));
+		#endif
+			break;
+		}
+	}
+
+public:
+	/// Test if inA and inB intersect
+	///
+	/// @param inA The convex object A, must support the GetSupport(Vec3) function.
+	/// @param inB The convex object B, must support the GetSupport(Vec3) function.
+	///	@param inTolerance Minimal distance between objects when the objects are considered to be colliding
+	///	@param ioV is used as initial separating axis (provide a zero vector if you don't know yet)
+	///
+	///	@return True if they intersect (in which case ioV = (0, 0, 0)).
+	///	False if they don't intersect in which case ioV is a separating axis in the direction from A to B (magnitude is meaningless)
+	template <typename A, typename B>
+	bool		Intersects(const A &inA, const B &inB, float inTolerance, Vec3 &ioV)
+	{
+		float tolerance_sq = Square(inTolerance);
+
+		// Reset state
+		mNumPoints = 0;
+
+#ifdef JPH_GJK_DEBUG
+		for (int i = 0; i < 4; ++i)
+			mY[i] = Vec3::sZero();
+#endif
+
+		// Previous length^2 of v
+		float prev_v_len_sq = FLT_MAX;
+
+		for (;;)
+		{
+#ifdef JPH_GJK_DEBUG
+			Trace("v = [%s], num_points = %d", ConvertToString(ioV).c_str(), mNumPoints);
+#endif
+
+			// Get support points for shape A and B in the direction of v
+			Vec3 p = inA.GetSupport(ioV);
+			Vec3 q = inB.GetSupport(-ioV);
+
+			// Get support point of the minkowski sum A - B of v
+			Vec3 w = p - q;
+
+			// If the support point sA-B(v) is in the opposite direction as v, then we have found a separating axis and there is no intersection
+			if (ioV.Dot(w) < 0.0f)
+			{
+				// Separating axis found
+#ifdef JPH_GJK_DEBUG
+				Trace("Separating axis");
+#endif
+				return false;
+			}
+
+			// Store the point for later use
+			mY[mNumPoints] = w;
+			++mNumPoints;
+
+#ifdef JPH_GJK_DEBUG
+			Trace("w = [%s]", ConvertToString(w).c_str());
+#endif
+
+			// Determine the new closest point
+			float v_len_sq;			// Length^2 of v
+			uint32 set;				// Set of points that form the new simplex
+			if (!GetClosest<true>(prev_v_len_sq, ioV, v_len_sq, set))
+				return false;
+
+			// If there are 4 points, the origin is inside the tetrahedron and we're done
+			if (set == 0xf)
+			{
+#ifdef JPH_GJK_DEBUG
+				Trace("Full simplex");
+#endif
+				ioV = Vec3::sZero();
+				return true;
+			}
+
+			// If v is very close to zero, we consider this a collision
+			if (v_len_sq <= tolerance_sq)
+			{
+#ifdef JPH_GJK_DEBUG
+				Trace("Distance zero");
+#endif
+				ioV = Vec3::sZero();
+				return true;
+			}
+
+			// If v is very small compared to the length of y, we also consider this a collision
+			if (v_len_sq <= FLT_EPSILON * GetMaxYLengthSq())
+			{
+#ifdef JPH_GJK_DEBUG
+				Trace("Machine precision reached");
+#endif
+				ioV = Vec3::sZero();
+				return true;
+			}
+
+			// The next separation axis to test is the negative of the closest point of the Minkowski sum to the origin
+			// Note: This must be done before terminating as converged since the separating axis is -v
+			ioV = -ioV;
+
+			// If the squared length of v is not changing enough, we've converged and there is no collision
+			JPH_ASSERT(prev_v_len_sq >= v_len_sq);
+			if (prev_v_len_sq - v_len_sq <= FLT_EPSILON * prev_v_len_sq)
+			{
+				// v is a separating axis
+#ifdef JPH_GJK_DEBUG
+				Trace("Converged");
+#endif
+				return false;
+			}
+			prev_v_len_sq = v_len_sq;
+
+			// Update the points of the simplex
+			UpdatePointSetY(set);
+		}
+	}
+
+	/// Get closest points between inA and inB
+	///
+	/// @param inA The convex object A, must support the GetSupport(Vec3) function.
+	/// @param inB The convex object B, must support the GetSupport(Vec3) function.
+	///	@param inTolerance The minimal distance between A and B before the objects are considered colliding and processing is terminated.
+	///	@param inMaxDistSq The maximum squared distance between A and B before the objects are considered infinitely far away and processing is terminated.
+	///	@param ioV Initial guess for the separating axis. Start with any non-zero vector if you don't know.
+	///		If return value is 0, ioV = (0, 0, 0).
+	///		If the return value is bigger than 0 but smaller than FLT_MAX, ioV will be the separating axis in the direction from A to B and its length the squared distance between A and B.
+	///		If the return value is FLT_MAX, ioV will be the separating axis in the direction from A to B and the magnitude of the vector is meaningless.
+	///	@param outPointA , outPointB
+	///		If the return value is 0 the points are invalid.
+	///		If the return value is bigger than 0 but smaller than FLT_MAX these will contain the closest point on A and B.
+	///		If the return value is FLT_MAX the points are invalid.
+	///
+	///	@return The squared distance between A and B or FLT_MAX when they are further away than inMaxDistSq.
+	template <typename A, typename B>
+	float		GetClosestPoints(const A &inA, const B &inB, float inTolerance, float inMaxDistSq, Vec3 &ioV, Vec3 &outPointA, Vec3 &outPointB)
+	{
+		float tolerance_sq = Square(inTolerance);
+
+		// Reset state
+		mNumPoints = 0;
+
+#ifdef JPH_GJK_DEBUG
+		// Generate the hull of the Minkowski difference for visualization
+		MinkowskiDifference diff(inA, inB);
+		mGeometry = DebugRenderer::sInstance->CreateTriangleGeometryForConvex([&diff](Vec3Arg inDirection) { return diff.GetSupport(inDirection); });
+
+		for (int i = 0; i < 4; ++i)
+		{
+			mY[i] = Vec3::sZero();
+			mP[i] = Vec3::sZero();
+			mQ[i] = Vec3::sZero();
+		}
+#endif
+
+		// Length^2 of v
+		float v_len_sq = ioV.LengthSq();
+
+		// Previous length^2 of v
+		float prev_v_len_sq = FLT_MAX;
+
+		for (;;)
+		{
+#ifdef JPH_GJK_DEBUG
+			Trace("v = [%s], num_points = %d", ConvertToString(ioV).c_str(), mNumPoints);
+#endif
+
+			// Get support points for shape A and B in the direction of v
+			Vec3 p = inA.GetSupport(ioV);
+			Vec3 q = inB.GetSupport(-ioV);
+
+			// Get support point of the minkowski sum A - B of v
+			Vec3 w = p - q;
+
+			float dot = ioV.Dot(w);
+
+#ifdef JPH_GJK_DEBUG
+			// Draw -ioV to show the closest point to the origin from the previous simplex
+			DebugRenderer::sInstance->DrawArrow(mOffset, mOffset - ioV, Color::sOrange, 0.05f);
+
+			// Draw ioV to show where we're probing next
+			DebugRenderer::sInstance->DrawArrow(mOffset, mOffset + ioV, Color::sCyan, 0.05f);
+
+			// Draw w, the support point
+			DebugRenderer::sInstance->DrawArrow(mOffset, mOffset + w, Color::sGreen, 0.05f);
+			DebugRenderer::sInstance->DrawMarker(mOffset + w, Color::sGreen, 1.0f);
+
+			// Draw the simplex and the Minkowski difference around it
+			DrawState();
+#endif
+
+			// Test if we have a separation of more than inMaxDistSq, in which case we terminate early
+			if (dot < 0.0f && dot * dot > v_len_sq * inMaxDistSq)
+			{
+#ifdef JPH_GJK_DEBUG
+				Trace("Distance bigger than max");
+#endif
+#ifdef JPH_DEBUG
+				memset(&outPointA, 0xcd, sizeof(outPointA));
+				memset(&outPointB, 0xcd, sizeof(outPointB));
+#endif
+				return FLT_MAX;
+			}
+
+			// Store the point for later use
+			mY[mNumPoints] = w;
+			mP[mNumPoints] = p;
+			mQ[mNumPoints] = q;
+			++mNumPoints;
+
+#ifdef JPH_GJK_DEBUG
+			Trace("w = [%s]", ConvertToString(w).c_str());
+#endif
+
+			uint32 set;
+			if (!GetClosest<true>(prev_v_len_sq, ioV, v_len_sq, set))
+			{
+				--mNumPoints; // Undo add last point
+				break;
+			}
+
+			// If there are 4 points, the origin is inside the tetrahedron and we're done
+			if (set == 0xf)
+			{
+#ifdef JPH_GJK_DEBUG
+				Trace("Full simplex");
+#endif
+				ioV = Vec3::sZero();
+				v_len_sq = 0.0f;
+				break;
+			}
+
+			// Update the points of the simplex
+			UpdatePointSetYPQ(set);
+
+			// If v is very close to zero, we consider this a collision
+			if (v_len_sq <= tolerance_sq)
+			{
+#ifdef JPH_GJK_DEBUG
+				Trace("Distance zero");
+#endif
+				ioV = Vec3::sZero();
+				v_len_sq = 0.0f;
+				break;
+			}
+
+			// If v is very small compared to the length of y, we also consider this a collision
+#ifdef JPH_GJK_DEBUG
+			Trace("Check v small compared to y: %g <= %g", (double)v_len_sq, (double)(FLT_EPSILON * GetMaxYLengthSq()));
+#endif
+			if (v_len_sq <= FLT_EPSILON * GetMaxYLengthSq())
+			{
+#ifdef JPH_GJK_DEBUG
+				Trace("Machine precision reached");
+#endif
+				ioV = Vec3::sZero();
+				v_len_sq = 0.0f;
+				break;
+			}
+
+			// The next separation axis to test is the negative of the closest point of the Minkowski sum to the origin
+			// Note: This must be done before terminating as converged since the separating axis is -v
+			ioV = -ioV;
+
+			// If the squared length of v is not changing enough, we've converged and there is no collision
+#ifdef JPH_GJK_DEBUG
+			Trace("Check v not changing enough: %g <= %g", (double)(prev_v_len_sq - v_len_sq), (double)(FLT_EPSILON * prev_v_len_sq));
+#endif
+			JPH_ASSERT(prev_v_len_sq >= v_len_sq);
+			if (prev_v_len_sq - v_len_sq <= FLT_EPSILON * prev_v_len_sq)
+			{
+				// v is a separating axis
+#ifdef JPH_GJK_DEBUG
+				Trace("Converged");
+#endif
+				break;
+			}
+			prev_v_len_sq = v_len_sq;
+		}
+
+		// Get the closest points
+		CalculatePointAAndB(outPointA, outPointB);
+
+#ifdef JPH_GJK_DEBUG
+		Trace("Return: v = [%s], |v| = %g", ConvertToString(ioV).c_str(), (double)ioV.Length());
+
+		// Draw -ioV to show the closest point to the origin from the previous simplex
+		DebugRenderer::sInstance->DrawArrow(mOffset, mOffset - ioV, Color::sOrange, 0.05f);
+
+		// Draw the closest points
+		DebugRenderer::sInstance->DrawMarker(mOffset + outPointA, Color::sGreen, 1.0f);
+		DebugRenderer::sInstance->DrawMarker(mOffset + outPointB, Color::sPurple, 1.0f);
+
+		// Draw the simplex and the Minkowski difference around it
+		DrawState();
+#endif
+
+		JPH_ASSERT(ioV.LengthSq() == v_len_sq);
+		return v_len_sq;
+	}
+
+	/// Get the resulting simplex after the GetClosestPoints algorithm finishes.
+	/// If it returned a squared distance of 0, the origin will be contained in the simplex.
+	void		GetClosestPointsSimplex(Vec3 *outY, Vec3 *outP, Vec3 *outQ, uint &outNumPoints) const
+	{
+		uint size = sizeof(Vec3) * mNumPoints;
+		memcpy(outY, mY, size);
+		memcpy(outP, mP, size);
+		memcpy(outQ, mQ, size);
+		outNumPoints = mNumPoints;
+	}
+
+	/// Test if a ray inRayOrigin + lambda * inRayDirection for lambda e [0, ioLambda> intersects inA
+	///
+	/// Code based upon: Ray Casting against General Convex Objects with Application to Continuous Collision Detection - Gino van den Bergen
+	///
+	/// @param inRayOrigin Origin of the ray
+	/// @param inRayDirection Direction of the ray (ioLambda * inDirection determines length)
+	///	@param inTolerance The minimal distance between the ray and A before it is considered colliding
+	/// @param inA A convex object that has the GetSupport(Vec3) function
+	/// @param ioLambda The max fraction along the ray, on output updated with the actual collision fraction.
+	///
+	///	@return true if a hit was found, ioLambda is the solution for lambda.
+	template <typename A>
+	bool		CastRay(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, float inTolerance, const A &inA, float &ioLambda)
+	{
+		float tolerance_sq = Square(inTolerance);
+
+		// Reset state
+		mNumPoints = 0;
+
+		float lambda = 0.0f;
+		Vec3 x = inRayOrigin;
+		Vec3 v = x - inA.GetSupport(Vec3::sZero());
+		float v_len_sq = FLT_MAX;
+		bool allow_restart = false;
+
+		for (;;)
+		{
+#ifdef JPH_GJK_DEBUG
+			Trace("v = [%s], num_points = %d", ConvertToString(v).c_str(), mNumPoints);
+#endif
+
+			// Get new support point
+			Vec3 p = inA.GetSupport(v);
+			Vec3 w = x - p;
+
+#ifdef JPH_GJK_DEBUG
+			Trace("w = [%s]", ConvertToString(w).c_str());
+#endif
+
+			float v_dot_w = v.Dot(w);
+#ifdef JPH_GJK_DEBUG
+			Trace("v . w = %g", (double)v_dot_w);
+#endif
+			if (v_dot_w > 0.0f)
+			{
+				// If ray and normal are in the same direction, we've passed A and there's no collision
+				float v_dot_r = v.Dot(inRayDirection);
+#ifdef JPH_GJK_DEBUG
+				Trace("v . r = %g", (double)v_dot_r);
+#endif
+				if (v_dot_r >= 0.0f)
+					return false;
+
+				// Update the lower bound for lambda
+				float delta = v_dot_w / v_dot_r;
+				float old_lambda = lambda;
+				lambda -= delta;
+#ifdef JPH_GJK_DEBUG
+				Trace("lambda = %g, delta = %g", (double)lambda, (double)delta);
+#endif
+
+				// If lambda didn't change, we cannot converge any further and we assume a hit
+				if (old_lambda == lambda)
+					break;
+
+				// If lambda is bigger or equal than max, we don't have a hit
+				if (lambda >= ioLambda)
+					return false;
+
+				// Update x to new closest point on the ray
+				x = inRayOrigin + lambda * inRayDirection;
+
+				// We've shifted x, so reset v_len_sq so that it is not used as early out for GetClosest
+				v_len_sq = FLT_MAX;
+
+				// We allow rebuilding the simplex once after x changes because the simplex was built
+				// for another x and numerical round off builds up as you keep adding points to an
+				// existing simplex
+				allow_restart = true;
+			}
+
+			// Add p to set P: P = P U {p}
+			mP[mNumPoints] = p;
+			++mNumPoints;
+
+			// Calculate Y = {x} - P
+			for (int i = 0; i < mNumPoints; ++i)
+				mY[i] = x - mP[i];
+
+			// Determine the new closest point from Y to origin
+			uint32 set;						// Set of points that form the new simplex
+			if (!GetClosest<false>(v_len_sq, v, v_len_sq, set))
+			{
+#ifdef JPH_GJK_DEBUG
+				Trace("Failed to converge");
+#endif
+
+				// Only allow 1 restart, if we still can't get a closest point
+				// we're so close that we return this as a hit
+				if (!allow_restart)
+					break;
+
+				// If we fail to converge, we start again with the last point as simplex
+#ifdef JPH_GJK_DEBUG
+				Trace("Restarting");
+#endif
+				allow_restart = false;
+				mP[0] = p;
+				mNumPoints = 1;
+				v = x - p;
+				v_len_sq = FLT_MAX;
+				continue;
+			}
+			else if (set == 0xf)
+			{
+#ifdef JPH_GJK_DEBUG
+				Trace("Full simplex");
+#endif
+
+				// We're inside the tetrahedron, we have a hit (verify that length of v is 0)
+				JPH_ASSERT(v_len_sq == 0.0f);
+				break;
+			}
+
+			// Update the points P to form the new simplex
+			// Note: We're not updating Y as Y will shift with x so we have to calculate it every iteration
+			UpdatePointSetP(set);
+
+			// Check if x is close enough to inA
+			if (v_len_sq <= tolerance_sq)
+			{
+#ifdef JPH_GJK_DEBUG
+				Trace("Converged");
+#endif
+				break;
+			}
+		}
+
+		// Store hit fraction
+		ioLambda = lambda;
+		return true;
+	}
+
+	/// Test if a cast shape inA moving from inStart to lambda * inStart.GetTranslation() + inDirection where lambda e [0, ioLambda> intersects inB
+	///
+	/// @param inStart Start position and orientation of the convex object
+	/// @param inDirection Direction of the sweep (ioLambda * inDirection determines length)
+	///	@param inTolerance The minimal distance between A and B before they are considered colliding
+	/// @param inA The convex object A, must support the GetSupport(Vec3) function.
+	/// @param inB The convex object B, must support the GetSupport(Vec3) function.
+	/// @param ioLambda The max fraction along the sweep, on output updated with the actual collision fraction.
+	///
+	/// @return true if a hit was found, ioLambda is the solution for lambda.
+	template <typename A, typename B>
+	bool		CastShape(Mat44Arg inStart, Vec3Arg inDirection, float inTolerance, const A &inA, const B &inB, float &ioLambda)
+	{
+		// Transform the shape to be cast to the starting position
+		TransformedConvexObject transformed_a(inStart, inA);
+
+		// Calculate the minkowski difference inB - inA
+		// inA is moving, so we need to add the back side of inB to the front side of inA
+		MinkowskiDifference difference(inB, transformed_a);
+
+		// Do a raycast against the Minkowski difference
+		return CastRay(Vec3::sZero(), inDirection, inTolerance, difference, ioLambda);
+	}
+
+	/// Test if a cast shape inA moving from inStart to lambda * inStart.GetTranslation() + inDirection where lambda e [0, ioLambda> intersects inB
+	///
+	/// @param inStart Start position and orientation of the convex object
+	/// @param inDirection Direction of the sweep (ioLambda * inDirection determines length)
+	///	@param inTolerance The minimal distance between A and B before they are considered colliding
+	/// @param inA The convex object A, must support the GetSupport(Vec3) function.
+	/// @param inB The convex object B, must support the GetSupport(Vec3) function.
+	/// @param inConvexRadiusA The convex radius of A, this will be added on all sides to pad A.
+	/// @param inConvexRadiusB The convex radius of B, this will be added on all sides to pad B.
+	/// @param ioLambda The max fraction along the sweep, on output updated with the actual collision fraction.
+	///	@param outPointA is the contact point on A (if outSeparatingAxis is near zero, this may not be not the deepest point)
+	///	@param outPointB is the contact point on B (if outSeparatingAxis is near zero, this may not be not the deepest point)
+	/// @param outSeparatingAxis On return this will contain a vector that points from A to B along the smallest distance of separation.
+	/// The length of this vector indicates the separation of A and B without their convex radius.
+	/// If it is near zero, the direction may not be accurate as the bodies may overlap when lambda = 0.
+	///
+	///	@return true if a hit was found, ioLambda is the solution for lambda and outPoint and outSeparatingAxis are valid.
+	template <typename A, typename B>
+	bool		CastShape(Mat44Arg inStart, Vec3Arg inDirection, float inTolerance, const A &inA, const B &inB, float inConvexRadiusA, float inConvexRadiusB, float &ioLambda, Vec3 &outPointA, Vec3 &outPointB, Vec3 &outSeparatingAxis)
+	{
+		float tolerance_sq = Square(inTolerance);
+
+		// Calculate how close A and B (without their convex radius) need to be to each other in order for us to consider this a collision
+		float sum_convex_radius = inConvexRadiusA + inConvexRadiusB;
+
+		// Transform the shape to be cast to the starting position
+		TransformedConvexObject transformed_a(inStart, inA);
+
+		// Reset state
+		mNumPoints = 0;
+
+		float lambda = 0.0f;
+		Vec3 x = Vec3::sZero(); // Since A is already transformed we can start the cast from zero
+		Vec3 v = -inB.GetSupport(Vec3::sZero()) + transformed_a.GetSupport(Vec3::sZero()); // See CastRay: v = x - inA.GetSupport(Vec3::sZero()) where inA is the Minkowski difference inB - transformed_a (see CastShape above) and x is zero
+		float v_len_sq = FLT_MAX;
+		bool allow_restart = false;
+
+		// Keeps track of separating axis of the previous iteration.
+		// Initialized at zero as we don't know if our first v is actually a separating axis.
+		Vec3 prev_v = Vec3::sZero();
+
+		for (;;)
+		{
+#ifdef JPH_GJK_DEBUG
+			Trace("v = [%s], num_points = %d", ConvertToString(v).c_str(), mNumPoints);
+#endif
+
+			// Calculate the minkowski difference inB - inA
+			// inA is moving, so we need to add the back side of inB to the front side of inA
+			// Keep the support points on A and B separate so that in the end we can calculate a contact point
+			Vec3 p = transformed_a.GetSupport(-v);
+			Vec3 q = inB.GetSupport(v);
+			Vec3 w = x - (q - p);
+
+#ifdef JPH_GJK_DEBUG
+			Trace("w = [%s]", ConvertToString(w).c_str());
+#endif
+
+			// Difference from article to this code:
+			// We did not include the convex radius in p and q in order to be able to calculate a good separating axis at the end of the algorithm.
+			// However when moving forward along inDirection we do need to take this into account so that we keep A and B separated by the sum of their convex radii.
+			// From p we have to subtract: inConvexRadiusA * v / |v|
+			// To q we have to add: inConvexRadiusB * v / |v|
+			// This means that to w we have to add: -(inConvexRadiusA + inConvexRadiusB) * v / |v|
+			// So to v . w we have to add: v . (-(inConvexRadiusA + inConvexRadiusB) * v / |v|) = -(inConvexRadiusA + inConvexRadiusB) * |v|
+			float v_dot_w = v.Dot(w) - sum_convex_radius * v.Length();
+#ifdef JPH_GJK_DEBUG
+			Trace("v . w = %g", (double)v_dot_w);
+#endif
+			if (v_dot_w > 0.0f)
+			{
+				// If ray and normal are in the same direction, we've passed A and there's no collision
+				float v_dot_r = v.Dot(inDirection);
+#ifdef JPH_GJK_DEBUG
+				Trace("v . r = %g", (double)v_dot_r);
+#endif
+				if (v_dot_r >= 0.0f)
+					return false;
+
+				// Update the lower bound for lambda
+				float delta = v_dot_w / v_dot_r;
+				float old_lambda = lambda;
+				lambda -= delta;
+#ifdef JPH_GJK_DEBUG
+				Trace("lambda = %g, delta = %g", (double)lambda, (double)delta);
+#endif
+
+				// If lambda didn't change, we cannot converge any further and we assume a hit
+				if (old_lambda == lambda)
+					break;
+
+				// If lambda is bigger or equal than max, we don't have a hit
+				if (lambda >= ioLambda)
+					return false;
+
+				// Update x to new closest point on the ray
+				x = lambda * inDirection;
+
+				// We've shifted x, so reset v_len_sq so that it is not used as early out when GetClosest returns false
+				v_len_sq = FLT_MAX;
+
+				// Now that we've moved, we know that A and B are not intersecting at lambda = 0, so we can update our tolerance to stop iterating
+				// as soon as A and B are inConvexRadiusA + inConvexRadiusB apart
+				tolerance_sq = Square(inTolerance + sum_convex_radius);
+
+				// We allow rebuilding the simplex once after x changes because the simplex was built
+				// for another x and numerical round off builds up as you keep adding points to an
+				// existing simplex
+				allow_restart = true;
+			}
+
+			// Add p to set P, q to set Q: P = P U {p}, Q = Q U {q}
+			mP[mNumPoints] = p;
+			mQ[mNumPoints] = q;
+			++mNumPoints;
+
+			// Calculate Y = {x} - (Q - P)
+			for (int i = 0; i < mNumPoints; ++i)
+				mY[i] = x - (mQ[i] - mP[i]);
+
+			// Determine the new closest point from Y to origin
+			uint32 set;						// Set of points that form the new simplex
+			if (!GetClosest<false>(v_len_sq, v, v_len_sq, set))
+			{
+#ifdef JPH_GJK_DEBUG
+				Trace("Failed to converge");
+#endif
+
+				// Only allow 1 restart, if we still can't get a closest point
+				// we're so close that we return this as a hit
+				if (!allow_restart)
+					break;
+
+				// If we fail to converge, we start again with the last point as simplex
+#ifdef JPH_GJK_DEBUG
+				Trace("Restarting");
+#endif
+				allow_restart = false;
+				mP[0] = p;
+				mQ[0] = q;
+				mNumPoints = 1;
+				v = x - q;
+				v_len_sq = FLT_MAX;
+				continue;
+			}
+			else if (set == 0xf)
+			{
+#ifdef JPH_GJK_DEBUG
+				Trace("Full simplex");
+#endif
+
+				// We're inside the tetrahedron, we have a hit (verify that length of v is 0)
+				JPH_ASSERT(v_len_sq == 0.0f);
+				break;
+			}
+
+			// Update the points P and Q to form the new simplex
+			// Note: We're not updating Y as Y will shift with x so we have to calculate it every iteration
+			UpdatePointSetPQ(set);
+
+			// Check if A and B are touching according to our tolerance
+			if (v_len_sq <= tolerance_sq)
+			{
+#ifdef JPH_GJK_DEBUG
+				Trace("Converged");
+#endif
+				break;
+			}
+
+			// Store our v to return as separating axis
+			prev_v = v;
+		}
+
+		// Calculate Y = {x} - (Q - P) again so we can calculate the contact points
+		for (int i = 0; i < mNumPoints; ++i)
+			mY[i] = x - (mQ[i] - mP[i]);
+
+		// Calculate the offset we need to apply to A and B to correct for the convex radius
+		Vec3 normalized_v = v.NormalizedOr(Vec3::sZero());
+		Vec3 convex_radius_a = inConvexRadiusA * normalized_v;
+		Vec3 convex_radius_b = inConvexRadiusB * normalized_v;
+
+		// Get the contact point
+		// Note that A and B will coincide when lambda > 0. In this case we calculate only B as it is more accurate as it contains less terms.
+		switch (mNumPoints)
+		{
+		case 1:
+			outPointB = mQ[0] + convex_radius_b;
+			outPointA = lambda > 0.0f? outPointB : mP[0] - convex_radius_a;
+			break;
+
+		case 2:
+			{
+				float bu, bv;
+				ClosestPoint::GetBaryCentricCoordinates(mY[0], mY[1], bu, bv);
+				outPointB = bu * mQ[0] + bv * mQ[1] + convex_radius_b;
+				outPointA = lambda > 0.0f? outPointB : bu * mP[0] + bv * mP[1] - convex_radius_a;
+			}
+			break;
+
+		case 3:
+		case 4: // A full simplex, we can't properly determine a contact point! As contact point we take the closest point of the previous iteration.
+			{
+				float bu, bv, bw;
+				ClosestPoint::GetBaryCentricCoordinates(mY[0], mY[1], mY[2], bu, bv, bw);
+				outPointB = bu * mQ[0] + bv * mQ[1] + bw * mQ[2] + convex_radius_b;
+				outPointA = lambda > 0.0f? outPointB : bu * mP[0] + bv * mP[1] + bw * mP[2] - convex_radius_a;
+			}
+			break;
+		}
+
+		// Store separating axis, in case we have a convex radius we can just return v,
+		// otherwise v will be very small and we resort to returning previous v as an approximation.
+		outSeparatingAxis = sum_convex_radius > 0.0f? -v : -prev_v;
+
+		// Store hit fraction
+		ioLambda = lambda;
+		return true;
+	}
+
+private:
+#ifdef JPH_GJK_DEBUG
+	/// Draw state of algorithm
+	void		DrawState()
+	{
+		RMat44 origin = RMat44::sTranslation(mOffset);
+
+		// Draw origin
+		DebugRenderer::sInstance->DrawCoordinateSystem(origin, 1.0f);
+
+		// Draw the hull
+		DebugRenderer::sInstance->DrawGeometry(origin, mGeometry->mBounds.Transformed(origin), mGeometry->mBounds.GetExtent().LengthSq(), Color::sYellow, mGeometry);
+
+		// Draw Y
+		for (int i = 0; i < mNumPoints; ++i)
+		{
+			// Draw support point
+			RVec3 y_i = origin * mY[i];
+			DebugRenderer::sInstance->DrawMarker(y_i, Color::sRed, 1.0f);
+			for (int j = i + 1; j < mNumPoints; ++j)
+			{
+				// Draw edge
+				RVec3 y_j = origin * mY[j];
+				DebugRenderer::sInstance->DrawLine(y_i, y_j, Color::sRed);
+				for (int k = j + 1; k < mNumPoints; ++k)
+				{
+					// Make sure triangle faces the origin
+					RVec3 y_k = origin * mY[k];
+					RVec3 center = (y_i + y_j + y_k) / Real(3);
+					RVec3 normal = (y_j - y_i).Cross(y_k - y_i);
+					if (normal.Dot(center) < Real(0))
+						DebugRenderer::sInstance->DrawTriangle(y_i, y_j, y_k, Color::sLightGrey);
+					else
+						DebugRenderer::sInstance->DrawTriangle(y_i, y_k, y_j, Color::sLightGrey);
+				}
+			}
+		}
+
+		// Offset to the right
+		mOffset += Vec3(mGeometry->mBounds.GetSize().GetX() + 2.0f, 0, 0);
+	}
+#endif // JPH_GJK_DEBUG
+
+	Vec3		mY[4];						///< Support points on A - B
+	Vec3		mP[4];						///< Support point on A
+	Vec3		mQ[4];						///< Support point on B
+	int			mNumPoints = 0;				///< Number of points in mY, mP and mQ that are valid
+
+#ifdef JPH_GJK_DEBUG
+	DebugRenderer::GeometryRef	mGeometry;	///< A visualization of the minkowski difference for state drawing
+	RVec3		mOffset = RVec3::sZero();	///< Offset to use for state drawing
+#endif
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Geometry/IndexedTriangle.h b/thirdparty/jolt_physics/Jolt/Geometry/IndexedTriangle.h
new file mode 100644
index 0000000000..7ebffc29c5
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Geometry/IndexedTriangle.h
@@ -0,0 +1,116 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/HashCombine.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Triangle with 32-bit indices
+class IndexedTriangleNoMaterial
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+					IndexedTriangleNoMaterial() = default;
+	constexpr		IndexedTriangleNoMaterial(uint32 inI1, uint32 inI2, uint32 inI3) : mIdx { inI1, inI2, inI3 } { }
+
+	/// Check if two triangles are identical
+	bool			operator == (const IndexedTriangleNoMaterial &inRHS) const
+	{
+		return mIdx[0] == inRHS.mIdx[0] && mIdx[1] == inRHS.mIdx[1] && mIdx[2] == inRHS.mIdx[2];
+	}
+
+	/// Check if two triangles are equivalent (using the same vertices)
+	bool			IsEquivalent(const IndexedTriangleNoMaterial &inRHS) const
+	{
+		return (mIdx[0] == inRHS.mIdx[0] && mIdx[1] == inRHS.mIdx[1] && mIdx[2] == inRHS.mIdx[2])
+			|| (mIdx[0] == inRHS.mIdx[1] && mIdx[1] == inRHS.mIdx[2] && mIdx[2] == inRHS.mIdx[0])
+			|| (mIdx[0] == inRHS.mIdx[2] && mIdx[1] == inRHS.mIdx[0] && mIdx[2] == inRHS.mIdx[1]);
+	}
+
+	/// Check if two triangles are opposite (using the same vertices but in opposing order)
+	bool			IsOpposite(const IndexedTriangleNoMaterial &inRHS) const
+	{
+		return (mIdx[0] == inRHS.mIdx[0] && mIdx[1] == inRHS.mIdx[2] && mIdx[2] == inRHS.mIdx[1])
+			|| (mIdx[0] == inRHS.mIdx[1] && mIdx[1] == inRHS.mIdx[0] && mIdx[2] == inRHS.mIdx[2])
+			|| (mIdx[0] == inRHS.mIdx[2] && mIdx[1] == inRHS.mIdx[1] && mIdx[2] == inRHS.mIdx[0]);
+	}
+
+	/// Check if triangle is degenerate
+	bool			IsDegenerate(const VertexList &inVertices) const
+	{
+		Vec3 v0(inVertices[mIdx[0]]);
+		Vec3 v1(inVertices[mIdx[1]]);
+		Vec3 v2(inVertices[mIdx[2]]);
+
+		return (v1 - v0).Cross(v2 - v0).IsNearZero();
+	}
+
+	/// Rotate the vertices so that the second vertex becomes first etc. This does not change the represented triangle.
+	void			Rotate()
+	{
+		uint32 tmp = mIdx[0];
+		mIdx[0] = mIdx[1];
+		mIdx[1] = mIdx[2];
+		mIdx[2] = tmp;
+	}
+
+	/// Get center of triangle
+	Vec3			GetCentroid(const VertexList &inVertices) const
+	{
+		return (Vec3(inVertices[mIdx[0]]) + Vec3(inVertices[mIdx[1]]) + Vec3(inVertices[mIdx[2]])) / 3.0f;
+	}
+
+	uint32			mIdx[3];
+};
+
+/// Triangle with 32-bit indices and material index
+class IndexedTriangle : public IndexedTriangleNoMaterial
+{
+public:
+	using IndexedTriangleNoMaterial::IndexedTriangleNoMaterial;
+
+	/// Constructor
+	constexpr		IndexedTriangle(uint32 inI1, uint32 inI2, uint32 inI3, uint32 inMaterialIndex, uint inUserData = 0) : IndexedTriangleNoMaterial(inI1, inI2, inI3), mMaterialIndex(inMaterialIndex), mUserData(inUserData) { }
+
+	/// Check if two triangles are identical
+	bool			operator == (const IndexedTriangle &inRHS) const
+	{
+		return mMaterialIndex == inRHS.mMaterialIndex && mUserData == inRHS.mUserData && IndexedTriangleNoMaterial::operator==(inRHS);
+	}
+
+	/// Rotate the vertices so that the lowest vertex becomes the first. This does not change the represented triangle.
+	IndexedTriangle	GetLowestIndexFirst() const
+	{
+		if (mIdx[0] < mIdx[1])
+		{
+			if (mIdx[0] < mIdx[2])
+				return IndexedTriangle(mIdx[0], mIdx[1], mIdx[2], mMaterialIndex, mUserData); // 0 is smallest
+			else
+				return IndexedTriangle(mIdx[2], mIdx[0], mIdx[1], mMaterialIndex, mUserData); // 2 is smallest
+		}
+		else
+		{
+			if (mIdx[1] < mIdx[2])
+				return IndexedTriangle(mIdx[1], mIdx[2], mIdx[0], mMaterialIndex, mUserData); // 1 is smallest
+			else
+				return IndexedTriangle(mIdx[2], mIdx[0], mIdx[1], mMaterialIndex, mUserData); // 2 is smallest
+		}
+	}
+
+	uint32			mMaterialIndex = 0;
+	uint32			mUserData = 0;				///< User data that can be used for anything by the application, e.g. for tracking the original index of the triangle
+};
+
+using IndexedTriangleNoMaterialList = Array<IndexedTriangleNoMaterial>;
+using IndexedTriangleList = Array<IndexedTriangle>;
+
+JPH_NAMESPACE_END
+
+// Create a std::hash/JPH::Hash for IndexedTriangleNoMaterial and IndexedTriangle
+JPH_MAKE_HASHABLE(JPH::IndexedTriangleNoMaterial, t.mIdx[0], t.mIdx[1], t.mIdx[2])
+JPH_MAKE_HASHABLE(JPH::IndexedTriangle, t.mIdx[0], t.mIdx[1], t.mIdx[2], t.mMaterialIndex, t.mUserData)
diff --git a/thirdparty/jolt_physics/Jolt/Geometry/Indexify.cpp b/thirdparty/jolt_physics/Jolt/Geometry/Indexify.cpp
new file mode 100644
index 0000000000..019dbeeda4
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Geometry/Indexify.cpp
@@ -0,0 +1,222 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Geometry/Indexify.h>
+#include <Jolt/Geometry/AABox.h>
+
+JPH_NAMESPACE_BEGIN
+
+static JPH_INLINE const Float3 &sIndexifyGetFloat3(const TriangleList &inTriangles, uint32 inVertexIndex)
+{
+	return inTriangles[inVertexIndex / 3].mV[inVertexIndex % 3];
+}
+
+static JPH_INLINE Vec3 sIndexifyGetVec3(const TriangleList &inTriangles, uint32 inVertexIndex)
+{
+	return Vec3::sLoadFloat3Unsafe(sIndexifyGetFloat3(inTriangles, inVertexIndex));
+}
+
+static void sIndexifyVerticesBruteForce(const TriangleList &inTriangles, const uint32 *inVertexIndices, const uint32 *inVertexIndicesEnd, Array<uint32> &ioWeldedVertices, float inVertexWeldDistance)
+{
+	float weld_dist_sq = Square(inVertexWeldDistance);
+
+	// Compare every vertex
+	for (const uint32 *v1_idx = inVertexIndices; v1_idx < inVertexIndicesEnd; ++v1_idx)
+	{
+		Vec3 v1 = sIndexifyGetVec3(inTriangles, *v1_idx);
+
+		// with every other vertex...
+		for (const uint32 *v2_idx = v1_idx + 1; v2_idx < inVertexIndicesEnd; ++v2_idx)
+		{
+			Vec3 v2 = sIndexifyGetVec3(inTriangles, *v2_idx);
+
+			// If they're weldable
+			if ((v2 - v1).LengthSq() <= weld_dist_sq)
+			{
+				// Find the lowest indices both indices link to
+				uint32 idx1 = *v1_idx;
+				for (;;)
+				{
+					uint32 new_idx1 = ioWeldedVertices[idx1];
+					if (new_idx1 >= idx1)
+						break;
+					idx1 = new_idx1;
+				}
+				uint32 idx2 = *v2_idx;
+				for (;;)
+				{
+					uint32 new_idx2 = ioWeldedVertices[idx2];
+					if (new_idx2 >= idx2)
+						break;
+					idx2 = new_idx2;
+				}
+
+				// Order the vertices
+				uint32 lowest = min(idx1, idx2);
+				uint32 highest = max(idx1, idx2);
+
+				// Link highest to lowest
+				ioWeldedVertices[highest] = lowest;
+
+				// Also update the vertices we started from to avoid creating long chains
+				ioWeldedVertices[*v1_idx] = lowest;
+				ioWeldedVertices[*v2_idx] = lowest;
+				break;
+			}
+		}
+	}
+}
+
+static void sIndexifyVerticesRecursively(const TriangleList &inTriangles, uint32 *ioVertexIndices, uint inNumVertices, uint32 *ioScratch, Array<uint32> &ioWeldedVertices, float inVertexWeldDistance, uint inMaxRecursion)
+{
+	// Check if we have few enough vertices to do a brute force search
+	// Or if we've recursed too deep (this means we chipped off a few vertices each iteration because all points are very close)
+	if (inNumVertices <= 8 || inMaxRecursion == 0)
+	{
+		sIndexifyVerticesBruteForce(inTriangles, ioVertexIndices, ioVertexIndices + inNumVertices, ioWeldedVertices, inVertexWeldDistance);
+		return;
+	}
+
+	// Calculate bounds
+	AABox bounds;
+	for (const uint32 *v = ioVertexIndices, *v_end = ioVertexIndices + inNumVertices; v < v_end; ++v)
+		bounds.Encapsulate(sIndexifyGetVec3(inTriangles, *v));
+
+	// Determine split plane
+	int split_axis = bounds.GetExtent().GetHighestComponentIndex();
+	float split_value = bounds.GetCenter()[split_axis];
+
+	// Partition vertices
+	uint32 *v_read = ioVertexIndices, *v_write = ioVertexIndices, *v_end = ioVertexIndices + inNumVertices;
+	uint32 *scratch = ioScratch;
+	while (v_read < v_end)
+	{
+		// Calculate distance to plane
+		float distance_to_split_plane = sIndexifyGetFloat3(inTriangles, *v_read)[split_axis] - split_value;
+		if (distance_to_split_plane < -inVertexWeldDistance)
+		{
+			// Vertex is on the right side
+			*v_write = *v_read;
+			++v_read;
+			++v_write;
+		}
+		else if (distance_to_split_plane > inVertexWeldDistance)
+		{
+			// Vertex is on the wrong side, swap with the last vertex
+			--v_end;
+			std::swap(*v_read, *v_end);
+		}
+		else
+		{
+			// Vertex is too close to the split plane, it goes on both sides
+			*scratch++ = *v_read++;
+		}
+	}
+
+	// Check if we made any progress
+	uint num_vertices_on_both_sides = (uint)(scratch - ioScratch);
+	if (num_vertices_on_both_sides == inNumVertices)
+	{
+		sIndexifyVerticesBruteForce(inTriangles, ioVertexIndices, ioVertexIndices + inNumVertices, ioWeldedVertices, inVertexWeldDistance);
+		return;
+	}
+
+	// Calculate how we classified the vertices
+	uint num_vertices_left = (uint)(v_write - ioVertexIndices);
+	uint num_vertices_right = (uint)(ioVertexIndices + inNumVertices - v_end);
+	JPH_ASSERT(num_vertices_left + num_vertices_right + num_vertices_on_both_sides == inNumVertices);
+	memcpy(v_write, ioScratch, num_vertices_on_both_sides * sizeof(uint32));
+
+	// Recurse
+	uint max_recursion = inMaxRecursion - 1;
+	sIndexifyVerticesRecursively(inTriangles, ioVertexIndices, num_vertices_left + num_vertices_on_both_sides, ioScratch, ioWeldedVertices, inVertexWeldDistance, max_recursion);
+	sIndexifyVerticesRecursively(inTriangles, ioVertexIndices + num_vertices_left, num_vertices_right + num_vertices_on_both_sides, ioScratch, ioWeldedVertices, inVertexWeldDistance, max_recursion);
+}
+
+void Indexify(const TriangleList &inTriangles, VertexList &outVertices, IndexedTriangleList &outTriangles, float inVertexWeldDistance)
+{
+	uint num_triangles = (uint)inTriangles.size();
+	uint num_vertices = num_triangles * 3;
+
+	// Create a list of all vertex indices
+	Array<uint32> vertex_indices;
+	vertex_indices.resize(num_vertices);
+	for (uint i = 0; i < num_vertices; ++i)
+		vertex_indices[i] = i;
+
+	// Link each vertex to itself
+	Array<uint32> welded_vertices;
+	welded_vertices.resize(num_vertices);
+	for (uint i = 0; i < num_vertices; ++i)
+		welded_vertices[i] = i;
+
+	// A scope to free memory used by the scratch array
+	{
+		// Some scratch memory, used for the vertices that fall in both partitions
+		Array<uint32> scratch;
+		scratch.resize(num_vertices);
+
+		// Recursively split the vertices
+		sIndexifyVerticesRecursively(inTriangles, vertex_indices.data(), num_vertices, scratch.data(), welded_vertices, inVertexWeldDistance, 32);
+	}
+
+	// Do a pass to complete the welding, linking each vertex to the vertex it is welded to
+	// (and since we're going from 0 to N we can be sure that the vertex we're linking to is already linked to the lowest vertex)
+	uint num_resulting_vertices = 0;
+	for (uint i = 0; i < num_vertices; ++i)
+	{
+		JPH_ASSERT(welded_vertices[welded_vertices[i]] <= welded_vertices[i]);
+		welded_vertices[i] = welded_vertices[welded_vertices[i]];
+		if (welded_vertices[i] == i)
+			++num_resulting_vertices;
+	}
+
+	// Collect the vertices
+	outVertices.clear();
+	outVertices.reserve(num_resulting_vertices);
+	for (uint i = 0; i < num_vertices; ++i)
+		if (welded_vertices[i] == i)
+		{
+			// New vertex
+			welded_vertices[i] = (uint32)outVertices.size();
+			outVertices.push_back(sIndexifyGetFloat3(inTriangles, i));
+		}
+		else
+		{
+			// Reused vertex, remap index
+			welded_vertices[i] = welded_vertices[welded_vertices[i]];
+		}
+
+	// Create indexed triangles
+	outTriangles.clear();
+	outTriangles.reserve(num_triangles);
+	for (uint t = 0; t < num_triangles; ++t)
+	{
+		IndexedTriangle it;
+		it.mMaterialIndex = inTriangles[t].mMaterialIndex;
+		it.mUserData = inTriangles[t].mUserData;
+		for (int v = 0; v < 3; ++v)
+			it.mIdx[v] = welded_vertices[t * 3 + v];
+		if (!it.IsDegenerate(outVertices))
+			outTriangles.push_back(it);
+	}
+}
+
+void Deindexify(const VertexList &inVertices, const IndexedTriangleList &inTriangles, TriangleList &outTriangles)
+{
+	outTriangles.resize(inTriangles.size());
+	for (size_t t = 0; t < inTriangles.size(); ++t)
+	{
+		const IndexedTriangle &in = inTriangles[t];
+		Triangle &out = outTriangles[t];
+		out.mMaterialIndex = in.mMaterialIndex;
+		out.mUserData = in.mUserData;
+		for (int v = 0; v < 3; ++v)
+			out.mV[v] = inVertices[in.mIdx[v]];
+	}
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Geometry/Indexify.h b/thirdparty/jolt_physics/Jolt/Geometry/Indexify.h
new file mode 100644
index 0000000000..01fb805305
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Geometry/Indexify.h
@@ -0,0 +1,19 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/Triangle.h>
+#include <Jolt/Geometry/IndexedTriangle.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Take a list of triangles and get the unique set of vertices and use them to create indexed triangles.
+/// Vertices that are less than inVertexWeldDistance apart will be combined to a single vertex.
+JPH_EXPORT void Indexify(const TriangleList &inTriangles, VertexList &outVertices, IndexedTriangleList &outTriangles, float inVertexWeldDistance = 1.0e-4f);
+
+/// Take a list of indexed triangles and unpack them
+JPH_EXPORT void Deindexify(const VertexList &inVertices, const IndexedTriangleList &inTriangles, TriangleList &outTriangles);
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Geometry/MortonCode.h b/thirdparty/jolt_physics/Jolt/Geometry/MortonCode.h
new file mode 100644
index 0000000000..e750d7e6d1
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Geometry/MortonCode.h
@@ -0,0 +1,40 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/AABox.h>
+
+JPH_NAMESPACE_BEGIN
+
+class MortonCode
+{
+public:
+	/// First converts a floating point value in the range [0, 1] to a 10 bit fixed point integer.
+	/// Then expands a 10-bit integer into 30 bits by inserting 2 zeros after each bit.
+	static uint32 sExpandBits(float inV)
+	{
+		JPH_ASSERT(inV >= 0.0f && inV <= 1.0f);
+		uint32 v = uint32(inV * 1023.0f + 0.5f);
+		JPH_ASSERT(v < 1024);
+		v = (v * 0x00010001u) & 0xFF0000FFu;
+		v = (v * 0x00000101u) & 0x0F00F00Fu;
+		v = (v * 0x00000011u) & 0xC30C30C3u;
+		v = (v * 0x00000005u) & 0x49249249u;
+		return v;
+	}
+
+	/// Calculate the morton code for inVector, given that all vectors lie in inVectorBounds
+	static uint32 sGetMortonCode(Vec3Arg inVector, const AABox &inVectorBounds)
+	{
+		// Convert to 10 bit fixed point
+		Vec3 scaled = (inVector - inVectorBounds.mMin) / inVectorBounds.GetSize();
+		uint x = sExpandBits(scaled.GetX());
+		uint y = sExpandBits(scaled.GetY());
+		uint z = sExpandBits(scaled.GetZ());
+		return (x << 2) + (y << 1) + z;
+	}
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Geometry/OrientedBox.cpp b/thirdparty/jolt_physics/Jolt/Geometry/OrientedBox.cpp
new file mode 100644
index 0000000000..31c38c877e
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Geometry/OrientedBox.cpp
@@ -0,0 +1,178 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Geometry/OrientedBox.h>
+#include <Jolt/Geometry/AABox.h>
+
+JPH_NAMESPACE_BEGIN
+
+bool OrientedBox::Overlaps(const AABox &inBox, float inEpsilon) const
+{
+	// Taken from: Real Time Collision Detection - Christer Ericson
+	// Chapter 4.4.1, page 103-105.
+	// Note that the code is swapped around: A is the aabox and B is the oriented box (this saves us from having to invert the orientation of the oriented box)
+
+	// Convert AABox to center / extent representation
+	Vec3 a_center = inBox.GetCenter();
+	Vec3 a_half_extents = inBox.GetExtent();
+
+	// Compute rotation matrix expressing b in a's coordinate frame
+	Mat44 rot(mOrientation.GetColumn4(0), mOrientation.GetColumn4(1), mOrientation.GetColumn4(2), mOrientation.GetColumn4(3) - Vec4(a_center, 0));
+
+	// Compute common subexpressions. Add in an epsilon term to
+	// counteract arithmetic errors when two edges are parallel and
+	// their cross product is (near) null (see text for details)
+	Vec3 epsilon = Vec3::sReplicate(inEpsilon);
+	Vec3 abs_r[3] { rot.GetAxisX().Abs() + epsilon, rot.GetAxisY().Abs() + epsilon, rot.GetAxisZ().Abs() + epsilon };
+
+	// Test axes L = A0, L = A1, L = A2
+	float ra, rb;
+	for (int i = 0; i < 3; i++)
+	{
+		ra = a_half_extents[i];
+		rb = mHalfExtents[0] * abs_r[0][i] + mHalfExtents[1] * abs_r[1][i] + mHalfExtents[2] * abs_r[2][i];
+		if (abs(rot(i, 3)) > ra + rb) return false;
+	}
+
+	// Test axes L = B0, L = B1, L = B2
+	for (int i = 0; i < 3; i++)
+	{
+		ra = a_half_extents.Dot(abs_r[i]);
+		rb = mHalfExtents[i];
+		if (abs(rot.GetTranslation().Dot(rot.GetColumn3(i))) > ra + rb) return false;
+	}
+
+	// Test axis L = A0 x B0
+	ra = a_half_extents[1] * abs_r[0][2] + a_half_extents[2] * abs_r[0][1];
+	rb = mHalfExtents[1] * abs_r[2][0] + mHalfExtents[2] * abs_r[1][0];
+	if (abs(rot(2, 3) * rot(1, 0) - rot(1, 3) * rot(2, 0)) > ra + rb) return false;
+
+	// Test axis L = A0 x B1
+	ra = a_half_extents[1] * abs_r[1][2] + a_half_extents[2] * abs_r[1][1];
+	rb = mHalfExtents[0] * abs_r[2][0] + mHalfExtents[2] * abs_r[0][0];
+	if (abs(rot(2, 3) * rot(1, 1) - rot(1, 3) * rot(2, 1)) > ra + rb) return false;
+
+	// Test axis L = A0 x B2
+	ra = a_half_extents[1] * abs_r[2][2] + a_half_extents[2] * abs_r[2][1];
+	rb = mHalfExtents[0] * abs_r[1][0] + mHalfExtents[1] * abs_r[0][0];
+	if (abs(rot(2, 3) * rot(1, 2) - rot(1, 3) * rot(2, 2)) > ra + rb) return false;
+
+	// Test axis L = A1 x B0
+	ra = a_half_extents[0] * abs_r[0][2] + a_half_extents[2] * abs_r[0][0];
+	rb = mHalfExtents[1] * abs_r[2][1] + mHalfExtents[2] * abs_r[1][1];
+	if (abs(rot(0, 3) * rot(2, 0) - rot(2, 3) * rot(0, 0)) > ra + rb) return false;
+
+	// Test axis L = A1 x B1
+	ra = a_half_extents[0] * abs_r[1][2] + a_half_extents[2] * abs_r[1][0];
+	rb = mHalfExtents[0] * abs_r[2][1] + mHalfExtents[2] * abs_r[0][1];
+	if (abs(rot(0, 3) * rot(2, 1) - rot(2, 3) * rot(0, 1)) > ra + rb) return false;
+
+	// Test axis L = A1 x B2
+	ra = a_half_extents[0] * abs_r[2][2] + a_half_extents[2] * abs_r[2][0];
+	rb = mHalfExtents[0] * abs_r[1][1] + mHalfExtents[1] * abs_r[0][1];
+	if (abs(rot(0, 3) * rot(2, 2) - rot(2, 3) * rot(0, 2)) > ra + rb) return false;
+
+	// Test axis L = A2 x B0
+	ra = a_half_extents[0] * abs_r[0][1] + a_half_extents[1] * abs_r[0][0];
+	rb = mHalfExtents[1] * abs_r[2][2] + mHalfExtents[2] * abs_r[1][2];
+	if (abs(rot(1, 3) * rot(0, 0) - rot(0, 3) * rot(1, 0)) > ra + rb) return false;
+
+	// Test axis L = A2 x B1
+	ra = a_half_extents[0] * abs_r[1][1] + a_half_extents[1] * abs_r[1][0];
+	rb = mHalfExtents[0] * abs_r[2][2] + mHalfExtents[2] * abs_r[0][2];
+	if (abs(rot(1, 3) * rot(0, 1) - rot(0, 3) * rot(1, 1)) > ra + rb) return false;
+
+	// Test axis L = A2 x B2
+	ra = a_half_extents[0] * abs_r[2][1] + a_half_extents[1] * abs_r[2][0];
+	rb = mHalfExtents[0] * abs_r[1][2] + mHalfExtents[1] * abs_r[0][2];
+	if (abs(rot(1, 3) * rot(0, 2) - rot(0, 3) * rot(1, 2)) > ra + rb) return false;
+
+	// Since no separating axis is found, the OBB and AAB must be intersecting
+	return true;
+}
+
+bool OrientedBox::Overlaps(const OrientedBox &inBox, float inEpsilon) const
+{
+	// Taken from: Real Time Collision Detection - Christer Ericson
+	// Chapter 4.4.1, page 103-105.
+	// Note that A is this, B is inBox
+
+	// Compute rotation matrix expressing b in a's coordinate frame
+	Mat44 rot = mOrientation.InversedRotationTranslation() * inBox.mOrientation;
+
+	// Compute common subexpressions. Add in an epsilon term to
+	// counteract arithmetic errors when two edges are parallel and
+	// their cross product is (near) null (see text for details)
+	Vec3 epsilon = Vec3::sReplicate(inEpsilon);
+	Vec3 abs_r[3] { rot.GetAxisX().Abs() + epsilon, rot.GetAxisY().Abs() + epsilon, rot.GetAxisZ().Abs() + epsilon };
+
+	// Test axes L = A0, L = A1, L = A2
+	float ra, rb;
+	for (int i = 0; i < 3; i++)
+	{
+		ra = mHalfExtents[i];
+		rb = inBox.mHalfExtents[0] * abs_r[0][i] + inBox.mHalfExtents[1] * abs_r[1][i] + inBox.mHalfExtents[2] * abs_r[2][i];
+		if (abs(rot(i, 3)) > ra + rb) return false;
+	}
+
+	// Test axes L = B0, L = B1, L = B2
+	for (int i = 0; i < 3; i++)
+	{
+		ra = mHalfExtents.Dot(abs_r[i]);
+		rb = inBox.mHalfExtents[i];
+		if (abs(rot.GetTranslation().Dot(rot.GetColumn3(i))) > ra + rb) return false;
+	}
+
+	// Test axis L = A0 x B0
+	ra = mHalfExtents[1] * abs_r[0][2] + mHalfExtents[2] * abs_r[0][1];
+	rb = inBox.mHalfExtents[1] * abs_r[2][0] + inBox.mHalfExtents[2] * abs_r[1][0];
+	if (abs(rot(2, 3) * rot(1, 0) - rot(1, 3) * rot(2, 0)) > ra + rb) return false;
+
+	// Test axis L = A0 x B1
+	ra = mHalfExtents[1] * abs_r[1][2] + mHalfExtents[2] * abs_r[1][1];
+	rb = inBox.mHalfExtents[0] * abs_r[2][0] + inBox.mHalfExtents[2] * abs_r[0][0];
+	if (abs(rot(2, 3) * rot(1, 1) - rot(1, 3) * rot(2, 1)) > ra + rb) return false;
+
+	// Test axis L = A0 x B2
+	ra = mHalfExtents[1] * abs_r[2][2] + mHalfExtents[2] * abs_r[2][1];
+	rb = inBox.mHalfExtents[0] * abs_r[1][0] + inBox.mHalfExtents[1] * abs_r[0][0];
+	if (abs(rot(2, 3) * rot(1, 2) - rot(1, 3) * rot(2, 2)) > ra + rb) return false;
+
+	// Test axis L = A1 x B0
+	ra = mHalfExtents[0] * abs_r[0][2] + mHalfExtents[2] * abs_r[0][0];
+	rb = inBox.mHalfExtents[1] * abs_r[2][1] + inBox.mHalfExtents[2] * abs_r[1][1];
+	if (abs(rot(0, 3) * rot(2, 0) - rot(2, 3) * rot(0, 0)) > ra + rb) return false;
+
+	// Test axis L = A1 x B1
+	ra = mHalfExtents[0] * abs_r[1][2] + mHalfExtents[2] * abs_r[1][0];
+	rb = inBox.mHalfExtents[0] * abs_r[2][1] + inBox.mHalfExtents[2] * abs_r[0][1];
+	if (abs(rot(0, 3) * rot(2, 1) - rot(2, 3) * rot(0, 1)) > ra + rb) return false;
+
+	// Test axis L = A1 x B2
+	ra = mHalfExtents[0] * abs_r[2][2] + mHalfExtents[2] * abs_r[2][0];
+	rb = inBox.mHalfExtents[0] * abs_r[1][1] + inBox.mHalfExtents[1] * abs_r[0][1];
+	if (abs(rot(0, 3) * rot(2, 2) - rot(2, 3) * rot(0, 2)) > ra + rb) return false;
+
+	// Test axis L = A2 x B0
+	ra = mHalfExtents[0] * abs_r[0][1] + mHalfExtents[1] * abs_r[0][0];
+	rb = inBox.mHalfExtents[1] * abs_r[2][2] + inBox.mHalfExtents[2] * abs_r[1][2];
+	if (abs(rot(1, 3) * rot(0, 0) - rot(0, 3) * rot(1, 0)) > ra + rb) return false;
+
+	// Test axis L = A2 x B1
+	ra = mHalfExtents[0] * abs_r[1][1] + mHalfExtents[1] * abs_r[1][0];
+	rb = inBox.mHalfExtents[0] * abs_r[2][2] + inBox.mHalfExtents[2] * abs_r[0][2];
+	if (abs(rot(1, 3) * rot(0, 1) - rot(0, 3) * rot(1, 1)) > ra + rb) return false;
+
+	// Test axis L = A2 x B2
+	ra = mHalfExtents[0] * abs_r[2][1] + mHalfExtents[1] * abs_r[2][0];
+	rb = inBox.mHalfExtents[0] * abs_r[1][2] + inBox.mHalfExtents[1] * abs_r[0][2];
+	if (abs(rot(1, 3) * rot(0, 2) - rot(0, 3) * rot(1, 2)) > ra + rb) return false;
+
+	// Since no separating axis is found, the OBBs must be intersecting
+	return true;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Geometry/OrientedBox.h b/thirdparty/jolt_physics/Jolt/Geometry/OrientedBox.h
new file mode 100644
index 0000000000..c5c2a0e16c
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Geometry/OrientedBox.h
@@ -0,0 +1,39 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/Triangle.h>
+#include <Jolt/Geometry/IndexedTriangle.h>
+#include <Jolt/Geometry/AABox.h>
+#include <Jolt/Math/Mat44.h>
+
+JPH_NAMESPACE_BEGIN
+
+class AABox;
+
+/// Oriented box
+class [[nodiscard]] JPH_EXPORT_GCC_BUG_WORKAROUND OrientedBox
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+					OrientedBox() = default;
+					OrientedBox(Mat44Arg inOrientation, Vec3Arg inHalfExtents)			: mOrientation(inOrientation), mHalfExtents(inHalfExtents) { }
+
+	/// Construct from axis aligned box and transform. Only works for rotation/translation matrix (no scaling / shearing).
+					OrientedBox(Mat44Arg inOrientation, const AABox &inBox)				: OrientedBox(inOrientation.PreTranslated(inBox.GetCenter()), inBox.GetExtent()) { }
+
+	/// Test if oriented box overlaps with axis aligned box each other
+	bool			Overlaps(const AABox &inBox, float inEpsilon = 1.0e-6f) const;
+
+	/// Test if two oriented boxes overlap each other
+	bool			Overlaps(const OrientedBox &inBox, float inEpsilon = 1.0e-6f) const;
+
+	Mat44			mOrientation;														///< Transform that positions and rotates the local space axis aligned box into world space
+	Vec3			mHalfExtents;														///< Half extents (half the size of the edge) of the local space axis aligned box
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Geometry/Plane.h b/thirdparty/jolt_physics/Jolt/Geometry/Plane.h
new file mode 100644
index 0000000000..7e3b8a8c4f
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Geometry/Plane.h
@@ -0,0 +1,101 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// An infinite plane described by the formula X . Normal + Constant = 0.
+class [[nodiscard]] Plane
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+					Plane() = default;
+	explicit		Plane(Vec4Arg inNormalAndConstant)										: mNormalAndConstant(inNormalAndConstant) { }
+					Plane(Vec3Arg inNormal, float inConstant)								: mNormalAndConstant(inNormal, inConstant) { }
+
+	/// Create from point and normal
+	static Plane	sFromPointAndNormal(Vec3Arg inPoint, Vec3Arg inNormal)					{ return Plane(Vec4(inNormal, -inNormal.Dot(inPoint))); }
+
+	/// Create from point and normal, double precision version that more accurately calculates the plane constant
+	static Plane	sFromPointAndNormal(DVec3Arg inPoint, Vec3Arg inNormal)					{ return Plane(Vec4(inNormal, -float(DVec3(inNormal).Dot(inPoint)))); }
+
+	/// Create from 3 counter clockwise points
+	static Plane	sFromPointsCCW(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3)				{ return sFromPointAndNormal(inV1, (inV2 - inV1).Cross(inV3 - inV1).Normalized()); }
+
+	// Properties
+	Vec3			GetNormal() const														{ return Vec3(mNormalAndConstant); }
+	void			SetNormal(Vec3Arg inNormal)												{ mNormalAndConstant = Vec4(inNormal, mNormalAndConstant.GetW()); }
+	float			GetConstant() const														{ return mNormalAndConstant.GetW(); }
+	void			SetConstant(float inConstant)											{ mNormalAndConstant.SetW(inConstant); }
+
+	/// Offset the plane (positive value means move it in the direction of the plane normal)
+	Plane			Offset(float inDistance) const											{ return Plane(mNormalAndConstant - Vec4(Vec3::sZero(), inDistance)); }
+
+	/// Transform the plane by a matrix
+	inline Plane	GetTransformed(Mat44Arg inTransform) const
+	{
+		Vec3 transformed_normal = inTransform.Multiply3x3(GetNormal());
+		return Plane(transformed_normal, GetConstant() - inTransform.GetTranslation().Dot(transformed_normal));
+	}
+
+	/// Scale the plane, can handle non-uniform and negative scaling
+	inline Plane	Scaled(Vec3Arg inScale) const
+	{
+		Vec3 scaled_normal = GetNormal() / inScale;
+		float scaled_normal_length = scaled_normal.Length();
+		return Plane(scaled_normal / scaled_normal_length, GetConstant() / scaled_normal_length);
+	}
+
+	/// Distance point to plane
+	float			SignedDistance(Vec3Arg inPoint) const									{ return inPoint.Dot(GetNormal()) + GetConstant(); }
+
+	/// Project inPoint onto the plane
+	Vec3			ProjectPointOnPlane(Vec3Arg inPoint) const								{ return inPoint - GetNormal() * SignedDistance(inPoint); }
+
+	/// Returns intersection point between 3 planes
+	static bool		sIntersectPlanes(const Plane &inP1, const Plane &inP2, const Plane &inP3, Vec3 &outPoint)
+	{
+		// We solve the equation:
+		// |ax, ay, az, aw|   | x |   | 0 |
+		// |bx, by, bz, bw| * | y | = | 0 |
+		// |cx, cy, cz, cw|   | z |   | 0 |
+		// | 0,	 0,	 0,	 1|   | 1 |   | 1 |
+		// Where normal of plane 1 = (ax, ay, az), plane constant of 1 = aw, normal of plane 2 = (bx, by, bz) etc.
+		// This involves inverting the matrix and multiplying it with [0, 0, 0, 1]
+
+		// Fetch the normals and plane constants for the three planes
+		Vec4 a = inP1.mNormalAndConstant;
+		Vec4 b = inP2.mNormalAndConstant;
+		Vec4 c = inP3.mNormalAndConstant;
+
+		// Result is a vector that we have to divide by:
+		float denominator = Vec3(a).Dot(Vec3(b).Cross(Vec3(c)));
+		if (denominator == 0.0f)
+			return false;
+
+		// The numerator is:
+		// [aw*(bz*cy-by*cz)+ay*(bw*cz-bz*cw)+az*(by*cw-bw*cy)]
+		// [aw*(bx*cz-bz*cx)+ax*(bz*cw-bw*cz)+az*(bw*cx-bx*cw)]
+		// [aw*(by*cx-bx*cy)+ax*(bw*cy-by*cw)+ay*(bx*cw-bw*cx)]
+		Vec4 numerator =
+			a.SplatW() * (b.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_UNUSED>() * c.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_UNUSED>() - b.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X, SWIZZLE_UNUSED>() * c.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_UNUSED>())
+			+ a.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_X, SWIZZLE_UNUSED>() * (b.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_UNUSED>() * c.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_UNUSED>() - b.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_UNUSED>() * c.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_UNUSED>())
+			+ a.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_UNUSED>() * (b.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_X, SWIZZLE_UNUSED>() * c.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_W, SWIZZLE_UNUSED>() - b.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_W, SWIZZLE_UNUSED>() * c.Swizzle<SWIZZLE_Y, SWIZZLE_W, SWIZZLE_X, SWIZZLE_UNUSED>());
+
+		outPoint = Vec3(numerator) / denominator;
+		return true;
+	}
+
+private:
+#ifdef JPH_OBJECT_STREAM
+	friend void		CreateRTTIPlane(class RTTI &);										// For JPH_IMPLEMENT_SERIALIZABLE_OUTSIDE_CLASS
+#endif
+
+	Vec4			mNormalAndConstant;													///< XYZ = normal, W = constant, plane: x . normal + constant = 0
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Geometry/RayAABox.h b/thirdparty/jolt_physics/Jolt/Geometry/RayAABox.h
new file mode 100644
index 0000000000..4506fadbb7
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Geometry/RayAABox.h
@@ -0,0 +1,241 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Helper structure holding the reciprocal of a ray for Ray vs AABox testing
+class RayInvDirection
+{
+public:
+	/// Constructors
+	inline			RayInvDirection() = default;
+	inline explicit	RayInvDirection(Vec3Arg inDirection) { Set(inDirection); }
+
+	/// Set reciprocal from ray direction
+	inline void		Set(Vec3Arg inDirection)
+	{
+		// if (abs(inDirection) <= Epsilon) the ray is nearly parallel to the slab.
+		mIsParallel = Vec3::sLessOrEqual(inDirection.Abs(), Vec3::sReplicate(1.0e-20f));
+
+		// Calculate 1 / direction while avoiding division by zero
+		mInvDirection = Vec3::sSelect(inDirection, Vec3::sReplicate(1.0f), mIsParallel).Reciprocal();
+	}
+
+	Vec3			mInvDirection;					///< 1 / ray direction
+	UVec4			mIsParallel;					///< for each component if it is parallel to the coordinate axis
+};
+
+/// Intersect AABB with ray, returns minimal distance along ray or FLT_MAX if no hit
+/// Note: Can return negative value if ray starts in box
+JPH_INLINE float RayAABox(Vec3Arg inOrigin, const RayInvDirection &inInvDirection, Vec3Arg inBoundsMin, Vec3Arg inBoundsMax)
+{
+	// Constants
+	Vec3 flt_min = Vec3::sReplicate(-FLT_MAX);
+	Vec3 flt_max = Vec3::sReplicate(FLT_MAX);
+
+	// Test against all three axii simultaneously.
+	Vec3 t1 = (inBoundsMin - inOrigin) * inInvDirection.mInvDirection;
+	Vec3 t2 = (inBoundsMax - inOrigin) * inInvDirection.mInvDirection;
+
+	// Compute the max of min(t1,t2) and the min of max(t1,t2) ensuring we don't
+	// use the results from any directions parallel to the slab.
+	Vec3 t_min = Vec3::sSelect(Vec3::sMin(t1, t2), flt_min, inInvDirection.mIsParallel);
+	Vec3 t_max = Vec3::sSelect(Vec3::sMax(t1, t2), flt_max, inInvDirection.mIsParallel);
+
+	// t_min.xyz = maximum(t_min.x, t_min.y, t_min.z);
+	t_min = Vec3::sMax(t_min, t_min.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>());
+	t_min = Vec3::sMax(t_min, t_min.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y>());
+
+	// t_max.xyz = minimum(t_max.x, t_max.y, t_max.z);
+	t_max = Vec3::sMin(t_max, t_max.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>());
+	t_max = Vec3::sMin(t_max, t_max.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y>());
+
+	// if (t_min > t_max) return FLT_MAX;
+	UVec4 no_intersection = Vec3::sGreater(t_min, t_max);
+
+	// if (t_max < 0.0f) return FLT_MAX;
+	no_intersection = UVec4::sOr(no_intersection, Vec3::sLess(t_max, Vec3::sZero()));
+
+	// if (inInvDirection.mIsParallel && !(Min <= inOrigin && inOrigin <= Max)) return FLT_MAX; else return t_min;
+	UVec4 no_parallel_overlap = UVec4::sOr(Vec3::sLess(inOrigin, inBoundsMin), Vec3::sGreater(inOrigin, inBoundsMax));
+	no_intersection = UVec4::sOr(no_intersection, UVec4::sAnd(inInvDirection.mIsParallel, no_parallel_overlap));
+	no_intersection = UVec4::sOr(no_intersection, no_intersection.SplatY());
+	no_intersection = UVec4::sOr(no_intersection, no_intersection.SplatZ());
+	return Vec3::sSelect(t_min, flt_max, no_intersection).GetX();
+}
+
+/// Intersect 4 AABBs with ray, returns minimal distance along ray or FLT_MAX if no hit
+/// Note: Can return negative value if ray starts in box
+JPH_INLINE Vec4 RayAABox4(Vec3Arg inOrigin, const RayInvDirection &inInvDirection, Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ)
+{
+	// Constants
+	Vec4 flt_min = Vec4::sReplicate(-FLT_MAX);
+	Vec4 flt_max = Vec4::sReplicate(FLT_MAX);
+
+	// Origin
+	Vec4 originx = inOrigin.SplatX();
+	Vec4 originy = inOrigin.SplatY();
+	Vec4 originz = inOrigin.SplatZ();
+
+	// Parallel
+	UVec4 parallelx = inInvDirection.mIsParallel.SplatX();
+	UVec4 parallely = inInvDirection.mIsParallel.SplatY();
+	UVec4 parallelz = inInvDirection.mIsParallel.SplatZ();
+
+	// Inverse direction
+	Vec4 invdirx = inInvDirection.mInvDirection.SplatX();
+	Vec4 invdiry = inInvDirection.mInvDirection.SplatY();
+	Vec4 invdirz = inInvDirection.mInvDirection.SplatZ();
+
+	// Test against all three axii simultaneously.
+	Vec4 t1x = (inBoundsMinX - originx) * invdirx;
+	Vec4 t1y = (inBoundsMinY - originy) * invdiry;
+	Vec4 t1z = (inBoundsMinZ - originz) * invdirz;
+	Vec4 t2x = (inBoundsMaxX - originx) * invdirx;
+	Vec4 t2y = (inBoundsMaxY - originy) * invdiry;
+	Vec4 t2z = (inBoundsMaxZ - originz) * invdirz;
+
+	// Compute the max of min(t1,t2) and the min of max(t1,t2) ensuring we don't
+	// use the results from any directions parallel to the slab.
+	Vec4 t_minx = Vec4::sSelect(Vec4::sMin(t1x, t2x), flt_min, parallelx);
+	Vec4 t_miny = Vec4::sSelect(Vec4::sMin(t1y, t2y), flt_min, parallely);
+	Vec4 t_minz = Vec4::sSelect(Vec4::sMin(t1z, t2z), flt_min, parallelz);
+	Vec4 t_maxx = Vec4::sSelect(Vec4::sMax(t1x, t2x), flt_max, parallelx);
+	Vec4 t_maxy = Vec4::sSelect(Vec4::sMax(t1y, t2y), flt_max, parallely);
+	Vec4 t_maxz = Vec4::sSelect(Vec4::sMax(t1z, t2z), flt_max, parallelz);
+
+	// t_min.xyz = maximum(t_min.x, t_min.y, t_min.z);
+	Vec4 t_min = Vec4::sMax(Vec4::sMax(t_minx, t_miny), t_minz);
+
+	// t_max.xyz = minimum(t_max.x, t_max.y, t_max.z);
+	Vec4 t_max = Vec4::sMin(Vec4::sMin(t_maxx, t_maxy), t_maxz);
+
+	// if (t_min > t_max) return FLT_MAX;
+	UVec4 no_intersection = Vec4::sGreater(t_min, t_max);
+
+	// if (t_max < 0.0f) return FLT_MAX;
+	no_intersection = UVec4::sOr(no_intersection, Vec4::sLess(t_max, Vec4::sZero()));
+
+	// if bounds are invalid return FLOAT_MAX;
+	UVec4 bounds_invalid = UVec4::sOr(UVec4::sOr(Vec4::sGreater(inBoundsMinX, inBoundsMaxX), Vec4::sGreater(inBoundsMinY, inBoundsMaxY)), Vec4::sGreater(inBoundsMinZ, inBoundsMaxZ));
+	no_intersection = UVec4::sOr(no_intersection, bounds_invalid);
+
+	// if (inInvDirection.mIsParallel && !(Min <= inOrigin && inOrigin <= Max)) return FLT_MAX; else return t_min;
+	UVec4 no_parallel_overlapx = UVec4::sAnd(parallelx, UVec4::sOr(Vec4::sLess(originx, inBoundsMinX), Vec4::sGreater(originx, inBoundsMaxX)));
+	UVec4 no_parallel_overlapy = UVec4::sAnd(parallely, UVec4::sOr(Vec4::sLess(originy, inBoundsMinY), Vec4::sGreater(originy, inBoundsMaxY)));
+	UVec4 no_parallel_overlapz = UVec4::sAnd(parallelz, UVec4::sOr(Vec4::sLess(originz, inBoundsMinZ), Vec4::sGreater(originz, inBoundsMaxZ)));
+	no_intersection = UVec4::sOr(no_intersection, UVec4::sOr(UVec4::sOr(no_parallel_overlapx, no_parallel_overlapy), no_parallel_overlapz));
+	return Vec4::sSelect(t_min, flt_max, no_intersection);
+}
+
+/// Intersect AABB with ray, returns minimal and maximal distance along ray or FLT_MAX, -FLT_MAX if no hit
+/// Note: Can return negative value for outMin if ray starts in box
+JPH_INLINE void RayAABox(Vec3Arg inOrigin, const RayInvDirection &inInvDirection, Vec3Arg inBoundsMin, Vec3Arg inBoundsMax, float &outMin, float &outMax)
+{
+	// Constants
+	Vec3 flt_min = Vec3::sReplicate(-FLT_MAX);
+	Vec3 flt_max = Vec3::sReplicate(FLT_MAX);
+
+	// Test against all three axii simultaneously.
+	Vec3 t1 = (inBoundsMin - inOrigin) * inInvDirection.mInvDirection;
+	Vec3 t2 = (inBoundsMax - inOrigin) * inInvDirection.mInvDirection;
+
+	// Compute the max of min(t1,t2) and the min of max(t1,t2) ensuring we don't
+	// use the results from any directions parallel to the slab.
+	Vec3 t_min = Vec3::sSelect(Vec3::sMin(t1, t2), flt_min, inInvDirection.mIsParallel);
+	Vec3 t_max = Vec3::sSelect(Vec3::sMax(t1, t2), flt_max, inInvDirection.mIsParallel);
+
+	// t_min.xyz = maximum(t_min.x, t_min.y, t_min.z);
+	t_min = Vec3::sMax(t_min, t_min.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>());
+	t_min = Vec3::sMax(t_min, t_min.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y>());
+
+	// t_max.xyz = minimum(t_max.x, t_max.y, t_max.z);
+	t_max = Vec3::sMin(t_max, t_max.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>());
+	t_max = Vec3::sMin(t_max, t_max.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y>());
+
+	// if (t_min > t_max) return FLT_MAX;
+	UVec4 no_intersection = Vec3::sGreater(t_min, t_max);
+
+	// if (t_max < 0.0f) return FLT_MAX;
+	no_intersection = UVec4::sOr(no_intersection, Vec3::sLess(t_max, Vec3::sZero()));
+
+	// if (inInvDirection.mIsParallel && !(Min <= inOrigin && inOrigin <= Max)) return FLT_MAX; else return t_min;
+	UVec4 no_parallel_overlap = UVec4::sOr(Vec3::sLess(inOrigin, inBoundsMin), Vec3::sGreater(inOrigin, inBoundsMax));
+	no_intersection = UVec4::sOr(no_intersection, UVec4::sAnd(inInvDirection.mIsParallel, no_parallel_overlap));
+	no_intersection = UVec4::sOr(no_intersection, no_intersection.SplatY());
+	no_intersection = UVec4::sOr(no_intersection, no_intersection.SplatZ());
+	outMin = Vec3::sSelect(t_min, flt_max, no_intersection).GetX();
+	outMax = Vec3::sSelect(t_max, flt_min, no_intersection).GetX();
+}
+
+/// Intersect AABB with ray, returns true if there is a hit closer than inClosest
+JPH_INLINE bool RayAABoxHits(Vec3Arg inOrigin, const RayInvDirection &inInvDirection, Vec3Arg inBoundsMin, Vec3Arg inBoundsMax, float inClosest)
+{
+	// Constants
+	Vec3 flt_min = Vec3::sReplicate(-FLT_MAX);
+	Vec3 flt_max = Vec3::sReplicate(FLT_MAX);
+
+	// Test against all three axii simultaneously.
+	Vec3 t1 = (inBoundsMin - inOrigin) * inInvDirection.mInvDirection;
+	Vec3 t2 = (inBoundsMax - inOrigin) * inInvDirection.mInvDirection;
+
+	// Compute the max of min(t1,t2) and the min of max(t1,t2) ensuring we don't
+	// use the results from any directions parallel to the slab.
+	Vec3 t_min = Vec3::sSelect(Vec3::sMin(t1, t2), flt_min, inInvDirection.mIsParallel);
+	Vec3 t_max = Vec3::sSelect(Vec3::sMax(t1, t2), flt_max, inInvDirection.mIsParallel);
+
+	// t_min.xyz = maximum(t_min.x, t_min.y, t_min.z);
+	t_min = Vec3::sMax(t_min, t_min.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>());
+	t_min = Vec3::sMax(t_min, t_min.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y>());
+
+	// t_max.xyz = minimum(t_max.x, t_max.y, t_max.z);
+	t_max = Vec3::sMin(t_max, t_max.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>());
+	t_max = Vec3::sMin(t_max, t_max.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y>());
+
+	// if (t_min > t_max) return false;
+	UVec4 no_intersection = Vec3::sGreater(t_min, t_max);
+
+	// if (t_max < 0.0f) return false;
+	no_intersection = UVec4::sOr(no_intersection, Vec3::sLess(t_max, Vec3::sZero()));
+
+	// if (t_min > inClosest) return false;
+	no_intersection = UVec4::sOr(no_intersection, Vec3::sGreater(t_min, Vec3::sReplicate(inClosest)));
+
+	// if (inInvDirection.mIsParallel && !(Min <= inOrigin && inOrigin <= Max)) return false; else return true;
+	UVec4 no_parallel_overlap = UVec4::sOr(Vec3::sLess(inOrigin, inBoundsMin), Vec3::sGreater(inOrigin, inBoundsMax));
+	no_intersection = UVec4::sOr(no_intersection, UVec4::sAnd(inInvDirection.mIsParallel, no_parallel_overlap));
+
+	return !no_intersection.TestAnyXYZTrue();
+}
+
+/// Intersect AABB with ray without hit fraction, based on separating axis test
+/// @see http://www.codercorner.com/RayAABB.cpp
+JPH_INLINE bool RayAABoxHits(Vec3Arg inOrigin, Vec3Arg inDirection, Vec3Arg inBoundsMin, Vec3Arg inBoundsMax)
+{
+	Vec3 extents = inBoundsMax - inBoundsMin;
+
+	Vec3 diff = 2.0f * inOrigin - inBoundsMin - inBoundsMax;
+	Vec3 abs_diff = diff.Abs();
+
+	UVec4 no_intersection = UVec4::sAnd(Vec3::sGreater(abs_diff, extents), Vec3::sGreaterOrEqual(diff * inDirection, Vec3::sZero()));
+
+	Vec3 abs_dir = inDirection.Abs();
+	Vec3 abs_dir_yzz = abs_dir.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Z>();
+	Vec3 abs_dir_xyx = abs_dir.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_X>();
+
+	Vec3 extents_yzz = extents.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Z>();
+	Vec3 extents_xyx = extents.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_X>();
+
+	Vec3 diff_yzx = diff.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>();
+
+	Vec3 dir_yzx = inDirection.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>();
+
+	no_intersection = UVec4::sOr(no_intersection, Vec3::sGreater((inDirection * diff_yzx - dir_yzx * diff).Abs(), extents_xyx * abs_dir_yzz + extents_yzz * abs_dir_xyx));
+
+	return !no_intersection.TestAnyXYZTrue();
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Geometry/RayCapsule.h b/thirdparty/jolt_physics/Jolt/Geometry/RayCapsule.h
new file mode 100644
index 0000000000..4862931b68
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Geometry/RayCapsule.h
@@ -0,0 +1,37 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/RayCylinder.h>
+#include <Jolt/Geometry/RaySphere.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Tests a ray starting at inRayOrigin and extending infinitely in inRayDirection
+/// against a capsule centered around the origin with its axis along the Y axis and half height specified.
+/// @return FLT_MAX if there is no intersection, otherwise the fraction along the ray.
+/// @param inRayDirection Ray direction. Does not need to be normalized.
+/// @param inRayOrigin Origin of the ray. If the ray starts inside the capsule, the returned fraction will be 0.
+/// @param inCapsuleHalfHeight Distance from the origin to the center of the top sphere (or that of the bottom)
+/// @param inCapsuleRadius Radius of the top/bottom sphere
+JPH_INLINE float RayCapsule(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, float inCapsuleHalfHeight, float inCapsuleRadius)
+{
+	// Test infinite cylinder
+	float cylinder = RayCylinder(inRayOrigin, inRayDirection, inCapsuleRadius);
+	if (cylinder == FLT_MAX)
+		return FLT_MAX;
+
+	// If this hit is in the finite cylinder we have our fraction
+	if (abs(inRayOrigin.GetY() + cylinder * inRayDirection.GetY()) <= inCapsuleHalfHeight)
+		return cylinder;
+
+	// Test upper and lower sphere
+	Vec3 sphere_center(0, inCapsuleHalfHeight, 0);
+	float upper = RaySphere(inRayOrigin, inRayDirection, sphere_center, inCapsuleRadius);
+	float lower = RaySphere(inRayOrigin, inRayDirection, -sphere_center, inCapsuleRadius);
+	return min(upper, lower);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Geometry/RayCylinder.h b/thirdparty/jolt_physics/Jolt/Geometry/RayCylinder.h
new file mode 100644
index 0000000000..cabed0680a
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Geometry/RayCylinder.h
@@ -0,0 +1,101 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Math/FindRoot.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Tests a ray starting at inRayOrigin and extending infinitely in inRayDirection
+/// against an infinite cylinder centered along the Y axis
+/// @return FLT_MAX if there is no intersection, otherwise the fraction along the ray.
+/// @param inRayDirection Direction of the ray. Does not need to be normalized.
+/// @param inRayOrigin Origin of the ray. If the ray starts inside the cylinder, the returned fraction will be 0.
+/// @param inCylinderRadius Radius of the infinite cylinder
+JPH_INLINE float RayCylinder(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, float inCylinderRadius)
+{
+	// Remove Y component of ray to see of ray intersects with infinite cylinder
+	UVec4 mask_y = UVec4(0, 0xffffffff, 0, 0);
+	Vec3 origin_xz = Vec3::sSelect(inRayOrigin, Vec3::sZero(), mask_y);
+	float origin_xz_len_sq = origin_xz.LengthSq();
+	float r_sq = Square(inCylinderRadius);
+	if (origin_xz_len_sq > r_sq)
+	{
+		// Ray starts outside of the infinite cylinder
+		// Solve: |RayOrigin_xz + fraction * RayDirection_xz|^2 = r^2 to find fraction
+		Vec3 direction_xz = Vec3::sSelect(inRayDirection, Vec3::sZero(), mask_y);
+		float a = direction_xz.LengthSq();
+		float b = 2.0f * origin_xz.Dot(direction_xz);
+		float c = origin_xz_len_sq - r_sq;
+		float fraction1, fraction2;
+		if (FindRoot(a, b, c, fraction1, fraction2) == 0)
+			return FLT_MAX; // No intersection with infinite cylinder
+
+		// Get fraction corresponding to the ray entering the circle
+		float fraction = min(fraction1, fraction2);
+		if (fraction >= 0.0f)
+			return fraction;
+	}
+	else
+	{
+		// Ray starts inside the infinite cylinder
+		return 0.0f;
+	}
+
+	// No collision
+	return FLT_MAX;
+}
+
+/// Test a ray against a cylinder centered around the origin with its axis along the Y axis and half height specified.
+/// @return FLT_MAX if there is no intersection, otherwise the fraction along the ray.
+/// @param inRayDirection Ray direction. Does not need to be normalized.
+/// @param inRayOrigin Origin of the ray. If the ray starts inside the cylinder, the returned fraction will be 0.
+/// @param inCylinderRadius Radius of the cylinder
+/// @param inCylinderHalfHeight Distance from the origin to the top (or bottom) of the cylinder
+JPH_INLINE float RayCylinder(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, float inCylinderHalfHeight, float inCylinderRadius)
+{
+	// Test infinite cylinder
+	float fraction = RayCylinder(inRayOrigin, inRayDirection, inCylinderRadius);
+	if (fraction == FLT_MAX)
+		return FLT_MAX;
+
+	// If this hit is in the finite cylinder we have our fraction
+	if (abs(inRayOrigin.GetY() + fraction * inRayDirection.GetY()) <= inCylinderHalfHeight)
+		return fraction;
+
+	// Check if ray could hit the top or bottom plane of the cylinder
+	float direction_y = inRayDirection.GetY();
+	if (direction_y != 0.0f)
+	{
+		// Solving line equation: x = ray_origin + fraction * ray_direction
+		// and plane equation: plane_normal . x + plane_constant = 0
+		// fraction = (-plane_constant - plane_normal . ray_origin) / (plane_normal . ray_direction)
+		// when the ray_direction.y < 0:
+		// plane_constant = -cylinder_half_height, plane_normal = (0, 1, 0)
+		// else
+		// plane_constant = -cylinder_half_height, plane_normal = (0, -1, 0)
+		float origin_y = inRayOrigin.GetY();
+		float plane_fraction;
+		if (direction_y < 0.0f)
+			plane_fraction = (inCylinderHalfHeight - origin_y) / direction_y;
+		else
+			plane_fraction = -(inCylinderHalfHeight + origin_y) / direction_y;
+
+		// Check if the hit is in front of the ray
+		if (plane_fraction >= 0.0f)
+		{
+			// Test if this hit is inside the cylinder
+			Vec3 point = inRayOrigin + plane_fraction * inRayDirection;
+			float dist_sq = Square(point.GetX()) + Square(point.GetZ());
+			if (dist_sq <= Square(inCylinderRadius))
+				return plane_fraction;
+		}
+	}
+
+	// No collision
+	return FLT_MAX;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Geometry/RaySphere.h b/thirdparty/jolt_physics/Jolt/Geometry/RaySphere.h
new file mode 100644
index 0000000000..93ad268280
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Geometry/RaySphere.h
@@ -0,0 +1,96 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Math/FindRoot.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Tests a ray starting at inRayOrigin and extending infinitely in inRayDirection against a sphere,
+/// @return FLT_MAX if there is no intersection, otherwise the fraction along the ray.
+/// @param inRayOrigin Ray origin. If the ray starts inside the sphere, the returned fraction will be 0.
+/// @param inRayDirection Ray direction. Does not need to be normalized.
+/// @param inSphereCenter Position of the center of the sphere
+/// @param inSphereRadius Radius of the sphere
+JPH_INLINE float RaySphere(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, Vec3Arg inSphereCenter, float inSphereRadius)
+{
+	// Solve: |RayOrigin + fraction * RayDirection - SphereCenter|^2 = SphereRadius^2 for fraction
+	Vec3 center_origin = inRayOrigin - inSphereCenter;
+	float a = inRayDirection.LengthSq();
+	float b = 2.0f * inRayDirection.Dot(center_origin);
+	float c = center_origin.LengthSq() - inSphereRadius * inSphereRadius;
+	float fraction1, fraction2;
+	if (FindRoot(a, b, c, fraction1, fraction2) == 0)
+		return c <= 0.0f? 0.0f : FLT_MAX; // Return if origin is inside the sphere
+
+	// Sort so that the smallest is first
+	if (fraction1 > fraction2)
+		std::swap(fraction1, fraction2);
+
+	// Test solution with lowest fraction, this will be the ray entering the sphere
+	if (fraction1 >= 0.0f)
+		return fraction1; // Sphere is before the ray start
+
+	// Test solution with highest fraction, this will be the ray leaving the sphere
+	if (fraction2 >= 0.0f)
+		return 0.0f; // We start inside the sphere
+
+	// No solution
+	return FLT_MAX;
+}
+
+/// Tests a ray starting at inRayOrigin and extending infinitely in inRayDirection against a sphere.
+/// Outputs entry and exit points (outMinFraction and outMaxFraction) along the ray (which could be negative if the hit point is before the start of the ray).
+/// @param inRayOrigin Ray origin. If the ray starts inside the sphere, the returned fraction will be 0.
+/// @param inRayDirection Ray direction. Does not need to be normalized.
+/// @param inSphereCenter Position of the center of the sphere.
+/// @param inSphereRadius Radius of the sphere.
+/// @param outMinFraction Returned lowest intersection fraction
+/// @param outMaxFraction Returned highest intersection fraction
+/// @return The amount of intersections with the sphere.
+/// If 1 intersection is returned outMinFraction will be equal to outMaxFraction
+JPH_INLINE int RaySphere(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, Vec3Arg inSphereCenter, float inSphereRadius, float &outMinFraction, float &outMaxFraction)
+{
+	// Solve: |RayOrigin + fraction * RayDirection - SphereCenter|^2 = SphereRadius^2 for fraction
+	Vec3 center_origin = inRayOrigin - inSphereCenter;
+	float a = inRayDirection.LengthSq();
+	float b = 2.0f * inRayDirection.Dot(center_origin);
+	float c = center_origin.LengthSq() - inSphereRadius * inSphereRadius;
+	float fraction1, fraction2;
+	switch (FindRoot(a, b, c, fraction1, fraction2))
+	{
+	case 0:
+		if (c <= 0.0f)
+		{
+			// Origin inside sphere
+			outMinFraction = outMaxFraction = 0.0f;
+			return 1;
+		}
+		else
+		{
+			// Origin outside of the sphere
+			return 0;
+		}
+		break;
+
+	case 1:
+		// Ray is touching the sphere
+		outMinFraction = outMaxFraction = fraction1;
+		return 1;
+
+	default:
+		// Ray enters and exits the sphere
+
+		// Sort so that the smallest is first
+		if (fraction1 > fraction2)
+			std::swap(fraction1, fraction2);
+
+		outMinFraction = fraction1;
+		outMaxFraction = fraction2;
+		return 2;
+	}
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Geometry/RayTriangle.h b/thirdparty/jolt_physics/Jolt/Geometry/RayTriangle.h
new file mode 100644
index 0000000000..dabd0275cc
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Geometry/RayTriangle.h
@@ -0,0 +1,158 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Intersect ray with triangle, returns closest point or FLT_MAX if no hit (branch less version)
+/// Adapted from: http://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm
+JPH_INLINE float RayTriangle(Vec3Arg inOrigin, Vec3Arg inDirection, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
+{
+	// Epsilon
+	Vec3 epsilon = Vec3::sReplicate(1.0e-12f);
+
+	// Zero & one
+	Vec3 zero = Vec3::sZero();
+	Vec3 one = Vec3::sReplicate(1.0f);
+
+	// Find vectors for two edges sharing inV0
+	Vec3 e1 = inV1 - inV0;
+	Vec3 e2 = inV2 - inV0;
+
+	// Begin calculating determinant - also used to calculate u parameter
+	Vec3 p = inDirection.Cross(e2);
+
+	// if determinant is near zero, ray lies in plane of triangle
+	Vec3 det = Vec3::sReplicate(e1.Dot(p));
+
+	// Check if determinant is near zero
+	UVec4 det_near_zero = Vec3::sLess(det.Abs(), epsilon);
+
+	// When the determinant is near zero, set it to one to avoid dividing by zero
+	det = Vec3::sSelect(det, Vec3::sReplicate(1.0f), det_near_zero);
+
+	// Calculate distance from inV0 to ray origin
+	Vec3 s = inOrigin - inV0;
+
+	// Calculate u parameter
+	Vec3 u = Vec3::sReplicate(s.Dot(p)) / det;
+
+	// Prepare to test v parameter
+	Vec3 q = s.Cross(e1);
+
+	// Calculate v parameter
+	Vec3 v = Vec3::sReplicate(inDirection.Dot(q)) / det;
+
+	// Get intersection point
+	Vec3 t = Vec3::sReplicate(e2.Dot(q)) / det;
+
+	// Check if there is an intersection
+	UVec4 no_intersection =
+		UVec4::sOr
+		(
+			UVec4::sOr
+			(
+				UVec4::sOr
+				(
+					det_near_zero,
+					Vec3::sLess(u, zero)
+				),
+				UVec4::sOr
+				(
+					Vec3::sLess(v, zero),
+					Vec3::sGreater(u + v, one)
+				)
+			),
+			Vec3::sLess(t, zero)
+		);
+
+	// Select intersection point or FLT_MAX based on if there is an intersection or not
+	return Vec3::sSelect(t, Vec3::sReplicate(FLT_MAX), no_intersection).GetX();
+}
+
+/// Intersect ray with 4 triangles in SOA format, returns 4 vector of closest points or FLT_MAX if no hit (uses bit tricks to do less divisions)
+JPH_INLINE Vec4 RayTriangle4(Vec3Arg inOrigin, Vec3Arg inDirection, Vec4Arg inV0X, Vec4Arg inV0Y, Vec4Arg inV0Z, Vec4Arg inV1X, Vec4Arg inV1Y, Vec4Arg inV1Z, Vec4Arg inV2X, Vec4Arg inV2Y, Vec4Arg inV2Z)
+{
+	// Epsilon
+	Vec4 epsilon = Vec4::sReplicate(1.0e-12f);
+
+	// Zero
+	Vec4 zero = Vec4::sZero();
+
+	// Find vectors for two edges sharing inV0
+	Vec4 e1x = inV1X - inV0X;
+	Vec4 e1y = inV1Y - inV0Y;
+	Vec4 e1z = inV1Z - inV0Z;
+	Vec4 e2x = inV2X - inV0X;
+	Vec4 e2y = inV2Y - inV0Y;
+	Vec4 e2z = inV2Z - inV0Z;
+
+	// Get direction vector components
+	Vec4 dx = inDirection.SplatX();
+	Vec4 dy = inDirection.SplatY();
+	Vec4 dz = inDirection.SplatZ();
+
+	// Begin calculating determinant - also used to calculate u parameter
+	Vec4 px = dy * e2z - dz * e2y;
+	Vec4 py = dz * e2x - dx * e2z;
+	Vec4 pz = dx * e2y - dy * e2x;
+
+	// if determinant is near zero, ray lies in plane of triangle
+	Vec4 det = e1x * px + e1y * py + e1z * pz;
+
+	// Get sign bit for determinant and make positive
+	Vec4 det_sign = Vec4::sAnd(det, UVec4::sReplicate(0x80000000).ReinterpretAsFloat());
+	det = Vec4::sXor(det, det_sign);
+
+	// Check which determinants are near zero
+	UVec4 det_near_zero = Vec4::sLess(det, epsilon);
+
+	// Set components of the determinant to 1 that are near zero to avoid dividing by zero
+	det = Vec4::sSelect(det, Vec4::sReplicate(1.0f), det_near_zero);
+
+	// Calculate distance from inV0 to ray origin
+	Vec4 sx = inOrigin.SplatX() - inV0X;
+	Vec4 sy = inOrigin.SplatY() - inV0Y;
+	Vec4 sz = inOrigin.SplatZ() - inV0Z;
+
+	// Calculate u parameter and flip sign if determinant was negative
+	Vec4 u = Vec4::sXor(sx * px + sy * py + sz * pz, det_sign);
+
+	// Prepare to test v parameter
+	Vec4 qx = sy * e1z - sz * e1y;
+	Vec4 qy = sz * e1x - sx * e1z;
+	Vec4 qz = sx * e1y - sy * e1x;
+
+	// Calculate v parameter and flip sign if determinant was negative
+	Vec4 v = Vec4::sXor(dx * qx + dy * qy + dz * qz, det_sign);
+
+	// Get intersection point and flip sign if determinant was negative
+	Vec4 t = Vec4::sXor(e2x * qx + e2y * qy + e2z * qz, det_sign);
+
+	// Check if there is an intersection
+	UVec4 no_intersection =
+		UVec4::sOr
+		(
+			UVec4::sOr
+			(
+				UVec4::sOr
+				(
+					det_near_zero,
+					Vec4::sLess(u, zero)
+				),
+				UVec4::sOr
+				(
+					Vec4::sLess(v, zero),
+					Vec4::sGreater(u + v, det)
+				)
+			),
+			Vec4::sLess(t, zero)
+		);
+
+	// Select intersection point or FLT_MAX based on if there is an intersection or not
+	return Vec4::sSelect(t / det, Vec4::sReplicate(FLT_MAX), no_intersection);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Geometry/Sphere.h b/thirdparty/jolt_physics/Jolt/Geometry/Sphere.h
new file mode 100644
index 0000000000..05f7dd14f3
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Geometry/Sphere.h
@@ -0,0 +1,72 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/AABox.h>
+
+JPH_NAMESPACE_BEGIN
+
+class [[nodiscard]] Sphere
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+	inline				Sphere() = default;
+	inline				Sphere(const Float3 &inCenter, float inRadius)			: mCenter(inCenter), mRadius(inRadius) { }
+	inline				Sphere(Vec3Arg inCenter, float inRadius)				: mRadius(inRadius) { inCenter.StoreFloat3(&mCenter); }
+
+	/// Calculate the support vector for this convex shape.
+	inline Vec3			GetSupport(Vec3Arg inDirection) const
+	{
+		float length = inDirection.Length();
+		return length > 0.0f ? Vec3::sLoadFloat3Unsafe(mCenter) + (mRadius/ length) * inDirection : Vec3::sLoadFloat3Unsafe(mCenter);
+	}
+
+	// Properties
+	inline Vec3			GetCenter() const										{ return Vec3::sLoadFloat3Unsafe(mCenter); }
+	inline float		GetRadius() const										{ return mRadius; }
+
+	/// Test if two spheres overlap
+	inline bool			Overlaps(const Sphere &inB) const
+	{
+		return (Vec3::sLoadFloat3Unsafe(mCenter) - Vec3::sLoadFloat3Unsafe(inB.mCenter)).LengthSq() <= Square(mRadius + inB.mRadius);
+	}
+
+	/// Check if this sphere overlaps with a box
+	inline bool			Overlaps(const AABox &inOther) const
+	{
+		return inOther.GetSqDistanceTo(GetCenter()) <= Square(mRadius);
+	}
+
+	/// Create the minimal sphere that encapsulates this sphere and inPoint
+	inline void			EncapsulatePoint(Vec3Arg inPoint)
+	{
+		// Calculate distance between point and center
+		Vec3 center = GetCenter();
+		Vec3 d_vec = inPoint - center;
+		float d_sq = d_vec.LengthSq();
+		if (d_sq > Square(mRadius))
+		{
+			// It is further away than radius, we need to widen the sphere
+			// The diameter of the new sphere is radius + d, so the new radius is half of that
+			float d = sqrt(d_sq);
+			float radius = 0.5f * (mRadius + d);
+
+			// The center needs to shift by new radius - old radius in the direction of d
+			center += (radius - mRadius) / d * d_vec;
+
+			// Store new sphere
+			center.StoreFloat3(&mCenter);
+			mRadius = radius;
+		}
+	}
+
+private:
+	Float3				mCenter;
+	float				mRadius;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Geometry/Triangle.h b/thirdparty/jolt_physics/Jolt/Geometry/Triangle.h
new file mode 100644
index 0000000000..7ad718fc2a
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Geometry/Triangle.h
@@ -0,0 +1,34 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// A simple triangle and its material
+class Triangle
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+					Triangle() = default;
+					Triangle(const Float3 &inV1, const Float3 &inV2, const Float3 &inV3, uint32 inMaterialIndex = 0, uint32 inUserData = 0) : mV { inV1, inV2, inV3 }, mMaterialIndex(inMaterialIndex), mUserData(inUserData) { }
+					Triangle(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, uint32 inMaterialIndex = 0, uint32 inUserData = 0) : mMaterialIndex(inMaterialIndex), mUserData(inUserData) { inV1.StoreFloat3(&mV[0]); inV2.StoreFloat3(&mV[1]); inV3.StoreFloat3(&mV[2]); }
+
+	/// Get center of triangle
+	Vec3			GetCentroid() const
+	{
+		return (Vec3::sLoadFloat3Unsafe(mV[0]) + Vec3::sLoadFloat3Unsafe(mV[1]) + Vec3::sLoadFloat3Unsafe(mV[2])) * (1.0f / 3.0f);
+	}
+
+	/// Vertices
+	Float3			mV[3];
+	uint32			mMaterialIndex = 0;			///< Follows mV[3] so that we can read mV as 4 vectors
+	uint32			mUserData = 0;				///< User data that can be used for anything by the application, e.g. for tracking the original index of the triangle
+};
+
+using TriangleList = Array<Triangle>;
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Jolt.h b/thirdparty/jolt_physics/Jolt/Jolt.h
new file mode 100644
index 0000000000..acc400ce9e
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Jolt.h
@@ -0,0 +1,16 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+// Project includes
+#include <Jolt/Core/Core.h>
+#include <Jolt/Core/ARMNeon.h>
+#include <Jolt/Core/Memory.h>
+#include <Jolt/Core/IssueReporting.h>
+#include <Jolt/Core/Array.h>
+#include <Jolt/Math/Math.h>
+#include <Jolt/Math/Vec4.h>
+#include <Jolt/Math/Mat44.h>
+#include <Jolt/Math/Real.h>
diff --git a/thirdparty/jolt_physics/Jolt/Jolt.natvis b/thirdparty/jolt_physics/Jolt/Jolt.natvis
new file mode 100644
index 0000000000..bebafa1adc
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Jolt.natvis
@@ -0,0 +1,116 @@
+<?xml version="1.0" encoding="utf-8"?>
+<AutoVisualizer xmlns="http://schemas.microsoft.com/vstudio/debugger/natvis/2010">
+	<Type Name="JPH::Color">
+		<DisplayString>r={(int)r}, g={(int)g}, b={(int)b}, a={(int)a}</DisplayString>
+	</Type>
+	<Type Name="JPH::Float2">
+		<DisplayString>{x}, {y}</DisplayString>
+	</Type>
+	<Type Name="JPH::Float3">
+		<DisplayString>{x}, {y}, {z}</DisplayString>
+	</Type>
+	<Type Name="JPH::Float4">
+		<DisplayString>{x}, {y}, {z}, {w}</DisplayString>
+	</Type>
+	<Type Name="JPH::Vec3">
+		<DisplayString>{mF32[0]}, {mF32[1]}, {mF32[2]}, L^2={mF32[0]*mF32[0]+mF32[1]*mF32[1]+mF32[2]*mF32[2]}</DisplayString>
+	</Type>
+	<Type Name="JPH::DVec3">
+		<DisplayString>{mF64[0]}, {mF64[1]}, {mF64[2]}, L^2={mF64[0]*mF64[0]+mF64[1]*mF64[1]+mF64[2]*mF64[2]}</DisplayString>
+	</Type>
+	<Type Name="JPH::Vec4">
+		<DisplayString>{mF32[0]}, {mF32[1]}, {mF32[2]}, {mF32[3]}, L^2={mF32[0]*mF32[0]+mF32[1]*mF32[1]+mF32[2]*mF32[2]+mF32[3]*mF32[3]}</DisplayString>
+	</Type>
+	<Type Name="JPH::UVec4">
+		<DisplayString>{mU32[0]}, {mU32[1]}, {mU32[2]}, {mU32[3]}</DisplayString>
+	</Type>
+	<Type Name="JPH::BVec16">
+		<DisplayString>{uint(mU8[0])}, {uint(mU8[1])}, {uint(mU8[2])}, {uint(mU8[3])}, {uint(mU8[4])}, {uint(mU8[5])}, {uint(mU8[6])}, {uint(mU8[7])}, {uint(mU8[8])}, {uint(mU8[9])}, {uint(mU8[10])}, {uint(mU8[11])}, {uint(mU8[12])}, {uint(mU8[13])}, {uint(mU8[14])}, {uint(mU8[15])}</DisplayString>
+	</Type>
+	<Type Name="JPH::Quat">
+		<DisplayString>{mValue}</DisplayString>
+	</Type>
+	<Type Name="JPH::Mat44">
+		<DisplayString>{mCol[0].mF32[0]}, {mCol[1].mF32[0]}, {mCol[2].mF32[0]}, {mCol[3].mF32[0]} | {mCol[0].mF32[1]}, {mCol[1].mF32[1]}, {mCol[2].mF32[1]}, {mCol[3].mF32[1]} | {mCol[0].mF32[2]}, {mCol[1].mF32[2]}, {mCol[2].mF32[2]}, {mCol[3].mF32[2]}</DisplayString>
+		<Expand>
+			<Synthetic Name="[Row 0]">
+				<DisplayString>{mCol[0].mF32[0]}, {mCol[1].mF32[0]}, {mCol[2].mF32[0]}, {mCol[3].mF32[0]}</DisplayString>
+			</Synthetic>
+			<Synthetic Name="[Row 1]">
+				<DisplayString>{mCol[0].mF32[1]}, {mCol[1].mF32[1]}, {mCol[2].mF32[1]}, {mCol[3].mF32[1]}</DisplayString>
+			</Synthetic>
+			<Synthetic Name="[Row 2]">
+				<DisplayString>{mCol[0].mF32[2]}, {mCol[1].mF32[2]}, {mCol[2].mF32[2]}, {mCol[3].mF32[2]}</DisplayString>
+			</Synthetic>
+			<Synthetic Name="[Row 3]">
+				<DisplayString>{mCol[0].mF32[3]}, {mCol[1].mF32[3]}, {mCol[2].mF32[3]}, {mCol[3].mF32[3]}</DisplayString>
+			</Synthetic>
+		</Expand>
+	</Type>
+	<Type Name="JPH::DMat44">
+		<DisplayString>{mCol[0].mF32[0]}, {mCol[1].mF32[0]}, {mCol[2].mF32[0]}, {mCol3.mF64[0]} | {mCol[0].mF32[1]}, {mCol[1].mF32[1]}, {mCol[2].mF32[1]}, {mCol3.mF64[1]} | {mCol[0].mF32[2]}, {mCol[1].mF32[2]}, {mCol[2].mF32[2]}, {mCol3.mF64[2]}</DisplayString>
+		<Expand>
+			<Synthetic Name="[Row 0]">
+				<DisplayString>{mCol[0].mF32[0]}, {mCol[1].mF32[0]}, {mCol[2].mF32[0]}, {mCol3.mF64[0]}</DisplayString>
+			</Synthetic>
+			<Synthetic Name="[Row 1]">
+				<DisplayString>{mCol[0].mF32[1]}, {mCol[1].mF32[1]}, {mCol[2].mF32[1]}, {mCol3.mF64[1]}</DisplayString>
+			</Synthetic>
+			<Synthetic Name="[Row 2]">
+				<DisplayString>{mCol[0].mF32[2]}, {mCol[1].mF32[2]}, {mCol[2].mF32[2]}, {mCol3.mF64[2]}</DisplayString>
+			</Synthetic>
+			<Synthetic Name="[Row 3]">
+				<DisplayString>{mCol[0].mF32[3]}, {mCol[1].mF32[3]}, {mCol[2].mF32[3]}, 1}</DisplayString>
+			</Synthetic>
+		</Expand>
+	</Type>
+	<Type Name="JPH::AABox">
+		<DisplayString>min=({mMin}), max=({mMax})</DisplayString>
+	</Type>
+	<Type Name="JPH::BodyID">
+		<DisplayString>{mID}</DisplayString>
+	</Type>
+	<Type Name="JPH::Body">
+		<DisplayString>{mDebugName}: p=({mPosition.mF32[0],g}, {mPosition.mF32[1],g}, {mPosition.mF32[2],g}), r=({mRotation.mValue.mF32[0],g}, {mRotation.mValue.mF32[1],g}, {mRotation.mValue.mF32[2],g}, {mRotation.mValue.mF32[3],g}), v=({mLinearVelocity.mF32[0],g}, {mLinearVelocity.mF32[1],g}, {mLinearVelocity.mF32[2],g}), w=({mAngularVelocity.mF32[0],g}, {mAngularVelocity.mF32[1],g}, {mAngularVelocity.mF32[2],g})</DisplayString>
+	</Type>
+	<Type Name="JPH::BodyManager">
+		<DisplayString>bodies={mBodies._Mypair._Myval2._Mylast - mBodies._Mypair._Myval2._Myfirst}, active={mActiveBodies._Mypair._Myval2._Mylast - mActiveBodies._Mypair._Myval2._Myfirst}</DisplayString>
+	</Type>
+	<Type Name="JPH::StaticArray&lt;*&gt;">
+		<DisplayString>size={mSize}</DisplayString>
+		<Expand>
+			<Item Name="[size]" ExcludeView="simple">mSize</Item>
+			<ArrayItems>
+				<Size>mSize</Size>
+				<ValuePointer>(value_type *)mElements</ValuePointer>
+			</ArrayItems>
+		</Expand>
+	</Type>
+	<Type Name="JPH::Array&lt;*&gt;">
+		<DisplayString>size={mSize}</DisplayString>
+		<Expand>
+			<Item Name="[size]" ExcludeView="simple">mSize</Item>
+			<Item Name="[capacity]" ExcludeView="simple">mCapacity</Item>
+			<ArrayItems>
+				<Size>mSize</Size>
+				<ValuePointer>mElements</ValuePointer>
+			</ArrayItems>
+		</Expand>
+	</Type>
+	<Type Name="JPH::HashTable&lt;*&gt;">
+		<DisplayString>size={mSize}</DisplayString>
+		<Expand>
+			<Item Name="[size]" ExcludeView="simple">mSize</Item>
+			<Item Name="[bucket_count]" ExcludeView="simple">mMaxSize</Item>
+			<IndexListItems Condition="mData != nullptr">
+				<Size>mMaxSize</Size>
+				<ValueNode Condition="mControl[$i] &amp; 0x80">mData[$i]</ValueNode>
+				<ValueNode Condition="mControl[$i] == 0">"--Empty--"</ValueNode>
+				<ValueNode Condition="mControl[$i] == 0x7f">"--Deleted--"</ValueNode>
+			</IndexListItems>
+		</Expand>
+	</Type>
+	<Type Name="JPH::StridedPtr&lt;*&gt;">
+		<DisplayString>{(value_type *)mPtr}, stride={mStride}</DisplayString>
+	</Type>
+</AutoVisualizer>
diff --git a/thirdparty/jolt_physics/Jolt/Math/BVec16.h b/thirdparty/jolt_physics/Jolt/Math/BVec16.h
new file mode 100644
index 0000000000..a4ac12bf79
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/BVec16.h
@@ -0,0 +1,99 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// A vector consisting of 16 bytes
+class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) BVec16
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	// Underlying vector type
+#if defined(JPH_USE_SSE)
+	using Type = __m128i;
+#elif defined(JPH_USE_NEON)
+	using Type = uint8x16_t;
+#else
+	using Type = struct { uint64 mData[2]; };
+#endif
+
+	/// Constructor
+								BVec16() = default; ///< Intentionally not initialized for performance reasons
+								BVec16(const BVec16 &inRHS) = default;
+	BVec16 &					operator = (const BVec16 &inRHS) = default;
+	JPH_INLINE					BVec16(Type inRHS) : mValue(inRHS)					{ }
+
+	/// Create a vector from 16 bytes
+	JPH_INLINE					BVec16(uint8 inB0, uint8 inB1, uint8 inB2, uint8 inB3, uint8 inB4, uint8 inB5, uint8 inB6, uint8 inB7, uint8 inB8, uint8 inB9, uint8 inB10, uint8 inB11, uint8 inB12, uint8 inB13, uint8 inB14, uint8 inB15);
+
+	/// Create a vector from two uint64's
+	JPH_INLINE					BVec16(uint64 inV0, uint64 inV1);
+
+	/// Comparison
+	JPH_INLINE bool				operator == (BVec16Arg inV2) const;
+	JPH_INLINE bool				operator != (BVec16Arg inV2) const					{ return !(*this == inV2); }
+
+	/// Vector with all zeros
+	static JPH_INLINE BVec16	sZero();
+
+	/// Replicate int inV across all components
+	static JPH_INLINE BVec16	sReplicate(uint8 inV);
+
+	/// Load 16 bytes from memory
+	static JPH_INLINE BVec16	sLoadByte16(const uint8 *inV);
+
+	/// Equals (component wise), highest bit of each component that is set is considered true
+	static JPH_INLINE BVec16	sEquals(BVec16Arg inV1, BVec16Arg inV2);
+
+	/// Logical or (component wise)
+	static JPH_INLINE BVec16	sOr(BVec16Arg inV1, BVec16Arg inV2);
+
+	/// Logical xor (component wise)
+	static JPH_INLINE BVec16	sXor(BVec16Arg inV1, BVec16Arg inV2);
+
+	/// Logical and (component wise)
+	static JPH_INLINE BVec16	sAnd(BVec16Arg inV1, BVec16Arg inV2);
+
+	/// Logical not (component wise)
+	static JPH_INLINE BVec16	sNot(BVec16Arg inV1);
+
+	/// Get component by index
+	JPH_INLINE uint8			operator [] (uint inCoordinate) const				{ JPH_ASSERT(inCoordinate < 16); return mU8[inCoordinate]; }
+	JPH_INLINE uint8 &			operator [] (uint inCoordinate)						{ JPH_ASSERT(inCoordinate < 16); return mU8[inCoordinate]; }
+
+	/// Test if any of the components are true (true is when highest bit of component is set)
+	JPH_INLINE bool				TestAnyTrue() const;
+
+	/// Test if all components are true (true is when highest bit of component is set)
+	JPH_INLINE bool				TestAllTrue() const;
+
+	/// Store if mU8[0] is true in bit 0, mU8[1] in bit 1, etc. (true is when highest bit of component is set)
+	JPH_INLINE int				GetTrues() const;
+
+	/// To String
+	friend ostream &			operator << (ostream &inStream, BVec16Arg inV)
+	{
+		inStream << uint(inV.mU8[0]) << ", " << uint(inV.mU8[1]) << ", " << uint(inV.mU8[2]) << ", " << uint(inV.mU8[3]) << ", "
+				 << uint(inV.mU8[4]) << ", " << uint(inV.mU8[5]) << ", " << uint(inV.mU8[6]) << ", " << uint(inV.mU8[7]) << ", "
+				 << uint(inV.mU8[8]) << ", " << uint(inV.mU8[9]) << ", " << uint(inV.mU8[10]) << ", " << uint(inV.mU8[11]) << ", "
+				 << uint(inV.mU8[12]) << ", " << uint(inV.mU8[13]) << ", " << uint(inV.mU8[14]) << ", " << uint(inV.mU8[15]);
+		return inStream;
+	}
+
+	union
+	{
+		Type					mValue;
+		uint8					mU8[16];
+		uint64					mU64[2];
+	};
+};
+
+static_assert(std::is_trivial<BVec16>(), "Is supposed to be a trivial type!");
+
+JPH_NAMESPACE_END
+
+#include "BVec16.inl"
diff --git a/thirdparty/jolt_physics/Jolt/Math/BVec16.inl b/thirdparty/jolt_physics/Jolt/Math/BVec16.inl
new file mode 100644
index 0000000000..91e063a013
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/BVec16.inl
@@ -0,0 +1,177 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+JPH_NAMESPACE_BEGIN
+
+BVec16::BVec16(uint8 inB0, uint8 inB1, uint8 inB2, uint8 inB3, uint8 inB4, uint8 inB5, uint8 inB6, uint8 inB7, uint8 inB8, uint8 inB9, uint8 inB10, uint8 inB11, uint8 inB12, uint8 inB13, uint8 inB14, uint8 inB15)
+{
+#if defined(JPH_USE_SSE)
+	mValue = _mm_set_epi8(char(inB15), char(inB14), char(inB13), char(inB12), char(inB11), char(inB10), char(inB9), char(inB8), char(inB7), char(inB6), char(inB5), char(inB4), char(inB3), char(inB2), char(inB1), char(inB0));
+#elif defined(JPH_USE_NEON)
+	uint8x8_t v1 = vcreate_u8(uint64(inB0) | (uint64(inB1) << 8) | (uint64(inB2) << 16) | (uint64(inB3) << 24) | (uint64(inB4) << 32) | (uint64(inB5) << 40) | (uint64(inB6) << 48) | (uint64(inB7) << 56));
+	uint8x8_t v2 = vcreate_u8(uint64(inB8) | (uint64(inB9) << 8) | (uint64(inB10) << 16) | (uint64(inB11) << 24) | (uint64(inB12) << 32) | (uint64(inB13) << 40) | (uint64(inB14) << 48) | (uint64(inB15) << 56));
+	mValue = vcombine_u8(v1, v2);
+#else
+	mU8[0] = inB0;
+	mU8[1] = inB1;
+	mU8[2] = inB2;
+	mU8[3] = inB3;
+	mU8[4] = inB4;
+	mU8[5] = inB5;
+	mU8[6] = inB6;
+	mU8[7] = inB7;
+	mU8[8] = inB8;
+	mU8[9] = inB9;
+	mU8[10] = inB10;
+	mU8[11] = inB11;
+	mU8[12] = inB12;
+	mU8[13] = inB13;
+	mU8[14] = inB14;
+	mU8[15] = inB15;
+#endif
+}
+
+BVec16::BVec16(uint64 inV0, uint64 inV1)
+{
+	mU64[0] = inV0;
+	mU64[1] = inV1;
+}
+
+bool BVec16::operator == (BVec16Arg inV2) const
+{
+	return sEquals(*this, inV2).TestAllTrue();
+}
+
+BVec16 BVec16::sZero()
+{
+#if defined(JPH_USE_SSE)
+	return _mm_setzero_si128();
+#elif defined(JPH_USE_NEON)
+	return vdupq_n_u8(0);
+#else
+	return BVec16(0, 0);
+#endif
+}
+
+BVec16 BVec16::sReplicate(uint8 inV)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_set1_epi8(char(inV));
+#elif defined(JPH_USE_NEON)
+	return vdupq_n_u8(inV);
+#else
+	uint64 v(inV);
+	v |= v << 8;
+	v |= v << 16;
+	v |= v << 32;
+	return BVec16(v, v);
+#endif
+}
+
+BVec16 BVec16::sLoadByte16(const uint8 *inV)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_loadu_si128(reinterpret_cast<const __m128i *>(inV));
+#elif defined(JPH_USE_NEON)
+	return vld1q_u8(inV);
+#else
+	return BVec16(inV[0], inV[1], inV[2], inV[3], inV[4], inV[5], inV[6], inV[7], inV[8], inV[9], inV[10], inV[11], inV[12], inV[13], inV[14], inV[15]);
+#endif
+}
+
+BVec16 BVec16::sEquals(BVec16Arg inV1, BVec16Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_cmpeq_epi8(inV1.mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vceqq_u8(inV1.mValue, inV2.mValue);
+#else
+	auto equals = [](uint64 inV1, uint64 inV2) {
+		uint64 r = inV1 ^ ~inV2; // Bits that are equal are 1
+		r &= r << 1; // Combine bit 0 through 1
+		r &= r << 2; // Combine bit 0 through 3
+		r &= r << 4; // Combine bit 0 through 7
+		r &= 0x8080808080808080UL; // Keep only the highest bit of each byte
+		return r;
+	};
+	return BVec16(equals(inV1.mU64[0], inV2.mU64[0]), equals(inV1.mU64[1], inV2.mU64[1]));
+#endif
+}
+
+BVec16 BVec16::sOr(BVec16Arg inV1, BVec16Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_or_si128(inV1.mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vorrq_u8(inV1.mValue, inV2.mValue);
+#else
+	return BVec16(inV1.mU64[0] | inV2.mU64[0], inV1.mU64[1] | inV2.mU64[1]);
+#endif
+}
+
+BVec16 BVec16::sXor(BVec16Arg inV1, BVec16Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_xor_si128(inV1.mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return veorq_u8(inV1.mValue, inV2.mValue);
+#else
+	return BVec16(inV1.mU64[0] ^ inV2.mU64[0], inV1.mU64[1] ^ inV2.mU64[1]);
+#endif
+}
+
+BVec16 BVec16::sAnd(BVec16Arg inV1, BVec16Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_and_si128(inV1.mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vandq_u8(inV1.mValue, inV2.mValue);
+#else
+	return BVec16(inV1.mU64[0] & inV2.mU64[0], inV1.mU64[1] & inV2.mU64[1]);
+#endif
+}
+
+
+BVec16 BVec16::sNot(BVec16Arg inV1)
+{
+#if defined(JPH_USE_SSE)
+	return sXor(inV1, sReplicate(0xff));
+#elif defined(JPH_USE_NEON)
+	return vmvnq_u8(inV1.mValue);
+#else
+	return BVec16(~inV1.mU64[0], ~inV1.mU64[1]);
+#endif
+}
+
+int BVec16::GetTrues() const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_movemask_epi8(mValue);
+#else
+	int result = 0;
+	for (int i = 0; i < 16; ++i)
+		result |= int(mU8[i] >> 7) << i;
+	return result;
+#endif
+}
+
+bool BVec16::TestAnyTrue() const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_movemask_epi8(mValue) != 0;
+#else
+	return ((mU64[0] | mU64[1]) & 0x8080808080808080UL) != 0;
+#endif
+}
+
+bool BVec16::TestAllTrue() const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_movemask_epi8(mValue) == 0b1111111111111111;
+#else
+	return ((mU64[0] & mU64[1]) & 0x8080808080808080UL) == 0x8080808080808080UL;
+#endif
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Math/DMat44.h b/thirdparty/jolt_physics/Jolt/Math/DMat44.h
new file mode 100644
index 0000000000..cd8042107a
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/DMat44.h
@@ -0,0 +1,158 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Math/MathTypes.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Holds a 4x4 matrix of floats with the last column consisting of doubles
+class [[nodiscard]] alignas(JPH_DVECTOR_ALIGNMENT) DMat44
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	// Underlying column type
+	using Type = Vec4::Type;
+	using DType = DVec3::Type;
+	using DTypeArg = DVec3::TypeArg;
+
+	// Argument type
+	using ArgType = DMat44Arg;
+
+	/// Constructor
+								DMat44() = default; ///< Intentionally not initialized for performance reasons
+	JPH_INLINE					DMat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, DVec3Arg inC4);
+								DMat44(const DMat44 &inM2) = default;
+	DMat44 &					operator = (const DMat44 &inM2) = default;
+	JPH_INLINE explicit			DMat44(Mat44Arg inM);
+	JPH_INLINE					DMat44(Mat44Arg inRot, DVec3Arg inT);
+	JPH_INLINE					DMat44(Type inC1, Type inC2, Type inC3, DTypeArg inC4);
+
+	/// Zero matrix
+	static JPH_INLINE DMat44	sZero();
+
+	/// Identity matrix
+	static JPH_INLINE DMat44	sIdentity();
+
+	/// Rotate from quaternion
+	static JPH_INLINE DMat44	sRotation(QuatArg inQuat)								{ return DMat44(Mat44::sRotation(inQuat), DVec3::sZero()); }
+
+	/// Get matrix that translates
+	static JPH_INLINE DMat44	sTranslation(DVec3Arg inV)								{ return DMat44(Vec4(1, 0, 0, 0), Vec4(0, 1, 0, 0), Vec4(0, 0, 1, 0), inV); }
+
+	/// Get matrix that rotates and translates
+	static JPH_INLINE DMat44	sRotationTranslation(QuatArg inR, DVec3Arg inT)			{ return DMat44(Mat44::sRotation(inR), inT); }
+
+	/// Get inverse matrix of sRotationTranslation
+	static JPH_INLINE DMat44	sInverseRotationTranslation(QuatArg inR, DVec3Arg inT);
+
+	/// Get matrix that scales (produces a matrix with (inV, 1) on its diagonal)
+	static JPH_INLINE DMat44	sScale(Vec3Arg inV)										{ return DMat44(Mat44::sScale(inV), DVec3::sZero()); }
+
+	/// Convert to Mat44 rounding to nearest
+	JPH_INLINE Mat44			ToMat44() const											{ return Mat44(mCol[0], mCol[1], mCol[2], Vec3(mCol3)); }
+
+	/// Comparison
+	JPH_INLINE bool				operator == (DMat44Arg inM2) const;
+	JPH_INLINE bool				operator != (DMat44Arg inM2) const						{ return !(*this == inM2); }
+
+	/// Test if two matrices are close
+	JPH_INLINE bool				IsClose(DMat44Arg inM2, float inMaxDistSq = 1.0e-12f) const;
+
+	/// Multiply matrix by matrix
+	JPH_INLINE DMat44			operator * (Mat44Arg inM) const;
+
+	/// Multiply matrix by matrix
+	JPH_INLINE DMat44			operator * (DMat44Arg inM) const;
+
+	/// Multiply vector by matrix
+	JPH_INLINE DVec3			operator * (Vec3Arg inV) const;
+
+	/// Multiply vector by matrix
+	JPH_INLINE DVec3			operator * (DVec3Arg inV) const;
+
+	/// Multiply vector by only 3x3 part of the matrix
+	JPH_INLINE Vec3				Multiply3x3(Vec3Arg inV) const							{ return GetRotation().Multiply3x3(inV); }
+
+	/// Multiply vector by only 3x3 part of the matrix
+	JPH_INLINE DVec3			Multiply3x3(DVec3Arg inV) const;
+
+	/// Multiply vector by only 3x3 part of the transpose of the matrix (\f$result = this^T \: inV\f$)
+	JPH_INLINE Vec3				Multiply3x3Transposed(Vec3Arg inV) const				{ return GetRotation().Multiply3x3Transposed(inV); }
+
+	/// Scale a matrix: result = this * Mat44::sScale(inScale)
+	JPH_INLINE DMat44			PreScaled(Vec3Arg inScale) const;
+
+	/// Scale a matrix: result = Mat44::sScale(inScale) * this
+	JPH_INLINE DMat44			PostScaled(Vec3Arg inScale) const;
+
+	/// Pre multiply by translation matrix: result = this * Mat44::sTranslation(inTranslation)
+	JPH_INLINE DMat44			PreTranslated(Vec3Arg inTranslation) const;
+
+	/// Pre multiply by translation matrix: result = this * Mat44::sTranslation(inTranslation)
+	JPH_INLINE DMat44			PreTranslated(DVec3Arg inTranslation) const;
+
+	/// Post multiply by translation matrix: result = Mat44::sTranslation(inTranslation) * this (i.e. add inTranslation to the 4-th column)
+	JPH_INLINE DMat44			PostTranslated(Vec3Arg inTranslation) const;
+
+	/// Post multiply by translation matrix: result = Mat44::sTranslation(inTranslation) * this (i.e. add inTranslation to the 4-th column)
+	JPH_INLINE DMat44			PostTranslated(DVec3Arg inTranslation) const;
+
+	/// Access to the columns
+	JPH_INLINE Vec3				GetAxisX() const										{ return Vec3(mCol[0]); }
+	JPH_INLINE void				SetAxisX(Vec3Arg inV)									{ mCol[0] = Vec4(inV, 0.0f); }
+	JPH_INLINE Vec3				GetAxisY() const										{ return Vec3(mCol[1]); }
+	JPH_INLINE void				SetAxisY(Vec3Arg inV)									{ mCol[1] = Vec4(inV, 0.0f); }
+	JPH_INLINE Vec3				GetAxisZ() const										{ return Vec3(mCol[2]); }
+	JPH_INLINE void				SetAxisZ(Vec3Arg inV)									{ mCol[2] = Vec4(inV, 0.0f); }
+	JPH_INLINE DVec3			GetTranslation() const									{ return mCol3; }
+	JPH_INLINE void				SetTranslation(DVec3Arg inV)							{ mCol3 = inV; }
+	JPH_INLINE Vec3				GetColumn3(uint inCol) const							{ JPH_ASSERT(inCol < 3); return Vec3(mCol[inCol]); }
+	JPH_INLINE void				SetColumn3(uint inCol, Vec3Arg inV)						{ JPH_ASSERT(inCol < 3); mCol[inCol] = Vec4(inV, 0.0f); }
+	JPH_INLINE Vec4				GetColumn4(uint inCol) const							{ JPH_ASSERT(inCol < 3); return mCol[inCol]; }
+	JPH_INLINE void				SetColumn4(uint inCol, Vec4Arg inV)						{ JPH_ASSERT(inCol < 3); mCol[inCol] = inV; }
+
+	/// Transpose 3x3 subpart of matrix
+	JPH_INLINE Mat44			Transposed3x3() const									{ return GetRotation().Transposed3x3(); }
+
+	/// Inverse 4x4 matrix
+	JPH_INLINE DMat44			Inversed() const;
+
+	/// Inverse 4x4 matrix when it only contains rotation and translation
+	JPH_INLINE DMat44			InversedRotationTranslation() const;
+
+	/// Get rotation part only (note: retains the first 3 values from the bottom row)
+	JPH_INLINE Mat44			GetRotation() const										{ return Mat44(mCol[0], mCol[1], mCol[2], Vec4(0, 0, 0, 1)); }
+
+	/// Updates the rotation part of this matrix (the first 3 columns)
+	JPH_INLINE void				SetRotation(Mat44Arg inRotation);
+
+	/// Convert to quaternion
+	JPH_INLINE Quat				GetQuaternion() const									{ return GetRotation().GetQuaternion(); }
+
+	/// Get matrix that transforms a direction with the same transform as this matrix (length is not preserved)
+	JPH_INLINE Mat44			GetDirectionPreservingMatrix() const					{ return GetRotation().Inversed3x3().Transposed3x3(); }
+
+	/// Works identical to Mat44::Decompose
+	JPH_INLINE DMat44			Decompose(Vec3 &outScale) const							{ return DMat44(GetRotation().Decompose(outScale), mCol3); }
+
+	/// To String
+	friend ostream &			operator << (ostream &inStream, DMat44Arg inM)
+	{
+		inStream << inM.mCol[0] << ", " << inM.mCol[1] << ", " << inM.mCol[2] << ", " << inM.mCol3;
+		return inStream;
+	}
+
+private:
+	Vec4						mCol[3];												///< Rotation columns
+	DVec3						mCol3;													///< Translation column, 4th element is assumed to be 1
+};
+
+static_assert(std::is_trivial<DMat44>(), "Is supposed to be a trivial type!");
+
+JPH_NAMESPACE_END
+
+#include "DMat44.inl"
diff --git a/thirdparty/jolt_physics/Jolt/Math/DMat44.inl b/thirdparty/jolt_physics/Jolt/Math/DMat44.inl
new file mode 100644
index 0000000000..462cf79114
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/DMat44.inl
@@ -0,0 +1,310 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Math/DVec3.h>
+
+JPH_NAMESPACE_BEGIN
+
+DMat44::DMat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, DVec3Arg inC4) :
+	mCol { inC1, inC2, inC3 },
+	mCol3(inC4)
+{
+}
+
+DMat44::DMat44(Type inC1, Type inC2, Type inC3, DTypeArg inC4) :
+	mCol { inC1, inC2, inC3 },
+	mCol3(inC4)
+{
+}
+
+DMat44::DMat44(Mat44Arg inM) :
+	mCol { inM.GetColumn4(0), inM.GetColumn4(1), inM.GetColumn4(2) },
+	mCol3(inM.GetTranslation())
+{
+}
+
+DMat44::DMat44(Mat44Arg inRot, DVec3Arg inT) :
+	mCol { inRot.GetColumn4(0), inRot.GetColumn4(1), inRot.GetColumn4(2) },
+	mCol3(inT)
+{
+}
+
+DMat44 DMat44::sZero()
+{
+	return DMat44(Vec4::sZero(), Vec4::sZero(), Vec4::sZero(), DVec3::sZero());
+}
+
+DMat44 DMat44::sIdentity()
+{
+	return DMat44(Vec4(1, 0, 0, 0), Vec4(0, 1, 0, 0), Vec4(0, 0, 1, 0), DVec3::sZero());
+}
+
+DMat44 DMat44::sInverseRotationTranslation(QuatArg inR, DVec3Arg inT)
+{
+	Mat44 m = Mat44::sRotation(inR.Conjugated());
+	DMat44 dm(m, DVec3::sZero());
+	dm.SetTranslation(-dm.Multiply3x3(inT));
+	return dm;
+}
+
+bool DMat44::operator == (DMat44Arg inM2) const
+{
+	return mCol[0] == inM2.mCol[0]
+		&& mCol[1] == inM2.mCol[1]
+		&& mCol[2] == inM2.mCol[2]
+		&& mCol3 == inM2.mCol3;
+}
+
+bool DMat44::IsClose(DMat44Arg inM2, float inMaxDistSq) const
+{
+	for (int i = 0; i < 3; ++i)
+		if (!mCol[i].IsClose(inM2.mCol[i], inMaxDistSq))
+			return false;
+	return mCol3.IsClose(inM2.mCol3, double(inMaxDistSq));
+}
+
+DVec3 DMat44::operator * (Vec3Arg inV) const
+{
+#if defined(JPH_USE_AVX)
+	__m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0)));
+	t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1))));
+	t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2))));
+	return DVec3::sFixW(_mm256_add_pd(mCol3.mValue, _mm256_cvtps_pd(t)));
+#elif defined(JPH_USE_SSE)
+	__m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0)));
+	t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1))));
+	t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2))));
+	__m128d low = _mm_add_pd(mCol3.mValue.mLow, _mm_cvtps_pd(t));
+	__m128d high = _mm_add_pd(mCol3.mValue.mHigh, _mm_cvtps_pd(_mm_shuffle_ps(t, t, _MM_SHUFFLE(2, 2, 2, 2))));
+	return DVec3({ low, high });
+#elif defined(JPH_USE_NEON)
+	float32x4_t t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(inV.mValue, 0));
+	t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(inV.mValue, 1));
+	t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(inV.mValue, 2));
+	float64x2_t low = vaddq_f64(mCol3.mValue.val[0], vcvt_f64_f32(vget_low_f32(t)));
+	float64x2_t high = vaddq_f64(mCol3.mValue.val[1], vcvt_high_f64_f32(t));
+	return DVec3::sFixW({ low, high });
+#else
+	return DVec3(
+		mCol3.mF64[0] + double(mCol[0].mF32[0] * inV.mF32[0] + mCol[1].mF32[0] * inV.mF32[1] + mCol[2].mF32[0] * inV.mF32[2]),
+		mCol3.mF64[1] + double(mCol[0].mF32[1] * inV.mF32[0] + mCol[1].mF32[1] * inV.mF32[1] + mCol[2].mF32[1] * inV.mF32[2]),
+		mCol3.mF64[2] + double(mCol[0].mF32[2] * inV.mF32[0] + mCol[1].mF32[2] * inV.mF32[1] + mCol[2].mF32[2] * inV.mF32[2]));
+#endif
+}
+
+DVec3 DMat44::operator * (DVec3Arg inV) const
+{
+#if defined(JPH_USE_AVX)
+	__m256d t = _mm256_add_pd(mCol3.mValue, _mm256_mul_pd(_mm256_cvtps_pd(mCol[0].mValue), _mm256_set1_pd(inV.mF64[0])));
+	t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[1].mValue), _mm256_set1_pd(inV.mF64[1])));
+	t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[2].mValue), _mm256_set1_pd(inV.mF64[2])));
+	return DVec3::sFixW(t);
+#elif defined(JPH_USE_SSE)
+	__m128d xxxx = _mm_set1_pd(inV.mF64[0]);
+	__m128d yyyy = _mm_set1_pd(inV.mF64[1]);
+	__m128d zzzz = _mm_set1_pd(inV.mF64[2]);
+	__m128 col0 = mCol[0].mValue;
+	__m128 col1 = mCol[1].mValue;
+	__m128 col2 = mCol[2].mValue;
+	__m128d t_low = _mm_add_pd(mCol3.mValue.mLow, _mm_mul_pd(_mm_cvtps_pd(col0), xxxx));
+	t_low = _mm_add_pd(t_low, _mm_mul_pd(_mm_cvtps_pd(col1), yyyy));
+	t_low = _mm_add_pd(t_low, _mm_mul_pd(_mm_cvtps_pd(col2), zzzz));
+	__m128d t_high = _mm_add_pd(mCol3.mValue.mHigh, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col0, col0, _MM_SHUFFLE(2, 2, 2, 2))), xxxx));
+	t_high = _mm_add_pd(t_high, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col1, col1, _MM_SHUFFLE(2, 2, 2, 2))), yyyy));
+	t_high = _mm_add_pd(t_high, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col2, col2, _MM_SHUFFLE(2, 2, 2, 2))), zzzz));
+	return DVec3({ t_low, t_high });
+#elif defined(JPH_USE_NEON)
+	float64x2_t xxxx = vdupq_laneq_f64(inV.mValue.val[0], 0);
+	float64x2_t yyyy = vdupq_laneq_f64(inV.mValue.val[0], 1);
+	float64x2_t zzzz = vdupq_laneq_f64(inV.mValue.val[1], 0);
+	float32x4_t col0 = mCol[0].mValue;
+	float32x4_t col1 = mCol[1].mValue;
+	float32x4_t col2 = mCol[2].mValue;
+	float64x2_t t_low = vaddq_f64(mCol3.mValue.val[0], vmulq_f64(vcvt_f64_f32(vget_low_f32(col0)), xxxx));
+	t_low = vaddq_f64(t_low, vmulq_f64(vcvt_f64_f32(vget_low_f32(col1)), yyyy));
+	t_low = vaddq_f64(t_low, vmulq_f64(vcvt_f64_f32(vget_low_f32(col2)), zzzz));
+	float64x2_t t_high = vaddq_f64(mCol3.mValue.val[1], vmulq_f64(vcvt_high_f64_f32(col0), xxxx));
+	t_high = vaddq_f64(t_high, vmulq_f64(vcvt_high_f64_f32(col1), yyyy));
+	t_high = vaddq_f64(t_high, vmulq_f64(vcvt_high_f64_f32(col2), zzzz));
+	return DVec3::sFixW({ t_low, t_high });
+#else
+	return DVec3(
+		mCol3.mF64[0] + double(mCol[0].mF32[0]) * inV.mF64[0] + double(mCol[1].mF32[0]) * inV.mF64[1] + double(mCol[2].mF32[0]) * inV.mF64[2],
+		mCol3.mF64[1] + double(mCol[0].mF32[1]) * inV.mF64[0] + double(mCol[1].mF32[1]) * inV.mF64[1] + double(mCol[2].mF32[1]) * inV.mF64[2],
+		mCol3.mF64[2] + double(mCol[0].mF32[2]) * inV.mF64[0] + double(mCol[1].mF32[2]) * inV.mF64[1] + double(mCol[2].mF32[2]) * inV.mF64[2]);
+#endif
+}
+
+DVec3 DMat44::Multiply3x3(DVec3Arg inV) const
+{
+#if defined(JPH_USE_AVX)
+	__m256d t = _mm256_mul_pd(_mm256_cvtps_pd(mCol[0].mValue), _mm256_set1_pd(inV.mF64[0]));
+	t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[1].mValue), _mm256_set1_pd(inV.mF64[1])));
+	t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[2].mValue), _mm256_set1_pd(inV.mF64[2])));
+	return DVec3::sFixW(t);
+#elif defined(JPH_USE_SSE)
+	__m128d xxxx = _mm_set1_pd(inV.mF64[0]);
+	__m128d yyyy = _mm_set1_pd(inV.mF64[1]);
+	__m128d zzzz = _mm_set1_pd(inV.mF64[2]);
+	__m128 col0 = mCol[0].mValue;
+	__m128 col1 = mCol[1].mValue;
+	__m128 col2 = mCol[2].mValue;
+	__m128d t_low = _mm_mul_pd(_mm_cvtps_pd(col0), xxxx);
+	t_low = _mm_add_pd(t_low, _mm_mul_pd(_mm_cvtps_pd(col1), yyyy));
+	t_low = _mm_add_pd(t_low, _mm_mul_pd(_mm_cvtps_pd(col2), zzzz));
+	__m128d t_high = _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col0, col0, _MM_SHUFFLE(2, 2, 2, 2))), xxxx);
+	t_high = _mm_add_pd(t_high, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col1, col1, _MM_SHUFFLE(2, 2, 2, 2))), yyyy));
+	t_high = _mm_add_pd(t_high, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col2, col2, _MM_SHUFFLE(2, 2, 2, 2))), zzzz));
+	return DVec3({ t_low, t_high });
+#elif defined(JPH_USE_NEON)
+	float64x2_t xxxx = vdupq_laneq_f64(inV.mValue.val[0], 0);
+	float64x2_t yyyy = vdupq_laneq_f64(inV.mValue.val[0], 1);
+	float64x2_t zzzz = vdupq_laneq_f64(inV.mValue.val[1], 0);
+	float32x4_t col0 = mCol[0].mValue;
+	float32x4_t col1 = mCol[1].mValue;
+	float32x4_t col2 = mCol[2].mValue;
+	float64x2_t t_low = vmulq_f64(vcvt_f64_f32(vget_low_f32(col0)), xxxx);
+	t_low = vaddq_f64(t_low, vmulq_f64(vcvt_f64_f32(vget_low_f32(col1)), yyyy));
+	t_low = vaddq_f64(t_low, vmulq_f64(vcvt_f64_f32(vget_low_f32(col2)), zzzz));
+	float64x2_t t_high = vmulq_f64(vcvt_high_f64_f32(col0), xxxx);
+	t_high = vaddq_f64(t_high, vmulq_f64(vcvt_high_f64_f32(col1), yyyy));
+	t_high = vaddq_f64(t_high, vmulq_f64(vcvt_high_f64_f32(col2), zzzz));
+	return DVec3::sFixW({ t_low, t_high });
+#else
+	return DVec3(
+		double(mCol[0].mF32[0]) * inV.mF64[0] + double(mCol[1].mF32[0]) * inV.mF64[1] + double(mCol[2].mF32[0]) * inV.mF64[2],
+		double(mCol[0].mF32[1]) * inV.mF64[0] + double(mCol[1].mF32[1]) * inV.mF64[1] + double(mCol[2].mF32[1]) * inV.mF64[2],
+		double(mCol[0].mF32[2]) * inV.mF64[0] + double(mCol[1].mF32[2]) * inV.mF64[1] + double(mCol[2].mF32[2]) * inV.mF64[2]);
+#endif
+}
+
+DMat44 DMat44::operator * (Mat44Arg inM) const
+{
+	DMat44 result;
+
+	// Rotation part
+#if defined(JPH_USE_SSE)
+	for (int i = 0; i < 3; ++i)
+	{
+		__m128 c = inM.GetColumn4(i).mValue;
+		__m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0)));
+		t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1))));
+		t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2))));
+		result.mCol[i].mValue = t;
+	}
+#elif defined(JPH_USE_NEON)
+	for (int i = 0; i < 3; ++i)
+	{
+		Type c = inM.GetColumn4(i).mValue;
+		Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(c, 0));
+		t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(c, 1));
+		t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(c, 2));
+		result.mCol[i].mValue = t;
+	}
+#else
+	for (int i = 0; i < 3; ++i)
+	{
+		Vec4 coli = inM.GetColumn4(i);
+		result.mCol[i] = mCol[0] * coli.mF32[0] + mCol[1] * coli.mF32[1] + mCol[2] * coli.mF32[2];
+	}
+#endif
+
+	// Translation part
+	result.mCol3 = *this * inM.GetTranslation();
+
+	return result;
+}
+
+DMat44 DMat44::operator * (DMat44Arg inM) const
+{
+	DMat44 result;
+
+	// Rotation part
+#if defined(JPH_USE_SSE)
+	for (int i = 0; i < 3; ++i)
+	{
+		__m128 c = inM.mCol[i].mValue;
+		__m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0)));
+		t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1))));
+		t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2))));
+		result.mCol[i].mValue = t;
+	}
+#elif defined(JPH_USE_NEON)
+	for (int i = 0; i < 3; ++i)
+	{
+		Type c = inM.GetColumn4(i).mValue;
+		Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(c, 0));
+		t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(c, 1));
+		t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(c, 2));
+		result.mCol[i].mValue = t;
+	}
+#else
+	for (int i = 0; i < 3; ++i)
+	{
+		Vec4 coli = inM.mCol[i];
+		result.mCol[i] = mCol[0] * coli.mF32[0] + mCol[1] * coli.mF32[1] + mCol[2] * coli.mF32[2];
+	}
+#endif
+
+	// Translation part
+	result.mCol3 = *this * inM.GetTranslation();
+
+	return result;
+}
+
+void DMat44::SetRotation(Mat44Arg inRotation)
+{
+	mCol[0] = inRotation.GetColumn4(0);
+	mCol[1] = inRotation.GetColumn4(1);
+	mCol[2] = inRotation.GetColumn4(2);
+}
+
+DMat44 DMat44::PreScaled(Vec3Arg inScale) const
+{
+	return DMat44(inScale.GetX() * mCol[0], inScale.GetY() * mCol[1], inScale.GetZ() * mCol[2], mCol3);
+}
+
+DMat44 DMat44::PostScaled(Vec3Arg inScale) const
+{
+	Vec4 scale(inScale, 1);
+	return DMat44(scale * mCol[0], scale * mCol[1], scale * mCol[2], DVec3(scale) * mCol3);
+}
+
+DMat44 DMat44::PreTranslated(Vec3Arg inTranslation) const
+{
+	return DMat44(mCol[0], mCol[1], mCol[2], GetTranslation() + Multiply3x3(inTranslation));
+}
+
+DMat44 DMat44::PreTranslated(DVec3Arg inTranslation) const
+{
+	return DMat44(mCol[0], mCol[1], mCol[2], GetTranslation() + Multiply3x3(inTranslation));
+}
+
+DMat44 DMat44::PostTranslated(Vec3Arg inTranslation) const
+{
+	return DMat44(mCol[0], mCol[1], mCol[2], GetTranslation() + inTranslation);
+}
+
+DMat44 DMat44::PostTranslated(DVec3Arg inTranslation) const
+{
+	return DMat44(mCol[0], mCol[1], mCol[2], GetTranslation() + inTranslation);
+}
+
+DMat44 DMat44::Inversed() const
+{
+	DMat44 m(GetRotation().Inversed3x3());
+	m.mCol3 = -m.Multiply3x3(mCol3);
+	return m;
+}
+
+DMat44 DMat44::InversedRotationTranslation() const
+{
+	DMat44 m(GetRotation().Transposed3x3());
+	m.mCol3 = -m.Multiply3x3(mCol3);
+	return m;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Math/DVec3.h b/thirdparty/jolt_physics/Jolt/Math/DVec3.h
new file mode 100644
index 0000000000..74d209b2a8
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/DVec3.h
@@ -0,0 +1,288 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Math/Double3.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// 3 component vector of doubles (stored as 4 vectors).
+/// Note that we keep the 4th component the same as the 3rd component to avoid divisions by zero when JPH_FLOATING_POINT_EXCEPTIONS_ENABLED defined
+class [[nodiscard]] alignas(JPH_DVECTOR_ALIGNMENT) DVec3
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	// Underlying vector type
+#if defined(JPH_USE_AVX)
+	using Type = __m256d;
+	using TypeArg = __m256d;
+#elif defined(JPH_USE_SSE)
+	using Type = struct { __m128d mLow, mHigh; };
+	using TypeArg = const Type &;
+#elif defined(JPH_USE_NEON)
+	using Type = float64x2x2_t;
+	using TypeArg = const Type &;
+#else
+	using Type = struct { double mData[4]; };
+	using TypeArg = const Type &;
+#endif
+
+	// Argument type
+	using ArgType = DVec3Arg;
+
+	/// Constructor
+								DVec3() = default; ///< Intentionally not initialized for performance reasons
+								DVec3(const DVec3 &inRHS) = default;
+	DVec3 &						operator = (const DVec3 &inRHS) = default;
+	JPH_INLINE explicit			DVec3(Vec3Arg inRHS);
+	JPH_INLINE explicit			DVec3(Vec4Arg inRHS);
+	JPH_INLINE					DVec3(TypeArg inRHS) : mValue(inRHS)			{ CheckW(); }
+
+	/// Create a vector from 3 components
+	JPH_INLINE					DVec3(double inX, double inY, double inZ);
+
+	/// Load 3 doubles from memory
+	explicit JPH_INLINE			DVec3(const Double3 &inV);
+
+	/// Vector with all zeros
+	static JPH_INLINE DVec3		sZero();
+
+	/// Vectors with the principal axis
+	static JPH_INLINE DVec3		sAxisX()										{ return DVec3(1, 0, 0); }
+	static JPH_INLINE DVec3		sAxisY()										{ return DVec3(0, 1, 0); }
+	static JPH_INLINE DVec3		sAxisZ()										{ return DVec3(0, 0, 1); }
+
+	/// Replicate inV across all components
+	static JPH_INLINE DVec3		sReplicate(double inV);
+
+	/// Vector with all NaN's
+	static JPH_INLINE DVec3		sNaN();
+
+	/// Load 3 doubles from memory (reads 64 bits extra which it doesn't use)
+	static JPH_INLINE DVec3		sLoadDouble3Unsafe(const Double3 &inV);
+
+	/// Store 3 doubles to memory
+	JPH_INLINE void				StoreDouble3(Double3 *outV) const;
+
+	/// Convert to float vector 3 rounding to nearest
+	JPH_INLINE explicit			operator Vec3() const;
+
+	/// Prepare to convert to float vector 3 rounding towards zero (returns DVec3 that can be converted to a Vec3 to get the rounding)
+	JPH_INLINE DVec3			PrepareRoundToZero() const;
+
+	/// Prepare to convert to float vector 3 rounding towards positive/negative inf (returns DVec3 that can be converted to a Vec3 to get the rounding)
+	JPH_INLINE DVec3			PrepareRoundToInf() const;
+
+	/// Convert to float vector 3 rounding down
+	JPH_INLINE Vec3				ToVec3RoundDown() const;
+
+	/// Convert to float vector 3 rounding up
+	JPH_INLINE Vec3				ToVec3RoundUp() const;
+
+	/// Return the minimum value of each of the components
+	static JPH_INLINE DVec3		sMin(DVec3Arg inV1, DVec3Arg inV2);
+
+	/// Return the maximum of each of the components
+	static JPH_INLINE DVec3		sMax(DVec3Arg inV1, DVec3Arg inV2);
+
+	/// Clamp a vector between min and max (component wise)
+	static JPH_INLINE DVec3		sClamp(DVec3Arg inV, DVec3Arg inMin, DVec3Arg inMax);
+
+	/// Equals (component wise)
+	static JPH_INLINE DVec3		sEquals(DVec3Arg inV1, DVec3Arg inV2);
+
+	/// Less than (component wise)
+	static JPH_INLINE DVec3		sLess(DVec3Arg inV1, DVec3Arg inV2);
+
+	/// Less than or equal (component wise)
+	static JPH_INLINE DVec3		sLessOrEqual(DVec3Arg inV1, DVec3Arg inV2);
+
+	/// Greater than (component wise)
+	static JPH_INLINE DVec3		sGreater(DVec3Arg inV1, DVec3Arg inV2);
+
+	/// Greater than or equal (component wise)
+	static JPH_INLINE DVec3		sGreaterOrEqual(DVec3Arg inV1, DVec3Arg inV2);
+
+	/// Calculates inMul1 * inMul2 + inAdd
+	static JPH_INLINE DVec3		sFusedMultiplyAdd(DVec3Arg inMul1, DVec3Arg inMul2, DVec3Arg inAdd);
+
+	/// Component wise select, returns inNotSet when highest bit of inControl = 0 and inSet when highest bit of inControl = 1
+	static JPH_INLINE DVec3		sSelect(DVec3Arg inNotSet, DVec3Arg inSet, DVec3Arg inControl);
+
+	/// Logical or (component wise)
+	static JPH_INLINE DVec3		sOr(DVec3Arg inV1, DVec3Arg inV2);
+
+	/// Logical xor (component wise)
+	static JPH_INLINE DVec3		sXor(DVec3Arg inV1, DVec3Arg inV2);
+
+	/// Logical and (component wise)
+	static JPH_INLINE DVec3		sAnd(DVec3Arg inV1, DVec3Arg inV2);
+
+	/// Store if X is true in bit 0, Y in bit 1, Z in bit 2 and W in bit 3 (true is when highest bit of component is set)
+	JPH_INLINE int				GetTrues() const;
+
+	/// Test if any of the components are true (true is when highest bit of component is set)
+	JPH_INLINE bool				TestAnyTrue() const;
+
+	/// Test if all components are true (true is when highest bit of component is set)
+	JPH_INLINE bool				TestAllTrue() const;
+
+	/// Get individual components
+#if defined(JPH_USE_AVX)
+	JPH_INLINE double			GetX() const									{ return _mm_cvtsd_f64(_mm256_castpd256_pd128(mValue)); }
+	JPH_INLINE double			GetY() const									{ return mF64[1]; }
+	JPH_INLINE double			GetZ() const									{ return mF64[2]; }
+#elif defined(JPH_USE_SSE)
+	JPH_INLINE double			GetX() const									{ return _mm_cvtsd_f64(mValue.mLow); }
+	JPH_INLINE double			GetY() const									{ return mF64[1]; }
+	JPH_INLINE double			GetZ() const									{ return _mm_cvtsd_f64(mValue.mHigh); }
+#elif defined(JPH_USE_NEON)
+	JPH_INLINE double			GetX() const									{ return vgetq_lane_f64(mValue.val[0], 0); }
+	JPH_INLINE double			GetY() const									{ return vgetq_lane_f64(mValue.val[0], 1); }
+	JPH_INLINE double			GetZ() const									{ return vgetq_lane_f64(mValue.val[1], 0); }
+#else
+	JPH_INLINE double			GetX() const									{ return mF64[0]; }
+	JPH_INLINE double			GetY() const									{ return mF64[1]; }
+	JPH_INLINE double			GetZ() const									{ return mF64[2]; }
+#endif
+
+	/// Set individual components
+	JPH_INLINE void				SetX(double inX)								{ mF64[0] = inX; }
+	JPH_INLINE void				SetY(double inY)								{ mF64[1] = inY; }
+	JPH_INLINE void				SetZ(double inZ)								{ mF64[2] = mF64[3] = inZ; } // Assure Z and W are the same
+
+	/// Set all components
+	JPH_INLINE void				Set(double inX, double inY, double inZ)			{ *this = DVec3(inX, inY, inZ); }
+
+	/// Get double component by index
+	JPH_INLINE double			operator [] (uint inCoordinate) const			{ JPH_ASSERT(inCoordinate < 3); return mF64[inCoordinate]; }
+
+	/// Set double component by index
+	JPH_INLINE void				SetComponent(uint inCoordinate, double inValue)	{ JPH_ASSERT(inCoordinate < 3); mF64[inCoordinate] = inValue; mValue = sFixW(mValue); } // Assure Z and W are the same
+
+	/// Comparison
+	JPH_INLINE bool				operator == (DVec3Arg inV2) const;
+	JPH_INLINE bool				operator != (DVec3Arg inV2) const				{ return !(*this == inV2); }
+
+	/// Test if two vectors are close
+	JPH_INLINE bool				IsClose(DVec3Arg inV2, double inMaxDistSq = 1.0e-24) const;
+
+	/// Test if vector is near zero
+	JPH_INLINE bool				IsNearZero(double inMaxDistSq = 1.0e-24) const;
+
+	/// Test if vector is normalized
+	JPH_INLINE bool				IsNormalized(double inTolerance = 1.0e-12) const;
+
+	/// Test if vector contains NaN elements
+	JPH_INLINE bool				IsNaN() const;
+
+	/// Multiply two double vectors (component wise)
+	JPH_INLINE DVec3			operator * (DVec3Arg inV2) const;
+
+	/// Multiply vector with double
+	JPH_INLINE DVec3			operator * (double inV2) const;
+
+	/// Multiply vector with double
+	friend JPH_INLINE DVec3		operator * (double inV1, DVec3Arg inV2);
+
+	/// Divide vector by double
+	JPH_INLINE DVec3			operator / (double inV2) const;
+
+	/// Multiply vector with double
+	JPH_INLINE DVec3 &			operator *= (double inV2);
+
+	/// Multiply vector with vector
+	JPH_INLINE DVec3 &			operator *= (DVec3Arg inV2);
+
+	/// Divide vector by double
+	JPH_INLINE DVec3 &			operator /= (double inV2);
+
+	/// Add two vectors (component wise)
+	JPH_INLINE DVec3			operator + (Vec3Arg inV2) const;
+
+	/// Add two double vectors (component wise)
+	JPH_INLINE DVec3			operator + (DVec3Arg inV2) const;
+
+	/// Add two vectors (component wise)
+	JPH_INLINE DVec3 &			operator += (Vec3Arg inV2);
+
+	/// Add two double vectors (component wise)
+	JPH_INLINE DVec3 &			operator += (DVec3Arg inV2);
+
+	/// Negate
+	JPH_INLINE DVec3			operator - () const;
+
+	/// Subtract two vectors (component wise)
+	JPH_INLINE DVec3			operator - (Vec3Arg inV2) const;
+
+	/// Subtract two double vectors (component wise)
+	JPH_INLINE DVec3			operator - (DVec3Arg inV2) const;
+
+	/// Subtract two vectors (component wise)
+	JPH_INLINE DVec3 &			operator -= (Vec3Arg inV2);
+
+	/// Subtract two vectors (component wise)
+	JPH_INLINE DVec3 &			operator -= (DVec3Arg inV2);
+
+	/// Divide (component wise)
+	JPH_INLINE DVec3			operator / (DVec3Arg inV2) const;
+
+	/// Return the absolute value of each of the components
+	JPH_INLINE DVec3			Abs() const;
+
+	/// Reciprocal vector (1 / value) for each of the components
+	JPH_INLINE DVec3			Reciprocal() const;
+
+	/// Cross product
+	JPH_INLINE DVec3			Cross(DVec3Arg inV2) const;
+
+	/// Dot product
+	JPH_INLINE double			Dot(DVec3Arg inV2) const;
+
+	/// Squared length of vector
+	JPH_INLINE double			LengthSq() const;
+
+	/// Length of vector
+	JPH_INLINE double			Length() const;
+
+	/// Normalize vector
+	JPH_INLINE DVec3			Normalized() const;
+
+	/// Component wise square root
+	JPH_INLINE DVec3			Sqrt() const;
+
+	/// Get vector that contains the sign of each element (returns 1 if positive, -1 if negative)
+	JPH_INLINE DVec3			GetSign() const;
+
+	/// To String
+	friend ostream &			operator << (ostream &inStream, DVec3Arg inV)
+	{
+		inStream << inV.mF64[0] << ", " << inV.mF64[1] << ", " << inV.mF64[2];
+		return inStream;
+	}
+
+	/// Internal helper function that checks that W is equal to Z, so e.g. dividing by it should not generate div by 0
+	JPH_INLINE void				CheckW() const;
+
+	/// Internal helper function that ensures that the Z component is replicated to the W component to prevent divisions by zero
+	static JPH_INLINE Type		sFixW(TypeArg inValue);
+
+	/// Representations of true and false for boolean operations
+	inline static const double	cTrue = BitCast<double>(~uint64(0));
+	inline static const double	cFalse = 0.0;
+
+	union
+	{
+		Type					mValue;
+		double					mF64[4];
+	};
+};
+
+static_assert(std::is_trivial<DVec3>(), "Is supposed to be a trivial type!");
+
+JPH_NAMESPACE_END
+
+#include "DVec3.inl"
diff --git a/thirdparty/jolt_physics/Jolt/Math/DVec3.inl b/thirdparty/jolt_physics/Jolt/Math/DVec3.inl
new file mode 100644
index 0000000000..eb0bcf4393
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/DVec3.inl
@@ -0,0 +1,936 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/HashCombine.h>
+
+// Create a std::hash/JPH::Hash for DVec3
+JPH_MAKE_HASHABLE(JPH::DVec3, t.GetX(), t.GetY(), t.GetZ())
+
+JPH_NAMESPACE_BEGIN
+
+DVec3::DVec3(Vec3Arg inRHS)
+{
+#if defined(JPH_USE_AVX)
+	mValue = _mm256_cvtps_pd(inRHS.mValue);
+#elif defined(JPH_USE_SSE)
+	mValue.mLow = _mm_cvtps_pd(inRHS.mValue);
+	mValue.mHigh = _mm_cvtps_pd(_mm_shuffle_ps(inRHS.mValue, inRHS.mValue, _MM_SHUFFLE(2, 2, 2, 2)));
+#elif defined(JPH_USE_NEON)
+	mValue.val[0] = vcvt_f64_f32(vget_low_f32(inRHS.mValue));
+	mValue.val[1] = vcvt_high_f64_f32(inRHS.mValue);
+#else
+	mF64[0] = (double)inRHS.GetX();
+	mF64[1] = (double)inRHS.GetY();
+	mF64[2] = (double)inRHS.GetZ();
+	#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+		mF64[3] = mF64[2];
+	#endif
+#endif
+}
+
+DVec3::DVec3(Vec4Arg inRHS) :
+	DVec3(Vec3(inRHS))
+{
+}
+
+DVec3::DVec3(double inX, double inY, double inZ)
+{
+#if defined(JPH_USE_AVX)
+	mValue = _mm256_set_pd(inZ, inZ, inY, inX); // Assure Z and W are the same
+#elif defined(JPH_USE_SSE)
+	mValue.mLow = _mm_set_pd(inY, inX);
+	mValue.mHigh = _mm_set1_pd(inZ);
+#elif defined(JPH_USE_NEON)
+	mValue.val[0] = vcombine_f64(vcreate_f64(BitCast<uint64>(inX)), vcreate_f64(BitCast<uint64>(inY)));
+	mValue.val[1] = vdupq_n_f64(inZ);
+#else
+	mF64[0] = inX;
+	mF64[1] = inY;
+	mF64[2] = inZ;
+	#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+		mF64[3] = mF64[2];
+	#endif
+#endif
+}
+
+DVec3::DVec3(const Double3 &inV)
+{
+#if defined(JPH_USE_AVX)
+	Type x = _mm256_castpd128_pd256(_mm_load_sd(&inV.x));
+	Type y = _mm256_castpd128_pd256(_mm_load_sd(&inV.y));
+	Type z = _mm256_broadcast_sd(&inV.z);
+	Type xy = _mm256_unpacklo_pd(x, y);
+	mValue = _mm256_blend_pd(xy, z, 0b1100); // Assure Z and W are the same
+#elif defined(JPH_USE_SSE)
+	mValue.mLow = _mm_loadu_pd(&inV.x);
+	mValue.mHigh = _mm_set1_pd(inV.z);
+#elif defined(JPH_USE_NEON)
+	mValue.val[0] = vld1q_f64(&inV.x);
+	mValue.val[1] = vdupq_n_f64(inV.z);
+#else
+	mF64[0] = inV.x;
+	mF64[1] = inV.y;
+	mF64[2] = inV.z;
+	#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+		mF64[3] = mF64[2];
+	#endif
+#endif
+}
+
+void DVec3::CheckW() const
+{
+#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+	// Avoid asserts when both components are NaN
+	JPH_ASSERT(reinterpret_cast<const uint64 *>(mF64)[2] == reinterpret_cast<const uint64 *>(mF64)[3]);
+#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+}
+
+/// Internal helper function that ensures that the Z component is replicated to the W component to prevent divisions by zero
+DVec3::Type DVec3::sFixW(TypeArg inValue)
+{
+#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+	#if defined(JPH_USE_AVX)
+		return _mm256_shuffle_pd(inValue, inValue, 2);
+	#elif defined(JPH_USE_SSE)
+		Type value;
+		value.mLow = inValue.mLow;
+		value.mHigh = _mm_shuffle_pd(inValue.mHigh, inValue.mHigh, 0);
+		return value;
+	#elif defined(JPH_USE_NEON)
+		Type value;
+		value.val[0] = inValue.val[0];
+		value.val[1] = vdupq_laneq_f64(inValue.val[1], 0);
+		return value;
+	#else
+		Type value;
+		value.mData[0] = inValue.mData[0];
+		value.mData[1] = inValue.mData[1];
+		value.mData[2] = inValue.mData[2];
+		value.mData[3] = inValue.mData[2];
+		return value;
+	#endif
+#else
+	return inValue;
+#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+}
+
+DVec3 DVec3::sZero()
+{
+#if defined(JPH_USE_AVX)
+	return _mm256_setzero_pd();
+#elif defined(JPH_USE_SSE)
+	__m128d zero = _mm_setzero_pd();
+	return DVec3({ zero, zero });
+#elif defined(JPH_USE_NEON)
+	float64x2_t zero = vdupq_n_f64(0.0);
+	return DVec3({ zero, zero });
+#else
+	return DVec3(0, 0, 0);
+#endif
+}
+
+DVec3 DVec3::sReplicate(double inV)
+{
+#if defined(JPH_USE_AVX)
+	return _mm256_set1_pd(inV);
+#elif defined(JPH_USE_SSE)
+	__m128d value = _mm_set1_pd(inV);
+	return DVec3({ value, value });
+#elif defined(JPH_USE_NEON)
+	float64x2_t value = vdupq_n_f64(inV);
+	return DVec3({ value, value });
+#else
+	return DVec3(inV, inV, inV);
+#endif
+}
+
+DVec3 DVec3::sNaN()
+{
+	return sReplicate(numeric_limits<double>::quiet_NaN());
+}
+
+DVec3 DVec3::sLoadDouble3Unsafe(const Double3 &inV)
+{
+#if defined(JPH_USE_AVX)
+	Type v = _mm256_loadu_pd(&inV.x);
+#elif defined(JPH_USE_SSE)
+	Type v;
+	v.mLow = _mm_loadu_pd(&inV.x);
+	v.mHigh = _mm_set1_pd(inV.z);
+#elif defined(JPH_USE_NEON)
+	Type v = vld1q_f64_x2(&inV.x);
+#else
+	Type v = { inV.x, inV.y, inV.z };
+#endif
+	return sFixW(v);
+}
+
+void DVec3::StoreDouble3(Double3 *outV) const
+{
+	outV->x = mF64[0];
+	outV->y = mF64[1];
+	outV->z = mF64[2];
+}
+
+DVec3::operator Vec3() const
+{
+#if defined(JPH_USE_AVX)
+	return _mm256_cvtpd_ps(mValue);
+#elif defined(JPH_USE_SSE)
+	__m128 low = _mm_cvtpd_ps(mValue.mLow);
+	__m128 high = _mm_cvtpd_ps(mValue.mHigh);
+	return _mm_shuffle_ps(low, high, _MM_SHUFFLE(1, 0, 1, 0));
+#elif defined(JPH_USE_NEON)
+	return vcvt_high_f32_f64(vcvtx_f32_f64(mValue.val[0]), mValue.val[1]);
+#else
+	return Vec3((float)GetX(), (float)GetY(), (float)GetZ());
+#endif
+}
+
+DVec3 DVec3::sMin(DVec3Arg inV1, DVec3Arg inV2)
+{
+#if defined(JPH_USE_AVX)
+	return _mm256_min_pd(inV1.mValue, inV2.mValue);
+#elif defined(JPH_USE_SSE)
+	return DVec3({ _mm_min_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_min_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
+#elif defined(JPH_USE_NEON)
+	return DVec3({ vminq_f64(inV1.mValue.val[0], inV2.mValue.val[0]), vminq_f64(inV1.mValue.val[1], inV2.mValue.val[1]) });
+#else
+	return DVec3(min(inV1.mF64[0], inV2.mF64[0]),
+				 min(inV1.mF64[1], inV2.mF64[1]),
+				 min(inV1.mF64[2], inV2.mF64[2]));
+#endif
+}
+
+DVec3 DVec3::sMax(DVec3Arg inV1, DVec3Arg inV2)
+{
+#if defined(JPH_USE_AVX)
+	return _mm256_max_pd(inV1.mValue, inV2.mValue);
+#elif defined(JPH_USE_SSE)
+	return DVec3({ _mm_max_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_max_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
+#elif defined(JPH_USE_NEON)
+	return DVec3({ vmaxq_f64(inV1.mValue.val[0], inV2.mValue.val[0]), vmaxq_f64(inV1.mValue.val[1], inV2.mValue.val[1]) });
+#else
+	return DVec3(max(inV1.mF64[0], inV2.mF64[0]),
+				 max(inV1.mF64[1], inV2.mF64[1]),
+				 max(inV1.mF64[2], inV2.mF64[2]));
+#endif
+}
+
+DVec3 DVec3::sClamp(DVec3Arg inV, DVec3Arg inMin, DVec3Arg inMax)
+{
+	return sMax(sMin(inV, inMax), inMin);
+}
+
+DVec3 DVec3::sEquals(DVec3Arg inV1, DVec3Arg inV2)
+{
+#if defined(JPH_USE_AVX)
+	return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_EQ_OQ);
+#elif defined(JPH_USE_SSE)
+	return DVec3({ _mm_cmpeq_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmpeq_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
+#elif defined(JPH_USE_NEON)
+	return DVec3({ vreinterpretq_f64_u64(vceqq_f64(inV1.mValue.val[0], inV2.mValue.val[0])), vreinterpretq_f64_u64(vceqq_f64(inV1.mValue.val[1], inV2.mValue.val[1])) });
+#else
+	return DVec3(inV1.mF64[0] == inV2.mF64[0]? cTrue : cFalse,
+				 inV1.mF64[1] == inV2.mF64[1]? cTrue : cFalse,
+				 inV1.mF64[2] == inV2.mF64[2]? cTrue : cFalse);
+#endif
+}
+
+DVec3 DVec3::sLess(DVec3Arg inV1, DVec3Arg inV2)
+{
+#if defined(JPH_USE_AVX)
+	return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_LT_OQ);
+#elif defined(JPH_USE_SSE)
+	return DVec3({ _mm_cmplt_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmplt_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
+#elif defined(JPH_USE_NEON)
+	return DVec3({ vreinterpretq_f64_u64(vcltq_f64(inV1.mValue.val[0], inV2.mValue.val[0])), vreinterpretq_f64_u64(vcltq_f64(inV1.mValue.val[1], inV2.mValue.val[1])) });
+#else
+	return DVec3(inV1.mF64[0] < inV2.mF64[0]? cTrue : cFalse,
+				 inV1.mF64[1] < inV2.mF64[1]? cTrue : cFalse,
+				 inV1.mF64[2] < inV2.mF64[2]? cTrue : cFalse);
+#endif
+}
+
+DVec3 DVec3::sLessOrEqual(DVec3Arg inV1, DVec3Arg inV2)
+{
+#if defined(JPH_USE_AVX)
+	return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_LE_OQ);
+#elif defined(JPH_USE_SSE)
+	return DVec3({ _mm_cmple_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmple_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
+#elif defined(JPH_USE_NEON)
+	return DVec3({ vreinterpretq_f64_u64(vcleq_f64(inV1.mValue.val[0], inV2.mValue.val[0])), vreinterpretq_f64_u64(vcleq_f64(inV1.mValue.val[1], inV2.mValue.val[1])) });
+#else
+	return DVec3(inV1.mF64[0] <= inV2.mF64[0]? cTrue : cFalse,
+				 inV1.mF64[1] <= inV2.mF64[1]? cTrue : cFalse,
+				 inV1.mF64[2] <= inV2.mF64[2]? cTrue : cFalse);
+#endif
+}
+
+DVec3 DVec3::sGreater(DVec3Arg inV1, DVec3Arg inV2)
+{
+#if defined(JPH_USE_AVX)
+	return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_GT_OQ);
+#elif defined(JPH_USE_SSE)
+	return DVec3({ _mm_cmpgt_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmpgt_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
+#elif defined(JPH_USE_NEON)
+	return DVec3({ vreinterpretq_f64_u64(vcgtq_f64(inV1.mValue.val[0], inV2.mValue.val[0])), vreinterpretq_f64_u64(vcgtq_f64(inV1.mValue.val[1], inV2.mValue.val[1])) });
+#else
+	return DVec3(inV1.mF64[0] > inV2.mF64[0]? cTrue : cFalse,
+				 inV1.mF64[1] > inV2.mF64[1]? cTrue : cFalse,
+				 inV1.mF64[2] > inV2.mF64[2]? cTrue : cFalse);
+#endif
+}
+
+DVec3 DVec3::sGreaterOrEqual(DVec3Arg inV1, DVec3Arg inV2)
+{
+#if defined(JPH_USE_AVX)
+	return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_GE_OQ);
+#elif defined(JPH_USE_SSE)
+	return DVec3({ _mm_cmpge_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmpge_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
+#elif defined(JPH_USE_NEON)
+	return DVec3({ vreinterpretq_f64_u64(vcgeq_f64(inV1.mValue.val[0], inV2.mValue.val[0])), vreinterpretq_f64_u64(vcgeq_f64(inV1.mValue.val[1], inV2.mValue.val[1])) });
+#else
+	return DVec3(inV1.mF64[0] >= inV2.mF64[0]? cTrue : cFalse,
+				 inV1.mF64[1] >= inV2.mF64[1]? cTrue : cFalse,
+				 inV1.mF64[2] >= inV2.mF64[2]? cTrue : cFalse);
+#endif
+}
+
+DVec3 DVec3::sFusedMultiplyAdd(DVec3Arg inMul1, DVec3Arg inMul2, DVec3Arg inAdd)
+{
+#if defined(JPH_USE_AVX)
+	#ifdef JPH_USE_FMADD
+		return _mm256_fmadd_pd(inMul1.mValue, inMul2.mValue, inAdd.mValue);
+	#else
+		return _mm256_add_pd(_mm256_mul_pd(inMul1.mValue, inMul2.mValue), inAdd.mValue);
+	#endif
+#elif defined(JPH_USE_NEON)
+	return DVec3({ vmlaq_f64(inAdd.mValue.val[0], inMul1.mValue.val[0], inMul2.mValue.val[0]), vmlaq_f64(inAdd.mValue.val[1], inMul1.mValue.val[1], inMul2.mValue.val[1]) });
+#else
+	return inMul1 * inMul2 + inAdd;
+#endif
+}
+
+DVec3 DVec3::sSelect(DVec3Arg inNotSet, DVec3Arg inSet, DVec3Arg inControl)
+{
+#if defined(JPH_USE_AVX)
+	return _mm256_blendv_pd(inNotSet.mValue, inSet.mValue, inControl.mValue);
+#elif defined(JPH_USE_SSE4_1)
+	Type v = { _mm_blendv_pd(inNotSet.mValue.mLow, inSet.mValue.mLow, inControl.mValue.mLow), _mm_blendv_pd(inNotSet.mValue.mHigh, inSet.mValue.mHigh, inControl.mValue.mHigh) };
+	return sFixW(v);
+#elif defined(JPH_USE_NEON)
+	Type v = { vbslq_f64(vreinterpretq_u64_s64(vshrq_n_s64(vreinterpretq_s64_f64(inControl.mValue.val[0]), 63)), inSet.mValue.val[0], inNotSet.mValue.val[0]),
+			   vbslq_f64(vreinterpretq_u64_s64(vshrq_n_s64(vreinterpretq_s64_f64(inControl.mValue.val[1]), 63)), inSet.mValue.val[1], inNotSet.mValue.val[1]) };
+	return sFixW(v);
+#else
+	DVec3 result;
+	for (int i = 0; i < 3; i++)
+		result.mF64[i] = (BitCast<uint64>(inControl.mF64[i]) & (uint64(1) << 63))? inSet.mF64[i] : inNotSet.mF64[i];
+#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+	result.mF64[3] = result.mF64[2];
+#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+	return result;
+#endif
+}
+
+DVec3 DVec3::sOr(DVec3Arg inV1, DVec3Arg inV2)
+{
+#if defined(JPH_USE_AVX)
+	return _mm256_or_pd(inV1.mValue, inV2.mValue);
+#elif defined(JPH_USE_SSE)
+	return DVec3({ _mm_or_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_or_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
+#elif defined(JPH_USE_NEON)
+	return DVec3({ vreinterpretq_f64_u64(vorrq_u64(vreinterpretq_u64_f64(inV1.mValue.val[0]), vreinterpretq_u64_f64(inV2.mValue.val[0]))),
+				   vreinterpretq_f64_u64(vorrq_u64(vreinterpretq_u64_f64(inV1.mValue.val[1]), vreinterpretq_u64_f64(inV2.mValue.val[1]))) });
+#else
+	return DVec3(BitCast<double>(BitCast<uint64>(inV1.mF64[0]) | BitCast<uint64>(inV2.mF64[0])),
+				 BitCast<double>(BitCast<uint64>(inV1.mF64[1]) | BitCast<uint64>(inV2.mF64[1])),
+				 BitCast<double>(BitCast<uint64>(inV1.mF64[2]) | BitCast<uint64>(inV2.mF64[2])));
+#endif
+}
+
+DVec3 DVec3::sXor(DVec3Arg inV1, DVec3Arg inV2)
+{
+#if defined(JPH_USE_AVX)
+	return _mm256_xor_pd(inV1.mValue, inV2.mValue);
+#elif defined(JPH_USE_SSE)
+	return DVec3({ _mm_xor_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_xor_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
+#elif defined(JPH_USE_NEON)
+	return DVec3({ vreinterpretq_f64_u64(veorq_u64(vreinterpretq_u64_f64(inV1.mValue.val[0]), vreinterpretq_u64_f64(inV2.mValue.val[0]))),
+				   vreinterpretq_f64_u64(veorq_u64(vreinterpretq_u64_f64(inV1.mValue.val[1]), vreinterpretq_u64_f64(inV2.mValue.val[1]))) });
+#else
+	return DVec3(BitCast<double>(BitCast<uint64>(inV1.mF64[0]) ^ BitCast<uint64>(inV2.mF64[0])),
+				 BitCast<double>(BitCast<uint64>(inV1.mF64[1]) ^ BitCast<uint64>(inV2.mF64[1])),
+				 BitCast<double>(BitCast<uint64>(inV1.mF64[2]) ^ BitCast<uint64>(inV2.mF64[2])));
+#endif
+}
+
+DVec3 DVec3::sAnd(DVec3Arg inV1, DVec3Arg inV2)
+{
+#if defined(JPH_USE_AVX)
+	return _mm256_and_pd(inV1.mValue, inV2.mValue);
+#elif defined(JPH_USE_SSE)
+	return DVec3({ _mm_and_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_and_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
+#elif defined(JPH_USE_NEON)
+	return DVec3({ vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(inV1.mValue.val[0]), vreinterpretq_u64_f64(inV2.mValue.val[0]))),
+				   vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(inV1.mValue.val[1]), vreinterpretq_u64_f64(inV2.mValue.val[1]))) });
+#else
+	return DVec3(BitCast<double>(BitCast<uint64>(inV1.mF64[0]) & BitCast<uint64>(inV2.mF64[0])),
+				 BitCast<double>(BitCast<uint64>(inV1.mF64[1]) & BitCast<uint64>(inV2.mF64[1])),
+				 BitCast<double>(BitCast<uint64>(inV1.mF64[2]) & BitCast<uint64>(inV2.mF64[2])));
+#endif
+}
+
+int DVec3::GetTrues() const
+{
+#if defined(JPH_USE_AVX)
+	return _mm256_movemask_pd(mValue) & 0x7;
+#elif defined(JPH_USE_SSE)
+	return (_mm_movemask_pd(mValue.mLow) + (_mm_movemask_pd(mValue.mHigh) << 2)) & 0x7;
+#else
+	return int((BitCast<uint64>(mF64[0]) >> 63) | ((BitCast<uint64>(mF64[1]) >> 63) << 1) | ((BitCast<uint64>(mF64[2]) >> 63) << 2));
+#endif
+}
+
+bool DVec3::TestAnyTrue() const
+{
+	return GetTrues() != 0;
+}
+
+bool DVec3::TestAllTrue() const
+{
+	return GetTrues() == 0x7;
+}
+
+bool DVec3::operator == (DVec3Arg inV2) const
+{
+	return sEquals(*this, inV2).TestAllTrue();
+}
+
+bool DVec3::IsClose(DVec3Arg inV2, double inMaxDistSq) const
+{
+	return (inV2 - *this).LengthSq() <= inMaxDistSq;
+}
+
+bool DVec3::IsNearZero(double inMaxDistSq) const
+{
+	return LengthSq() <= inMaxDistSq;
+}
+
+DVec3 DVec3::operator * (DVec3Arg inV2) const
+{
+#if defined(JPH_USE_AVX)
+	return _mm256_mul_pd(mValue, inV2.mValue);
+#elif defined(JPH_USE_SSE)
+	return DVec3({ _mm_mul_pd(mValue.mLow, inV2.mValue.mLow), _mm_mul_pd(mValue.mHigh, inV2.mValue.mHigh) });
+#elif defined(JPH_USE_NEON)
+	return DVec3({ vmulq_f64(mValue.val[0], inV2.mValue.val[0]), vmulq_f64(mValue.val[1], inV2.mValue.val[1]) });
+#else
+	return DVec3(mF64[0] * inV2.mF64[0], mF64[1] * inV2.mF64[1], mF64[2] * inV2.mF64[2]);
+#endif
+}
+
+DVec3 DVec3::operator * (double inV2) const
+{
+#if defined(JPH_USE_AVX)
+	return _mm256_mul_pd(mValue, _mm256_set1_pd(inV2));
+#elif defined(JPH_USE_SSE)
+	__m128d v = _mm_set1_pd(inV2);
+	return DVec3({ _mm_mul_pd(mValue.mLow, v), _mm_mul_pd(mValue.mHigh, v) });
+#elif defined(JPH_USE_NEON)
+	return DVec3({ vmulq_n_f64(mValue.val[0], inV2), vmulq_n_f64(mValue.val[1], inV2) });
+#else
+	return DVec3(mF64[0] * inV2, mF64[1] * inV2, mF64[2] * inV2);
+#endif
+}
+
+DVec3 operator * (double inV1, DVec3Arg inV2)
+{
+#if defined(JPH_USE_AVX)
+	return _mm256_mul_pd(_mm256_set1_pd(inV1), inV2.mValue);
+#elif defined(JPH_USE_SSE)
+	__m128d v = _mm_set1_pd(inV1);
+	return DVec3({ _mm_mul_pd(v, inV2.mValue.mLow), _mm_mul_pd(v, inV2.mValue.mHigh) });
+#elif defined(JPH_USE_NEON)
+	return DVec3({ vmulq_n_f64(inV2.mValue.val[0], inV1), vmulq_n_f64(inV2.mValue.val[1], inV1) });
+#else
+	return DVec3(inV1 * inV2.mF64[0], inV1 * inV2.mF64[1], inV1 * inV2.mF64[2]);
+#endif
+}
+
+DVec3 DVec3::operator / (double inV2) const
+{
+#if defined(JPH_USE_AVX)
+	return _mm256_div_pd(mValue, _mm256_set1_pd(inV2));
+#elif defined(JPH_USE_SSE)
+	__m128d v = _mm_set1_pd(inV2);
+	return DVec3({ _mm_div_pd(mValue.mLow, v), _mm_div_pd(mValue.mHigh, v) });
+#elif defined(JPH_USE_NEON)
+	float64x2_t v = vdupq_n_f64(inV2);
+	return DVec3({ vdivq_f64(mValue.val[0], v), vdivq_f64(mValue.val[1], v) });
+#else
+	return DVec3(mF64[0] / inV2, mF64[1] / inV2, mF64[2] / inV2);
+#endif
+}
+
+DVec3 &DVec3::operator *= (double inV2)
+{
+#if defined(JPH_USE_AVX)
+	mValue = _mm256_mul_pd(mValue, _mm256_set1_pd(inV2));
+#elif defined(JPH_USE_SSE)
+	__m128d v = _mm_set1_pd(inV2);
+	mValue.mLow = _mm_mul_pd(mValue.mLow, v);
+	mValue.mHigh = _mm_mul_pd(mValue.mHigh, v);
+#elif defined(JPH_USE_NEON)
+	mValue.val[0] = vmulq_n_f64(mValue.val[0], inV2);
+	mValue.val[1] = vmulq_n_f64(mValue.val[1], inV2);
+#else
+	for (int i = 0; i < 3; ++i)
+		mF64[i] *= inV2;
+	#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+		mF64[3] = mF64[2];
+	#endif
+#endif
+	return *this;
+}
+
+DVec3 &DVec3::operator *= (DVec3Arg inV2)
+{
+#if defined(JPH_USE_AVX)
+	mValue = _mm256_mul_pd(mValue, inV2.mValue);
+#elif defined(JPH_USE_SSE)
+	mValue.mLow = _mm_mul_pd(mValue.mLow, inV2.mValue.mLow);
+	mValue.mHigh = _mm_mul_pd(mValue.mHigh, inV2.mValue.mHigh);
+#elif defined(JPH_USE_NEON)
+	mValue.val[0] = vmulq_f64(mValue.val[0], inV2.mValue.val[0]);
+	mValue.val[1] = vmulq_f64(mValue.val[1], inV2.mValue.val[1]);
+#else
+	for (int i = 0; i < 3; ++i)
+		mF64[i] *= inV2.mF64[i];
+	#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+		mF64[3] = mF64[2];
+	#endif
+#endif
+	return *this;
+}
+
+DVec3 &DVec3::operator /= (double inV2)
+{
+#if defined(JPH_USE_AVX)
+	mValue = _mm256_div_pd(mValue, _mm256_set1_pd(inV2));
+#elif defined(JPH_USE_SSE)
+	__m128d v = _mm_set1_pd(inV2);
+	mValue.mLow = _mm_div_pd(mValue.mLow, v);
+	mValue.mHigh = _mm_div_pd(mValue.mHigh, v);
+#elif defined(JPH_USE_NEON)
+	float64x2_t v = vdupq_n_f64(inV2);
+	mValue.val[0] = vdivq_f64(mValue.val[0], v);
+	mValue.val[1] = vdivq_f64(mValue.val[1], v);
+#else
+	for (int i = 0; i < 3; ++i)
+		mF64[i] /= inV2;
+	#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+		mF64[3] = mF64[2];
+	#endif
+#endif
+	return *this;
+}
+
+DVec3 DVec3::operator + (Vec3Arg inV2) const
+{
+#if defined(JPH_USE_AVX)
+	return _mm256_add_pd(mValue, _mm256_cvtps_pd(inV2.mValue));
+#elif defined(JPH_USE_SSE)
+	return DVec3({ _mm_add_pd(mValue.mLow, _mm_cvtps_pd(inV2.mValue)), _mm_add_pd(mValue.mHigh, _mm_cvtps_pd(_mm_shuffle_ps(inV2.mValue, inV2.mValue, _MM_SHUFFLE(2, 2, 2, 2)))) });
+#elif defined(JPH_USE_NEON)
+	return DVec3({ vaddq_f64(mValue.val[0], vcvt_f64_f32(vget_low_f32(inV2.mValue))), vaddq_f64(mValue.val[1], vcvt_high_f64_f32(inV2.mValue)) });
+#else
+	return DVec3(mF64[0] + inV2.mF32[0], mF64[1] + inV2.mF32[1], mF64[2] + inV2.mF32[2]);
+#endif
+}
+
+DVec3 DVec3::operator + (DVec3Arg inV2) const
+{
+#if defined(JPH_USE_AVX)
+	return _mm256_add_pd(mValue, inV2.mValue);
+#elif defined(JPH_USE_SSE)
+	return DVec3({ _mm_add_pd(mValue.mLow, inV2.mValue.mLow), _mm_add_pd(mValue.mHigh, inV2.mValue.mHigh) });
+#elif defined(JPH_USE_NEON)
+	return DVec3({ vaddq_f64(mValue.val[0], inV2.mValue.val[0]), vaddq_f64(mValue.val[1], inV2.mValue.val[1]) });
+#else
+	return DVec3(mF64[0] + inV2.mF64[0], mF64[1] + inV2.mF64[1], mF64[2] + inV2.mF64[2]);
+#endif
+}
+
+DVec3 &DVec3::operator += (Vec3Arg inV2)
+{
+#if defined(JPH_USE_AVX)
+	mValue = _mm256_add_pd(mValue, _mm256_cvtps_pd(inV2.mValue));
+#elif defined(JPH_USE_SSE)
+	mValue.mLow = _mm_add_pd(mValue.mLow, _mm_cvtps_pd(inV2.mValue));
+	mValue.mHigh = _mm_add_pd(mValue.mHigh, _mm_cvtps_pd(_mm_shuffle_ps(inV2.mValue, inV2.mValue, _MM_SHUFFLE(2, 2, 2, 2))));
+#elif defined(JPH_USE_NEON)
+	mValue.val[0] = vaddq_f64(mValue.val[0], vcvt_f64_f32(vget_low_f32(inV2.mValue)));
+	mValue.val[1] = vaddq_f64(mValue.val[1], vcvt_high_f64_f32(inV2.mValue));
+#else
+	for (int i = 0; i < 3; ++i)
+		mF64[i] += inV2.mF32[i];
+	#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+		mF64[3] = mF64[2];
+	#endif
+#endif
+	return *this;
+}
+
+DVec3 &DVec3::operator += (DVec3Arg inV2)
+{
+#if defined(JPH_USE_AVX)
+	mValue = _mm256_add_pd(mValue, inV2.mValue);
+#elif defined(JPH_USE_SSE)
+	mValue.mLow = _mm_add_pd(mValue.mLow, inV2.mValue.mLow);
+	mValue.mHigh = _mm_add_pd(mValue.mHigh, inV2.mValue.mHigh);
+#elif defined(JPH_USE_NEON)
+	mValue.val[0] = vaddq_f64(mValue.val[0], inV2.mValue.val[0]);
+	mValue.val[1] = vaddq_f64(mValue.val[1], inV2.mValue.val[1]);
+#else
+	for (int i = 0; i < 3; ++i)
+		mF64[i] += inV2.mF64[i];
+	#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+		mF64[3] = mF64[2];
+	#endif
+#endif
+	return *this;
+}
+
+DVec3 DVec3::operator - () const
+{
+#if defined(JPH_USE_AVX)
+	return _mm256_sub_pd(_mm256_setzero_pd(), mValue);
+#elif defined(JPH_USE_SSE)
+	__m128d zero = _mm_setzero_pd();
+	return DVec3({ _mm_sub_pd(zero, mValue.mLow), _mm_sub_pd(zero, mValue.mHigh) });
+#elif defined(JPH_USE_NEON)
+	#ifdef JPH_CROSS_PLATFORM_DETERMINISTIC
+		float64x2_t zero = vdupq_n_f64(0);
+		return DVec3({ vsubq_f64(zero, mValue.val[0]), vsubq_f64(zero, mValue.val[1]) });
+	#else
+		return DVec3({ vnegq_f64(mValue.val[0]), vnegq_f64(mValue.val[1]) });
+	#endif
+#else
+	#ifdef JPH_CROSS_PLATFORM_DETERMINISTIC
+		return DVec3(0.0 - mF64[0], 0.0 - mF64[1], 0.0 - mF64[2]);
+	#else
+		return DVec3(-mF64[0], -mF64[1], -mF64[2]);
+	#endif
+#endif
+}
+
+DVec3 DVec3::operator - (Vec3Arg inV2) const
+{
+#if defined(JPH_USE_AVX)
+	return _mm256_sub_pd(mValue, _mm256_cvtps_pd(inV2.mValue));
+#elif defined(JPH_USE_SSE)
+	return DVec3({ _mm_sub_pd(mValue.mLow, _mm_cvtps_pd(inV2.mValue)), _mm_sub_pd(mValue.mHigh, _mm_cvtps_pd(_mm_shuffle_ps(inV2.mValue, inV2.mValue, _MM_SHUFFLE(2, 2, 2, 2)))) });
+#elif defined(JPH_USE_NEON)
+	return DVec3({ vsubq_f64(mValue.val[0], vcvt_f64_f32(vget_low_f32(inV2.mValue))), vsubq_f64(mValue.val[1], vcvt_high_f64_f32(inV2.mValue)) });
+#else
+	return DVec3(mF64[0] - inV2.mF32[0], mF64[1] - inV2.mF32[1], mF64[2] - inV2.mF32[2]);
+#endif
+}
+
+DVec3 DVec3::operator - (DVec3Arg inV2) const
+{
+#if defined(JPH_USE_AVX)
+	return _mm256_sub_pd(mValue, inV2.mValue);
+#elif defined(JPH_USE_SSE)
+	return DVec3({ _mm_sub_pd(mValue.mLow, inV2.mValue.mLow), _mm_sub_pd(mValue.mHigh, inV2.mValue.mHigh) });
+#elif defined(JPH_USE_NEON)
+	return DVec3({ vsubq_f64(mValue.val[0], inV2.mValue.val[0]), vsubq_f64(mValue.val[1], inV2.mValue.val[1]) });
+#else
+	return DVec3(mF64[0] - inV2.mF64[0], mF64[1] - inV2.mF64[1], mF64[2] - inV2.mF64[2]);
+#endif
+}
+
+DVec3 &DVec3::operator -= (Vec3Arg inV2)
+{
+#if defined(JPH_USE_AVX)
+	mValue = _mm256_sub_pd(mValue, _mm256_cvtps_pd(inV2.mValue));
+#elif defined(JPH_USE_SSE)
+	mValue.mLow = _mm_sub_pd(mValue.mLow, _mm_cvtps_pd(inV2.mValue));
+	mValue.mHigh = _mm_sub_pd(mValue.mHigh, _mm_cvtps_pd(_mm_shuffle_ps(inV2.mValue, inV2.mValue, _MM_SHUFFLE(2, 2, 2, 2))));
+#elif defined(JPH_USE_NEON)
+	mValue.val[0] = vsubq_f64(mValue.val[0], vcvt_f64_f32(vget_low_f32(inV2.mValue)));
+	mValue.val[1] = vsubq_f64(mValue.val[1], vcvt_high_f64_f32(inV2.mValue));
+#else
+	for (int i = 0; i < 3; ++i)
+		mF64[i] -= inV2.mF32[i];
+	#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+		mF64[3] = mF64[2];
+	#endif
+#endif
+	return *this;
+}
+
+DVec3 &DVec3::operator -= (DVec3Arg inV2)
+{
+#if defined(JPH_USE_AVX)
+	mValue = _mm256_sub_pd(mValue, inV2.mValue);
+#elif defined(JPH_USE_SSE)
+	mValue.mLow = _mm_sub_pd(mValue.mLow, inV2.mValue.mLow);
+	mValue.mHigh = _mm_sub_pd(mValue.mHigh, inV2.mValue.mHigh);
+#elif defined(JPH_USE_NEON)
+	mValue.val[0] = vsubq_f64(mValue.val[0], inV2.mValue.val[0]);
+	mValue.val[1] = vsubq_f64(mValue.val[1], inV2.mValue.val[1]);
+#else
+	for (int i = 0; i < 3; ++i)
+		mF64[i] -= inV2.mF64[i];
+	#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+		mF64[3] = mF64[2];
+	#endif
+#endif
+	return *this;
+}
+
+DVec3 DVec3::operator / (DVec3Arg inV2) const
+{
+	inV2.CheckW();
+#if defined(JPH_USE_AVX)
+	return _mm256_div_pd(mValue, inV2.mValue);
+#elif defined(JPH_USE_SSE)
+	return DVec3({ _mm_div_pd(mValue.mLow, inV2.mValue.mLow), _mm_div_pd(mValue.mHigh, inV2.mValue.mHigh) });
+#elif defined(JPH_USE_NEON)
+	return DVec3({ vdivq_f64(mValue.val[0], inV2.mValue.val[0]), vdivq_f64(mValue.val[1], inV2.mValue.val[1]) });
+#else
+	return DVec3(mF64[0] / inV2.mF64[0], mF64[1] / inV2.mF64[1], mF64[2] / inV2.mF64[2]);
+#endif
+}
+
+DVec3 DVec3::Abs() const
+{
+#if defined(JPH_USE_AVX512)
+	return _mm256_range_pd(mValue, mValue, 0b1000);
+#elif defined(JPH_USE_AVX)
+	return _mm256_max_pd(_mm256_sub_pd(_mm256_setzero_pd(), mValue), mValue);
+#elif defined(JPH_USE_SSE)
+	__m128d zero = _mm_setzero_pd();
+	return DVec3({ _mm_max_pd(_mm_sub_pd(zero, mValue.mLow), mValue.mLow), _mm_max_pd(_mm_sub_pd(zero, mValue.mHigh), mValue.mHigh) });
+#elif defined(JPH_USE_NEON)
+	return DVec3({ vabsq_f64(mValue.val[0]), vabsq_f64(mValue.val[1]) });
+#else
+	return DVec3(abs(mF64[0]), abs(mF64[1]), abs(mF64[2]));
+#endif
+}
+
+DVec3 DVec3::Reciprocal() const
+{
+	return sReplicate(1.0) / mValue;
+}
+
+DVec3 DVec3::Cross(DVec3Arg inV2) const
+{
+#if defined(JPH_USE_AVX2)
+	__m256d t1 = _mm256_permute4x64_pd(inV2.mValue, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same
+	t1 = _mm256_mul_pd(t1, mValue);
+	__m256d t2 = _mm256_permute4x64_pd(mValue, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same
+	t2 = _mm256_mul_pd(t2, inV2.mValue);
+	__m256d t3 = _mm256_sub_pd(t1, t2);
+	return _mm256_permute4x64_pd(t3, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same
+#else
+	return DVec3(mF64[1] * inV2.mF64[2] - mF64[2] * inV2.mF64[1],
+				 mF64[2] * inV2.mF64[0] - mF64[0] * inV2.mF64[2],
+				 mF64[0] * inV2.mF64[1] - mF64[1] * inV2.mF64[0]);
+#endif
+}
+
+double DVec3::Dot(DVec3Arg inV2) const
+{
+#if defined(JPH_USE_AVX)
+	__m256d mul = _mm256_mul_pd(mValue, inV2.mValue);
+	__m128d xy = _mm256_castpd256_pd128(mul);
+	__m128d yx = _mm_shuffle_pd(xy, xy, 1);
+	__m128d sum = _mm_add_pd(xy, yx);
+	__m128d zw = _mm256_extractf128_pd(mul, 1);
+	sum = _mm_add_pd(sum, zw);
+	return _mm_cvtsd_f64(sum);
+#elif defined(JPH_USE_SSE)
+	__m128d xy = _mm_mul_pd(mValue.mLow, inV2.mValue.mLow);
+	__m128d yx = _mm_shuffle_pd(xy, xy, 1);
+	__m128d sum = _mm_add_pd(xy, yx);
+	__m128d z = _mm_mul_sd(mValue.mHigh, inV2.mValue.mHigh);
+	sum = _mm_add_pd(sum, z);
+	return _mm_cvtsd_f64(sum);
+#elif defined(JPH_USE_NEON)
+	float64x2_t mul_low = vmulq_f64(mValue.val[0], inV2.mValue.val[0]);
+	float64x2_t mul_high = vmulq_f64(mValue.val[1], inV2.mValue.val[1]);
+	return vaddvq_f64(mul_low) + vgetq_lane_f64(mul_high, 0);
+#else
+	double dot = 0.0;
+	for (int i = 0; i < 3; i++)
+		dot += mF64[i] * inV2.mF64[i];
+	return dot;
+#endif
+}
+
+double DVec3::LengthSq() const
+{
+	return Dot(*this);
+}
+
+DVec3 DVec3::Sqrt() const
+{
+#if defined(JPH_USE_AVX)
+	return _mm256_sqrt_pd(mValue);
+#elif defined(JPH_USE_SSE)
+	return DVec3({ _mm_sqrt_pd(mValue.mLow), _mm_sqrt_pd(mValue.mHigh) });
+#elif defined(JPH_USE_NEON)
+	return DVec3({ vsqrtq_f64(mValue.val[0]), vsqrtq_f64(mValue.val[1]) });
+#else
+	return DVec3(sqrt(mF64[0]), sqrt(mF64[1]), sqrt(mF64[2]));
+#endif
+}
+
+double DVec3::Length() const
+{
+	return sqrt(Dot(*this));
+}
+
+DVec3 DVec3::Normalized() const
+{
+	return *this / Length();
+}
+
+bool DVec3::IsNormalized(double inTolerance) const
+{
+	return abs(LengthSq() - 1.0) <= inTolerance;
+}
+
+bool DVec3::IsNaN() const
+{
+#if defined(JPH_USE_AVX512)
+	return (_mm256_fpclass_pd_mask(mValue, 0b10000001) & 0x7) != 0;
+#elif defined(JPH_USE_AVX)
+	return (_mm256_movemask_pd(_mm256_cmp_pd(mValue, mValue, _CMP_UNORD_Q)) & 0x7) != 0;
+#elif defined(JPH_USE_SSE)
+	return ((_mm_movemask_pd(_mm_cmpunord_pd(mValue.mLow, mValue.mLow)) + (_mm_movemask_pd(_mm_cmpunord_pd(mValue.mHigh, mValue.mHigh)) << 2)) & 0x7) != 0;
+#else
+	return isnan(mF64[0]) || isnan(mF64[1]) || isnan(mF64[2]);
+#endif
+}
+
+DVec3 DVec3::GetSign() const
+{
+#if defined(JPH_USE_AVX512)
+	return _mm256_fixupimm_pd(mValue, mValue, _mm256_set1_epi32(0xA9A90A00), 0);
+#elif defined(JPH_USE_AVX)
+	__m256d minus_one = _mm256_set1_pd(-1.0);
+	__m256d one = _mm256_set1_pd(1.0);
+	return _mm256_or_pd(_mm256_and_pd(mValue, minus_one), one);
+#elif defined(JPH_USE_SSE)
+	__m128d minus_one = _mm_set1_pd(-1.0);
+	__m128d one = _mm_set1_pd(1.0);
+	return DVec3({ _mm_or_pd(_mm_and_pd(mValue.mLow, minus_one), one), _mm_or_pd(_mm_and_pd(mValue.mHigh, minus_one), one) });
+#elif defined(JPH_USE_NEON)
+	uint64x2_t minus_one = vreinterpretq_u64_f64(vdupq_n_f64(-1.0f));
+	uint64x2_t one = vreinterpretq_u64_f64(vdupq_n_f64(1.0f));
+	return DVec3({ vreinterpretq_f64_u64(vorrq_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[0]), minus_one), one)),
+				   vreinterpretq_f64_u64(vorrq_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[1]), minus_one), one)) });
+#else
+	return DVec3(std::signbit(mF64[0])? -1.0 : 1.0,
+				 std::signbit(mF64[1])? -1.0 : 1.0,
+				 std::signbit(mF64[2])? -1.0 : 1.0);
+#endif
+}
+
+DVec3 DVec3::PrepareRoundToZero() const
+{
+	// Float has 23 bit mantissa, double 52 bit mantissa => we lose 29 bits when converting from double to float
+	constexpr uint64 cDoubleToFloatMantissaLoss = (1U << 29) - 1;
+
+#if defined(JPH_USE_AVX)
+	return _mm256_and_pd(mValue, _mm256_castsi256_pd(_mm256_set1_epi64x(int64_t(~cDoubleToFloatMantissaLoss))));
+#elif defined(JPH_USE_SSE)
+	__m128d mask = _mm_castsi128_pd(_mm_set1_epi64x(int64_t(~cDoubleToFloatMantissaLoss)));
+	return DVec3({ _mm_and_pd(mValue.mLow, mask), _mm_and_pd(mValue.mHigh, mask) });
+#elif defined(JPH_USE_NEON)
+	uint64x2_t mask = vdupq_n_u64(~cDoubleToFloatMantissaLoss);
+	return DVec3({ vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[0]), mask)),
+				   vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[1]), mask)) });
+#else
+	double x = BitCast<double>(BitCast<uint64>(mF64[0]) & ~cDoubleToFloatMantissaLoss);
+	double y = BitCast<double>(BitCast<uint64>(mF64[1]) & ~cDoubleToFloatMantissaLoss);
+	double z = BitCast<double>(BitCast<uint64>(mF64[2]) & ~cDoubleToFloatMantissaLoss);
+
+	return DVec3(x, y, z);
+#endif
+}
+
+DVec3 DVec3::PrepareRoundToInf() const
+{
+	// Float has 23 bit mantissa, double 52 bit mantissa => we lose 29 bits when converting from double to float
+	constexpr uint64 cDoubleToFloatMantissaLoss = (1U << 29) - 1;
+
+#if defined(JPH_USE_AVX512)
+	__m256i mantissa_loss = _mm256_set1_epi64x(cDoubleToFloatMantissaLoss);
+	__mmask8 is_zero = _mm256_testn_epi64_mask(_mm256_castpd_si256(mValue), mantissa_loss);
+	__m256d value_or_mantissa_loss = _mm256_or_pd(mValue, _mm256_castsi256_pd(mantissa_loss));
+	return _mm256_mask_blend_pd(is_zero, value_or_mantissa_loss, mValue);
+#elif defined(JPH_USE_AVX)
+	__m256i mantissa_loss = _mm256_set1_epi64x(cDoubleToFloatMantissaLoss);
+	__m256d value_and_mantissa_loss = _mm256_and_pd(mValue, _mm256_castsi256_pd(mantissa_loss));
+	__m256d is_zero = _mm256_cmp_pd(value_and_mantissa_loss, _mm256_setzero_pd(), _CMP_EQ_OQ);
+	__m256d value_or_mantissa_loss = _mm256_or_pd(mValue, _mm256_castsi256_pd(mantissa_loss));
+	return _mm256_blendv_pd(value_or_mantissa_loss, mValue, is_zero);
+#elif defined(JPH_USE_SSE4_1)
+	__m128i mantissa_loss = _mm_set1_epi64x(cDoubleToFloatMantissaLoss);
+	__m128d zero = _mm_setzero_pd();
+	__m128d value_and_mantissa_loss_low = _mm_and_pd(mValue.mLow, _mm_castsi128_pd(mantissa_loss));
+	__m128d is_zero_low = _mm_cmpeq_pd(value_and_mantissa_loss_low, zero);
+	__m128d value_or_mantissa_loss_low = _mm_or_pd(mValue.mLow, _mm_castsi128_pd(mantissa_loss));
+	__m128d value_and_mantissa_loss_high = _mm_and_pd(mValue.mHigh, _mm_castsi128_pd(mantissa_loss));
+	__m128d is_zero_high = _mm_cmpeq_pd(value_and_mantissa_loss_high, zero);
+	__m128d value_or_mantissa_loss_high = _mm_or_pd(mValue.mHigh, _mm_castsi128_pd(mantissa_loss));
+	return DVec3({ _mm_blendv_pd(value_or_mantissa_loss_low, mValue.mLow, is_zero_low), _mm_blendv_pd(value_or_mantissa_loss_high, mValue.mHigh, is_zero_high) });
+#elif defined(JPH_USE_NEON)
+	uint64x2_t mantissa_loss = vdupq_n_u64(cDoubleToFloatMantissaLoss);
+	float64x2_t zero = vdupq_n_f64(0.0);
+	float64x2_t value_and_mantissa_loss_low = vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[0]), mantissa_loss));
+	uint64x2_t is_zero_low = vceqq_f64(value_and_mantissa_loss_low, zero);
+	float64x2_t value_or_mantissa_loss_low = vreinterpretq_f64_u64(vorrq_u64(vreinterpretq_u64_f64(mValue.val[0]), mantissa_loss));
+	float64x2_t value_and_mantissa_loss_high = vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[1]), mantissa_loss));
+	float64x2_t value_low = vbslq_f64(is_zero_low, mValue.val[0], value_or_mantissa_loss_low);
+	uint64x2_t is_zero_high = vceqq_f64(value_and_mantissa_loss_high, zero);
+	float64x2_t value_or_mantissa_loss_high = vreinterpretq_f64_u64(vorrq_u64(vreinterpretq_u64_f64(mValue.val[1]), mantissa_loss));
+	float64x2_t value_high = vbslq_f64(is_zero_high, mValue.val[1], value_or_mantissa_loss_high);
+	return DVec3({ value_low, value_high });
+#else
+	uint64 ux = BitCast<uint64>(mF64[0]);
+	uint64 uy = BitCast<uint64>(mF64[1]);
+	uint64 uz = BitCast<uint64>(mF64[2]);
+
+	double x = BitCast<double>((ux & cDoubleToFloatMantissaLoss) == 0? ux : (ux | cDoubleToFloatMantissaLoss));
+	double y = BitCast<double>((uy & cDoubleToFloatMantissaLoss) == 0? uy : (uy | cDoubleToFloatMantissaLoss));
+	double z = BitCast<double>((uz & cDoubleToFloatMantissaLoss) == 0? uz : (uz | cDoubleToFloatMantissaLoss));
+
+	return DVec3(x, y, z);
+#endif
+}
+
+Vec3 DVec3::ToVec3RoundDown() const
+{
+	DVec3 to_zero = PrepareRoundToZero();
+	DVec3 to_inf = PrepareRoundToInf();
+	return Vec3(DVec3::sSelect(to_zero, to_inf, DVec3::sLess(*this, DVec3::sZero())));
+}
+
+Vec3 DVec3::ToVec3RoundUp() const
+{
+	DVec3 to_zero = PrepareRoundToZero();
+	DVec3 to_inf = PrepareRoundToInf();
+	return Vec3(DVec3::sSelect(to_inf, to_zero, DVec3::sLess(*this, DVec3::sZero())));
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Math/Double3.h b/thirdparty/jolt_physics/Jolt/Math/Double3.h
new file mode 100644
index 0000000000..9b738ff8f8
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/Double3.h
@@ -0,0 +1,48 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/HashCombine.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that holds 3 doubles. Used as a storage class. Convert to DVec3 for calculations.
+class [[nodiscard]] Double3
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+				Double3() = default; ///< Intentionally not initialized for performance reasons
+				Double3(const Double3 &inRHS) = default;
+	Double3 &	operator = (const Double3 &inRHS) = default;
+				Double3(double inX, double inY, double inZ) : x(inX), y(inY), z(inZ) { }
+
+	double		operator [] (int inCoordinate) const
+	{
+		JPH_ASSERT(inCoordinate < 3);
+		return *(&x + inCoordinate);
+	}
+
+	bool		operator == (const Double3 &inRHS) const
+	{
+		return x == inRHS.x && y == inRHS.y && z == inRHS.z;
+	}
+
+	bool		operator != (const Double3 &inRHS) const
+	{
+		return x != inRHS.x || y != inRHS.y || z != inRHS.z;
+	}
+
+	double		x;
+	double		y;
+	double		z;
+};
+
+static_assert(std::is_trivial<Double3>(), "Is supposed to be a trivial type!");
+
+JPH_NAMESPACE_END
+
+// Create a std::hash/JPH::Hash for Double3
+JPH_MAKE_HASHABLE(JPH::Double3, t.x, t.y, t.z)
diff --git a/thirdparty/jolt_physics/Jolt/Math/DynMatrix.h b/thirdparty/jolt_physics/Jolt/Math/DynMatrix.h
new file mode 100644
index 0000000000..76db294c78
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/DynMatrix.h
@@ -0,0 +1,31 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Dynamic resizable matrix class
+class [[nodiscard]] DynMatrix
+{
+public:
+	/// Constructor
+					DynMatrix(const DynMatrix &) = default;
+					DynMatrix(uint inRows, uint inCols)			: mRows(inRows), mCols(inCols) { mElements.resize(inRows * inCols); }
+
+	/// Access an element
+	float			operator () (uint inRow, uint inCol) const	{ JPH_ASSERT(inRow < mRows && inCol < mCols); return mElements[inRow * mCols + inCol]; }
+	float &			operator () (uint inRow, uint inCol)		{ JPH_ASSERT(inRow < mRows && inCol < mCols); return mElements[inRow * mCols + inCol]; }
+
+	/// Get dimensions
+	uint			GetCols() const								{ return mCols; }
+	uint			GetRows() const								{ return mRows; }
+
+private:
+	uint			mRows;
+	uint			mCols;
+	Array<float>	mElements;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Math/EigenValueSymmetric.h b/thirdparty/jolt_physics/Jolt/Math/EigenValueSymmetric.h
new file mode 100644
index 0000000000..920ddb9c9e
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/EigenValueSymmetric.h
@@ -0,0 +1,177 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/FPException.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Function to determine the eigen vectors and values of a N x N real symmetric matrix
+/// by Jacobi transformations. This method is most suitable for N < 10.
+///
+/// Taken and adapted from Numerical Recipies paragraph 11.1
+///
+/// An eigen vector is a vector v for which \f$A \: v = \lambda \: v\f$
+///
+/// Where:
+/// A: A square matrix.
+/// \f$\lambda\f$: a non-zero constant value.
+///
+/// @see https://en.wikipedia.org/wiki/Eigenvalues_and_eigenvectors
+///
+/// Matrix is a matrix type, which has dimensions N x N.
+/// @param inMatrix is the matrix of which to return the eigenvalues and vectors
+/// @param outEigVec will contain a matrix whose columns contain the normalized eigenvectors (must be identity before call)
+/// @param outEigVal will contain the eigenvalues
+template <class Vector, class Matrix>
+bool EigenValueSymmetric(const Matrix &inMatrix, Matrix &outEigVec, Vector &outEigVal)
+{
+	// This algorithm can generate infinite values, see comment below
+	FPExceptionDisableInvalid disable_invalid;
+	(void)disable_invalid;
+
+	// Maximum number of sweeps to make
+	const int cMaxSweeps = 50;
+
+	// Get problem dimension
+	const uint n = inMatrix.GetRows();
+
+	// Make sure the dimensions are right
+	JPH_ASSERT(inMatrix.GetRows() == n);
+	JPH_ASSERT(inMatrix.GetCols() == n);
+	JPH_ASSERT(outEigVec.GetRows() == n);
+	JPH_ASSERT(outEigVec.GetCols() == n);
+	JPH_ASSERT(outEigVal.GetRows() == n);
+	JPH_ASSERT(outEigVec.IsIdentity());
+
+	// Get the matrix in a so we can mess with it
+	Matrix a = inMatrix;
+
+	Vector b, z;
+
+	for (uint ip = 0; ip < n; ++ip)
+	{
+		// Initialize b to diagonal of a
+		b[ip] = a(ip, ip);
+
+		// Initialize output to diagonal of a
+		outEigVal[ip] = a(ip, ip);
+
+		// Reset z
+		z[ip] = 0.0f;
+	}
+
+	for (int sweep = 0; sweep < cMaxSweeps; ++sweep)
+	{
+		// Get the sum of the off-diagonal elements of a
+		float sm = 0.0f;
+		for (uint ip = 0; ip < n - 1; ++ip)
+			for (uint iq = ip + 1; iq < n; ++iq)
+				sm += abs(a(ip, iq));
+		float avg_sm = sm / Square(n);
+
+		// Normal return, convergence to machine underflow
+		if (avg_sm < FLT_MIN) // Original code: sm == 0.0f, when the average is denormal, we also consider it machine underflow
+		{
+			// Sanity checks
+			#ifdef JPH_ENABLE_ASSERTS
+				for (uint c = 0; c < n; ++c)
+				{
+					// Check if the eigenvector is normalized
+					JPH_ASSERT(outEigVec.GetColumn(c).IsNormalized());
+
+					// Check if inMatrix * eigen_vector = eigen_value * eigen_vector
+					Vector mat_eigvec = inMatrix * outEigVec.GetColumn(c);
+					Vector eigval_eigvec = outEigVal[c] * outEigVec.GetColumn(c);
+					JPH_ASSERT(mat_eigvec.IsClose(eigval_eigvec, max(mat_eigvec.LengthSq(), eigval_eigvec.LengthSq()) * 1.0e-6f));
+				}
+			#endif
+
+			// Success
+			return true;
+		}
+
+		// On the first three sweeps use a fraction of the sum of the off diagonal elements as threshold
+		// Note that we pick a minimum threshold of FLT_MIN because dividing by a denormalized number is likely to result in infinity.
+		float tresh = sweep < 4? 0.2f * avg_sm : FLT_MIN; // Original code: 0.0f instead of FLT_MIN
+
+		for (uint ip = 0; ip < n - 1; ++ip)
+			for (uint iq = ip + 1; iq < n; ++iq)
+			{
+				float &a_pq = a(ip, iq);
+				float &eigval_p = outEigVal[ip];
+				float &eigval_q = outEigVal[iq];
+
+				float abs_a_pq = abs(a_pq);
+				float g = 100.0f * abs_a_pq;
+
+				// After four sweeps, skip the rotation if the off-diagonal element is small
+				if (sweep > 4
+					&& abs(eigval_p) + g == abs(eigval_p)
+					&& abs(eigval_q) + g == abs(eigval_q))
+				{
+					a_pq = 0.0f;
+				}
+				else if (abs_a_pq > tresh)
+				{
+					float h = eigval_q - eigval_p;
+					float abs_h = abs(h);
+
+					float t;
+					if (abs_h + g == abs_h)
+					{
+						t = a_pq / h;
+					}
+					else
+					{
+						float theta = 0.5f * h / a_pq; // Warning: Can become infinite if a(ip, iq) is very small which may trigger an invalid float exception
+						t = 1.0f / (abs(theta) + sqrt(1.0f + theta * theta)); // If theta becomes inf, t will be 0 so the infinite is not a problem for the algorithm
+						if (theta < 0.0f) t = -t;
+					}
+
+					float c = 1.0f / sqrt(1.0f + t * t);
+					float s = t * c;
+					float tau = s / (1.0f + c);
+					h = t * a_pq;
+
+					a_pq = 0.0f;
+
+					z[ip] -= h;
+					z[iq] += h;
+
+					eigval_p -= h;
+					eigval_q += h;
+
+					#define JPH_EVS_ROTATE(a, i, j, k, l)		\
+						g = a(i, j),							\
+						h = a(k, l),							\
+						a(i, j) = g - s * (h + g * tau),		\
+						a(k, l) = h + s * (g - h * tau)
+
+					uint j;
+					for (j = 0; j < ip; ++j)		JPH_EVS_ROTATE(a, j, ip, j, iq);
+					for (j = ip + 1; j < iq; ++j)	JPH_EVS_ROTATE(a, ip, j, j, iq);
+					for (j = iq + 1; j < n; ++j)	JPH_EVS_ROTATE(a, ip, j, iq, j);
+					for (j = 0; j < n; ++j)			JPH_EVS_ROTATE(outEigVec, j, ip, j, iq);
+
+					#undef JPH_EVS_ROTATE
+				}
+			}
+
+		// Update eigenvalues with the sum of ta_pq and reinitialize z
+		for (uint ip = 0; ip < n; ++ip)
+		{
+			b[ip] += z[ip];
+			outEigVal[ip] = b[ip];
+			z[ip] = 0.0f;
+		}
+	}
+
+	// Failure
+	JPH_ASSERT(false, "Too many iterations");
+	return false;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Math/FindRoot.h b/thirdparty/jolt_physics/Jolt/Math/FindRoot.h
new file mode 100644
index 0000000000..21fef9f61f
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/FindRoot.h
@@ -0,0 +1,42 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Find the roots of \f$inA \: x^2 + inB \: x + inC = 0\f$.
+/// @return The number of roots, actual roots in outX1 and outX2.
+/// If number of roots returned is 1 then outX1 == outX2.
+template <typename T>
+inline int FindRoot(const T inA, const T inB, const T inC, T &outX1, T &outX2)
+{
+	// Check if this is a linear equation
+	if (inA == T(0))
+	{
+		// Check if this is a constant equation
+		if (inB == T(0))
+			return 0;
+
+		// Linear equation with 1 solution
+		outX1 = outX2 = -inC / inB;
+		return 1;
+	}
+
+	// See Numerical Recipes in C, Chapter 5.6 Quadratic and Cubic Equations
+	T det = Square(inB) - T(4) * inA * inC;
+	if (det < T(0))
+		return 0;
+	T q = (inB + Sign(inB) * sqrt(det)) / T(-2);
+	outX1 = q / inA;
+	if (q == T(0))
+	{
+		outX2 = outX1;
+		return 1;
+	}
+	outX2 = inC / q;
+	return 2;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Math/Float2.h b/thirdparty/jolt_physics/Jolt/Math/Float2.h
new file mode 100644
index 0000000000..3e16e8c6c0
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/Float2.h
@@ -0,0 +1,36 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that holds 2 floats, used as a storage class mainly.
+class [[nodiscard]] Float2
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+						Float2() = default; ///< Intentionally not initialized for performance reasons
+						Float2(const Float2 &inRHS) = default;
+	Float2 &			operator = (const Float2 &inRHS) = default;
+						Float2(float inX, float inY)					: x(inX), y(inY) { }
+
+	bool				operator == (const Float2 &inRHS) const			{ return x == inRHS.x && y == inRHS.y; }
+	bool				operator != (const Float2 &inRHS) const			{ return x != inRHS.x || y != inRHS.y; }
+
+	/// To String
+	friend ostream &	operator << (ostream &inStream, const Float2 &inV)
+	{
+		inStream << inV.x << ", " << inV.y;
+		return inStream;
+	}
+
+	float				x;
+	float				y;
+};
+
+static_assert(std::is_trivial<Float2>(), "Is supposed to be a trivial type!");
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Math/Float3.h b/thirdparty/jolt_physics/Jolt/Math/Float3.h
new file mode 100644
index 0000000000..1355e9ca0f
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/Float3.h
@@ -0,0 +1,50 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/HashCombine.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that holds 3 floats. Used as a storage class. Convert to Vec3 for calculations.
+class [[nodiscard]] Float3
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+				Float3() = default; ///< Intentionally not initialized for performance reasons
+				Float3(const Float3 &inRHS) = default;
+	Float3 &	operator = (const Float3 &inRHS) = default;
+	constexpr	Float3(float inX, float inY, float inZ) : x(inX), y(inY), z(inZ) { }
+
+	float		operator [] (int inCoordinate) const
+	{
+		JPH_ASSERT(inCoordinate < 3);
+		return *(&x + inCoordinate);
+	}
+
+	bool		operator == (const Float3 &inRHS) const
+	{
+		return x == inRHS.x && y == inRHS.y && z == inRHS.z;
+	}
+
+	bool		operator != (const Float3 &inRHS) const
+	{
+		return x != inRHS.x || y != inRHS.y || z != inRHS.z;
+	}
+
+	float		x;
+	float		y;
+	float		z;
+};
+
+using VertexList = Array<Float3>;
+
+static_assert(std::is_trivial<Float3>(), "Is supposed to be a trivial type!");
+
+JPH_NAMESPACE_END
+
+// Create a std::hash/JPH::Hash for Float3
+JPH_MAKE_HASHABLE(JPH::Float3, t.x, t.y, t.z)
diff --git a/thirdparty/jolt_physics/Jolt/Math/Float4.h b/thirdparty/jolt_physics/Jolt/Math/Float4.h
new file mode 100644
index 0000000000..ca292b3ac3
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/Float4.h
@@ -0,0 +1,33 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that holds 4 float values. Convert to Vec4 to perform calculations.
+class [[nodiscard]] Float4
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+				Float4() = default; ///< Intentionally not initialized for performance reasons
+				Float4(const Float4 &inRHS) = default;
+				Float4(float inX, float inY, float inZ, float inW) : x(inX), y(inY), z(inZ), w(inW) { }
+
+	float		operator [] (int inCoordinate) const
+	{
+		JPH_ASSERT(inCoordinate < 4);
+		return *(&x + inCoordinate);
+	}
+
+	float		x;
+	float		y;
+	float		z;
+	float		w;
+};
+
+static_assert(std::is_trivial<Float4>(), "Is supposed to be a trivial type!");
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Math/GaussianElimination.h b/thirdparty/jolt_physics/Jolt/Math/GaussianElimination.h
new file mode 100644
index 0000000000..f986cf3b3b
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/GaussianElimination.h
@@ -0,0 +1,102 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// This function performs Gauss-Jordan elimination to solve a matrix equation.
+/// A must be an NxN matrix and B must be an NxM matrix forming the equation A * x = B
+/// on output B will contain x and A will be destroyed.
+///
+/// This code can be used for example to compute the inverse of a matrix.
+/// Set A to the matrix to invert, set B to identity and let GaussianElimination solve
+/// the equation, on return B will be the inverse of A. And A is destroyed.
+///
+/// Taken and adapted from Numerical Recipies in C paragraph 2.1
+template <class MatrixA, class MatrixB>
+bool GaussianElimination(MatrixA &ioA, MatrixB &ioB, float inTolerance = 1.0e-16f)
+{
+	// Get problem dimensions
+	const uint n = ioA.GetCols();
+	const uint m = ioB.GetCols();
+
+	// Check matrix requirement
+	JPH_ASSERT(ioA.GetRows() == n);
+	JPH_ASSERT(ioB.GetRows() == n);
+
+	// Create array for bookkeeping on pivoting
+	int *ipiv = (int *)JPH_STACK_ALLOC(n * sizeof(int));
+	memset(ipiv, 0, n * sizeof(int));
+
+	for (uint i = 0; i < n; ++i)
+	{
+		// Initialize pivot element as the diagonal
+		uint pivot_row = i, pivot_col = i;
+
+		// Determine pivot element
+		float largest_element = 0.0f;
+		for (uint j = 0; j < n; ++j)
+			if (ipiv[j] != 1)
+				for (uint k = 0; k < n; ++k)
+				{
+					if (ipiv[k] == 0)
+					{
+						float element = abs(ioA(j, k));
+						if (element >= largest_element)
+						{
+							largest_element = element;
+							pivot_row = j;
+							pivot_col = k;
+						}
+					}
+					else if (ipiv[k] > 1)
+					{
+						return false;
+					}
+				}
+
+		// Mark this column as used
+		++ipiv[pivot_col];
+
+		// Exchange rows when needed so that the pivot element is at ioA(pivot_col, pivot_col) instead of at ioA(pivot_row, pivot_col)
+		if (pivot_row != pivot_col)
+		{
+			for (uint j = 0; j < n; ++j)
+				std::swap(ioA(pivot_row, j), ioA(pivot_col, j));
+			for (uint j = 0; j < m; ++j)
+				std::swap(ioB(pivot_row, j), ioB(pivot_col, j));
+		}
+
+		// Get diagonal element that we are about to set to 1
+		float diagonal_element = ioA(pivot_col, pivot_col);
+		if (abs(diagonal_element) < inTolerance)
+			return false;
+
+		// Divide the whole row by the pivot element, making ioA(pivot_col, pivot_col) = 1
+		for (uint j = 0; j < n; ++j)
+			ioA(pivot_col, j) /= diagonal_element;
+		for (uint j = 0; j < m; ++j)
+			ioB(pivot_col, j) /= diagonal_element;
+		ioA(pivot_col, pivot_col) = 1.0f;
+
+		// Next reduce the rows, except for the pivot one,
+		// after this step the pivot_col column is zero except for the pivot element which is 1
+		for (uint j = 0; j < n; ++j)
+			if (j != pivot_col)
+			{
+				float element = ioA(j, pivot_col);
+				for (uint k = 0; k < n; ++k)
+					ioA(j, k) -= ioA(pivot_col, k) * element;
+				for (uint k = 0; k < m; ++k)
+					ioB(j, k) -= ioB(pivot_col, k) * element;
+				ioA(j, pivot_col) = 0.0f;
+			}
+	}
+
+	// Success
+	return true;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Math/HalfFloat.h b/thirdparty/jolt_physics/Jolt/Math/HalfFloat.h
new file mode 100644
index 0000000000..5b36cb0951
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/HalfFloat.h
@@ -0,0 +1,204 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Math/Vec4.h>
+
+JPH_NAMESPACE_BEGIN
+
+using HalfFloat = uint16;
+
+// Define half float constant values
+static constexpr HalfFloat HALF_FLT_MAX				= 0x7bff;
+static constexpr HalfFloat HALF_FLT_MAX_NEGATIVE	= 0xfbff;
+static constexpr HalfFloat HALF_FLT_INF				= 0x7c00;
+static constexpr HalfFloat HALF_FLT_INF_NEGATIVE	= 0xfc00;
+static constexpr HalfFloat HALF_FLT_NANQ			= 0x7e00;
+static constexpr HalfFloat HALF_FLT_NANQ_NEGATIVE	= 0xfe00;
+
+namespace HalfFloatConversion {
+
+// Layout of a float
+static constexpr int FLOAT_SIGN_POS = 31;
+static constexpr int FLOAT_EXPONENT_POS = 23;
+static constexpr int FLOAT_EXPONENT_BITS = 8;
+static constexpr int FLOAT_EXPONENT_MASK = (1 << FLOAT_EXPONENT_BITS) - 1;
+static constexpr int FLOAT_EXPONENT_BIAS = 127;
+static constexpr int FLOAT_MANTISSA_BITS = 23;
+static constexpr int FLOAT_MANTISSA_MASK = (1 << FLOAT_MANTISSA_BITS) - 1;
+static constexpr int FLOAT_EXPONENT_AND_MANTISSA_MASK = FLOAT_MANTISSA_MASK + (FLOAT_EXPONENT_MASK << FLOAT_EXPONENT_POS);
+
+// Layout of half float
+static constexpr int HALF_FLT_SIGN_POS = 15;
+static constexpr int HALF_FLT_EXPONENT_POS = 10;
+static constexpr int HALF_FLT_EXPONENT_BITS = 5;
+static constexpr int HALF_FLT_EXPONENT_MASK = (1 << HALF_FLT_EXPONENT_BITS) - 1;
+static constexpr int HALF_FLT_EXPONENT_BIAS = 15;
+static constexpr int HALF_FLT_MANTISSA_BITS = 10;
+static constexpr int HALF_FLT_MANTISSA_MASK = (1 << HALF_FLT_MANTISSA_BITS) - 1;
+static constexpr int HALF_FLT_EXPONENT_AND_MANTISSA_MASK = HALF_FLT_MANTISSA_MASK + (HALF_FLT_EXPONENT_MASK << HALF_FLT_EXPONENT_POS);
+
+/// Define half-float rounding modes
+enum ERoundingMode
+{
+	ROUND_TO_NEG_INF,				///< Round to negative infinity
+	ROUND_TO_POS_INF,				///< Round to positive infinity
+	ROUND_TO_NEAREST,				///< Round to nearest value
+};
+
+/// Convert a float (32-bits) to a half float (16-bits), fallback version when no intrinsics available
+template <int RoundingMode>
+inline HalfFloat FromFloatFallback(float inV)
+{
+	// Reinterpret the float as an uint32
+	uint32 value = BitCast<uint32>(inV);
+
+	// Extract exponent
+	uint32 exponent = (value >> FLOAT_EXPONENT_POS) & FLOAT_EXPONENT_MASK;
+
+	// Extract mantissa
+	uint32 mantissa = value & FLOAT_MANTISSA_MASK;
+
+	// Extract the sign and move it into the right spot for the half float (so we can just or it in at the end)
+	HalfFloat hf_sign = HalfFloat(value >> (FLOAT_SIGN_POS - HALF_FLT_SIGN_POS)) & (1 << HALF_FLT_SIGN_POS);
+
+	// Check NaN or INF
+	if (exponent == FLOAT_EXPONENT_MASK) // NaN or INF
+		return hf_sign | (mantissa == 0? HALF_FLT_INF : HALF_FLT_NANQ);
+
+	// Rebias the exponent for half floats
+	int rebiased_exponent = int(exponent) - FLOAT_EXPONENT_BIAS + HALF_FLT_EXPONENT_BIAS;
+
+	// Check overflow to infinity
+	if (rebiased_exponent >= HALF_FLT_EXPONENT_MASK)
+	{
+		bool round_up = RoundingMode == ROUND_TO_NEAREST || (hf_sign == 0) == (RoundingMode == ROUND_TO_POS_INF);
+		return hf_sign | (round_up? HALF_FLT_INF : HALF_FLT_MAX);
+	}
+
+	// Check underflow to zero
+	if (rebiased_exponent < -HALF_FLT_MANTISSA_BITS)
+	{
+		bool round_up = RoundingMode != ROUND_TO_NEAREST && (hf_sign == 0) == (RoundingMode == ROUND_TO_POS_INF) && (value & FLOAT_EXPONENT_AND_MANTISSA_MASK) != 0;
+		return hf_sign | (round_up? 1 : 0);
+	}
+
+	HalfFloat hf_exponent;
+	int shift;
+	if (rebiased_exponent <= 0)
+	{
+		// Underflow to denormalized number
+		hf_exponent = 0;
+		mantissa |= 1 << FLOAT_MANTISSA_BITS; // Add the implicit 1 bit to the mantissa
+		shift = FLOAT_MANTISSA_BITS - HALF_FLT_MANTISSA_BITS + 1 - rebiased_exponent;
+	}
+	else
+	{
+		// Normal half float
+		hf_exponent = HalfFloat(rebiased_exponent << HALF_FLT_EXPONENT_POS);
+		shift = FLOAT_MANTISSA_BITS - HALF_FLT_MANTISSA_BITS;
+	}
+
+	// Compose the half float
+	HalfFloat hf_mantissa = HalfFloat(mantissa >> shift);
+	HalfFloat hf = hf_sign | hf_exponent | hf_mantissa;
+
+	// Calculate the remaining bits that we're discarding
+	uint remainder = mantissa & ((1 << shift) - 1);
+
+	if constexpr (RoundingMode == ROUND_TO_NEAREST)
+	{
+		// Round to nearest
+		uint round_threshold = 1 << (shift - 1);
+		if (remainder > round_threshold // Above threshold, we must always round
+			|| (remainder == round_threshold && (hf_mantissa & 1))) // When equal, round to nearest even
+			hf++; // May overflow to infinity
+	}
+	else
+	{
+		// Round up or down (truncate) depending on the rounding mode
+		bool round_up = (hf_sign == 0) == (RoundingMode == ROUND_TO_POS_INF) && remainder != 0;
+		if (round_up)
+			hf++; // May overflow to infinity
+	}
+
+	return hf;
+}
+
+/// Convert a float (32-bits) to a half float (16-bits)
+template <int RoundingMode>
+JPH_INLINE HalfFloat FromFloat(float inV)
+{
+#ifdef JPH_USE_F16C
+	union
+	{
+		__m128i		u128;
+		HalfFloat	u16[8];
+	} hf;
+	__m128 val = _mm_load_ss(&inV);
+	switch (RoundingMode)
+	{
+	case ROUND_TO_NEG_INF:
+		hf.u128 = _mm_cvtps_ph(val, _MM_FROUND_TO_NEG_INF);
+		break;
+	case ROUND_TO_POS_INF:
+		hf.u128 = _mm_cvtps_ph(val, _MM_FROUND_TO_POS_INF);
+		break;
+	case ROUND_TO_NEAREST:
+		hf.u128 = _mm_cvtps_ph(val, _MM_FROUND_TO_NEAREST_INT);
+		break;
+	}
+	return hf.u16[0];
+#else
+	return FromFloatFallback<RoundingMode>(inV);
+#endif
+}
+
+/// Convert 4 half floats (lower 64 bits) to floats, fallback version when no intrinsics available
+inline Vec4 ToFloatFallback(UVec4Arg inValue)
+{
+	// Unpack half floats to 4 uint32's
+	UVec4 value = inValue.Expand4Uint16Lo();
+
+	// Normal half float path, extract the exponent and mantissa, shift them into place and update the exponent bias
+	UVec4 exponent_mantissa = UVec4::sAnd(value, UVec4::sReplicate(HALF_FLT_EXPONENT_AND_MANTISSA_MASK)).LogicalShiftLeft<FLOAT_EXPONENT_POS - HALF_FLT_EXPONENT_POS>() + UVec4::sReplicate((FLOAT_EXPONENT_BIAS - HALF_FLT_EXPONENT_BIAS) << FLOAT_EXPONENT_POS);
+
+	// Denormalized half float path, renormalize the float
+	UVec4 exponent_mantissa_denormalized = ((exponent_mantissa + UVec4::sReplicate(1 << FLOAT_EXPONENT_POS)).ReinterpretAsFloat() - UVec4::sReplicate((FLOAT_EXPONENT_BIAS - HALF_FLT_EXPONENT_BIAS + 1) << FLOAT_EXPONENT_POS).ReinterpretAsFloat()).ReinterpretAsInt();
+
+	// NaN / INF path, set all exponent bits
+	UVec4 exponent_mantissa_nan_inf = UVec4::sOr(exponent_mantissa, UVec4::sReplicate(FLOAT_EXPONENT_MASK << FLOAT_EXPONENT_POS));
+
+	// Get the exponent to determine which of the paths we should take
+	UVec4 exponent_mask = UVec4::sReplicate(HALF_FLT_EXPONENT_MASK << HALF_FLT_EXPONENT_POS);
+	UVec4 exponent = UVec4::sAnd(value, exponent_mask);
+	UVec4 is_denormalized = UVec4::sEquals(exponent, UVec4::sZero());
+	UVec4 is_nan_inf = UVec4::sEquals(exponent, exponent_mask);
+
+	// Select the correct result
+	UVec4 result_exponent_mantissa = UVec4::sSelect(UVec4::sSelect(exponent_mantissa, exponent_mantissa_nan_inf, is_nan_inf), exponent_mantissa_denormalized, is_denormalized);
+
+	// Extract the sign bit and shift it to the left
+	UVec4 sign = UVec4::sAnd(value, UVec4::sReplicate(1 << HALF_FLT_SIGN_POS)).LogicalShiftLeft<FLOAT_SIGN_POS - HALF_FLT_SIGN_POS>();
+
+	// Construct the float
+	return UVec4::sOr(sign, result_exponent_mantissa).ReinterpretAsFloat();
+}
+
+/// Convert 4 half floats (lower 64 bits) to floats
+JPH_INLINE Vec4 ToFloat(UVec4Arg inValue)
+{
+#if defined(JPH_USE_F16C)
+	return _mm_cvtph_ps(inValue.mValue);
+#elif defined(JPH_USE_NEON)
+	return vcvt_f32_f16(vreinterpret_f16_u32(vget_low_u32(inValue.mValue)));
+#else
+	return ToFloatFallback(inValue);
+#endif
+}
+
+} // HalfFloatConversion
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Math/Mat44.h b/thirdparty/jolt_physics/Jolt/Math/Mat44.h
new file mode 100644
index 0000000000..4774935efc
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/Mat44.h
@@ -0,0 +1,243 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Math/MathTypes.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Holds a 4x4 matrix of floats, but supports also operations on the 3x3 upper left part of the matrix.
+class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) Mat44
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	// Underlying column type
+	using Type = Vec4::Type;
+
+	// Argument type
+	using ArgType = Mat44Arg;
+
+	/// Constructor
+								Mat44() = default; ///< Intentionally not initialized for performance reasons
+	JPH_INLINE					Mat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, Vec4Arg inC4);
+	JPH_INLINE					Mat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, Vec3Arg inC4);
+								Mat44(const Mat44 &inM2) = default;
+	Mat44 &						operator = (const Mat44 &inM2) = default;
+	JPH_INLINE					Mat44(Type inC1, Type inC2, Type inC3, Type inC4);
+
+	/// Zero matrix
+	static JPH_INLINE Mat44		sZero();
+
+	/// Identity matrix
+	static JPH_INLINE Mat44		sIdentity();
+
+	/// Matrix filled with NaN's
+	static JPH_INLINE Mat44		sNaN();
+
+	/// Load 16 floats from memory
+	static JPH_INLINE Mat44		sLoadFloat4x4(const Float4 *inV);
+
+	/// Load 16 floats from memory, 16 bytes aligned
+	static JPH_INLINE Mat44		sLoadFloat4x4Aligned(const Float4 *inV);
+
+	/// Rotate around X, Y or Z axis (angle in radians)
+	static JPH_INLINE Mat44		sRotationX(float inX);
+	static JPH_INLINE Mat44		sRotationY(float inY);
+	static JPH_INLINE Mat44		sRotationZ(float inZ);
+
+	/// Rotate around arbitrary axis
+	static JPH_INLINE Mat44		sRotation(Vec3Arg inAxis, float inAngle);
+
+	/// Rotate from quaternion
+	static JPH_INLINE Mat44		sRotation(QuatArg inQuat);
+
+	/// Get matrix that translates
+	static JPH_INLINE Mat44		sTranslation(Vec3Arg inV);
+
+	/// Get matrix that rotates and translates
+	static JPH_INLINE Mat44		sRotationTranslation(QuatArg inR, Vec3Arg inT);
+
+	/// Get inverse matrix of sRotationTranslation
+	static JPH_INLINE Mat44		sInverseRotationTranslation(QuatArg inR, Vec3Arg inT);
+
+	/// Get matrix that scales uniformly
+	static JPH_INLINE Mat44		sScale(float inScale);
+
+	/// Get matrix that scales (produces a matrix with (inV, 1) on its diagonal)
+	static JPH_INLINE Mat44		sScale(Vec3Arg inV);
+
+	/// Get outer product of inV and inV2 (equivalent to \f$inV1 \otimes inV2\f$)
+	static JPH_INLINE Mat44		sOuterProduct(Vec3Arg inV1, Vec3Arg inV2);
+
+	/// Get matrix that represents a cross product \f$A \times B = \text{sCrossProduct}(A) \: B\f$
+	static JPH_INLINE Mat44		sCrossProduct(Vec3Arg inV);
+
+	/// Returns matrix ML so that \f$ML(q) \: p = q \: p\f$ (where p and q are quaternions)
+	static JPH_INLINE Mat44		sQuatLeftMultiply(QuatArg inQ);
+
+	/// Returns matrix MR so that \f$MR(q) \: p = p \: q\f$ (where p and q are quaternions)
+	static JPH_INLINE Mat44		sQuatRightMultiply(QuatArg inQ);
+
+	/// Returns a look at matrix that transforms from world space to view space
+	/// @param inPos Position of the camera
+	/// @param inTarget Target of the camera
+	/// @param inUp Up vector
+	static JPH_INLINE Mat44		sLookAt(Vec3Arg inPos, Vec3Arg inTarget, Vec3Arg inUp);
+
+	/// Returns a right-handed perspective projection matrix
+	static JPH_INLINE Mat44		sPerspective(float inFovY, float inAspect, float inNear, float inFar);
+
+	/// Get float component by element index
+	JPH_INLINE float			operator () (uint inRow, uint inColumn) const			{ JPH_ASSERT(inRow < 4); JPH_ASSERT(inColumn < 4); return mCol[inColumn].mF32[inRow]; }
+	JPH_INLINE float &			operator () (uint inRow, uint inColumn)					{ JPH_ASSERT(inRow < 4); JPH_ASSERT(inColumn < 4); return mCol[inColumn].mF32[inRow]; }
+
+	/// Comparison
+	JPH_INLINE bool				operator == (Mat44Arg inM2) const;
+	JPH_INLINE bool				operator != (Mat44Arg inM2) const						{ return !(*this == inM2); }
+
+	/// Test if two matrices are close
+	JPH_INLINE bool				IsClose(Mat44Arg inM2, float inMaxDistSq = 1.0e-12f) const;
+
+	/// Multiply matrix by matrix
+	JPH_INLINE Mat44			operator * (Mat44Arg inM) const;
+
+	/// Multiply vector by matrix
+	JPH_INLINE Vec3				operator * (Vec3Arg inV) const;
+	JPH_INLINE Vec4				operator * (Vec4Arg inV) const;
+
+	/// Multiply vector by only 3x3 part of the matrix
+	JPH_INLINE Vec3				Multiply3x3(Vec3Arg inV) const;
+
+	/// Multiply vector by only 3x3 part of the transpose of the matrix (\f$result = this^T \: inV\f$)
+	JPH_INLINE Vec3				Multiply3x3Transposed(Vec3Arg inV) const;
+
+	/// Multiply 3x3 matrix by 3x3 matrix
+	JPH_INLINE Mat44			Multiply3x3(Mat44Arg inM) const;
+
+	/// Multiply transpose of 3x3 matrix by 3x3 matrix (\f$result = this^T \: inM\f$)
+	JPH_INLINE Mat44			Multiply3x3LeftTransposed(Mat44Arg inM) const;
+
+	/// Multiply 3x3 matrix by the transpose of a 3x3 matrix (\f$result = this \: inM^T\f$)
+	JPH_INLINE Mat44			Multiply3x3RightTransposed(Mat44Arg inM) const;
+
+	/// Multiply matrix with float
+	JPH_INLINE Mat44			operator * (float inV) const;
+	friend JPH_INLINE Mat44		operator * (float inV, Mat44Arg inM)					{ return inM * inV; }
+
+	/// Multiply matrix with float
+	JPH_INLINE Mat44 &			operator *= (float inV);
+
+	/// Per element addition of matrix
+	JPH_INLINE Mat44			operator + (Mat44Arg inM) const;
+
+	/// Negate
+	JPH_INLINE Mat44			operator - () const;
+
+	/// Per element subtraction of matrix
+	JPH_INLINE Mat44			operator - (Mat44Arg inM) const;
+
+	/// Per element addition of matrix
+	JPH_INLINE Mat44 &			operator += (Mat44Arg inM);
+
+	/// Access to the columns
+	JPH_INLINE Vec3				GetAxisX() const										{ return Vec3(mCol[0]); }
+	JPH_INLINE void				SetAxisX(Vec3Arg inV)									{ mCol[0] = Vec4(inV, 0.0f); }
+	JPH_INLINE Vec3				GetAxisY() const										{ return Vec3(mCol[1]); }
+	JPH_INLINE void				SetAxisY(Vec3Arg inV)									{ mCol[1] = Vec4(inV, 0.0f); }
+	JPH_INLINE Vec3				GetAxisZ() const										{ return Vec3(mCol[2]); }
+	JPH_INLINE void				SetAxisZ(Vec3Arg inV)									{ mCol[2] = Vec4(inV, 0.0f); }
+	JPH_INLINE Vec3				GetTranslation() const									{ return Vec3(mCol[3]); }
+	JPH_INLINE void				SetTranslation(Vec3Arg inV)								{ mCol[3] = Vec4(inV, 1.0f); }
+	JPH_INLINE Vec3				GetDiagonal3() const									{ return Vec3(mCol[0][0], mCol[1][1], mCol[2][2]); }
+	JPH_INLINE void				SetDiagonal3(Vec3Arg inV)								{ mCol[0][0] = inV.GetX(); mCol[1][1] = inV.GetY(); mCol[2][2] = inV.GetZ(); }
+	JPH_INLINE Vec4				GetDiagonal4() const									{ return Vec4(mCol[0][0], mCol[1][1], mCol[2][2], mCol[3][3]); }
+	JPH_INLINE void				SetDiagonal4(Vec4Arg inV)								{ mCol[0][0] = inV.GetX(); mCol[1][1] = inV.GetY(); mCol[2][2] = inV.GetZ(); mCol[3][3] = inV.GetW(); }
+	JPH_INLINE Vec3				GetColumn3(uint inCol) const							{ JPH_ASSERT(inCol < 4); return Vec3(mCol[inCol]); }
+	JPH_INLINE void				SetColumn3(uint inCol, Vec3Arg inV)						{ JPH_ASSERT(inCol < 4); mCol[inCol] = Vec4(inV, inCol == 3? 1.0f : 0.0f); }
+	JPH_INLINE Vec4				GetColumn4(uint inCol) const							{ JPH_ASSERT(inCol < 4); return mCol[inCol]; }
+	JPH_INLINE void				SetColumn4(uint inCol, Vec4Arg inV)						{ JPH_ASSERT(inCol < 4); mCol[inCol] = inV; }
+
+	/// Store matrix to memory
+	JPH_INLINE void				StoreFloat4x4(Float4 *outV) const;
+
+	/// Transpose matrix
+	JPH_INLINE Mat44			Transposed() const;
+
+	/// Transpose 3x3 subpart of matrix
+	JPH_INLINE Mat44			Transposed3x3() const;
+
+	/// Inverse 4x4 matrix
+	JPH_INLINE Mat44			Inversed() const;
+
+	/// Inverse 4x4 matrix when it only contains rotation and translation
+	JPH_INLINE Mat44			InversedRotationTranslation() const;
+
+	/// Get the determinant of a 3x3 matrix
+	JPH_INLINE float			GetDeterminant3x3() const;
+
+	/// Get the adjoint of a 3x3 matrix
+	JPH_INLINE Mat44			Adjointed3x3() const;
+
+	/// Inverse 3x3 matrix
+	JPH_INLINE Mat44			Inversed3x3() const;
+
+	/// *this = inM.Inversed3x3(), returns false if the matrix is singular in which case *this is unchanged
+	JPH_INLINE bool				SetInversed3x3(Mat44Arg inM);
+
+	/// Get rotation part only (note: retains the first 3 values from the bottom row)
+	JPH_INLINE Mat44			GetRotation() const;
+
+	/// Get rotation part only (note: also clears the bottom row)
+	JPH_INLINE Mat44			GetRotationSafe() const;
+
+	/// Updates the rotation part of this matrix (the first 3 columns)
+	JPH_INLINE void				SetRotation(Mat44Arg inRotation);
+
+	/// Convert to quaternion
+	JPH_INLINE Quat				GetQuaternion() const;
+
+	/// Get matrix that transforms a direction with the same transform as this matrix (length is not preserved)
+	JPH_INLINE Mat44			GetDirectionPreservingMatrix() const					{ return GetRotation().Inversed3x3().Transposed3x3(); }
+
+	/// Pre multiply by translation matrix: result = this * Mat44::sTranslation(inTranslation)
+	JPH_INLINE Mat44			PreTranslated(Vec3Arg inTranslation) const;
+
+	/// Post multiply by translation matrix: result = Mat44::sTranslation(inTranslation) * this (i.e. add inTranslation to the 4-th column)
+	JPH_INLINE Mat44			PostTranslated(Vec3Arg inTranslation) const;
+
+	/// Scale a matrix: result = this * Mat44::sScale(inScale)
+	JPH_INLINE Mat44			PreScaled(Vec3Arg inScale) const;
+
+	/// Scale a matrix: result = Mat44::sScale(inScale) * this
+	JPH_INLINE Mat44			PostScaled(Vec3Arg inScale) const;
+
+	/// Decompose a matrix into a rotation & translation part and into a scale part so that:
+	/// this = return_value * Mat44::sScale(outScale).
+	/// This equation only holds when the matrix is orthogonal, if it is not the returned matrix
+	/// will be made orthogonal using the modified Gram-Schmidt algorithm (see: https://en.wikipedia.org/wiki/Gram%E2%80%93Schmidt_process)
+	JPH_INLINE Mat44			Decompose(Vec3 &outScale) const;
+
+#ifndef JPH_DOUBLE_PRECISION
+	/// In single precision mode just return the matrix itself
+	JPH_INLINE Mat44			ToMat44() const											{ return *this; }
+#endif // !JPH_DOUBLE_PRECISION
+
+	/// To String
+	friend ostream &			operator << (ostream &inStream, Mat44Arg inM)
+	{
+		inStream << inM.mCol[0] << ", " << inM.mCol[1] << ", " << inM.mCol[2] << ", " << inM.mCol[3];
+		return inStream;
+	}
+
+private:
+	Vec4						mCol[4];												///< Column
+};
+
+static_assert(std::is_trivial<Mat44>(), "Is supposed to be a trivial type!");
+
+JPH_NAMESPACE_END
+
+#include "Mat44.inl"
diff --git a/thirdparty/jolt_physics/Jolt/Math/Mat44.inl b/thirdparty/jolt_physics/Jolt/Math/Mat44.inl
new file mode 100644
index 0000000000..76577b7153
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/Mat44.inl
@@ -0,0 +1,952 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Math/Vec3.h>
+#include <Jolt/Math/Vec4.h>
+#include <Jolt/Math/Quat.h>
+
+JPH_NAMESPACE_BEGIN
+
+#define JPH_EL(r, c) mCol[c].mF32[r]
+
+Mat44::Mat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, Vec4Arg inC4) :
+	mCol { inC1, inC2, inC3, inC4 }
+{
+}
+
+Mat44::Mat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, Vec3Arg inC4) :
+	mCol { inC1, inC2, inC3, Vec4(inC4, 1.0f) }
+{
+}
+
+Mat44::Mat44(Type inC1, Type inC2, Type inC3, Type inC4) :
+	mCol { inC1, inC2, inC3, inC4 }
+{
+}
+
+Mat44 Mat44::sZero()
+{
+	return Mat44(Vec4::sZero(), Vec4::sZero(), Vec4::sZero(), Vec4::sZero());
+}
+
+Mat44 Mat44::sIdentity()
+{
+	return Mat44(Vec4(1, 0, 0, 0), Vec4(0, 1, 0, 0), Vec4(0, 0, 1, 0), Vec4(0, 0, 0, 1));
+}
+
+Mat44 Mat44::sNaN()
+{
+	return Mat44(Vec4::sNaN(), Vec4::sNaN(), Vec4::sNaN(), Vec4::sNaN());
+}
+
+Mat44 Mat44::sLoadFloat4x4(const Float4 *inV)
+{
+	Mat44 result;
+	for (int c = 0; c < 4; ++c)
+		result.mCol[c] = Vec4::sLoadFloat4(inV + c);
+	return result;
+}
+
+Mat44 Mat44::sLoadFloat4x4Aligned(const Float4 *inV)
+{
+	Mat44 result;
+	for (int c = 0; c < 4; ++c)
+		result.mCol[c] = Vec4::sLoadFloat4Aligned(inV + c);
+	return result;
+}
+
+Mat44 Mat44::sRotationX(float inX)
+{
+	Vec4 sv, cv;
+	Vec4::sReplicate(inX).SinCos(sv, cv);
+	float s = sv.GetX(), c = cv.GetX();
+	return Mat44(Vec4(1, 0, 0, 0), Vec4(0, c, s, 0), Vec4(0, -s, c, 0), Vec4(0, 0, 0, 1));
+}
+
+Mat44 Mat44::sRotationY(float inY)
+{
+	Vec4 sv, cv;
+	Vec4::sReplicate(inY).SinCos(sv, cv);
+	float s = sv.GetX(), c = cv.GetX();
+	return Mat44(Vec4(c, 0, -s, 0), Vec4(0, 1, 0, 0), Vec4(s, 0, c, 0), Vec4(0, 0, 0, 1));
+}
+
+Mat44 Mat44::sRotationZ(float inZ)
+{
+	Vec4 sv, cv;
+	Vec4::sReplicate(inZ).SinCos(sv, cv);
+	float s = sv.GetX(), c = cv.GetX();
+	return Mat44(Vec4(c, s, 0, 0), Vec4(-s, c, 0, 0), Vec4(0, 0, 1, 0), Vec4(0, 0, 0, 1));
+}
+
+Mat44 Mat44::sRotation(QuatArg inQuat)
+{
+	JPH_ASSERT(inQuat.IsNormalized());
+
+	// See: https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation section 'Quaternion-derived rotation matrix'
+#ifdef JPH_USE_SSE4_1
+	__m128 xyzw = inQuat.mValue.mValue;
+	__m128 two_xyzw = _mm_add_ps(xyzw, xyzw);
+	__m128 yzxw = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(3, 0, 2, 1));
+	__m128 two_yzxw = _mm_add_ps(yzxw, yzxw);
+	__m128 zxyw = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(3, 1, 0, 2));
+	__m128 two_zxyw = _mm_add_ps(zxyw, zxyw);
+	__m128 wwww = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(3, 3, 3, 3));
+	__m128 diagonal = _mm_sub_ps(_mm_sub_ps(_mm_set1_ps(1.0f), _mm_mul_ps(two_yzxw, yzxw)), _mm_mul_ps(two_zxyw, zxyw));	// (1 - 2 y^2 - 2 z^2, 1 - 2 x^2 - 2 z^2, 1 - 2 x^2 - 2 y^2, 1 - 4 w^2)
+	__m128 plus = _mm_add_ps(_mm_mul_ps(two_xyzw, zxyw), _mm_mul_ps(two_yzxw, wwww));										// 2 * (xz + yw, xy + zw, yz + xw, ww)
+	__m128 minus = _mm_sub_ps(_mm_mul_ps(two_yzxw, xyzw), _mm_mul_ps(two_zxyw, wwww));										// 2 * (xy - zw, yz - xw, xz - yw, 0)
+
+	// Workaround for compiler changing _mm_sub_ps(_mm_mul_ps(...), ...) into a fused multiply sub instruction, resulting in w not being 0
+	// There doesn't appear to be a reliable way to turn this off in Clang
+	minus = _mm_insert_ps(minus, minus, 0b1000);
+
+	__m128 col0 = _mm_blend_ps(_mm_blend_ps(plus, diagonal, 0b0001), minus, 0b1100);	// (1 - 2 y^2 - 2 z^2, 2 xy + 2 zw, 2 xz - 2 yw, 0)
+	__m128 col1 = _mm_blend_ps(_mm_blend_ps(diagonal, minus, 0b1001), plus, 0b0100);	// (2 xy - 2 zw, 1 - 2 x^2 - 2 z^2, 2 yz + 2 xw, 0)
+	__m128 col2 = _mm_blend_ps(_mm_blend_ps(minus, plus, 0b0001), diagonal, 0b0100);	// (2 xz + 2 yw, 2 yz - 2 xw, 1 - 2 x^2 - 2 y^2, 0)
+	__m128 col3 = _mm_set_ps(1, 0, 0, 0);
+
+	return Mat44(col0, col1, col2, col3);
+#else
+	float x = inQuat.GetX();
+	float y = inQuat.GetY();
+	float z = inQuat.GetZ();
+	float w = inQuat.GetW();
+
+	float tx = x + x; // Note: Using x + x instead of 2.0f * x to force this function to return the same value as the SSE4.1 version across platforms.
+	float ty = y + y;
+	float tz = z + z;
+
+	float xx = tx * x;
+	float yy = ty * y;
+	float zz = tz * z;
+	float xy = tx * y;
+	float xz = tx * z;
+	float xw = tx * w;
+	float yz = ty * z;
+	float yw = ty * w;
+	float zw = tz * w;
+
+	return Mat44(Vec4((1.0f - yy) - zz, xy + zw, xz - yw, 0.0f), // Note: Added extra brackets to force this function to return the same value as the SSE4.1 version across platforms.
+				 Vec4(xy - zw, (1.0f - zz) - xx, yz + xw, 0.0f),
+				 Vec4(xz + yw, yz - xw, (1.0f - xx) - yy, 0.0f),
+				 Vec4(0.0f, 0.0f, 0.0f, 1.0f));
+#endif
+}
+
+Mat44 Mat44::sRotation(Vec3Arg inAxis, float inAngle)
+{
+	return sRotation(Quat::sRotation(inAxis, inAngle));
+}
+
+Mat44 Mat44::sTranslation(Vec3Arg inV)
+{
+	return Mat44(Vec4(1, 0, 0, 0), Vec4(0, 1, 0, 0), Vec4(0, 0, 1, 0), Vec4(inV, 1));
+}
+
+Mat44 Mat44::sRotationTranslation(QuatArg inR, Vec3Arg inT)
+{
+	Mat44 m = sRotation(inR);
+	m.SetTranslation(inT);
+	return m;
+}
+
+Mat44 Mat44::sInverseRotationTranslation(QuatArg inR, Vec3Arg inT)
+{
+	Mat44 m = sRotation(inR.Conjugated());
+	m.SetTranslation(-m.Multiply3x3(inT));
+	return m;
+}
+
+Mat44 Mat44::sScale(float inScale)
+{
+	return Mat44(Vec4(inScale, 0, 0, 0), Vec4(0, inScale, 0, 0), Vec4(0, 0, inScale, 0), Vec4(0, 0, 0, 1));
+}
+
+Mat44 Mat44::sScale(Vec3Arg inV)
+{
+	return Mat44(Vec4(inV.GetX(), 0, 0, 0), Vec4(0, inV.GetY(), 0, 0), Vec4(0, 0, inV.GetZ(), 0), Vec4(0, 0, 0, 1));
+}
+
+Mat44 Mat44::sOuterProduct(Vec3Arg inV1, Vec3Arg inV2)
+{
+	Vec4 v1(inV1, 0);
+	return Mat44(v1 * inV2.SplatX(), v1 * inV2.SplatY(), v1 * inV2.SplatZ(), Vec4(0, 0, 0, 1));
+}
+
+Mat44 Mat44::sCrossProduct(Vec3Arg inV)
+{
+#ifdef JPH_USE_SSE4_1
+	// Zero out the W component
+	__m128 zero = _mm_setzero_ps();
+	__m128 v = _mm_blend_ps(inV.mValue, zero, 0b1000);
+
+	// Negate
+	__m128 min_v = _mm_sub_ps(zero, v);
+
+	return Mat44(
+		_mm_shuffle_ps(v, min_v, _MM_SHUFFLE(3, 1, 2, 3)), // [0, z, -y, 0]
+		_mm_shuffle_ps(min_v, v, _MM_SHUFFLE(3, 0, 3, 2)), // [-z, 0, x, 0]
+		_mm_blend_ps(_mm_shuffle_ps(v, v, _MM_SHUFFLE(3, 3, 3, 1)), _mm_shuffle_ps(min_v, min_v, _MM_SHUFFLE(3, 3, 0, 3)), 0b0010), // [y, -x, 0, 0]
+		Vec4(0, 0, 0, 1));
+#else
+	float x = inV.GetX();
+	float y = inV.GetY();
+	float z = inV.GetZ();
+
+	return Mat44(
+		Vec4(0, z, -y, 0),
+		Vec4(-z, 0, x, 0),
+		Vec4(y, -x, 0, 0),
+		Vec4(0, 0, 0, 1));
+#endif
+}
+
+Mat44 Mat44::sLookAt(Vec3Arg inPos, Vec3Arg inTarget, Vec3Arg inUp)
+{
+	Vec3 direction = (inTarget - inPos).NormalizedOr(-Vec3::sAxisZ());
+	Vec3 right = direction.Cross(inUp).NormalizedOr(Vec3::sAxisX());
+	Vec3 up = right.Cross(direction);
+
+	return Mat44(Vec4(right, 0), Vec4(up, 0), Vec4(-direction, 0), Vec4(inPos, 1)).InversedRotationTranslation();
+}
+
+Mat44 Mat44::sPerspective(float inFovY, float inAspect, float inNear, float inFar)
+{
+	float height = 1.0f / Tan(0.5f * inFovY);
+	float width = height / inAspect;
+	float range = inFar / (inNear - inFar);
+
+	return Mat44(Vec4(width, 0.0f, 0.0f, 0.0f), Vec4(0.0f, height, 0.0f, 0.0f), Vec4(0.0f, 0.0f, range, -1.0f), Vec4(0.0f, 0.0f, range * inNear, 0.0f));
+}
+
+bool Mat44::operator == (Mat44Arg inM2) const
+{
+	return UVec4::sAnd(
+		UVec4::sAnd(Vec4::sEquals(mCol[0], inM2.mCol[0]), Vec4::sEquals(mCol[1], inM2.mCol[1])),
+		UVec4::sAnd(Vec4::sEquals(mCol[2], inM2.mCol[2]), Vec4::sEquals(mCol[3], inM2.mCol[3]))
+	).TestAllTrue();
+}
+
+bool Mat44::IsClose(Mat44Arg inM2, float inMaxDistSq) const
+{
+	for (int i = 0; i < 4; ++i)
+		if (!mCol[i].IsClose(inM2.mCol[i], inMaxDistSq))
+			return false;
+	return true;
+}
+
+Mat44 Mat44::operator * (Mat44Arg inM) const
+{
+	Mat44 result;
+#if defined(JPH_USE_SSE)
+	for (int i = 0; i < 4; ++i)
+	{
+		__m128 c = inM.mCol[i].mValue;
+		__m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0)));
+		t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1))));
+		t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2))));
+		t = _mm_add_ps(t, _mm_mul_ps(mCol[3].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(3, 3, 3, 3))));
+		result.mCol[i].mValue = t;
+	}
+#elif defined(JPH_USE_NEON)
+	for (int i = 0; i < 4; ++i)
+	{
+		Type c = inM.mCol[i].mValue;
+		Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(c, 0));
+		t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(c, 1));
+		t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(c, 2));
+		t = vmlaq_f32(t, mCol[3].mValue, vdupq_laneq_f32(c, 3));
+		result.mCol[i].mValue = t;
+	}
+#else
+	for (int i = 0; i < 4; ++i)
+		result.mCol[i] = mCol[0] * inM.mCol[i].mF32[0] + mCol[1] * inM.mCol[i].mF32[1] + mCol[2] * inM.mCol[i].mF32[2] + mCol[3] * inM.mCol[i].mF32[3];
+#endif
+	return result;
+}
+
+Vec3 Mat44::operator * (Vec3Arg inV) const
+{
+#if defined(JPH_USE_SSE)
+	__m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0)));
+	t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1))));
+	t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2))));
+	t = _mm_add_ps(t, mCol[3].mValue);
+	return Vec3::sFixW(t);
+#elif defined(JPH_USE_NEON)
+	Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(inV.mValue, 0));
+	t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(inV.mValue, 1));
+	t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(inV.mValue, 2));
+	t = vaddq_f32(t, mCol[3].mValue); // Don't combine this with the first mul into a fused multiply add, causes precision issues
+	return Vec3::sFixW(t);
+#else
+	return Vec3(
+		mCol[0].mF32[0] * inV.mF32[0] + mCol[1].mF32[0] * inV.mF32[1] + mCol[2].mF32[0] * inV.mF32[2] + mCol[3].mF32[0],
+		mCol[0].mF32[1] * inV.mF32[0] + mCol[1].mF32[1] * inV.mF32[1] + mCol[2].mF32[1] * inV.mF32[2] + mCol[3].mF32[1],
+		mCol[0].mF32[2] * inV.mF32[0] + mCol[1].mF32[2] * inV.mF32[1] + mCol[2].mF32[2] * inV.mF32[2] + mCol[3].mF32[2]);
+#endif
+}
+
+Vec4 Mat44::operator * (Vec4Arg inV) const
+{
+#if defined(JPH_USE_SSE)
+	__m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0)));
+	t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1))));
+	t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2))));
+	t = _mm_add_ps(t, _mm_mul_ps(mCol[3].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(3, 3, 3, 3))));
+	return t;
+#elif defined(JPH_USE_NEON)
+	Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(inV.mValue, 0));
+	t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(inV.mValue, 1));
+	t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(inV.mValue, 2));
+	t = vmlaq_f32(t, mCol[3].mValue, vdupq_laneq_f32(inV.mValue, 3));
+	return t;
+#else
+	return Vec4(
+		mCol[0].mF32[0] * inV.mF32[0] + mCol[1].mF32[0] * inV.mF32[1] + mCol[2].mF32[0] * inV.mF32[2] + mCol[3].mF32[0] * inV.mF32[3],
+		mCol[0].mF32[1] * inV.mF32[0] + mCol[1].mF32[1] * inV.mF32[1] + mCol[2].mF32[1] * inV.mF32[2] + mCol[3].mF32[1] * inV.mF32[3],
+		mCol[0].mF32[2] * inV.mF32[0] + mCol[1].mF32[2] * inV.mF32[1] + mCol[2].mF32[2] * inV.mF32[2] + mCol[3].mF32[2] * inV.mF32[3],
+		mCol[0].mF32[3] * inV.mF32[0] + mCol[1].mF32[3] * inV.mF32[1] + mCol[2].mF32[3] * inV.mF32[2] + mCol[3].mF32[3] * inV.mF32[3]);
+#endif
+}
+
+Vec3 Mat44::Multiply3x3(Vec3Arg inV) const
+{
+#if defined(JPH_USE_SSE)
+	__m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0)));
+	t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1))));
+	t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2))));
+	return Vec3::sFixW(t);
+#elif defined(JPH_USE_NEON)
+	Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(inV.mValue, 0));
+	t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(inV.mValue, 1));
+	t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(inV.mValue, 2));
+	return Vec3::sFixW(t);
+#else
+	return Vec3(
+		mCol[0].mF32[0] * inV.mF32[0] + mCol[1].mF32[0] * inV.mF32[1] + mCol[2].mF32[0] * inV.mF32[2],
+		mCol[0].mF32[1] * inV.mF32[0] + mCol[1].mF32[1] * inV.mF32[1] + mCol[2].mF32[1] * inV.mF32[2],
+		mCol[0].mF32[2] * inV.mF32[0] + mCol[1].mF32[2] * inV.mF32[1] + mCol[2].mF32[2] * inV.mF32[2]);
+#endif
+}
+
+Vec3 Mat44::Multiply3x3Transposed(Vec3Arg inV) const
+{
+#if defined(JPH_USE_SSE4_1)
+	__m128 x = _mm_dp_ps(mCol[0].mValue, inV.mValue, 0x7f);
+	__m128 y = _mm_dp_ps(mCol[1].mValue, inV.mValue, 0x7f);
+	__m128 xy = _mm_blend_ps(x, y, 0b0010);
+	__m128 z = _mm_dp_ps(mCol[2].mValue, inV.mValue, 0x7f);
+	__m128 xyzz = _mm_blend_ps(xy, z, 0b1100);
+	return xyzz;
+#else
+	return Transposed3x3().Multiply3x3(inV);
+#endif
+}
+
+Mat44 Mat44::Multiply3x3(Mat44Arg inM) const
+{
+	JPH_ASSERT(mCol[0][3] == 0.0f);
+	JPH_ASSERT(mCol[1][3] == 0.0f);
+	JPH_ASSERT(mCol[2][3] == 0.0f);
+
+	Mat44 result;
+#if defined(JPH_USE_SSE)
+	for (int i = 0; i < 3; ++i)
+	{
+		__m128 c = inM.mCol[i].mValue;
+		__m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0)));
+		t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1))));
+		t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2))));
+		result.mCol[i].mValue = t;
+	}
+#elif defined(JPH_USE_NEON)
+	for (int i = 0; i < 3; ++i)
+	{
+		Type c = inM.mCol[i].mValue;
+		Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(c, 0));
+		t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(c, 1));
+		t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(c, 2));
+		result.mCol[i].mValue = t;
+	}
+#else
+	for (int i = 0; i < 3; ++i)
+		result.mCol[i] = mCol[0] * inM.mCol[i].mF32[0] + mCol[1] * inM.mCol[i].mF32[1] + mCol[2] * inM.mCol[i].mF32[2];
+#endif
+	result.mCol[3] = Vec4(0, 0, 0, 1);
+	return result;
+}
+
+Mat44 Mat44::Multiply3x3LeftTransposed(Mat44Arg inM) const
+{
+	// Transpose left hand side
+	Mat44 trans = Transposed3x3();
+
+	// Do 3x3 matrix multiply
+	Mat44 result;
+	result.mCol[0] = trans.mCol[0] * inM.mCol[0].SplatX() + trans.mCol[1] * inM.mCol[0].SplatY() + trans.mCol[2] * inM.mCol[0].SplatZ();
+	result.mCol[1] = trans.mCol[0] * inM.mCol[1].SplatX() + trans.mCol[1] * inM.mCol[1].SplatY() + trans.mCol[2] * inM.mCol[1].SplatZ();
+	result.mCol[2] = trans.mCol[0] * inM.mCol[2].SplatX() + trans.mCol[1] * inM.mCol[2].SplatY() + trans.mCol[2] * inM.mCol[2].SplatZ();
+	result.mCol[3] = Vec4(0, 0, 0, 1);
+	return result;
+}
+
+Mat44 Mat44::Multiply3x3RightTransposed(Mat44Arg inM) const
+{
+	JPH_ASSERT(mCol[0][3] == 0.0f);
+	JPH_ASSERT(mCol[1][3] == 0.0f);
+	JPH_ASSERT(mCol[2][3] == 0.0f);
+
+	Mat44 result;
+	result.mCol[0] = mCol[0] * inM.mCol[0].SplatX() + mCol[1] * inM.mCol[1].SplatX() + mCol[2] * inM.mCol[2].SplatX();
+	result.mCol[1] = mCol[0] * inM.mCol[0].SplatY() + mCol[1] * inM.mCol[1].SplatY() + mCol[2] * inM.mCol[2].SplatY();
+	result.mCol[2] = mCol[0] * inM.mCol[0].SplatZ() + mCol[1] * inM.mCol[1].SplatZ() + mCol[2] * inM.mCol[2].SplatZ();
+	result.mCol[3] = Vec4(0, 0, 0, 1);
+	return result;
+}
+
+Mat44 Mat44::operator * (float inV) const
+{
+	Vec4 multiplier = Vec4::sReplicate(inV);
+
+	Mat44 result;
+	for (int c = 0; c < 4; ++c)
+		result.mCol[c] = mCol[c] * multiplier;
+	return result;
+}
+
+Mat44 &Mat44::operator *= (float inV)
+{
+	for (int c = 0; c < 4; ++c)
+		mCol[c] *= inV;
+
+	return *this;
+}
+
+Mat44 Mat44::operator + (Mat44Arg inM) const
+{
+	Mat44 result;
+	for (int i = 0; i < 4; ++i)
+		result.mCol[i] = mCol[i] + inM.mCol[i];
+	return result;
+}
+
+Mat44 Mat44::operator - () const
+{
+	Mat44 result;
+	for (int i = 0; i < 4; ++i)
+		result.mCol[i] = -mCol[i];
+	return result;
+}
+
+Mat44 Mat44::operator - (Mat44Arg inM) const
+{
+	Mat44 result;
+	for (int i = 0; i < 4; ++i)
+		result.mCol[i] = mCol[i] - inM.mCol[i];
+	return result;
+}
+
+Mat44 &Mat44::operator += (Mat44Arg inM)
+{
+	for (int c = 0; c < 4; ++c)
+		mCol[c] += inM.mCol[c];
+
+	return *this;
+}
+
+void Mat44::StoreFloat4x4(Float4 *outV) const
+{
+	for (int c = 0; c < 4; ++c)
+		mCol[c].StoreFloat4(outV + c);
+}
+
+Mat44 Mat44::Transposed() const
+{
+#if defined(JPH_USE_SSE)
+	__m128 tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(1, 0, 1, 0));
+	__m128 tmp3 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(3, 2, 3, 2));
+	__m128 tmp2 = _mm_shuffle_ps(mCol[2].mValue, mCol[3].mValue, _MM_SHUFFLE(1, 0, 1, 0));
+	__m128 tmp4 = _mm_shuffle_ps(mCol[2].mValue, mCol[3].mValue, _MM_SHUFFLE(3, 2, 3, 2));
+
+	Mat44 result;
+	result.mCol[0].mValue = _mm_shuffle_ps(tmp1, tmp2, _MM_SHUFFLE(2, 0, 2, 0));
+	result.mCol[1].mValue = _mm_shuffle_ps(tmp1, tmp2, _MM_SHUFFLE(3, 1, 3, 1));
+	result.mCol[2].mValue = _mm_shuffle_ps(tmp3, tmp4, _MM_SHUFFLE(2, 0, 2, 0));
+	result.mCol[3].mValue = _mm_shuffle_ps(tmp3, tmp4, _MM_SHUFFLE(3, 1, 3, 1));
+	return result;
+#elif defined(JPH_USE_NEON)
+	float32x4x2_t tmp1 = vzipq_f32(mCol[0].mValue, mCol[2].mValue);
+	float32x4x2_t tmp2 = vzipq_f32(mCol[1].mValue, mCol[3].mValue);
+	float32x4x2_t tmp3 = vzipq_f32(tmp1.val[0], tmp2.val[0]);
+	float32x4x2_t tmp4 = vzipq_f32(tmp1.val[1], tmp2.val[1]);
+
+	Mat44 result;
+	result.mCol[0].mValue = tmp3.val[0];
+	result.mCol[1].mValue = tmp3.val[1];
+	result.mCol[2].mValue = tmp4.val[0];
+	result.mCol[3].mValue = tmp4.val[1];
+	return result;
+#else
+	Mat44 result;
+	for (int c = 0; c < 4; ++c)
+		for (int r = 0; r < 4; ++r)
+			result.mCol[r].mF32[c] = mCol[c].mF32[r];
+	return result;
+#endif
+}
+
+Mat44 Mat44::Transposed3x3() const
+{
+#if defined(JPH_USE_SSE)
+	__m128 zero = _mm_setzero_ps();
+	__m128 tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(1, 0, 1, 0));
+	__m128 tmp3 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(3, 2, 3, 2));
+	__m128 tmp2 = _mm_shuffle_ps(mCol[2].mValue, zero, _MM_SHUFFLE(1, 0, 1, 0));
+	__m128 tmp4 = _mm_shuffle_ps(mCol[2].mValue, zero, _MM_SHUFFLE(3, 2, 3, 2));
+
+	Mat44 result;
+	result.mCol[0].mValue = _mm_shuffle_ps(tmp1, tmp2, _MM_SHUFFLE(2, 0, 2, 0));
+	result.mCol[1].mValue = _mm_shuffle_ps(tmp1, tmp2, _MM_SHUFFLE(3, 1, 3, 1));
+	result.mCol[2].mValue = _mm_shuffle_ps(tmp3, tmp4, _MM_SHUFFLE(2, 0, 2, 0));
+#elif defined(JPH_USE_NEON)
+	float32x4x2_t tmp1 = vzipq_f32(mCol[0].mValue, mCol[2].mValue);
+	float32x4x2_t tmp2 = vzipq_f32(mCol[1].mValue, vdupq_n_f32(0));
+	float32x4x2_t tmp3 = vzipq_f32(tmp1.val[0], tmp2.val[0]);
+	float32x4x2_t tmp4 = vzipq_f32(tmp1.val[1], tmp2.val[1]);
+
+	Mat44 result;
+	result.mCol[0].mValue = tmp3.val[0];
+	result.mCol[1].mValue = tmp3.val[1];
+	result.mCol[2].mValue = tmp4.val[0];
+#else
+	Mat44 result;
+	for (int c = 0; c < 3; ++c)
+	{
+		for (int r = 0; r < 3; ++r)
+			result.mCol[c].mF32[r] = mCol[r].mF32[c];
+		result.mCol[c].mF32[3] = 0;
+	}
+#endif
+	result.mCol[3] = Vec4(0, 0, 0, 1);
+	return result;
+}
+
+Mat44 Mat44::Inversed() const
+{
+#if defined(JPH_USE_SSE)
+	// Algorithm from: http://download.intel.com/design/PentiumIII/sml/24504301.pdf
+	// Streaming SIMD Extensions - Inverse of 4x4 Matrix
+	// Adapted to load data using _mm_shuffle_ps instead of loading from memory
+	// Replaced _mm_rcp_ps with _mm_div_ps for better accuracy
+
+	__m128 tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(1, 0, 1, 0));
+	__m128 row1 = _mm_shuffle_ps(mCol[2].mValue, mCol[3].mValue, _MM_SHUFFLE(1, 0, 1, 0));
+	__m128 row0 = _mm_shuffle_ps(tmp1, row1, _MM_SHUFFLE(2, 0, 2, 0));
+	row1 = _mm_shuffle_ps(row1, tmp1, _MM_SHUFFLE(3, 1, 3, 1));
+	tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(3, 2, 3, 2));
+	__m128 row3 = _mm_shuffle_ps(mCol[2].mValue, mCol[3].mValue, _MM_SHUFFLE(3, 2, 3, 2));
+	__m128 row2 = _mm_shuffle_ps(tmp1, row3, _MM_SHUFFLE(2, 0, 2, 0));
+	row3 = _mm_shuffle_ps(row3, tmp1, _MM_SHUFFLE(3, 1, 3, 1));
+
+	tmp1 = _mm_mul_ps(row2, row3);
+	tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
+	__m128 minor0 = _mm_mul_ps(row1, tmp1);
+	__m128 minor1 = _mm_mul_ps(row0, tmp1);
+	tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
+	minor0 = _mm_sub_ps(_mm_mul_ps(row1, tmp1), minor0);
+	minor1 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor1);
+	minor1 = _mm_shuffle_ps(minor1, minor1, _MM_SHUFFLE(1, 0, 3, 2));
+
+	tmp1 = _mm_mul_ps(row1, row2);
+	tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
+	minor0 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor0);
+	__m128 minor3 = _mm_mul_ps(row0, tmp1);
+	tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
+	minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row3, tmp1));
+	minor3 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor3);
+	minor3 = _mm_shuffle_ps(minor3, minor3, _MM_SHUFFLE(1, 0, 3, 2));
+
+	tmp1 = _mm_mul_ps(_mm_shuffle_ps(row1, row1, _MM_SHUFFLE(1, 0, 3, 2)), row3);
+	tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
+	row2 = _mm_shuffle_ps(row2, row2, _MM_SHUFFLE(1, 0, 3, 2));
+	minor0 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor0);
+	__m128 minor2 = _mm_mul_ps(row0, tmp1);
+	tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
+	minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row2, tmp1));
+	minor2 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor2);
+	minor2 = _mm_shuffle_ps(minor2, minor2, _MM_SHUFFLE(1, 0, 3, 2));
+
+	tmp1 = _mm_mul_ps(row0, row1);
+	tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
+	minor2 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor2);
+	minor3 = _mm_sub_ps(_mm_mul_ps(row2, tmp1), minor3);
+	tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
+	minor2 = _mm_sub_ps(_mm_mul_ps(row3, tmp1), minor2);
+	minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row2, tmp1));
+
+	tmp1 = _mm_mul_ps(row0, row3);
+	tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
+	minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row2, tmp1));
+	minor2 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor2);
+	tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
+	minor1 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor1);
+	minor2 = _mm_sub_ps(minor2, _mm_mul_ps(row1, tmp1));
+
+	tmp1 = _mm_mul_ps(row0, row2);
+	tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1));
+	minor1 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor1);
+	minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row1, tmp1));
+	tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2));
+	minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row3, tmp1));
+	minor3 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor3);
+
+	__m128 det = _mm_mul_ps(row0, minor0);
+	det = _mm_add_ps(_mm_shuffle_ps(det, det, _MM_SHUFFLE(2, 3, 0, 1)), det); // Original code did (x + z) + (y + w), changed to (x + y) + (z + w) to match the ARM code below and make the result cross platform deterministic
+	det = _mm_add_ss(_mm_shuffle_ps(det, det, _MM_SHUFFLE(1, 0, 3, 2)), det);
+	det = _mm_div_ss(_mm_set_ss(1.0f), det);
+	det = _mm_shuffle_ps(det, det, _MM_SHUFFLE(0, 0, 0, 0));
+
+	Mat44 result;
+	result.mCol[0].mValue = _mm_mul_ps(det, minor0);
+	result.mCol[1].mValue = _mm_mul_ps(det, minor1);
+	result.mCol[2].mValue = _mm_mul_ps(det, minor2);
+	result.mCol[3].mValue = _mm_mul_ps(det, minor3);
+	return result;
+#elif defined(JPH_USE_NEON)
+	// Adapted from the SSE version, there's surprising few articles about efficient ways of calculating an inverse for ARM on the internet
+	Type tmp1 = JPH_NEON_SHUFFLE_F32x4(mCol[0].mValue, mCol[1].mValue, 0, 1, 4, 5);
+	Type row1 = JPH_NEON_SHUFFLE_F32x4(mCol[2].mValue, mCol[3].mValue, 0, 1, 4, 5);
+	Type row0 = JPH_NEON_SHUFFLE_F32x4(tmp1, row1, 0, 2, 4, 6);
+	row1 = JPH_NEON_SHUFFLE_F32x4(row1, tmp1, 1, 3, 5, 7);
+	tmp1 = JPH_NEON_SHUFFLE_F32x4(mCol[0].mValue, mCol[1].mValue, 2, 3, 6, 7);
+	Type row3 = JPH_NEON_SHUFFLE_F32x4(mCol[2].mValue, mCol[3].mValue, 2, 3, 6, 7);
+	Type row2 = JPH_NEON_SHUFFLE_F32x4(tmp1, row3, 0, 2, 4, 6);
+	row3 = JPH_NEON_SHUFFLE_F32x4(row3, tmp1, 1, 3, 5, 7);
+
+	tmp1 = vmulq_f32(row2, row3);
+	tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
+	Type minor0 = vmulq_f32(row1, tmp1);
+	Type minor1 = vmulq_f32(row0, tmp1);
+	tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
+	minor0 = vsubq_f32(vmulq_f32(row1, tmp1), minor0);
+	minor1 = vsubq_f32(vmulq_f32(row0, tmp1), minor1);
+	minor1 = JPH_NEON_SHUFFLE_F32x4(minor1, minor1, 2, 3, 0, 1);
+
+	tmp1 = vmulq_f32(row1, row2);
+	tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
+	minor0 = vaddq_f32(vmulq_f32(row3, tmp1), minor0);
+	Type minor3 = vmulq_f32(row0, tmp1);
+	tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
+	minor0 = vsubq_f32(minor0, vmulq_f32(row3, tmp1));
+	minor3 = vsubq_f32(vmulq_f32(row0, tmp1), minor3);
+	minor3 = JPH_NEON_SHUFFLE_F32x4(minor3, minor3, 2, 3, 0, 1);
+
+	tmp1 = JPH_NEON_SHUFFLE_F32x4(row1, row1, 2, 3, 0, 1);
+	tmp1 = vmulq_f32(tmp1, row3);
+	tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
+	row2 = JPH_NEON_SHUFFLE_F32x4(row2, row2, 2, 3, 0, 1);
+	minor0 = vaddq_f32(vmulq_f32(row2, tmp1), minor0);
+	Type minor2 = vmulq_f32(row0, tmp1);
+	tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
+	minor0 = vsubq_f32(minor0, vmulq_f32(row2, tmp1));
+	minor2 = vsubq_f32(vmulq_f32(row0, tmp1), minor2);
+	minor2 = JPH_NEON_SHUFFLE_F32x4(minor2, minor2, 2, 3, 0, 1);
+
+	tmp1 = vmulq_f32(row0, row1);
+	tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
+	minor2 = vaddq_f32(vmulq_f32(row3, tmp1), minor2);
+	minor3 = vsubq_f32(vmulq_f32(row2, tmp1), minor3);
+	tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
+	minor2 = vsubq_f32(vmulq_f32(row3, tmp1), minor2);
+	minor3 = vsubq_f32(minor3, vmulq_f32(row2, tmp1));
+
+	tmp1 = vmulq_f32(row0, row3);
+	tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
+	minor1 = vsubq_f32(minor1, vmulq_f32(row2, tmp1));
+	minor2 = vaddq_f32(vmulq_f32(row1, tmp1), minor2);
+	tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
+	minor1 = vaddq_f32(vmulq_f32(row2, tmp1), minor1);
+	minor2 = vsubq_f32(minor2, vmulq_f32(row1, tmp1));
+
+	tmp1 = vmulq_f32(row0, row2);
+	tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2);
+	minor1 = vaddq_f32(vmulq_f32(row3, tmp1), minor1);
+	minor3 = vsubq_f32(minor3, vmulq_f32(row1, tmp1));
+	tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1);
+	minor1 = vsubq_f32(minor1, vmulq_f32(row3, tmp1));
+	minor3 = vaddq_f32(vmulq_f32(row1, tmp1), minor3);
+
+	Type det = vmulq_f32(row0, minor0);
+	det = vdupq_n_f32(vaddvq_f32(det));
+	det = vdivq_f32(vdupq_n_f32(1.0f), det);
+
+	Mat44 result;
+	result.mCol[0].mValue = vmulq_f32(det, minor0);
+	result.mCol[1].mValue = vmulq_f32(det, minor1);
+	result.mCol[2].mValue = vmulq_f32(det, minor2);
+	result.mCol[3].mValue = vmulq_f32(det, minor3);
+	return result;
+#else
+	float m00 = JPH_EL(0, 0), m10 = JPH_EL(1, 0), m20 = JPH_EL(2, 0), m30 = JPH_EL(3, 0);
+	float m01 = JPH_EL(0, 1), m11 = JPH_EL(1, 1), m21 = JPH_EL(2, 1), m31 = JPH_EL(3, 1);
+	float m02 = JPH_EL(0, 2), m12 = JPH_EL(1, 2), m22 = JPH_EL(2, 2), m32 = JPH_EL(3, 2);
+	float m03 = JPH_EL(0, 3), m13 = JPH_EL(1, 3), m23 = JPH_EL(2, 3), m33 = JPH_EL(3, 3);
+
+	float m10211120 = m10 * m21 - m11 * m20;
+	float m10221220 = m10 * m22 - m12 * m20;
+	float m10231320 = m10 * m23 - m13 * m20;
+	float m10311130 = m10 * m31 - m11 * m30;
+	float m10321230 = m10 * m32 - m12 * m30;
+	float m10331330 = m10 * m33 - m13 * m30;
+	float m11221221 = m11 * m22 - m12 * m21;
+	float m11231321 = m11 * m23 - m13 * m21;
+	float m11321231 = m11 * m32 - m12 * m31;
+	float m11331331 = m11 * m33 - m13 * m31;
+	float m12231322 = m12 * m23 - m13 * m22;
+	float m12331332 = m12 * m33 - m13 * m32;
+	float m20312130 = m20 * m31 - m21 * m30;
+	float m20322230 = m20 * m32 - m22 * m30;
+	float m20332330 = m20 * m33 - m23 * m30;
+	float m21322231 = m21 * m32 - m22 * m31;
+	float m21332331 = m21 * m33 - m23 * m31;
+	float m22332332 = m22 * m33 - m23 * m32;
+
+	Vec4 col0(m11 * m22332332 - m12 * m21332331 + m13 * m21322231,		-m10 * m22332332 + m12 * m20332330 - m13 * m20322230,		m10 * m21332331 - m11 * m20332330 + m13 * m20312130,		-m10 * m21322231 + m11 * m20322230 - m12 * m20312130);
+	Vec4 col1(-m01 * m22332332 + m02 * m21332331 - m03 * m21322231,		m00 * m22332332 - m02 * m20332330 + m03 * m20322230,		-m00 * m21332331 + m01 * m20332330 - m03 * m20312130,		m00 * m21322231 - m01 * m20322230 + m02 * m20312130);
+	Vec4 col2(m01 * m12331332 - m02 * m11331331 + m03 * m11321231,		-m00 * m12331332 + m02 * m10331330 - m03 * m10321230,		m00 * m11331331 - m01 * m10331330 + m03 * m10311130,		-m00 * m11321231 + m01 * m10321230 - m02 * m10311130);
+	Vec4 col3(-m01 * m12231322 + m02 * m11231321 - m03 * m11221221,		m00 * m12231322 - m02 * m10231320 + m03 * m10221220,		-m00 * m11231321 + m01 * m10231320 - m03 * m10211120,		m00 * m11221221 - m01 * m10221220 + m02 * m10211120);
+
+	float det = m00 * col0.mF32[0] + m01 * col0.mF32[1] + m02 * col0.mF32[2] + m03 * col0.mF32[3];
+
+	return Mat44(col0 / det, col1 / det, col2 / det, col3 / det);
+#endif
+}
+
+Mat44 Mat44::InversedRotationTranslation() const
+{
+	Mat44 m = Transposed3x3();
+	m.SetTranslation(-m.Multiply3x3(GetTranslation()));
+	return m;
+}
+
+float Mat44::GetDeterminant3x3() const
+{
+	return GetAxisX().Dot(GetAxisY().Cross(GetAxisZ()));
+}
+
+Mat44 Mat44::Adjointed3x3() const
+{
+	return Mat44(
+		Vec4(JPH_EL(1, 1), JPH_EL(1, 2), JPH_EL(1, 0), 0) * Vec4(JPH_EL(2, 2), JPH_EL(2, 0), JPH_EL(2, 1), 0)
+			- Vec4(JPH_EL(1, 2), JPH_EL(1, 0), JPH_EL(1, 1), 0) * Vec4(JPH_EL(2, 1), JPH_EL(2, 2), JPH_EL(2, 0), 0),
+		Vec4(JPH_EL(0, 2), JPH_EL(0, 0), JPH_EL(0, 1), 0) * Vec4(JPH_EL(2, 1), JPH_EL(2, 2), JPH_EL(2, 0), 0)
+			- Vec4(JPH_EL(0, 1), JPH_EL(0, 2), JPH_EL(0, 0), 0) * Vec4(JPH_EL(2, 2), JPH_EL(2, 0), JPH_EL(2, 1), 0),
+		Vec4(JPH_EL(0, 1), JPH_EL(0, 2), JPH_EL(0, 0), 0) * Vec4(JPH_EL(1, 2), JPH_EL(1, 0), JPH_EL(1, 1), 0)
+			- Vec4(JPH_EL(0, 2), JPH_EL(0, 0), JPH_EL(0, 1), 0) * Vec4(JPH_EL(1, 1), JPH_EL(1, 2), JPH_EL(1, 0), 0),
+		Vec4(0, 0, 0, 1));
+}
+
+Mat44 Mat44::Inversed3x3() const
+{
+	float det = GetDeterminant3x3();
+
+	return Mat44(
+		(Vec4(JPH_EL(1, 1), JPH_EL(1, 2), JPH_EL(1, 0), 0) * Vec4(JPH_EL(2, 2), JPH_EL(2, 0), JPH_EL(2, 1), 0)
+			- Vec4(JPH_EL(1, 2), JPH_EL(1, 0), JPH_EL(1, 1), 0) * Vec4(JPH_EL(2, 1), JPH_EL(2, 2), JPH_EL(2, 0), 0)) / det,
+		(Vec4(JPH_EL(0, 2), JPH_EL(0, 0), JPH_EL(0, 1), 0) * Vec4(JPH_EL(2, 1), JPH_EL(2, 2), JPH_EL(2, 0), 0)
+			- Vec4(JPH_EL(0, 1), JPH_EL(0, 2), JPH_EL(0, 0), 0) * Vec4(JPH_EL(2, 2), JPH_EL(2, 0), JPH_EL(2, 1), 0)) / det,
+		(Vec4(JPH_EL(0, 1), JPH_EL(0, 2), JPH_EL(0, 0), 0) * Vec4(JPH_EL(1, 2), JPH_EL(1, 0), JPH_EL(1, 1), 0)
+			- Vec4(JPH_EL(0, 2), JPH_EL(0, 0), JPH_EL(0, 1), 0) * Vec4(JPH_EL(1, 1), JPH_EL(1, 2), JPH_EL(1, 0), 0)) / det,
+		Vec4(0, 0, 0, 1));
+}
+
+bool Mat44::SetInversed3x3(Mat44Arg inM)
+{
+	float det = inM.GetDeterminant3x3();
+
+	// If the determinant is zero the matrix is singular and we return false
+	if (det == 0.0f)
+		return false;
+
+	// Finish calculating the inverse
+	*this = inM.Adjointed3x3();
+	mCol[0] /= det;
+	mCol[1] /= det;
+	mCol[2] /= det;
+	return true;
+}
+
+Quat Mat44::GetQuaternion() const
+{
+	float tr = mCol[0].mF32[0] + mCol[1].mF32[1] + mCol[2].mF32[2];
+
+	if (tr >= 0.0f)
+	{
+		float s = sqrt(tr + 1.0f);
+		float is = 0.5f / s;
+		return Quat(
+			(mCol[1].mF32[2] - mCol[2].mF32[1]) * is,
+			(mCol[2].mF32[0] - mCol[0].mF32[2]) * is,
+			(mCol[0].mF32[1] - mCol[1].mF32[0]) * is,
+			0.5f * s);
+	}
+	else
+	{
+		int i = 0;
+		if (mCol[1].mF32[1] > mCol[0].mF32[0]) i = 1;
+		if (mCol[2].mF32[2] > mCol[i].mF32[i]) i = 2;
+
+		if (i == 0)
+		{
+			float s = sqrt(mCol[0].mF32[0] - (mCol[1].mF32[1] + mCol[2].mF32[2]) + 1);
+			float is = 0.5f / s;
+			return Quat(
+				0.5f * s,
+				(mCol[1].mF32[0] + mCol[0].mF32[1]) * is,
+				(mCol[0].mF32[2] + mCol[2].mF32[0]) * is,
+				(mCol[1].mF32[2] - mCol[2].mF32[1]) * is);
+		}
+		else if (i == 1)
+		{
+			float s = sqrt(mCol[1].mF32[1] - (mCol[2].mF32[2] + mCol[0].mF32[0]) + 1);
+			float is = 0.5f / s;
+			return Quat(
+				(mCol[1].mF32[0] + mCol[0].mF32[1]) * is,
+				0.5f * s,
+				(mCol[2].mF32[1] + mCol[1].mF32[2]) * is,
+				(mCol[2].mF32[0] - mCol[0].mF32[2]) * is);
+		}
+		else
+		{
+			JPH_ASSERT(i == 2);
+
+			float s = sqrt(mCol[2].mF32[2] - (mCol[0].mF32[0] + mCol[1].mF32[1]) + 1);
+			float is = 0.5f / s;
+			return Quat(
+				(mCol[0].mF32[2] + mCol[2].mF32[0]) * is,
+				(mCol[2].mF32[1] + mCol[1].mF32[2]) * is,
+				0.5f * s,
+				(mCol[0].mF32[1] - mCol[1].mF32[0]) * is);
+		}
+	}
+}
+
+Mat44 Mat44::sQuatLeftMultiply(QuatArg inQ)
+{
+	return Mat44(
+		Vec4(1, 1, -1, -1) * inQ.mValue.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X>(),
+		Vec4(-1, 1, 1, -1) * inQ.mValue.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y>(),
+		Vec4(1, -1, 1, -1) * inQ.mValue.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>(),
+		inQ.mValue);
+}
+
+Mat44 Mat44::sQuatRightMultiply(QuatArg inQ)
+{
+	return Mat44(
+		Vec4(1, -1, 1, -1) * inQ.mValue.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X>(),
+		Vec4(1, 1, -1, -1) * inQ.mValue.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y>(),
+		Vec4(-1, 1, 1, -1) * inQ.mValue.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>(),
+		inQ.mValue);
+}
+
+Mat44 Mat44::GetRotation() const
+{
+	JPH_ASSERT(mCol[0][3] == 0.0f);
+	JPH_ASSERT(mCol[1][3] == 0.0f);
+	JPH_ASSERT(mCol[2][3] == 0.0f);
+
+	return Mat44(mCol[0], mCol[1], mCol[2], Vec4(0, 0, 0, 1));
+}
+
+Mat44 Mat44::GetRotationSafe() const
+{
+#if defined(JPH_USE_AVX512)
+	return Mat44(_mm_maskz_mov_ps(0b0111, mCol[0].mValue),
+				 _mm_maskz_mov_ps(0b0111, mCol[1].mValue),
+				 _mm_maskz_mov_ps(0b0111, mCol[2].mValue),
+				 Vec4(0, 0, 0, 1));
+#elif defined(JPH_USE_SSE4_1)
+	__m128 zero = _mm_setzero_ps();
+	return Mat44(_mm_blend_ps(mCol[0].mValue, zero, 8),
+				 _mm_blend_ps(mCol[1].mValue, zero, 8),
+				 _mm_blend_ps(mCol[2].mValue, zero, 8),
+				 Vec4(0, 0, 0, 1));
+#elif defined(JPH_USE_NEON)
+	return Mat44(vsetq_lane_f32(0, mCol[0].mValue, 3),
+				 vsetq_lane_f32(0, mCol[1].mValue, 3),
+				 vsetq_lane_f32(0, mCol[2].mValue, 3),
+				 Vec4(0, 0, 0, 1));
+#else
+	return Mat44(Vec4(mCol[0].mF32[0], mCol[0].mF32[1], mCol[0].mF32[2], 0),
+				 Vec4(mCol[1].mF32[0], mCol[1].mF32[1], mCol[1].mF32[2], 0),
+				 Vec4(mCol[2].mF32[0], mCol[2].mF32[1], mCol[2].mF32[2], 0),
+				 Vec4(0, 0, 0, 1));
+#endif
+}
+
+void Mat44::SetRotation(Mat44Arg inRotation)
+{
+	mCol[0] = inRotation.mCol[0];
+	mCol[1] = inRotation.mCol[1];
+	mCol[2] = inRotation.mCol[2];
+}
+
+Mat44 Mat44::PreTranslated(Vec3Arg inTranslation) const
+{
+	return Mat44(mCol[0], mCol[1], mCol[2], Vec4(GetTranslation() + Multiply3x3(inTranslation), 1));
+}
+
+Mat44 Mat44::PostTranslated(Vec3Arg inTranslation) const
+{
+	return Mat44(mCol[0], mCol[1], mCol[2], Vec4(GetTranslation() + inTranslation, 1));
+}
+
+Mat44 Mat44::PreScaled(Vec3Arg inScale) const
+{
+	return Mat44(inScale.GetX() * mCol[0], inScale.GetY() * mCol[1], inScale.GetZ() * mCol[2], mCol[3]);
+}
+
+Mat44 Mat44::PostScaled(Vec3Arg inScale) const
+{
+	Vec4 scale(inScale, 1);
+	return Mat44(scale * mCol[0], scale * mCol[1], scale * mCol[2], scale * mCol[3]);
+}
+
+Mat44 Mat44::Decompose(Vec3 &outScale) const
+{
+	// Start the modified Gram-Schmidt algorithm
+	// X axis will just be normalized
+	Vec3 x = GetAxisX();
+
+	// Make Y axis perpendicular to X
+	Vec3 y = GetAxisY();
+	float x_dot_x = x.LengthSq();
+	y -= (x.Dot(y) / x_dot_x) * x;
+
+	// Make Z axis perpendicular to X
+	Vec3 z = GetAxisZ();
+	z -= (x.Dot(z) / x_dot_x) * x;
+
+	// Make Z axis perpendicular to Y
+	float y_dot_y = y.LengthSq();
+	z -= (y.Dot(z) / y_dot_y) * y;
+
+	// Determine the scale
+	float z_dot_z = z.LengthSq();
+	outScale = Vec3(x_dot_x, y_dot_y, z_dot_z).Sqrt();
+
+	// If the resulting x, y and z vectors don't form a right handed matrix, flip the z axis.
+	if (x.Cross(y).Dot(z) < 0.0f)
+		outScale.SetZ(-outScale.GetZ());
+
+	// Determine the rotation and translation
+	return Mat44(Vec4(x / outScale.GetX(), 0), Vec4(y / outScale.GetY(), 0), Vec4(z / outScale.GetZ(), 0), GetColumn4(3));
+}
+
+#undef JPH_EL
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Math/Math.h b/thirdparty/jolt_physics/Jolt/Math/Math.h
new file mode 100644
index 0000000000..729d5403e3
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/Math.h
@@ -0,0 +1,205 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// The constant \f$\pi\f$
+static constexpr float JPH_PI = 3.14159265358979323846f;
+
+/// Convert a value from degrees to radians
+JPH_INLINE constexpr float DegreesToRadians(float inV)
+{
+	return inV * (JPH_PI / 180.0f);
+}
+
+/// Convert a value from radians to degrees
+JPH_INLINE constexpr float RadiansToDegrees(float inV)
+{
+	return inV * (180.0f / JPH_PI);
+}
+
+/// Convert angle in radians to the range \f$[-\pi, \pi]\f$
+inline float CenterAngleAroundZero(float inV)
+{
+	if (inV < -JPH_PI)
+	{
+		do
+			inV += 2.0f * JPH_PI;
+		while (inV < -JPH_PI);
+	}
+	else if (inV > JPH_PI)
+	{
+		do
+			inV -= 2.0f * JPH_PI;
+		while (inV > JPH_PI);
+	}
+	JPH_ASSERT(inV >= -JPH_PI && inV <= JPH_PI);
+	return inV;
+}
+
+/// Clamp a value between two values
+template <typename T>
+JPH_INLINE constexpr T Clamp(T inV, T inMin, T inMax)
+{
+	return min(max(inV, inMin), inMax);
+}
+
+/// Square a value
+template <typename T>
+JPH_INLINE constexpr T Square(T inV)
+{
+	return inV * inV;
+}
+
+/// Returns \f$inV^3\f$.
+template <typename T>
+JPH_INLINE constexpr T Cubed(T inV)
+{
+	return inV * inV * inV;
+}
+
+/// Get the sign of a value
+template <typename T>
+JPH_INLINE constexpr T Sign(T inV)
+{
+	return inV < 0? T(-1) : T(1);
+}
+
+/// Check if inV is a power of 2
+template <typename T>
+constexpr bool IsPowerOf2(T inV)
+{
+	return (inV & (inV - 1)) == 0;
+}
+
+/// Align inV up to the next inAlignment bytes
+template <typename T>
+inline T AlignUp(T inV, uint64 inAlignment)
+{
+	JPH_ASSERT(IsPowerOf2(inAlignment));
+	return T((uint64(inV) + inAlignment - 1) & ~(inAlignment - 1));
+}
+
+/// Check if inV is inAlignment aligned
+template <typename T>
+inline bool IsAligned(T inV, uint64 inAlignment)
+{
+	JPH_ASSERT(IsPowerOf2(inAlignment));
+	return (uint64(inV) & (inAlignment - 1)) == 0;
+}
+
+/// Compute number of trailing zero bits (how many low bits are zero)
+inline uint CountTrailingZeros(uint32 inValue)
+{
+#if defined(JPH_CPU_X86) || defined(JPH_CPU_WASM)
+	#if defined(JPH_USE_TZCNT)
+		return _tzcnt_u32(inValue);
+	#elif defined(JPH_COMPILER_MSVC)
+		if (inValue == 0)
+			return 32;
+		unsigned long result;
+		_BitScanForward(&result, inValue);
+		return result;
+	#else
+		if (inValue == 0)
+			return 32;
+		return __builtin_ctz(inValue);
+	#endif
+#elif defined(JPH_CPU_ARM)
+	#if defined(JPH_COMPILER_MSVC)
+		if (inValue == 0)
+			return 32;
+		unsigned long result;
+		_BitScanForward(&result, inValue);
+		return result;
+	#else
+		if (inValue == 0)
+			return 32;
+		return __builtin_ctz(inValue);
+	#endif
+#elif defined(JPH_CPU_E2K)
+		return inValue ? __builtin_ctz(inValue) : 32;
+#else
+	#error Undefined
+#endif
+}
+
+/// Compute the number of leading zero bits (how many high bits are zero)
+inline uint CountLeadingZeros(uint32 inValue)
+{
+#if defined(JPH_CPU_X86) || defined(JPH_CPU_WASM)
+	#if defined(JPH_USE_LZCNT)
+		return _lzcnt_u32(inValue);
+	#elif defined(JPH_COMPILER_MSVC)
+		if (inValue == 0)
+			return 32;
+		unsigned long result;
+		_BitScanReverse(&result, inValue);
+		return 31 - result;
+	#else
+		if (inValue == 0)
+			return 32;
+		return __builtin_clz(inValue);
+	#endif
+#elif defined(JPH_CPU_ARM)
+	#if defined(JPH_COMPILER_MSVC)
+		return _CountLeadingZeros(inValue);
+	#else
+		return __builtin_clz(inValue);
+	#endif
+#elif defined(JPH_CPU_E2K)
+		return inValue ? __builtin_clz(inValue) : 32;
+#else
+	#error Undefined
+#endif
+}
+
+/// Count the number of 1 bits in a value
+inline uint CountBits(uint32 inValue)
+{
+#if defined(JPH_COMPILER_CLANG) || defined(JPH_COMPILER_GCC)
+	return __builtin_popcount(inValue);
+#elif defined(JPH_COMPILER_MSVC)
+	#if defined(JPH_USE_SSE4_2)
+		return _mm_popcnt_u32(inValue);
+	#elif defined(JPH_USE_NEON) && (_MSC_VER >= 1930) // _CountOneBits not available on MSVC2019
+		return _CountOneBits(inValue);
+	#else
+		inValue = inValue - ((inValue >> 1) & 0x55555555);
+		inValue = (inValue & 0x33333333) + ((inValue >> 2) & 0x33333333);
+		inValue = (inValue + (inValue >> 4)) & 0x0F0F0F0F;
+		return (inValue * 0x01010101) >> 24;
+	#endif
+#else
+	#error Undefined
+#endif
+}
+
+/// Get the next higher power of 2 of a value, or the value itself if the value is already a power of 2
+inline uint32 GetNextPowerOf2(uint32 inValue)
+{
+	return inValue <= 1? uint32(1) : uint32(1) << (32 - CountLeadingZeros(inValue - 1));
+}
+
+// Simple implementation of C++20 std::bit_cast (unfortunately not constexpr)
+template <class To, class From>
+JPH_INLINE To BitCast(const From &inValue)
+{
+	static_assert(std::is_trivially_constructible_v<To>);
+	static_assert(sizeof(From) == sizeof(To));
+
+	union FromTo
+	{
+		To			mTo;
+		From		mFrom;
+	};
+
+	FromTo convert;
+	convert.mFrom = inValue;
+	return convert.mTo;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Math/MathTypes.h b/thirdparty/jolt_physics/Jolt/Math/MathTypes.h
new file mode 100644
index 0000000000..e58dc9dabe
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/MathTypes.h
@@ -0,0 +1,32 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+class Vec3;
+class DVec3;
+class Vec4;
+class UVec4;
+class BVec16;
+class Quat;
+class Mat44;
+class DMat44;
+
+// Types to use for passing arguments to functions
+using Vec3Arg = const Vec3;
+#ifdef JPH_USE_AVX
+	using DVec3Arg = const DVec3;
+#else
+	using DVec3Arg = const DVec3 &;
+#endif
+using Vec4Arg = const Vec4;
+using UVec4Arg = const UVec4;
+using BVec16Arg = const BVec16;
+using QuatArg = const Quat;
+using Mat44Arg = const Mat44 &;
+using DMat44Arg = const DMat44 &;
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Math/Matrix.h b/thirdparty/jolt_physics/Jolt/Math/Matrix.h
new file mode 100644
index 0000000000..031665bbe7
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/Matrix.h
@@ -0,0 +1,259 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Math/Vector.h>
+#include <Jolt/Math/GaussianElimination.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Templatized matrix class
+template <uint Rows, uint Cols>
+class [[nodiscard]] Matrix
+{
+public:
+	/// Constructor
+	inline									Matrix() = default;
+	inline									Matrix(const Matrix &inM2)								{ *this = inM2; }
+
+	/// Dimensions
+	inline uint								GetRows() const											{ return Rows; }
+	inline uint								GetCols() const											{ return Cols; }
+
+	/// Zero matrix
+	inline void								SetZero()
+	{
+		for (uint c = 0; c < Cols; ++c)
+			mCol[c].SetZero();
+	}
+
+	inline static Matrix					sZero()													{ Matrix m; m.SetZero(); return m; }
+
+	/// Check if this matrix consists of all zeros
+	inline bool								IsZero() const
+	{
+		for (uint c = 0; c < Cols; ++c)
+			if (!mCol[c].IsZero())
+				return false;
+
+		return true;
+	}
+
+	/// Identity matrix
+	inline void								SetIdentity()
+	{
+		// Clear matrix
+		SetZero();
+
+		// Set diagonal to 1
+		for (uint rc = 0, min_rc = min(Rows, Cols); rc < min_rc; ++rc)
+			mCol[rc].mF32[rc] = 1.0f;
+	}
+
+	inline static Matrix					sIdentity()												{ Matrix m; m.SetIdentity(); return m; }
+
+	/// Check if this matrix is identity
+	bool									IsIdentity() const										{ return *this == sIdentity(); }
+
+	/// Diagonal matrix
+	inline void								SetDiagonal(const Vector<Rows < Cols? Rows : Cols> &inV)
+	{
+		// Clear matrix
+		SetZero();
+
+		// Set diagonal
+		for (uint rc = 0, min_rc = min(Rows, Cols); rc < min_rc; ++rc)
+			mCol[rc].mF32[rc] = inV[rc];
+	}
+
+	inline static Matrix					sDiagonal(const Vector<Rows < Cols? Rows : Cols> &inV)
+	{
+		Matrix m;
+		m.SetDiagonal(inV);
+		return m;
+	}
+
+	/// Copy a (part) of another matrix into this matrix
+	template <class OtherMatrix>
+		void								CopyPart(const OtherMatrix &inM, uint inSourceRow, uint inSourceCol, uint inNumRows, uint inNumCols, uint inDestRow, uint inDestCol)
+		{
+			for (uint c = 0; c < inNumCols; ++c)
+				for (uint r = 0; r < inNumRows; ++r)
+					mCol[inDestCol + c].mF32[inDestRow + r] = inM(inSourceRow + r, inSourceCol + c);
+		}
+
+	/// Get float component by element index
+	inline float							operator () (uint inRow, uint inColumn) const
+	{
+		JPH_ASSERT(inRow < Rows);
+		JPH_ASSERT(inColumn < Cols);
+		return mCol[inColumn].mF32[inRow];
+	}
+
+	inline float &							operator () (uint inRow, uint inColumn)
+	{
+		JPH_ASSERT(inRow < Rows);
+		JPH_ASSERT(inColumn < Cols);
+		return mCol[inColumn].mF32[inRow];
+	}
+
+	/// Comparison
+	inline bool								operator == (const Matrix &inM2) const
+	{
+		for (uint c = 0; c < Cols; ++c)
+			if (mCol[c] != inM2.mCol[c])
+				return false;
+		return true;
+	}
+
+	inline bool								operator != (const Matrix &inM2) const
+	{
+		for (uint c = 0; c < Cols; ++c)
+			if (mCol[c] != inM2.mCol[c])
+				return true;
+		return false;
+	}
+
+	/// Assignment
+	inline Matrix &							operator = (const Matrix &inM2)
+	{
+		for (uint c = 0; c < Cols; ++c)
+			mCol[c] = inM2.mCol[c];
+		return *this;
+	}
+
+	/// Multiply matrix by matrix
+	template <uint OtherCols>
+	inline Matrix<Rows, OtherCols>	operator * (const Matrix<Cols, OtherCols> &inM) const
+	{
+		Matrix<Rows, OtherCols> m;
+		for (uint c = 0; c < OtherCols; ++c)
+			for (uint r = 0; r < Rows; ++r)
+			{
+				float dot = 0.0f;
+				for (uint i = 0; i < Cols; ++i)
+					dot += mCol[i].mF32[r] * inM.mCol[c].mF32[i];
+				m.mCol[c].mF32[r] = dot;
+			}
+		return m;
+	}
+
+	/// Multiply vector by matrix
+	inline Vector<Rows>						operator * (const Vector<Cols> &inV) const
+	{
+		Vector<Rows> v;
+		for (uint r = 0; r < Rows; ++r)
+		{
+			float dot = 0.0f;
+			for (uint c = 0; c < Cols; ++c)
+				dot += mCol[c].mF32[r] * inV.mF32[c];
+			v.mF32[r] = dot;
+		}
+		return v;
+	}
+
+	/// Multiply matrix with float
+	inline Matrix							operator * (float inV) const
+	{
+		Matrix m;
+		for (uint c = 0; c < Cols; ++c)
+			m.mCol[c] = mCol[c] * inV;
+		return m;
+	}
+
+	inline friend Matrix					operator * (float inV, const Matrix &inM)
+	{
+		return inM * inV;
+	}
+
+	/// Per element addition of matrix
+	inline Matrix							operator + (const Matrix &inM) const
+	{
+		Matrix m;
+		for (uint c = 0; c < Cols; ++c)
+			m.mCol[c] = mCol[c] + inM.mCol[c];
+		return m;
+	}
+
+	/// Per element subtraction of matrix
+	inline Matrix							operator - (const Matrix &inM) const
+	{
+		Matrix m;
+		for (uint c = 0; c < Cols; ++c)
+			m.mCol[c] = mCol[c] - inM.mCol[c];
+		return m;
+	}
+
+	/// Transpose matrix
+	inline Matrix<Cols, Rows>				Transposed() const
+	{
+		Matrix<Cols, Rows> m;
+		for (uint r = 0; r < Rows; ++r)
+			for (uint c = 0; c < Cols; ++c)
+				m.mCol[r].mF32[c] = mCol[c].mF32[r];
+		return m;
+	}
+
+	/// Inverse matrix
+	bool									SetInversed(const Matrix &inM)
+	{
+		if constexpr (Rows != Cols) JPH_ASSERT(false);
+		Matrix copy(inM);
+		SetIdentity();
+		return GaussianElimination(copy, *this);
+	}
+
+	inline Matrix							Inversed() const
+	{
+		Matrix m;
+		m.SetInversed(*this);
+		return m;
+	}
+
+	/// To String
+	friend ostream &						operator << (ostream &inStream, const Matrix &inM)
+	{
+		for (uint i = 0; i < Cols - 1; ++i)
+			inStream << inM.mCol[i] << ", ";
+		inStream << inM.mCol[Cols - 1];
+		return inStream;
+	}
+
+	/// Column access
+	const Vector<Rows> &					GetColumn(int inIdx) const					{ return mCol[inIdx]; }
+	Vector<Rows> &							GetColumn(int inIdx)						{ return mCol[inIdx]; }
+
+	Vector<Rows>							mCol[Cols];									///< Column
+};
+
+// The template specialization doesn't sit well with Doxygen
+#ifndef JPH_PLATFORM_DOXYGEN
+
+/// Specialization of SetInversed for 2x2 matrix
+template <>
+inline bool Matrix<2, 2>::SetInversed(const Matrix<2, 2> &inM)
+{
+	// Fetch elements
+	float a = inM.mCol[0].mF32[0];
+	float b = inM.mCol[1].mF32[0];
+	float c = inM.mCol[0].mF32[1];
+	float d = inM.mCol[1].mF32[1];
+
+	// Calculate determinant
+	float det = a * d - b * c;
+	if (det == 0.0f)
+		return false;
+
+	// Construct inverse
+	mCol[0].mF32[0] = d / det;
+	mCol[1].mF32[0] = -b / det;
+	mCol[0].mF32[1] = -c / det;
+	mCol[1].mF32[1] = a / det;
+	return true;
+}
+
+#endif // !JPH_PLATFORM_DOXYGEN
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Math/Quat.h b/thirdparty/jolt_physics/Jolt/Math/Quat.h
new file mode 100644
index 0000000000..a68e45be29
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/Quat.h
@@ -0,0 +1,255 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Math/Vec3.h>
+#include <Jolt/Math/Vec4.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Quaternion class, quaternions are 4 dimensional vectors which can describe rotations in 3 dimensional
+/// space if their length is 1.
+///
+/// They are written as:
+///
+/// \f$q = w + x \: i + y \: j + z \: k\f$
+///
+/// or in vector notation:
+///
+/// \f$q = [w, v] = [w, x, y, z]\f$
+///
+/// Where:
+///
+/// w = the real part
+/// v = the imaginary part, (x, y, z)
+///
+/// Note that we store the quaternion in a Vec4 as [x, y, z, w] because that makes
+/// it easy to extract the rotation axis of the quaternion:
+///
+/// q = [cos(angle / 2), sin(angle / 2) * rotation_axis]
+class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) Quat
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	///@name Constructors
+	///@{
+	inline						Quat() = default; ///< Intentionally not initialized for performance reasons
+								Quat(const Quat &inRHS) = default;
+	Quat &						operator = (const Quat &inRHS) = default;
+	inline						Quat(float inX, float inY, float inZ, float inW)				: mValue(inX, inY, inZ, inW) { }
+	inline explicit				Quat(Vec4Arg inV)												: mValue(inV) { }
+	///@}
+
+	///@name Tests
+	///@{
+
+	/// Check if two quaternions are exactly equal
+	inline bool					operator == (QuatArg inRHS) const								{ return mValue == inRHS.mValue; }
+
+	/// Check if two quaternions are different
+	inline bool					operator != (QuatArg inRHS) const								{ return mValue != inRHS.mValue; }
+
+	/// If this quaternion is close to inRHS. Note that q and -q represent the same rotation, this is not checked here.
+	inline bool					IsClose(QuatArg inRHS, float inMaxDistSq = 1.0e-12f) const		{ return mValue.IsClose(inRHS.mValue, inMaxDistSq); }
+
+	/// If the length of this quaternion is 1 +/- inTolerance
+	inline bool					IsNormalized(float inTolerance = 1.0e-5f) const					{ return mValue.IsNormalized(inTolerance); }
+
+	/// If any component of this quaternion is a NaN (not a number)
+	inline bool					IsNaN() const													{ return mValue.IsNaN(); }
+
+	///@}
+	///@name Get components
+	///@{
+
+	/// Get X component (imaginary part i)
+	JPH_INLINE float			GetX() const													{ return mValue.GetX(); }
+
+	/// Get Y component (imaginary part j)
+	JPH_INLINE float			GetY() const													{ return mValue.GetY(); }
+
+	/// Get Z component (imaginary part k)
+	JPH_INLINE float			GetZ() const													{ return mValue.GetZ(); }
+
+	/// Get W component (real part)
+	JPH_INLINE float			GetW() const													{ return mValue.GetW(); }
+
+	/// Get the imaginary part of the quaternion
+	JPH_INLINE Vec3				GetXYZ() const													{ return Vec3(mValue); }
+
+	/// Get the quaternion as a Vec4
+	JPH_INLINE Vec4				GetXYZW() const													{ return mValue; }
+
+	/// Set individual components
+	JPH_INLINE void				SetX(float inX)													{ mValue.SetX(inX); }
+	JPH_INLINE void				SetY(float inY)													{ mValue.SetY(inY); }
+	JPH_INLINE void				SetZ(float inZ)													{ mValue.SetZ(inZ); }
+	JPH_INLINE void				SetW(float inW)													{ mValue.SetW(inW); }
+
+	/// Set all components
+	JPH_INLINE void				Set(float inX, float inY, float inZ, float inW)					{ mValue.Set(inX, inY, inZ, inW); }
+
+	///@}
+	///@name Default quaternions
+	///@{
+
+	/// @return [0, 0, 0, 0]
+	JPH_INLINE static Quat		sZero()															{ return Quat(Vec4::sZero()); }
+
+	/// @return [1, 0, 0, 0] (or in storage format Quat(0, 0, 0, 1))
+	JPH_INLINE static Quat		sIdentity()														{ return Quat(0, 0, 0, 1); }
+
+	///@}
+
+	/// Rotation from axis and angle
+	JPH_INLINE static Quat		sRotation(Vec3Arg inAxis, float inAngle);
+
+	/// Get axis and angle that represents this quaternion, outAngle will always be in the range \f$[0, \pi]\f$
+	JPH_INLINE void				GetAxisAngle(Vec3 &outAxis, float &outAngle) const;
+
+	/// Create quaternion that rotates a vector from the direction of inFrom to the direction of inTo along the shortest path
+	/// @see https://www.euclideanspace.com/maths/algebra/vectors/angleBetween/index.htm
+	JPH_INLINE static Quat		sFromTo(Vec3Arg inFrom, Vec3Arg inTo);
+
+	/// Random unit quaternion
+	template <class Random>
+	inline static Quat			sRandom(Random &inRandom);
+
+	/// Conversion from Euler angles. Rotation order is X then Y then Z (RotZ * RotY * RotX). Angles in radians.
+	inline static Quat			sEulerAngles(Vec3Arg inAngles);
+
+	/// Conversion to Euler angles. Rotation order is X then Y then Z (RotZ * RotY * RotX). Angles in radians.
+	inline Vec3					GetEulerAngles() const;
+
+	///@name Length / normalization operations
+	///@{
+
+	/// Squared length of quaternion.
+	/// @return Squared length of quaternion (\f$|v|^2\f$)
+	JPH_INLINE float			LengthSq() const												{ return mValue.LengthSq(); }
+
+	/// Length of quaternion.
+	/// @return Length of quaternion (\f$|v|\f$)
+	JPH_INLINE float			Length() const													{ return mValue.Length(); }
+
+	/// Normalize the quaternion (make it length 1)
+	JPH_INLINE Quat				Normalized() const												{ return Quat(mValue.Normalized()); }
+
+	///@}
+	///@name Additions / multiplications
+	///@{
+
+	JPH_INLINE void				operator += (QuatArg inRHS)										{ mValue += inRHS.mValue; }
+	JPH_INLINE void				operator -= (QuatArg inRHS)										{ mValue -= inRHS.mValue; }
+	JPH_INLINE void				operator *= (float inValue)										{ mValue *= inValue; }
+	JPH_INLINE void				operator /= (float inValue)										{ mValue /= inValue; }
+	JPH_INLINE Quat				operator - () const												{ return Quat(-mValue); }
+	JPH_INLINE Quat				operator + (QuatArg inRHS) const								{ return Quat(mValue + inRHS.mValue); }
+	JPH_INLINE Quat				operator - (QuatArg inRHS) const								{ return Quat(mValue - inRHS.mValue); }
+	JPH_INLINE Quat				operator * (QuatArg inRHS) const;
+	JPH_INLINE Quat				operator * (float inValue) const								{ return Quat(mValue * inValue); }
+	inline friend Quat			operator * (float inValue, QuatArg inRHS)						{ return Quat(inRHS.mValue * inValue); }
+	JPH_INLINE Quat				operator / (float inValue) const								{ return Quat(mValue / inValue); }
+
+	///@}
+
+	/// Rotate a vector by this quaternion
+	JPH_INLINE Vec3				operator * (Vec3Arg inValue) const;
+
+	/// Rotate a vector by the inverse of this quaternion
+	JPH_INLINE Vec3				InverseRotate(Vec3Arg inValue) const;
+
+	/// Rotate a the vector (1, 0, 0) with this quaternion
+	JPH_INLINE Vec3				RotateAxisX() const;
+
+	/// Rotate a the vector (0, 1, 0) with this quaternion
+	JPH_INLINE Vec3				RotateAxisY() const;
+
+	/// Rotate a the vector (0, 0, 1) with this quaternion
+	JPH_INLINE Vec3				RotateAxisZ() const;
+
+	/// Dot product
+	JPH_INLINE float			Dot(QuatArg inRHS) const										{ return mValue.Dot(inRHS.mValue); }
+
+	/// The conjugate [w, -x, -y, -z] is the same as the inverse for unit quaternions
+	JPH_INLINE Quat				Conjugated() const												{ return Quat(Vec4::sXor(mValue, UVec4(0x80000000, 0x80000000, 0x80000000, 0).ReinterpretAsFloat())); }
+
+	/// Get inverse quaternion
+	JPH_INLINE Quat				Inversed() const												{ return Conjugated() / Length(); }
+
+	/// Ensures that the W component is positive by negating the entire quaternion if it is not. This is useful when you want to store a quaternion as a 3 vector by discarding W and reconstructing it as sqrt(1 - x^2 - y^2 - z^2).
+	JPH_INLINE Quat				EnsureWPositive() const											{ return Quat(Vec4::sXor(mValue, Vec4::sAnd(mValue.SplatW(), UVec4::sReplicate(0x80000000).ReinterpretAsFloat()))); }
+
+	/// Get a quaternion that is perpendicular to this quaternion
+	JPH_INLINE Quat				GetPerpendicular() const										{ return Quat(Vec4(1, -1, 1, -1) * mValue.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>()); }
+
+	/// Get rotation angle around inAxis (uses Swing Twist Decomposition to get the twist quaternion and uses q(axis, angle) = [cos(angle / 2), axis * sin(angle / 2)])
+	JPH_INLINE float			GetRotationAngle(Vec3Arg inAxis) const							{ return GetW() == 0.0f? JPH_PI : 2.0f * ATan(GetXYZ().Dot(inAxis) / GetW()); }
+
+	/// Swing Twist Decomposition: any quaternion can be split up as:
+	///
+	/// \f[q = q_{swing} \: q_{twist}\f]
+	///
+	/// where \f$q_{twist}\f$ rotates only around axis v.
+	///
+	/// \f$q_{twist}\f$ is:
+	///
+	/// \f[q_{twist} = \frac{[q_w, q_{ijk} \cdot v \: v]}{\left|[q_w, q_{ijk} \cdot v \: v]\right|}\f]
+	///
+	/// where q_w is the real part of the quaternion and q_i the imaginary part (a 3 vector).
+	///
+	/// The swing can then be calculated as:
+	///
+	/// \f[q_{swing} = q \: q_{twist}^* \f]
+	///
+	/// Where \f$q_{twist}^*\f$ = complex conjugate of \f$q_{twist}\f$
+	JPH_INLINE Quat				GetTwist(Vec3Arg inAxis) const;
+
+	/// Decomposes quaternion into swing and twist component:
+	///
+	/// \f$q = q_{swing} \: q_{twist}\f$
+	///
+	/// where \f$q_{swing} \: \hat{x} = q_{twist} \: \hat{y} = q_{twist} \: \hat{z} = 0\f$
+	///
+	/// In other words:
+	///
+	/// - \f$q_{twist}\f$ only rotates around the X-axis.
+	/// - \f$q_{swing}\f$ only rotates around the Y and Z-axis.
+	///
+	/// @see Gino van den Bergen - Rotational Joint Limits in Quaternion Space - GDC 2016
+	JPH_INLINE void				GetSwingTwist(Quat &outSwing, Quat &outTwist) const;
+
+	/// Linear interpolation between two quaternions (for small steps).
+	/// @param inFraction is in the range [0, 1]
+	/// @param inDestination The destination quaternion
+	/// @return (1 - inFraction) * this + fraction * inDestination
+	JPH_INLINE Quat				LERP(QuatArg inDestination, float inFraction) const;
+
+	/// Spherical linear interpolation between two quaternions.
+	/// @param inFraction is in the range [0, 1]
+	/// @param inDestination The destination quaternion
+	/// @return When fraction is zero this quaternion is returned, when fraction is 1 inDestination is returned.
+	/// When fraction is between 0 and 1 an interpolation along the shortest path is returned.
+	JPH_INLINE Quat				SLERP(QuatArg inDestination, float inFraction) const;
+
+	/// Load 3 floats from memory (X, Y and Z component and then calculates W) reads 32 bits extra which it doesn't use
+	static JPH_INLINE Quat		sLoadFloat3Unsafe(const Float3 &inV);
+
+	/// Store 3 as floats to memory (X, Y and Z component)
+	JPH_INLINE void				StoreFloat3(Float3 *outV) const;
+
+	/// To String
+	friend ostream &			operator << (ostream &inStream, QuatArg inQ)					{ inStream << inQ.mValue; return inStream; }
+
+	/// 4 vector that stores [x, y, z, w] parts of the quaternion
+	Vec4						mValue;
+};
+
+static_assert(std::is_trivial<Quat>(), "Is supposed to be a trivial type!");
+
+JPH_NAMESPACE_END
+
+#include "Quat.inl"
diff --git a/thirdparty/jolt_physics/Jolt/Math/Quat.inl b/thirdparty/jolt_physics/Jolt/Math/Quat.inl
new file mode 100644
index 0000000000..201481929d
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/Quat.inl
@@ -0,0 +1,328 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+JPH_NAMESPACE_BEGIN
+
+Quat Quat::operator * (QuatArg inRHS) const
+{
+#if defined(JPH_USE_SSE4_1)
+	// Taken from: http://momchil-velikov.blogspot.nl/2013/10/fast-sse-quternion-multiplication.html
+	__m128 abcd = mValue.mValue;
+	__m128 xyzw = inRHS.mValue.mValue;
+
+	__m128 t0 = _mm_shuffle_ps(abcd, abcd, _MM_SHUFFLE(3, 3, 3, 3));
+	__m128 t1 = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(2, 3, 0, 1));
+
+	__m128 t3 = _mm_shuffle_ps(abcd, abcd, _MM_SHUFFLE(0, 0, 0, 0));
+	__m128 t4 = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(1, 0, 3, 2));
+
+	__m128 t5 = _mm_shuffle_ps(abcd, abcd, _MM_SHUFFLE(1, 1, 1, 1));
+	__m128 t6 = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(2, 0, 3, 1));
+
+	// [d,d,d,d] * [z,w,x,y] = [dz,dw,dx,dy]
+	__m128 m0 = _mm_mul_ps(t0, t1);
+
+	// [a,a,a,a] * [y,x,w,z] = [ay,ax,aw,az]
+	__m128 m1 = _mm_mul_ps(t3, t4);
+
+	// [b,b,b,b] * [z,x,w,y] = [bz,bx,bw,by]
+	__m128 m2 = _mm_mul_ps(t5, t6);
+
+	// [c,c,c,c] * [w,z,x,y] = [cw,cz,cx,cy]
+	__m128 t7 = _mm_shuffle_ps(abcd, abcd, _MM_SHUFFLE(2, 2, 2, 2));
+	__m128 t8 = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(3, 2, 0, 1));
+	__m128 m3 = _mm_mul_ps(t7, t8);
+
+	// [dz,dw,dx,dy] + -[ay,ax,aw,az] = [dz+ay,dw-ax,dx+aw,dy-az]
+	__m128 e = _mm_addsub_ps(m0, m1);
+
+	// [dx+aw,dz+ay,dy-az,dw-ax]
+	e = _mm_shuffle_ps(e, e, _MM_SHUFFLE(1, 3, 0, 2));
+
+	// [dx+aw,dz+ay,dy-az,dw-ax] + -[bz,bx,bw,by] = [dx+aw+bz,dz+ay-bx,dy-az+bw,dw-ax-by]
+	e = _mm_addsub_ps(e, m2);
+
+	// [dz+ay-bx,dw-ax-by,dy-az+bw,dx+aw+bz]
+	e = _mm_shuffle_ps(e, e, _MM_SHUFFLE(2, 0, 1, 3));
+
+	// [dz+ay-bx,dw-ax-by,dy-az+bw,dx+aw+bz] + -[cw,cz,cx,cy] = [dz+ay-bx+cw,dw-ax-by-cz,dy-az+bw+cx,dx+aw+bz-cy]
+	e = _mm_addsub_ps(e, m3);
+
+	// [dw-ax-by-cz,dz+ay-bx+cw,dy-az+bw+cx,dx+aw+bz-cy]
+	return Quat(Vec4(_mm_shuffle_ps(e, e, _MM_SHUFFLE(2, 3, 1, 0))));
+#else
+	float lx = mValue.GetX();
+	float ly = mValue.GetY();
+	float lz = mValue.GetZ();
+	float lw = mValue.GetW();
+
+	float rx = inRHS.mValue.GetX();
+	float ry = inRHS.mValue.GetY();
+	float rz = inRHS.mValue.GetZ();
+	float rw = inRHS.mValue.GetW();
+
+	float x = lw * rx + lx * rw + ly * rz - lz * ry;
+	float y = lw * ry - lx * rz + ly * rw + lz * rx;
+	float z = lw * rz + lx * ry - ly * rx + lz * rw;
+	float w = lw * rw - lx * rx - ly * ry - lz * rz;
+
+	return Quat(x, y, z, w);
+#endif
+}
+
+Quat Quat::sRotation(Vec3Arg inAxis, float inAngle)
+{
+	// returns [inAxis * sin(0.5f * inAngle), cos(0.5f * inAngle)]
+	JPH_ASSERT(inAxis.IsNormalized());
+	Vec4 s, c;
+	Vec4::sReplicate(0.5f * inAngle).SinCos(s, c);
+	return Quat(Vec4::sSelect(Vec4(inAxis) * s, c, UVec4(0, 0, 0, 0xffffffffU)));
+}
+
+void Quat::GetAxisAngle(Vec3 &outAxis, float &outAngle) const
+{
+	JPH_ASSERT(IsNormalized());
+	Quat w_pos = EnsureWPositive();
+	float abs_w = w_pos.GetW();
+	if (abs_w >= 1.0f)
+	{
+		outAxis = Vec3::sZero();
+		outAngle = 0.0f;
+	}
+	else
+	{
+		outAngle = 2.0f * ACos(abs_w);
+		outAxis = w_pos.GetXYZ().NormalizedOr(Vec3::sZero());
+	}
+}
+
+Quat Quat::sFromTo(Vec3Arg inFrom, Vec3Arg inTo)
+{
+	/*
+		Uses (inFrom = v1, inTo = v2):
+
+		angle = arcos(v1 . v2 / |v1||v2|)
+		axis = normalize(v1 x v2)
+
+		Quaternion is then:
+
+		s = sin(angle / 2)
+		x = axis.x * s
+		y = axis.y * s
+		z = axis.z * s
+		w = cos(angle / 2)
+
+		Using identities:
+
+		sin(2 * a) = 2 * sin(a) * cos(a)
+		cos(2 * a) = cos(a)^2 - sin(a)^2
+		sin(a)^2 + cos(a)^2 = 1
+
+		This reduces to:
+
+		x = (v1 x v2).x
+		y = (v1 x v2).y
+		z = (v1 x v2).z
+		w = |v1||v2| + v1 . v2
+
+		which then needs to be normalized because the whole equation was multiplied by 2 cos(angle / 2)
+	*/
+
+	float len_v1_v2 = sqrt(inFrom.LengthSq() * inTo.LengthSq());
+	float w = len_v1_v2 + inFrom.Dot(inTo);
+
+	if (w == 0.0f)
+	{
+		if (len_v1_v2 == 0.0f)
+		{
+			// If either of the vectors has zero length, there is no rotation and we return identity
+			return Quat::sIdentity();
+		}
+		else
+		{
+			// If vectors are perpendicular, take one of the many 180 degree rotations that exist
+			return Quat(Vec4(inFrom.GetNormalizedPerpendicular(), 0));
+		}
+	}
+
+	Vec3 v = inFrom.Cross(inTo);
+	return Quat(Vec4(v, w)).Normalized();
+}
+
+template <class Random>
+Quat Quat::sRandom(Random &inRandom)
+{
+	std::uniform_real_distribution<float> zero_to_one(0.0f, 1.0f);
+	float x0 = zero_to_one(inRandom);
+	float r1 = sqrt(1.0f - x0), r2 = sqrt(x0);
+	std::uniform_real_distribution<float> zero_to_two_pi(0.0f, 2.0f * JPH_PI);
+	Vec4 s, c;
+	Vec4(zero_to_two_pi(inRandom), zero_to_two_pi(inRandom), 0, 0).SinCos(s, c);
+	return Quat(s.GetX() * r1, c.GetX() * r1, s.GetY() * r2, c.GetY() * r2);
+}
+
+Quat Quat::sEulerAngles(Vec3Arg inAngles)
+{
+	Vec4 half(0.5f * inAngles);
+	Vec4 s, c;
+	half.SinCos(s, c);
+
+	float cx = c.GetX();
+	float sx = s.GetX();
+	float cy = c.GetY();
+	float sy = s.GetY();
+	float cz = c.GetZ();
+	float sz = s.GetZ();
+
+	return Quat(
+		cz * sx * cy - sz * cx * sy,
+		cz * cx * sy + sz * sx * cy,
+		sz * cx * cy - cz * sx * sy,
+		cz * cx * cy + sz * sx * sy);
+}
+
+Vec3 Quat::GetEulerAngles() const
+{
+	float y_sq = GetY() * GetY();
+
+	// X
+	float t0 = 2.0f * (GetW() * GetX() + GetY() * GetZ());
+	float t1 = 1.0f - 2.0f * (GetX() * GetX() + y_sq);
+
+	// Y
+	float t2 = 2.0f * (GetW() * GetY() - GetZ() * GetX());
+	t2 = t2 > 1.0f? 1.0f : t2;
+	t2 = t2 < -1.0f? -1.0f : t2;
+
+	// Z
+	float t3 = 2.0f * (GetW() * GetZ() + GetX() * GetY());
+	float t4 = 1.0f - 2.0f * (y_sq + GetZ() * GetZ());
+
+	return Vec3(ATan2(t0, t1), ASin(t2), ATan2(t3, t4));
+}
+
+Quat Quat::GetTwist(Vec3Arg inAxis) const
+{
+	Quat twist(Vec4(GetXYZ().Dot(inAxis) * inAxis, GetW()));
+	float twist_len = twist.LengthSq();
+	if (twist_len != 0.0f)
+		return twist / sqrt(twist_len);
+	else
+		return Quat::sIdentity();
+}
+
+void Quat::GetSwingTwist(Quat &outSwing, Quat &outTwist) const
+{
+	float x = GetX(), y = GetY(), z = GetZ(), w = GetW();
+	float s = sqrt(Square(w) + Square(x));
+	if (s != 0.0f)
+	{
+		outTwist = Quat(x / s, 0, 0, w / s);
+		outSwing = Quat(0, (w * y - x * z) / s, (w * z + x * y) / s, s);
+	}
+	else
+	{
+		// If both x and w are zero, this must be a 180 degree rotation around either y or z
+		outTwist = Quat::sIdentity();
+		outSwing = *this;
+	}
+}
+
+Quat Quat::LERP(QuatArg inDestination, float inFraction) const
+{
+	float scale0 = 1.0f - inFraction;
+	return Quat(Vec4::sReplicate(scale0) * mValue + Vec4::sReplicate(inFraction) * inDestination.mValue);
+}
+
+Quat Quat::SLERP(QuatArg inDestination, float inFraction) const
+{
+	// Difference at which to LERP instead of SLERP
+	const float delta = 0.0001f;
+
+	// Calc cosine
+	float sign_scale1 = 1.0f;
+	float cos_omega = Dot(inDestination);
+
+	// Adjust signs (if necessary)
+	if (cos_omega < 0.0f)
+	{
+		cos_omega = -cos_omega;
+		sign_scale1 = -1.0f;
+	}
+
+	// Calculate coefficients
+	float scale0, scale1;
+	if (1.0f - cos_omega > delta)
+	{
+		// Standard case (slerp)
+		float omega = ACos(cos_omega);
+		float sin_omega = Sin(omega);
+		scale0 = Sin((1.0f - inFraction) * omega) / sin_omega;
+		scale1 = sign_scale1 * Sin(inFraction * omega) / sin_omega;
+	}
+	else
+	{
+		// Quaternions are very close so we can do a linear interpolation
+		scale0 = 1.0f - inFraction;
+		scale1 = sign_scale1 * inFraction;
+	}
+
+	// Interpolate between the two quaternions
+	return Quat(Vec4::sReplicate(scale0) * mValue + Vec4::sReplicate(scale1) * inDestination.mValue).Normalized();
+}
+
+Vec3 Quat::operator * (Vec3Arg inValue) const
+{
+	// Rotating a vector by a quaternion is done by: p' = q * p * q^-1 (q^-1 = conjugated(q) for a unit quaternion)
+	JPH_ASSERT(IsNormalized());
+	return Vec3((*this * Quat(Vec4(inValue, 0)) * Conjugated()).mValue);
+}
+
+Vec3 Quat::InverseRotate(Vec3Arg inValue) const
+{
+	JPH_ASSERT(IsNormalized());
+	return Vec3((Conjugated() * Quat(Vec4(inValue, 0)) * *this).mValue);
+}
+
+Vec3 Quat::RotateAxisX() const
+{
+	// This is *this * Vec3::sAxisX() written out:
+	JPH_ASSERT(IsNormalized());
+	float x = GetX(), y = GetY(), z = GetZ(), w = GetW();
+	float tx = 2.0f * x, tw = 2.0f * w;
+	return Vec3(tx * x + tw * w - 1.0f, tx * y + z * tw, tx * z - y * tw);
+}
+
+Vec3 Quat::RotateAxisY() const
+{
+	// This is *this * Vec3::sAxisY() written out:
+	JPH_ASSERT(IsNormalized());
+	float x = GetX(), y = GetY(), z = GetZ(), w = GetW();
+	float ty = 2.0f * y, tw = 2.0f * w;
+	return Vec3(x * ty - z * tw, tw * w + ty * y - 1.0f, x * tw + ty * z);
+}
+
+Vec3 Quat::RotateAxisZ() const
+{
+	// This is *this * Vec3::sAxisZ() written out:
+	JPH_ASSERT(IsNormalized());
+	float x = GetX(), y = GetY(), z = GetZ(), w = GetW();
+	float tz = 2.0f * z, tw = 2.0f * w;
+	return Vec3(x * tz + y * tw, y * tz - x * tw, tw * w + tz * z - 1.0f);
+}
+
+void Quat::StoreFloat3(Float3 *outV) const
+{
+	JPH_ASSERT(IsNormalized());
+	EnsureWPositive().GetXYZ().StoreFloat3(outV);
+}
+
+Quat Quat::sLoadFloat3Unsafe(const Float3 &inV)
+{
+	Vec3 v = Vec3::sLoadFloat3Unsafe(inV);
+	float w = sqrt(max(1.0f - v.LengthSq(), 0.0f)); // It is possible that the length of v is a fraction above 1, and we don't want to introduce NaN's in that case so we clamp to 0
+	return Quat(Vec4(v, w));
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Math/Real.h b/thirdparty/jolt_physics/Jolt/Math/Real.h
new file mode 100644
index 0000000000..ca8cf5049e
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/Real.h
@@ -0,0 +1,44 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Math/DVec3.h>
+#include <Jolt/Math/DMat44.h>
+
+JPH_NAMESPACE_BEGIN
+
+#ifdef JPH_DOUBLE_PRECISION
+
+// Define real to double
+using Real = double;
+using Real3 = Double3;
+using RVec3 = DVec3;
+using RVec3Arg = DVec3Arg;
+using RMat44 = DMat44;
+using RMat44Arg = DMat44Arg;
+
+#define JPH_RVECTOR_ALIGNMENT JPH_DVECTOR_ALIGNMENT
+
+#else
+
+// Define real to float
+using Real = float;
+using Real3 = Float3;
+using RVec3 = Vec3;
+using RVec3Arg = Vec3Arg;
+using RMat44 = Mat44;
+using RMat44Arg = Mat44Arg;
+
+#define JPH_RVECTOR_ALIGNMENT JPH_VECTOR_ALIGNMENT
+
+#endif // JPH_DOUBLE_PRECISION
+
+// Put the 'real' operator in a namespace so that users can opt in to use it:
+// using namespace JPH::literals;
+namespace literals {
+	constexpr Real operator ""_r (long double inValue) { return Real(inValue); }
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Math/Swizzle.h b/thirdparty/jolt_physics/Jolt/Math/Swizzle.h
new file mode 100644
index 0000000000..ad8dfbc144
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/Swizzle.h
@@ -0,0 +1,19 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Enum indicating which component to use when swizzling
+enum
+{
+	SWIZZLE_X = 0,			///< Use the X component
+	SWIZZLE_Y = 1,			///< Use the Y component
+	SWIZZLE_Z = 2,			///< Use the Z component
+	SWIZZLE_W = 3,			///< Use the W component
+	SWIZZLE_UNUSED = 2,		///< We always use the Z component when we don't specifically want to initialize a value, this is consistent with what is done in Vec3(x, y, z), Vec3(Float3 &) and Vec3::sLoadFloat3Unsafe
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Math/Trigonometry.h b/thirdparty/jolt_physics/Jolt/Math/Trigonometry.h
new file mode 100644
index 0000000000..3503dd17bd
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/Trigonometry.h
@@ -0,0 +1,79 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+// Note that this file exists because std::sin etc. are not platform independent and will lead to non-deterministic simulation
+
+/// Sine of x (input in radians)
+JPH_INLINE float Sin(float inX)
+{
+	Vec4 s, c;
+	Vec4::sReplicate(inX).SinCos(s, c);
+	return s.GetX();
+}
+
+/// Cosine of x (input in radians)
+JPH_INLINE float Cos(float inX)
+{
+	Vec4 s, c;
+	Vec4::sReplicate(inX).SinCos(s, c);
+	return c.GetX();
+}
+
+/// Tangent of x (input in radians)
+JPH_INLINE float Tan(float inX)
+{
+	return Vec4::sReplicate(inX).Tan().GetX();
+}
+
+/// Arc sine of x (returns value in the range [-PI / 2, PI / 2])
+/// Note that all input values will be clamped to the range [-1, 1] and this function will not return NaNs like std::asin
+JPH_INLINE float ASin(float inX)
+{
+	return Vec4::sReplicate(inX).ASin().GetX();
+}
+
+/// Arc cosine of x (returns value in the range [0, PI])
+/// Note that all input values will be clamped to the range [-1, 1] and this function will not return NaNs like std::acos
+JPH_INLINE float ACos(float inX)
+{
+	return Vec4::sReplicate(inX).ACos().GetX();
+}
+
+/// An approximation of ACos, max error is 4.2e-3 over the entire range [-1, 1], is approximately 2.5x faster than ACos
+JPH_INLINE float ACosApproximate(float inX)
+{
+	// See: https://www.johndcook.com/blog/2022/09/06/inverse-cosine-near-1/
+	// See also: https://seblagarde.wordpress.com/2014/12/01/inverse-trigonometric-functions-gpu-optimization-for-amd-gcn-architecture/
+	// Taylor of cos(x) = 1 - x^2 / 2 + ...
+	// Substitute x = sqrt(2 y) we get: cos(sqrt(2 y)) = 1 - y
+	// Substitute z = 1 - y we get: cos(sqrt(2 (1 - z))) = z <=> acos(z) = sqrt(2 (1 - z))
+	// To avoid the discontinuity at 1, instead of using the Taylor expansion of acos(x) we use acos(x) / sqrt(2 (1 - x)) = 1 + (1 - x) / 12 + ...
+	// Since the approximation was made at 1, it has quite a large error at 0 meaning that if we want to extend to the
+	// range [-1, 1] by mirroring the range [0, 1], the value at 0+ is not the same as 0-.
+	// So we observe that the form of the Taylor expansion is f(x) = sqrt(1 - x) * (a + b x) and we fit the function so that f(0) = pi / 2
+	// this gives us a = pi / 2. f(1) = 0 regardless of b. We search for a constant b that minimizes the error in the range [0, 1].
+	float abs_x = min(abs(inX), 1.0f); // Ensure that we don't get a value larger than 1
+	float val = sqrt(1.0f - abs_x) * (JPH_PI / 2 - 0.175394f * abs_x);
+
+	// Our approximation is valid in the range [0, 1], extend it to the range [-1, 1]
+	return inX < 0? JPH_PI - val : val;
+}
+
+/// Arc tangent of x (returns value in the range [-PI / 2, PI / 2])
+JPH_INLINE float ATan(float inX)
+{
+	return Vec4::sReplicate(inX).ATan().GetX();
+}
+
+/// Arc tangent of y / x using the signs of the arguments to determine the correct quadrant (returns value in the range [-PI, PI])
+JPH_INLINE float ATan2(float inY, float inX)
+{
+	return Vec4::sATan2(Vec4::sReplicate(inY), Vec4::sReplicate(inX)).GetX();
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Math/UVec4.h b/thirdparty/jolt_physics/Jolt/Math/UVec4.h
new file mode 100644
index 0000000000..3f22d43ca6
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/UVec4.h
@@ -0,0 +1,220 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Math/Vec4.h>
+
+JPH_NAMESPACE_BEGIN
+
+class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) UVec4
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	// Underlying vector type
+#if defined(JPH_USE_SSE)
+	using Type = __m128i;
+#elif defined(JPH_USE_NEON)
+	using Type = uint32x4_t;
+#else
+	using Type = struct { uint32 mData[4]; };
+#endif
+
+	/// Constructor
+								UVec4() = default; ///< Intentionally not initialized for performance reasons
+								UVec4(const UVec4 &inRHS) = default;
+	UVec4 &						operator = (const UVec4 &inRHS) = default;
+	JPH_INLINE					UVec4(Type inRHS) : mValue(inRHS)					{ }
+
+	/// Create a vector from 4 integer components
+	JPH_INLINE					UVec4(uint32 inX, uint32 inY, uint32 inZ, uint32 inW);
+
+	/// Comparison
+	JPH_INLINE bool				operator == (UVec4Arg inV2) const;
+	JPH_INLINE bool				operator != (UVec4Arg inV2) const					{ return !(*this == inV2); }
+
+	/// Swizzle the elements in inV
+	template<uint32 SwizzleX, uint32 SwizzleY, uint32 SwizzleZ, uint32 SwizzleW>
+	JPH_INLINE UVec4			Swizzle() const;
+
+	/// Vector with all zeros
+	static JPH_INLINE UVec4		sZero();
+
+	/// Replicate int inV across all components
+	static JPH_INLINE UVec4		sReplicate(uint32 inV);
+
+	/// Load 1 int from memory and place it in the X component, zeros Y, Z and W
+	static JPH_INLINE UVec4		sLoadInt(const uint32 *inV);
+
+	/// Load 4 ints from memory
+	static JPH_INLINE UVec4		sLoadInt4(const uint32 *inV);
+
+	/// Load 4 ints from memory, aligned to 16 bytes
+	static JPH_INLINE UVec4		sLoadInt4Aligned(const uint32 *inV);
+
+	/// Gather 4 ints from memory at inBase + inOffsets[i] * Scale
+	template <const int Scale>
+	static JPH_INLINE UVec4		sGatherInt4(const uint32 *inBase, UVec4Arg inOffsets);
+
+	/// Return the minimum value of each of the components
+	static JPH_INLINE UVec4		sMin(UVec4Arg inV1, UVec4Arg inV2);
+
+	/// Return the maximum of each of the components
+	static JPH_INLINE UVec4		sMax(UVec4Arg inV1, UVec4Arg inV2);
+
+	/// Equals (component wise)
+	static JPH_INLINE UVec4		sEquals(UVec4Arg inV1, UVec4Arg inV2);
+
+	/// Component wise select, returns inNotSet when highest bit of inControl = 0 and inSet when highest bit of inControl = 1
+	static JPH_INLINE UVec4		sSelect(UVec4Arg inNotSet, UVec4Arg inSet, UVec4Arg inControl);
+
+	/// Logical or (component wise)
+	static JPH_INLINE UVec4		sOr(UVec4Arg inV1, UVec4Arg inV2);
+
+	/// Logical xor (component wise)
+	static JPH_INLINE UVec4		sXor(UVec4Arg inV1, UVec4Arg inV2);
+
+	/// Logical and (component wise)
+	static JPH_INLINE UVec4		sAnd(UVec4Arg inV1, UVec4Arg inV2);
+
+	/// Logical not (component wise)
+	static JPH_INLINE UVec4		sNot(UVec4Arg inV1);
+
+	/// Sorts the elements in inIndex so that the values that correspond to trues in inValue are the first elements.
+	/// The remaining elements will be set to inValue.w.
+	/// I.e. if inValue = (true, false, true, false) and inIndex = (1, 2, 3, 4) the function returns (1, 3, 4, 4).
+	static JPH_INLINE UVec4		sSort4True(UVec4Arg inValue, UVec4Arg inIndex);
+
+	/// Get individual components
+#if defined(JPH_USE_SSE)
+	JPH_INLINE uint32			GetX() const										{ return uint32(_mm_cvtsi128_si32(mValue)); }
+	JPH_INLINE uint32			GetY() const										{ return mU32[1]; }
+	JPH_INLINE uint32			GetZ() const										{ return mU32[2]; }
+	JPH_INLINE uint32			GetW() const										{ return mU32[3]; }
+#elif defined(JPH_USE_NEON)
+	JPH_INLINE uint32			GetX() const										{ return vgetq_lane_u32(mValue, 0); }
+	JPH_INLINE uint32			GetY() const										{ return vgetq_lane_u32(mValue, 1); }
+	JPH_INLINE uint32			GetZ() const										{ return vgetq_lane_u32(mValue, 2); }
+	JPH_INLINE uint32			GetW() const										{ return vgetq_lane_u32(mValue, 3); }
+#else
+	JPH_INLINE uint32			GetX() const										{ return mU32[0]; }
+	JPH_INLINE uint32			GetY() const										{ return mU32[1]; }
+	JPH_INLINE uint32			GetZ() const										{ return mU32[2]; }
+	JPH_INLINE uint32			GetW() const										{ return mU32[3]; }
+#endif
+
+	/// Set individual components
+	JPH_INLINE void				SetX(uint32 inX)									{ mU32[0] = inX; }
+	JPH_INLINE void				SetY(uint32 inY)									{ mU32[1] = inY; }
+	JPH_INLINE void				SetZ(uint32 inZ)									{ mU32[2] = inZ; }
+	JPH_INLINE void				SetW(uint32 inW)									{ mU32[3] = inW; }
+
+	/// Get component by index
+	JPH_INLINE uint32			operator [] (uint inCoordinate) const				{ JPH_ASSERT(inCoordinate < 4); return mU32[inCoordinate]; }
+	JPH_INLINE uint32 &			operator [] (uint inCoordinate)						{ JPH_ASSERT(inCoordinate < 4); return mU32[inCoordinate]; }
+
+	/// Multiplies each of the 4 integer components with an integer (discards any overflow)
+	JPH_INLINE UVec4			operator * (UVec4Arg inV2) const;
+
+	/// Adds an integer value to all integer components (discards any overflow)
+	JPH_INLINE UVec4			operator + (UVec4Arg inV2);
+
+	/// Add two integer vectors (component wise)
+	JPH_INLINE UVec4 &			operator += (UVec4Arg inV2);
+
+	/// Replicate the X component to all components
+	JPH_INLINE UVec4			SplatX() const;
+
+	/// Replicate the Y component to all components
+	JPH_INLINE UVec4			SplatY() const;
+
+	/// Replicate the Z component to all components
+	JPH_INLINE UVec4			SplatZ() const;
+
+	/// Replicate the W component to all components
+	JPH_INLINE UVec4			SplatW() const;
+
+	/// Convert each component from an int to a float
+	JPH_INLINE Vec4				ToFloat() const;
+
+	/// Reinterpret UVec4 as a Vec4 (doesn't change the bits)
+	JPH_INLINE Vec4				ReinterpretAsFloat() const;
+
+	/// Store 4 ints to memory
+	JPH_INLINE void				StoreInt4(uint32 *outV) const;
+
+	/// Store 4 ints to memory, aligned to 16 bytes
+	JPH_INLINE void				StoreInt4Aligned(uint32 *outV) const;
+
+	/// Test if any of the components are true (true is when highest bit of component is set)
+	JPH_INLINE bool				TestAnyTrue() const;
+
+	/// Test if any of X, Y or Z components are true (true is when highest bit of component is set)
+	JPH_INLINE bool				TestAnyXYZTrue() const;
+
+	/// Test if all components are true (true is when highest bit of component is set)
+	JPH_INLINE bool				TestAllTrue() const;
+
+	/// Test if X, Y and Z components are true (true is when highest bit of component is set)
+	JPH_INLINE bool				TestAllXYZTrue() const;
+
+	/// Count the number of components that are true (true is when highest bit of component is set)
+	JPH_INLINE int				CountTrues() const;
+
+	/// Store if X is true in bit 0, Y in bit 1, Z in bit 2 and W in bit 3 (true is when highest bit of component is set)
+	JPH_INLINE int				GetTrues() const;
+
+	/// Shift all components by Count bits to the left (filling with zeros from the left)
+	template <const uint Count>
+	JPH_INLINE UVec4			LogicalShiftLeft() const;
+
+	/// Shift all components by Count bits to the right (filling with zeros from the right)
+	template <const uint Count>
+	JPH_INLINE UVec4			LogicalShiftRight() const;
+
+	/// Shift all components by Count bits to the right (shifting in the value of the highest bit)
+	template <const uint Count>
+	JPH_INLINE UVec4			ArithmeticShiftRight() const;
+
+	/// Takes the lower 4 16 bits and expands them to X, Y, Z and W
+	JPH_INLINE UVec4			Expand4Uint16Lo() const;
+
+	/// Takes the upper 4 16 bits and expands them to X, Y, Z and W
+	JPH_INLINE UVec4			Expand4Uint16Hi() const;
+
+	/// Takes byte 0 .. 3 and expands them to X, Y, Z and W
+	JPH_INLINE UVec4			Expand4Byte0() const;
+
+	/// Takes byte 4 .. 7 and expands them to X, Y, Z and W
+	JPH_INLINE UVec4			Expand4Byte4() const;
+
+	/// Takes byte 8 .. 11 and expands them to X, Y, Z and W
+	JPH_INLINE UVec4			Expand4Byte8() const;
+
+	/// Takes byte 12 .. 15 and expands them to X, Y, Z and W
+	JPH_INLINE UVec4			Expand4Byte12() const;
+
+	/// Shift vector components by 4 - Count floats to the left, so if Count = 1 the resulting vector is (W, 0, 0, 0), when Count = 3 the resulting vector is (Y, Z, W, 0)
+	JPH_INLINE UVec4			ShiftComponents4Minus(int inCount) const;
+
+	/// To String
+	friend ostream &			operator << (ostream &inStream, UVec4Arg inV)
+	{
+		inStream << inV.mU32[0] << ", " << inV.mU32[1] << ", " << inV.mU32[2] << ", " << inV.mU32[3];
+		return inStream;
+	}
+
+	union
+	{
+		Type					mValue;
+		uint32					mU32[4];
+	};
+};
+
+static_assert(std::is_trivial<UVec4>(), "Is supposed to be a trivial type!");
+
+JPH_NAMESPACE_END
+
+#include "UVec4.inl"
diff --git a/thirdparty/jolt_physics/Jolt/Math/UVec4.inl b/thirdparty/jolt_physics/Jolt/Math/UVec4.inl
new file mode 100644
index 0000000000..f9014acc28
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/UVec4.inl
@@ -0,0 +1,581 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+JPH_NAMESPACE_BEGIN
+
+UVec4::UVec4(uint32 inX, uint32 inY, uint32 inZ, uint32 inW)
+{
+#if defined(JPH_USE_SSE)
+	mValue = _mm_set_epi32(int(inW), int(inZ), int(inY), int(inX));
+#elif defined(JPH_USE_NEON)
+	uint32x2_t xy = vcreate_u32(static_cast<uint64>(inX) | (static_cast<uint64>(inY) << 32));
+	uint32x2_t zw = vcreate_u32(static_cast<uint64>(inZ) | (static_cast<uint64>(inW) << 32));
+	mValue = vcombine_u32(xy, zw);
+#else
+	mU32[0] = inX;
+	mU32[1] = inY;
+	mU32[2] = inZ;
+	mU32[3] = inW;
+#endif
+}
+
+bool UVec4::operator == (UVec4Arg inV2) const
+{
+	return sEquals(*this, inV2).TestAllTrue();
+}
+
+template<uint32 SwizzleX, uint32 SwizzleY, uint32 SwizzleZ, uint32 SwizzleW>
+UVec4 UVec4::Swizzle() const
+{
+	static_assert(SwizzleX <= 3, "SwizzleX template parameter out of range");
+	static_assert(SwizzleY <= 3, "SwizzleY template parameter out of range");
+	static_assert(SwizzleZ <= 3, "SwizzleZ template parameter out of range");
+	static_assert(SwizzleW <= 3, "SwizzleW template parameter out of range");
+
+#if defined(JPH_USE_SSE)
+	return _mm_shuffle_epi32(mValue, _MM_SHUFFLE(SwizzleW, SwizzleZ, SwizzleY, SwizzleX));
+#elif defined(JPH_USE_NEON)
+	return JPH_NEON_SHUFFLE_U32x4(mValue, mValue, SwizzleX, SwizzleY, SwizzleZ, SwizzleW);
+#else
+	return UVec4(mU32[SwizzleX], mU32[SwizzleY], mU32[SwizzleZ], mU32[SwizzleW]);
+#endif
+}
+
+UVec4 UVec4::sZero()
+{
+#if defined(JPH_USE_SSE)
+	return _mm_setzero_si128();
+#elif defined(JPH_USE_NEON)
+	return vdupq_n_u32(0);
+#else
+	return UVec4(0, 0, 0, 0);
+#endif
+}
+
+UVec4 UVec4::sReplicate(uint32 inV)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_set1_epi32(int(inV));
+#elif defined(JPH_USE_NEON)
+	return vdupq_n_u32(inV);
+#else
+	return UVec4(inV, inV, inV, inV);
+#endif
+}
+
+UVec4 UVec4::sLoadInt(const uint32 *inV)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_castps_si128(_mm_load_ss(reinterpret_cast<const float*>(inV)));
+#elif defined(JPH_USE_NEON)
+	return vsetq_lane_u32(*inV, vdupq_n_u32(0), 0);
+#else
+	return UVec4(*inV, 0, 0, 0);
+#endif
+}
+
+UVec4 UVec4::sLoadInt4(const uint32 *inV)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_loadu_si128(reinterpret_cast<const __m128i *>(inV));
+#elif defined(JPH_USE_NEON)
+	return vld1q_u32(inV);
+#else
+	return UVec4(inV[0], inV[1], inV[2], inV[3]);
+#endif
+}
+
+UVec4 UVec4::sLoadInt4Aligned(const uint32 *inV)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_load_si128(reinterpret_cast<const __m128i *>(inV));
+#elif defined(JPH_USE_NEON)
+	return vld1q_u32(inV); // ARM doesn't make distinction between aligned or not
+#else
+	return UVec4(inV[0], inV[1], inV[2], inV[3]);
+#endif
+}
+
+template <const int Scale>
+UVec4 UVec4::sGatherInt4(const uint32 *inBase, UVec4Arg inOffsets)
+{
+#ifdef JPH_USE_AVX2
+	return _mm_i32gather_epi32(reinterpret_cast<const int *>(inBase), inOffsets.mValue, Scale);
+#else
+	const uint8 *base = reinterpret_cast<const uint8 *>(inBase);
+	uint32 x = *reinterpret_cast<const uint32 *>(base + inOffsets.GetX() * Scale);
+	uint32 y = *reinterpret_cast<const uint32 *>(base + inOffsets.GetY() * Scale);
+	uint32 z = *reinterpret_cast<const uint32 *>(base + inOffsets.GetZ() * Scale);
+	uint32 w = *reinterpret_cast<const uint32 *>(base + inOffsets.GetW() * Scale);
+	return UVec4(x, y, z, w);
+#endif
+}
+
+UVec4 UVec4::sMin(UVec4Arg inV1, UVec4Arg inV2)
+{
+#if defined(JPH_USE_SSE4_1)
+	return _mm_min_epu32(inV1.mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vminq_u32(inV1.mValue, inV2.mValue);
+#else
+	UVec4 result;
+	for (int i = 0; i < 4; i++)
+		result.mU32[i] = min(inV1.mU32[i], inV2.mU32[i]);
+	return result;
+#endif
+}
+
+UVec4 UVec4::sMax(UVec4Arg inV1, UVec4Arg inV2)
+{
+#if defined(JPH_USE_SSE4_1)
+	return _mm_max_epu32(inV1.mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vmaxq_u32(inV1.mValue, inV2.mValue);
+#else
+	UVec4 result;
+	for (int i = 0; i < 4; i++)
+		result.mU32[i] = max(inV1.mU32[i], inV2.mU32[i]);
+	return result;
+#endif
+}
+
+UVec4 UVec4::sEquals(UVec4Arg inV1, UVec4Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_cmpeq_epi32(inV1.mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vceqq_u32(inV1.mValue, inV2.mValue);
+#else
+	return UVec4(inV1.mU32[0] == inV2.mU32[0]? 0xffffffffu : 0,
+				 inV1.mU32[1] == inV2.mU32[1]? 0xffffffffu : 0,
+				 inV1.mU32[2] == inV2.mU32[2]? 0xffffffffu : 0,
+				 inV1.mU32[3] == inV2.mU32[3]? 0xffffffffu : 0);
+#endif
+}
+
+UVec4 UVec4::sSelect(UVec4Arg inNotSet, UVec4Arg inSet, UVec4Arg inControl)
+{
+#if defined(JPH_USE_SSE4_1) && !defined(JPH_PLATFORM_WASM) // _mm_blendv_ps has problems on FireFox
+	return _mm_castps_si128(_mm_blendv_ps(_mm_castsi128_ps(inNotSet.mValue), _mm_castsi128_ps(inSet.mValue), _mm_castsi128_ps(inControl.mValue)));
+#elif defined(JPH_USE_SSE)
+	__m128 is_set = _mm_castsi128_ps(_mm_srai_epi32(inControl.mValue, 31));
+	return _mm_castps_si128(_mm_or_ps(_mm_and_ps(is_set, _mm_castsi128_ps(inSet.mValue)), _mm_andnot_ps(is_set, _mm_castsi128_ps(inNotSet.mValue))));
+#elif defined(JPH_USE_NEON)
+	return vbslq_u32(vreinterpretq_u32_s32(vshrq_n_s32(vreinterpretq_s32_u32(inControl.mValue), 31)), inSet.mValue, inNotSet.mValue);
+#else
+	UVec4 result;
+	for (int i = 0; i < 4; i++)
+		result.mU32[i] = (inControl.mU32[i] & 0x80000000u) ? inSet.mU32[i] : inNotSet.mU32[i];
+	return result;
+#endif
+}
+
+UVec4 UVec4::sOr(UVec4Arg inV1, UVec4Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_or_si128(inV1.mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vorrq_u32(inV1.mValue, inV2.mValue);
+#else
+	return UVec4(inV1.mU32[0] | inV2.mU32[0],
+				 inV1.mU32[1] | inV2.mU32[1],
+				 inV1.mU32[2] | inV2.mU32[2],
+				 inV1.mU32[3] | inV2.mU32[3]);
+#endif
+}
+
+UVec4 UVec4::sXor(UVec4Arg inV1, UVec4Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_xor_si128(inV1.mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return veorq_u32(inV1.mValue, inV2.mValue);
+#else
+	return UVec4(inV1.mU32[0] ^ inV2.mU32[0],
+				 inV1.mU32[1] ^ inV2.mU32[1],
+				 inV1.mU32[2] ^ inV2.mU32[2],
+				 inV1.mU32[3] ^ inV2.mU32[3]);
+#endif
+}
+
+UVec4 UVec4::sAnd(UVec4Arg inV1, UVec4Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_and_si128(inV1.mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vandq_u32(inV1.mValue, inV2.mValue);
+#else
+	return UVec4(inV1.mU32[0] & inV2.mU32[0],
+				 inV1.mU32[1] & inV2.mU32[1],
+				 inV1.mU32[2] & inV2.mU32[2],
+				 inV1.mU32[3] & inV2.mU32[3]);
+#endif
+}
+
+
+UVec4 UVec4::sNot(UVec4Arg inV1)
+{
+#if defined(JPH_USE_AVX512)
+	return _mm_ternarylogic_epi32(inV1.mValue, inV1.mValue, inV1.mValue, 0b01010101);
+#elif defined(JPH_USE_SSE)
+	return sXor(inV1, sReplicate(0xffffffff));
+#elif defined(JPH_USE_NEON)
+	return vmvnq_u32(inV1.mValue);
+#else
+	return UVec4(~inV1.mU32[0], ~inV1.mU32[1], ~inV1.mU32[2], ~inV1.mU32[3]);
+#endif
+}
+
+UVec4 UVec4::sSort4True(UVec4Arg inValue, UVec4Arg inIndex)
+{
+	// If inValue.z is false then shift W to Z
+	UVec4 v = UVec4::sSelect(inIndex.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_W>(), inIndex, inValue.SplatZ());
+
+	// If inValue.y is false then shift Z and further to Y and further
+	v = UVec4::sSelect(v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_W>(), v, inValue.SplatY());
+
+	// If inValue.x is false then shift X and further to Y and further
+	v = UVec4::sSelect(v.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_W>(), v, inValue.SplatX());
+
+	return v;
+}
+
+UVec4 UVec4::operator * (UVec4Arg inV2) const
+{
+#if defined(JPH_USE_SSE4_1)
+	return _mm_mullo_epi32(mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vmulq_u32(mValue, inV2.mValue);
+#else
+	UVec4 result;
+	for (int i = 0; i < 4; i++)
+		result.mU32[i] = mU32[i] * inV2.mU32[i];
+	return result;
+#endif
+}
+
+UVec4 UVec4::operator + (UVec4Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_add_epi32(mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vaddq_u32(mValue, inV2.mValue);
+#else
+	return UVec4(mU32[0] + inV2.mU32[0],
+				 mU32[1] + inV2.mU32[1],
+				 mU32[2] + inV2.mU32[2],
+				 mU32[3] + inV2.mU32[3]);
+#endif
+}
+
+UVec4 &UVec4::operator += (UVec4Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	mValue = _mm_add_epi32(mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	mValue = vaddq_u32(mValue, inV2.mValue);
+#else
+	for (int i = 0; i < 4; ++i)
+		mU32[i] += inV2.mU32[i];
+#endif
+	return *this;
+}
+
+UVec4 UVec4::SplatX() const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_shuffle_epi32(mValue, _MM_SHUFFLE(0, 0, 0, 0));
+#elif defined(JPH_USE_NEON)
+	return vdupq_laneq_u32(mValue, 0);
+#else
+	return UVec4(mU32[0], mU32[0], mU32[0], mU32[0]);
+#endif
+}
+
+UVec4 UVec4::SplatY() const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_shuffle_epi32(mValue, _MM_SHUFFLE(1, 1, 1, 1));
+#elif defined(JPH_USE_NEON)
+	return vdupq_laneq_u32(mValue, 1);
+#else
+	return UVec4(mU32[1], mU32[1], mU32[1], mU32[1]);
+#endif
+}
+
+UVec4 UVec4::SplatZ() const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_shuffle_epi32(mValue, _MM_SHUFFLE(2, 2, 2, 2));
+#elif defined(JPH_USE_NEON)
+	return vdupq_laneq_u32(mValue, 2);
+#else
+	return UVec4(mU32[2], mU32[2], mU32[2], mU32[2]);
+#endif
+}
+
+UVec4 UVec4::SplatW() const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_shuffle_epi32(mValue, _MM_SHUFFLE(3, 3, 3, 3));
+#elif defined(JPH_USE_NEON)
+	return vdupq_laneq_u32(mValue, 3);
+#else
+	return UVec4(mU32[3], mU32[3], mU32[3], mU32[3]);
+#endif
+}
+
+Vec4 UVec4::ToFloat() const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_cvtepi32_ps(mValue);
+#elif defined(JPH_USE_NEON)
+	return vcvtq_f32_u32(mValue);
+#else
+	return Vec4((float)mU32[0], (float)mU32[1], (float)mU32[2], (float)mU32[3]);
+#endif
+}
+
+Vec4 UVec4::ReinterpretAsFloat() const
+{
+#if defined(JPH_USE_SSE)
+	return Vec4(_mm_castsi128_ps(mValue));
+#elif defined(JPH_USE_NEON)
+	return vreinterpretq_f32_u32(mValue);
+#else
+	return *reinterpret_cast<const Vec4 *>(this);
+#endif
+}
+
+void UVec4::StoreInt4(uint32 *outV) const
+{
+#if defined(JPH_USE_SSE)
+	_mm_storeu_si128(reinterpret_cast<__m128i *>(outV), mValue);
+#elif defined(JPH_USE_NEON)
+	vst1q_u32(outV, mValue);
+#else
+	for (int i = 0; i < 4; ++i)
+		outV[i] = mU32[i];
+#endif
+}
+
+void UVec4::StoreInt4Aligned(uint32 *outV) const
+{
+#if defined(JPH_USE_SSE)
+	_mm_store_si128(reinterpret_cast<__m128i *>(outV), mValue);
+#elif defined(JPH_USE_NEON)
+	vst1q_u32(outV, mValue); // ARM doesn't make distinction between aligned or not
+#else
+	for (int i = 0; i < 4; ++i)
+		outV[i] = mU32[i];
+#endif
+}
+
+int UVec4::CountTrues() const
+{
+#if defined(JPH_USE_SSE)
+	return CountBits(_mm_movemask_ps(_mm_castsi128_ps(mValue)));
+#elif defined(JPH_USE_NEON)
+	return vaddvq_u32(vshrq_n_u32(mValue, 31));
+#else
+	return (mU32[0] >> 31) + (mU32[1] >> 31) + (mU32[2] >> 31) + (mU32[3] >> 31);
+#endif
+}
+
+int UVec4::GetTrues() const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_movemask_ps(_mm_castsi128_ps(mValue));
+#elif defined(JPH_USE_NEON)
+	int32x4_t shift = JPH_NEON_INT32x4(0, 1, 2, 3);
+	return vaddvq_u32(vshlq_u32(vshrq_n_u32(mValue, 31), shift));
+#else
+	return (mU32[0] >> 31) | ((mU32[1] >> 31) << 1) | ((mU32[2] >> 31) << 2) | ((mU32[3] >> 31) << 3);
+#endif
+}
+
+bool UVec4::TestAnyTrue() const
+{
+	return GetTrues() != 0;
+}
+
+bool UVec4::TestAnyXYZTrue() const
+{
+	return (GetTrues() & 0b111) != 0;
+}
+
+bool UVec4::TestAllTrue() const
+{
+	return GetTrues() == 0b1111;
+}
+
+bool UVec4::TestAllXYZTrue() const
+{
+	return (GetTrues() & 0b111) == 0b111;
+}
+
+template <const uint Count>
+UVec4 UVec4::LogicalShiftLeft() const
+{
+	static_assert(Count <= 31, "Invalid shift");
+
+#if defined(JPH_USE_SSE)
+	return _mm_slli_epi32(mValue, Count);
+#elif defined(JPH_USE_NEON)
+	return vshlq_n_u32(mValue, Count);
+#else
+	return UVec4(mU32[0] << Count, mU32[1] << Count, mU32[2] << Count, mU32[3] << Count);
+#endif
+}
+
+template <const uint Count>
+UVec4 UVec4::LogicalShiftRight() const
+{
+	static_assert(Count <= 31, "Invalid shift");
+
+#if defined(JPH_USE_SSE)
+	return _mm_srli_epi32(mValue, Count);
+#elif defined(JPH_USE_NEON)
+	return vshrq_n_u32(mValue, Count);
+#else
+	return UVec4(mU32[0] >> Count, mU32[1] >> Count, mU32[2] >> Count, mU32[3] >> Count);
+#endif
+}
+
+template <const uint Count>
+UVec4 UVec4::ArithmeticShiftRight() const
+{
+	static_assert(Count <= 31, "Invalid shift");
+
+#if defined(JPH_USE_SSE)
+	return _mm_srai_epi32(mValue, Count);
+#elif defined(JPH_USE_NEON)
+	return vreinterpretq_u32_s32(vshrq_n_s32(vreinterpretq_s32_u32(mValue), Count));
+#else
+	return UVec4(uint32(int32_t(mU32[0]) >> Count),
+				 uint32(int32_t(mU32[1]) >> Count),
+				 uint32(int32_t(mU32[2]) >> Count),
+				 uint32(int32_t(mU32[3]) >> Count));
+#endif
+}
+
+UVec4 UVec4::Expand4Uint16Lo() const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_unpacklo_epi16(mValue, _mm_castps_si128(_mm_setzero_ps()));
+#elif defined(JPH_USE_NEON)
+	uint16x4_t value = vget_low_u16(vreinterpretq_u16_u32(mValue));
+	uint16x4_t zero = vdup_n_u16(0);
+	return vreinterpretq_u32_u16(vcombine_u16(vzip1_u16(value, zero), vzip2_u16(value, zero)));
+#else
+	return UVec4(mU32[0] & 0xffff,
+				 (mU32[0] >> 16) & 0xffff,
+				 mU32[1] & 0xffff,
+				 (mU32[1] >> 16) & 0xffff);
+#endif
+}
+
+UVec4 UVec4::Expand4Uint16Hi() const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_unpackhi_epi16(mValue, _mm_castps_si128(_mm_setzero_ps()));
+#elif defined(JPH_USE_NEON)
+	uint16x4_t value = vget_high_u16(vreinterpretq_u16_u32(mValue));
+	uint16x4_t zero = vdup_n_u16(0);
+	return vreinterpretq_u32_u16(vcombine_u16(vzip1_u16(value, zero), vzip2_u16(value, zero)));
+#else
+	return UVec4(mU32[2] & 0xffff,
+				 (mU32[2] >> 16) & 0xffff,
+				 mU32[3] & 0xffff,
+				 (mU32[3] >> 16) & 0xffff);
+#endif
+}
+
+UVec4 UVec4::Expand4Byte0() const
+{
+#if defined(JPH_USE_SSE4_1)
+	return _mm_shuffle_epi8(mValue, _mm_set_epi32(int(0xffffff03), int(0xffffff02), int(0xffffff01), int(0xffffff00)));
+#elif defined(JPH_USE_NEON)
+	uint8x16_t idx = JPH_NEON_UINT8x16(0x00, 0x7f, 0x7f, 0x7f, 0x01, 0x7f, 0x7f, 0x7f, 0x02, 0x7f, 0x7f, 0x7f, 0x03, 0x7f, 0x7f, 0x7f);
+	return vreinterpretq_u32_s8(vqtbl1q_s8(vreinterpretq_s8_u32(mValue), idx));
+#else
+	UVec4 result;
+	for (int i = 0; i < 4; i++)
+		result.mU32[i] = (mU32[0] >> (i * 8)) & 0xff;
+	return result;
+#endif
+}
+
+UVec4 UVec4::Expand4Byte4() const
+{
+#if defined(JPH_USE_SSE4_1)
+	return _mm_shuffle_epi8(mValue, _mm_set_epi32(int(0xffffff07), int(0xffffff06), int(0xffffff05), int(0xffffff04)));
+#elif defined(JPH_USE_NEON)
+	uint8x16_t idx = JPH_NEON_UINT8x16(0x04, 0x7f, 0x7f, 0x7f, 0x05, 0x7f, 0x7f, 0x7f, 0x06, 0x7f, 0x7f, 0x7f, 0x07, 0x7f, 0x7f, 0x7f);
+	return vreinterpretq_u32_s8(vqtbl1q_s8(vreinterpretq_s8_u32(mValue), idx));
+#else
+	UVec4 result;
+	for (int i = 0; i < 4; i++)
+		result.mU32[i] = (mU32[1] >> (i * 8)) & 0xff;
+	return result;
+#endif
+}
+
+UVec4 UVec4::Expand4Byte8() const
+{
+#if defined(JPH_USE_SSE4_1)
+	return _mm_shuffle_epi8(mValue, _mm_set_epi32(int(0xffffff0b), int(0xffffff0a), int(0xffffff09), int(0xffffff08)));
+#elif defined(JPH_USE_NEON)
+	uint8x16_t idx = JPH_NEON_UINT8x16(0x08, 0x7f, 0x7f, 0x7f, 0x09, 0x7f, 0x7f, 0x7f, 0x0a, 0x7f, 0x7f, 0x7f, 0x0b, 0x7f, 0x7f, 0x7f);
+	return vreinterpretq_u32_s8(vqtbl1q_s8(vreinterpretq_s8_u32(mValue), idx));
+#else
+	UVec4 result;
+	for (int i = 0; i < 4; i++)
+		result.mU32[i] = (mU32[2] >> (i * 8)) & 0xff;
+	return result;
+#endif
+}
+
+UVec4 UVec4::Expand4Byte12() const
+{
+#if defined(JPH_USE_SSE4_1)
+	return _mm_shuffle_epi8(mValue, _mm_set_epi32(int(0xffffff0f), int(0xffffff0e), int(0xffffff0d), int(0xffffff0c)));
+#elif defined(JPH_USE_NEON)
+	uint8x16_t idx = JPH_NEON_UINT8x16(0x0c, 0x7f, 0x7f, 0x7f, 0x0d, 0x7f, 0x7f, 0x7f, 0x0e, 0x7f, 0x7f, 0x7f, 0x0f, 0x7f, 0x7f, 0x7f);
+	return vreinterpretq_u32_s8(vqtbl1q_s8(vreinterpretq_s8_u32(mValue), idx));
+#else
+	UVec4 result;
+	for (int i = 0; i < 4; i++)
+		result.mU32[i] = (mU32[3] >> (i * 8)) & 0xff;
+	return result;
+#endif
+}
+
+UVec4 UVec4::ShiftComponents4Minus(int inCount) const
+{
+#if defined(JPH_USE_SSE4_1) || defined(JPH_USE_NEON)
+	alignas(UVec4) static constexpr uint32 sFourMinusXShuffle[5][4] =
+	{
+		{ 0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff },
+		{ 0x0f0e0d0c, 0xffffffff, 0xffffffff, 0xffffffff },
+		{ 0x0b0a0908, 0x0f0e0d0c, 0xffffffff, 0xffffffff },
+		{ 0x07060504, 0x0b0a0908, 0x0f0e0d0c, 0xffffffff },
+		{ 0x03020100, 0x07060504, 0x0b0a0908, 0x0f0e0d0c }
+	};
+#endif
+
+#if defined(JPH_USE_SSE4_1)
+	return _mm_shuffle_epi8(mValue, *reinterpret_cast<const UVec4::Type *>(sFourMinusXShuffle[inCount]));
+#elif defined(JPH_USE_NEON)
+	uint8x16_t idx = vreinterpretq_u8_u32(*reinterpret_cast<const UVec4::Type *>(sFourMinusXShuffle[inCount]));
+	return vreinterpretq_u32_s8(vqtbl1q_s8(vreinterpretq_s8_u32(mValue), idx));
+#else
+	UVec4 result = UVec4::sZero();
+	for (int i = 0; i < inCount; i++)
+		result.mU32[i] = mU32[i + 4 - inCount];
+	return result;
+#endif
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Math/Vec3.cpp b/thirdparty/jolt_physics/Jolt/Math/Vec3.cpp
new file mode 100644
index 0000000000..c865387f79
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/Vec3.cpp
@@ -0,0 +1,71 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Math/Vec3.h>
+
+JPH_NAMESPACE_BEGIN
+
+static void sAddVertex(StaticArray<Vec3, 1026> &ioVertices, Vec3Arg inVertex)
+{
+	bool found = false;
+	for (const Vec3 &v : ioVertices)
+		if (v == inVertex)
+		{
+			found = true;
+			break;
+		}
+	if (!found)
+		ioVertices.push_back(inVertex);
+}
+
+static void sCreateVertices(StaticArray<Vec3, 1026> &ioVertices, Vec3Arg inDir1, Vec3Arg inDir2, Vec3Arg inDir3, int inLevel)
+{
+	Vec3 center1 = (inDir1 + inDir2).Normalized();
+	Vec3 center2 = (inDir2 + inDir3).Normalized();
+	Vec3 center3 = (inDir3 + inDir1).Normalized();
+
+	sAddVertex(ioVertices, center1);
+	sAddVertex(ioVertices, center2);
+	sAddVertex(ioVertices, center3);
+
+	if (inLevel > 0)
+	{
+		int new_level = inLevel - 1;
+		sCreateVertices(ioVertices, inDir1, center1, center3, new_level);
+		sCreateVertices(ioVertices, center1, center2, center3, new_level);
+		sCreateVertices(ioVertices, center1, inDir2, center2, new_level);
+		sCreateVertices(ioVertices, center3, center2, inDir3, new_level);
+	}
+}
+
+const StaticArray<Vec3, 1026> Vec3::sUnitSphere = []() {
+
+	const int level = 3;
+
+	StaticArray<Vec3, 1026> verts;
+
+	// Add unit axis
+	verts.push_back(Vec3::sAxisX());
+	verts.push_back(-Vec3::sAxisX());
+	verts.push_back(Vec3::sAxisY());
+	verts.push_back(-Vec3::sAxisY());
+	verts.push_back(Vec3::sAxisZ());
+	verts.push_back(-Vec3::sAxisZ());
+
+	// Subdivide
+	sCreateVertices(verts, Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ(), level);
+	sCreateVertices(verts, -Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ(), level);
+	sCreateVertices(verts, Vec3::sAxisX(), -Vec3::sAxisY(), Vec3::sAxisZ(), level);
+	sCreateVertices(verts, -Vec3::sAxisX(), -Vec3::sAxisY(), Vec3::sAxisZ(), level);
+	sCreateVertices(verts, Vec3::sAxisX(), Vec3::sAxisY(), -Vec3::sAxisZ(), level);
+	sCreateVertices(verts, -Vec3::sAxisX(), Vec3::sAxisY(), -Vec3::sAxisZ(), level);
+	sCreateVertices(verts, Vec3::sAxisX(), -Vec3::sAxisY(), -Vec3::sAxisZ(), level);
+	sCreateVertices(verts, -Vec3::sAxisX(), -Vec3::sAxisY(), -Vec3::sAxisZ(), level);
+
+	return verts;
+}();
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Math/Vec3.h b/thirdparty/jolt_physics/Jolt/Math/Vec3.h
new file mode 100644
index 0000000000..18264db7e2
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/Vec3.h
@@ -0,0 +1,295 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/StaticArray.h>
+#include <Jolt/Math/Float3.h>
+#include <Jolt/Math/Swizzle.h>
+#include <Jolt/Math/MathTypes.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// 3 component vector (stored as 4 vectors).
+/// Note that we keep the 4th component the same as the 3rd component to avoid divisions by zero when JPH_FLOATING_POINT_EXCEPTIONS_ENABLED defined
+class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) Vec3
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	// Underlying vector type
+#if defined(JPH_USE_SSE)
+	using Type = __m128;
+#elif defined(JPH_USE_NEON)
+	using Type = float32x4_t;
+#else
+	using Type = Vec4::Type;
+#endif
+
+	// Argument type
+	using ArgType = Vec3Arg;
+
+	/// Constructor
+								Vec3() = default; ///< Intentionally not initialized for performance reasons
+								Vec3(const Vec3 &inRHS) = default;
+	Vec3 &						operator = (const Vec3 &inRHS) = default;
+	explicit JPH_INLINE			Vec3(Vec4Arg inRHS);
+	JPH_INLINE					Vec3(Type inRHS) : mValue(inRHS)				{ CheckW(); }
+
+	/// Load 3 floats from memory
+	explicit JPH_INLINE			Vec3(const Float3 &inV);
+
+	/// Create a vector from 3 components
+	JPH_INLINE					Vec3(float inX, float inY, float inZ);
+
+	/// Vector with all zeros
+	static JPH_INLINE Vec3		sZero();
+
+	/// Vector with all NaN's
+	static JPH_INLINE Vec3		sNaN();
+
+	/// Vectors with the principal axis
+	static JPH_INLINE Vec3		sAxisX()										{ return Vec3(1, 0, 0); }
+	static JPH_INLINE Vec3		sAxisY()										{ return Vec3(0, 1, 0); }
+	static JPH_INLINE Vec3		sAxisZ()										{ return Vec3(0, 0, 1); }
+
+	/// Replicate inV across all components
+	static JPH_INLINE Vec3		sReplicate(float inV);
+
+	/// Load 3 floats from memory (reads 32 bits extra which it doesn't use)
+	static JPH_INLINE Vec3		sLoadFloat3Unsafe(const Float3 &inV);
+
+	/// Return the minimum value of each of the components
+	static JPH_INLINE Vec3		sMin(Vec3Arg inV1, Vec3Arg inV2);
+
+	/// Return the maximum of each of the components
+	static JPH_INLINE Vec3		sMax(Vec3Arg inV1, Vec3Arg inV2);
+
+	/// Clamp a vector between min and max (component wise)
+	static JPH_INLINE Vec3		sClamp(Vec3Arg inV, Vec3Arg inMin, Vec3Arg inMax);
+
+	/// Equals (component wise)
+	static JPH_INLINE UVec4		sEquals(Vec3Arg inV1, Vec3Arg inV2);
+
+	/// Less than (component wise)
+	static JPH_INLINE UVec4		sLess(Vec3Arg inV1, Vec3Arg inV2);
+
+	/// Less than or equal (component wise)
+	static JPH_INLINE UVec4		sLessOrEqual(Vec3Arg inV1, Vec3Arg inV2);
+
+	/// Greater than (component wise)
+	static JPH_INLINE UVec4		sGreater(Vec3Arg inV1, Vec3Arg inV2);
+
+	/// Greater than or equal (component wise)
+	static JPH_INLINE UVec4		sGreaterOrEqual(Vec3Arg inV1, Vec3Arg inV2);
+
+	/// Calculates inMul1 * inMul2 + inAdd
+	static JPH_INLINE Vec3		sFusedMultiplyAdd(Vec3Arg inMul1, Vec3Arg inMul2, Vec3Arg inAdd);
+
+	/// Component wise select, returns inNotSet when highest bit of inControl = 0 and inSet when highest bit of inControl = 1
+	static JPH_INLINE Vec3		sSelect(Vec3Arg inNotSet, Vec3Arg inSet, UVec4Arg inControl);
+
+	/// Logical or (component wise)
+	static JPH_INLINE Vec3		sOr(Vec3Arg inV1, Vec3Arg inV2);
+
+	/// Logical xor (component wise)
+	static JPH_INLINE Vec3		sXor(Vec3Arg inV1, Vec3Arg inV2);
+
+	/// Logical and (component wise)
+	static JPH_INLINE Vec3		sAnd(Vec3Arg inV1, Vec3Arg inV2);
+
+	/// Get unit vector given spherical coordinates
+	/// inTheta \f$\in [0, \pi]\f$ is angle between vector and z-axis
+	/// inPhi \f$\in [0, 2 \pi]\f$ is the angle in the xy-plane starting from the x axis and rotating counter clockwise around the z-axis
+	static JPH_INLINE Vec3		sUnitSpherical(float inTheta, float inPhi);
+
+	/// A set of vectors uniformly spanning the surface of a unit sphere, usable for debug purposes
+	JPH_EXPORT static const StaticArray<Vec3, 1026> sUnitSphere;
+
+	/// Get random unit vector
+	template <class Random>
+	static inline Vec3			sRandom(Random &inRandom);
+
+	/// Get individual components
+#if defined(JPH_USE_SSE)
+	JPH_INLINE float			GetX() const									{ return _mm_cvtss_f32(mValue); }
+	JPH_INLINE float			GetY() const									{ return mF32[1]; }
+	JPH_INLINE float			GetZ() const									{ return mF32[2]; }
+#elif defined(JPH_USE_NEON)
+	JPH_INLINE float			GetX() const									{ return vgetq_lane_f32(mValue, 0); }
+	JPH_INLINE float			GetY() const									{ return vgetq_lane_f32(mValue, 1); }
+	JPH_INLINE float			GetZ() const									{ return vgetq_lane_f32(mValue, 2); }
+#else
+	JPH_INLINE float			GetX() const									{ return mF32[0]; }
+	JPH_INLINE float			GetY() const									{ return mF32[1]; }
+	JPH_INLINE float			GetZ() const									{ return mF32[2]; }
+#endif
+
+	/// Set individual components
+	JPH_INLINE void				SetX(float inX)									{ mF32[0] = inX; }
+	JPH_INLINE void				SetY(float inY)									{ mF32[1] = inY; }
+	JPH_INLINE void				SetZ(float inZ)									{ mF32[2] = mF32[3] = inZ; } // Assure Z and W are the same
+
+	/// Set all components
+	JPH_INLINE void				Set(float inX, float inY, float inZ)			{ *this = Vec3(inX, inY, inZ); }
+
+	/// Get float component by index
+	JPH_INLINE float			operator [] (uint inCoordinate) const			{ JPH_ASSERT(inCoordinate < 3); return mF32[inCoordinate]; }
+
+	/// Set float component by index
+	JPH_INLINE void				SetComponent(uint inCoordinate, float inValue)	{ JPH_ASSERT(inCoordinate < 3); mF32[inCoordinate] = inValue; mValue = sFixW(mValue); } // Assure Z and W are the same
+
+	/// Comparison
+	JPH_INLINE bool				operator == (Vec3Arg inV2) const;
+	JPH_INLINE bool				operator != (Vec3Arg inV2) const				{ return !(*this == inV2); }
+
+	/// Test if two vectors are close
+	JPH_INLINE bool				IsClose(Vec3Arg inV2, float inMaxDistSq = 1.0e-12f) const;
+
+	/// Test if vector is near zero
+	JPH_INLINE bool				IsNearZero(float inMaxDistSq = 1.0e-12f) const;
+
+	/// Test if vector is normalized
+	JPH_INLINE bool				IsNormalized(float inTolerance = 1.0e-6f) const;
+
+	/// Test if vector contains NaN elements
+	JPH_INLINE bool				IsNaN() const;
+
+	/// Multiply two float vectors (component wise)
+	JPH_INLINE Vec3				operator * (Vec3Arg inV2) const;
+
+	/// Multiply vector with float
+	JPH_INLINE Vec3				operator * (float inV2) const;
+
+	/// Multiply vector with float
+	friend JPH_INLINE Vec3		operator * (float inV1, Vec3Arg inV2);
+
+	/// Divide vector by float
+	JPH_INLINE Vec3				operator / (float inV2) const;
+
+	/// Multiply vector with float
+	JPH_INLINE Vec3 &			operator *= (float inV2);
+
+	/// Multiply vector with vector
+	JPH_INLINE Vec3 &			operator *= (Vec3Arg inV2);
+
+	/// Divide vector by float
+	JPH_INLINE Vec3 &			operator /= (float inV2);
+
+	/// Add two float vectors (component wise)
+	JPH_INLINE Vec3				operator + (Vec3Arg inV2) const;
+
+	/// Add two float vectors (component wise)
+	JPH_INLINE Vec3 &			operator += (Vec3Arg inV2);
+
+	/// Negate
+	JPH_INLINE Vec3				operator - () const;
+
+	/// Subtract two float vectors (component wise)
+	JPH_INLINE Vec3				operator - (Vec3Arg inV2) const;
+
+	/// Subtract two float vectors (component wise)
+	JPH_INLINE Vec3 &			operator -= (Vec3Arg inV2);
+
+	/// Divide (component wise)
+	JPH_INLINE Vec3				operator / (Vec3Arg inV2) const;
+
+	/// Swizzle the elements in inV
+	template<uint32 SwizzleX, uint32 SwizzleY, uint32 SwizzleZ>
+	JPH_INLINE Vec3				Swizzle() const;
+
+	/// Replicate the X component to all components
+	JPH_INLINE Vec4				SplatX() const;
+
+	/// Replicate the Y component to all components
+	JPH_INLINE Vec4				SplatY() const;
+
+	/// Replicate the Z component to all components
+	JPH_INLINE Vec4				SplatZ() const;
+
+	/// Get index of component with lowest value
+	JPH_INLINE int				GetLowestComponentIndex() const;
+
+	/// Get index of component with highest value
+	JPH_INLINE int				GetHighestComponentIndex() const;
+
+	/// Return the absolute value of each of the components
+	JPH_INLINE Vec3				Abs() const;
+
+	/// Reciprocal vector (1 / value) for each of the components
+	JPH_INLINE Vec3				Reciprocal() const;
+
+	/// Cross product
+	JPH_INLINE Vec3				Cross(Vec3Arg inV2) const;
+
+	/// Dot product, returns the dot product in X, Y and Z components
+	JPH_INLINE Vec3				DotV(Vec3Arg inV2) const;
+
+	/// Dot product, returns the dot product in X, Y, Z and W components
+	JPH_INLINE Vec4				DotV4(Vec3Arg inV2) const;
+
+	/// Dot product
+	JPH_INLINE float			Dot(Vec3Arg inV2) const;
+
+	/// Squared length of vector
+	JPH_INLINE float			LengthSq() const;
+
+	/// Length of vector
+	JPH_INLINE float			Length() const;
+
+	/// Normalize vector
+	JPH_INLINE Vec3				Normalized() const;
+
+	/// Normalize vector or return inZeroValue if the length of the vector is zero
+	JPH_INLINE Vec3				NormalizedOr(Vec3Arg inZeroValue) const;
+
+	/// Store 3 floats to memory
+	JPH_INLINE void				StoreFloat3(Float3 *outV) const;
+
+	/// Convert each component from a float to an int
+	JPH_INLINE UVec4			ToInt() const;
+
+	/// Reinterpret Vec3 as a UVec4 (doesn't change the bits)
+	JPH_INLINE UVec4			ReinterpretAsInt() const;
+
+	/// Get the minimum of X, Y and Z
+	JPH_INLINE float			ReduceMin() const;
+
+	/// Get the maximum of X, Y and Z
+	JPH_INLINE float			ReduceMax() const;
+
+	/// Component wise square root
+	JPH_INLINE Vec3				Sqrt() const;
+
+	/// Get normalized vector that is perpendicular to this vector
+	JPH_INLINE Vec3				GetNormalizedPerpendicular() const;
+
+	/// Get vector that contains the sign of each element (returns 1.0f if positive, -1.0f if negative)
+	JPH_INLINE Vec3				GetSign() const;
+
+	/// To String
+	friend ostream &			operator << (ostream &inStream, Vec3Arg inV)
+	{
+		inStream << inV.mF32[0] << ", " << inV.mF32[1] << ", " << inV.mF32[2];
+		return inStream;
+	}
+
+	/// Internal helper function that checks that W is equal to Z, so e.g. dividing by it should not generate div by 0
+	JPH_INLINE void				CheckW() const;
+
+	/// Internal helper function that ensures that the Z component is replicated to the W component to prevent divisions by zero
+	static JPH_INLINE Type		sFixW(Type inValue);
+
+	union
+	{
+		Type					mValue;
+		float					mF32[4];
+	};
+};
+
+static_assert(std::is_trivial<Vec3>(), "Is supposed to be a trivial type!");
+
+JPH_NAMESPACE_END
+
+#include "Vec3.inl"
diff --git a/thirdparty/jolt_physics/Jolt/Math/Vec3.inl b/thirdparty/jolt_physics/Jolt/Math/Vec3.inl
new file mode 100644
index 0000000000..5a47f40ce6
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/Vec3.inl
@@ -0,0 +1,859 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Math/Vec4.h>
+#include <Jolt/Math/UVec4.h>
+#include <Jolt/Core/HashCombine.h>
+
+JPH_SUPPRESS_WARNINGS_STD_BEGIN
+#include <random>
+JPH_SUPPRESS_WARNINGS_STD_END
+
+// Create a std::hash/JPH::Hash for Vec3
+JPH_MAKE_HASHABLE(JPH::Vec3, t.GetX(), t.GetY(), t.GetZ())
+
+JPH_NAMESPACE_BEGIN
+
+void Vec3::CheckW() const
+{
+#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+	// Avoid asserts when both components are NaN
+	JPH_ASSERT(reinterpret_cast<const uint32 *>(mF32)[2] == reinterpret_cast<const uint32 *>(mF32)[3]);
+#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+}
+
+JPH_INLINE Vec3::Type Vec3::sFixW(Type inValue)
+{
+#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+	#if defined(JPH_USE_SSE)
+		return _mm_shuffle_ps(inValue, inValue, _MM_SHUFFLE(2, 2, 1, 0));
+	#elif defined(JPH_USE_NEON)
+		return JPH_NEON_SHUFFLE_F32x4(inValue, inValue, 0, 1, 2, 2);
+	#else
+		Type value;
+		value.mData[0] = inValue.mData[0];
+		value.mData[1] = inValue.mData[1];
+		value.mData[2] = inValue.mData[2];
+		value.mData[3] = inValue.mData[2];
+		return value;
+	#endif
+#else
+	return inValue;
+#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+}
+
+Vec3::Vec3(Vec4Arg inRHS) :
+	mValue(sFixW(inRHS.mValue))
+{
+}
+
+Vec3::Vec3(const Float3 &inV)
+{
+#if defined(JPH_USE_SSE)
+	Type x = _mm_load_ss(&inV.x);
+	Type y = _mm_load_ss(&inV.y);
+	Type z = _mm_load_ss(&inV.z);
+	Type xy = _mm_unpacklo_ps(x, y);
+	mValue = _mm_shuffle_ps(xy, z, _MM_SHUFFLE(0, 0, 1, 0)); // Assure Z and W are the same
+#elif defined(JPH_USE_NEON)
+	float32x2_t xy = vld1_f32(&inV.x);
+	float32x2_t zz = vdup_n_f32(inV.z); // Assure Z and W are the same
+	mValue = vcombine_f32(xy, zz);
+#else
+	mF32[0] = inV[0];
+	mF32[1] = inV[1];
+	mF32[2] = inV[2];
+	#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+		mF32[3] = inV[2];
+	#endif
+#endif
+}
+
+Vec3::Vec3(float inX, float inY, float inZ)
+{
+#if defined(JPH_USE_SSE)
+	mValue = _mm_set_ps(inZ, inZ, inY, inX);
+#elif defined(JPH_USE_NEON)
+	uint32x2_t xy = vcreate_u32(static_cast<uint64>(BitCast<uint32>(inX)) | (static_cast<uint64>(BitCast<uint32>(inY)) << 32));
+	uint32x2_t zz = vreinterpret_u32_f32(vdup_n_f32(inZ));
+	mValue = vreinterpretq_f32_u32(vcombine_u32(xy, zz));
+#else
+	mF32[0] = inX;
+	mF32[1] = inY;
+	mF32[2] = inZ;
+	#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+		mF32[3] = inZ;
+	#endif
+#endif
+}
+
+template<uint32 SwizzleX, uint32 SwizzleY, uint32 SwizzleZ>
+Vec3 Vec3::Swizzle() const
+{
+	static_assert(SwizzleX <= 3, "SwizzleX template parameter out of range");
+	static_assert(SwizzleY <= 3, "SwizzleY template parameter out of range");
+	static_assert(SwizzleZ <= 3, "SwizzleZ template parameter out of range");
+
+#if defined(JPH_USE_SSE)
+	return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(SwizzleZ, SwizzleZ, SwizzleY, SwizzleX)); // Assure Z and W are the same
+#elif defined(JPH_USE_NEON)
+	return JPH_NEON_SHUFFLE_F32x4(mValue, mValue, SwizzleX, SwizzleY, SwizzleZ, SwizzleZ);
+#else
+	return Vec3(mF32[SwizzleX], mF32[SwizzleY], mF32[SwizzleZ]);
+#endif
+}
+
+Vec3 Vec3::sZero()
+{
+#if defined(JPH_USE_SSE)
+	return _mm_setzero_ps();
+#elif defined(JPH_USE_NEON)
+	return vdupq_n_f32(0);
+#else
+	return Vec3(0, 0, 0);
+#endif
+}
+
+Vec3 Vec3::sReplicate(float inV)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_set1_ps(inV);
+#elif defined(JPH_USE_NEON)
+	return vdupq_n_f32(inV);
+#else
+	return Vec3(inV, inV, inV);
+#endif
+}
+
+Vec3 Vec3::sNaN()
+{
+	return sReplicate(numeric_limits<float>::quiet_NaN());
+}
+
+Vec3 Vec3::sLoadFloat3Unsafe(const Float3 &inV)
+{
+#if defined(JPH_USE_SSE)
+	Type v = _mm_loadu_ps(&inV.x);
+#elif defined(JPH_USE_NEON)
+	Type v = vld1q_f32(&inV.x);
+#else
+	Type v = { inV.x, inV.y, inV.z };
+#endif
+	return sFixW(v);
+}
+
+Vec3 Vec3::sMin(Vec3Arg inV1, Vec3Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_min_ps(inV1.mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vminq_f32(inV1.mValue, inV2.mValue);
+#else
+	return Vec3(min(inV1.mF32[0], inV2.mF32[0]),
+				min(inV1.mF32[1], inV2.mF32[1]),
+				min(inV1.mF32[2], inV2.mF32[2]));
+#endif
+}
+
+Vec3 Vec3::sMax(Vec3Arg inV1, Vec3Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_max_ps(inV1.mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vmaxq_f32(inV1.mValue, inV2.mValue);
+#else
+	return Vec3(max(inV1.mF32[0], inV2.mF32[0]),
+				max(inV1.mF32[1], inV2.mF32[1]),
+				max(inV1.mF32[2], inV2.mF32[2]));
+#endif
+}
+
+Vec3 Vec3::sClamp(Vec3Arg inV, Vec3Arg inMin, Vec3Arg inMax)
+{
+	return sMax(sMin(inV, inMax), inMin);
+}
+
+UVec4 Vec3::sEquals(Vec3Arg inV1, Vec3Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_castps_si128(_mm_cmpeq_ps(inV1.mValue, inV2.mValue));
+#elif defined(JPH_USE_NEON)
+	return vceqq_f32(inV1.mValue, inV2.mValue);
+#else
+	uint32 z = inV1.mF32[2] == inV2.mF32[2]? 0xffffffffu : 0;
+	return UVec4(inV1.mF32[0] == inV2.mF32[0]? 0xffffffffu : 0,
+				 inV1.mF32[1] == inV2.mF32[1]? 0xffffffffu : 0,
+				 z,
+				 z);
+#endif
+}
+
+UVec4 Vec3::sLess(Vec3Arg inV1, Vec3Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_castps_si128(_mm_cmplt_ps(inV1.mValue, inV2.mValue));
+#elif defined(JPH_USE_NEON)
+	return vcltq_f32(inV1.mValue, inV2.mValue);
+#else
+	uint32 z = inV1.mF32[2] < inV2.mF32[2]? 0xffffffffu : 0;
+	return UVec4(inV1.mF32[0] < inV2.mF32[0]? 0xffffffffu : 0,
+				 inV1.mF32[1] < inV2.mF32[1]? 0xffffffffu : 0,
+				 z,
+				 z);
+#endif
+}
+
+UVec4 Vec3::sLessOrEqual(Vec3Arg inV1, Vec3Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_castps_si128(_mm_cmple_ps(inV1.mValue, inV2.mValue));
+#elif defined(JPH_USE_NEON)
+	return vcleq_f32(inV1.mValue, inV2.mValue);
+#else
+	uint32 z = inV1.mF32[2] <= inV2.mF32[2]? 0xffffffffu : 0;
+	return UVec4(inV1.mF32[0] <= inV2.mF32[0]? 0xffffffffu : 0,
+				 inV1.mF32[1] <= inV2.mF32[1]? 0xffffffffu : 0,
+				 z,
+				 z);
+#endif
+}
+
+UVec4 Vec3::sGreater(Vec3Arg inV1, Vec3Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_castps_si128(_mm_cmpgt_ps(inV1.mValue, inV2.mValue));
+#elif defined(JPH_USE_NEON)
+	return vcgtq_f32(inV1.mValue, inV2.mValue);
+#else
+	uint32 z = inV1.mF32[2] > inV2.mF32[2]? 0xffffffffu : 0;
+	return UVec4(inV1.mF32[0] > inV2.mF32[0]? 0xffffffffu : 0,
+				 inV1.mF32[1] > inV2.mF32[1]? 0xffffffffu : 0,
+				 z,
+				 z);
+#endif
+}
+
+UVec4 Vec3::sGreaterOrEqual(Vec3Arg inV1, Vec3Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_castps_si128(_mm_cmpge_ps(inV1.mValue, inV2.mValue));
+#elif defined(JPH_USE_NEON)
+	return vcgeq_f32(inV1.mValue, inV2.mValue);
+#else
+	uint32 z = inV1.mF32[2] >= inV2.mF32[2]? 0xffffffffu : 0;
+	return UVec4(inV1.mF32[0] >= inV2.mF32[0]? 0xffffffffu : 0,
+				 inV1.mF32[1] >= inV2.mF32[1]? 0xffffffffu : 0,
+				 z,
+				 z);
+#endif
+}
+
+Vec3 Vec3::sFusedMultiplyAdd(Vec3Arg inMul1, Vec3Arg inMul2, Vec3Arg inAdd)
+{
+#if defined(JPH_USE_SSE)
+	#ifdef JPH_USE_FMADD
+		return _mm_fmadd_ps(inMul1.mValue, inMul2.mValue, inAdd.mValue);
+	#else
+		return _mm_add_ps(_mm_mul_ps(inMul1.mValue, inMul2.mValue), inAdd.mValue);
+	#endif
+#elif defined(JPH_USE_NEON)
+	return vmlaq_f32(inAdd.mValue, inMul1.mValue, inMul2.mValue);
+#else
+	return Vec3(inMul1.mF32[0] * inMul2.mF32[0] + inAdd.mF32[0],
+				inMul1.mF32[1] * inMul2.mF32[1] + inAdd.mF32[1],
+				inMul1.mF32[2] * inMul2.mF32[2] + inAdd.mF32[2]);
+#endif
+}
+
+Vec3 Vec3::sSelect(Vec3Arg inNotSet, Vec3Arg inSet, UVec4Arg inControl)
+{
+#if defined(JPH_USE_SSE4_1) && !defined(JPH_PLATFORM_WASM) // _mm_blendv_ps has problems on FireFox
+	Type v = _mm_blendv_ps(inNotSet.mValue, inSet.mValue, _mm_castsi128_ps(inControl.mValue));
+	return sFixW(v);
+#elif defined(JPH_USE_SSE)
+	__m128 is_set = _mm_castsi128_ps(_mm_srai_epi32(inControl.mValue, 31));
+	Type v = _mm_or_ps(_mm_and_ps(is_set, inSet.mValue), _mm_andnot_ps(is_set, inNotSet.mValue));
+	return sFixW(v);
+#elif defined(JPH_USE_NEON)
+	Type v = vbslq_f32(vreinterpretq_u32_s32(vshrq_n_s32(vreinterpretq_s32_u32(inControl.mValue), 31)), inSet.mValue, inNotSet.mValue);
+	return sFixW(v);
+#else
+	Vec3 result;
+	for (int i = 0; i < 3; i++)
+		result.mF32[i] = (inControl.mU32[i] & 0x80000000u) ? inSet.mF32[i] : inNotSet.mF32[i];
+#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+	result.mF32[3] = result.mF32[2];
+#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+	return result;
+#endif
+}
+
+Vec3 Vec3::sOr(Vec3Arg inV1, Vec3Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_or_ps(inV1.mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vreinterpretq_f32_u32(vorrq_u32(vreinterpretq_u32_f32(inV1.mValue), vreinterpretq_u32_f32(inV2.mValue)));
+#else
+	return Vec3(UVec4::sOr(inV1.ReinterpretAsInt(), inV2.ReinterpretAsInt()).ReinterpretAsFloat());
+#endif
+}
+
+Vec3 Vec3::sXor(Vec3Arg inV1, Vec3Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_xor_ps(inV1.mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vreinterpretq_f32_u32(veorq_u32(vreinterpretq_u32_f32(inV1.mValue), vreinterpretq_u32_f32(inV2.mValue)));
+#else
+	return Vec3(UVec4::sXor(inV1.ReinterpretAsInt(), inV2.ReinterpretAsInt()).ReinterpretAsFloat());
+#endif
+}
+
+Vec3 Vec3::sAnd(Vec3Arg inV1, Vec3Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_and_ps(inV1.mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vreinterpretq_f32_u32(vandq_u32(vreinterpretq_u32_f32(inV1.mValue), vreinterpretq_u32_f32(inV2.mValue)));
+#else
+	return Vec3(UVec4::sAnd(inV1.ReinterpretAsInt(), inV2.ReinterpretAsInt()).ReinterpretAsFloat());
+#endif
+}
+
+Vec3 Vec3::sUnitSpherical(float inTheta, float inPhi)
+{
+	Vec4 s, c;
+	Vec4(inTheta, inPhi, 0, 0).SinCos(s, c);
+	return Vec3(s.GetX() * c.GetY(), s.GetX() * s.GetY(), c.GetX());
+}
+
+template <class Random>
+Vec3 Vec3::sRandom(Random &inRandom)
+{
+	std::uniform_real_distribution<float> zero_to_one(0.0f, 1.0f);
+	float theta = JPH_PI * zero_to_one(inRandom);
+	float phi = 2.0f * JPH_PI * zero_to_one(inRandom);
+	return sUnitSpherical(theta, phi);
+}
+
+bool Vec3::operator == (Vec3Arg inV2) const
+{
+	return sEquals(*this, inV2).TestAllXYZTrue();
+}
+
+bool Vec3::IsClose(Vec3Arg inV2, float inMaxDistSq) const
+{
+	return (inV2 - *this).LengthSq() <= inMaxDistSq;
+}
+
+bool Vec3::IsNearZero(float inMaxDistSq) const
+{
+	return LengthSq() <= inMaxDistSq;
+}
+
+Vec3 Vec3::operator * (Vec3Arg inV2) const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_mul_ps(mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vmulq_f32(mValue, inV2.mValue);
+#else
+	return Vec3(mF32[0] * inV2.mF32[0], mF32[1] * inV2.mF32[1], mF32[2] * inV2.mF32[2]);
+#endif
+}
+
+Vec3 Vec3::operator * (float inV2) const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_mul_ps(mValue, _mm_set1_ps(inV2));
+#elif defined(JPH_USE_NEON)
+	return vmulq_n_f32(mValue, inV2);
+#else
+	return Vec3(mF32[0] * inV2, mF32[1] * inV2, mF32[2] * inV2);
+#endif
+}
+
+Vec3 operator * (float inV1, Vec3Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_mul_ps(_mm_set1_ps(inV1), inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vmulq_n_f32(inV2.mValue, inV1);
+#else
+	return Vec3(inV1 * inV2.mF32[0], inV1 * inV2.mF32[1], inV1 * inV2.mF32[2]);
+#endif
+}
+
+Vec3 Vec3::operator / (float inV2) const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_div_ps(mValue, _mm_set1_ps(inV2));
+#elif defined(JPH_USE_NEON)
+	return vdivq_f32(mValue, vdupq_n_f32(inV2));
+#else
+	return Vec3(mF32[0] / inV2, mF32[1] / inV2, mF32[2] / inV2);
+#endif
+}
+
+Vec3 &Vec3::operator *= (float inV2)
+{
+#if defined(JPH_USE_SSE)
+	mValue = _mm_mul_ps(mValue, _mm_set1_ps(inV2));
+#elif defined(JPH_USE_NEON)
+	mValue = vmulq_n_f32(mValue, inV2);
+#else
+	for (int i = 0; i < 3; ++i)
+		mF32[i] *= inV2;
+	#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+		mF32[3] = mF32[2];
+	#endif
+#endif
+	return *this;
+}
+
+Vec3 &Vec3::operator *= (Vec3Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	mValue = _mm_mul_ps(mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	mValue = vmulq_f32(mValue, inV2.mValue);
+#else
+	for (int i = 0; i < 3; ++i)
+		mF32[i] *= inV2.mF32[i];
+	#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+		mF32[3] = mF32[2];
+	#endif
+#endif
+	return *this;
+}
+
+Vec3 &Vec3::operator /= (float inV2)
+{
+#if defined(JPH_USE_SSE)
+	mValue = _mm_div_ps(mValue, _mm_set1_ps(inV2));
+#elif defined(JPH_USE_NEON)
+	mValue = vdivq_f32(mValue, vdupq_n_f32(inV2));
+#else
+	for (int i = 0; i < 3; ++i)
+		mF32[i] /= inV2;
+	#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+		mF32[3] = mF32[2];
+	#endif
+#endif
+	return *this;
+}
+
+Vec3 Vec3::operator + (Vec3Arg inV2) const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_add_ps(mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vaddq_f32(mValue, inV2.mValue);
+#else
+	return Vec3(mF32[0] + inV2.mF32[0], mF32[1] + inV2.mF32[1], mF32[2] + inV2.mF32[2]);
+#endif
+}
+
+Vec3 &Vec3::operator += (Vec3Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	mValue = _mm_add_ps(mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	mValue = vaddq_f32(mValue, inV2.mValue);
+#else
+	for (int i = 0; i < 3; ++i)
+		mF32[i] += inV2.mF32[i];
+	#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+		mF32[3] = mF32[2];
+	#endif
+#endif
+	return *this;
+}
+
+Vec3 Vec3::operator - () const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_sub_ps(_mm_setzero_ps(), mValue);
+#elif defined(JPH_USE_NEON)
+	#ifdef JPH_CROSS_PLATFORM_DETERMINISTIC
+		return vsubq_f32(vdupq_n_f32(0), mValue);
+	#else
+		return vnegq_f32(mValue);
+	#endif
+#else
+	#ifdef JPH_CROSS_PLATFORM_DETERMINISTIC
+		return Vec3(0.0f - mF32[0], 0.0f - mF32[1], 0.0f - mF32[2]);
+	#else
+		return Vec3(-mF32[0], -mF32[1], -mF32[2]);
+	#endif
+#endif
+}
+
+Vec3 Vec3::operator - (Vec3Arg inV2) const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_sub_ps(mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vsubq_f32(mValue, inV2.mValue);
+#else
+	return Vec3(mF32[0] - inV2.mF32[0], mF32[1] - inV2.mF32[1], mF32[2] - inV2.mF32[2]);
+#endif
+}
+
+Vec3 &Vec3::operator -= (Vec3Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	mValue = _mm_sub_ps(mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	mValue = vsubq_f32(mValue, inV2.mValue);
+#else
+	for (int i = 0; i < 3; ++i)
+		mF32[i] -= inV2.mF32[i];
+	#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+		mF32[3] = mF32[2];
+	#endif
+#endif
+	return *this;
+}
+
+Vec3 Vec3::operator / (Vec3Arg inV2) const
+{
+	inV2.CheckW(); // Check W equals Z to avoid div by zero
+#if defined(JPH_USE_SSE)
+	return _mm_div_ps(mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vdivq_f32(mValue, inV2.mValue);
+#else
+	return Vec3(mF32[0] / inV2.mF32[0], mF32[1] / inV2.mF32[1], mF32[2] / inV2.mF32[2]);
+#endif
+}
+
+Vec4 Vec3::SplatX() const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(0, 0, 0, 0));
+#elif defined(JPH_USE_NEON)
+	return vdupq_laneq_f32(mValue, 0);
+#else
+	return Vec4(mF32[0], mF32[0], mF32[0], mF32[0]);
+#endif
+}
+
+Vec4 Vec3::SplatY() const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(1, 1, 1, 1));
+#elif defined(JPH_USE_NEON)
+	return vdupq_laneq_f32(mValue, 1);
+#else
+	return Vec4(mF32[1], mF32[1], mF32[1], mF32[1]);
+#endif
+}
+
+Vec4 Vec3::SplatZ() const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(2, 2, 2, 2));
+#elif defined(JPH_USE_NEON)
+	return vdupq_laneq_f32(mValue, 2);
+#else
+	return Vec4(mF32[2], mF32[2], mF32[2], mF32[2]);
+#endif
+}
+
+int Vec3::GetLowestComponentIndex() const
+{
+	return GetX() < GetY() ? (GetZ() < GetX() ? 2 : 0) : (GetZ() < GetY() ? 2 : 1);
+}
+
+int Vec3::GetHighestComponentIndex() const
+{
+	return GetX() > GetY() ? (GetZ() > GetX() ? 2 : 0) : (GetZ() > GetY() ? 2 : 1);
+}
+
+Vec3 Vec3::Abs() const
+{
+#if defined(JPH_USE_AVX512)
+	return _mm_range_ps(mValue, mValue, 0b1000);
+#elif defined(JPH_USE_SSE)
+	return _mm_max_ps(_mm_sub_ps(_mm_setzero_ps(), mValue), mValue);
+#elif defined(JPH_USE_NEON)
+	return vabsq_f32(mValue);
+#else
+	return Vec3(abs(mF32[0]), abs(mF32[1]), abs(mF32[2]));
+#endif
+}
+
+Vec3 Vec3::Reciprocal() const
+{
+	return sReplicate(1.0f) / mValue;
+}
+
+Vec3 Vec3::Cross(Vec3Arg inV2) const
+{
+#if defined(JPH_USE_SSE)
+	Type t1 = _mm_shuffle_ps(inV2.mValue, inV2.mValue, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same
+	t1 = _mm_mul_ps(t1, mValue);
+	Type t2 = _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same
+	t2 = _mm_mul_ps(t2, inV2.mValue);
+	Type t3 = _mm_sub_ps(t1, t2);
+	return _mm_shuffle_ps(t3, t3, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same
+#elif defined(JPH_USE_NEON)
+	Type t1 = JPH_NEON_SHUFFLE_F32x4(inV2.mValue, inV2.mValue, 1, 2, 0, 0); // Assure Z and W are the same
+	t1 = vmulq_f32(t1, mValue);
+	Type t2 = JPH_NEON_SHUFFLE_F32x4(mValue, mValue, 1, 2, 0, 0); // Assure Z and W are the same
+	t2 = vmulq_f32(t2, inV2.mValue);
+	Type t3 = vsubq_f32(t1, t2);
+	return JPH_NEON_SHUFFLE_F32x4(t3, t3, 1, 2, 0, 0); // Assure Z and W are the same
+#else
+	return Vec3(mF32[1] * inV2.mF32[2] - mF32[2] * inV2.mF32[1],
+				mF32[2] * inV2.mF32[0] - mF32[0] * inV2.mF32[2],
+				mF32[0] * inV2.mF32[1] - mF32[1] * inV2.mF32[0]);
+#endif
+}
+
+Vec3 Vec3::DotV(Vec3Arg inV2) const
+{
+#if defined(JPH_USE_SSE4_1)
+	return _mm_dp_ps(mValue, inV2.mValue, 0x7f);
+#elif defined(JPH_USE_NEON)
+	float32x4_t mul = vmulq_f32(mValue, inV2.mValue);
+	mul = vsetq_lane_f32(0, mul, 3);
+	return vdupq_n_f32(vaddvq_f32(mul));
+#else
+	float dot = 0.0f;
+	for (int i = 0; i < 3; i++)
+		dot += mF32[i] * inV2.mF32[i];
+	return Vec3::sReplicate(dot);
+#endif
+}
+
+Vec4 Vec3::DotV4(Vec3Arg inV2) const
+{
+#if defined(JPH_USE_SSE4_1)
+	return _mm_dp_ps(mValue, inV2.mValue, 0x7f);
+#elif defined(JPH_USE_NEON)
+	float32x4_t mul = vmulq_f32(mValue, inV2.mValue);
+	mul = vsetq_lane_f32(0, mul, 3);
+	return vdupq_n_f32(vaddvq_f32(mul));
+#else
+	float dot = 0.0f;
+	for (int i = 0; i < 3; i++)
+		dot += mF32[i] * inV2.mF32[i];
+	return Vec4::sReplicate(dot);
+#endif
+}
+
+float Vec3::Dot(Vec3Arg inV2) const
+{
+#if defined(JPH_USE_SSE4_1)
+	return _mm_cvtss_f32(_mm_dp_ps(mValue, inV2.mValue, 0x7f));
+#elif defined(JPH_USE_NEON)
+	float32x4_t mul = vmulq_f32(mValue, inV2.mValue);
+	mul = vsetq_lane_f32(0, mul, 3);
+	return vaddvq_f32(mul);
+#else
+	float dot = 0.0f;
+	for (int i = 0; i < 3; i++)
+		dot += mF32[i] * inV2.mF32[i];
+	return dot;
+#endif
+}
+
+float Vec3::LengthSq() const
+{
+#if defined(JPH_USE_SSE4_1)
+	return _mm_cvtss_f32(_mm_dp_ps(mValue, mValue, 0x7f));
+#elif defined(JPH_USE_NEON)
+	float32x4_t mul = vmulq_f32(mValue, mValue);
+	mul = vsetq_lane_f32(0, mul, 3);
+	return vaddvq_f32(mul);
+#else
+	float len_sq = 0.0f;
+	for (int i = 0; i < 3; i++)
+		len_sq += mF32[i] * mF32[i];
+	return len_sq;
+#endif
+}
+
+float Vec3::Length() const
+{
+#if defined(JPH_USE_SSE4_1)
+	return _mm_cvtss_f32(_mm_sqrt_ss(_mm_dp_ps(mValue, mValue, 0x7f)));
+#elif defined(JPH_USE_NEON)
+	float32x4_t mul = vmulq_f32(mValue, mValue);
+	mul = vsetq_lane_f32(0, mul, 3);
+	float32x2_t sum = vdup_n_f32(vaddvq_f32(mul));
+	return vget_lane_f32(vsqrt_f32(sum), 0);
+#else
+	return sqrt(LengthSq());
+#endif
+}
+
+Vec3 Vec3::Sqrt() const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_sqrt_ps(mValue);
+#elif defined(JPH_USE_NEON)
+	return vsqrtq_f32(mValue);
+#else
+	return Vec3(sqrt(mF32[0]), sqrt(mF32[1]), sqrt(mF32[2]));
+#endif
+}
+
+Vec3 Vec3::Normalized() const
+{
+#if defined(JPH_USE_SSE4_1)
+	return _mm_div_ps(mValue, _mm_sqrt_ps(_mm_dp_ps(mValue, mValue, 0x7f)));
+#elif defined(JPH_USE_NEON)
+	float32x4_t mul = vmulq_f32(mValue, mValue);
+	mul = vsetq_lane_f32(0, mul, 3);
+	float32x4_t sum = vdupq_n_f32(vaddvq_f32(mul));
+	return vdivq_f32(mValue, vsqrtq_f32(sum));
+#else
+	return *this / Length();
+#endif
+}
+
+Vec3 Vec3::NormalizedOr(Vec3Arg inZeroValue) const
+{
+#if defined(JPH_USE_SSE4_1) && !defined(JPH_PLATFORM_WASM) // _mm_blendv_ps has problems on FireFox
+	Type len_sq = _mm_dp_ps(mValue, mValue, 0x7f);
+	// clang with '-ffast-math' (which you should not use!) can generate _mm_rsqrt_ps
+	// instructions which produce INFs/NaNs when they get a denormal float as input.
+	// We therefore treat denormals as zero here.
+	Type is_zero = _mm_cmple_ps(len_sq, _mm_set1_ps(FLT_MIN));
+#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+	if (_mm_movemask_ps(is_zero) == 0xf)
+		return inZeroValue;
+	else
+		return _mm_div_ps(mValue, _mm_sqrt_ps(len_sq));
+#else
+	return _mm_blendv_ps(_mm_div_ps(mValue, _mm_sqrt_ps(len_sq)), inZeroValue.mValue, is_zero);
+#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+#elif defined(JPH_USE_NEON)
+	float32x4_t mul = vmulq_f32(mValue, mValue);
+	mul = vsetq_lane_f32(0, mul, 3);
+	float32x4_t len_sq = vdupq_n_f32(vaddvq_f32(mul));
+	uint32x4_t is_zero = vcleq_f32(len_sq, vdupq_n_f32(FLT_MIN));
+	return vbslq_f32(is_zero, inZeroValue.mValue, vdivq_f32(mValue, vsqrtq_f32(len_sq)));
+#else
+	float len_sq = LengthSq();
+	if (len_sq <= FLT_MIN)
+		return inZeroValue;
+	else
+		return *this / sqrt(len_sq);
+#endif
+}
+
+bool Vec3::IsNormalized(float inTolerance) const
+{
+	return abs(LengthSq() - 1.0f) <= inTolerance;
+}
+
+bool Vec3::IsNaN() const
+{
+#if defined(JPH_USE_AVX512)
+	return (_mm_fpclass_ps_mask(mValue, 0b10000001) & 0x7) != 0;
+#elif defined(JPH_USE_SSE)
+	return (_mm_movemask_ps(_mm_cmpunord_ps(mValue, mValue)) & 0x7) != 0;
+#elif defined(JPH_USE_NEON)
+	uint32x4_t mask = JPH_NEON_UINT32x4(1, 1, 1, 0);
+	uint32x4_t is_equal = vceqq_f32(mValue, mValue); // If a number is not equal to itself it's a NaN
+	return vaddvq_u32(vandq_u32(is_equal, mask)) != 3;
+#else
+	return isnan(mF32[0]) || isnan(mF32[1]) || isnan(mF32[2]);
+#endif
+}
+
+void Vec3::StoreFloat3(Float3 *outV) const
+{
+#if defined(JPH_USE_SSE)
+	_mm_store_ss(&outV->x, mValue);
+	Vec3 t = Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_UNUSED>();
+	_mm_store_ss(&outV->y, t.mValue);
+	t = t.Swizzle<SWIZZLE_Y, SWIZZLE_UNUSED, SWIZZLE_UNUSED>();
+	_mm_store_ss(&outV->z, t.mValue);
+#elif defined(JPH_USE_NEON)
+	float32x2_t xy = vget_low_f32(mValue);
+	vst1_f32(&outV->x, xy);
+	vst1q_lane_f32(&outV->z, mValue, 2);
+#else
+	outV->x = mF32[0];
+	outV->y = mF32[1];
+	outV->z = mF32[2];
+#endif
+}
+
+UVec4 Vec3::ToInt() const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_cvttps_epi32(mValue);
+#elif defined(JPH_USE_NEON)
+	return vcvtq_u32_f32(mValue);
+#else
+	return UVec4(uint32(mF32[0]), uint32(mF32[1]), uint32(mF32[2]), uint32(mF32[3]));
+#endif
+}
+
+UVec4 Vec3::ReinterpretAsInt() const
+{
+#if defined(JPH_USE_SSE)
+	return UVec4(_mm_castps_si128(mValue));
+#elif defined(JPH_USE_NEON)
+	return vreinterpretq_u32_f32(mValue);
+#else
+	return *reinterpret_cast<const UVec4 *>(this);
+#endif
+}
+
+float Vec3::ReduceMin() const
+{
+	Vec3 v = sMin(mValue, Swizzle<SWIZZLE_Y, SWIZZLE_UNUSED, SWIZZLE_Z>());
+	v = sMin(v, v.Swizzle<SWIZZLE_Z, SWIZZLE_UNUSED, SWIZZLE_UNUSED>());
+	return v.GetX();
+}
+
+float Vec3::ReduceMax() const
+{
+	Vec3 v = sMax(mValue, Swizzle<SWIZZLE_Y, SWIZZLE_UNUSED, SWIZZLE_Z>());
+	v = sMax(v, v.Swizzle<SWIZZLE_Z, SWIZZLE_UNUSED, SWIZZLE_UNUSED>());
+	return v.GetX();
+}
+
+Vec3 Vec3::GetNormalizedPerpendicular() const
+{
+	if (abs(mF32[0]) > abs(mF32[1]))
+	{
+		float len = sqrt(mF32[0] * mF32[0] + mF32[2] * mF32[2]);
+		return Vec3(mF32[2], 0.0f, -mF32[0]) / len;
+	}
+	else
+	{
+		float len = sqrt(mF32[1] * mF32[1] + mF32[2] * mF32[2]);
+		return Vec3(0.0f, mF32[2], -mF32[1]) / len;
+	}
+}
+
+Vec3 Vec3::GetSign() const
+{
+#if defined(JPH_USE_AVX512)
+	return _mm_fixupimm_ps(mValue, mValue, _mm_set1_epi32(0xA9A90A00), 0);
+#elif defined(JPH_USE_SSE)
+	Type minus_one = _mm_set1_ps(-1.0f);
+	Type one = _mm_set1_ps(1.0f);
+	return _mm_or_ps(_mm_and_ps(mValue, minus_one), one);
+#elif defined(JPH_USE_NEON)
+	Type minus_one = vdupq_n_f32(-1.0f);
+	Type one = vdupq_n_f32(1.0f);
+	return vreinterpretq_f32_u32(vorrq_u32(vandq_u32(vreinterpretq_u32_f32(mValue), vreinterpretq_u32_f32(minus_one)), vreinterpretq_u32_f32(one)));
+#else
+	return Vec3(std::signbit(mF32[0])? -1.0f : 1.0f,
+				std::signbit(mF32[1])? -1.0f : 1.0f,
+				std::signbit(mF32[2])? -1.0f : 1.0f);
+#endif
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Math/Vec4.h b/thirdparty/jolt_physics/Jolt/Math/Vec4.h
new file mode 100644
index 0000000000..eb924a0f67
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/Vec4.h
@@ -0,0 +1,283 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Math/Float4.h>
+#include <Jolt/Math/Swizzle.h>
+#include <Jolt/Math/MathTypes.h>
+
+JPH_NAMESPACE_BEGIN
+
+class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) Vec4
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	// Underlying vector type
+#if defined(JPH_USE_SSE)
+	using Type = __m128;
+#elif defined(JPH_USE_NEON)
+	using Type = float32x4_t;
+#else
+	using Type = struct { float mData[4]; };
+#endif
+
+	/// Constructor
+								Vec4() = default; ///< Intentionally not initialized for performance reasons
+								Vec4(const Vec4 &inRHS) = default;
+	Vec4 &						operator = (const Vec4 &inRHS) = default;
+	explicit JPH_INLINE			Vec4(Vec3Arg inRHS);							///< WARNING: W component undefined!
+	JPH_INLINE					Vec4(Vec3Arg inRHS, float inW);
+	JPH_INLINE					Vec4(Type inRHS) : mValue(inRHS)				{ }
+
+	/// Create a vector from 4 components
+	JPH_INLINE					Vec4(float inX, float inY, float inZ, float inW);
+
+	/// Vector with all zeros
+	static JPH_INLINE Vec4		sZero();
+
+	/// Vector with all NaN's
+	static JPH_INLINE Vec4		sNaN();
+
+	/// Replicate inV across all components
+	static JPH_INLINE Vec4		sReplicate(float inV);
+
+	/// Load 4 floats from memory
+	static JPH_INLINE Vec4		sLoadFloat4(const Float4 *inV);
+
+	/// Load 4 floats from memory, 16 bytes aligned
+	static JPH_INLINE Vec4		sLoadFloat4Aligned(const Float4 *inV);
+
+	/// Gather 4 floats from memory at inBase + inOffsets[i] * Scale
+	template <const int Scale>
+	static JPH_INLINE Vec4		sGatherFloat4(const float *inBase, UVec4Arg inOffsets);
+
+	/// Return the minimum value of each of the components
+	static JPH_INLINE Vec4		sMin(Vec4Arg inV1, Vec4Arg inV2);
+
+	/// Return the maximum of each of the components
+	static JPH_INLINE Vec4		sMax(Vec4Arg inV1, Vec4Arg inV2);
+
+	/// Equals (component wise)
+	static JPH_INLINE UVec4		sEquals(Vec4Arg inV1, Vec4Arg inV2);
+
+	/// Less than (component wise)
+	static JPH_INLINE UVec4		sLess(Vec4Arg inV1, Vec4Arg inV2);
+
+	/// Less than or equal (component wise)
+	static JPH_INLINE UVec4		sLessOrEqual(Vec4Arg inV1, Vec4Arg inV2);
+
+	/// Greater than (component wise)
+	static JPH_INLINE UVec4		sGreater(Vec4Arg inV1, Vec4Arg inV2);
+
+	/// Greater than or equal (component wise)
+	static JPH_INLINE UVec4		sGreaterOrEqual(Vec4Arg inV1, Vec4Arg inV2);
+
+	/// Calculates inMul1 * inMul2 + inAdd
+	static JPH_INLINE Vec4		sFusedMultiplyAdd(Vec4Arg inMul1, Vec4Arg inMul2, Vec4Arg inAdd);
+
+	/// Component wise select, returns inNotSet when highest bit of inControl = 0 and inSet when highest bit of inControl = 1
+	static JPH_INLINE Vec4		sSelect(Vec4Arg inNotSet, Vec4Arg inSet, UVec4Arg inControl);
+
+	/// Logical or (component wise)
+	static JPH_INLINE Vec4		sOr(Vec4Arg inV1, Vec4Arg inV2);
+
+	/// Logical xor (component wise)
+	static JPH_INLINE Vec4		sXor(Vec4Arg inV1, Vec4Arg inV2);
+
+	/// Logical and (component wise)
+	static JPH_INLINE Vec4		sAnd(Vec4Arg inV1, Vec4Arg inV2);
+
+	/// Sort the four elements of ioValue and sort ioIndex at the same time.
+	/// Based on a sorting network: http://en.wikipedia.org/wiki/Sorting_network
+	static JPH_INLINE void		sSort4(Vec4 &ioValue, UVec4 &ioIndex);
+
+	/// Reverse sort the four elements of ioValue (highest first) and sort ioIndex at the same time.
+	/// Based on a sorting network: http://en.wikipedia.org/wiki/Sorting_network
+	static JPH_INLINE void		sSort4Reverse(Vec4 &ioValue, UVec4 &ioIndex);
+
+	/// Get individual components
+#if defined(JPH_USE_SSE)
+	JPH_INLINE float			GetX() const									{ return _mm_cvtss_f32(mValue); }
+	JPH_INLINE float			GetY() const									{ return mF32[1]; }
+	JPH_INLINE float			GetZ() const									{ return mF32[2]; }
+	JPH_INLINE float			GetW() const									{ return mF32[3]; }
+#elif defined(JPH_USE_NEON)
+	JPH_INLINE float			GetX() const									{ return vgetq_lane_f32(mValue, 0); }
+	JPH_INLINE float			GetY() const									{ return vgetq_lane_f32(mValue, 1); }
+	JPH_INLINE float			GetZ() const									{ return vgetq_lane_f32(mValue, 2); }
+	JPH_INLINE float			GetW() const									{ return vgetq_lane_f32(mValue, 3); }
+#else
+	JPH_INLINE float			GetX() const									{ return mF32[0]; }
+	JPH_INLINE float			GetY() const									{ return mF32[1]; }
+	JPH_INLINE float			GetZ() const									{ return mF32[2]; }
+	JPH_INLINE float			GetW() const									{ return mF32[3]; }
+#endif
+
+	/// Set individual components
+	JPH_INLINE void				SetX(float inX)									{ mF32[0] = inX; }
+	JPH_INLINE void				SetY(float inY)									{ mF32[1] = inY; }
+	JPH_INLINE void				SetZ(float inZ)									{ mF32[2] = inZ; }
+	JPH_INLINE void				SetW(float inW)									{ mF32[3] = inW; }
+
+	/// Set all components
+	JPH_INLINE void				Set(float inX, float inY, float inZ, float inW)	{ *this = Vec4(inX, inY, inZ, inW); }
+
+	/// Get float component by index
+	JPH_INLINE float			operator [] (uint inCoordinate) const			{ JPH_ASSERT(inCoordinate < 4); return mF32[inCoordinate]; }
+	JPH_INLINE float &			operator [] (uint inCoordinate)					{ JPH_ASSERT(inCoordinate < 4); return mF32[inCoordinate]; }
+
+	/// Comparison
+	JPH_INLINE bool				operator == (Vec4Arg inV2) const;
+	JPH_INLINE bool				operator != (Vec4Arg inV2) const			{ return !(*this == inV2); }
+
+	/// Test if two vectors are close
+	JPH_INLINE bool				IsClose(Vec4Arg inV2, float inMaxDistSq = 1.0e-12f) const;
+
+	/// Test if vector is normalized
+	JPH_INLINE bool				IsNormalized(float inTolerance = 1.0e-6f) const;
+
+	/// Test if vector contains NaN elements
+	JPH_INLINE bool				IsNaN() const;
+
+	/// Multiply two float vectors (component wise)
+	JPH_INLINE Vec4				operator * (Vec4Arg inV2) const;
+
+	/// Multiply vector with float
+	JPH_INLINE Vec4				operator * (float inV2) const;
+
+	/// Multiply vector with float
+	friend JPH_INLINE Vec4		operator * (float inV1, Vec4Arg inV2);
+
+	/// Divide vector by float
+	JPH_INLINE Vec4				operator / (float inV2) const;
+
+	/// Multiply vector with float
+	JPH_INLINE Vec4 &			operator *= (float inV2);
+
+	/// Multiply vector with vector
+	JPH_INLINE Vec4 &			operator *= (Vec4Arg inV2);
+
+	/// Divide vector by float
+	JPH_INLINE Vec4 &			operator /= (float inV2);
+
+	/// Add two float vectors (component wise)
+	JPH_INLINE Vec4				operator + (Vec4Arg inV2) const;
+
+	/// Add two float vectors (component wise)
+	JPH_INLINE Vec4 &			operator += (Vec4Arg inV2);
+
+	/// Negate
+	JPH_INLINE Vec4				operator - () const;
+
+	/// Subtract two float vectors (component wise)
+	JPH_INLINE Vec4				operator - (Vec4Arg inV2) const;
+
+	/// Subtract two float vectors (component wise)
+	JPH_INLINE Vec4 &			operator -= (Vec4Arg inV2);
+
+	/// Divide (component wise)
+	JPH_INLINE Vec4				operator / (Vec4Arg inV2) const;
+
+	/// Swizzle the elements in inV
+	template<uint32 SwizzleX, uint32 SwizzleY, uint32 SwizzleZ, uint32 SwizzleW>
+	JPH_INLINE Vec4				Swizzle() const;
+
+	/// Replicate the X component to all components
+	JPH_INLINE Vec4				SplatX() const;
+
+	/// Replicate the Y component to all components
+	JPH_INLINE Vec4				SplatY() const;
+
+	/// Replicate the Z component to all components
+	JPH_INLINE Vec4				SplatZ() const;
+
+	/// Replicate the W component to all components
+	JPH_INLINE Vec4				SplatW() const;
+
+	/// Return the absolute value of each of the components
+	JPH_INLINE Vec4				Abs() const;
+
+	/// Reciprocal vector (1 / value) for each of the components
+	JPH_INLINE Vec4				Reciprocal() const;
+
+	/// Dot product, returns the dot product in X, Y and Z components
+	JPH_INLINE Vec4				DotV(Vec4Arg inV2) const;
+
+	/// Dot product
+	JPH_INLINE float			Dot(Vec4Arg inV2) const;
+
+	/// Squared length of vector
+	JPH_INLINE float			LengthSq() const;
+
+	/// Length of vector
+	JPH_INLINE float			Length() const;
+
+	/// Normalize vector
+	JPH_INLINE Vec4				Normalized() const;
+
+	/// Store 4 floats to memory
+	JPH_INLINE void				StoreFloat4(Float4 *outV) const;
+
+	/// Convert each component from a float to an int
+	JPH_INLINE UVec4			ToInt() const;
+
+	/// Reinterpret Vec4 as a UVec4 (doesn't change the bits)
+	JPH_INLINE UVec4			ReinterpretAsInt() const;
+
+	/// Store if X is negative in bit 0, Y in bit 1, Z in bit 2 and W in bit 3
+	JPH_INLINE int				GetSignBits() const;
+
+	/// Get the minimum of X, Y, Z and W
+	JPH_INLINE float			ReduceMin() const;
+
+	/// Get the maximum of X, Y, Z and W
+	JPH_INLINE float			ReduceMax() const;
+
+	/// Component wise square root
+	JPH_INLINE Vec4				Sqrt() const;
+
+	/// Get vector that contains the sign of each element (returns 1.0f if positive, -1.0f if negative)
+	JPH_INLINE Vec4				GetSign() const;
+
+	/// Calculate the sine and cosine for each element of this vector (input in radians)
+	inline void					SinCos(Vec4 &outSin, Vec4 &outCos) const;
+
+	/// Calculate the tangent for each element of this vector (input in radians)
+	inline Vec4					Tan() const;
+
+	/// Calculate the arc sine for each element of this vector (returns value in the range [-PI / 2, PI / 2])
+	/// Note that all input values will be clamped to the range [-1, 1] and this function will not return NaNs like std::asin
+	inline Vec4					ASin() const;
+
+	/// Calculate the arc cosine for each element of this vector (returns value in the range [0, PI])
+	/// Note that all input values will be clamped to the range [-1, 1] and this function will not return NaNs like std::acos
+	inline Vec4					ACos() const;
+
+	/// Calculate the arc tangent for each element of this vector (returns value in the range [-PI / 2, PI / 2])
+	inline Vec4					ATan() const;
+
+	/// Calculate the arc tangent of y / x using the signs of the arguments to determine the correct quadrant (returns value in the range [-PI, PI])
+	inline static Vec4			sATan2(Vec4Arg inY, Vec4Arg inX);
+
+	/// To String
+	friend ostream &			operator << (ostream &inStream, Vec4Arg inV)
+	{
+		inStream << inV.mF32[0] << ", " << inV.mF32[1] << ", " << inV.mF32[2] << ", " << inV.mF32[3];
+		return inStream;
+	}
+
+	union
+	{
+		Type					mValue;
+		float					mF32[4];
+	};
+};
+
+static_assert(std::is_trivial<Vec4>(), "Is supposed to be a trivial type!");
+
+JPH_NAMESPACE_END
+
+#include "Vec4.inl"
diff --git a/thirdparty/jolt_physics/Jolt/Math/Vec4.inl b/thirdparty/jolt_physics/Jolt/Math/Vec4.inl
new file mode 100644
index 0000000000..43676361c5
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/Vec4.inl
@@ -0,0 +1,981 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Math/Trigonometry.h>
+#include <Jolt/Math/Vec3.h>
+#include <Jolt/Math/UVec4.h>
+
+JPH_NAMESPACE_BEGIN
+
+// Constructor
+Vec4::Vec4(Vec3Arg inRHS) :
+	mValue(inRHS.mValue)
+{
+}
+
+Vec4::Vec4(Vec3Arg inRHS, float inW)
+{
+#if defined(JPH_USE_SSE4_1)
+	mValue = _mm_blend_ps(inRHS.mValue, _mm_set1_ps(inW), 8);
+#elif defined(JPH_USE_NEON)
+	mValue = vsetq_lane_f32(inW, inRHS.mValue, 3);
+#else
+	for (int i = 0; i < 3; i++)
+		mF32[i] = inRHS.mF32[i];
+	mF32[3] = inW;
+#endif
+}
+
+Vec4::Vec4(float inX, float inY, float inZ, float inW)
+{
+#if defined(JPH_USE_SSE)
+	mValue = _mm_set_ps(inW, inZ, inY, inX);
+#elif defined(JPH_USE_NEON)
+	uint32x2_t xy = vcreate_u32(static_cast<uint64>(BitCast<uint32>(inX)) | (static_cast<uint64>(BitCast<uint32>(inY)) << 32));
+	uint32x2_t zw = vcreate_u32(static_cast<uint64>(BitCast<uint32>(inZ)) | (static_cast<uint64>(BitCast<uint32>(inW)) << 32));
+	mValue = vreinterpretq_f32_u32(vcombine_u32(xy, zw));
+#else
+	mF32[0] = inX;
+	mF32[1] = inY;
+	mF32[2] = inZ;
+	mF32[3] = inW;
+#endif
+}
+
+template<uint32 SwizzleX, uint32 SwizzleY, uint32 SwizzleZ, uint32 SwizzleW>
+Vec4 Vec4::Swizzle() const
+{
+	static_assert(SwizzleX <= 3, "SwizzleX template parameter out of range");
+	static_assert(SwizzleY <= 3, "SwizzleY template parameter out of range");
+	static_assert(SwizzleZ <= 3, "SwizzleZ template parameter out of range");
+	static_assert(SwizzleW <= 3, "SwizzleW template parameter out of range");
+
+#if defined(JPH_USE_SSE)
+	return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(SwizzleW, SwizzleZ, SwizzleY, SwizzleX));
+#elif defined(JPH_USE_NEON)
+	return JPH_NEON_SHUFFLE_F32x4(mValue, mValue, SwizzleX, SwizzleY, SwizzleZ, SwizzleW);
+#else
+	return Vec4(mF32[SwizzleX], mF32[SwizzleY], mF32[SwizzleZ], mF32[SwizzleW]);
+#endif
+}
+
+Vec4 Vec4::sZero()
+{
+#if defined(JPH_USE_SSE)
+	return _mm_setzero_ps();
+#elif defined(JPH_USE_NEON)
+	return vdupq_n_f32(0);
+#else
+	return Vec4(0, 0, 0, 0);
+#endif
+}
+
+Vec4 Vec4::sReplicate(float inV)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_set1_ps(inV);
+#elif defined(JPH_USE_NEON)
+	return vdupq_n_f32(inV);
+#else
+	return Vec4(inV, inV, inV, inV);
+#endif
+}
+
+Vec4 Vec4::sNaN()
+{
+	return sReplicate(numeric_limits<float>::quiet_NaN());
+}
+
+Vec4 Vec4::sLoadFloat4(const Float4 *inV)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_loadu_ps(&inV->x);
+#elif defined(JPH_USE_NEON)
+	return vld1q_f32(&inV->x);
+#else
+	return Vec4(inV->x, inV->y, inV->z, inV->w);
+#endif
+}
+
+Vec4 Vec4::sLoadFloat4Aligned(const Float4 *inV)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_load_ps(&inV->x);
+#elif defined(JPH_USE_NEON)
+	return vld1q_f32(&inV->x);
+#else
+	return Vec4(inV->x, inV->y, inV->z, inV->w);
+#endif
+}
+
+template <const int Scale>
+Vec4 Vec4::sGatherFloat4(const float *inBase, UVec4Arg inOffsets)
+{
+#if defined(JPH_USE_SSE)
+	#ifdef JPH_USE_AVX2
+		return _mm_i32gather_ps(inBase, inOffsets.mValue, Scale);
+	#else
+		const uint8 *base = reinterpret_cast<const uint8 *>(inBase);
+		Type x = _mm_load_ss(reinterpret_cast<const float *>(base + inOffsets.GetX() * Scale));
+		Type y = _mm_load_ss(reinterpret_cast<const float *>(base + inOffsets.GetY() * Scale));
+		Type xy = _mm_unpacklo_ps(x, y);
+		Type z = _mm_load_ss(reinterpret_cast<const float *>(base + inOffsets.GetZ() * Scale));
+		Type w = _mm_load_ss(reinterpret_cast<const float *>(base + inOffsets.GetW() * Scale));
+		Type zw = _mm_unpacklo_ps(z, w);
+		return _mm_movelh_ps(xy, zw);
+	#endif
+#else
+	const uint8 *base = reinterpret_cast<const uint8 *>(inBase);
+	float x = *reinterpret_cast<const float *>(base + inOffsets.GetX() * Scale);
+	float y = *reinterpret_cast<const float *>(base + inOffsets.GetY() * Scale);
+	float z = *reinterpret_cast<const float *>(base + inOffsets.GetZ() * Scale);
+	float w = *reinterpret_cast<const float *>(base + inOffsets.GetW() * Scale);
+	return Vec4(x, y, z, w);
+#endif
+}
+
+Vec4 Vec4::sMin(Vec4Arg inV1, Vec4Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_min_ps(inV1.mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vminq_f32(inV1.mValue, inV2.mValue);
+#else
+	return Vec4(min(inV1.mF32[0], inV2.mF32[0]),
+				min(inV1.mF32[1], inV2.mF32[1]),
+				min(inV1.mF32[2], inV2.mF32[2]),
+				min(inV1.mF32[3], inV2.mF32[3]));
+#endif
+}
+
+Vec4 Vec4::sMax(Vec4Arg inV1, Vec4Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_max_ps(inV1.mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vmaxq_f32(inV1.mValue, inV2.mValue);
+#else
+	return Vec4(max(inV1.mF32[0], inV2.mF32[0]),
+				max(inV1.mF32[1], inV2.mF32[1]),
+				max(inV1.mF32[2], inV2.mF32[2]),
+				max(inV1.mF32[3], inV2.mF32[3]));
+#endif
+}
+
+UVec4 Vec4::sEquals(Vec4Arg inV1, Vec4Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_castps_si128(_mm_cmpeq_ps(inV1.mValue, inV2.mValue));
+#elif defined(JPH_USE_NEON)
+	return vceqq_f32(inV1.mValue, inV2.mValue);
+#else
+	return UVec4(inV1.mF32[0] == inV2.mF32[0]? 0xffffffffu : 0,
+				 inV1.mF32[1] == inV2.mF32[1]? 0xffffffffu : 0,
+				 inV1.mF32[2] == inV2.mF32[2]? 0xffffffffu : 0,
+				 inV1.mF32[3] == inV2.mF32[3]? 0xffffffffu : 0);
+#endif
+}
+
+UVec4 Vec4::sLess(Vec4Arg inV1, Vec4Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_castps_si128(_mm_cmplt_ps(inV1.mValue, inV2.mValue));
+#elif defined(JPH_USE_NEON)
+	return vcltq_f32(inV1.mValue, inV2.mValue);
+#else
+	return UVec4(inV1.mF32[0] < inV2.mF32[0]? 0xffffffffu : 0,
+				 inV1.mF32[1] < inV2.mF32[1]? 0xffffffffu : 0,
+				 inV1.mF32[2] < inV2.mF32[2]? 0xffffffffu : 0,
+				 inV1.mF32[3] < inV2.mF32[3]? 0xffffffffu : 0);
+#endif
+}
+
+UVec4 Vec4::sLessOrEqual(Vec4Arg inV1, Vec4Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_castps_si128(_mm_cmple_ps(inV1.mValue, inV2.mValue));
+#elif defined(JPH_USE_NEON)
+	return vcleq_f32(inV1.mValue, inV2.mValue);
+#else
+	return UVec4(inV1.mF32[0] <= inV2.mF32[0]? 0xffffffffu : 0,
+				 inV1.mF32[1] <= inV2.mF32[1]? 0xffffffffu : 0,
+				 inV1.mF32[2] <= inV2.mF32[2]? 0xffffffffu : 0,
+				 inV1.mF32[3] <= inV2.mF32[3]? 0xffffffffu : 0);
+#endif
+}
+
+UVec4 Vec4::sGreater(Vec4Arg inV1, Vec4Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_castps_si128(_mm_cmpgt_ps(inV1.mValue, inV2.mValue));
+#elif defined(JPH_USE_NEON)
+	return vcgtq_f32(inV1.mValue, inV2.mValue);
+#else
+	return UVec4(inV1.mF32[0] > inV2.mF32[0]? 0xffffffffu : 0,
+				 inV1.mF32[1] > inV2.mF32[1]? 0xffffffffu : 0,
+				 inV1.mF32[2] > inV2.mF32[2]? 0xffffffffu : 0,
+				 inV1.mF32[3] > inV2.mF32[3]? 0xffffffffu : 0);
+#endif
+}
+
+UVec4 Vec4::sGreaterOrEqual(Vec4Arg inV1, Vec4Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_castps_si128(_mm_cmpge_ps(inV1.mValue, inV2.mValue));
+#elif defined(JPH_USE_NEON)
+	return vcgeq_f32(inV1.mValue, inV2.mValue);
+#else
+	return UVec4(inV1.mF32[0] >= inV2.mF32[0]? 0xffffffffu : 0,
+				 inV1.mF32[1] >= inV2.mF32[1]? 0xffffffffu : 0,
+				 inV1.mF32[2] >= inV2.mF32[2]? 0xffffffffu : 0,
+				 inV1.mF32[3] >= inV2.mF32[3]? 0xffffffffu : 0);
+#endif
+}
+
+Vec4 Vec4::sFusedMultiplyAdd(Vec4Arg inMul1, Vec4Arg inMul2, Vec4Arg inAdd)
+{
+#if defined(JPH_USE_SSE)
+	#ifdef JPH_USE_FMADD
+		return _mm_fmadd_ps(inMul1.mValue, inMul2.mValue, inAdd.mValue);
+	#else
+		return _mm_add_ps(_mm_mul_ps(inMul1.mValue, inMul2.mValue), inAdd.mValue);
+	#endif
+#elif defined(JPH_USE_NEON)
+	return vmlaq_f32(inAdd.mValue, inMul1.mValue, inMul2.mValue);
+#else
+	return Vec4(inMul1.mF32[0] * inMul2.mF32[0] + inAdd.mF32[0],
+				inMul1.mF32[1] * inMul2.mF32[1] + inAdd.mF32[1],
+				inMul1.mF32[2] * inMul2.mF32[2] + inAdd.mF32[2],
+				inMul1.mF32[3] * inMul2.mF32[3] + inAdd.mF32[3]);
+#endif
+}
+
+Vec4 Vec4::sSelect(Vec4Arg inNotSet, Vec4Arg inSet, UVec4Arg inControl)
+{
+#if defined(JPH_USE_SSE4_1) && !defined(JPH_PLATFORM_WASM) // _mm_blendv_ps has problems on FireFox
+	return _mm_blendv_ps(inNotSet.mValue, inSet.mValue, _mm_castsi128_ps(inControl.mValue));
+#elif defined(JPH_USE_SSE)
+	__m128 is_set = _mm_castsi128_ps(_mm_srai_epi32(inControl.mValue, 31));
+	return _mm_or_ps(_mm_and_ps(is_set, inSet.mValue), _mm_andnot_ps(is_set, inNotSet.mValue));
+#elif defined(JPH_USE_NEON)
+	return vbslq_f32(vreinterpretq_u32_s32(vshrq_n_s32(vreinterpretq_s32_u32(inControl.mValue), 31)), inSet.mValue, inNotSet.mValue);
+#else
+	Vec4 result;
+	for (int i = 0; i < 4; i++)
+		result.mF32[i] = (inControl.mU32[i] & 0x80000000u) ? inSet.mF32[i] : inNotSet.mF32[i];
+	return result;
+#endif
+}
+
+Vec4 Vec4::sOr(Vec4Arg inV1, Vec4Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_or_ps(inV1.mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vreinterpretq_f32_u32(vorrq_u32(vreinterpretq_u32_f32(inV1.mValue), vreinterpretq_u32_f32(inV2.mValue)));
+#else
+	return UVec4::sOr(inV1.ReinterpretAsInt(), inV2.ReinterpretAsInt()).ReinterpretAsFloat();
+#endif
+}
+
+Vec4 Vec4::sXor(Vec4Arg inV1, Vec4Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_xor_ps(inV1.mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vreinterpretq_f32_u32(veorq_u32(vreinterpretq_u32_f32(inV1.mValue), vreinterpretq_u32_f32(inV2.mValue)));
+#else
+	return UVec4::sXor(inV1.ReinterpretAsInt(), inV2.ReinterpretAsInt()).ReinterpretAsFloat();
+#endif
+}
+
+Vec4 Vec4::sAnd(Vec4Arg inV1, Vec4Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_and_ps(inV1.mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vreinterpretq_f32_u32(vandq_u32(vreinterpretq_u32_f32(inV1.mValue), vreinterpretq_u32_f32(inV2.mValue)));
+#else
+	return UVec4::sAnd(inV1.ReinterpretAsInt(), inV2.ReinterpretAsInt()).ReinterpretAsFloat();
+#endif
+}
+
+void Vec4::sSort4(Vec4 &ioValue, UVec4 &ioIndex)
+{
+	// Pass 1, test 1st vs 3rd, 2nd vs 4th
+	Vec4 v1 = ioValue.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y>();
+	UVec4 i1 = ioIndex.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y>();
+	UVec4 c1 = sLess(ioValue, v1).Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_W>();
+	ioValue = sSelect(ioValue, v1, c1);
+	ioIndex = UVec4::sSelect(ioIndex, i1, c1);
+
+	// Pass 2, test 1st vs 2nd, 3rd vs 4th
+	Vec4 v2 = ioValue.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>();
+	UVec4 i2 = ioIndex.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>();
+	UVec4 c2 = sLess(ioValue, v2).Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_W>();
+	ioValue = sSelect(ioValue, v2, c2);
+	ioIndex = UVec4::sSelect(ioIndex, i2, c2);
+
+	// Pass 3, test 2nd vs 3rd component
+	Vec4 v3 = ioValue.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_W>();
+	UVec4 i3 = ioIndex.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_W>();
+	UVec4 c3 = sLess(ioValue, v3).Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_W>();
+	ioValue = sSelect(ioValue, v3, c3);
+	ioIndex = UVec4::sSelect(ioIndex, i3, c3);
+}
+
+void Vec4::sSort4Reverse(Vec4 &ioValue, UVec4 &ioIndex)
+{
+	// Pass 1, test 1st vs 3rd, 2nd vs 4th
+	Vec4 v1 = ioValue.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y>();
+	UVec4 i1 = ioIndex.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y>();
+	UVec4 c1 = sGreater(ioValue, v1).Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_Z, SWIZZLE_W>();
+	ioValue = sSelect(ioValue, v1, c1);
+	ioIndex = UVec4::sSelect(ioIndex, i1, c1);
+
+	// Pass 2, test 1st vs 2nd, 3rd vs 4th
+	Vec4 v2 = ioValue.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>();
+	UVec4 i2 = ioIndex.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>();
+	UVec4 c2 = sGreater(ioValue, v2).Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_W>();
+	ioValue = sSelect(ioValue, v2, c2);
+	ioIndex = UVec4::sSelect(ioIndex, i2, c2);
+
+	// Pass 3, test 2nd vs 3rd component
+	Vec4 v3 = ioValue.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_W>();
+	UVec4 i3 = ioIndex.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_W>();
+	UVec4 c3 = sGreater(ioValue, v3).Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_W>();
+	ioValue = sSelect(ioValue, v3, c3);
+	ioIndex = UVec4::sSelect(ioIndex, i3, c3);
+}
+
+bool Vec4::operator == (Vec4Arg inV2) const
+{
+	return sEquals(*this, inV2).TestAllTrue();
+}
+
+bool Vec4::IsClose(Vec4Arg inV2, float inMaxDistSq) const
+{
+	return (inV2 - *this).LengthSq() <= inMaxDistSq;
+}
+
+bool Vec4::IsNormalized(float inTolerance) const
+{
+	return abs(LengthSq() - 1.0f) <= inTolerance;
+}
+
+bool Vec4::IsNaN() const
+{
+#if defined(JPH_USE_AVX512)
+	return _mm_fpclass_ps_mask(mValue, 0b10000001) != 0;
+#elif defined(JPH_USE_SSE)
+	return _mm_movemask_ps(_mm_cmpunord_ps(mValue, mValue)) != 0;
+#elif defined(JPH_USE_NEON)
+	uint32x4_t is_equal = vceqq_f32(mValue, mValue); // If a number is not equal to itself it's a NaN
+	return vaddvq_u32(vshrq_n_u32(is_equal, 31)) != 4;
+#else
+	return isnan(mF32[0]) || isnan(mF32[1]) || isnan(mF32[2]) || isnan(mF32[3]);
+#endif
+}
+
+Vec4 Vec4::operator * (Vec4Arg inV2) const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_mul_ps(mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vmulq_f32(mValue, inV2.mValue);
+#else
+	return Vec4(mF32[0] * inV2.mF32[0],
+				mF32[1] * inV2.mF32[1],
+				mF32[2] * inV2.mF32[2],
+				mF32[3] * inV2.mF32[3]);
+#endif
+}
+
+Vec4 Vec4::operator * (float inV2) const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_mul_ps(mValue, _mm_set1_ps(inV2));
+#elif defined(JPH_USE_NEON)
+	return vmulq_n_f32(mValue, inV2);
+#else
+	return Vec4(mF32[0] * inV2, mF32[1] * inV2, mF32[2] * inV2, mF32[3] * inV2);
+#endif
+}
+
+/// Multiply vector with float
+Vec4 operator * (float inV1, Vec4Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	return _mm_mul_ps(_mm_set1_ps(inV1), inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vmulq_n_f32(inV2.mValue, inV1);
+#else
+	return Vec4(inV1 * inV2.mF32[0],
+				inV1 * inV2.mF32[1],
+				inV1 * inV2.mF32[2],
+				inV1 * inV2.mF32[3]);
+#endif
+}
+
+Vec4 Vec4::operator / (float inV2) const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_div_ps(mValue, _mm_set1_ps(inV2));
+#elif defined(JPH_USE_NEON)
+	return vdivq_f32(mValue, vdupq_n_f32(inV2));
+#else
+	return Vec4(mF32[0] / inV2, mF32[1] / inV2, mF32[2] / inV2, mF32[3] / inV2);
+#endif
+}
+
+Vec4 &Vec4::operator *= (float inV2)
+{
+#if defined(JPH_USE_SSE)
+	mValue = _mm_mul_ps(mValue, _mm_set1_ps(inV2));
+#elif defined(JPH_USE_NEON)
+	mValue = vmulq_n_f32(mValue, inV2);
+#else
+	for (int i = 0; i < 4; ++i)
+		mF32[i] *= inV2;
+#endif
+	return *this;
+}
+
+Vec4 &Vec4::operator *= (Vec4Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	mValue = _mm_mul_ps(mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	mValue = vmulq_f32(mValue, inV2.mValue);
+#else
+	for (int i = 0; i < 4; ++i)
+		mF32[i] *= inV2.mF32[i];
+#endif
+	return *this;
+}
+
+Vec4 &Vec4::operator /= (float inV2)
+{
+#if defined(JPH_USE_SSE)
+	mValue = _mm_div_ps(mValue, _mm_set1_ps(inV2));
+#elif defined(JPH_USE_NEON)
+	mValue = vdivq_f32(mValue, vdupq_n_f32(inV2));
+#else
+	for (int i = 0; i < 4; ++i)
+		mF32[i] /= inV2;
+#endif
+	return *this;
+}
+
+Vec4 Vec4::operator + (Vec4Arg inV2) const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_add_ps(mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vaddq_f32(mValue, inV2.mValue);
+#else
+	return Vec4(mF32[0] + inV2.mF32[0],
+				mF32[1] + inV2.mF32[1],
+				mF32[2] + inV2.mF32[2],
+				mF32[3] + inV2.mF32[3]);
+#endif
+}
+
+Vec4 &Vec4::operator += (Vec4Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	mValue = _mm_add_ps(mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	mValue = vaddq_f32(mValue, inV2.mValue);
+#else
+	for (int i = 0; i < 4; ++i)
+		mF32[i] += inV2.mF32[i];
+#endif
+	return *this;
+}
+
+Vec4 Vec4::operator - () const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_sub_ps(_mm_setzero_ps(), mValue);
+#elif defined(JPH_USE_NEON)
+	#ifdef JPH_CROSS_PLATFORM_DETERMINISTIC
+		return vsubq_f32(vdupq_n_f32(0), mValue);
+	#else
+		return vnegq_f32(mValue);
+	#endif
+#else
+	#ifdef JPH_CROSS_PLATFORM_DETERMINISTIC
+		return Vec4(0.0f - mF32[0], 0.0f - mF32[1], 0.0f - mF32[2], 0.0f - mF32[3]);
+	#else
+		return Vec4(-mF32[0], -mF32[1], -mF32[2], -mF32[3]);
+	#endif
+#endif
+}
+
+Vec4 Vec4::operator - (Vec4Arg inV2) const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_sub_ps(mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vsubq_f32(mValue, inV2.mValue);
+#else
+	return Vec4(mF32[0] - inV2.mF32[0],
+				mF32[1] - inV2.mF32[1],
+				mF32[2] - inV2.mF32[2],
+				mF32[3] - inV2.mF32[3]);
+#endif
+}
+
+Vec4 &Vec4::operator -= (Vec4Arg inV2)
+{
+#if defined(JPH_USE_SSE)
+	mValue = _mm_sub_ps(mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	mValue = vsubq_f32(mValue, inV2.mValue);
+#else
+	for (int i = 0; i < 4; ++i)
+		mF32[i] -= inV2.mF32[i];
+#endif
+	return *this;
+}
+
+Vec4 Vec4::operator / (Vec4Arg inV2) const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_div_ps(mValue, inV2.mValue);
+#elif defined(JPH_USE_NEON)
+	return vdivq_f32(mValue, inV2.mValue);
+#else
+	return Vec4(mF32[0] / inV2.mF32[0],
+				mF32[1] / inV2.mF32[1],
+				mF32[2] / inV2.mF32[2],
+				mF32[3] / inV2.mF32[3]);
+#endif
+}
+
+Vec4 Vec4::SplatX() const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(0, 0, 0, 0));
+#elif defined(JPH_USE_NEON)
+	return vdupq_laneq_f32(mValue, 0);
+#else
+	return Vec4(mF32[0], mF32[0], mF32[0], mF32[0]);
+#endif
+}
+
+Vec4 Vec4::SplatY() const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(1, 1, 1, 1));
+#elif defined(JPH_USE_NEON)
+	return vdupq_laneq_f32(mValue, 1);
+#else
+	return Vec4(mF32[1], mF32[1], mF32[1], mF32[1]);
+#endif
+}
+
+Vec4 Vec4::SplatZ() const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(2, 2, 2, 2));
+#elif defined(JPH_USE_NEON)
+	return vdupq_laneq_f32(mValue, 2);
+#else
+	return Vec4(mF32[2], mF32[2], mF32[2], mF32[2]);
+#endif
+}
+
+Vec4 Vec4::SplatW() const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(3, 3, 3, 3));
+#elif defined(JPH_USE_NEON)
+	return vdupq_laneq_f32(mValue, 3);
+#else
+	return Vec4(mF32[3], mF32[3], mF32[3], mF32[3]);
+#endif
+}
+
+Vec4 Vec4::Abs() const
+{
+#if defined(JPH_USE_AVX512)
+	return _mm_range_ps(mValue, mValue, 0b1000);
+#elif defined(JPH_USE_SSE)
+	return _mm_max_ps(_mm_sub_ps(_mm_setzero_ps(), mValue), mValue);
+#elif defined(JPH_USE_NEON)
+	return vabsq_f32(mValue);
+#else
+	return Vec4(abs(mF32[0]), abs(mF32[1]), abs(mF32[2]), abs(mF32[3]));
+#endif
+}
+
+Vec4 Vec4::Reciprocal() const
+{
+	return sReplicate(1.0f) / mValue;
+}
+
+Vec4 Vec4::DotV(Vec4Arg inV2) const
+{
+#if defined(JPH_USE_SSE4_1)
+	return _mm_dp_ps(mValue, inV2.mValue, 0xff);
+#elif defined(JPH_USE_NEON)
+	float32x4_t mul = vmulq_f32(mValue, inV2.mValue);
+	return vdupq_n_f32(vaddvq_f32(mul));
+#else
+	// Brackets placed so that the order is consistent with the vectorized version
+	return Vec4::sReplicate((mF32[0] * inV2.mF32[0] + mF32[1] * inV2.mF32[1]) + (mF32[2] * inV2.mF32[2] + mF32[3] * inV2.mF32[3]));
+#endif
+}
+
+float Vec4::Dot(Vec4Arg inV2) const
+{
+#if defined(JPH_USE_SSE4_1)
+	return _mm_cvtss_f32(_mm_dp_ps(mValue, inV2.mValue, 0xff));
+#elif defined(JPH_USE_NEON)
+	float32x4_t mul = vmulq_f32(mValue, inV2.mValue);
+	return vaddvq_f32(mul);
+#else
+	// Brackets placed so that the order is consistent with the vectorized version
+	return (mF32[0] * inV2.mF32[0] + mF32[1] * inV2.mF32[1]) + (mF32[2] * inV2.mF32[2] + mF32[3] * inV2.mF32[3]);
+#endif
+}
+
+float Vec4::LengthSq() const
+{
+#if defined(JPH_USE_SSE4_1)
+	return _mm_cvtss_f32(_mm_dp_ps(mValue, mValue, 0xff));
+#elif defined(JPH_USE_NEON)
+	float32x4_t mul = vmulq_f32(mValue, mValue);
+	return vaddvq_f32(mul);
+#else
+	// Brackets placed so that the order is consistent with the vectorized version
+	return (mF32[0] * mF32[0] + mF32[1] * mF32[1]) + (mF32[2] * mF32[2] + mF32[3] * mF32[3]);
+#endif
+}
+
+float Vec4::Length() const
+{
+#if defined(JPH_USE_SSE4_1)
+	return _mm_cvtss_f32(_mm_sqrt_ss(_mm_dp_ps(mValue, mValue, 0xff)));
+#elif defined(JPH_USE_NEON)
+	float32x4_t mul = vmulq_f32(mValue, mValue);
+	float32x2_t sum = vdup_n_f32(vaddvq_f32(mul));
+	return vget_lane_f32(vsqrt_f32(sum), 0);
+#else
+	// Brackets placed so that the order is consistent with the vectorized version
+	return sqrt((mF32[0] * mF32[0] + mF32[1] * mF32[1]) + (mF32[2] * mF32[2] + mF32[3] * mF32[3]));
+#endif
+}
+
+Vec4 Vec4::Sqrt() const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_sqrt_ps(mValue);
+#elif defined(JPH_USE_NEON)
+	return vsqrtq_f32(mValue);
+#else
+	return Vec4(sqrt(mF32[0]), sqrt(mF32[1]), sqrt(mF32[2]), sqrt(mF32[3]));
+#endif
+}
+
+
+Vec4 Vec4::GetSign() const
+{
+#if defined(JPH_USE_AVX512)
+	return _mm_fixupimm_ps(mValue, mValue, _mm_set1_epi32(0xA9A90A00), 0);
+#elif defined(JPH_USE_SSE)
+	Type minus_one = _mm_set1_ps(-1.0f);
+	Type one = _mm_set1_ps(1.0f);
+	return _mm_or_ps(_mm_and_ps(mValue, minus_one), one);
+#elif defined(JPH_USE_NEON)
+	Type minus_one = vdupq_n_f32(-1.0f);
+	Type one = vdupq_n_f32(1.0f);
+	return vreinterpretq_f32_u32(vorrq_u32(vandq_u32(vreinterpretq_u32_f32(mValue), vreinterpretq_u32_f32(minus_one)), vreinterpretq_u32_f32(one)));
+#else
+	return Vec4(std::signbit(mF32[0])? -1.0f : 1.0f,
+				std::signbit(mF32[1])? -1.0f : 1.0f,
+				std::signbit(mF32[2])? -1.0f : 1.0f,
+				std::signbit(mF32[3])? -1.0f : 1.0f);
+#endif
+}
+
+Vec4 Vec4::Normalized() const
+{
+#if defined(JPH_USE_SSE4_1)
+	return _mm_div_ps(mValue, _mm_sqrt_ps(_mm_dp_ps(mValue, mValue, 0xff)));
+#elif defined(JPH_USE_NEON)
+	float32x4_t mul = vmulq_f32(mValue, mValue);
+	float32x4_t sum = vdupq_n_f32(vaddvq_f32(mul));
+	return vdivq_f32(mValue, vsqrtq_f32(sum));
+#else
+	return *this / Length();
+#endif
+}
+
+void Vec4::StoreFloat4(Float4 *outV) const
+{
+#if defined(JPH_USE_SSE)
+	_mm_storeu_ps(&outV->x, mValue);
+#elif defined(JPH_USE_NEON)
+	vst1q_f32(&outV->x, mValue);
+#else
+	for (int i = 0; i < 4; ++i)
+		(&outV->x)[i] = mF32[i];
+#endif
+}
+
+UVec4 Vec4::ToInt() const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_cvttps_epi32(mValue);
+#elif defined(JPH_USE_NEON)
+	return vcvtq_u32_f32(mValue);
+#else
+	return UVec4(uint32(mF32[0]), uint32(mF32[1]), uint32(mF32[2]), uint32(mF32[3]));
+#endif
+}
+
+UVec4 Vec4::ReinterpretAsInt() const
+{
+#if defined(JPH_USE_SSE)
+	return UVec4(_mm_castps_si128(mValue));
+#elif defined(JPH_USE_NEON)
+	return vreinterpretq_u32_f32(mValue);
+#else
+	return *reinterpret_cast<const UVec4 *>(this);
+#endif
+}
+
+int Vec4::GetSignBits() const
+{
+#if defined(JPH_USE_SSE)
+	return _mm_movemask_ps(mValue);
+#elif defined(JPH_USE_NEON)
+	int32x4_t shift = JPH_NEON_INT32x4(0, 1, 2, 3);
+	return vaddvq_u32(vshlq_u32(vshrq_n_u32(vreinterpretq_u32_f32(mValue), 31), shift));
+#else
+	return (std::signbit(mF32[0])? 1 : 0) | (std::signbit(mF32[1])? 2 : 0) | (std::signbit(mF32[2])? 4 : 0) | (std::signbit(mF32[3])? 8 : 0);
+#endif
+}
+
+float Vec4::ReduceMin() const
+{
+	Vec4 v = sMin(mValue, Swizzle<SWIZZLE_Y, SWIZZLE_UNUSED, SWIZZLE_W, SWIZZLE_UNUSED>());
+	v = sMin(v, v.Swizzle<SWIZZLE_Z, SWIZZLE_UNUSED, SWIZZLE_UNUSED, SWIZZLE_UNUSED>());
+	return v.GetX();
+}
+
+float Vec4::ReduceMax() const
+{
+	Vec4 v = sMax(mValue, Swizzle<SWIZZLE_Y, SWIZZLE_UNUSED, SWIZZLE_W, SWIZZLE_UNUSED>());
+	v = sMax(v, v.Swizzle<SWIZZLE_Z, SWIZZLE_UNUSED, SWIZZLE_UNUSED, SWIZZLE_UNUSED>());
+	return v.GetX();
+}
+
+void Vec4::SinCos(Vec4 &outSin, Vec4 &outCos) const
+{
+	// Implementation based on sinf.c from the cephes library, combines sinf and cosf in a single function, changes octants to quadrants and vectorizes it
+	// Original implementation by Stephen L. Moshier (See: http://www.moshier.net/)
+
+	// Make argument positive and remember sign for sin only since cos is symmetric around x (highest bit of a float is the sign bit)
+	UVec4 sin_sign = UVec4::sAnd(ReinterpretAsInt(), UVec4::sReplicate(0x80000000U));
+	Vec4 x = Vec4::sXor(*this, sin_sign.ReinterpretAsFloat());
+
+	// x / (PI / 2) rounded to nearest int gives us the quadrant closest to x
+	UVec4 quadrant = (0.6366197723675814f * x + Vec4::sReplicate(0.5f)).ToInt();
+
+	// Make x relative to the closest quadrant.
+	// This does x = x - quadrant * PI / 2 using a two step Cody-Waite argument reduction.
+	// This improves the accuracy of the result by avoiding loss of significant bits in the subtraction.
+	// We start with x = x - quadrant * PI / 2, PI / 2 in hexadecimal notation is 0x3fc90fdb, we remove the lowest 16 bits to
+	// get 0x3fc90000 (= 1.5703125) this means we can now multiply with a number of up to 2^16 without losing any bits.
+	// This leaves us with: x = (x - quadrant * 1.5703125) - quadrant * (PI / 2 - 1.5703125).
+	// PI / 2 - 1.5703125 in hexadecimal is 0x39fdaa22, stripping the lowest 12 bits we get 0x39fda000 (= 0.0004837512969970703125)
+	// This leaves uw with: x = ((x - quadrant * 1.5703125) - quadrant * 0.0004837512969970703125) - quadrant * (PI / 2 - 1.5703125 - 0.0004837512969970703125)
+	// See: https://stackoverflow.com/questions/42455143/sine-cosine-modular-extended-precision-arithmetic
+	// After this we have x in the range [-PI / 4, PI / 4].
+	Vec4 float_quadrant = quadrant.ToFloat();
+	x = ((x - float_quadrant * 1.5703125f) - float_quadrant * 0.0004837512969970703125f) - float_quadrant * 7.549789948768648e-8f;
+
+	// Calculate x2 = x^2
+	Vec4 x2 = x * x;
+
+	// Taylor expansion:
+	// Cos(x) = 1 - x^2/2! + x^4/4! - x^6/6! + x^8/8! + ... = (((x2/8!- 1/6!) * x2 + 1/4!) * x2 - 1/2!) * x2 + 1
+	Vec4 taylor_cos = ((2.443315711809948e-5f * x2 - Vec4::sReplicate(1.388731625493765e-3f)) * x2 + Vec4::sReplicate(4.166664568298827e-2f)) * x2 * x2 - 0.5f * x2 + Vec4::sReplicate(1.0f);
+	// Sin(x) = x - x^3/3! + x^5/5! - x^7/7! + ... = ((-x2/7! + 1/5!) * x2 - 1/3!) * x2 * x + x
+	Vec4 taylor_sin = ((-1.9515295891e-4f * x2 + Vec4::sReplicate(8.3321608736e-3f)) * x2 - Vec4::sReplicate(1.6666654611e-1f)) * x2 * x + x;
+
+	// The lowest 2 bits of quadrant indicate the quadrant that we are in.
+	// Let x be the original input value and x' our value that has been mapped to the range [-PI / 4, PI / 4].
+	// since cos(x) = sin(x - PI / 2) and since we want to use the Taylor expansion as close as possible to 0,
+	// we can alternate between using the Taylor expansion for sin and cos according to the following table:
+	//
+	// quadrant	 sin(x)		 cos(x)
+	// XXX00b	 sin(x')	 cos(x')
+	// XXX01b	 cos(x')	-sin(x')
+	// XXX10b	-sin(x')	-cos(x')
+	// XXX11b	-cos(x')	 sin(x')
+	//
+	// So: sin_sign = bit2, cos_sign = bit1 ^ bit2, bit1 determines if we use sin or cos Taylor expansion
+	UVec4 bit1 = quadrant.LogicalShiftLeft<31>();
+	UVec4 bit2 = UVec4::sAnd(quadrant.LogicalShiftLeft<30>(), UVec4::sReplicate(0x80000000U));
+
+	// Select which one of the results is sin and which one is cos
+	Vec4 s = Vec4::sSelect(taylor_sin, taylor_cos, bit1);
+	Vec4 c = Vec4::sSelect(taylor_cos, taylor_sin, bit1);
+
+	// Update the signs
+	sin_sign = UVec4::sXor(sin_sign, bit2);
+	UVec4 cos_sign = UVec4::sXor(bit1, bit2);
+
+	// Correct the signs
+	outSin = Vec4::sXor(s, sin_sign.ReinterpretAsFloat());
+	outCos = Vec4::sXor(c, cos_sign.ReinterpretAsFloat());
+}
+
+Vec4 Vec4::Tan() const
+{
+	// Implementation based on tanf.c from the cephes library, see Vec4::SinCos for further details
+	// Original implementation by Stephen L. Moshier (See: http://www.moshier.net/)
+
+	// Make argument positive
+	UVec4 tan_sign = UVec4::sAnd(ReinterpretAsInt(), UVec4::sReplicate(0x80000000U));
+	Vec4 x = Vec4::sXor(*this, tan_sign.ReinterpretAsFloat());
+
+	// x / (PI / 2) rounded to nearest int gives us the quadrant closest to x
+	UVec4 quadrant = (0.6366197723675814f * x + Vec4::sReplicate(0.5f)).ToInt();
+
+	// Remap x to range [-PI / 4, PI / 4], see Vec4::SinCos
+	Vec4 float_quadrant = quadrant.ToFloat();
+	x = ((x - float_quadrant * 1.5703125f) - float_quadrant * 0.0004837512969970703125f) - float_quadrant * 7.549789948768648e-8f;
+
+	// Calculate x2 = x^2
+	Vec4 x2 = x * x;
+
+	// Roughly equivalent to the Taylor expansion:
+	// Tan(x) = x + x^3/3 + 2*x^5/15 + 17*x^7/315 + 62*x^9/2835 + ...
+	Vec4 tan =
+		(((((9.38540185543e-3f * x2 + Vec4::sReplicate(3.11992232697e-3f)) * x2 + Vec4::sReplicate(2.44301354525e-2f)) * x2
+		+ Vec4::sReplicate(5.34112807005e-2f)) * x2 + Vec4::sReplicate(1.33387994085e-1f)) * x2 + Vec4::sReplicate(3.33331568548e-1f)) * x2 * x + x;
+
+	// For the 2nd and 4th quadrant we need to invert the value
+	UVec4 bit1 = quadrant.LogicalShiftLeft<31>();
+	tan = Vec4::sSelect(tan, Vec4::sReplicate(-1.0f) / (tan JPH_IF_FLOATING_POINT_EXCEPTIONS_ENABLED(+ Vec4::sReplicate(FLT_MIN))), bit1); // Add small epsilon to prevent div by zero, works because tan is always positive
+
+	// Put the sign back
+	return Vec4::sXor(tan, tan_sign.ReinterpretAsFloat());
+}
+
+Vec4 Vec4::ASin() const
+{
+	// Implementation based on asinf.c from the cephes library
+	// Original implementation by Stephen L. Moshier (See: http://www.moshier.net/)
+
+	// Make argument positive
+	UVec4 asin_sign = UVec4::sAnd(ReinterpretAsInt(), UVec4::sReplicate(0x80000000U));
+	Vec4 a = Vec4::sXor(*this, asin_sign.ReinterpretAsFloat());
+
+	// ASin is not defined outside the range [-1, 1] but it often happens that a value is slightly above 1 so we just clamp here
+	a = Vec4::sMin(a, Vec4::sReplicate(1.0f));
+
+	// When |x| <= 0.5 we use the asin approximation as is
+	Vec4 z1 = a * a;
+	Vec4 x1 = a;
+
+	// When |x| > 0.5 we use the identity asin(x) = PI / 2 - 2 * asin(sqrt((1 - x) / 2))
+	Vec4 z2 = 0.5f * (Vec4::sReplicate(1.0f) - a);
+	Vec4 x2 = z2.Sqrt();
+
+	// Select which of the two situations we have
+	UVec4 greater = Vec4::sGreater(a, Vec4::sReplicate(0.5f));
+	Vec4 z = Vec4::sSelect(z1, z2, greater);
+	Vec4 x = Vec4::sSelect(x1, x2, greater);
+
+	// Polynomial approximation of asin
+	z = ((((4.2163199048e-2f * z + Vec4::sReplicate(2.4181311049e-2f)) * z + Vec4::sReplicate(4.5470025998e-2f)) * z + Vec4::sReplicate(7.4953002686e-2f)) * z + Vec4::sReplicate(1.6666752422e-1f)) * z * x + x;
+
+	// If |x| > 0.5 we need to apply the remainder of the identity above
+	z = Vec4::sSelect(z, Vec4::sReplicate(0.5f * JPH_PI) - (z + z), greater);
+
+	// Put the sign back
+	return Vec4::sXor(z, asin_sign.ReinterpretAsFloat());
+}
+
+Vec4 Vec4::ACos() const
+{
+	// Not the most accurate, but simple
+	return Vec4::sReplicate(0.5f * JPH_PI) - ASin();
+}
+
+Vec4 Vec4::ATan() const
+{
+	// Implementation based on atanf.c from the cephes library
+	// Original implementation by Stephen L. Moshier (See: http://www.moshier.net/)
+
+	// Make argument positive
+	UVec4 atan_sign = UVec4::sAnd(ReinterpretAsInt(), UVec4::sReplicate(0x80000000U));
+	Vec4 x = Vec4::sXor(*this, atan_sign.ReinterpretAsFloat());
+	Vec4 y = Vec4::sZero();
+
+	// If x > Tan(PI / 8)
+	UVec4 greater1 = Vec4::sGreater(x, Vec4::sReplicate(0.4142135623730950f));
+	Vec4 x1 = (x - Vec4::sReplicate(1.0f)) / (x + Vec4::sReplicate(1.0f));
+
+	// If x > Tan(3 * PI / 8)
+	UVec4 greater2 = Vec4::sGreater(x, Vec4::sReplicate(2.414213562373095f));
+	Vec4 x2 = Vec4::sReplicate(-1.0f) / (x JPH_IF_FLOATING_POINT_EXCEPTIONS_ENABLED(+ Vec4::sReplicate(FLT_MIN))); // Add small epsilon to prevent div by zero, works because x is always positive
+
+	// Apply first if
+	x = Vec4::sSelect(x, x1, greater1);
+	y = Vec4::sSelect(y, Vec4::sReplicate(0.25f * JPH_PI), greater1);
+
+	// Apply second if
+	x = Vec4::sSelect(x, x2, greater2);
+	y = Vec4::sSelect(y, Vec4::sReplicate(0.5f * JPH_PI), greater2);
+
+	// Polynomial approximation
+	Vec4 z = x * x;
+	y += (((8.05374449538e-2f * z - Vec4::sReplicate(1.38776856032e-1f)) * z + Vec4::sReplicate(1.99777106478e-1f)) * z - Vec4::sReplicate(3.33329491539e-1f)) * z * x + x;
+
+	// Put the sign back
+	return Vec4::sXor(y, atan_sign.ReinterpretAsFloat());
+}
+
+Vec4 Vec4::sATan2(Vec4Arg inY, Vec4Arg inX)
+{
+	UVec4 sign_mask = UVec4::sReplicate(0x80000000U);
+
+	// Determine absolute value and sign of y
+	UVec4 y_sign = UVec4::sAnd(inY.ReinterpretAsInt(), sign_mask);
+	Vec4 y_abs = Vec4::sXor(inY, y_sign.ReinterpretAsFloat());
+
+	// Determine absolute value and sign of x
+	UVec4 x_sign = UVec4::sAnd(inX.ReinterpretAsInt(), sign_mask);
+	Vec4 x_abs = Vec4::sXor(inX, x_sign.ReinterpretAsFloat());
+
+	// Always divide smallest / largest to avoid dividing by zero
+	UVec4 x_is_numerator = Vec4::sLess(x_abs, y_abs);
+	Vec4 numerator = Vec4::sSelect(y_abs, x_abs, x_is_numerator);
+	Vec4 denominator = Vec4::sSelect(x_abs, y_abs, x_is_numerator);
+	Vec4 atan = (numerator / denominator).ATan();
+
+	// If we calculated x / y instead of y / x the result is PI / 2 - result (note that this is true because we know the result is positive because the input was positive)
+	atan = Vec4::sSelect(atan, Vec4::sReplicate(0.5f * JPH_PI) - atan, x_is_numerator);
+
+	// Now we need to map to the correct quadrant
+	// x_sign	y_sign	result
+	// +1		+1		atan
+	// -1		+1		-atan + PI
+	// -1		-1		atan - PI
+	// +1		-1		-atan
+	// This can be written as: x_sign * y_sign * (atan - (x_sign < 0? PI : 0))
+	atan -= Vec4::sAnd(x_sign.ArithmeticShiftRight<31>().ReinterpretAsFloat(), Vec4::sReplicate(JPH_PI));
+	atan = Vec4::sXor(atan, UVec4::sXor(x_sign, y_sign).ReinterpretAsFloat());
+	return atan;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Math/Vector.h b/thirdparty/jolt_physics/Jolt/Math/Vector.h
new file mode 100644
index 0000000000..b51a93c07b
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Math/Vector.h
@@ -0,0 +1,211 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Templatized vector class
+template <uint Rows>
+class [[nodiscard]] Vector
+{
+public:
+	/// Constructor
+	inline						Vector() = default;
+	inline						Vector(const Vector &) = default;
+
+	/// Dimensions
+	inline uint					GetRows() const											{ return Rows; }
+
+	/// Vector with all zeros
+	inline void					SetZero()
+	{
+		for (uint r = 0; r < Rows; ++r)
+			mF32[r] = 0.0f;
+	}
+
+	inline static Vector		sZero()													{ Vector v; v.SetZero(); return v; }
+
+	/// Copy a (part) of another vector into this vector
+	template <class OtherVector>
+		void					CopyPart(const OtherVector &inV, uint inSourceRow, uint inNumRows, uint inDestRow)
+		{
+			for (uint r = 0; r < inNumRows; ++r)
+				mF32[inDestRow + r] = inV[inSourceRow + r];
+		}
+
+	/// Get float component by index
+	inline float				operator [] (uint inCoordinate) const
+	{
+		JPH_ASSERT(inCoordinate < Rows);
+		return mF32[inCoordinate];
+	}
+
+	inline float &				operator [] (uint inCoordinate)
+	{
+		JPH_ASSERT(inCoordinate < Rows);
+		return mF32[inCoordinate];
+	}
+
+	/// Comparison
+	inline bool					operator == (const Vector &inV2) const
+	{
+		for (uint r = 0; r < Rows; ++r)
+			if (mF32[r] != inV2.mF32[r])
+				return false;
+		return true;
+	}
+
+	inline bool					operator != (const Vector &inV2) const
+	{
+		for (uint r = 0; r < Rows; ++r)
+			if (mF32[r] != inV2.mF32[r])
+				return true;
+		return false;
+	}
+
+	/// Test if vector consists of all zeros
+	inline bool					IsZero() const
+	{
+		for (uint r = 0; r < Rows; ++r)
+			if (mF32[r] != 0.0f)
+				return false;
+		return true;
+	}
+
+	/// Test if two vectors are close to each other
+	inline bool					IsClose(const Vector &inV2, float inMaxDistSq = 1.0e-12f) const
+	{
+		return (inV2 - *this).LengthSq() <= inMaxDistSq;
+	}
+
+	/// Assignment
+	inline Vector &				operator = (const Vector &) = default;
+
+	/// Multiply vector with float
+	inline Vector				operator * (const float inV2) const
+	{
+		Vector v;
+		for (uint r = 0; r < Rows; ++r)
+			v.mF32[r] = mF32[r] * inV2;
+		return v;
+	}
+
+	inline Vector &				operator *= (const float inV2)
+	{
+		for (uint r = 0; r < Rows; ++r)
+			mF32[r] *= inV2;
+		return *this;
+	}
+
+	/// Multiply vector with float
+	inline friend Vector		operator * (const float inV1, const Vector &inV2)
+	{
+		return inV2 * inV1;
+	}
+
+	/// Divide vector by float
+	inline Vector				operator / (float inV2) const
+	{
+		Vector v;
+		for (uint r = 0; r < Rows; ++r)
+			v.mF32[r] = mF32[r] / inV2;
+		return v;
+	}
+
+	inline Vector &				operator /= (float inV2)
+	{
+		for (uint r = 0; r < Rows; ++r)
+			mF32[r] /= inV2;
+		return *this;
+	}
+
+	/// Add two float vectors (component wise)
+	inline Vector				operator + (const Vector &inV2) const
+	{
+		Vector v;
+		for (uint r = 0; r < Rows; ++r)
+			v.mF32[r] = mF32[r] + inV2.mF32[r];
+		return v;
+	}
+
+	inline Vector &				operator += (const Vector &inV2)
+	{
+		for (uint r = 0; r < Rows; ++r)
+			mF32[r] += inV2.mF32[r];
+		return *this;
+	}
+
+	/// Negate
+	inline Vector				operator - () const
+	{
+		Vector v;
+		for (uint r = 0; r < Rows; ++r)
+			v.mF32[r] = -mF32[r];
+		return v;
+	}
+
+	/// Subtract two float vectors (component wise)
+	inline Vector				operator - (const Vector &inV2) const
+	{
+		Vector v;
+		for (uint r = 0; r < Rows; ++r)
+			v.mF32[r] = mF32[r] - inV2.mF32[r];
+		return v;
+	}
+
+	inline Vector &				operator -= (const Vector &inV2)
+	{
+		for (uint r = 0; r < Rows; ++r)
+			mF32[r] -= inV2.mF32[r];
+		return *this;
+	}
+
+	/// Dot product
+	inline float				Dot(const Vector &inV2) const
+	{
+		float dot = 0.0f;
+		for (uint r = 0; r < Rows; ++r)
+			dot += mF32[r] * inV2.mF32[r];
+		return dot;
+	}
+
+	/// Squared length of vector
+	inline float				LengthSq() const
+	{
+		return Dot(*this);
+	}
+
+	/// Length of vector
+	inline float				Length() const
+	{
+		return sqrt(LengthSq());
+	}
+
+	/// Check if vector is normalized
+	inline bool					IsNormalized(float inToleranceSq = 1.0e-6f)
+	{
+		return abs(LengthSq() - 1.0f) <= inToleranceSq;
+	}
+
+	/// Normalize vector
+	inline Vector				Normalized() const
+	{
+		return *this / Length();
+	}
+
+	/// To String
+	friend ostream &			operator << (ostream &inStream, const Vector &inV)
+	{
+		inStream << "[";
+		for (uint i = 0; i < Rows - 1; ++i)
+			inStream << inV.mF32[i] << ", ";
+		inStream << inV.mF32[Rows - 1] << "]";
+		return inStream;
+	}
+
+	float						mF32[Rows];
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStream.h b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStream.h
new file mode 100644
index 0000000000..adbfbb773c
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/ObjectStream/ObjectStream.h
@@ -0,0 +1,333 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/StaticArray.h>
+#include <Jolt/Core/Reference.h>
+#include <Jolt/Core/RTTI.h>
+#include <Jolt/Core/NonCopyable.h>
+#include <Jolt/ObjectStream/SerializableAttribute.h>
+
+#ifdef JPH_OBJECT_STREAM
+
+JPH_NAMESPACE_BEGIN
+
+/// Base class for object stream input and output streams.
+class JPH_EXPORT ObjectStream : public NonCopyable
+{
+public:
+	/// Stream type
+	enum class EStreamType
+	{
+		Text,
+		Binary,
+	};
+
+protected:
+	/// Destructor
+	virtual							~ObjectStream() = default;
+
+	/// Identifier for objects
+	using Identifier = uint32;
+
+	static constexpr int			sVersion = 1;
+	static constexpr int			sRevision = 0;
+	static constexpr Identifier		sNullIdentifier = 0;
+};
+
+/// Interface class for reading from an object stream
+class JPH_EXPORT IObjectStreamIn : public ObjectStream
+{
+public:
+	///@name Input type specific operations
+	virtual bool				ReadDataType(EOSDataType &outType) = 0;
+	virtual bool				ReadName(String &outName) = 0;
+	virtual bool				ReadIdentifier(Identifier &outIdentifier) = 0;
+	virtual bool				ReadCount(uint32 &outCount) = 0;
+
+	///@name Read primitives
+	virtual bool				ReadPrimitiveData(uint8 &outPrimitive) = 0;
+	virtual bool				ReadPrimitiveData(uint16 &outPrimitive) = 0;
+	virtual bool				ReadPrimitiveData(int &outPrimitive) = 0;
+	virtual bool				ReadPrimitiveData(uint32 &outPrimitive) = 0;
+	virtual bool				ReadPrimitiveData(uint64 &outPrimitive) = 0;
+	virtual bool				ReadPrimitiveData(float &outPrimitive) = 0;
+	virtual bool				ReadPrimitiveData(double &outPrimitive) = 0;
+	virtual bool				ReadPrimitiveData(bool &outPrimitive) = 0;
+	virtual bool				ReadPrimitiveData(String &outPrimitive) = 0;
+	virtual bool				ReadPrimitiveData(Float3 &outPrimitive) = 0;
+	virtual bool				ReadPrimitiveData(Double3 &outPrimitive) = 0;
+	virtual bool				ReadPrimitiveData(Vec3 &outPrimitive) = 0;
+	virtual bool				ReadPrimitiveData(DVec3 &outPrimitive) = 0;
+	virtual bool				ReadPrimitiveData(Vec4 &outPrimitive) = 0;
+	virtual bool				ReadPrimitiveData(Quat &outPrimitive) = 0;
+	virtual bool				ReadPrimitiveData(Mat44 &outPrimitive) = 0;
+	virtual bool				ReadPrimitiveData(DMat44 &outPrimitive) = 0;
+
+	///@name Read compounds
+	virtual bool				ReadClassData(const char *inClassName, void *inInstance) = 0;
+	virtual bool				ReadPointerData(const RTTI *inRTTI, void **inPointer, int inRefCountOffset = -1) = 0;
+};
+
+/// Interface class for writing to an object stream
+class JPH_EXPORT IObjectStreamOut : public ObjectStream
+{
+public:
+	///@name Output type specific operations
+	virtual void				WriteDataType(EOSDataType inType) = 0;
+	virtual void				WriteName(const char *inName) = 0;
+	virtual void				WriteIdentifier(Identifier inIdentifier) = 0;
+	virtual void				WriteCount(uint32 inCount) = 0;
+
+	///@name Write primitives
+	virtual void				WritePrimitiveData(const uint8 &inPrimitive) = 0;
+	virtual void				WritePrimitiveData(const uint16 &inPrimitive) = 0;
+	virtual void				WritePrimitiveData(const int &inPrimitive) = 0;
+	virtual void				WritePrimitiveData(const uint32 &inPrimitive) = 0;
+	virtual void				WritePrimitiveData(const uint64 &inPrimitive) = 0;
+	virtual void				WritePrimitiveData(const float &inPrimitive) = 0;
+	virtual void				WritePrimitiveData(const double &inPrimitive) = 0;
+	virtual void				WritePrimitiveData(const bool &inPrimitive) = 0;
+	virtual void				WritePrimitiveData(const String &inPrimitive) = 0;
+	virtual void				WritePrimitiveData(const Float3 &inPrimitive) = 0;
+	virtual void				WritePrimitiveData(const Double3 &inPrimitive) = 0;
+	virtual void				WritePrimitiveData(const Vec3 &inPrimitive) = 0;
+	virtual void				WritePrimitiveData(const DVec3 &inPrimitive) = 0;
+	virtual void				WritePrimitiveData(const Vec4 &inPrimitive) = 0;
+	virtual void				WritePrimitiveData(const Quat &inPrimitive) = 0;
+	virtual void				WritePrimitiveData(const Mat44 &inPrimitive) = 0;
+	virtual void				WritePrimitiveData(const DMat44 &inPrimitive) = 0;
+
+	///@name Write compounds
+	virtual void				WritePointerData(const RTTI *inRTTI, const void *inPointer) = 0;
+	virtual void				WriteClassData(const RTTI *inRTTI, const void *inInstance) = 0;
+
+	///@name Layout hints (for text output)
+	virtual void				HintNextItem()												{ /* Default is do nothing */ }
+	virtual void				HintIndentUp()												{ /* Default is do nothing */ }
+	virtual void				HintIndentDown()											{ /* Default is do nothing */ }
+};
+
+// Define macro to declare functions for a specific primitive type
+#define JPH_DECLARE_PRIMITIVE(name)																			\
+	JPH_EXPORT bool	OSIsType(name *, int inArrayDepth, EOSDataType inDataType, const char *inClassName);	\
+	JPH_EXPORT bool	OSReadData(IObjectStreamIn &ioStream, name &outPrimitive);								\
+	JPH_EXPORT void	OSWriteDataType(IObjectStreamOut &ioStream, name *);									\
+	JPH_EXPORT void	OSWriteData(IObjectStreamOut &ioStream, const name &inPrimitive);
+
+// This file uses the JPH_DECLARE_PRIMITIVE macro to define all types
+#include <Jolt/ObjectStream/ObjectStreamTypes.h>
+
+// Define serialization templates
+template <class T, class A>
+bool OSIsType(Array<T, A> *, int inArrayDepth, EOSDataType inDataType, const char *inClassName)
+{
+	return (inArrayDepth > 0 && OSIsType(static_cast<T *>(nullptr), inArrayDepth - 1, inDataType, inClassName));
+}
+
+template <class T, uint N>
+bool OSIsType(StaticArray<T, N> *, int inArrayDepth, EOSDataType inDataType, const char *inClassName)
+{
+	return (inArrayDepth > 0 && OSIsType(static_cast<T *>(nullptr), inArrayDepth - 1, inDataType, inClassName));
+}
+
+template <class T, uint N>
+bool OSIsType(T (*)[N], int inArrayDepth, EOSDataType inDataType, const char *inClassName)
+{
+	return (inArrayDepth > 0 && OSIsType(static_cast<T *>(nullptr), inArrayDepth - 1, inDataType, inClassName));
+}
+
+template <class T>
+bool OSIsType(Ref<T> *, int inArrayDepth, EOSDataType inDataType, const char *inClassName)
+{
+	return OSIsType(static_cast<T *>(nullptr), inArrayDepth, inDataType, inClassName);
+}
+
+template <class T>
+bool OSIsType(RefConst<T> *, int inArrayDepth, EOSDataType inDataType, const char *inClassName)
+{
+	return OSIsType(static_cast<T *>(nullptr), inArrayDepth, inDataType, inClassName);
+}
+
+/// Define serialization templates for dynamic arrays
+template <class T, class A>
+bool OSReadData(IObjectStreamIn &ioStream, Array<T, A> &inArray)
+{
+	bool continue_reading = true;
+
+	// Read array length
+	uint32 array_length;
+	continue_reading = ioStream.ReadCount(array_length);
+
+	// Read array items
+	if (continue_reading)
+	{
+		inArray.clear();
+		inArray.resize(array_length);
+		for (uint32 el = 0; el < array_length && continue_reading; ++el)
+			continue_reading = OSReadData(ioStream, inArray[el]);
+	}
+
+	return continue_reading;
+}
+
+/// Define serialization templates for static arrays
+template <class T, uint N>
+bool OSReadData(IObjectStreamIn &ioStream, StaticArray<T, N> &inArray)
+{
+	bool continue_reading = true;
+
+	// Read array length
+	uint32 array_length;
+	continue_reading = ioStream.ReadCount(array_length);
+
+	// Check if we can fit this many elements
+	if (array_length > N)
+		return false;
+
+	// Read array items
+	if (continue_reading)
+	{
+		inArray.clear();
+		inArray.resize(array_length);
+		for (uint32 el = 0; el < array_length && continue_reading; ++el)
+			continue_reading = OSReadData(ioStream, inArray[el]);
+	}
+
+	return continue_reading;
+}
+
+/// Define serialization templates for C style arrays
+template <class T, uint N>
+bool OSReadData(IObjectStreamIn &ioStream, T (&inArray)[N])
+{
+	bool continue_reading = true;
+
+	// Read array length
+	uint32 array_length;
+	continue_reading = ioStream.ReadCount(array_length);
+	if (array_length != N)
+		return false;
+
+	// Read array items
+	for (uint32 el = 0; el < N && continue_reading; ++el)
+		continue_reading = OSReadData(ioStream, inArray[el]);
+
+	return continue_reading;
+}
+
+/// Define serialization templates for references
+template <class T>
+bool OSReadData(IObjectStreamIn &ioStream, Ref<T> &inRef)
+{
+	return ioStream.ReadPointerData(JPH_RTTI(T), inRef.InternalGetPointer(), T::sInternalGetRefCountOffset());
+}
+
+template <class T>
+bool OSReadData(IObjectStreamIn &ioStream, RefConst<T> &inRef)
+{
+	return ioStream.ReadPointerData(JPH_RTTI(T), inRef.InternalGetPointer(), T::sInternalGetRefCountOffset());
+}
+
+// Define serialization templates for dynamic arrays
+template <class T, class A>
+void OSWriteDataType(IObjectStreamOut &ioStream, Array<T, A> *)
+{
+	ioStream.WriteDataType(EOSDataType::Array);
+	OSWriteDataType(ioStream, static_cast<T *>(nullptr));
+}
+
+template <class T, class A>
+void OSWriteData(IObjectStreamOut &ioStream, const Array<T, A> &inArray)
+{
+	// Write size of array
+	ioStream.HintNextItem();
+	ioStream.WriteCount(static_cast<uint32>(inArray.size()));
+
+	// Write data in array
+	ioStream.HintIndentUp();
+	for (const T &v : inArray)
+		OSWriteData(ioStream, v);
+	ioStream.HintIndentDown();
+}
+
+/// Define serialization templates for static arrays
+template <class T, uint N>
+void OSWriteDataType(IObjectStreamOut &ioStream, StaticArray<T, N> *)
+{
+	ioStream.WriteDataType(EOSDataType::Array);
+	OSWriteDataType(ioStream, static_cast<T *>(nullptr));
+}
+
+template <class T, uint N>
+void OSWriteData(IObjectStreamOut &ioStream, const StaticArray<T, N> &inArray)
+{
+	// Write size of array
+	ioStream.HintNextItem();
+	ioStream.WriteCount(inArray.size());
+
+	// Write data in array
+	ioStream.HintIndentUp();
+	for (const typename StaticArray<T, N>::value_type &v : inArray)
+		OSWriteData(ioStream, v);
+	ioStream.HintIndentDown();
+}
+
+/// Define serialization templates for C style arrays
+template <class T, uint N>
+void OSWriteDataType(IObjectStreamOut &ioStream, T (*)[N])
+{
+	ioStream.WriteDataType(EOSDataType::Array);
+	OSWriteDataType(ioStream, static_cast<T *>(nullptr));
+}
+
+template <class T, uint N>
+void OSWriteData(IObjectStreamOut &ioStream, const T (&inArray)[N])
+{
+	// Write size of array
+	ioStream.HintNextItem();
+	ioStream.WriteCount(uint32(N));
+
+	// Write data in array
+	ioStream.HintIndentUp();
+	for (const T &v : inArray)
+		OSWriteData(ioStream, v);
+	ioStream.HintIndentDown();
+}
+
+/// Define serialization templates for references
+template <class T>
+void OSWriteDataType(IObjectStreamOut &ioStream, Ref<T> *)
+{
+	OSWriteDataType(ioStream, static_cast<T *>(nullptr));
+}
+
+template <class T>
+void OSWriteData(IObjectStreamOut &ioStream, const Ref<T> &inRef)
+{
+	if (inRef != nullptr)
+		ioStream.WritePointerData(GetRTTI(inRef.GetPtr()), inRef.GetPtr());
+	else
+		ioStream.WritePointerData(nullptr, nullptr);
+}
+
+template <class T>
+void OSWriteDataType(IObjectStreamOut &ioStream, RefConst<T> *)
+{
+	OSWriteDataType(ioStream, static_cast<T *>(nullptr));
+}
+
+template <class T>
+void OSWriteData(IObjectStreamOut &ioStream, const RefConst<T> &inRef)
+{
+	if (inRef != nullptr)
+		ioStream.WritePointerData(GetRTTI(inRef.GetPtr()), inRef.GetPtr());
+	else
+		ioStream.WritePointerData(nullptr, nullptr);
+}
+
+JPH_NAMESPACE_END
+
+#endif // JPH_OBJECT_STREAM
diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttribute.h b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttribute.h
new file mode 100644
index 0000000000..9a6d880218
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttribute.h
@@ -0,0 +1,111 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#ifdef JPH_OBJECT_STREAM
+
+JPH_NAMESPACE_BEGIN
+
+class RTTI;
+class IObjectStreamIn;
+class IObjectStreamOut;
+
+/// Data type
+enum class EOSDataType
+{
+	/// Control codes
+	Declare,																		///< Used to declare the attributes of a new object type
+	Object,																			///< Start of a new object
+	Instance,																		///< Used in attribute declaration, indicates that an object is an instanced attribute (no pointer)
+	Pointer,																		///< Used in attribute declaration, indicates that an object is a pointer attribute
+	Array,																			///< Used in attribute declaration, indicates that this is an array of objects
+
+	// Basic types (primitives)
+	#define JPH_DECLARE_PRIMITIVE(name)	T_##name,
+
+	// This file uses the JPH_DECLARE_PRIMITIVE macro to define all types
+	#include <Jolt/ObjectStream/ObjectStreamTypes.h>
+
+	// Error values for read functions
+	Invalid,																		///< Next token on the stream was not a valid data type
+};
+
+/// Attributes are members of classes that need to be serialized.
+class SerializableAttribute
+{
+public:
+	///@ Serialization functions
+	using pGetMemberPrimitiveType = const RTTI * (*)();
+	using pIsType = bool (*)(int inArrayDepth, EOSDataType inDataType, const char *inClassName);
+	using pReadData = bool (*)(IObjectStreamIn &ioStream, void *inObject);
+	using pWriteData = void (*)(IObjectStreamOut &ioStream, const void *inObject);
+	using pWriteDataType = void (*)(IObjectStreamOut &ioStream);
+
+	/// Constructor
+								SerializableAttribute(const char *inName, uint inMemberOffset, pGetMemberPrimitiveType inGetMemberPrimitiveType, pIsType inIsType, pReadData inReadData, pWriteData inWriteData, pWriteDataType inWriteDataType) : mName(inName), mMemberOffset(inMemberOffset), mGetMemberPrimitiveType(inGetMemberPrimitiveType), mIsType(inIsType), mReadData(inReadData), mWriteData(inWriteData), mWriteDataType(inWriteDataType) { }
+
+	/// Construct from other attribute with base class offset
+								SerializableAttribute(const SerializableAttribute &inOther, int inBaseOffset) : mName(inOther.mName), mMemberOffset(inOther.mMemberOffset + inBaseOffset), mGetMemberPrimitiveType(inOther.mGetMemberPrimitiveType), mIsType(inOther.mIsType), mReadData(inOther.mReadData), mWriteData(inOther.mWriteData), mWriteDataType(inOther.mWriteDataType) { }
+
+	/// Name of the attribute
+	void						SetName(const char *inName)							{ mName = inName; }
+	const char *				GetName() const										{ return mName; }
+
+	/// Access to the memory location that contains the member
+	template <class T>
+	inline T *					GetMemberPointer(void *inObject) const				{ return reinterpret_cast<T *>(reinterpret_cast<uint8 *>(inObject) + mMemberOffset); }
+	template <class T>
+	inline const T *			GetMemberPointer(const void *inObject) const		{ return reinterpret_cast<const T *>(reinterpret_cast<const uint8 *>(inObject) + mMemberOffset); }
+
+	/// In case this attribute contains an RTTI type, return it (note that a Array<sometype> will return the rtti of sometype)
+	const RTTI *				GetMemberPrimitiveType() const
+	{
+		return mGetMemberPrimitiveType();
+	}
+
+	/// Check if this attribute is of a specific type
+	bool						IsType(int inArrayDepth, EOSDataType inDataType, const char *inClassName) const
+	{
+		return mIsType(inArrayDepth, inDataType, inClassName);
+	}
+
+	/// Read the data for this attribute into attribute containing class inObject
+	bool						ReadData(IObjectStreamIn &ioStream, void *inObject) const
+	{
+		return mReadData(ioStream, GetMemberPointer<void>(inObject));
+	}
+
+	/// Write the data for this attribute from attribute containing class inObject
+	void						WriteData(IObjectStreamOut &ioStream, const void *inObject) const
+	{
+		mWriteData(ioStream, GetMemberPointer<void>(inObject));
+	}
+
+	/// Write the data type of this attribute to a stream
+	void						WriteDataType(IObjectStreamOut &ioStream) const
+	{
+		mWriteDataType(ioStream);
+	}
+
+private:
+	// Name of the attribute
+	const char *				mName;
+
+	// Offset of the member relative to the class
+	uint						mMemberOffset;
+
+	// In case this attribute contains an RTTI type, return it (note that a Array<sometype> will return the rtti of sometype)
+	pGetMemberPrimitiveType		mGetMemberPrimitiveType;
+
+	// Serialization operations
+	pIsType						mIsType;
+	pReadData					mReadData;
+	pWriteData					mWriteData;
+	pWriteDataType				mWriteDataType;
+};
+
+JPH_NAMESPACE_END
+
+#endif // JPH_OBJECT_STREAM
diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttributeEnum.h b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttributeEnum.h
new file mode 100644
index 0000000000..c6f241ea41
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttributeEnum.h
@@ -0,0 +1,67 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#ifdef JPH_OBJECT_STREAM
+
+#include <Jolt/ObjectStream/SerializableAttribute.h>
+#include <Jolt/ObjectStream/ObjectStream.h>
+
+JPH_NAMESPACE_BEGIN
+
+//////////////////////////////////////////////////////////////////////////////////////////
+// Macros to add properties to be serialized
+//////////////////////////////////////////////////////////////////////////////////////////
+
+template <class MemberType>
+inline void AddSerializableAttributeEnum(RTTI &inRTTI, uint inOffset, const char *inName)
+{
+	inRTTI.AddAttribute(SerializableAttribute(inName, inOffset,
+		[]() -> const RTTI *
+		{
+			return nullptr;
+		},
+		[](int inArrayDepth, EOSDataType inDataType, [[maybe_unused]] const char *inClassName)
+		{
+			return inArrayDepth == 0 && inDataType == EOSDataType::T_uint32;
+		},
+		[](IObjectStreamIn &ioStream, void *inObject)
+		{
+			uint32 temporary;
+			if (OSReadData(ioStream, temporary))
+			{
+				*reinterpret_cast<MemberType *>(inObject) = static_cast<MemberType>(temporary);
+				return true;
+			}
+			return false;
+		},
+		[](IObjectStreamOut &ioStream, const void *inObject)
+		{
+			static_assert(sizeof(MemberType) <= sizeof(uint32));
+			uint32 temporary = uint32(*reinterpret_cast<const MemberType *>(inObject));
+			OSWriteData(ioStream, temporary);
+		},
+		[](IObjectStreamOut &ioStream)
+		{
+			ioStream.WriteDataType(EOSDataType::T_uint32);
+		}));
+}
+
+// JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS
+#define JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(class_name, member_name, alias_name) \
+	AddSerializableAttributeEnum<decltype(class_name::member_name)>(inRTTI, offsetof(class_name, member_name), alias_name);
+
+// JPH_ADD_ENUM_ATTRIBUTE
+#define JPH_ADD_ENUM_ATTRIBUTE(class_name, member_name) \
+	JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(class_name, member_name, #member_name);
+
+JPH_NAMESPACE_END
+
+#else
+
+#define JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(...)
+#define JPH_ADD_ENUM_ATTRIBUTE(...)
+
+#endif // JPH_OBJECT_STREAM
diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttributeTyped.h b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttributeTyped.h
new file mode 100644
index 0000000000..ce18325c46
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableAttributeTyped.h
@@ -0,0 +1,60 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#ifdef JPH_OBJECT_STREAM
+
+#include <Jolt/ObjectStream/SerializableAttribute.h>
+#include <Jolt/ObjectStream/GetPrimitiveTypeOfType.h>
+#include <Jolt/ObjectStream/ObjectStream.h>
+
+JPH_NAMESPACE_BEGIN
+
+//////////////////////////////////////////////////////////////////////////////////////////
+// Macros to add properties to be serialized
+//////////////////////////////////////////////////////////////////////////////////////////
+
+template <class MemberType>
+inline void AddSerializableAttributeTyped(RTTI &inRTTI, uint inOffset, const char *inName)
+{
+	inRTTI.AddAttribute(SerializableAttribute(inName, inOffset,
+		[]()
+		{
+			return GetPrimitiveTypeOfType((MemberType *)nullptr);
+		},
+		[](int inArrayDepth, EOSDataType inDataType, const char *inClassName)
+		{
+			return OSIsType((MemberType *)nullptr, inArrayDepth, inDataType, inClassName);
+		},
+		[](IObjectStreamIn &ioStream, void *inObject)
+		{
+			return OSReadData(ioStream, *reinterpret_cast<MemberType *>(inObject));
+		},
+		[](IObjectStreamOut &ioStream, const void *inObject)
+		{
+			OSWriteData(ioStream, *reinterpret_cast<const MemberType *>(inObject));
+		},
+		[](IObjectStreamOut &ioStream)
+		{
+			OSWriteDataType(ioStream, (MemberType *)nullptr);
+		}));
+}
+
+// JPH_ADD_ATTRIBUTE
+#define JPH_ADD_ATTRIBUTE_WITH_ALIAS(class_name, member_name, alias_name) \
+	AddSerializableAttributeTyped<decltype(class_name::member_name)>(inRTTI, offsetof(class_name, member_name), alias_name);
+
+// JPH_ADD_ATTRIBUTE
+#define JPH_ADD_ATTRIBUTE(class_name, member_name) \
+	JPH_ADD_ATTRIBUTE_WITH_ALIAS(class_name, member_name, #member_name)
+
+JPH_NAMESPACE_END
+
+#else
+
+#define JPH_ADD_ATTRIBUTE_WITH_ALIAS(...)
+#define JPH_ADD_ATTRIBUTE(...)
+
+#endif // JPH_OBJECT_STREAM
diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableObject.cpp b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableObject.cpp
new file mode 100644
index 0000000000..98d3b3cf8d
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableObject.cpp
@@ -0,0 +1,15 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/ObjectStream/SerializableObject.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(SerializableObject)
+{
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableObject.h b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableObject.h
new file mode 100644
index 0000000000..86b8830c25
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/ObjectStream/SerializableObject.h
@@ -0,0 +1,164 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/ObjectStream/ObjectStream.h>
+
+JPH_NAMESPACE_BEGIN
+
+//////////////////////////////////////////////////////////////////////////////////////////
+// Helper macros
+//////////////////////////////////////////////////////////////////////////////////////////
+
+#ifdef JPH_OBJECT_STREAM
+
+// JPH_DECLARE_SERIALIZATION_FUNCTIONS
+#define JPH_DECLARE_SERIALIZATION_FUNCTIONS(linkage, prefix, class_name)											\
+	linkage prefix bool	OSReadData(IObjectStreamIn &ioStream, class_name &inInstance);								\
+	linkage prefix bool	OSReadData(IObjectStreamIn &ioStream, class_name *&inPointer);								\
+	linkage prefix bool	OSIsType(class_name *, int inArrayDepth, EOSDataType inDataType, const char *inClassName);	\
+	linkage prefix bool	OSIsType(class_name **, int inArrayDepth, EOSDataType inDataType, const char *inClassName);	\
+	linkage prefix void	OSWriteData(IObjectStreamOut &ioStream, const class_name &inInstance);						\
+	linkage prefix void	OSWriteData(IObjectStreamOut &ioStream, class_name *const &inPointer);						\
+	linkage prefix void	OSWriteDataType(IObjectStreamOut &ioStream, class_name *);									\
+	linkage prefix void	OSWriteDataType(IObjectStreamOut &ioStream, class_name **);
+
+// JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS
+#define JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(class_name)															\
+	bool		OSReadData(IObjectStreamIn &ioStream, class_name &inInstance)										\
+	{																												\
+		return ioStream.ReadClassData(#class_name, (void *)&inInstance);											\
+	}																												\
+	bool		OSReadData(IObjectStreamIn &ioStream, class_name *&inPointer)										\
+	{																												\
+		return ioStream.ReadPointerData(JPH_RTTI(class_name), (void **)&inPointer);									\
+	}																												\
+	bool		OSIsType(class_name *, int inArrayDepth, EOSDataType inDataType, const char *inClassName)			\
+	{																												\
+		return inArrayDepth == 0 && inDataType == EOSDataType::Instance && strcmp(inClassName, #class_name) == 0;	\
+	}																												\
+	bool		OSIsType(class_name **, int inArrayDepth, EOSDataType inDataType, const char *inClassName)			\
+	{																												\
+		return inArrayDepth == 0 && inDataType == EOSDataType::Pointer && strcmp(inClassName, #class_name) == 0;	\
+	}																												\
+	void		OSWriteData(IObjectStreamOut &ioStream, const class_name &inInstance)								\
+	{																												\
+		ioStream.WriteClassData(JPH_RTTI(class_name), (void *)&inInstance);											\
+	}																												\
+	void		OSWriteData(IObjectStreamOut &ioStream, class_name *const &inPointer)								\
+	{																												\
+		if (inPointer)																								\
+			ioStream.WritePointerData(GetRTTI(inPointer), (void *)inPointer);										\
+		else																										\
+			ioStream.WritePointerData(nullptr, nullptr);															\
+	}																												\
+	void		OSWriteDataType(IObjectStreamOut &ioStream, class_name *)											\
+	{																												\
+		ioStream.WriteDataType(EOSDataType::Instance);																\
+		ioStream.WriteName(#class_name);																			\
+	}																												\
+	void		OSWriteDataType(IObjectStreamOut &ioStream, class_name **)											\
+	{																												\
+		ioStream.WriteDataType(EOSDataType::Pointer);																\
+		ioStream.WriteName(#class_name);																			\
+	}
+
+#else
+
+#define JPH_DECLARE_SERIALIZATION_FUNCTIONS(...)
+#define JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(...)
+
+#endif // JPH_OBJECT_STREAM
+
+//////////////////////////////////////////////////////////////////////////////////////////
+// Use these macros on non-virtual objects to make them serializable
+//////////////////////////////////////////////////////////////////////////////////////////
+
+// JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL
+#define JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(linkage, class_name)													\
+public:																												\
+	JPH_DECLARE_RTTI_NON_VIRTUAL(linkage, class_name)																\
+	JPH_DECLARE_SERIALIZATION_FUNCTIONS(linkage, friend, class_name)												\
+
+// JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL
+#define JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(class_name)															\
+	JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(class_name)																\
+	JPH_IMPLEMENT_RTTI_NON_VIRTUAL(class_name)																		\
+
+//////////////////////////////////////////////////////////////////////////////////////////
+// Same as above, but when you cannot insert the declaration in the class itself
+//////////////////////////////////////////////////////////////////////////////////////////
+
+// JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS
+#define JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS(linkage, class_name)													\
+	JPH_DECLARE_RTTI_OUTSIDE_CLASS(linkage, class_name)																\
+	JPH_DECLARE_SERIALIZATION_FUNCTIONS(linkage, extern, class_name)												\
+
+// JPH_IMPLEMENT_SERIALIZABLE_OUTSIDE_CLASS
+#define JPH_IMPLEMENT_SERIALIZABLE_OUTSIDE_CLASS(class_name)														\
+	JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(class_name)																\
+	JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(class_name)																	\
+
+//////////////////////////////////////////////////////////////////////////////////////////
+// Same as above, but for classes that have virtual functions
+//////////////////////////////////////////////////////////////////////////////////////////
+
+// JPH_DECLARE_SERIALIZABLE_VIRTUAL - Use for concrete, non-base classes
+#define JPH_DECLARE_SERIALIZABLE_VIRTUAL(linkage, class_name)														\
+public:																												\
+	JPH_DECLARE_RTTI_VIRTUAL(linkage, class_name)																	\
+	JPH_DECLARE_SERIALIZATION_FUNCTIONS(linkage, friend, class_name)												\
+
+// JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL
+#define JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(class_name)																\
+	JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(class_name)																\
+	JPH_IMPLEMENT_RTTI_VIRTUAL(class_name)																			\
+
+// JPH_DECLARE_SERIALIZABLE_ABSTRACT - Use for abstract, non-base classes
+#define JPH_DECLARE_SERIALIZABLE_ABSTRACT(linkage, class_name)														\
+public:																												\
+	JPH_DECLARE_RTTI_ABSTRACT(linkage, class_name)																	\
+	JPH_DECLARE_SERIALIZATION_FUNCTIONS(linkage, friend, class_name)												\
+
+// JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT
+#define JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(class_name)																\
+	JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(class_name)																\
+	JPH_IMPLEMENT_RTTI_ABSTRACT(class_name)																			\
+
+// JPH_DECLARE_SERIALIZABLE_VIRTUAL_BASE - Use for concrete base classes
+#define JPH_DECLARE_SERIALIZABLE_VIRTUAL_BASE(linkage, class_name)													\
+public:																												\
+	JPH_DECLARE_RTTI_VIRTUAL_BASE(linkage, class_name)																\
+	JPH_DECLARE_SERIALIZATION_FUNCTIONS(linkage, friend, class_name)												\
+
+// JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL_BASE
+#define JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL_BASE(class_name)															\
+	JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(class_name)																\
+	JPH_IMPLEMENT_RTTI_VIRTUAL_BASE(class_name)																		\
+
+// JPH_DECLARE_SERIALIZABLE_ABSTRACT_BASE - Use for abstract base class
+#define JPH_DECLARE_SERIALIZABLE_ABSTRACT_BASE(linkage, class_name)													\
+public:																												\
+	JPH_DECLARE_RTTI_ABSTRACT_BASE(linkage, class_name)																\
+	JPH_DECLARE_SERIALIZATION_FUNCTIONS(linkage, friend, class_name)												\
+
+// JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT_BASE
+#define JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT_BASE(class_name)														\
+	JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(class_name)																\
+	JPH_IMPLEMENT_RTTI_ABSTRACT_BASE(class_name)
+
+/// Classes must be derived from SerializableObject if you want to be able to save pointers or
+/// reference counting pointers to objects of this or derived classes. The type will automatically
+/// be determined during serialization and upon deserialization it will be restored correctly.
+class JPH_EXPORT SerializableObject : public NonCopyable
+{
+	JPH_DECLARE_SERIALIZABLE_ABSTRACT_BASE(JPH_EXPORT, SerializableObject)
+
+public:
+	/// Constructor
+	virtual						~SerializableObject() = default;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/ObjectStream/TypeDeclarations.h b/thirdparty/jolt_physics/Jolt/ObjectStream/TypeDeclarations.h
new file mode 100644
index 0000000000..015e1dbf14
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/ObjectStream/TypeDeclarations.h
@@ -0,0 +1,42 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/ObjectStream/SerializableObject.h>
+#include <Jolt/Core/Color.h>
+#include <Jolt/Geometry/AABox.h>
+#include <Jolt/Geometry/Triangle.h>
+#include <Jolt/Geometry/IndexedTriangle.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, uint8);
+JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, uint16);
+JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, int);
+JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, uint32);
+JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, uint64);
+JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, float);
+JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, double);
+JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, bool);
+JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, String);
+JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, Float3);
+JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, Double3);
+JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, Vec3);
+JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, DVec3);
+JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, Vec4);
+JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, Quat);
+JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, Mat44);
+JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, DMat44);
+JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS(JPH_EXPORT, Color);
+JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS(JPH_EXPORT, AABox);
+JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS(JPH_EXPORT, Triangle);
+JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS(JPH_EXPORT, IndexedTriangle);
+JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS(JPH_EXPORT, Plane);
+
+JPH_NAMESPACE_END
+
+// These need to be added after all types have been registered or else clang under linux will not find GetRTTIOfType for the type
+#include <Jolt/ObjectStream/SerializableAttributeTyped.h>
+#include <Jolt/ObjectStream/SerializableAttributeEnum.h>
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/AllowedDOFs.h b/thirdparty/jolt_physics/Jolt/Physics/Body/AllowedDOFs.h
new file mode 100644
index 0000000000..8445cb1863
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Body/AllowedDOFs.h
@@ -0,0 +1,68 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Enum used in BodyCreationSettings and MotionProperties to indicate which degrees of freedom a body has
+enum class EAllowedDOFs : uint8
+{
+	None				= 0b000000,									///< No degrees of freedom are allowed. Note that this is not valid and will crash. Use a static body instead.
+	All					= 0b111111,									///< All degrees of freedom are allowed
+	TranslationX		= 0b000001,									///< Body can move in world space X axis
+	TranslationY		= 0b000010,									///< Body can move in world space Y axis
+	TranslationZ		= 0b000100,									///< Body can move in world space Z axis
+	RotationX			= 0b001000,									///< Body can rotate around world space X axis
+	RotationY			= 0b010000,									///< Body can rotate around world space Y axis
+	RotationZ			= 0b100000,									///< Body can rotate around world space Z axis
+	Plane2D				= TranslationX | TranslationY | RotationZ,	///< Body can only move in X and Y axis and rotate around Z axis
+};
+
+/// Bitwise OR operator for EAllowedDOFs
+constexpr EAllowedDOFs operator | (EAllowedDOFs inLHS, EAllowedDOFs inRHS)
+{
+	return EAllowedDOFs(uint8(inLHS) | uint8(inRHS));
+}
+
+/// Bitwise AND operator for EAllowedDOFs
+constexpr EAllowedDOFs operator & (EAllowedDOFs inLHS, EAllowedDOFs inRHS)
+{
+	return EAllowedDOFs(uint8(inLHS) & uint8(inRHS));
+}
+
+/// Bitwise XOR operator for EAllowedDOFs
+constexpr EAllowedDOFs operator ^ (EAllowedDOFs inLHS, EAllowedDOFs inRHS)
+{
+	return EAllowedDOFs(uint8(inLHS) ^ uint8(inRHS));
+}
+
+/// Bitwise NOT operator for EAllowedDOFs
+constexpr EAllowedDOFs operator ~ (EAllowedDOFs inAllowedDOFs)
+{
+	return EAllowedDOFs(~uint8(inAllowedDOFs));
+}
+
+/// Bitwise OR assignment operator for EAllowedDOFs
+constexpr EAllowedDOFs & operator |= (EAllowedDOFs &ioLHS, EAllowedDOFs inRHS)
+{
+	ioLHS = ioLHS | inRHS;
+	return ioLHS;
+}
+
+/// Bitwise AND assignment operator for EAllowedDOFs
+constexpr EAllowedDOFs & operator &= (EAllowedDOFs &ioLHS, EAllowedDOFs inRHS)
+{
+	ioLHS = ioLHS & inRHS;
+	return ioLHS;
+}
+
+/// Bitwise XOR assignment operator for EAllowedDOFs
+constexpr EAllowedDOFs & operator ^= (EAllowedDOFs &ioLHS, EAllowedDOFs inRHS)
+{
+	ioLHS = ioLHS ^ inRHS;
+	return ioLHS;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/Body.cpp b/thirdparty/jolt_physics/Jolt/Physics/Body/Body.cpp
new file mode 100644
index 0000000000..4386ec2db2
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Body/Body.cpp
@@ -0,0 +1,423 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Physics/SoftBody/SoftBodyCreationSettings.h>
+#include <Jolt/Physics/SoftBody/SoftBodyMotionProperties.h>
+#include <Jolt/Physics/PhysicsSettings.h>
+#include <Jolt/Physics/StateRecorder.h>
+#include <Jolt/Physics/Collision/Shape/EmptyShape.h>
+#include <Jolt/Core/StringTools.h>
+#include <Jolt/Core/Profiler.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+static const EmptyShape sFixedToWorldShape;
+Body Body::sFixedToWorld(false);
+
+Body::Body(bool) :
+	mPosition(Vec3::sZero()),
+	mRotation(Quat::sIdentity()),
+	mShape(&sFixedToWorldShape), // Dummy shape
+	mFriction(0.0f),
+	mRestitution(0.0f),
+	mObjectLayer(cObjectLayerInvalid),
+	mMotionType(EMotionType::Static)
+{
+	sFixedToWorldShape.SetEmbedded();
+}
+
+void Body::SetMotionType(EMotionType inMotionType)
+{
+	if (mMotionType == inMotionType)
+		return;
+
+	JPH_ASSERT(inMotionType == EMotionType::Static || mMotionProperties != nullptr, "Body needs to be created with mAllowDynamicOrKinematic set tot true");
+	JPH_ASSERT(inMotionType != EMotionType::Static || !IsActive(), "Deactivate body first");
+	JPH_ASSERT(inMotionType == EMotionType::Dynamic || !IsSoftBody(), "Soft bodies can only be dynamic, you can make individual vertices kinematic by setting their inverse mass to 0");
+
+	// Store new motion type
+	mMotionType = inMotionType;
+
+	if (mMotionProperties != nullptr)
+	{
+		// Update cache
+		JPH_IF_ENABLE_ASSERTS(mMotionProperties->mCachedMotionType = inMotionType;)
+
+		switch (inMotionType)
+		{
+		case EMotionType::Static:
+			// Stop the object
+			mMotionProperties->mLinearVelocity = Vec3::sZero();
+			mMotionProperties->mAngularVelocity = Vec3::sZero();
+			[[fallthrough]];
+
+		case EMotionType::Kinematic:
+			// Cancel forces
+			mMotionProperties->ResetForce();
+			mMotionProperties->ResetTorque();
+			break;
+
+		case EMotionType::Dynamic:
+			break;
+		}
+	}
+}
+
+void Body::SetAllowSleeping(bool inAllow)
+{
+	mMotionProperties->mAllowSleeping = inAllow;
+	if (inAllow)
+		ResetSleepTimer();
+}
+
+void Body::MoveKinematic(RVec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime)
+{
+	JPH_ASSERT(IsRigidBody()); // Only valid for rigid bodies
+	JPH_ASSERT(!IsStatic());
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read));
+
+	// Calculate center of mass at end situation
+	RVec3 new_com = inTargetPosition + inTargetRotation * mShape->GetCenterOfMass();
+
+	// Calculate delta position and rotation
+	Vec3 delta_pos = Vec3(new_com - mPosition);
+	Quat delta_rotation = inTargetRotation * mRotation.Conjugated();
+
+	mMotionProperties->MoveKinematic(delta_pos, delta_rotation, inDeltaTime);
+}
+
+void Body::CalculateWorldSpaceBoundsInternal()
+{
+	mBounds = mShape->GetWorldSpaceBounds(GetCenterOfMassTransform(), Vec3::sReplicate(1.0f));
+}
+
+void Body::SetPositionAndRotationInternal(RVec3Arg inPosition, QuatArg inRotation, bool inResetSleepTimer)
+{
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite));
+
+	mPosition = inPosition + inRotation * mShape->GetCenterOfMass();
+	mRotation = inRotation;
+
+	// Initialize bounding box
+	CalculateWorldSpaceBoundsInternal();
+
+	// Reset sleeping test
+	if (inResetSleepTimer && mMotionProperties != nullptr)
+		ResetSleepTimer();
+}
+
+void Body::UpdateCenterOfMassInternal(Vec3Arg inPreviousCenterOfMass, bool inUpdateMassProperties)
+{
+	// Update center of mass position so the world position for this body stays the same
+	mPosition += mRotation * (mShape->GetCenterOfMass() - inPreviousCenterOfMass);
+
+	// Recalculate mass and inertia if requested
+	if (inUpdateMassProperties && mMotionProperties != nullptr)
+		mMotionProperties->SetMassProperties(mMotionProperties->GetAllowedDOFs(), mShape->GetMassProperties());
+}
+
+void Body::SetShapeInternal(const Shape *inShape, bool inUpdateMassProperties)
+{
+	JPH_ASSERT(IsRigidBody()); // Only valid for rigid bodies
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite));
+
+	// Get the old center of mass
+	Vec3 old_com = mShape->GetCenterOfMass();
+
+	// Update the shape
+	mShape = inShape;
+
+	// Update center of mass
+	UpdateCenterOfMassInternal(old_com, inUpdateMassProperties);
+
+	// Recalculate bounding box
+	CalculateWorldSpaceBoundsInternal();
+}
+
+ECanSleep Body::UpdateSleepStateInternal(float inDeltaTime, float inMaxMovement, float inTimeBeforeSleep)
+{
+	// Check override & sensors will never go to sleep (they would stop detecting collisions with sleeping bodies)
+	if (!mMotionProperties->mAllowSleeping || IsSensor())
+		return ECanSleep::CannotSleep;
+
+	// Get the points to test
+	RVec3 points[3];
+	GetSleepTestPoints(points);
+
+#ifdef JPH_DOUBLE_PRECISION
+	// Get base offset for spheres
+	DVec3 offset = mMotionProperties->GetSleepTestOffset();
+#endif // JPH_DOUBLE_PRECISION
+
+	for (int i = 0; i < 3; ++i)
+	{
+		Sphere &sphere = mMotionProperties->mSleepTestSpheres[i];
+
+		// Make point relative to base offset
+#ifdef JPH_DOUBLE_PRECISION
+		Vec3 p = Vec3(points[i] - offset);
+#else
+		Vec3 p = points[i];
+#endif // JPH_DOUBLE_PRECISION
+
+		// Encapsulate the point in a sphere
+		sphere.EncapsulatePoint(p);
+
+		// Test if it exceeded the max movement
+		if (sphere.GetRadius() > inMaxMovement)
+		{
+			// Body is not sleeping, reset test
+			mMotionProperties->ResetSleepTestSpheres(points);
+			return ECanSleep::CannotSleep;
+		}
+	}
+
+	return mMotionProperties->AccumulateSleepTime(inDeltaTime, inTimeBeforeSleep);
+}
+
+void Body::GetSubmergedVolume(RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outRelativeCenterOfBuoyancy) const
+{
+	// For GetSubmergedVolume we transform the surface relative to the body position for increased precision
+	Mat44 rotation = Mat44::sRotation(mRotation);
+	Plane surface_relative_to_body = Plane::sFromPointAndNormal(inSurfacePosition - mPosition, inSurfaceNormal);
+
+	// Calculate amount of volume that is submerged and what the center of buoyancy is
+	mShape->GetSubmergedVolume(rotation, Vec3::sReplicate(1.0f), surface_relative_to_body, outTotalVolume, outSubmergedVolume, outRelativeCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, mPosition));
+}
+
+bool Body::ApplyBuoyancyImpulse(float inTotalVolume, float inSubmergedVolume, Vec3Arg inRelativeCenterOfBuoyancy, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime)
+{
+	JPH_ASSERT(IsRigidBody()); // Only implemented for rigid bodies currently
+
+	// We follow the approach from 'Game Programming Gems 6' 2.5 Exact Buoyancy for Polyhedra
+	// All quantities below are in world space
+
+	// If we're not submerged, there's no point in doing the rest of the calculations
+	if (inSubmergedVolume > 0.0f)
+	{
+	#ifdef JPH_DEBUG_RENDERER
+		// Draw submerged volume properties
+		if (Shape::sDrawSubmergedVolumes)
+		{
+			RVec3 center_of_buoyancy = mPosition + inRelativeCenterOfBuoyancy;
+			DebugRenderer::sInstance->DrawMarker(center_of_buoyancy, Color::sWhite, 2.0f);
+			DebugRenderer::sInstance->DrawText3D(center_of_buoyancy, StringFormat("%.3f / %.3f", (double)inSubmergedVolume, (double)inTotalVolume));
+		}
+	#endif // JPH_DEBUG_RENDERER
+
+		// When buoyancy is 1 we want neutral buoyancy, this means that the density of the liquid is the same as the density of the body at that point.
+		// Buoyancy > 1 should make the object float, < 1 should make it sink.
+		float inverse_mass = mMotionProperties->GetInverseMass();
+		float fluid_density = inBuoyancy / (inTotalVolume * inverse_mass);
+
+		// Buoyancy force = Density of Fluid * Submerged volume * Magnitude of gravity * Up direction (eq 2.5.1)
+		// Impulse = Force * Delta time
+		// We should apply this at the center of buoyancy (= center of mass of submerged volume)
+		Vec3 buoyancy_impulse = -fluid_density * inSubmergedVolume * mMotionProperties->GetGravityFactor() * inGravity * inDeltaTime;
+
+		// Calculate the velocity of the center of buoyancy relative to the fluid
+		Vec3 linear_velocity = mMotionProperties->GetLinearVelocity();
+		Vec3 angular_velocity = mMotionProperties->GetAngularVelocity();
+		Vec3 center_of_buoyancy_velocity = linear_velocity + angular_velocity.Cross(inRelativeCenterOfBuoyancy);
+		Vec3 relative_center_of_buoyancy_velocity = inFluidVelocity - center_of_buoyancy_velocity;
+
+		// Here we deviate from the article, instead of eq 2.5.14 we use a quadratic drag formula: https://en.wikipedia.org/wiki/Drag_%28physics%29
+		// Drag force = 0.5 * Fluid Density * (Velocity of fluid - Velocity of center of buoyancy)^2 * Linear Drag * Area Facing the Relative Fluid Velocity
+		// Again Impulse = Force * Delta Time
+		// We should apply this at the center of buoyancy (= center of mass for submerged volume with no center of mass offset)
+
+		// Get size of local bounding box
+		Vec3 size = mShape->GetLocalBounds().GetSize();
+
+		// Determine area of the local space bounding box in the direction of the relative velocity between the fluid and the center of buoyancy
+		float area = 0.0f;
+		float relative_center_of_buoyancy_velocity_len_sq = relative_center_of_buoyancy_velocity.LengthSq();
+		if (relative_center_of_buoyancy_velocity_len_sq > 1.0e-12f)
+		{
+			Vec3 local_relative_center_of_buoyancy_velocity = GetRotation().Conjugated() * relative_center_of_buoyancy_velocity;
+			area = local_relative_center_of_buoyancy_velocity.Abs().Dot(size.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>() * size.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y>()) / sqrt(relative_center_of_buoyancy_velocity_len_sq);
+		}
+
+		// Calculate the impulse
+		Vec3 drag_impulse = (0.5f * fluid_density * inLinearDrag * area * inDeltaTime) * relative_center_of_buoyancy_velocity * relative_center_of_buoyancy_velocity.Length();
+
+		// Clamp magnitude against current linear velocity to prevent overshoot
+		float linear_velocity_len_sq = linear_velocity.LengthSq();
+		float drag_delta_linear_velocity_len_sq = (drag_impulse * inverse_mass).LengthSq();
+		if (drag_delta_linear_velocity_len_sq > linear_velocity_len_sq)
+			drag_impulse *= sqrt(linear_velocity_len_sq / drag_delta_linear_velocity_len_sq);
+
+		// Calculate the resulting delta linear velocity due to buoyancy and drag
+		Vec3 delta_linear_velocity = (drag_impulse + buoyancy_impulse) * inverse_mass;
+		mMotionProperties->AddLinearVelocityStep(delta_linear_velocity);
+
+		// Determine average width of the body (across the three axis)
+		float l = (size.GetX() + size.GetY() + size.GetZ()) / 3.0f;
+
+		// Drag torque = -Angular Drag * Mass * Submerged volume / Total volume * (Average width of body)^2 * Angular velocity (eq 2.5.15)
+		Vec3 drag_angular_impulse = (-inAngularDrag * inSubmergedVolume / inTotalVolume * inDeltaTime * Square(l) / inverse_mass) * angular_velocity;
+		Mat44 inv_inertia = GetInverseInertia();
+		Vec3 drag_delta_angular_velocity = inv_inertia * drag_angular_impulse;
+
+		// Clamp magnitude against the current angular velocity to prevent overshoot
+		float angular_velocity_len_sq = angular_velocity.LengthSq();
+		float drag_delta_angular_velocity_len_sq = drag_delta_angular_velocity.LengthSq();
+		if (drag_delta_angular_velocity_len_sq > angular_velocity_len_sq)
+			drag_delta_angular_velocity *= sqrt(angular_velocity_len_sq / drag_delta_angular_velocity_len_sq);
+
+		// Calculate total delta angular velocity due to drag and buoyancy
+		Vec3 delta_angular_velocity = drag_delta_angular_velocity + inv_inertia * inRelativeCenterOfBuoyancy.Cross(buoyancy_impulse + drag_impulse);
+		mMotionProperties->AddAngularVelocityStep(delta_angular_velocity);
+		return true;
+	}
+
+	return false;
+}
+
+bool Body::ApplyBuoyancyImpulse(RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime)
+{
+	JPH_PROFILE_FUNCTION();
+
+	float total_volume, submerged_volume;
+	Vec3 relative_center_of_buoyancy;
+	GetSubmergedVolume(inSurfacePosition, inSurfaceNormal, total_volume, submerged_volume, relative_center_of_buoyancy);
+
+	return ApplyBuoyancyImpulse(total_volume, submerged_volume, relative_center_of_buoyancy, inBuoyancy, inLinearDrag, inAngularDrag, inFluidVelocity, inGravity, inDeltaTime);
+}
+
+void Body::SaveState(StateRecorder &inStream) const
+{
+	// Only write properties that can change at runtime
+	inStream.Write(mPosition);
+	inStream.Write(mRotation);
+
+	if (mMotionProperties != nullptr)
+	{
+		if (IsSoftBody())
+			static_cast<const SoftBodyMotionProperties *>(mMotionProperties)->SaveState(inStream);
+		else
+			mMotionProperties->SaveState(inStream);
+	}
+}
+
+void Body::RestoreState(StateRecorder &inStream)
+{
+	inStream.Read(mPosition);
+	inStream.Read(mRotation);
+
+	if (mMotionProperties != nullptr)
+	{
+		if (IsSoftBody())
+			static_cast<SoftBodyMotionProperties *>(mMotionProperties)->RestoreState(inStream);
+		else
+			mMotionProperties->RestoreState(inStream);
+
+		JPH_IF_ENABLE_ASSERTS(mMotionProperties->mCachedMotionType = mMotionType);
+	}
+
+	// Initialize bounding box
+	CalculateWorldSpaceBoundsInternal();
+}
+
+BodyCreationSettings Body::GetBodyCreationSettings() const
+{
+	JPH_ASSERT(IsRigidBody());
+
+	BodyCreationSettings result;
+
+	result.mPosition = GetPosition();
+	result.mRotation = GetRotation();
+	result.mLinearVelocity = mMotionProperties != nullptr? mMotionProperties->GetLinearVelocity() : Vec3::sZero();
+	result.mAngularVelocity = mMotionProperties != nullptr? mMotionProperties->GetAngularVelocity() : Vec3::sZero();
+	result.mObjectLayer = GetObjectLayer();
+	result.mUserData = mUserData;
+	result.mCollisionGroup = GetCollisionGroup();
+	result.mMotionType = GetMotionType();
+	result.mAllowedDOFs = mMotionProperties != nullptr? mMotionProperties->GetAllowedDOFs() : EAllowedDOFs::All;
+	result.mAllowDynamicOrKinematic = mMotionProperties != nullptr;
+	result.mIsSensor = IsSensor();
+	result.mCollideKinematicVsNonDynamic = GetCollideKinematicVsNonDynamic();
+	result.mUseManifoldReduction = GetUseManifoldReduction();
+	result.mApplyGyroscopicForce = GetApplyGyroscopicForce();
+	result.mMotionQuality = mMotionProperties != nullptr? mMotionProperties->GetMotionQuality() : EMotionQuality::Discrete;
+	result.mEnhancedInternalEdgeRemoval = GetEnhancedInternalEdgeRemoval();
+	result.mAllowSleeping = mMotionProperties != nullptr? GetAllowSleeping() : true;
+	result.mFriction = GetFriction();
+	result.mRestitution = GetRestitution();
+	result.mLinearDamping = mMotionProperties != nullptr? mMotionProperties->GetLinearDamping() : 0.0f;
+	result.mAngularDamping = mMotionProperties != nullptr? mMotionProperties->GetAngularDamping() : 0.0f;
+	result.mMaxLinearVelocity = mMotionProperties != nullptr? mMotionProperties->GetMaxLinearVelocity() : 0.0f;
+	result.mMaxAngularVelocity = mMotionProperties != nullptr? mMotionProperties->GetMaxAngularVelocity() : 0.0f;
+	result.mGravityFactor = mMotionProperties != nullptr? mMotionProperties->GetGravityFactor() : 1.0f;
+	result.mNumVelocityStepsOverride = mMotionProperties != nullptr? mMotionProperties->GetNumVelocityStepsOverride() : 0;
+	result.mNumPositionStepsOverride = mMotionProperties != nullptr? mMotionProperties->GetNumPositionStepsOverride() : 0;
+	result.mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided;
+
+	// Invert inertia and mass
+	if (mMotionProperties != nullptr)
+	{
+		float inv_mass = mMotionProperties->GetInverseMassUnchecked();
+		Mat44 inv_inertia = mMotionProperties->GetLocalSpaceInverseInertiaUnchecked();
+
+		// Get mass
+		result.mMassPropertiesOverride.mMass = inv_mass != 0.0f? 1.0f / inv_mass : FLT_MAX;
+
+		// Get inertia
+		Mat44 inertia;
+		if (inertia.SetInversed3x3(inv_inertia))
+		{
+			// Inertia was invertible, we can use it
+			result.mMassPropertiesOverride.mInertia = inertia;
+		}
+		else
+		{
+			// Prevent division by zero
+			Vec3 diagonal = Vec3::sMax(inv_inertia.GetDiagonal3(), Vec3::sReplicate(FLT_MIN));
+			result.mMassPropertiesOverride.mInertia = Mat44::sScale(diagonal.Reciprocal());
+		}
+	}
+	else
+	{
+		result.mMassPropertiesOverride.mMass = FLT_MAX;
+		result.mMassPropertiesOverride.mInertia = Mat44::sScale(Vec3::sReplicate(FLT_MAX));
+	}
+
+	result.SetShape(GetShape());
+
+	return result;
+}
+
+SoftBodyCreationSettings Body::GetSoftBodyCreationSettings() const
+{
+	JPH_ASSERT(IsSoftBody());
+
+	SoftBodyCreationSettings result;
+
+	result.mPosition = GetPosition();
+	result.mRotation = GetRotation();
+	result.mUserData = mUserData;
+	result.mObjectLayer = GetObjectLayer();
+	result.mCollisionGroup = GetCollisionGroup();
+	result.mFriction = GetFriction();
+	result.mRestitution = GetRestitution();
+	const SoftBodyMotionProperties *mp = static_cast<const SoftBodyMotionProperties *>(mMotionProperties);
+	result.mNumIterations = mp->GetNumIterations();
+	result.mLinearDamping = mp->GetLinearDamping();
+	result.mMaxLinearVelocity = mp->GetMaxLinearVelocity();
+	result.mGravityFactor = mp->GetGravityFactor();
+	result.mPressure = mp->GetPressure();
+	result.mUpdatePosition = mp->GetUpdatePosition();
+	result.mSettings = mp->GetSettings();
+
+	return result;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/Body.h b/thirdparty/jolt_physics/Jolt/Physics/Body/Body.h
new file mode 100644
index 0000000000..63034f909f
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Body/Body.h
@@ -0,0 +1,429 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/NonCopyable.h>
+#include <Jolt/Geometry/AABox.h>
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h>
+#include <Jolt/Physics/Collision/ObjectLayer.h>
+#include <Jolt/Physics/Collision/CollisionGroup.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Body/MotionProperties.h>
+#include <Jolt/Physics/Body/BodyID.h>
+#include <Jolt/Physics/Body/BodyAccess.h>
+#include <Jolt/Physics/Body/BodyType.h>
+#include <Jolt/Core/StringTools.h>
+
+JPH_NAMESPACE_BEGIN
+
+class StateRecorder;
+class BodyCreationSettings;
+class SoftBodyCreationSettings;
+
+/// A rigid body that can be simulated using the physics system
+///
+/// Note that internally all properties (position, velocity etc.) are tracked relative to the center of mass of the object to simplify the simulation of the object.
+///
+/// The offset between the position of the body and the center of mass position of the body is GetShape()->GetCenterOfMass().
+/// The functions that get/set the position of the body all indicate if they are relative to the center of mass or to the original position in which the shape was created.
+///
+/// The linear velocity is also velocity of the center of mass, to correct for this: \f$VelocityCOM = Velocity - AngularVelocity \times ShapeCOM\f$.
+class alignas(JPH_RVECTOR_ALIGNMENT) JPH_EXPORT_GCC_BUG_WORKAROUND Body : public NonCopyable
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Get the id of this body
+	inline const BodyID &	GetID() const													{ return mID; }
+
+	/// Get the type of body (rigid or soft)
+	inline EBodyType		GetBodyType() const												{ return mBodyType; }
+
+	/// Check if this body is a rigid body
+	inline bool				IsRigidBody() const												{ return mBodyType == EBodyType::RigidBody; }
+
+	/// Check if this body is a soft body
+	inline bool				IsSoftBody() const												{ return mBodyType == EBodyType::SoftBody; }
+
+	// See comment at GetIndexInActiveBodiesInternal for reasoning why TSAN is disabled here
+	JPH_TSAN_NO_SANITIZE
+	/// If this body is currently actively simulating (true) or sleeping (false)
+	inline bool				IsActive() const												{ return mMotionProperties != nullptr && mMotionProperties->mIndexInActiveBodies != cInactiveIndex; }
+
+	/// Check if this body is static (not movable)
+	inline bool				IsStatic() const												{ return mMotionType == EMotionType::Static; }
+
+	/// Check if this body is kinematic (keyframed), which means that it will move according to its current velocity, but forces don't affect it
+	inline bool				IsKinematic() const												{ return mMotionType == EMotionType::Kinematic; }
+
+	/// Check if this body is dynamic, which means that it moves and forces can act on it
+	inline bool				IsDynamic() const												{ return mMotionType == EMotionType::Dynamic; }
+
+	/// Check if a body could be made kinematic or dynamic (if it was created dynamic or with mAllowDynamicOrKinematic set to true)
+	inline bool				CanBeKinematicOrDynamic() const									{ return mMotionProperties != nullptr; }
+
+	/// Change the body to a sensor. A sensor will receive collision callbacks, but will not cause any collision responses and can be used as a trigger volume.
+	/// The cheapest sensor (in terms of CPU usage) is a sensor with motion type Static (they can be moved around using BodyInterface::SetPosition/SetPositionAndRotation).
+	/// These sensors will only detect collisions with active Dynamic or Kinematic bodies. As soon as a body go to sleep, the contact point with the sensor will be lost.
+	/// If you make a sensor Dynamic or Kinematic and activate them, the sensor will be able to detect collisions with sleeping bodies too. An active sensor will never go to sleep automatically.
+	/// When you make a Dynamic or Kinematic sensor, make sure it is in an ObjectLayer that does not collide with Static bodies or other sensors to avoid extra overhead in the broad phase.
+	inline void				SetIsSensor(bool inIsSensor)									{ JPH_ASSERT(IsRigidBody()); if (inIsSensor) mFlags.fetch_or(uint8(EFlags::IsSensor), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::IsSensor)), memory_order_relaxed); }
+
+	/// Check if this body is a sensor.
+	inline bool				IsSensor() const												{ return (mFlags.load(memory_order_relaxed) & uint8(EFlags::IsSensor)) != 0; }
+
+	/// If kinematic objects can generate contact points against other kinematic or static objects.
+	/// Note that turning this on can be CPU intensive as much more collision detection work will be done without any effect on the simulation (kinematic objects are not affected by other kinematic/static objects).
+	/// This can be used to make sensors detect static objects. Note that the sensor must be kinematic and active for it to detect static objects.
+	inline void				SetCollideKinematicVsNonDynamic(bool inCollide)					{ JPH_ASSERT(IsRigidBody()); if (inCollide) mFlags.fetch_or(uint8(EFlags::CollideKinematicVsNonDynamic), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::CollideKinematicVsNonDynamic)), memory_order_relaxed); }
+
+	/// Check if kinematic objects can generate contact points against other kinematic or static objects.
+	inline bool				GetCollideKinematicVsNonDynamic() const							{ return (mFlags.load(memory_order_relaxed) & uint8(EFlags::CollideKinematicVsNonDynamic)) != 0; }
+
+	/// If PhysicsSettings::mUseManifoldReduction is true, this allows turning off manifold reduction for this specific body.
+	/// Manifold reduction by default will combine contacts with similar normals that come from different SubShapeIDs (e.g. different triangles in a mesh shape or different compound shapes).
+	/// If the application requires tracking exactly which SubShapeIDs are in contact, you can turn off manifold reduction. Note that this comes at a performance cost.
+	/// Consider using BodyInterface::SetUseManifoldReduction if the body could already be in contact with other bodies to ensure that the contact cache is invalidated and you get the correct contact callbacks.
+	inline void				SetUseManifoldReduction(bool inUseReduction)					{ JPH_ASSERT(IsRigidBody()); if (inUseReduction) mFlags.fetch_or(uint8(EFlags::UseManifoldReduction), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::UseManifoldReduction)), memory_order_relaxed); }
+
+	/// Check if this body can use manifold reduction.
+	inline bool				GetUseManifoldReduction() const									{ return (mFlags.load(memory_order_relaxed) & uint8(EFlags::UseManifoldReduction)) != 0; }
+
+	/// Checks if the combination of this body and inBody2 should use manifold reduction
+	inline bool				GetUseManifoldReductionWithBody(const Body &inBody2) const		{ return ((mFlags.load(memory_order_relaxed) & inBody2.mFlags.load(memory_order_relaxed)) & uint8(EFlags::UseManifoldReduction)) != 0; }
+
+	/// Set to indicate that the gyroscopic force should be applied to this body (aka Dzhanibekov effect, see https://en.wikipedia.org/wiki/Tennis_racket_theorem)
+	inline void				SetApplyGyroscopicForce(bool inApply)							{ JPH_ASSERT(IsRigidBody()); if (inApply) mFlags.fetch_or(uint8(EFlags::ApplyGyroscopicForce), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::ApplyGyroscopicForce)), memory_order_relaxed); }
+
+	/// Check if the gyroscopic force is being applied for this body
+	inline bool				GetApplyGyroscopicForce() const									{ return (mFlags.load(memory_order_relaxed) & uint8(EFlags::ApplyGyroscopicForce)) != 0; }
+
+	/// Set to indicate that extra effort should be made to try to remove ghost contacts (collisions with internal edges of a mesh). This is more expensive but makes bodies move smoother over a mesh with convex edges.
+	inline void				SetEnhancedInternalEdgeRemoval(bool inApply)					{ JPH_ASSERT(IsRigidBody()); if (inApply) mFlags.fetch_or(uint8(EFlags::EnhancedInternalEdgeRemoval), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::EnhancedInternalEdgeRemoval)), memory_order_relaxed); }
+
+	/// Check if enhanced internal edge removal is turned on
+	inline bool				GetEnhancedInternalEdgeRemoval() const							{ return (mFlags.load(memory_order_relaxed) & uint8(EFlags::EnhancedInternalEdgeRemoval)) != 0; }
+
+	/// Checks if the combination of this body and inBody2 should use enhanced internal edge removal
+	inline bool				GetEnhancedInternalEdgeRemovalWithBody(const Body &inBody2) const { return ((mFlags.load(memory_order_relaxed) | inBody2.mFlags.load(memory_order_relaxed)) & uint8(EFlags::EnhancedInternalEdgeRemoval)) != 0; }
+
+	/// Get the bodies motion type.
+	inline EMotionType		GetMotionType() const											{ return mMotionType; }
+
+	/// Set the motion type of this body. Consider using BodyInterface::SetMotionType instead of this function if the body may be active or if it needs to be activated.
+	void					SetMotionType(EMotionType inMotionType);
+
+	/// Get broadphase layer, this determines in which broad phase sub-tree the object is placed
+	inline BroadPhaseLayer	GetBroadPhaseLayer() const										{ return mBroadPhaseLayer; }
+
+	/// Get object layer, this determines which other objects it collides with
+	inline ObjectLayer		GetObjectLayer() const											{ return mObjectLayer; }
+
+	/// Collision group and sub-group ID, determines which other objects it collides with
+	const CollisionGroup &	GetCollisionGroup() const										{ return mCollisionGroup; }
+	CollisionGroup &		GetCollisionGroup()												{ return mCollisionGroup; }
+	void					SetCollisionGroup(const CollisionGroup &inGroup)				{ mCollisionGroup = inGroup; }
+
+	/// If this body can go to sleep. Note that disabling sleeping on a sleeping object will not wake it up.
+	bool					GetAllowSleeping() const										{ return mMotionProperties->mAllowSleeping; }
+	void					SetAllowSleeping(bool inAllow);
+
+	/// Resets the sleep timer. This does not wake up the body if it is sleeping, but allows resetting the system that detects when a body is sleeping.
+	inline void				ResetSleepTimer();
+
+	/// Friction (dimensionless number, usually between 0 and 1, 0 = no friction, 1 = friction force equals force that presses the two bodies together). Note that bodies can have negative friction but the combined friction (see PhysicsSystem::SetCombineFriction) should never go below zero.
+	inline float			GetFriction() const												{ return mFriction; }
+	void					SetFriction(float inFriction)									{ mFriction = inFriction; }
+
+	/// Restitution (dimensionless number, usually between 0 and 1, 0 = completely inelastic collision response, 1 = completely elastic collision response). Note that bodies can have negative restitution but the combined restitution (see PhysicsSystem::SetCombineRestitution) should never go below zero.
+	inline float			GetRestitution() const											{ return mRestitution; }
+	void					SetRestitution(float inRestitution)								{ mRestitution = inRestitution; }
+
+	/// Get world space linear velocity of the center of mass (unit: m/s)
+	inline Vec3				GetLinearVelocity() const										{ return !IsStatic()? mMotionProperties->GetLinearVelocity() : Vec3::sZero(); }
+
+	/// Set world space linear velocity of the center of mass (unit: m/s).
+	/// If you want the body to wake up when it is sleeping, use BodyInterface::SetLinearVelocity instead.
+	void					SetLinearVelocity(Vec3Arg inLinearVelocity)						{ JPH_ASSERT(!IsStatic()); mMotionProperties->SetLinearVelocity(inLinearVelocity); }
+
+	/// Set world space linear velocity of the center of mass, will make sure the value is clamped against the maximum linear velocity.
+	/// If you want the body to wake up when it is sleeping, use BodyInterface::SetLinearVelocity instead.
+	void					SetLinearVelocityClamped(Vec3Arg inLinearVelocity)				{ JPH_ASSERT(!IsStatic()); mMotionProperties->SetLinearVelocityClamped(inLinearVelocity); }
+
+	/// Get world space angular velocity of the center of mass (unit: rad/s)
+	inline Vec3				GetAngularVelocity() const										{ return !IsStatic()? mMotionProperties->GetAngularVelocity() : Vec3::sZero(); }
+
+	/// Set world space angular velocity of the center of mass (unit: rad/s).
+	/// If you want the body to wake up when it is sleeping, use BodyInterface::SetAngularVelocity instead.
+	void					SetAngularVelocity(Vec3Arg inAngularVelocity)					{ JPH_ASSERT(!IsStatic()); mMotionProperties->SetAngularVelocity(inAngularVelocity); }
+
+	/// Set world space angular velocity of the center of mass, will make sure the value is clamped against the maximum angular velocity.
+	/// If you want the body to wake up when it is sleeping, use BodyInterface::SetAngularVelocity instead.
+	void					SetAngularVelocityClamped(Vec3Arg inAngularVelocity)			{ JPH_ASSERT(!IsStatic()); mMotionProperties->SetAngularVelocityClamped(inAngularVelocity); }
+
+	/// Velocity of point inPoint (in center of mass space, e.g. on the surface of the body) of the body (unit: m/s)
+	inline Vec3				GetPointVelocityCOM(Vec3Arg inPointRelativeToCOM) const			{ return !IsStatic()? mMotionProperties->GetPointVelocityCOM(inPointRelativeToCOM) : Vec3::sZero(); }
+
+	/// Velocity of point inPoint (in world space, e.g. on the surface of the body) of the body (unit: m/s)
+	inline Vec3				GetPointVelocity(RVec3Arg inPoint) const						{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return GetPointVelocityCOM(Vec3(inPoint - mPosition)); }
+
+	/// Add force (unit: N) at center of mass for the next time step, will be reset after the next call to PhysicsSystem::Update.
+	/// If you want the body to wake up when it is sleeping, use BodyInterface::AddForce instead.
+	inline void				AddForce(Vec3Arg inForce)										{ JPH_ASSERT(IsDynamic()); (Vec3::sLoadFloat3Unsafe(mMotionProperties->mForce) + inForce).StoreFloat3(&mMotionProperties->mForce); }
+
+	/// Add force (unit: N) at inPosition for the next time step, will be reset after the next call to PhysicsSystem::Update.
+	/// If you want the body to wake up when it is sleeping, use BodyInterface::AddForce instead.
+	inline void				AddForce(Vec3Arg inForce, RVec3Arg inPosition);
+
+	/// Add torque (unit: N m) for the next time step, will be reset after the next call to PhysicsSystem::Update.
+	/// If you want the body to wake up when it is sleeping, use BodyInterface::AddTorque instead.
+	inline void				AddTorque(Vec3Arg inTorque)										{ JPH_ASSERT(IsDynamic()); (Vec3::sLoadFloat3Unsafe(mMotionProperties->mTorque) + inTorque).StoreFloat3(&mMotionProperties->mTorque); }
+
+	// Get the total amount of force applied to the center of mass this time step (through AddForce calls). Note that it will reset to zero after PhysicsSystem::Update.
+	inline Vec3				GetAccumulatedForce() const										{ JPH_ASSERT(IsDynamic()); return mMotionProperties->GetAccumulatedForce(); }
+
+	// Get the total amount of torque applied to the center of mass this time step (through AddForce/AddTorque calls). Note that it will reset to zero after PhysicsSystem::Update.
+	inline Vec3				GetAccumulatedTorque() const									{ JPH_ASSERT(IsDynamic()); return mMotionProperties->GetAccumulatedTorque(); }
+
+	// Reset the total accumulated force, not that this will be done automatically after every time step.
+	JPH_INLINE void			ResetForce()													{ JPH_ASSERT(IsDynamic()); return mMotionProperties->ResetForce(); }
+
+	// Reset the total accumulated torque, not that this will be done automatically after every time step.
+	JPH_INLINE void			ResetTorque()													{ JPH_ASSERT(IsDynamic()); return mMotionProperties->ResetTorque(); }
+
+	// Reset the current velocity and accumulated force and torque.
+	JPH_INLINE void			ResetMotion()													{ JPH_ASSERT(!IsStatic()); return mMotionProperties->ResetMotion(); }
+
+	/// Get inverse inertia tensor in world space
+	inline Mat44			GetInverseInertia() const;
+
+	/// Add impulse to center of mass (unit: kg m/s).
+	/// If you want the body to wake up when it is sleeping, use BodyInterface::AddImpulse instead.
+	inline void				AddImpulse(Vec3Arg inImpulse);
+
+	/// Add impulse to point in world space (unit: kg m/s).
+	/// If you want the body to wake up when it is sleeping, use BodyInterface::AddImpulse instead.
+	inline void				AddImpulse(Vec3Arg inImpulse, RVec3Arg inPosition);
+
+	/// Add angular impulse in world space (unit: N m s).
+	/// If you want the body to wake up when it is sleeping, use BodyInterface::AddAngularImpulse instead.
+	inline void				AddAngularImpulse(Vec3Arg inAngularImpulse);
+
+	/// Set velocity of body such that it will be positioned at inTargetPosition/Rotation in inDeltaTime seconds.
+	/// If you want the body to wake up when it is sleeping, use BodyInterface::MoveKinematic instead.
+	void					MoveKinematic(RVec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime);
+
+	/// Gets the properties needed to do buoyancy calculations
+	/// @param inSurfacePosition Position of the fluid surface in world space
+	/// @param inSurfaceNormal Normal of the fluid surface (should point up)
+	/// @param outTotalVolume On return this contains the total volume of the shape
+	/// @param outSubmergedVolume On return this contains the submerged volume of the shape
+	/// @param outRelativeCenterOfBuoyancy On return this contains the center of mass of the submerged volume relative to the center of mass of the body
+	void					GetSubmergedVolume(RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outRelativeCenterOfBuoyancy) const;
+
+	/// Applies an impulse to the body that simulates fluid buoyancy and drag.
+	/// If you want the body to wake up when it is sleeping, use BodyInterface::ApplyBuoyancyImpulse instead.
+	/// @param inSurfacePosition Position of the fluid surface in world space
+	/// @param inSurfaceNormal Normal of the fluid surface (should point up)
+	/// @param inBuoyancy The buoyancy factor for the body. 1 = neutral body, < 1 sinks, > 1 floats. Note that we don't use the fluid density since it is harder to configure than a simple number between [0, 2]
+	/// @param inLinearDrag Linear drag factor that slows down the body when in the fluid (approx. 0.5)
+	/// @param inAngularDrag Angular drag factor that slows down rotation when the body is in the fluid (approx. 0.01)
+	/// @param inFluidVelocity The average velocity of the fluid (in m/s) in which the body resides
+	/// @param inGravity The gravity vector (pointing down)
+	/// @param inDeltaTime Delta time of the next simulation step (in s)
+	/// @return true if an impulse was applied, false if the body was not in the fluid
+	bool					ApplyBuoyancyImpulse(RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime);
+
+	/// Applies an impulse to the body that simulates fluid buoyancy and drag.
+	/// If you want the body to wake up when it is sleeping, use BodyInterface::ApplyBuoyancyImpulse instead.
+	/// @param inTotalVolume Total volume of the shape of this body (m^3)
+	/// @param inSubmergedVolume Submerged volume of the shape of this body (m^3)
+	/// @param inRelativeCenterOfBuoyancy The center of mass of the submerged volume relative to the center of mass of the body
+	/// @param inBuoyancy The buoyancy factor for the body. 1 = neutral body, < 1 sinks, > 1 floats. Note that we don't use the fluid density since it is harder to configure than a simple number between [0, 2]
+	/// @param inLinearDrag Linear drag factor that slows down the body when in the fluid (approx. 0.5)
+	/// @param inAngularDrag Angular drag factor that slows down rotation when the body is in the fluid (approx. 0.01)
+	/// @param inFluidVelocity The average velocity of the fluid (in m/s) in which the body resides
+	/// @param inGravity The gravity vector (pointing down)
+	/// @param inDeltaTime Delta time of the next simulation step (in s)
+	/// @return true if an impulse was applied, false if the body was not in the fluid
+	bool					ApplyBuoyancyImpulse(float inTotalVolume, float inSubmergedVolume, Vec3Arg inRelativeCenterOfBuoyancy, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime);
+
+	/// Check if this body has been added to the physics system
+	inline bool				IsInBroadPhase() const											{ return (mFlags.load(memory_order_relaxed) & uint8(EFlags::IsInBroadPhase)) != 0; }
+
+	/// Check if this body has been changed in such a way that the collision cache should be considered invalid for any body interacting with this body
+	inline bool				IsCollisionCacheInvalid() const									{ return (mFlags.load(memory_order_relaxed) & uint8(EFlags::InvalidateContactCache)) != 0; }
+
+	/// Get the shape of this body
+	inline const Shape *	GetShape() const												{ return mShape; }
+
+	/// World space position of the body
+	inline RVec3			GetPosition() const												{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return mPosition - mRotation * mShape->GetCenterOfMass(); }
+
+	/// World space rotation of the body
+	inline Quat				GetRotation() const												{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return mRotation; }
+
+	/// Calculates the transform of this body
+	inline RMat44			GetWorldTransform() const;
+
+	/// Gets the world space position of this body's center of mass
+	inline RVec3			GetCenterOfMassPosition() const									{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return mPosition; }
+
+	/// Calculates the transform for this body's center of mass
+	inline RMat44			GetCenterOfMassTransform() const;
+
+	/// Calculates the inverse of the transform for this body's center of mass
+	inline RMat44			GetInverseCenterOfMassTransform() const;
+
+	/// Get world space bounding box
+	inline const AABox &	GetWorldSpaceBounds() const										{ return mBounds; }
+
+	/// Access to the motion properties
+	const MotionProperties *GetMotionProperties() const										{ JPH_ASSERT(!IsStatic()); return mMotionProperties; }
+	MotionProperties *		GetMotionProperties()											{ JPH_ASSERT(!IsStatic()); return mMotionProperties; }
+
+	/// Access to the motion properties (version that does not check if the object is kinematic or dynamic)
+	const MotionProperties *GetMotionPropertiesUnchecked() const							{ return mMotionProperties; }
+	MotionProperties *		GetMotionPropertiesUnchecked()									{ return mMotionProperties; }
+
+	/// Access to the user data, can be used for anything by the application
+	uint64					GetUserData() const												{ return mUserData; }
+	void					SetUserData(uint64 inUserData)									{ mUserData = inUserData; }
+
+	/// Get surface normal of a particular sub shape and its world space surface position on this body
+	inline Vec3				GetWorldSpaceSurfaceNormal(const SubShapeID &inSubShapeID, RVec3Arg inPosition) const;
+
+	/// Get the transformed shape of this body, which can be used to do collision detection outside of a body lock
+	inline TransformedShape	GetTransformedShape() const										{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read)); return TransformedShape(mPosition, mRotation, mShape, mID); }
+
+	/// Debug function to convert a body back to a body creation settings object to be able to save/recreate the body later
+	BodyCreationSettings	GetBodyCreationSettings() const;
+
+	/// Debug function to convert a soft body back to a soft body creation settings object to be able to save/recreate the body later
+	SoftBodyCreationSettings GetSoftBodyCreationSettings() const;
+
+	/// A dummy body that can be used by constraints to attach a constraint to the world instead of another body
+	static Body				sFixedToWorld;
+
+	///@name THESE FUNCTIONS ARE FOR INTERNAL USE ONLY AND SHOULD NOT BE CALLED BY THE APPLICATION
+	///@{
+
+	/// Helper function for BroadPhase::FindCollidingPairs that returns true when two bodies can collide
+	/// It assumes that body 1 is dynamic and active and guarantees that it body 1 collides with body 2 that body 2 will not collide with body 1 in order to avoid finding duplicate collision pairs
+	static inline bool		sFindCollidingPairsCanCollide(const Body &inBody1, const Body &inBody2);
+
+	/// Update position using an Euler step (used during position integrate & constraint solving)
+	inline void				AddPositionStep(Vec3Arg inLinearVelocityTimesDeltaTime)			{ JPH_ASSERT(IsRigidBody()); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite)); mPosition += mMotionProperties->LockTranslation(inLinearVelocityTimesDeltaTime); JPH_ASSERT(!mPosition.IsNaN()); }
+	inline void				SubPositionStep(Vec3Arg inLinearVelocityTimesDeltaTime)			{ JPH_ASSERT(IsRigidBody()); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite)); mPosition -= mMotionProperties->LockTranslation(inLinearVelocityTimesDeltaTime); JPH_ASSERT(!mPosition.IsNaN()); }
+
+	/// Update rotation using an Euler step (used during position integrate & constraint solving)
+	inline void				AddRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime);
+	inline void				SubRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime);
+
+	/// Flag if body is in the broadphase (should only be called by the BroadPhase)
+	inline void				SetInBroadPhaseInternal(bool inInBroadPhase)					{ if (inInBroadPhase) mFlags.fetch_or(uint8(EFlags::IsInBroadPhase), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::IsInBroadPhase)), memory_order_relaxed); }
+
+	/// Invalidate the contact cache (should only be called by the BodyManager), will be reset the next simulation step. Returns true if the contact cache was still valid.
+	inline bool				InvalidateContactCacheInternal()								{ return (mFlags.fetch_or(uint8(EFlags::InvalidateContactCache), memory_order_relaxed) & uint8(EFlags::InvalidateContactCache)) == 0; }
+
+	/// Reset the collision cache invalid flag (should only be called by the BodyManager).
+	inline void				ValidateContactCacheInternal()									{ JPH_IF_ENABLE_ASSERTS(uint8 old_val = ) mFlags.fetch_and(uint8(~uint8(EFlags::InvalidateContactCache)), memory_order_relaxed); JPH_ASSERT((old_val & uint8(EFlags::InvalidateContactCache)) != 0); }
+
+	/// Updates world space bounding box (should only be called by the PhysicsSystem)
+	void					CalculateWorldSpaceBoundsInternal();
+
+	/// Function to update body's position (should only be called by the BodyInterface since it also requires updating the broadphase)
+	void					SetPositionAndRotationInternal(RVec3Arg inPosition, QuatArg inRotation, bool inResetSleepTimer = true);
+
+	/// Updates the center of mass and optionally mass properties after shifting the center of mass or changes to the shape (should only be called by the BodyInterface since it also requires updating the broadphase)
+	/// @param inPreviousCenterOfMass Center of mass of the shape before the alterations
+	/// @param inUpdateMassProperties When true, the mass and inertia tensor is recalculated
+	void					UpdateCenterOfMassInternal(Vec3Arg inPreviousCenterOfMass, bool inUpdateMassProperties);
+
+	/// Function to update a body's shape (should only be called by the BodyInterface since it also requires updating the broadphase)
+	/// @param inShape The new shape for this body
+	/// @param inUpdateMassProperties When true, the mass and inertia tensor is recalculated
+	void					SetShapeInternal(const Shape *inShape, bool inUpdateMassProperties);
+
+	// TSAN detects a race between BodyManager::AddBodyToActiveBodies coming from PhysicsSystem::ProcessBodyPair and Body::GetIndexInActiveBodiesInternal coming from PhysicsSystem::ProcessBodyPair.
+	// When PhysicsSystem::ProcessBodyPair activates a body, it updates mIndexInActiveBodies and then updates BodyManager::mNumActiveBodies with release semantics. PhysicsSystem::ProcessBodyPair will
+	// then finish its loop of active bodies and at the end of the loop it will read BodyManager::mNumActiveBodies with acquire semantics to see if any bodies were activated during the loop.
+	// This means that changes to mIndexInActiveBodies must be visible to the thread, so TSANs report must be a false positive. We suppress the warning here.
+	JPH_TSAN_NO_SANITIZE
+	/// Access to the index in the BodyManager::mActiveBodies list
+	uint32					GetIndexInActiveBodiesInternal() const							{ return mMotionProperties != nullptr? mMotionProperties->mIndexInActiveBodies : cInactiveIndex; }
+
+	/// Update eligibility for sleeping
+	ECanSleep				UpdateSleepStateInternal(float inDeltaTime, float inMaxMovement, float inTimeBeforeSleep);
+
+	/// Saving state for replay
+	void					SaveState(StateRecorder &inStream) const;
+
+	/// Restoring state for replay
+	void					RestoreState(StateRecorder &inStream);
+
+	///@}
+
+	static constexpr uint32	cInactiveIndex = MotionProperties::cInactiveIndex;				///< Constant indicating that body is not active
+
+private:
+	friend class BodyManager;
+	friend class BodyWithMotionProperties;
+	friend class SoftBodyWithMotionPropertiesAndShape;
+
+							Body() = default;												///< Bodies must be created through BodyInterface::CreateBody
+
+	explicit				Body(bool);														///< Alternative constructor that initializes all members
+
+							~Body()															{ JPH_ASSERT(mMotionProperties == nullptr); } ///< Bodies must be destroyed through BodyInterface::DestroyBody
+
+	inline void				GetSleepTestPoints(RVec3 *outPoints) const;						///< Determine points to test for checking if body is sleeping: COM, COM + largest bounding box axis, COM + second largest bounding box axis
+
+	enum class EFlags : uint8
+	{
+		IsSensor						= 1 << 0,											///< If this object is a sensor. A sensor will receive collision callbacks, but will not cause any collision responses and can be used as a trigger volume.
+		CollideKinematicVsNonDynamic	= 1 << 1,											///< If kinematic objects can generate contact points against other kinematic or static objects.
+		IsInBroadPhase					= 1 << 2,											///< Set this bit to indicate that the body is in the broadphase
+		InvalidateContactCache			= 1 << 3,											///< Set this bit to indicate that all collision caches for this body are invalid, will be reset the next simulation step.
+		UseManifoldReduction			= 1 << 4,											///< Set this bit to indicate that this body can use manifold reduction (if PhysicsSettings::mUseManifoldReduction is true)
+		ApplyGyroscopicForce			= 1 << 5,											///< Set this bit to indicate that the gyroscopic force should be applied to this body (aka Dzhanibekov effect, see https://en.wikipedia.org/wiki/Tennis_racket_theorem)
+		EnhancedInternalEdgeRemoval		= 1 << 6,											///< Set this bit to indicate that enhanced internal edge removal should be used for this body (see BodyCreationSettings::mEnhancedInternalEdgeRemoval)
+	};
+
+	// 16 byte aligned
+	RVec3					mPosition;														///< World space position of center of mass
+	Quat					mRotation;														///< World space rotation of center of mass
+	AABox					mBounds;														///< World space bounding box of the body
+
+	// 8 byte aligned
+	RefConst<Shape>			mShape;															///< Shape representing the volume of this body
+	MotionProperties *		mMotionProperties = nullptr;									///< If this is a keyframed or dynamic object, this object holds all information about the movement
+	uint64					mUserData = 0;													///< User data, can be used for anything by the application
+	CollisionGroup			mCollisionGroup;												///< The collision group this body belongs to (determines if two objects can collide)
+
+	// 4 byte aligned
+	float					mFriction;														///< Friction of the body (dimensionless number, usually between 0 and 1, 0 = no friction, 1 = friction force equals force that presses the two bodies together). Note that bodies can have negative friction but the combined friction (see PhysicsSystem::SetCombineFriction) should never go below zero.
+	float					mRestitution;													///< Restitution of body (dimensionless number, usually between 0 and 1, 0 = completely inelastic collision response, 1 = completely elastic collision response). Note that bodies can have negative restitution but the combined restitution (see PhysicsSystem::SetCombineRestitution) should never go below zero.
+	BodyID					mID;															///< ID of the body (index in the bodies array)
+
+	// 2 or 4 bytes aligned
+	ObjectLayer				mObjectLayer;													///< The collision layer this body belongs to (determines if two objects can collide)
+
+	// 1 byte aligned
+	EBodyType				mBodyType;														///< Type of body (rigid or soft)
+	BroadPhaseLayer			mBroadPhaseLayer;												///< The broad phase layer this body belongs to
+	EMotionType				mMotionType;													///< Type of motion (static, dynamic or kinematic)
+	atomic<uint8>			mFlags = 0;														///< See EFlags for possible flags
+
+	// 122 bytes up to here (64-bit mode, single precision, 16-bit ObjectLayer)
+};
+
+static_assert(JPH_CPU_ADDRESS_BITS != 64 || sizeof(Body) == JPH_IF_SINGLE_PRECISION_ELSE(128, 160), "Body size is incorrect");
+static_assert(alignof(Body) == JPH_RVECTOR_ALIGNMENT, "Body should properly align");
+
+JPH_NAMESPACE_END
+
+#include "Body.inl"
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/Body.inl b/thirdparty/jolt_physics/Jolt/Physics/Body/Body.inl
new file mode 100644
index 0000000000..dbbd64dd7f
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Body/Body.inl
@@ -0,0 +1,197 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+RMat44 Body::GetWorldTransform() const
+{
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read));
+
+	return RMat44::sRotationTranslation(mRotation, mPosition).PreTranslated(-mShape->GetCenterOfMass());
+}
+
+RMat44 Body::GetCenterOfMassTransform() const
+{
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read));
+
+	return RMat44::sRotationTranslation(mRotation, mPosition);
+}
+
+RMat44 Body::GetInverseCenterOfMassTransform() const
+{
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read));
+
+	return RMat44::sInverseRotationTranslation(mRotation, mPosition);
+}
+
+inline bool Body::sFindCollidingPairsCanCollide(const Body &inBody1, const Body &inBody2)
+{
+	// First body should never be a soft body
+	JPH_ASSERT(!inBody1.IsSoftBody());
+
+	// One of these conditions must be true
+	// - We always allow detecting collisions between kinematic and non-dynamic bodies
+	// - One of the bodies must be dynamic to collide
+	// - A kinematic object can collide with a sensor
+	if (!inBody1.GetCollideKinematicVsNonDynamic()
+		&& !inBody2.GetCollideKinematicVsNonDynamic()
+		&& (!inBody1.IsDynamic() && !inBody2.IsDynamic())
+		&& !(inBody1.IsKinematic() && inBody2.IsSensor())
+		&& !(inBody2.IsKinematic() && inBody1.IsSensor()))
+		return false;
+
+	// Check that body 1 is active
+	uint32 body1_index_in_active_bodies = inBody1.GetIndexInActiveBodiesInternal();
+	JPH_ASSERT(!inBody1.IsStatic() && body1_index_in_active_bodies != Body::cInactiveIndex, "This function assumes that Body 1 is active");
+
+	// If the pair A, B collides we need to ensure that the pair B, A does not collide or else we will handle the collision twice.
+	// If A is the same body as B we don't want to collide (1)
+	// If A is dynamic / kinematic and B is static we should collide (2)
+	// If A is dynamic / kinematic and B is dynamic / kinematic we should only collide if
+	//	- A is active and B is not active (3)
+	//	- A is active and B will become active during this simulation step (4)
+	//	- A is active and B is active, we require a condition that makes A, B collide and B, A not (5)
+	//
+	// In order to implement this we use the index in the active body list and make use of the fact that
+	// a body not in the active list has Body.Index = 0xffffffff which is the highest possible value for an uint32.
+	//
+	// Because we know that A is active we know that A.Index != 0xffffffff:
+	// (1) Because A.Index != 0xffffffff, if A.Index = B.Index then A = B, so to collide A.Index != B.Index
+	// (2) A.Index != 0xffffffff, B.Index = 0xffffffff (because it's static and cannot be in the active list), so to collide A.Index != B.Index
+	// (3) A.Index != 0xffffffff, B.Index = 0xffffffff (because it's not yet active), so to collide A.Index != B.Index
+	// (4) A.Index != 0xffffffff, B.Index = 0xffffffff currently. But it can activate during the Broad/NarrowPhase step at which point it
+	//     will be added to the end of the active list which will make B.Index > A.Index (this holds only true when we don't deactivate
+	//     bodies during the Broad/NarrowPhase step), so to collide A.Index < B.Index.
+	// (5) As tie breaker we can use the same condition A.Index < B.Index to collide, this means that if A, B collides then B, A won't
+	static_assert(Body::cInactiveIndex == 0xffffffff, "The algorithm below uses this value");
+	if (!inBody2.IsSoftBody() && body1_index_in_active_bodies >= inBody2.GetIndexInActiveBodiesInternal())
+		return false;
+	JPH_ASSERT(inBody1.GetID() != inBody2.GetID(), "Read the comment above, A and B are the same body which should not be possible!");
+
+	// Check collision group filter
+	if (!inBody1.GetCollisionGroup().CanCollide(inBody2.GetCollisionGroup()))
+		return false;
+
+	return true;
+}
+
+void Body::AddRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime)
+{
+	JPH_ASSERT(IsRigidBody());
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite));
+
+	// This used to use the equation: d/dt R(t) = 1/2 * w(t) * R(t) so that R(t + dt) = R(t) + 1/2 * w(t) * R(t) * dt
+	// See: Appendix B of An Introduction to Physically Based Modeling: Rigid Body Simulation II-Nonpenetration Constraints
+	// URL: https://www.cs.cmu.edu/~baraff/sigcourse/notesd2.pdf
+	// But this is a first order approximation and does not work well for kinematic ragdolls that are driven to a new
+	// pose if the poses differ enough. So now we split w(t) * dt into an axis and angle part and create a quaternion with it.
+	// Note that the resulting quaternion is normalized since otherwise numerical drift will eventually make the rotation non-normalized.
+	float len = inAngularVelocityTimesDeltaTime.Length();
+	if (len > 1.0e-6f)
+	{
+		mRotation = (Quat::sRotation(inAngularVelocityTimesDeltaTime / len, len) * mRotation).Normalized();
+		JPH_ASSERT(!mRotation.IsNaN());
+	}
+}
+
+void Body::SubRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime)
+{
+	JPH_ASSERT(IsRigidBody());
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite));
+
+	// See comment at Body::AddRotationStep
+	float len = inAngularVelocityTimesDeltaTime.Length();
+	if (len > 1.0e-6f)
+	{
+		mRotation = (Quat::sRotation(inAngularVelocityTimesDeltaTime / len, -len) * mRotation).Normalized();
+		JPH_ASSERT(!mRotation.IsNaN());
+	}
+}
+
+Vec3 Body::GetWorldSpaceSurfaceNormal(const SubShapeID &inSubShapeID, RVec3Arg inPosition) const
+{
+	RMat44 inv_com = GetInverseCenterOfMassTransform();
+	return inv_com.Multiply3x3Transposed(mShape->GetSurfaceNormal(inSubShapeID, Vec3(inv_com * inPosition))).Normalized();
+}
+
+Mat44 Body::GetInverseInertia() const
+{
+	JPH_ASSERT(IsDynamic());
+
+	return GetMotionProperties()->GetInverseInertiaForRotation(Mat44::sRotation(mRotation));
+}
+
+void Body::AddForce(Vec3Arg inForce, RVec3Arg inPosition)
+{
+	AddForce(inForce);
+	AddTorque(Vec3(inPosition - mPosition).Cross(inForce));
+}
+
+void Body::AddImpulse(Vec3Arg inImpulse)
+{
+	JPH_ASSERT(IsDynamic());
+
+	SetLinearVelocityClamped(mMotionProperties->GetLinearVelocity() + inImpulse * mMotionProperties->GetInverseMass());
+}
+
+void Body::AddImpulse(Vec3Arg inImpulse, RVec3Arg inPosition)
+{
+	JPH_ASSERT(IsDynamic());
+
+	SetLinearVelocityClamped(mMotionProperties->GetLinearVelocity() + inImpulse * mMotionProperties->GetInverseMass());
+
+	SetAngularVelocityClamped(mMotionProperties->GetAngularVelocity() + mMotionProperties->MultiplyWorldSpaceInverseInertiaByVector(mRotation, Vec3(inPosition - mPosition).Cross(inImpulse)));
+}
+
+void Body::AddAngularImpulse(Vec3Arg inAngularImpulse)
+{
+	JPH_ASSERT(IsDynamic());
+
+	SetAngularVelocityClamped(mMotionProperties->GetAngularVelocity() + mMotionProperties->MultiplyWorldSpaceInverseInertiaByVector(mRotation, inAngularImpulse));
+}
+
+void Body::GetSleepTestPoints(RVec3 *outPoints) const
+{
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read));
+
+	// Center of mass is the first position
+	outPoints[0] = mPosition;
+
+	// The second and third position are on the largest axis of the bounding box
+	Vec3 extent = mShape->GetLocalBounds().GetExtent();
+	int lowest_component = extent.GetLowestComponentIndex();
+	Mat44 rotation = Mat44::sRotation(mRotation);
+	switch (lowest_component)
+	{
+	case 0:
+		outPoints[1] = mPosition + extent.GetY() * rotation.GetColumn3(1);
+		outPoints[2] = mPosition + extent.GetZ() * rotation.GetColumn3(2);
+		break;
+
+	case 1:
+		outPoints[1] = mPosition + extent.GetX() * rotation.GetColumn3(0);
+		outPoints[2] = mPosition + extent.GetZ() * rotation.GetColumn3(2);
+		break;
+
+	case 2:
+		outPoints[1] = mPosition + extent.GetX() * rotation.GetColumn3(0);
+		outPoints[2] = mPosition + extent.GetY() * rotation.GetColumn3(1);
+		break;
+
+	default:
+		JPH_ASSERT(false);
+		break;
+	}
+}
+
+void Body::ResetSleepTimer()
+{
+	RVec3 points[3];
+	GetSleepTestPoints(points);
+	mMotionProperties->ResetSleepTestSpheres(points);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyAccess.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyAccess.h
new file mode 100644
index 0000000000..a7fd6a4ac3
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyAccess.h
@@ -0,0 +1,68 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#ifdef JPH_ENABLE_ASSERTS
+
+JPH_NAMESPACE_BEGIN
+
+class JPH_EXPORT BodyAccess
+{
+public:
+	/// Access rules, used to detect race conditions during simulation
+	enum class EAccess : uint8
+	{
+		None		= 0,
+		Read		= 1,
+		ReadWrite	= 3,
+	};
+
+	/// Grant a scope specific access rights on the current thread
+	class Grant
+	{
+	public:
+		inline							Grant(EAccess inVelocity, EAccess inPosition)
+		{
+			EAccess &velocity = sVelocityAccess();
+			EAccess &position = sPositionAccess();
+
+			JPH_ASSERT(velocity == EAccess::ReadWrite);
+			JPH_ASSERT(position == EAccess::ReadWrite);
+
+			velocity = inVelocity;
+			position = inPosition;
+		}
+
+		inline							~Grant()
+		{
+			sVelocityAccess() = EAccess::ReadWrite;
+			sPositionAccess() = EAccess::ReadWrite;
+		}
+	};
+
+	/// Check if we have permission
+	static inline bool					sCheckRights(EAccess inRights, EAccess inDesiredRights)
+	{
+		return (uint8(inRights) & uint8(inDesiredRights)) == uint8(inDesiredRights);
+	}
+
+	/// Access to read/write velocities
+	static inline EAccess &				sVelocityAccess()
+	{
+		static thread_local EAccess sAccess = BodyAccess::EAccess::ReadWrite;
+		return sAccess;
+	}
+
+	/// Access to read/write positions
+	static inline EAccess &				sPositionAccess()
+	{
+		static thread_local EAccess sAccess = BodyAccess::EAccess::ReadWrite;
+		return sAccess;
+	}
+};
+
+JPH_NAMESPACE_END
+
+#endif // JPH_ENABLE_ASSERTS
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyActivationListener.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyActivationListener.h
new file mode 100644
index 0000000000..2c8808a150
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyActivationListener.h
@@ -0,0 +1,28 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+class BodyID;
+
+/// A listener class that receives events when a body activates or deactivates.
+/// It can be registered with the BodyManager (or PhysicsSystem).
+class BodyActivationListener
+{
+public:
+	/// Ensure virtual destructor
+	virtual					~BodyActivationListener() = default;
+
+	/// Called whenever a body activates, note this can be called from any thread so make sure your code is thread safe.
+	/// At the time of the callback the body inBodyID will be locked and no bodies can be written/activated/deactivated from the callback.
+	virtual void			OnBodyActivated(const BodyID &inBodyID, uint64 inBodyUserData) = 0;
+
+	/// Called whenever a body deactivates, note this can be called from any thread so make sure your code is thread safe.
+	/// At the time of the callback the body inBodyID will be locked and no bodies can be written/activated/deactivated from the callback.
+	virtual void			OnBodyDeactivated(const BodyID &inBodyID, uint64 inBodyUserData) = 0;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyCreationSettings.cpp b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyCreationSettings.cpp
new file mode 100644
index 0000000000..9b6d3f92c4
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyCreationSettings.cpp
@@ -0,0 +1,234 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(BodyCreationSettings)
+{
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mPosition)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mRotation)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mLinearVelocity)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mAngularVelocity)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mUserData)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mShape)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mCollisionGroup)
+	JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mObjectLayer)
+	JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mMotionType)
+	JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mAllowedDOFs)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mAllowDynamicOrKinematic)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mIsSensor)
+	JPH_ADD_ATTRIBUTE_WITH_ALIAS(BodyCreationSettings, mCollideKinematicVsNonDynamic, "mSensorDetectsStatic") // This is the old name to keep backwards compatibility
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mUseManifoldReduction)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mApplyGyroscopicForce)
+	JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mMotionQuality)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mEnhancedInternalEdgeRemoval)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mAllowSleeping)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mFriction)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mRestitution)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mLinearDamping)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mAngularDamping)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mMaxLinearVelocity)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mMaxAngularVelocity)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mGravityFactor)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mNumVelocityStepsOverride)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mNumPositionStepsOverride)
+	JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mOverrideMassProperties)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mInertiaMultiplier)
+	JPH_ADD_ATTRIBUTE(BodyCreationSettings, mMassPropertiesOverride)
+}
+
+void BodyCreationSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(mPosition);
+	inStream.Write(mRotation);
+	inStream.Write(mLinearVelocity);
+	inStream.Write(mAngularVelocity);
+	mCollisionGroup.SaveBinaryState(inStream);
+	inStream.Write(mObjectLayer);
+	inStream.Write(mMotionType);
+	inStream.Write(mAllowedDOFs);
+	inStream.Write(mAllowDynamicOrKinematic);
+	inStream.Write(mIsSensor);
+	inStream.Write(mCollideKinematicVsNonDynamic);
+	inStream.Write(mUseManifoldReduction);
+	inStream.Write(mApplyGyroscopicForce);
+	inStream.Write(mMotionQuality);
+	inStream.Write(mEnhancedInternalEdgeRemoval);
+	inStream.Write(mAllowSleeping);
+	inStream.Write(mFriction);
+	inStream.Write(mRestitution);
+	inStream.Write(mLinearDamping);
+	inStream.Write(mAngularDamping);
+	inStream.Write(mMaxLinearVelocity);
+	inStream.Write(mMaxAngularVelocity);
+	inStream.Write(mGravityFactor);
+	inStream.Write(mNumVelocityStepsOverride);
+	inStream.Write(mNumPositionStepsOverride);
+	inStream.Write(mOverrideMassProperties);
+	inStream.Write(mInertiaMultiplier);
+	mMassPropertiesOverride.SaveBinaryState(inStream);
+}
+
+void BodyCreationSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	inStream.Read(mPosition);
+	inStream.Read(mRotation);
+	inStream.Read(mLinearVelocity);
+	inStream.Read(mAngularVelocity);
+	mCollisionGroup.RestoreBinaryState(inStream);
+	inStream.Read(mObjectLayer);
+	inStream.Read(mMotionType);
+	inStream.Read(mAllowedDOFs);
+	inStream.Read(mAllowDynamicOrKinematic);
+	inStream.Read(mIsSensor);
+	inStream.Read(mCollideKinematicVsNonDynamic);
+	inStream.Read(mUseManifoldReduction);
+	inStream.Read(mApplyGyroscopicForce);
+	inStream.Read(mMotionQuality);
+	inStream.Read(mEnhancedInternalEdgeRemoval);
+	inStream.Read(mAllowSleeping);
+	inStream.Read(mFriction);
+	inStream.Read(mRestitution);
+	inStream.Read(mLinearDamping);
+	inStream.Read(mAngularDamping);
+	inStream.Read(mMaxLinearVelocity);
+	inStream.Read(mMaxAngularVelocity);
+	inStream.Read(mGravityFactor);
+	inStream.Read(mNumVelocityStepsOverride);
+	inStream.Read(mNumPositionStepsOverride);
+	inStream.Read(mOverrideMassProperties);
+	inStream.Read(mInertiaMultiplier);
+	mMassPropertiesOverride.RestoreBinaryState(inStream);
+}
+
+Shape::ShapeResult BodyCreationSettings::ConvertShapeSettings()
+{
+	// If we already have a shape, return it
+	if (mShapePtr != nullptr)
+	{
+		mShape = nullptr;
+
+		Shape::ShapeResult result;
+		result.Set(const_cast<Shape *>(mShapePtr.GetPtr()));
+		return result;
+	}
+
+	// Check if we have shape settings
+	if (mShape == nullptr)
+	{
+		Shape::ShapeResult result;
+		result.SetError("No shape present!");
+		return result;
+	}
+
+	// Create the shape
+	Shape::ShapeResult result = mShape->Create();
+	if (result.IsValid())
+		mShapePtr = result.Get();
+	mShape = nullptr;
+	return result;
+}
+
+const Shape *BodyCreationSettings::GetShape() const
+{
+	// If we already have a shape, return it
+	if (mShapePtr != nullptr)
+		return mShapePtr;
+
+	// Check if we have shape settings
+	if (mShape == nullptr)
+		return nullptr;
+
+	// Create the shape
+	Shape::ShapeResult result = mShape->Create();
+	if (result.IsValid())
+		return result.Get();
+
+	Trace("Error: %s", result.GetError().c_str());
+	JPH_ASSERT(false, "An error occurred during shape creation. Use ConvertShapeSettings() to convert the shape and get the error!");
+	return nullptr;
+}
+
+MassProperties BodyCreationSettings::GetMassProperties() const
+{
+	// Calculate mass properties
+	MassProperties mass_properties;
+	switch (mOverrideMassProperties)
+	{
+	case EOverrideMassProperties::CalculateMassAndInertia:
+		mass_properties = GetShape()->GetMassProperties();
+		mass_properties.mInertia *= mInertiaMultiplier;
+		mass_properties.mInertia(3, 3) = 1.0f;
+		break;
+	case EOverrideMassProperties::CalculateInertia:
+		mass_properties = GetShape()->GetMassProperties();
+		mass_properties.ScaleToMass(mMassPropertiesOverride.mMass);
+		mass_properties.mInertia *= mInertiaMultiplier;
+		mass_properties.mInertia(3, 3) = 1.0f;
+		break;
+	case EOverrideMassProperties::MassAndInertiaProvided:
+		mass_properties = mMassPropertiesOverride;
+		break;
+	}
+	return mass_properties;
+}
+
+void BodyCreationSettings::SaveWithChildren(StreamOut &inStream, ShapeToIDMap *ioShapeMap, MaterialToIDMap *ioMaterialMap, GroupFilterToIDMap *ioGroupFilterMap) const
+{
+	// Save creation settings
+	SaveBinaryState(inStream);
+
+	// Save shape
+	if (ioShapeMap != nullptr && ioMaterialMap != nullptr)
+		GetShape()->SaveWithChildren(inStream, *ioShapeMap, *ioMaterialMap);
+	else
+		inStream.Write(~uint32(0));
+
+	// Save group filter
+	StreamUtils::SaveObjectReference(inStream, mCollisionGroup.GetGroupFilter(), ioGroupFilterMap);
+}
+
+BodyCreationSettings::BCSResult BodyCreationSettings::sRestoreWithChildren(StreamIn &inStream, IDToShapeMap &ioShapeMap, IDToMaterialMap &ioMaterialMap, IDToGroupFilterMap &ioGroupFilterMap)
+{
+	BCSResult result;
+
+	// Read creation settings
+	BodyCreationSettings settings;
+	settings.RestoreBinaryState(inStream);
+	if (inStream.IsEOF() || inStream.IsFailed())
+	{
+		result.SetError("Error reading body creation settings");
+		return result;
+	}
+
+	// Read shape
+	Shape::ShapeResult shape_result = Shape::sRestoreWithChildren(inStream, ioShapeMap, ioMaterialMap);
+	if (shape_result.HasError())
+	{
+		result.SetError(shape_result.GetError());
+		return result;
+	}
+	settings.SetShape(shape_result.Get());
+
+	// Read group filter
+	Result gfresult = StreamUtils::RestoreObjectReference(inStream, ioGroupFilterMap);
+	if (gfresult.HasError())
+	{
+		result.SetError(gfresult.GetError());
+		return result;
+	}
+	settings.mCollisionGroup.SetGroupFilter(gfresult.Get());
+
+	result.Set(settings);
+	return result;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyCreationSettings.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyCreationSettings.h
new file mode 100644
index 0000000000..8949b2ec3e
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyCreationSettings.h
@@ -0,0 +1,124 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+#include <Jolt/Physics/Collision/ObjectLayer.h>
+#include <Jolt/Physics/Collision/CollisionGroup.h>
+#include <Jolt/Physics/Body/MotionType.h>
+#include <Jolt/Physics/Body/MotionQuality.h>
+#include <Jolt/Physics/Body/AllowedDOFs.h>
+#include <Jolt/ObjectStream/SerializableObject.h>
+#include <Jolt/Core/StreamUtils.h>
+
+JPH_NAMESPACE_BEGIN
+
+class StreamIn;
+class StreamOut;
+
+/// Enum used in BodyCreationSettings to indicate how mass and inertia should be calculated
+enum class EOverrideMassProperties : uint8
+{
+	CalculateMassAndInertia,			///< Tells the system to calculate the mass and inertia based on density
+	CalculateInertia,					///< Tells the system to take the mass from mMassPropertiesOverride and to calculate the inertia based on density of the shapes and to scale it to the provided mass
+	MassAndInertiaProvided				///< Tells the system to take the mass and inertia from mMassPropertiesOverride
+};
+
+/// Settings for constructing a rigid body
+class JPH_EXPORT BodyCreationSettings
+{
+	JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, BodyCreationSettings)
+
+public:
+	/// Constructor
+							BodyCreationSettings() = default;
+							BodyCreationSettings(const ShapeSettings *inShape, RVec3Arg inPosition, QuatArg inRotation, EMotionType inMotionType, ObjectLayer inObjectLayer) : mPosition(inPosition), mRotation(inRotation), mObjectLayer(inObjectLayer), mMotionType(inMotionType), mShape(inShape) { }
+							BodyCreationSettings(const Shape *inShape, RVec3Arg inPosition, QuatArg inRotation, EMotionType inMotionType, ObjectLayer inObjectLayer) : mPosition(inPosition), mRotation(inRotation), mObjectLayer(inObjectLayer), mMotionType(inMotionType), mShapePtr(inShape) { }
+
+	/// Access to the shape settings object. This contains serializable (non-runtime optimized) information about the Shape.
+	const ShapeSettings *	GetShapeSettings() const										{ return mShape; }
+	void					SetShapeSettings(const ShapeSettings *inShape)					{ mShape = inShape; mShapePtr = nullptr; }
+
+	/// Convert ShapeSettings object into a Shape object. This will free the ShapeSettings object and make the object ready for runtime. Serialization is no longer possible after this.
+	Shape::ShapeResult		ConvertShapeSettings();
+
+	/// Access to the run-time shape object. Will convert from ShapeSettings object if needed.
+	const Shape *			GetShape() const;
+	void					SetShape(const Shape *inShape)									{ mShapePtr = inShape; mShape = nullptr; }
+
+	/// Check if the mass properties of this body will be calculated (only relevant for kinematic or dynamic objects that need a MotionProperties object)
+	bool					HasMassProperties() const										{ return mAllowDynamicOrKinematic || mMotionType != EMotionType::Static; }
+
+	/// Calculate (or return when overridden) the mass and inertia for this body
+	MassProperties			GetMassProperties() const;
+
+	/// Saves the state of this object in binary form to inStream. Doesn't store the shape nor the group filter.
+	void					SaveBinaryState(StreamOut &inStream) const;
+
+	/// Restore the state of this object from inStream. Doesn't restore the shape nor the group filter.
+	void					RestoreBinaryState(StreamIn &inStream);
+
+	using GroupFilterToIDMap = StreamUtils::ObjectToIDMap<GroupFilter>;
+	using IDToGroupFilterMap = StreamUtils::IDToObjectMap<GroupFilter>;
+	using ShapeToIDMap = Shape::ShapeToIDMap;
+	using IDToShapeMap = Shape::IDToShapeMap;
+	using MaterialToIDMap = StreamUtils::ObjectToIDMap<PhysicsMaterial>;
+	using IDToMaterialMap = StreamUtils::IDToObjectMap<PhysicsMaterial>;
+
+	/// Save body creation settings, its shape, materials and group filter. Pass in an empty map in ioShapeMap / ioMaterialMap / ioGroupFilterMap or reuse the same map while saving multiple shapes to the same stream in order to avoid writing duplicates.
+	/// Pass nullptr to ioShapeMap and ioMaterial map to skip saving shapes
+	/// Pass nullptr to ioGroupFilterMap to skip saving group filters
+	void					SaveWithChildren(StreamOut &inStream, ShapeToIDMap *ioShapeMap, MaterialToIDMap *ioMaterialMap, GroupFilterToIDMap *ioGroupFilterMap) const;
+
+	using BCSResult = Result<BodyCreationSettings>;
+
+	/// Restore body creation settings, its shape, materials and group filter. Pass in an empty map in ioShapeMap / ioMaterialMap / ioGroupFilterMap or reuse the same map while reading multiple shapes from the same stream in order to restore duplicates.
+	static BCSResult		sRestoreWithChildren(StreamIn &inStream, IDToShapeMap &ioShapeMap, IDToMaterialMap &ioMaterialMap, IDToGroupFilterMap &ioGroupFilterMap);
+
+	RVec3					mPosition = RVec3::sZero();										///< Position of the body (not of the center of mass)
+	Quat					mRotation = Quat::sIdentity();									///< Rotation of the body
+	Vec3					mLinearVelocity = Vec3::sZero();								///< World space linear velocity of the center of mass (m/s)
+	Vec3					mAngularVelocity = Vec3::sZero();								///< World space angular velocity (rad/s)
+
+	/// User data value (can be used by application)
+	uint64					mUserData = 0;
+
+	///@name Collision settings
+	ObjectLayer				mObjectLayer = 0;												///< The collision layer this body belongs to (determines if two objects can collide)
+	CollisionGroup			mCollisionGroup;												///< The collision group this body belongs to (determines if two objects can collide)
+
+	///@name Simulation properties
+	EMotionType				mMotionType = EMotionType::Dynamic;								///< Motion type, determines if the object is static, dynamic or kinematic
+	EAllowedDOFs			mAllowedDOFs = EAllowedDOFs::All;								///< Which degrees of freedom this body has (can be used to limit simulation to 2D)
+	bool					mAllowDynamicOrKinematic = false;								///< When this body is created as static, this setting tells the system to create a MotionProperties object so that the object can be switched to kinematic or dynamic
+	bool					mIsSensor = false;												///< If this body is a sensor. A sensor will receive collision callbacks, but will not cause any collision responses and can be used as a trigger volume. See description at Body::SetIsSensor.
+	bool					mCollideKinematicVsNonDynamic = false;							///< If kinematic objects can generate contact points against other kinematic or static objects. See description at Body::SetCollideKinematicVsNonDynamic.
+	bool					mUseManifoldReduction = true;									///< If this body should use manifold reduction (see description at Body::SetUseManifoldReduction)
+	bool					mApplyGyroscopicForce = false;									///< Set to indicate that the gyroscopic force should be applied to this body (aka Dzhanibekov effect, see https://en.wikipedia.org/wiki/Tennis_racket_theorem)
+	EMotionQuality			mMotionQuality = EMotionQuality::Discrete;						///< Motion quality, or how well it detects collisions when it has a high velocity
+	bool					mEnhancedInternalEdgeRemoval = false;							///< Set to indicate that extra effort should be made to try to remove ghost contacts (collisions with internal edges of a mesh). This is more expensive but makes bodies move smoother over a mesh with convex edges.
+	bool					mAllowSleeping = true;											///< If this body can go to sleep or not
+	float					mFriction = 0.2f;												///< Friction of the body (dimensionless number, usually between 0 and 1, 0 = no friction, 1 = friction force equals force that presses the two bodies together). Note that bodies can have negative friction but the combined friction (see PhysicsSystem::SetCombineFriction) should never go below zero.
+	float					mRestitution = 0.0f;											///< Restitution of body (dimensionless number, usually between 0 and 1, 0 = completely inelastic collision response, 1 = completely elastic collision response). Note that bodies can have negative restitution but the combined restitution (see PhysicsSystem::SetCombineRestitution) should never go below zero.
+	float					mLinearDamping = 0.05f;											///< Linear damping: dv/dt = -c * v. c must be between 0 and 1 but is usually close to 0.
+	float					mAngularDamping = 0.05f;										///< Angular damping: dw/dt = -c * w. c must be between 0 and 1 but is usually close to 0.
+	float					mMaxLinearVelocity = 500.0f;									///< Maximum linear velocity that this body can reach (m/s)
+	float					mMaxAngularVelocity = 0.25f * JPH_PI * 60.0f;					///< Maximum angular velocity that this body can reach (rad/s)
+	float					mGravityFactor = 1.0f;											///< Value to multiply gravity with for this body
+	uint					mNumVelocityStepsOverride = 0;									///< Used only when this body is dynamic and colliding. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island.
+	uint					mNumPositionStepsOverride = 0;									///< Used only when this body is dynamic and colliding. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island.
+
+	///@name Mass properties of the body (by default calculated by the shape)
+	EOverrideMassProperties	mOverrideMassProperties = EOverrideMassProperties::CalculateMassAndInertia; ///< Determines how mMassPropertiesOverride will be used
+	float					mInertiaMultiplier = 1.0f;										///< When calculating the inertia (not when it is provided) the calculated inertia will be multiplied by this value
+	MassProperties			mMassPropertiesOverride;										///< Contains replacement mass settings which override the automatically calculated values
+
+private:
+	/// Collision volume for the body
+	RefConst<ShapeSettings>	mShape;															///< Shape settings, can be serialized. Mutually exclusive with mShapePtr
+	RefConst<Shape>			mShapePtr;														///< Actual shape, cannot be serialized. Mutually exclusive with mShape
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyFilter.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyFilter.h
new file mode 100644
index 0000000000..111d514018
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyFilter.h
@@ -0,0 +1,130 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/NonCopyable.h>
+#include <Jolt/Physics/Body/BodyID.h>
+
+JPH_NAMESPACE_BEGIN
+
+class Body;
+
+/// Class function to filter out bodies, returns true if test should collide with body
+class JPH_EXPORT BodyFilter : public NonCopyable
+{
+public:
+	/// Destructor
+	virtual					~BodyFilter() = default;
+
+	/// Filter function. Returns true if we should collide with inBodyID
+	virtual bool			ShouldCollide([[maybe_unused]] const BodyID &inBodyID) const
+	{
+		return true;
+	}
+
+	/// Filter function. Returns true if we should collide with inBody (this is called after the body is locked and makes it possible to filter based on body members)
+	virtual bool			ShouldCollideLocked([[maybe_unused]] const Body &inBody) const
+	{
+		return true;
+	}
+};
+
+/// A simple body filter implementation that ignores a single, specified body
+class JPH_EXPORT IgnoreSingleBodyFilter : public BodyFilter
+{
+public:
+	/// Constructor, pass the body you want to ignore
+	explicit				IgnoreSingleBodyFilter(const BodyID &inBodyID) :
+		mBodyID(inBodyID)
+	{
+	}
+
+	/// Filter function. Returns true if we should collide with inBodyID
+	virtual bool			ShouldCollide(const BodyID &inBodyID) const override
+	{
+		return mBodyID != inBodyID;
+	}
+
+private:
+	BodyID					mBodyID;
+};
+
+/// A simple body filter implementation that ignores multiple, specified bodies
+class JPH_EXPORT IgnoreMultipleBodiesFilter : public BodyFilter
+{
+public:
+	/// Remove all bodies from the filter
+	void					Clear()
+	{
+		mBodyIDs.clear();
+	}
+
+	/// Reserve space for inSize body ID's
+	void					Reserve(uint inSize)
+	{
+		mBodyIDs.reserve(inSize);
+	}
+
+	/// Add a body to be ignored
+	void					IgnoreBody(const BodyID &inBodyID)
+	{
+		mBodyIDs.push_back(inBodyID);
+	}
+
+	/// Filter function. Returns true if we should collide with inBodyID
+	virtual bool			ShouldCollide(const BodyID &inBodyID) const override
+	{
+		return std::find(mBodyIDs.begin(), mBodyIDs.end(), inBodyID) == mBodyIDs.end();
+	}
+
+private:
+	Array<BodyID>			mBodyIDs;
+};
+
+/// Ignores a single body and chains the filter to another filter
+class JPH_EXPORT IgnoreSingleBodyFilterChained : public BodyFilter
+{
+public:
+	/// Constructor
+	explicit				IgnoreSingleBodyFilterChained(const BodyID inBodyID, const BodyFilter &inFilter) :
+		mBodyID(inBodyID),
+		mFilter(inFilter)
+	{
+	}
+
+	/// Filter function. Returns true if we should collide with inBodyID
+	virtual bool			ShouldCollide(const BodyID &inBodyID) const override
+	{
+		return inBodyID != mBodyID && mFilter.ShouldCollide(inBodyID);
+	}
+
+	/// Filter function. Returns true if we should collide with inBody (this is called after the body is locked and makes it possible to filter based on body members)
+	virtual bool			ShouldCollideLocked(const Body &inBody) const override
+	{
+		return mFilter.ShouldCollideLocked(inBody);
+	}
+
+private:
+	BodyID					mBodyID;
+	const BodyFilter &		mFilter;
+};
+
+#ifdef JPH_DEBUG_RENDERER
+/// Class function to filter out bodies for debug rendering, returns true if body should be rendered
+class JPH_EXPORT BodyDrawFilter : public NonCopyable
+{
+public:
+	/// Destructor
+	virtual					~BodyDrawFilter() = default;
+
+	/// Filter function. Returns true if inBody should be rendered
+	virtual bool			ShouldDraw([[maybe_unused]] const Body& inBody) const
+	{
+		return true;
+	}
+};
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyID.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyID.h
new file mode 100644
index 0000000000..d2bbaccff7
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyID.h
@@ -0,0 +1,100 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/HashCombine.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// ID of a body. This is a way of reasoning about bodies in a multithreaded simulation while avoiding race conditions.
+class BodyID
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	static constexpr uint32	cInvalidBodyID = 0xffffffff;	///< The value for an invalid body ID
+	static constexpr uint32	cBroadPhaseBit = 0x00800000;	///< This bit is used by the broadphase
+	static constexpr uint32	cMaxBodyIndex = 0x7fffff;		///< Maximum value for body index (also the maximum amount of bodies supported - 1)
+	static constexpr uint8	cMaxSequenceNumber = 0xff;		///< Maximum value for the sequence number
+
+	/// Construct invalid body ID
+							BodyID() :
+		mID(cInvalidBodyID)
+	{
+	}
+
+	/// Construct from index and sequence number combined in a single uint32 (use with care!)
+	explicit				BodyID(uint32 inID) :
+		mID(inID)
+	{
+		JPH_ASSERT((inID & cBroadPhaseBit) == 0 || inID == cInvalidBodyID); // Check bit used by broadphase
+	}
+
+	/// Construct from index and sequence number
+	explicit				BodyID(uint32 inID, uint8 inSequenceNumber) :
+		mID((uint32(inSequenceNumber) << 24) | inID)
+	{
+		JPH_ASSERT(inID < cMaxBodyIndex); // Should not use bit pattern for invalid ID and should not use the broadphase bit
+	}
+
+	/// Get index in body array
+	inline uint32			GetIndex() const
+	{
+		return mID & cMaxBodyIndex;
+	}
+
+	/// Get sequence number of body.
+	/// The sequence number can be used to check if a body ID with the same body index has been reused by another body.
+	/// It is mainly used in multi threaded situations where a body is removed and its body index is immediately reused by a body created from another thread.
+	/// Functions querying the broadphase can (after acquiring a body lock) detect that the body has been removed (we assume that this won't happen more than 128 times in a row).
+	inline uint8			GetSequenceNumber() const
+	{
+		return uint8(mID >> 24);
+	}
+
+	/// Returns the index and sequence number combined in an uint32
+	inline uint32			GetIndexAndSequenceNumber() const
+	{
+		return mID;
+	}
+
+	/// Check if the ID is valid
+	inline bool				IsInvalid() const
+	{
+		return mID == cInvalidBodyID;
+	}
+
+	/// Equals check
+	inline bool				operator == (const BodyID &inRHS) const
+	{
+		return mID == inRHS.mID;
+	}
+
+	/// Not equals check
+	inline bool				operator != (const BodyID &inRHS) const
+	{
+		return mID != inRHS.mID;
+	}
+
+	/// Smaller than operator, can be used for sorting bodies
+	inline bool				operator < (const BodyID &inRHS) const
+	{
+		return mID < inRHS.mID;
+	}
+
+	/// Greater than operator, can be used for sorting bodies
+	inline bool				operator > (const BodyID &inRHS) const
+	{
+		return mID > inRHS.mID;
+	}
+
+private:
+	uint32					mID;
+};
+
+JPH_NAMESPACE_END
+
+// Create a std::hash/JPH::Hash for BodyID
+JPH_MAKE_HASHABLE(JPH::BodyID, t.GetIndexAndSequenceNumber())
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.cpp b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.cpp
new file mode 100644
index 0000000000..cc5f27de41
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.cpp
@@ -0,0 +1,1034 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Body/BodyInterface.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhase.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Physics/Body/BodyManager.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Physics/Body/BodyLock.h>
+#include <Jolt/Physics/Body/BodyLockMulti.h>
+#include <Jolt/Physics/Collision/PhysicsMaterial.h>
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+
+JPH_NAMESPACE_BEGIN
+
+void BodyInterface::ActivateBodyInternal(Body &ioBody) const
+{
+	// Activate body or reset its sleep timer.
+	// Note that BodyManager::ActivateBodies also resets the sleep timer internally, but we avoid a mutex lock if the body is already active by calling ResetSleepTimer directly.
+	if (!ioBody.IsActive())
+		mBodyManager->ActivateBodies(&ioBody.GetID(), 1);
+	else
+		ioBody.ResetSleepTimer();
+}
+
+Body *BodyInterface::CreateBody(const BodyCreationSettings &inSettings)
+{
+	Body *body = mBodyManager->AllocateBody(inSettings);
+	if (!mBodyManager->AddBody(body))
+	{
+		mBodyManager->FreeBody(body);
+		return nullptr;
+	}
+	return body;
+}
+
+Body *BodyInterface::CreateSoftBody(const SoftBodyCreationSettings &inSettings)
+{
+	Body *body = mBodyManager->AllocateSoftBody(inSettings);
+	if (!mBodyManager->AddBody(body))
+	{
+		mBodyManager->FreeBody(body);
+		return nullptr;
+	}
+	return body;
+}
+
+Body *BodyInterface::CreateBodyWithID(const BodyID &inBodyID, const BodyCreationSettings &inSettings)
+{
+	Body *body = mBodyManager->AllocateBody(inSettings);
+	if (!mBodyManager->AddBodyWithCustomID(body, inBodyID))
+	{
+		mBodyManager->FreeBody(body);
+		return nullptr;
+	}
+	return body;
+}
+
+Body *BodyInterface::CreateSoftBodyWithID(const BodyID &inBodyID, const SoftBodyCreationSettings &inSettings)
+{
+	Body *body = mBodyManager->AllocateSoftBody(inSettings);
+	if (!mBodyManager->AddBodyWithCustomID(body, inBodyID))
+	{
+		mBodyManager->FreeBody(body);
+		return nullptr;
+	}
+	return body;
+}
+
+Body *BodyInterface::CreateBodyWithoutID(const BodyCreationSettings &inSettings) const
+{
+	return mBodyManager->AllocateBody(inSettings);
+}
+
+Body *BodyInterface::CreateSoftBodyWithoutID(const SoftBodyCreationSettings &inSettings) const
+{
+	return mBodyManager->AllocateSoftBody(inSettings);
+}
+
+void BodyInterface::DestroyBodyWithoutID(Body *inBody) const
+{
+	mBodyManager->FreeBody(inBody);
+}
+
+bool BodyInterface::AssignBodyID(Body *ioBody)
+{
+	return mBodyManager->AddBody(ioBody);
+}
+
+bool BodyInterface::AssignBodyID(Body *ioBody, const BodyID &inBodyID)
+{
+	return mBodyManager->AddBodyWithCustomID(ioBody, inBodyID);
+}
+
+Body *BodyInterface::UnassignBodyID(const BodyID &inBodyID)
+{
+	Body *body = nullptr;
+	mBodyManager->RemoveBodies(&inBodyID, 1, &body);
+	return body;
+}
+
+void BodyInterface::UnassignBodyIDs(const BodyID *inBodyIDs, int inNumber, Body **outBodies)
+{
+	mBodyManager->RemoveBodies(inBodyIDs, inNumber, outBodies);
+}
+
+void BodyInterface::DestroyBody(const BodyID &inBodyID)
+{
+	mBodyManager->DestroyBodies(&inBodyID, 1);
+}
+
+void BodyInterface::DestroyBodies(const BodyID *inBodyIDs, int inNumber)
+{
+	mBodyManager->DestroyBodies(inBodyIDs, inNumber);
+}
+
+void BodyInterface::AddBody(const BodyID &inBodyID, EActivation inActivationMode)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		const Body &body = lock.GetBody();
+
+		// Add to broadphase
+		BodyID id = inBodyID;
+		BroadPhase::AddState add_state = mBroadPhase->AddBodiesPrepare(&id, 1);
+		mBroadPhase->AddBodiesFinalize(&id, 1, add_state);
+
+		// Optionally activate body
+		if (inActivationMode == EActivation::Activate && !body.IsStatic())
+			mBodyManager->ActivateBodies(&inBodyID, 1);
+	}
+}
+
+void BodyInterface::RemoveBody(const BodyID &inBodyID)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		const Body &body = lock.GetBody();
+
+		// Deactivate body
+		if (body.IsActive())
+			mBodyManager->DeactivateBodies(&inBodyID, 1);
+
+		// Remove from broadphase
+		BodyID id = inBodyID;
+		mBroadPhase->RemoveBodies(&id, 1);
+	}
+}
+
+bool BodyInterface::IsAdded(const BodyID &inBodyID) const
+{
+	BodyLockRead lock(*mBodyLockInterface, inBodyID);
+	return lock.SucceededAndIsInBroadPhase();
+}
+
+BodyID BodyInterface::CreateAndAddBody(const BodyCreationSettings &inSettings, EActivation inActivationMode)
+{
+	const Body *b = CreateBody(inSettings);
+	if (b == nullptr)
+		return BodyID(); // Out of bodies
+	AddBody(b->GetID(), inActivationMode);
+	return b->GetID();
+}
+
+BodyID BodyInterface::CreateAndAddSoftBody(const SoftBodyCreationSettings &inSettings, EActivation inActivationMode)
+{
+	const Body *b = CreateSoftBody(inSettings);
+	if (b == nullptr)
+		return BodyID(); // Out of bodies
+	AddBody(b->GetID(), inActivationMode);
+	return b->GetID();
+}
+
+BodyInterface::AddState BodyInterface::AddBodiesPrepare(BodyID *ioBodies, int inNumber)
+{
+	return mBroadPhase->AddBodiesPrepare(ioBodies, inNumber);
+}
+
+void BodyInterface::AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState, EActivation inActivationMode)
+{
+	BodyLockMultiWrite lock(*mBodyLockInterface, ioBodies, inNumber);
+
+	// Add to broadphase
+	mBroadPhase->AddBodiesFinalize(ioBodies, inNumber, inAddState);
+
+	// Optionally activate bodies
+	if (inActivationMode == EActivation::Activate)
+		mBodyManager->ActivateBodies(ioBodies, inNumber);
+}
+
+void BodyInterface::AddBodiesAbort(BodyID *ioBodies, int inNumber, AddState inAddState)
+{
+	mBroadPhase->AddBodiesAbort(ioBodies, inNumber, inAddState);
+}
+
+void BodyInterface::RemoveBodies(BodyID *ioBodies, int inNumber)
+{
+	BodyLockMultiWrite lock(*mBodyLockInterface, ioBodies, inNumber);
+
+	// Deactivate bodies
+	mBodyManager->DeactivateBodies(ioBodies, inNumber);
+
+	// Remove from broadphase
+	mBroadPhase->RemoveBodies(ioBodies, inNumber);
+}
+
+void BodyInterface::ActivateBody(const BodyID &inBodyID)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		Body &body = lock.GetBody();
+		ActivateBodyInternal(body);
+	}
+}
+
+void BodyInterface::ActivateBodies(const BodyID *inBodyIDs, int inNumber)
+{
+	BodyLockMultiWrite lock(*mBodyLockInterface, inBodyIDs, inNumber);
+
+	mBodyManager->ActivateBodies(inBodyIDs, inNumber);
+}
+
+void BodyInterface::ActivateBodiesInAABox(const AABox &inBox, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter)
+{
+	AllHitCollisionCollector<CollideShapeBodyCollector> collector;
+	mBroadPhase->CollideAABox(inBox, collector, inBroadPhaseLayerFilter, inObjectLayerFilter);
+	ActivateBodies(collector.mHits.data(), (int)collector.mHits.size());
+}
+
+void BodyInterface::DeactivateBody(const BodyID &inBodyID)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		const Body &body = lock.GetBody();
+
+		if (body.IsActive())
+			mBodyManager->DeactivateBodies(&inBodyID, 1);
+	}
+}
+
+void BodyInterface::DeactivateBodies(const BodyID *inBodyIDs, int inNumber)
+{
+	BodyLockMultiWrite lock(*mBodyLockInterface, inBodyIDs, inNumber);
+
+	mBodyManager->DeactivateBodies(inBodyIDs, inNumber);
+}
+
+bool BodyInterface::IsActive(const BodyID &inBodyID) const
+{
+	BodyLockRead lock(*mBodyLockInterface, inBodyID);
+	return lock.Succeeded() && lock.GetBody().IsActive();
+}
+
+void BodyInterface::ResetSleepTimer(const BodyID &inBodyID)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+		lock.GetBody().ResetSleepTimer();
+}
+
+TwoBodyConstraint *BodyInterface::CreateConstraint(const TwoBodyConstraintSettings *inSettings, const BodyID &inBodyID1, const BodyID &inBodyID2)
+{
+	BodyID constraint_bodies[] = { inBodyID1, inBodyID2 };
+	BodyLockMultiWrite lock(*mBodyLockInterface, constraint_bodies, 2);
+
+	Body *body1 = lock.GetBody(0);
+	Body *body2 = lock.GetBody(1);
+
+	JPH_ASSERT(body1 != body2);
+	JPH_ASSERT(body1 != nullptr || body2 != nullptr);
+
+	return inSettings->Create(body1 != nullptr? *body1 : Body::sFixedToWorld, body2 != nullptr? *body2 : Body::sFixedToWorld);
+}
+
+void BodyInterface::ActivateConstraint(const TwoBodyConstraint *inConstraint)
+{
+	BodyID bodies[] = { inConstraint->GetBody1()->GetID(), inConstraint->GetBody2()->GetID() };
+	ActivateBodies(bodies, 2);
+}
+
+RefConst<Shape> BodyInterface::GetShape(const BodyID &inBodyID) const
+{
+	RefConst<Shape> shape;
+	BodyLockRead lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+		shape = lock.GetBody().GetShape();
+	return shape;
+}
+
+void BodyInterface::SetShape(const BodyID &inBodyID, const Shape *inShape, bool inUpdateMassProperties, EActivation inActivationMode) const
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		Body &body = lock.GetBody();
+
+		// Check if shape actually changed
+		if (body.GetShape() != inShape)
+		{
+			// Update the shape
+			body.SetShapeInternal(inShape, inUpdateMassProperties);
+
+			// Flag collision cache invalid for this body
+			mBodyManager->InvalidateContactCacheForBody(body);
+
+			// Notify broadphase of change
+			if (body.IsInBroadPhase())
+			{
+				BodyID id = body.GetID();
+				mBroadPhase->NotifyBodiesAABBChanged(&id, 1);
+			}
+
+			// Optionally activate body
+			if (inActivationMode == EActivation::Activate && !body.IsStatic())
+				ActivateBodyInternal(body);
+		}
+	}
+}
+
+void BodyInterface::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inPreviousCenterOfMass, bool inUpdateMassProperties, EActivation inActivationMode) const
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		Body &body = lock.GetBody();
+
+		// Update center of mass, mass and inertia
+		body.UpdateCenterOfMassInternal(inPreviousCenterOfMass, inUpdateMassProperties);
+
+		// Recalculate bounding box
+		body.CalculateWorldSpaceBoundsInternal();
+
+		// Flag collision cache invalid for this body
+		mBodyManager->InvalidateContactCacheForBody(body);
+
+		// Notify broadphase of change
+		if (body.IsInBroadPhase())
+		{
+			BodyID id = body.GetID();
+			mBroadPhase->NotifyBodiesAABBChanged(&id, 1);
+		}
+
+		// Optionally activate body
+		if (inActivationMode == EActivation::Activate && !body.IsStatic())
+			ActivateBodyInternal(body);
+	}
+}
+
+void BodyInterface::SetObjectLayer(const BodyID &inBodyID, ObjectLayer inLayer)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		Body &body = lock.GetBody();
+
+		// Check if layer actually changed, updating the broadphase is rather expensive
+		if (body.GetObjectLayer() != inLayer)
+		{
+			// Update the layer on the body
+			mBodyManager->SetBodyObjectLayerInternal(body, inLayer);
+
+			// Notify broadphase of change
+			if (body.IsInBroadPhase())
+			{
+				BodyID id = body.GetID();
+				mBroadPhase->NotifyBodiesLayerChanged(&id, 1);
+			}
+		}
+	}
+}
+
+ObjectLayer BodyInterface::GetObjectLayer(const BodyID &inBodyID) const
+{
+	BodyLockRead lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+		return lock.GetBody().GetObjectLayer();
+	else
+		return cObjectLayerInvalid;
+}
+
+void BodyInterface::SetPositionAndRotation(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		Body &body = lock.GetBody();
+
+		// Update the position
+		body.SetPositionAndRotationInternal(inPosition, inRotation);
+
+		// Notify broadphase of change
+		if (body.IsInBroadPhase())
+		{
+			BodyID id = body.GetID();
+			mBroadPhase->NotifyBodiesAABBChanged(&id, 1);
+		}
+
+		// Optionally activate body
+		if (inActivationMode == EActivation::Activate && !body.IsStatic())
+			ActivateBodyInternal(body);
+	}
+}
+
+void BodyInterface::SetPositionAndRotationWhenChanged(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		Body &body = lock.GetBody();
+
+		// Check if there is enough change
+		if (!body.GetPosition().IsClose(inPosition)
+			|| !body.GetRotation().IsClose(inRotation))
+		{
+			// Update the position
+			body.SetPositionAndRotationInternal(inPosition, inRotation);
+
+			// Notify broadphase of change
+			if (body.IsInBroadPhase())
+			{
+				BodyID id = body.GetID();
+				mBroadPhase->NotifyBodiesAABBChanged(&id, 1);
+			}
+
+			// Optionally activate body
+			if (inActivationMode == EActivation::Activate && !body.IsStatic())
+				ActivateBodyInternal(body);
+		}
+	}
+}
+
+void BodyInterface::GetPositionAndRotation(const BodyID &inBodyID, RVec3 &outPosition, Quat &outRotation) const
+{
+	BodyLockRead lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		const Body &body = lock.GetBody();
+		outPosition = body.GetPosition();
+		outRotation = body.GetRotation();
+	}
+	else
+	{
+		outPosition = RVec3::sZero();
+		outRotation = Quat::sIdentity();
+	}
+}
+
+void BodyInterface::SetPosition(const BodyID &inBodyID, RVec3Arg inPosition, EActivation inActivationMode)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		Body &body = lock.GetBody();
+
+		// Update the position
+		body.SetPositionAndRotationInternal(inPosition, body.GetRotation());
+
+		// Notify broadphase of change
+		if (body.IsInBroadPhase())
+		{
+			BodyID id = body.GetID();
+			mBroadPhase->NotifyBodiesAABBChanged(&id, 1);
+		}
+
+		// Optionally activate body
+		if (inActivationMode == EActivation::Activate && !body.IsStatic())
+			ActivateBodyInternal(body);
+	}
+}
+
+RVec3 BodyInterface::GetPosition(const BodyID &inBodyID) const
+{
+	BodyLockRead lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+		return lock.GetBody().GetPosition();
+	else
+		return RVec3::sZero();
+}
+
+RVec3 BodyInterface::GetCenterOfMassPosition(const BodyID &inBodyID) const
+{
+	BodyLockRead lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+		return lock.GetBody().GetCenterOfMassPosition();
+	else
+		return RVec3::sZero();
+}
+
+void BodyInterface::SetRotation(const BodyID &inBodyID, QuatArg inRotation, EActivation inActivationMode)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		Body &body = lock.GetBody();
+
+		// Update the position
+		body.SetPositionAndRotationInternal(body.GetPosition(), inRotation);
+
+		// Notify broadphase of change
+		if (body.IsInBroadPhase())
+		{
+			BodyID id = body.GetID();
+			mBroadPhase->NotifyBodiesAABBChanged(&id, 1);
+		}
+
+		// Optionally activate body
+		if (inActivationMode == EActivation::Activate && !body.IsStatic())
+			ActivateBodyInternal(body);
+	}
+}
+
+Quat BodyInterface::GetRotation(const BodyID &inBodyID) const
+{
+	BodyLockRead lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+		return lock.GetBody().GetRotation();
+	else
+		return Quat::sIdentity();
+}
+
+RMat44 BodyInterface::GetWorldTransform(const BodyID &inBodyID) const
+{
+	BodyLockRead lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+		return lock.GetBody().GetWorldTransform();
+	else
+		return RMat44::sIdentity();
+}
+
+RMat44 BodyInterface::GetCenterOfMassTransform(const BodyID &inBodyID) const
+{
+	BodyLockRead lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+		return lock.GetBody().GetCenterOfMassTransform();
+	else
+		return RMat44::sIdentity();
+}
+
+void BodyInterface::MoveKinematic(const BodyID &inBodyID, RVec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		Body &body = lock.GetBody();
+
+		body.MoveKinematic(inTargetPosition, inTargetRotation, inDeltaTime);
+
+		if (!body.IsActive() && (!body.GetLinearVelocity().IsNearZero() || !body.GetAngularVelocity().IsNearZero()))
+			mBodyManager->ActivateBodies(&inBodyID, 1);
+	}
+}
+
+void BodyInterface::SetLinearAndAngularVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		Body &body = lock.GetBody();
+		if (!body.IsStatic())
+		{
+			body.SetLinearVelocityClamped(inLinearVelocity);
+			body.SetAngularVelocityClamped(inAngularVelocity);
+
+			if (!body.IsActive() && (!inLinearVelocity.IsNearZero() || !inAngularVelocity.IsNearZero()))
+				mBodyManager->ActivateBodies(&inBodyID, 1);
+		}
+	}
+}
+
+void BodyInterface::GetLinearAndAngularVelocity(const BodyID &inBodyID, Vec3 &outLinearVelocity, Vec3 &outAngularVelocity) const
+{
+	BodyLockRead lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		const Body &body = lock.GetBody();
+		if (!body.IsStatic())
+		{
+			outLinearVelocity = body.GetLinearVelocity();
+			outAngularVelocity = body.GetAngularVelocity();
+			return;
+		}
+	}
+
+	outLinearVelocity = outAngularVelocity = Vec3::sZero();
+}
+
+void BodyInterface::SetLinearVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		Body &body = lock.GetBody();
+		if (!body.IsStatic())
+		{
+			body.SetLinearVelocityClamped(inLinearVelocity);
+
+			if (!body.IsActive() && !inLinearVelocity.IsNearZero())
+				mBodyManager->ActivateBodies(&inBodyID, 1);
+		}
+	}
+}
+
+Vec3 BodyInterface::GetLinearVelocity(const BodyID &inBodyID) const
+{
+	BodyLockRead lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		const Body &body = lock.GetBody();
+		if (!body.IsStatic())
+			return body.GetLinearVelocity();
+	}
+
+	return Vec3::sZero();
+}
+
+void BodyInterface::AddLinearVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		Body &body = lock.GetBody();
+		if (!body.IsStatic())
+		{
+			body.SetLinearVelocityClamped(body.GetLinearVelocity() + inLinearVelocity);
+
+			if (!body.IsActive() && !body.GetLinearVelocity().IsNearZero())
+				mBodyManager->ActivateBodies(&inBodyID, 1);
+		}
+	}
+}
+
+void BodyInterface::AddLinearAndAngularVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		Body &body = lock.GetBody();
+		if (!body.IsStatic())
+		{
+			body.SetLinearVelocityClamped(body.GetLinearVelocity() + inLinearVelocity);
+			body.SetAngularVelocityClamped(body.GetAngularVelocity() + inAngularVelocity);
+
+			if (!body.IsActive() && (!body.GetLinearVelocity().IsNearZero() || !body.GetAngularVelocity().IsNearZero()))
+				mBodyManager->ActivateBodies(&inBodyID, 1);
+		}
+	}
+}
+
+void BodyInterface::SetAngularVelocity(const BodyID &inBodyID, Vec3Arg inAngularVelocity)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		Body &body = lock.GetBody();
+		if (!body.IsStatic())
+		{
+			body.SetAngularVelocityClamped(inAngularVelocity);
+
+			if (!body.IsActive() && !inAngularVelocity.IsNearZero())
+				mBodyManager->ActivateBodies(&inBodyID, 1);
+		}
+	}
+}
+
+Vec3 BodyInterface::GetAngularVelocity(const BodyID &inBodyID) const
+{
+	BodyLockRead lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		const Body &body = lock.GetBody();
+		if (!body.IsStatic())
+			return body.GetAngularVelocity();
+	}
+
+	return Vec3::sZero();
+}
+
+Vec3 BodyInterface::GetPointVelocity(const BodyID &inBodyID, RVec3Arg inPoint) const
+{
+	BodyLockRead lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		const Body &body = lock.GetBody();
+		if (!body.IsStatic())
+			return body.GetPointVelocity(inPoint);
+	}
+
+	return Vec3::sZero();
+}
+
+void BodyInterface::AddForce(const BodyID &inBodyID, Vec3Arg inForce, EActivation inActivationMode)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		Body &body = lock.GetBody();
+		if (body.IsDynamic() && (inActivationMode == EActivation::Activate || body.IsActive()))
+		{
+			body.AddForce(inForce);
+
+			if (inActivationMode == EActivation::Activate)
+				ActivateBodyInternal(body);
+		}
+	}
+}
+
+void BodyInterface::AddForce(const BodyID &inBodyID, Vec3Arg inForce, RVec3Arg inPoint, EActivation inActivationMode)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		Body &body = lock.GetBody();
+		if (body.IsDynamic() && (inActivationMode == EActivation::Activate || body.IsActive()))
+		{
+			body.AddForce(inForce, inPoint);
+
+			if (inActivationMode == EActivation::Activate)
+				ActivateBodyInternal(body);
+		}
+	}
+}
+
+void BodyInterface::AddTorque(const BodyID &inBodyID, Vec3Arg inTorque, EActivation inActivationMode)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		Body &body = lock.GetBody();
+		if (body.IsDynamic() && (inActivationMode == EActivation::Activate || body.IsActive()))
+		{
+			body.AddTorque(inTorque);
+
+			if (inActivationMode == EActivation::Activate)
+				ActivateBodyInternal(body);
+		}
+	}
+}
+
+void BodyInterface::AddForceAndTorque(const BodyID &inBodyID, Vec3Arg inForce, Vec3Arg inTorque, EActivation inActivationMode)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		Body &body = lock.GetBody();
+		if (body.IsDynamic() && (inActivationMode == EActivation::Activate || body.IsActive()))
+		{
+			body.AddForce(inForce);
+			body.AddTorque(inTorque);
+
+			if (inActivationMode == EActivation::Activate)
+				ActivateBodyInternal(body);
+		}
+	}
+}
+
+void BodyInterface::AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		Body &body = lock.GetBody();
+		if (body.IsDynamic())
+		{
+			body.AddImpulse(inImpulse);
+
+			if (!body.IsActive())
+				mBodyManager->ActivateBodies(&inBodyID, 1);
+		}
+	}
+}
+
+void BodyInterface::AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse, RVec3Arg inPoint)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		Body &body = lock.GetBody();
+		if (body.IsDynamic())
+		{
+			body.AddImpulse(inImpulse, inPoint);
+
+			if (!body.IsActive())
+				mBodyManager->ActivateBodies(&inBodyID, 1);
+		}
+	}
+}
+
+void BodyInterface::AddAngularImpulse(const BodyID &inBodyID, Vec3Arg inAngularImpulse)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		Body &body = lock.GetBody();
+		if (body.IsDynamic())
+		{
+			body.AddAngularImpulse(inAngularImpulse);
+
+			if (!body.IsActive())
+				mBodyManager->ActivateBodies(&inBodyID, 1);
+		}
+	}
+}
+
+bool BodyInterface::ApplyBuoyancyImpulse(const BodyID &inBodyID, RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		Body &body = lock.GetBody();
+		if (body.IsDynamic()
+			&& body.ApplyBuoyancyImpulse(inSurfacePosition, inSurfaceNormal, inBuoyancy, inLinearDrag, inAngularDrag, inFluidVelocity, inGravity, inDeltaTime))
+		{
+			ActivateBodyInternal(body);
+			return true;
+		}
+	}
+
+	return false;
+}
+
+void BodyInterface::SetPositionRotationAndVelocity(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		Body &body = lock.GetBody();
+
+		// Update the position
+		body.SetPositionAndRotationInternal(inPosition, inRotation);
+
+		// Notify broadphase of change
+		if (body.IsInBroadPhase())
+		{
+			BodyID id = body.GetID();
+			mBroadPhase->NotifyBodiesAABBChanged(&id, 1);
+		}
+
+		if (!body.IsStatic())
+		{
+			body.SetLinearVelocityClamped(inLinearVelocity);
+			body.SetAngularVelocityClamped(inAngularVelocity);
+
+			// Optionally activate body
+			if (!body.IsActive() && (!inLinearVelocity.IsNearZero() || !inAngularVelocity.IsNearZero()))
+				mBodyManager->ActivateBodies(&inBodyID, 1);
+		}
+	}
+}
+
+void BodyInterface::SetMotionType(const BodyID &inBodyID, EMotionType inMotionType, EActivation inActivationMode)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		Body &body = lock.GetBody();
+
+		// Deactivate if we're making the body static
+		if (body.IsActive() && inMotionType == EMotionType::Static)
+			mBodyManager->DeactivateBodies(&inBodyID, 1);
+
+		body.SetMotionType(inMotionType);
+
+		// Activate body if requested
+		if (inMotionType != EMotionType::Static && inActivationMode == EActivation::Activate)
+			ActivateBodyInternal(body);
+	}
+}
+
+EBodyType BodyInterface::GetBodyType(const BodyID &inBodyID) const
+{
+	BodyLockRead lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+		return lock.GetBody().GetBodyType();
+	else
+		return EBodyType::RigidBody;
+}
+
+EMotionType BodyInterface::GetMotionType(const BodyID &inBodyID) const
+{
+	BodyLockRead lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+		return lock.GetBody().GetMotionType();
+	else
+		return EMotionType::Static;
+}
+
+void BodyInterface::SetMotionQuality(const BodyID &inBodyID, EMotionQuality inMotionQuality)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+		mBodyManager->SetMotionQuality(lock.GetBody(), inMotionQuality);
+}
+
+EMotionQuality BodyInterface::GetMotionQuality(const BodyID &inBodyID) const
+{
+	BodyLockRead lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded() && !lock.GetBody().IsStatic())
+		return lock.GetBody().GetMotionProperties()->GetMotionQuality();
+	else
+		return EMotionQuality::Discrete;
+}
+
+Mat44 BodyInterface::GetInverseInertia(const BodyID &inBodyID) const
+{
+	BodyLockRead lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+		return lock.GetBody().GetInverseInertia();
+	else
+		return Mat44::sIdentity();
+}
+
+void BodyInterface::SetRestitution(const BodyID &inBodyID, float inRestitution)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+		lock.GetBody().SetRestitution(inRestitution);
+}
+
+float BodyInterface::GetRestitution(const BodyID &inBodyID) const
+{
+	BodyLockRead lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+		return lock.GetBody().GetRestitution();
+	else
+		return 0.0f;
+}
+
+void BodyInterface::SetFriction(const BodyID &inBodyID, float inFriction)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+		lock.GetBody().SetFriction(inFriction);
+}
+
+float BodyInterface::GetFriction(const BodyID &inBodyID) const
+{
+	BodyLockRead lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+		return lock.GetBody().GetFriction();
+	else
+		return 0.0f;
+}
+
+void BodyInterface::SetGravityFactor(const BodyID &inBodyID, float inGravityFactor)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded() && lock.GetBody().GetMotionPropertiesUnchecked() != nullptr)
+		lock.GetBody().GetMotionPropertiesUnchecked()->SetGravityFactor(inGravityFactor);
+}
+
+float BodyInterface::GetGravityFactor(const BodyID &inBodyID) const
+{
+	BodyLockRead lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded() && lock.GetBody().GetMotionPropertiesUnchecked() != nullptr)
+		return lock.GetBody().GetMotionPropertiesUnchecked()->GetGravityFactor();
+	else
+		return 1.0f;
+}
+
+void BodyInterface::SetUseManifoldReduction(const BodyID &inBodyID, bool inUseReduction)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+	{
+		Body &body = lock.GetBody();
+		if (body.GetUseManifoldReduction() != inUseReduction)
+		{
+			body.SetUseManifoldReduction(inUseReduction);
+
+			// Flag collision cache invalid for this body
+			mBodyManager->InvalidateContactCacheForBody(body);
+		}
+	}
+}
+
+bool BodyInterface::GetUseManifoldReduction(const BodyID &inBodyID) const
+{
+	BodyLockRead lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+		return lock.GetBody().GetUseManifoldReduction();
+	else
+		return true;
+}
+
+TransformedShape BodyInterface::GetTransformedShape(const BodyID &inBodyID) const
+{
+	BodyLockRead lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+		return lock.GetBody().GetTransformedShape();
+	else
+		return TransformedShape();
+}
+
+uint64 BodyInterface::GetUserData(const BodyID &inBodyID) const
+{
+	BodyLockRead lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+		return lock.GetBody().GetUserData();
+	else
+		return 0;
+}
+
+void BodyInterface::SetUserData(const BodyID &inBodyID, uint64 inUserData) const
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+		lock.GetBody().SetUserData(inUserData);
+}
+
+const PhysicsMaterial *BodyInterface::GetMaterial(const BodyID &inBodyID, const SubShapeID &inSubShapeID) const
+{
+	BodyLockRead lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+		return lock.GetBody().GetShape()->GetMaterial(inSubShapeID);
+	else
+		return PhysicsMaterial::sDefault;
+}
+
+void BodyInterface::InvalidateContactCache(const BodyID &inBodyID)
+{
+	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+		mBodyManager->InvalidateContactCacheForBody(lock.GetBody());
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.h
new file mode 100644
index 0000000000..477b3564e4
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyInterface.h
@@ -0,0 +1,298 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/BodyID.h>
+#include <Jolt/Physics/EActivation.h>
+#include <Jolt/Physics/Collision/ObjectLayer.h>
+#include <Jolt/Physics/Body/MotionType.h>
+#include <Jolt/Physics/Body/MotionQuality.h>
+#include <Jolt/Physics/Body/BodyType.h>
+#include <Jolt/Core/Reference.h>
+
+JPH_NAMESPACE_BEGIN
+
+class Body;
+class BodyCreationSettings;
+class SoftBodyCreationSettings;
+class BodyLockInterface;
+class BroadPhase;
+class BodyManager;
+class TransformedShape;
+class PhysicsMaterial;
+class SubShapeID;
+class Shape;
+class TwoBodyConstraintSettings;
+class TwoBodyConstraint;
+class BroadPhaseLayerFilter;
+class AABox;
+
+/// Class that provides operations on bodies using a body ID. Note that if you need to do multiple operations on a single body, it is more efficient to lock the body once and combine the operations.
+/// All quantities are in world space unless otherwise specified.
+class JPH_EXPORT BodyInterface : public NonCopyable
+{
+public:
+	/// Initialize the interface (should only be called by PhysicsSystem)
+	void						Init(BodyLockInterface &inBodyLockInterface, BodyManager &inBodyManager, BroadPhase &inBroadPhase) { mBodyLockInterface = &inBodyLockInterface; mBodyManager = &inBodyManager; mBroadPhase = &inBroadPhase; }
+
+	/// Create a rigid body
+	/// @return Created body or null when out of bodies
+	Body *						CreateBody(const BodyCreationSettings &inSettings);
+
+	/// Create a soft body
+	/// @return Created body or null when out of bodies
+	Body *						CreateSoftBody(const SoftBodyCreationSettings &inSettings);
+
+	/// Create a rigid body with specified ID. This function can be used if a simulation is to run in sync between clients or if a simulation needs to be restored exactly.
+	/// The ID created on the server can be replicated to the client and used to create a deterministic simulation.
+	/// @return Created body or null when the body ID is invalid or a body of the same ID already exists.
+	Body *						CreateBodyWithID(const BodyID &inBodyID, const BodyCreationSettings &inSettings);
+
+	/// Create a soft body with specified ID. See comments at CreateBodyWithID.
+	Body *						CreateSoftBodyWithID(const BodyID &inBodyID, const SoftBodyCreationSettings &inSettings);
+
+	/// Advanced use only. Creates a rigid body without specifying an ID. This body cannot be added to the physics system until it has been assigned a body ID.
+	/// This can be used to decouple allocation from registering the body. A call to CreateBodyWithoutID followed by AssignBodyID is equivalent to calling CreateBodyWithID.
+	/// @return Created body
+	Body *						CreateBodyWithoutID(const BodyCreationSettings &inSettings) const;
+
+	/// Advanced use only. Creates a body without specifying an ID. See comments at CreateBodyWithoutID.
+	Body *						CreateSoftBodyWithoutID(const SoftBodyCreationSettings &inSettings) const;
+
+	/// Advanced use only. Destroy a body previously created with CreateBodyWithoutID that hasn't gotten an ID yet through the AssignBodyID function,
+	/// or a body that has had its body ID unassigned through UnassignBodyIDs. Bodies that have an ID should be destroyed through DestroyBody.
+	void						DestroyBodyWithoutID(Body *inBody) const;
+
+	/// Advanced use only. Assigns the next available body ID to a body that was created using CreateBodyWithoutID. After this call, the body can be added to the physics system.
+	/// @return false if the body already has an ID or out of body ids.
+	bool						AssignBodyID(Body *ioBody);
+
+	/// Advanced use only. Assigns a body ID to a body that was created using CreateBodyWithoutID. After this call, the body can be added to the physics system.
+	/// @return false if the body already has an ID or if the ID is not valid.
+	bool						AssignBodyID(Body *ioBody, const BodyID &inBodyID);
+
+	/// Advanced use only. See UnassignBodyIDs. Unassigns the ID of a single body.
+	Body *						UnassignBodyID(const BodyID &inBodyID);
+
+	/// Advanced use only. Removes a number of body IDs from their bodies and returns the body pointers. Before calling this, the body should have been removed from the physics system.
+	/// The body can be destroyed through DestroyBodyWithoutID. This can be used to decouple deallocation. A call to UnassignBodyIDs followed by calls to DestroyBodyWithoutID is equivalent to calling DestroyBodies.
+	/// @param inBodyIDs A list of body IDs
+	/// @param inNumber Number of bodies in the list
+	/// @param outBodies If not null on input, this will contain a list of body pointers corresponding to inBodyIDs that can be destroyed afterwards (caller assumes ownership over these).
+	void						UnassignBodyIDs(const BodyID *inBodyIDs, int inNumber, Body **outBodies);
+
+	/// Destroy a body.
+	/// Make sure that you remove the body from the physics system using BodyInterface::RemoveBody before calling this function.
+	void						DestroyBody(const BodyID &inBodyID);
+
+	/// Destroy multiple bodies
+	/// Make sure that you remove the bodies from the physics system using BodyInterface::RemoveBody before calling this function.
+	void						DestroyBodies(const BodyID *inBodyIDs, int inNumber);
+
+	/// Add body to the physics system.
+	/// Note that if you need to add multiple bodies, use the AddBodiesPrepare/AddBodiesFinalize function.
+	/// Adding many bodies, one at a time, results in a really inefficient broadphase until PhysicsSystem::OptimizeBroadPhase is called or when PhysicsSystem::Update rebuilds the tree!
+	/// After adding, to get a body by ID use the BodyLockRead or BodyLockWrite interface!
+	void						AddBody(const BodyID &inBodyID, EActivation inActivationMode);
+
+	/// Remove body from the physics system.
+	void						RemoveBody(const BodyID &inBodyID);
+
+	/// Check if a body has been added to the physics system.
+	bool						IsAdded(const BodyID &inBodyID) const;
+
+	/// Combines CreateBody and AddBody
+	/// @return Created body ID or an invalid ID when out of bodies
+	BodyID						CreateAndAddBody(const BodyCreationSettings &inSettings, EActivation inActivationMode);
+
+	/// Combines CreateSoftBody and AddBody
+	/// @return Created body ID or an invalid ID when out of bodies
+	BodyID						CreateAndAddSoftBody(const SoftBodyCreationSettings &inSettings, EActivation inActivationMode);
+
+	/// Add state handle, used to keep track of a batch of bodies while adding them to the PhysicsSystem.
+	using AddState = void *;
+
+	///@name Batch adding interface
+	///@{
+
+	/// Prepare adding inNumber bodies at ioBodies to the PhysicsSystem, returns a handle that should be used in AddBodiesFinalize/Abort.
+	/// This can be done on a background thread without influencing the PhysicsSystem.
+	/// ioBodies may be shuffled around by this function and should be kept that way until AddBodiesFinalize/Abort is called.
+	AddState					AddBodiesPrepare(BodyID *ioBodies, int inNumber);
+
+	/// Finalize adding bodies to the PhysicsSystem, supply the return value of AddBodiesPrepare in inAddState.
+	/// Please ensure that the ioBodies array passed to AddBodiesPrepare is unmodified and passed again to this function.
+	void						AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState, EActivation inActivationMode);
+
+	/// Abort adding bodies to the PhysicsSystem, supply the return value of AddBodiesPrepare in inAddState.
+	/// This can be done on a background thread without influencing the PhysicsSystem.
+	/// Please ensure that the ioBodies array passed to AddBodiesPrepare is unmodified and passed again to this function.
+	void						AddBodiesAbort(BodyID *ioBodies, int inNumber, AddState inAddState);
+
+	/// Remove inNumber bodies in ioBodies from the PhysicsSystem.
+	/// ioBodies may be shuffled around by this function.
+	void						RemoveBodies(BodyID *ioBodies, int inNumber);
+	///@}
+
+	///@name Activate / deactivate a body
+	///@{
+	void						ActivateBody(const BodyID &inBodyID);
+	void						ActivateBodies(const BodyID *inBodyIDs, int inNumber);
+	void						ActivateBodiesInAABox(const AABox &inBox, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter);
+	void						DeactivateBody(const BodyID &inBodyID);
+	void						DeactivateBodies(const BodyID *inBodyIDs, int inNumber);
+	bool						IsActive(const BodyID &inBodyID) const;
+	void						ResetSleepTimer(const BodyID &inBodyID);
+	///@}
+
+	/// Create a two body constraint
+	TwoBodyConstraint *			CreateConstraint(const TwoBodyConstraintSettings *inSettings, const BodyID &inBodyID1, const BodyID &inBodyID2);
+
+	/// Activate non-static bodies attached to a constraint
+	void						ActivateConstraint(const TwoBodyConstraint *inConstraint);
+
+	///@name Access to the shape of a body
+	///@{
+
+	/// Get the current shape
+	RefConst<Shape>				GetShape(const BodyID &inBodyID) const;
+
+	/// Set a new shape on the body
+	/// @param inBodyID Body ID of body that had its shape changed
+	/// @param inShape The new shape
+	/// @param inUpdateMassProperties When true, the mass and inertia tensor is recalculated
+	/// @param inActivationMode Whether or not to activate the body
+	void						SetShape(const BodyID &inBodyID, const Shape *inShape, bool inUpdateMassProperties, EActivation inActivationMode) const;
+
+	/// Notify all systems to indicate that a shape has changed (usable for MutableCompoundShapes)
+	/// @param inBodyID Body ID of body that had its shape changed
+	/// @param inPreviousCenterOfMass Center of mass of the shape before the alterations
+	/// @param inUpdateMassProperties When true, the mass and inertia tensor is recalculated
+	/// @param inActivationMode Whether or not to activate the body
+	void						NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inPreviousCenterOfMass, bool inUpdateMassProperties, EActivation inActivationMode) const;
+	///@}
+
+	///@name Object layer of a body
+	///@{
+	void						SetObjectLayer(const BodyID &inBodyID, ObjectLayer inLayer);
+	ObjectLayer					GetObjectLayer(const BodyID &inBodyID) const;
+	///@}
+
+	///@name Position and rotation of a body
+	///@{
+	void						SetPositionAndRotation(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode);
+	void						SetPositionAndRotationWhenChanged(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode); ///< Will only update the position/rotation and activate the body when the difference is larger than a very small number. This avoids updating the broadphase/waking up a body when the resulting position/orientation doesn't really change.
+	void						GetPositionAndRotation(const BodyID &inBodyID, RVec3 &outPosition, Quat &outRotation) const;
+	void						SetPosition(const BodyID &inBodyID, RVec3Arg inPosition, EActivation inActivationMode);
+	RVec3						GetPosition(const BodyID &inBodyID) const;
+	RVec3						GetCenterOfMassPosition(const BodyID &inBodyID) const;
+	void						SetRotation(const BodyID &inBodyID, QuatArg inRotation, EActivation inActivationMode);
+	Quat						GetRotation(const BodyID &inBodyID) const;
+	RMat44						GetWorldTransform(const BodyID &inBodyID) const;
+	RMat44						GetCenterOfMassTransform(const BodyID &inBodyID) const;
+	///@}
+
+	/// Set velocity of body such that it will be positioned at inTargetPosition/Rotation in inDeltaTime seconds (will activate body if needed)
+	void						MoveKinematic(const BodyID &inBodyID, RVec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime);
+
+	/// Linear or angular velocity (functions will activate body if needed).
+	/// Note that the linear velocity is the velocity of the center of mass, which may not coincide with the position of your object, to correct for this: \f$VelocityCOM = Velocity - AngularVelocity \times ShapeCOM\f$
+	void						SetLinearAndAngularVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity);
+	void						GetLinearAndAngularVelocity(const BodyID &inBodyID, Vec3 &outLinearVelocity, Vec3 &outAngularVelocity) const;
+	void						SetLinearVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity);
+	Vec3						GetLinearVelocity(const BodyID &inBodyID) const;
+	void						AddLinearVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity); ///< Add velocity to current velocity
+	void						AddLinearAndAngularVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity); ///< Add linear and angular to current velocities
+	void						SetAngularVelocity(const BodyID &inBodyID, Vec3Arg inAngularVelocity);
+	Vec3						GetAngularVelocity(const BodyID &inBodyID) const;
+	Vec3						GetPointVelocity(const BodyID &inBodyID, RVec3Arg inPoint) const; ///< Velocity of point inPoint (in world space, e.g. on the surface of the body) of the body
+
+	/// Set the complete motion state of a body.
+	/// Note that the linear velocity is the velocity of the center of mass, which may not coincide with the position of your object, to correct for this: \f$VelocityCOM = Velocity - AngularVelocity \times ShapeCOM\f$
+	void						SetPositionRotationAndVelocity(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity);
+
+	///@name Add forces to the body
+	///@{
+	void						AddForce(const BodyID &inBodyID, Vec3Arg inForce, EActivation inActivationMode = EActivation::Activate); ///< See Body::AddForce
+	void						AddForce(const BodyID &inBodyID, Vec3Arg inForce, RVec3Arg inPoint, EActivation inActivationMode = EActivation::Activate); ///< Applied at inPoint
+	void						AddTorque(const BodyID &inBodyID, Vec3Arg inTorque, EActivation inActivationMode = EActivation::Activate); ///< See Body::AddTorque
+	void						AddForceAndTorque(const BodyID &inBodyID, Vec3Arg inForce, Vec3Arg inTorque, EActivation inActivationMode = EActivation::Activate); ///< A combination of Body::AddForce and Body::AddTorque
+	///@}
+
+	///@name Add an impulse to the body
+	///@{
+	void						AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse); ///< Applied at center of mass
+	void						AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse, RVec3Arg inPoint); ///< Applied at inPoint
+	void						AddAngularImpulse(const BodyID &inBodyID, Vec3Arg inAngularImpulse);
+	bool						ApplyBuoyancyImpulse(const BodyID &inBodyID, RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime);
+	///@}
+
+	///@name Body type
+	///@{
+	EBodyType					GetBodyType(const BodyID &inBodyID) const;
+	///@}
+
+	///@name Body motion type
+	///@{
+	void						SetMotionType(const BodyID &inBodyID, EMotionType inMotionType, EActivation inActivationMode);
+	EMotionType					GetMotionType(const BodyID &inBodyID) const;
+	///@}
+
+	///@name Body motion quality
+	///@{
+	void						SetMotionQuality(const BodyID &inBodyID, EMotionQuality inMotionQuality);
+	EMotionQuality				GetMotionQuality(const BodyID &inBodyID) const;
+	///@}
+
+	/// Get inverse inertia tensor in world space
+	Mat44						GetInverseInertia(const BodyID &inBodyID) const;
+
+	///@name Restitution
+	///@{
+	void						SetRestitution(const BodyID &inBodyID, float inRestitution);
+	float						GetRestitution(const BodyID &inBodyID) const;
+	///@}
+
+	///@name Friction
+	///@{
+	void						SetFriction(const BodyID &inBodyID, float inFriction);
+	float						GetFriction(const BodyID &inBodyID) const;
+	///@}
+
+	///@name Gravity factor
+	///@{
+	void						SetGravityFactor(const BodyID &inBodyID, float inGravityFactor);
+	float						GetGravityFactor(const BodyID &inBodyID) const;
+	///@}
+
+	///@name Manifold reduction
+	///@{
+	void						SetUseManifoldReduction(const BodyID &inBodyID, bool inUseReduction);
+	bool						GetUseManifoldReduction(const BodyID &inBodyID) const;
+	///@}
+
+	/// Get transform and shape for this body, used to perform collision detection
+	TransformedShape			GetTransformedShape(const BodyID &inBodyID) const;
+
+	/// Get the user data for a body
+	uint64						GetUserData(const BodyID &inBodyID) const;
+	void						SetUserData(const BodyID &inBodyID, uint64 inUserData) const;
+
+	/// Get the material for a particular sub shape
+	const PhysicsMaterial *		GetMaterial(const BodyID &inBodyID, const SubShapeID &inSubShapeID) const;
+
+	/// Set the Body::EFlags::InvalidateContactCache flag for the specified body. This means that the collision cache is invalid for any body pair involving that body until the next physics step.
+	void						InvalidateContactCache(const BodyID &inBodyID);
+
+private:
+	/// Helper function to activate a single body
+	JPH_INLINE void				ActivateBodyInternal(Body &ioBody) const;
+
+	BodyLockInterface *			mBodyLockInterface = nullptr;
+	BodyManager *				mBodyManager = nullptr;
+	BroadPhase *				mBroadPhase = nullptr;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLock.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLock.h
new file mode 100644
index 0000000000..eccfec4d1e
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLock.h
@@ -0,0 +1,111 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/BodyLockInterface.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Base class for locking bodies for the duration of the scope of this class (do not use directly)
+template <bool Write, class BodyType>
+class BodyLockBase : public NonCopyable
+{
+public:
+	/// Constructor will lock the body
+								BodyLockBase(const BodyLockInterface &inBodyLockInterface, const BodyID &inBodyID) :
+		mBodyLockInterface(inBodyLockInterface)
+	{
+		if (inBodyID == BodyID())
+		{
+			// Invalid body id
+			mBodyLockMutex = nullptr;
+			mBody = nullptr;
+		}
+		else
+		{
+			// Get mutex
+			mBodyLockMutex = Write? inBodyLockInterface.LockWrite(inBodyID) : inBodyLockInterface.LockRead(inBodyID);
+
+			// Get a reference to the body or nullptr when it is no longer valid
+			mBody = inBodyLockInterface.TryGetBody(inBodyID);
+		}
+	}
+
+	/// Explicitly release the lock (normally this is done in the destructor)
+	inline void					ReleaseLock()
+	{
+		if (mBodyLockMutex != nullptr)
+		{
+			if (Write)
+				mBodyLockInterface.UnlockWrite(mBodyLockMutex);
+			else
+				mBodyLockInterface.UnlockRead(mBodyLockMutex);
+
+			mBodyLockMutex = nullptr;
+			mBody = nullptr;
+		}
+	}
+
+	/// Destructor will unlock the body
+								~BodyLockBase()
+	{
+		ReleaseLock();
+	}
+
+	/// Test if the lock was successful (if the body ID was valid)
+	inline bool					Succeeded() const
+	{
+		return mBody != nullptr;
+	}
+
+	/// Test if the lock was successful (if the body ID was valid) and the body is still in the broad phase
+	inline bool					SucceededAndIsInBroadPhase() const
+	{
+		return mBody != nullptr && mBody->IsInBroadPhase();
+	}
+
+	/// Access the body
+	inline BodyType &			GetBody() const
+	{
+		JPH_ASSERT(mBody != nullptr, "Should check Succeeded() first");
+		return *mBody;
+	}
+
+private:
+	const BodyLockInterface &	mBodyLockInterface;
+	SharedMutex *				mBodyLockMutex;
+	BodyType *					mBody;
+};
+
+/// A body lock takes a body ID and locks the underlying body so that other threads cannot access its members
+///
+/// The common usage pattern is:
+///
+///		BodyLockInterface lock_interface = physics_system.GetBodyLockInterface(); // Or non-locking interface if the lock is already taken
+///		BodyID body_id = ...; // Obtain ID to body
+///
+///		// Scoped lock
+///		{
+///			BodyLockRead lock(lock_interface, body_id);
+///			if (lock.Succeeded()) // body_id may no longer be valid
+///			{
+///				const Body &body = lock.GetBody();
+///
+///				// Do something with body
+///				...
+///			}
+///		}
+class BodyLockRead : public BodyLockBase<false, const Body>
+{
+	using BodyLockBase::BodyLockBase;
+};
+
+/// Specialization that locks a body for writing to. @see BodyLockRead for usage patterns.
+class BodyLockWrite : public BodyLockBase<true, Body>
+{
+	using BodyLockBase::BodyLockBase;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLockInterface.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLockInterface.h
new file mode 100644
index 0000000000..b65951fc8c
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLockInterface.h
@@ -0,0 +1,134 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+
+#pragma once
+
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Physics/Body/BodyManager.h>
+#include <Jolt/Physics/PhysicsLock.h>
+#include <Jolt/Core/Mutex.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Base class interface for locking a body. Usually you will use BodyLockRead / BodyLockWrite / BodyLockMultiRead / BodyLockMultiWrite instead.
+class BodyLockInterface : public NonCopyable
+{
+public:
+	/// Redefine MutexMask
+	using MutexMask = BodyManager::MutexMask;
+
+	/// Constructor
+	explicit					BodyLockInterface(BodyManager &inBodyManager)		: mBodyManager(inBodyManager) { }
+	virtual						~BodyLockInterface() = default;
+
+	///@name Locking functions
+	///@{
+	virtual SharedMutex *		LockRead(const BodyID &inBodyID) const = 0;
+	virtual void				UnlockRead(SharedMutex *inMutex) const = 0;
+	virtual SharedMutex *		LockWrite(const BodyID &inBodyID) const = 0;
+	virtual void				UnlockWrite(SharedMutex *inMutex) const = 0;
+	///@}
+
+	/// Get the mask needed to lock all bodies
+	inline MutexMask			GetAllBodiesMutexMask() const
+	{
+		return mBodyManager.GetAllBodiesMutexMask();
+	}
+
+	///@name Batch locking functions
+	///@{
+	virtual MutexMask			GetMutexMask(const BodyID *inBodies, int inNumber) const = 0;
+	virtual void				LockRead(MutexMask inMutexMask) const = 0;
+	virtual void				UnlockRead(MutexMask inMutexMask) const = 0;
+	virtual void				LockWrite(MutexMask inMutexMask) const = 0;
+	virtual void				UnlockWrite(MutexMask inMutexMask) const = 0;
+	///@}
+
+	/// Convert body ID to body
+	inline Body *				TryGetBody(const BodyID &inBodyID) const			{ return mBodyManager.TryGetBody(inBodyID); }
+
+protected:
+	BodyManager &				mBodyManager;
+};
+
+/// Implementation that performs no locking (assumes the lock has already been taken)
+class BodyLockInterfaceNoLock final : public BodyLockInterface
+{
+public:
+	using BodyLockInterface::BodyLockInterface;
+
+	///@name Locking functions
+	virtual SharedMutex *		LockRead([[maybe_unused]] const BodyID &inBodyID) const override	{ return nullptr; }
+	virtual void				UnlockRead([[maybe_unused]] SharedMutex *inMutex) const override	{ /* Nothing to do */ }
+	virtual SharedMutex *		LockWrite([[maybe_unused]] const BodyID &inBodyID) const override	{ return nullptr; }
+	virtual void				UnlockWrite([[maybe_unused]] SharedMutex *inMutex) const override	{ /* Nothing to do */ }
+
+	///@name Batch locking functions
+	virtual MutexMask			GetMutexMask([[maybe_unused]] const BodyID *inBodies, [[maybe_unused]] int inNumber) const override { return 0; }
+	virtual void				LockRead([[maybe_unused]] MutexMask inMutexMask) const override		{ /* Nothing to do */ }
+	virtual void				UnlockRead([[maybe_unused]] MutexMask inMutexMask) const override	{ /* Nothing to do */ }
+	virtual void				LockWrite([[maybe_unused]] MutexMask inMutexMask) const override	{ /* Nothing to do */ }
+	virtual void				UnlockWrite([[maybe_unused]] MutexMask inMutexMask) const override	{ /* Nothing to do */ }
+};
+
+/// Implementation that uses the body manager to lock the correct mutex for a body
+class BodyLockInterfaceLocking final : public BodyLockInterface
+{
+public:
+	using BodyLockInterface::BodyLockInterface;
+
+	///@name Locking functions
+	virtual SharedMutex *		LockRead(const BodyID &inBodyID) const override
+	{
+		SharedMutex &mutex = mBodyManager.GetMutexForBody(inBodyID);
+		PhysicsLock::sLockShared(mutex JPH_IF_ENABLE_ASSERTS(, &mBodyManager, EPhysicsLockTypes::PerBody));
+		return &mutex;
+	}
+
+	virtual void				UnlockRead(SharedMutex *inMutex) const override
+	{
+		PhysicsLock::sUnlockShared(*inMutex JPH_IF_ENABLE_ASSERTS(, &mBodyManager, EPhysicsLockTypes::PerBody));
+	}
+
+	virtual SharedMutex *		LockWrite(const BodyID &inBodyID) const override
+	{
+		SharedMutex &mutex = mBodyManager.GetMutexForBody(inBodyID);
+		PhysicsLock::sLock(mutex JPH_IF_ENABLE_ASSERTS(, &mBodyManager, EPhysicsLockTypes::PerBody));
+		return &mutex;
+	}
+
+	virtual void				UnlockWrite(SharedMutex *inMutex) const override
+	{
+		PhysicsLock::sUnlock(*inMutex JPH_IF_ENABLE_ASSERTS(, &mBodyManager, EPhysicsLockTypes::PerBody));
+	}
+
+	///@name Batch locking functions
+	virtual MutexMask			GetMutexMask(const BodyID *inBodies, int inNumber) const override
+	{
+		return mBodyManager.GetMutexMask(inBodies, inNumber);
+	}
+
+	virtual void				LockRead(MutexMask inMutexMask) const override
+	{
+		mBodyManager.LockRead(inMutexMask);
+	}
+
+	virtual void				UnlockRead(MutexMask inMutexMask) const override
+	{
+		mBodyManager.UnlockRead(inMutexMask);
+	}
+
+	virtual void				LockWrite(MutexMask inMutexMask) const override
+	{
+		mBodyManager.LockWrite(inMutexMask);
+	}
+
+	virtual void				UnlockWrite(MutexMask inMutexMask) const override
+	{
+		mBodyManager.UnlockWrite(inMutexMask);
+	}
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLockMulti.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLockMulti.h
new file mode 100644
index 0000000000..5872729c02
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyLockMulti.h
@@ -0,0 +1,104 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/BodyLockInterface.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Base class for locking multiple bodies for the duration of the scope of this class (do not use directly)
+template <bool Write, class BodyType>
+class BodyLockMultiBase : public NonCopyable
+{
+public:
+	/// Redefine MutexMask
+	using MutexMask = BodyLockInterface::MutexMask;
+
+	/// Constructor will lock the bodies
+								BodyLockMultiBase(const BodyLockInterface &inBodyLockInterface, const BodyID *inBodyIDs, int inNumber) :
+		mBodyLockInterface(inBodyLockInterface),
+		mMutexMask(inBodyLockInterface.GetMutexMask(inBodyIDs, inNumber)),
+		mBodyIDs(inBodyIDs),
+		mNumBodyIDs(inNumber)
+	{
+		if (mMutexMask != 0)
+		{
+			// Get mutex
+			if (Write)
+				inBodyLockInterface.LockWrite(mMutexMask);
+			else
+				inBodyLockInterface.LockRead(mMutexMask);
+		}
+	}
+
+	/// Destructor will unlock the bodies
+								~BodyLockMultiBase()
+	{
+		if (mMutexMask != 0)
+		{
+			if (Write)
+				mBodyLockInterface.UnlockWrite(mMutexMask);
+			else
+				mBodyLockInterface.UnlockRead(mMutexMask);
+		}
+	}
+
+	/// Access the body (returns null if body was not properly locked)
+	inline BodyType *			GetBody(int inBodyIndex) const
+	{
+		// Range check
+		JPH_ASSERT(inBodyIndex >= 0 && inBodyIndex < mNumBodyIDs);
+
+		// Get body ID
+		const BodyID &body_id = mBodyIDs[inBodyIndex];
+		if (body_id.IsInvalid())
+			return nullptr;
+
+		// Get a reference to the body or nullptr when it is no longer valid
+		return mBodyLockInterface.TryGetBody(body_id);
+	}
+
+private:
+	const BodyLockInterface &	mBodyLockInterface;
+	MutexMask					mMutexMask;
+	const BodyID *				mBodyIDs;
+	int							mNumBodyIDs;
+};
+
+/// A multi body lock takes a number of body IDs and locks the underlying bodies so that other threads cannot access its members
+///
+/// The common usage pattern is:
+///
+///		BodyLockInterface lock_interface = physics_system.GetBodyLockInterface(); // Or non-locking interface if the lock is already taken
+///		const BodyID *body_id = ...; // Obtain IDs to bodies
+///		int num_body_ids = ...;
+///
+///		// Scoped lock
+///		{
+///			BodyLockMultiRead lock(lock_interface, body_ids, num_body_ids);
+///			for (int i = 0; i < num_body_ids; ++i)
+///			{
+///				const Body *body = lock.GetBody(i);
+///				if (body != nullptr)
+///				{
+///					const Body &body = lock.Body();
+///
+///					// Do something with body
+///					...
+///				}
+///			}
+///		}
+class BodyLockMultiRead : public BodyLockMultiBase<false, const Body>
+{
+	using BodyLockMultiBase::BodyLockMultiBase;
+};
+
+/// Specialization that locks multiple bodies for writing to. @see BodyLockMultiRead for usage patterns.
+class BodyLockMultiWrite : public BodyLockMultiBase<true, Body>
+{
+	using BodyLockMultiBase::BodyLockMultiBase;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.cpp b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.cpp
new file mode 100644
index 0000000000..a48e4b0f82
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.cpp
@@ -0,0 +1,1156 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Body/BodyManager.h>
+#include <Jolt/Physics/PhysicsSettings.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Physics/Body/BodyLock.h>
+#include <Jolt/Physics/Body/BodyActivationListener.h>
+#include <Jolt/Physics/SoftBody/SoftBodyMotionProperties.h>
+#include <Jolt/Physics/SoftBody/SoftBodyCreationSettings.h>
+#include <Jolt/Physics/SoftBody/SoftBodyShape.h>
+#include <Jolt/Physics/StateRecorder.h>
+#include <Jolt/Core/StringTools.h>
+#include <Jolt/Core/QuickSort.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+	#include <Jolt/Physics/Body/BodyFilter.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+#ifdef JPH_ENABLE_ASSERTS
+	static thread_local bool sOverrideAllowActivation = false;
+	static thread_local bool sOverrideAllowDeactivation = false;
+
+	bool BodyManager::sGetOverrideAllowActivation()
+	{
+		return sOverrideAllowActivation;
+	}
+
+	void BodyManager::sSetOverrideAllowActivation(bool inValue)
+	{
+		sOverrideAllowActivation = inValue;
+	}
+
+	bool BodyManager::sGetOverrideAllowDeactivation()
+	{
+		return sOverrideAllowDeactivation;
+	}
+
+	void BodyManager::sSetOverrideAllowDeactivation(bool inValue)
+	{
+		sOverrideAllowDeactivation = inValue;
+	}
+#endif
+
+// Helper class that combines a body and its motion properties
+class BodyWithMotionProperties : public Body
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	MotionProperties			mMotionProperties;
+};
+
+// Helper class that combines a soft body its motion properties and shape
+class SoftBodyWithMotionPropertiesAndShape : public Body
+{
+public:
+								SoftBodyWithMotionPropertiesAndShape()
+	{
+		mShape.SetEmbedded();
+	}
+
+	SoftBodyMotionProperties	mMotionProperties;
+	SoftBodyShape				mShape;
+};
+
+inline void BodyManager::sDeleteBody(Body *inBody)
+{
+	if (inBody->mMotionProperties != nullptr)
+	{
+		JPH_IF_ENABLE_ASSERTS(inBody->mMotionProperties = nullptr;)
+		if (inBody->IsSoftBody())
+		{
+			inBody->mShape = nullptr; // Release the shape to avoid assertion on shape destruction because of embedded object with refcount > 0
+			delete static_cast<SoftBodyWithMotionPropertiesAndShape *>(inBody);
+		}
+		else
+			delete static_cast<BodyWithMotionProperties *>(inBody);
+	}
+	else
+		delete inBody;
+}
+
+BodyManager::~BodyManager()
+{
+	UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList));
+
+	// Destroy any bodies that are still alive
+	for (Body *b : mBodies)
+		if (sIsValidBodyPointer(b))
+			sDeleteBody(b);
+
+	for (BodyID *active_bodies : mActiveBodies)
+		delete [] active_bodies;
+}
+
+void BodyManager::Init(uint inMaxBodies, uint inNumBodyMutexes, const BroadPhaseLayerInterface &inLayerInterface)
+{
+	UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList));
+
+	// Num body mutexes must be a power of two and not bigger than our MutexMask
+	uint num_body_mutexes = Clamp<uint>(GetNextPowerOf2(inNumBodyMutexes == 0? 2 * thread::hardware_concurrency() : inNumBodyMutexes), 1, sizeof(MutexMask) * 8);
+#ifdef JPH_TSAN_ENABLED
+	num_body_mutexes = min(num_body_mutexes, 32U); // TSAN errors out when locking too many mutexes on the same thread, see: https://github.com/google/sanitizers/issues/950
+#endif
+
+	// Allocate the body mutexes
+	mBodyMutexes.Init(num_body_mutexes);
+
+	// Allocate space for bodies
+	mBodies.reserve(inMaxBodies);
+
+	// Allocate space for active bodies
+	for (BodyID *&active_bodies : mActiveBodies)
+	{
+		JPH_ASSERT(active_bodies == nullptr);
+		active_bodies = new BodyID [inMaxBodies];
+	}
+
+	// Allocate space for sequence numbers
+	mBodySequenceNumbers.resize(inMaxBodies, 0);
+
+	// Keep layer interface
+	mBroadPhaseLayerInterface = &inLayerInterface;
+}
+
+uint BodyManager::GetNumBodies() const
+{
+	UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList));
+
+	return mNumBodies;
+}
+
+BodyManager::BodyStats BodyManager::GetBodyStats() const
+{
+	UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList));
+
+	BodyStats stats;
+	stats.mNumBodies = mNumBodies;
+	stats.mMaxBodies = uint(mBodies.capacity());
+
+	for (const Body *body : mBodies)
+		if (sIsValidBodyPointer(body))
+		{
+			if (body->IsSoftBody())
+			{
+				stats.mNumSoftBodies++;
+				if (body->IsActive())
+					stats.mNumActiveSoftBodies++;
+			}
+			else
+			{
+				switch (body->GetMotionType())
+				{
+				case EMotionType::Static:
+					stats.mNumBodiesStatic++;
+					break;
+
+				case EMotionType::Dynamic:
+					stats.mNumBodiesDynamic++;
+					if (body->IsActive())
+						stats.mNumActiveBodiesDynamic++;
+					break;
+
+				case EMotionType::Kinematic:
+					stats.mNumBodiesKinematic++;
+					if (body->IsActive())
+						stats.mNumActiveBodiesKinematic++;
+					break;
+				}
+			}
+		}
+
+	return stats;
+}
+
+Body *BodyManager::AllocateBody(const BodyCreationSettings &inBodyCreationSettings) const
+{
+	// Fill in basic properties
+	Body *body;
+	if (inBodyCreationSettings.HasMassProperties())
+	{
+		BodyWithMotionProperties *bmp = new BodyWithMotionProperties;
+		body = bmp;
+		body->mMotionProperties = &bmp->mMotionProperties;
+	}
+	else
+	{
+		body = new Body;
+	}
+	body->mBodyType = EBodyType::RigidBody;
+	body->mShape = inBodyCreationSettings.GetShape();
+	body->mUserData = inBodyCreationSettings.mUserData;
+	body->SetFriction(inBodyCreationSettings.mFriction);
+	body->SetRestitution(inBodyCreationSettings.mRestitution);
+	body->mMotionType = inBodyCreationSettings.mMotionType;
+	if (inBodyCreationSettings.mIsSensor)
+		body->SetIsSensor(true);
+	if (inBodyCreationSettings.mCollideKinematicVsNonDynamic)
+		body->SetCollideKinematicVsNonDynamic(true);
+	if (inBodyCreationSettings.mUseManifoldReduction)
+		body->SetUseManifoldReduction(true);
+	if (inBodyCreationSettings.mApplyGyroscopicForce)
+		body->SetApplyGyroscopicForce(true);
+	if (inBodyCreationSettings.mEnhancedInternalEdgeRemoval)
+		body->SetEnhancedInternalEdgeRemoval(true);
+	SetBodyObjectLayerInternal(*body, inBodyCreationSettings.mObjectLayer);
+	body->mObjectLayer = inBodyCreationSettings.mObjectLayer;
+	body->mCollisionGroup = inBodyCreationSettings.mCollisionGroup;
+
+	if (inBodyCreationSettings.HasMassProperties())
+	{
+		MotionProperties *mp = body->mMotionProperties;
+		mp->SetLinearDamping(inBodyCreationSettings.mLinearDamping);
+		mp->SetAngularDamping(inBodyCreationSettings.mAngularDamping);
+		mp->SetMaxLinearVelocity(inBodyCreationSettings.mMaxLinearVelocity);
+		mp->SetMaxAngularVelocity(inBodyCreationSettings.mMaxAngularVelocity);
+		mp->SetMassProperties(inBodyCreationSettings.mAllowedDOFs, inBodyCreationSettings.GetMassProperties());
+		mp->SetLinearVelocity(inBodyCreationSettings.mLinearVelocity); // Needs to happen after setting the max linear/angular velocity and setting allowed DOFs
+		mp->SetAngularVelocity(inBodyCreationSettings.mAngularVelocity);
+		mp->SetGravityFactor(inBodyCreationSettings.mGravityFactor);
+		mp->SetNumVelocityStepsOverride(inBodyCreationSettings.mNumVelocityStepsOverride);
+		mp->SetNumPositionStepsOverride(inBodyCreationSettings.mNumPositionStepsOverride);
+		mp->mMotionQuality = inBodyCreationSettings.mMotionQuality;
+		mp->mAllowSleeping = inBodyCreationSettings.mAllowSleeping;
+		JPH_IF_ENABLE_ASSERTS(mp->mCachedBodyType = body->mBodyType;)
+		JPH_IF_ENABLE_ASSERTS(mp->mCachedMotionType = body->mMotionType;)
+	}
+
+	// Position body
+	body->SetPositionAndRotationInternal(inBodyCreationSettings.mPosition, inBodyCreationSettings.mRotation);
+
+	return body;
+}
+
+/// Create a soft body using creation settings. The returned body will not be part of the body manager yet.
+Body *BodyManager::AllocateSoftBody(const SoftBodyCreationSettings &inSoftBodyCreationSettings) const
+{
+	// Fill in basic properties
+	SoftBodyWithMotionPropertiesAndShape *bmp = new SoftBodyWithMotionPropertiesAndShape;
+	SoftBodyMotionProperties *mp = &bmp->mMotionProperties;
+	SoftBodyShape *shape = &bmp->mShape;
+	Body *body = bmp;
+	shape->mSoftBodyMotionProperties = mp;
+	body->mBodyType = EBodyType::SoftBody;
+	body->mMotionProperties = mp;
+	body->mShape = shape;
+	body->mUserData = inSoftBodyCreationSettings.mUserData;
+	body->SetFriction(inSoftBodyCreationSettings.mFriction);
+	body->SetRestitution(inSoftBodyCreationSettings.mRestitution);
+	body->mMotionType = EMotionType::Dynamic;
+	SetBodyObjectLayerInternal(*body, inSoftBodyCreationSettings.mObjectLayer);
+	body->mObjectLayer = inSoftBodyCreationSettings.mObjectLayer;
+	body->mCollisionGroup = inSoftBodyCreationSettings.mCollisionGroup;
+	mp->SetLinearDamping(inSoftBodyCreationSettings.mLinearDamping);
+	mp->SetAngularDamping(0);
+	mp->SetMaxLinearVelocity(inSoftBodyCreationSettings.mMaxLinearVelocity);
+	mp->SetMaxAngularVelocity(FLT_MAX);
+	mp->SetLinearVelocity(Vec3::sZero());
+	mp->SetAngularVelocity(Vec3::sZero());
+	mp->SetGravityFactor(inSoftBodyCreationSettings.mGravityFactor);
+	mp->mMotionQuality = EMotionQuality::Discrete;
+	mp->mAllowSleeping = inSoftBodyCreationSettings.mAllowSleeping;
+	JPH_IF_ENABLE_ASSERTS(mp->mCachedBodyType = body->mBodyType;)
+	JPH_IF_ENABLE_ASSERTS(mp->mCachedMotionType = body->mMotionType;)
+	mp->Initialize(inSoftBodyCreationSettings);
+
+	body->SetPositionAndRotationInternal(inSoftBodyCreationSettings.mPosition, inSoftBodyCreationSettings.mMakeRotationIdentity? Quat::sIdentity() : inSoftBodyCreationSettings.mRotation);
+
+	return body;
+}
+
+void BodyManager::FreeBody(Body *inBody) const
+{
+	JPH_ASSERT(inBody->GetID().IsInvalid(), "This function should only be called on a body that doesn't have an ID yet, use DestroyBody otherwise");
+
+	sDeleteBody(inBody);
+}
+
+bool BodyManager::AddBody(Body *ioBody)
+{
+	// Return error when body was already added
+	if (!ioBody->GetID().IsInvalid())
+		return false;
+
+	// Determine next free index
+	uint32 idx;
+	{
+		UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList));
+
+		if (mBodyIDFreeListStart != cBodyIDFreeListEnd)
+		{
+			// Pop an item from the freelist
+			JPH_ASSERT(mBodyIDFreeListStart & cIsFreedBody);
+			idx = uint32(mBodyIDFreeListStart >> cFreedBodyIndexShift);
+			JPH_ASSERT(!sIsValidBodyPointer(mBodies[idx]));
+			mBodyIDFreeListStart = uintptr_t(mBodies[idx]);
+			mBodies[idx] = ioBody;
+		}
+		else
+		{
+			if (mBodies.size() < mBodies.capacity())
+			{
+				// Allocate a new entry, note that the array should not actually resize since we've reserved it at init time
+				idx = uint32(mBodies.size());
+				mBodies.push_back(ioBody);
+			}
+			else
+			{
+				// Out of bodies
+				return false;
+			}
+		}
+
+		// Update cached number of bodies
+		mNumBodies++;
+	}
+
+	// Get next sequence number and assign the ID
+	uint8 seq_no = GetNextSequenceNumber(idx);
+	ioBody->mID = BodyID(idx, seq_no);
+	return true;
+}
+
+bool BodyManager::AddBodyWithCustomID(Body *ioBody, const BodyID &inBodyID)
+{
+	// Return error when body was already added
+	if (!ioBody->GetID().IsInvalid())
+		return false;
+
+	{
+		UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList));
+
+		// Check if index is beyond the max body ID
+		uint32 idx = inBodyID.GetIndex();
+		if (idx >= mBodies.capacity())
+			return false; // Return error
+
+		if (idx < mBodies.size())
+		{
+			// Body array entry has already been allocated, check if there's a free body here
+			if (sIsValidBodyPointer(mBodies[idx]))
+				return false; // Return error
+
+			// Remove the entry from the freelist
+			uintptr_t idx_start = mBodyIDFreeListStart >> cFreedBodyIndexShift;
+			if (idx == idx_start)
+			{
+				// First entry, easy to remove, the start of the list is our next
+				mBodyIDFreeListStart = uintptr_t(mBodies[idx]);
+			}
+			else
+			{
+				// Loop over the freelist and find the entry in the freelist pointing to our index
+				// TODO: This is O(N), see if this becomes a performance problem (don't want to put the freed bodies in a double linked list)
+				uintptr_t cur, next;
+				for (cur = idx_start; cur != cBodyIDFreeListEnd >> cFreedBodyIndexShift; cur = next)
+				{
+					next = uintptr_t(mBodies[cur]) >> cFreedBodyIndexShift;
+					if (next == idx)
+					{
+						mBodies[cur] = mBodies[idx];
+						break;
+					}
+				}
+				JPH_ASSERT(cur != cBodyIDFreeListEnd >> cFreedBodyIndexShift);
+			}
+
+			// Put the body in the slot
+			mBodies[idx] = ioBody;
+		}
+		else
+		{
+			// Ensure that all body IDs up to this body ID have been allocated and added to the free list
+			while (idx > mBodies.size())
+			{
+				// Push the id onto the freelist
+				mBodies.push_back((Body *)mBodyIDFreeListStart);
+				mBodyIDFreeListStart = (uintptr_t(mBodies.size() - 1) << cFreedBodyIndexShift) | cIsFreedBody;
+			}
+
+			// Add the element to the list
+			mBodies.push_back(ioBody);
+		}
+
+		// Update cached number of bodies
+		mNumBodies++;
+	}
+
+	// Assign the ID
+	ioBody->mID = inBodyID;
+	return true;
+}
+
+Body *BodyManager::RemoveBodyInternal(const BodyID &inBodyID)
+{
+	// Get body
+	uint32 idx = inBodyID.GetIndex();
+	Body *body = mBodies[idx];
+
+	// Validate that it can be removed
+	JPH_ASSERT(body->GetID() == inBodyID);
+	JPH_ASSERT(!body->IsActive());
+	JPH_ASSERT(!body->IsInBroadPhase(), "Use BodyInterface::RemoveBody to remove this body first!");
+
+	// Push the id onto the freelist
+	mBodies[idx] = (Body *)mBodyIDFreeListStart;
+	mBodyIDFreeListStart = (uintptr_t(idx) << cFreedBodyIndexShift) | cIsFreedBody;
+
+	return body;
+}
+
+#if defined(JPH_DEBUG) && defined(JPH_ENABLE_ASSERTS)
+
+void BodyManager::ValidateFreeList() const
+{
+	// Check that the freelist is correct
+	size_t num_freed = 0;
+	for (uintptr_t start = mBodyIDFreeListStart; start != cBodyIDFreeListEnd; start = uintptr_t(mBodies[start >> cFreedBodyIndexShift]))
+	{
+		JPH_ASSERT(start & cIsFreedBody);
+		num_freed++;
+	}
+	JPH_ASSERT(mNumBodies == mBodies.size() - num_freed);
+}
+
+#endif // defined(JPH_DEBUG) && _defined(JPH_ENABLE_ASSERTS)
+
+void BodyManager::RemoveBodies(const BodyID *inBodyIDs, int inNumber, Body **outBodies)
+{
+	// Don't take lock if no bodies are to be destroyed
+	if (inNumber <= 0)
+		return;
+
+	UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList));
+
+	// Update cached number of bodies
+	JPH_ASSERT(mNumBodies >= (uint)inNumber);
+	mNumBodies -= inNumber;
+
+	for (const BodyID *b = inBodyIDs, *b_end = inBodyIDs + inNumber; b < b_end; b++)
+	{
+		// Remove body
+		Body *body = RemoveBodyInternal(*b);
+
+		// Clear the ID
+		body->mID = BodyID();
+
+		// Return the body to the caller
+		if (outBodies != nullptr)
+		{
+			*outBodies = body;
+			++outBodies;
+		}
+	}
+
+#if defined(JPH_DEBUG) && defined(JPH_ENABLE_ASSERTS)
+	ValidateFreeList();
+#endif // defined(JPH_DEBUG) && _defined(JPH_ENABLE_ASSERTS)
+}
+
+void BodyManager::DestroyBodies(const BodyID *inBodyIDs, int inNumber)
+{
+	// Don't take lock if no bodies are to be destroyed
+	if (inNumber <= 0)
+		return;
+
+	UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList));
+
+	// Update cached number of bodies
+	JPH_ASSERT(mNumBodies >= (uint)inNumber);
+	mNumBodies -= inNumber;
+
+	for (const BodyID *b = inBodyIDs, *b_end = inBodyIDs + inNumber; b < b_end; b++)
+	{
+		// Remove body
+		Body *body = RemoveBodyInternal(*b);
+
+		// Free the body
+		sDeleteBody(body);
+	}
+
+#if defined(JPH_DEBUG) && defined(JPH_ENABLE_ASSERTS)
+	ValidateFreeList();
+#endif // defined(JPH_DEBUG) && _defined(JPH_ENABLE_ASSERTS)
+}
+
+void BodyManager::AddBodyToActiveBodies(Body &ioBody)
+{
+	// Select the correct array to use
+	int type = (int)ioBody.GetBodyType();
+	atomic<uint32> &num_active_bodies = mNumActiveBodies[type];
+	BodyID *active_bodies = mActiveBodies[type];
+
+	MotionProperties *mp = ioBody.mMotionProperties;
+	uint32 num_active_bodies_val = num_active_bodies.load(memory_order_relaxed);
+	mp->mIndexInActiveBodies = num_active_bodies_val;
+	JPH_ASSERT(num_active_bodies_val < GetMaxBodies());
+	active_bodies[num_active_bodies_val] = ioBody.GetID();
+	num_active_bodies.fetch_add(1, memory_order_release); // Increment atomic after setting the body ID so that PhysicsSystem::JobFindCollisions (which doesn't lock the mActiveBodiesMutex) will only read valid IDs
+
+	// Count CCD bodies
+	if (mp->GetMotionQuality() == EMotionQuality::LinearCast)
+		mNumActiveCCDBodies++;
+}
+
+void BodyManager::RemoveBodyFromActiveBodies(Body &ioBody)
+{
+	// Select the correct array to use
+	int type = (int)ioBody.GetBodyType();
+	atomic<uint32> &num_active_bodies = mNumActiveBodies[type];
+	BodyID *active_bodies = mActiveBodies[type];
+
+	uint32 last_body_index = num_active_bodies.load(memory_order_relaxed) - 1;
+	MotionProperties *mp = ioBody.mMotionProperties;
+	if (mp->mIndexInActiveBodies != last_body_index)
+	{
+		// This is not the last body, use the last body to fill the hole
+		BodyID last_body_id = active_bodies[last_body_index];
+		active_bodies[mp->mIndexInActiveBodies] = last_body_id;
+
+		// Update that body's index in the active list
+		Body &last_body = *mBodies[last_body_id.GetIndex()];
+		JPH_ASSERT(last_body.mMotionProperties->mIndexInActiveBodies == last_body_index);
+		last_body.mMotionProperties->mIndexInActiveBodies = mp->mIndexInActiveBodies;
+	}
+
+	// Mark this body as no longer active
+	mp->mIndexInActiveBodies = Body::cInactiveIndex;
+
+	// Remove unused element from active bodies list
+	num_active_bodies.fetch_sub(1, memory_order_release);
+
+	// Count CCD bodies
+	if (mp->GetMotionQuality() == EMotionQuality::LinearCast)
+		mNumActiveCCDBodies--;
+}
+
+void BodyManager::ActivateBodies(const BodyID *inBodyIDs, int inNumber)
+{
+	// Don't take lock if no bodies are to be activated
+	if (inNumber <= 0)
+		return;
+
+	UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList));
+
+	JPH_ASSERT(!mActiveBodiesLocked || sOverrideAllowActivation);
+
+	for (const BodyID *b = inBodyIDs, *b_end = inBodyIDs + inNumber; b < b_end; b++)
+		if (!b->IsInvalid())
+		{
+			BodyID body_id = *b;
+			Body &body = *mBodies[body_id.GetIndex()];
+
+			JPH_ASSERT(body.GetID() == body_id);
+			JPH_ASSERT(body.IsInBroadPhase(), "Use BodyInterface::AddBody to add the body first!");
+
+			if (!body.IsStatic())
+			{
+				// Reset sleeping timer so that we don't immediately go to sleep again
+				body.ResetSleepTimer();
+
+				// Check if we're sleeping
+				if (body.mMotionProperties->mIndexInActiveBodies == Body::cInactiveIndex)
+				{
+					AddBodyToActiveBodies(body);
+
+					// Call activation listener
+					if (mActivationListener != nullptr)
+						mActivationListener->OnBodyActivated(body_id, body.GetUserData());
+				}
+			}
+		}
+}
+
+void BodyManager::DeactivateBodies(const BodyID *inBodyIDs, int inNumber)
+{
+	// Don't take lock if no bodies are to be deactivated
+	if (inNumber <= 0)
+		return;
+
+	UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList));
+
+	JPH_ASSERT(!mActiveBodiesLocked || sOverrideAllowDeactivation);
+
+	for (const BodyID *b = inBodyIDs, *b_end = inBodyIDs + inNumber; b < b_end; b++)
+		if (!b->IsInvalid())
+		{
+			BodyID body_id = *b;
+			Body &body = *mBodies[body_id.GetIndex()];
+
+			JPH_ASSERT(body.GetID() == body_id);
+			JPH_ASSERT(body.IsInBroadPhase(), "Use BodyInterface::AddBody to add the body first!");
+
+			if (body.mMotionProperties != nullptr
+				&& body.mMotionProperties->mIndexInActiveBodies != Body::cInactiveIndex)
+			{
+				// Remove the body from the active bodies list
+				RemoveBodyFromActiveBodies(body);
+
+				// Mark this body as no longer active
+				body.mMotionProperties->mIslandIndex = Body::cInactiveIndex;
+
+				// Reset velocity
+				body.mMotionProperties->mLinearVelocity = Vec3::sZero();
+				body.mMotionProperties->mAngularVelocity = Vec3::sZero();
+
+				// Call activation listener
+				if (mActivationListener != nullptr)
+					mActivationListener->OnBodyDeactivated(body_id, body.GetUserData());
+			}
+		}
+}
+
+void BodyManager::SetMotionQuality(Body &ioBody, EMotionQuality inMotionQuality)
+{
+	MotionProperties *mp = ioBody.GetMotionPropertiesUnchecked();
+	if (mp != nullptr && mp->GetMotionQuality() != inMotionQuality)
+	{
+		UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList));
+
+		JPH_ASSERT(!mActiveBodiesLocked);
+
+		bool is_active = ioBody.IsActive();
+		if (is_active && mp->GetMotionQuality() == EMotionQuality::LinearCast)
+			--mNumActiveCCDBodies;
+
+		mp->mMotionQuality = inMotionQuality;
+
+		if (is_active && mp->GetMotionQuality() == EMotionQuality::LinearCast)
+			++mNumActiveCCDBodies;
+	}
+}
+
+void BodyManager::GetActiveBodies(EBodyType inType, BodyIDVector &outBodyIDs) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList));
+
+	const BodyID *active_bodies = mActiveBodies[(int)inType];
+	outBodyIDs.assign(active_bodies, active_bodies + mNumActiveBodies[(int)inType].load(memory_order_relaxed));
+}
+
+void BodyManager::GetBodyIDs(BodyIDVector &outBodies) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList));
+
+	// Reserve space for all bodies
+	outBodies.clear();
+	outBodies.reserve(mNumBodies);
+
+	// Iterate the list and find the bodies that are not null
+	for (const Body *b : mBodies)
+		if (sIsValidBodyPointer(b))
+			outBodies.push_back(b->GetID());
+
+	// Validate that our reservation was correct
+	JPH_ASSERT(outBodies.size() == mNumBodies);
+}
+
+void BodyManager::SetBodyActivationListener(BodyActivationListener *inListener)
+{
+	UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList));
+
+	mActivationListener = inListener;
+}
+
+BodyManager::MutexMask BodyManager::GetMutexMask(const BodyID *inBodies, int inNumber) const
+{
+	JPH_ASSERT(sizeof(MutexMask) * 8 >= mBodyMutexes.GetNumMutexes(), "MutexMask must have enough bits");
+
+	if (inNumber >= (int)mBodyMutexes.GetNumMutexes())
+	{
+		// Just lock everything if there are too many bodies
+		return GetAllBodiesMutexMask();
+	}
+	else
+	{
+		MutexMask mask = 0;
+		for (const BodyID *b = inBodies, *b_end = inBodies + inNumber; b < b_end; ++b)
+			if (!b->IsInvalid())
+			{
+				uint32 index = mBodyMutexes.GetMutexIndex(b->GetIndex());
+				mask |= (MutexMask(1) << index);
+			}
+		return mask;
+	}
+}
+
+void BodyManager::LockRead(MutexMask inMutexMask) const
+{
+	JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckLock(this, EPhysicsLockTypes::PerBody));
+
+	int index = 0;
+	for (MutexMask mask = inMutexMask; mask != 0; mask >>= 1, index++)
+		if (mask & 1)
+			mBodyMutexes.GetMutexByIndex(index).lock_shared();
+}
+
+void BodyManager::UnlockRead(MutexMask inMutexMask) const
+{
+	JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckUnlock(this, EPhysicsLockTypes::PerBody));
+
+	int index = 0;
+	for (MutexMask mask = inMutexMask; mask != 0; mask >>= 1, index++)
+		if (mask & 1)
+			mBodyMutexes.GetMutexByIndex(index).unlock_shared();
+}
+
+void BodyManager::LockWrite(MutexMask inMutexMask) const
+{
+	JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckLock(this, EPhysicsLockTypes::PerBody));
+
+	int index = 0;
+	for (MutexMask mask = inMutexMask; mask != 0; mask >>= 1, index++)
+		if (mask & 1)
+			mBodyMutexes.GetMutexByIndex(index).lock();
+}
+
+void BodyManager::UnlockWrite(MutexMask inMutexMask) const
+{
+	JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckUnlock(this, EPhysicsLockTypes::PerBody));
+
+	int index = 0;
+	for (MutexMask mask = inMutexMask; mask != 0; mask >>= 1, index++)
+		if (mask & 1)
+			mBodyMutexes.GetMutexByIndex(index).unlock();
+}
+
+void BodyManager::LockAllBodies() const
+{
+	JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckLock(this, EPhysicsLockTypes::PerBody));
+	mBodyMutexes.LockAll();
+
+	PhysicsLock::sLock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList));
+}
+
+void BodyManager::UnlockAllBodies() const
+{
+	PhysicsLock::sUnlock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList));
+
+	JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckUnlock(this, EPhysicsLockTypes::PerBody));
+	mBodyMutexes.UnlockAll();
+}
+
+void BodyManager::SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const
+{
+	{
+		LockAllBodies();
+
+		// Determine which bodies to save
+		Array<const Body *> bodies;
+		bodies.reserve(mNumBodies);
+		for (const Body *b : mBodies)
+			if (sIsValidBodyPointer(b) && b->IsInBroadPhase() && (inFilter == nullptr || inFilter->ShouldSaveBody(*b)))
+				bodies.push_back(b);
+
+		// Write state of bodies
+		uint32 num_bodies = (uint32)bodies.size();
+		inStream.Write(num_bodies);
+		for (const Body *b : bodies)
+		{
+			inStream.Write(b->GetID());
+			inStream.Write(b->IsActive());
+			b->SaveState(inStream);
+		}
+
+		UnlockAllBodies();
+	}
+}
+
+bool BodyManager::RestoreState(StateRecorder &inStream)
+{
+	BodyIDVector bodies_to_activate, bodies_to_deactivate;
+
+	{
+		LockAllBodies();
+
+		if (inStream.IsValidating())
+		{
+			// Read state of bodies, note this reads it in a way to be consistent with validation
+			uint32 old_num_bodies = 0;
+			for (const Body *b : mBodies)
+				if (sIsValidBodyPointer(b) && b->IsInBroadPhase())
+					++old_num_bodies;
+			uint32 num_bodies = old_num_bodies; // Initialize to current value for validation
+			inStream.Read(num_bodies);
+			if (num_bodies != old_num_bodies)
+			{
+				JPH_ASSERT(false, "Cannot handle adding/removing bodies");
+				UnlockAllBodies();
+				return false;
+			}
+
+			for (Body *b : mBodies)
+				if (sIsValidBodyPointer(b) && b->IsInBroadPhase())
+				{
+					BodyID body_id = b->GetID(); // Initialize to current value for validation
+					inStream.Read(body_id);
+					if (body_id != b->GetID())
+					{
+						JPH_ASSERT(false, "Cannot handle adding/removing bodies");
+						UnlockAllBodies();
+						return false;
+					}
+					bool is_active = b->IsActive(); // Initialize to current value for validation
+					inStream.Read(is_active);
+					if (is_active != b->IsActive())
+					{
+						if (is_active)
+							bodies_to_activate.push_back(body_id);
+						else
+							bodies_to_deactivate.push_back(body_id);
+					}
+					b->RestoreState(inStream);
+				}
+		}
+		else
+		{
+			// Not validating, we can be a bit more loose, read number of bodies
+			uint32 num_bodies = 0;
+			inStream.Read(num_bodies);
+
+			// Iterate over the stored bodies and restore their state
+			for (uint32 idx = 0; idx < num_bodies; ++idx)
+			{
+				BodyID body_id;
+				inStream.Read(body_id);
+				Body *b = TryGetBody(body_id);
+				if (b == nullptr)
+				{
+					JPH_ASSERT(false, "Restoring state for non-existing body");
+					UnlockAllBodies();
+					return false;
+				}
+				bool is_active;
+				inStream.Read(is_active);
+				if (is_active != b->IsActive())
+				{
+					if (is_active)
+						bodies_to_activate.push_back(body_id);
+					else
+						bodies_to_deactivate.push_back(body_id);
+				}
+				b->RestoreState(inStream);
+			}
+		}
+
+		UnlockAllBodies();
+	}
+
+	{
+		UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList));
+
+		for (BodyID body_id : bodies_to_activate)
+		{
+			Body *body = TryGetBody(body_id);
+			AddBodyToActiveBodies(*body);
+		}
+
+		for (BodyID body_id : bodies_to_deactivate)
+		{
+			Body *body = TryGetBody(body_id);
+			RemoveBodyFromActiveBodies(*body);
+		}
+	}
+
+	return true;
+}
+
+void BodyManager::SaveBodyState(const Body &inBody, StateRecorder &inStream) const
+{
+	inStream.Write(inBody.IsActive());
+
+	inBody.SaveState(inStream);
+}
+
+void BodyManager::RestoreBodyState(Body &ioBody, StateRecorder &inStream)
+{
+	bool is_active = ioBody.IsActive();
+	inStream.Read(is_active);
+
+	ioBody.RestoreState(inStream);
+
+	if (is_active != ioBody.IsActive())
+	{
+		UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList));
+
+		JPH_ASSERT(!mActiveBodiesLocked || sOverrideAllowActivation);
+
+		if (is_active)
+			AddBodyToActiveBodies(ioBody);
+		else
+			RemoveBodyFromActiveBodies(ioBody);
+	}
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void BodyManager::Draw(const DrawSettings &inDrawSettings, const PhysicsSettings &inPhysicsSettings, DebugRenderer *inRenderer, const BodyDrawFilter *inBodyFilter)
+{
+	JPH_PROFILE_FUNCTION();
+
+	LockAllBodies();
+
+	for (const Body *body : mBodies)
+		if (sIsValidBodyPointer(body) && body->IsInBroadPhase() && (!inBodyFilter || inBodyFilter->ShouldDraw(*body)))
+		{
+			JPH_ASSERT(mBodies[body->GetID().GetIndex()] == body);
+
+			bool is_sensor = body->IsSensor();
+
+			// Determine drawing mode
+			Color color;
+			if (is_sensor)
+				color = Color::sYellow;
+			else
+				switch (inDrawSettings.mDrawShapeColor)
+				{
+				case EShapeColor::InstanceColor:
+					// Each instance has own color
+					color = Color::sGetDistinctColor(body->mID.GetIndex());
+					break;
+
+				case EShapeColor::ShapeTypeColor:
+					color = ShapeFunctions::sGet(body->GetShape()->GetSubType()).mColor;
+					break;
+
+				case EShapeColor::MotionTypeColor:
+					// Determine color based on motion type
+					switch (body->mMotionType)
+					{
+					case EMotionType::Static:
+						color = Color::sGrey;
+						break;
+
+					case EMotionType::Kinematic:
+						color = Color::sGreen;
+						break;
+
+					case EMotionType::Dynamic:
+						color = Color::sGetDistinctColor(body->mID.GetIndex());
+						break;
+
+					default:
+						JPH_ASSERT(false);
+						color = Color::sBlack;
+						break;
+					}
+					break;
+
+				case EShapeColor::SleepColor:
+					// Determine color based on motion type
+					switch (body->mMotionType)
+					{
+					case EMotionType::Static:
+						color = Color::sGrey;
+						break;
+
+					case EMotionType::Kinematic:
+						color = body->IsActive()? Color::sGreen : Color::sRed;
+						break;
+
+					case EMotionType::Dynamic:
+						color = body->IsActive()? Color::sYellow : Color::sRed;
+						break;
+
+					default:
+						JPH_ASSERT(false);
+						color = Color::sBlack;
+						break;
+					}
+					break;
+
+				case EShapeColor::IslandColor:
+					// Determine color based on motion type
+					switch (body->mMotionType)
+					{
+					case EMotionType::Static:
+						color = Color::sGrey;
+						break;
+
+					case EMotionType::Kinematic:
+					case EMotionType::Dynamic:
+						{
+							uint32 idx = body->GetMotionProperties()->GetIslandIndexInternal();
+							color = idx != Body::cInactiveIndex? Color::sGetDistinctColor(idx) : Color::sLightGrey;
+						}
+						break;
+
+					default:
+						JPH_ASSERT(false);
+						color = Color::sBlack;
+						break;
+					}
+					break;
+
+				case EShapeColor::MaterialColor:
+					color = Color::sWhite;
+					break;
+
+				default:
+					JPH_ASSERT(false);
+					color = Color::sBlack;
+					break;
+				}
+
+			// Draw the results of GetSupportFunction
+			if (inDrawSettings.mDrawGetSupportFunction)
+				body->mShape->DrawGetSupportFunction(inRenderer, body->GetCenterOfMassTransform(), Vec3::sReplicate(1.0f), color, inDrawSettings.mDrawSupportDirection);
+
+			// Draw the results of GetSupportingFace
+			if (inDrawSettings.mDrawGetSupportingFace)
+				body->mShape->DrawGetSupportingFace(inRenderer, body->GetCenterOfMassTransform(), Vec3::sReplicate(1.0f));
+
+			// Draw the shape
+			if (inDrawSettings.mDrawShape)
+				body->mShape->Draw(inRenderer, body->GetCenterOfMassTransform(), Vec3::sReplicate(1.0f), color, inDrawSettings.mDrawShapeColor == EShapeColor::MaterialColor, inDrawSettings.mDrawShapeWireframe || is_sensor);
+
+			// Draw bounding box
+			if (inDrawSettings.mDrawBoundingBox)
+				inRenderer->DrawWireBox(body->mBounds, color);
+
+			// Draw center of mass transform
+			if (inDrawSettings.mDrawCenterOfMassTransform)
+				inRenderer->DrawCoordinateSystem(body->GetCenterOfMassTransform(), 0.2f);
+
+			// Draw world transform
+			if (inDrawSettings.mDrawWorldTransform)
+				inRenderer->DrawCoordinateSystem(body->GetWorldTransform(), 0.2f);
+
+			// Draw world space linear and angular velocity
+			if (inDrawSettings.mDrawVelocity)
+			{
+				RVec3 pos = body->GetCenterOfMassPosition();
+				inRenderer->DrawArrow(pos, pos + body->GetLinearVelocity(), Color::sGreen, 0.1f);
+				inRenderer->DrawArrow(pos, pos + body->GetAngularVelocity(), Color::sRed, 0.1f);
+			}
+
+			if (inDrawSettings.mDrawMassAndInertia && body->IsDynamic())
+			{
+				const MotionProperties *mp = body->GetMotionProperties();
+				if (mp->GetInverseMass() > 0.0f
+					&& !Vec3::sEquals(mp->GetInverseInertiaDiagonal(), Vec3::sZero()).TestAnyXYZTrue())
+				{
+					// Invert mass again
+					float mass = 1.0f / mp->GetInverseMass();
+
+					// Invert diagonal again
+					Vec3 diagonal = mp->GetInverseInertiaDiagonal().Reciprocal();
+
+					// Determine how big of a box has the equivalent inertia
+					Vec3 box_size = MassProperties::sGetEquivalentSolidBoxSize(mass, diagonal);
+
+					// Draw box with equivalent inertia
+					inRenderer->DrawWireBox(body->GetCenterOfMassTransform() * Mat44::sRotation(mp->GetInertiaRotation()), AABox(-0.5f * box_size, 0.5f * box_size), Color::sOrange);
+
+					// Draw mass
+					inRenderer->DrawText3D(body->GetCenterOfMassPosition(), StringFormat("%.2f", (double)mass), Color::sOrange, 0.2f);
+				}
+			}
+
+			if (inDrawSettings.mDrawSleepStats && body->IsDynamic() && body->IsActive())
+			{
+				// Draw stats to know which bodies could go to sleep
+				String text = StringFormat("t: %.1f", (double)body->mMotionProperties->mSleepTestTimer);
+				uint8 g = uint8(Clamp(255.0f * body->mMotionProperties->mSleepTestTimer / inPhysicsSettings.mTimeBeforeSleep, 0.0f, 255.0f));
+				Color sleep_color = Color(0, 255 - g, g);
+				inRenderer->DrawText3D(body->GetCenterOfMassPosition(), text, sleep_color, 0.2f);
+				for (int i = 0; i < 3; ++i)
+					inRenderer->DrawWireSphere(JPH_IF_DOUBLE_PRECISION(body->mMotionProperties->GetSleepTestOffset() +) body->mMotionProperties->mSleepTestSpheres[i].GetCenter(), body->mMotionProperties->mSleepTestSpheres[i].GetRadius(), sleep_color);
+			}
+
+			if (body->IsSoftBody())
+			{
+				const SoftBodyMotionProperties *mp = static_cast<const SoftBodyMotionProperties *>(body->GetMotionProperties());
+				RMat44 com = body->GetCenterOfMassTransform();
+
+				if (inDrawSettings.mDrawSoftBodyVertices)
+					mp->DrawVertices(inRenderer, com);
+
+				if (inDrawSettings.mDrawSoftBodyVertexVelocities)
+					mp->DrawVertexVelocities(inRenderer, com);
+
+				if (inDrawSettings.mDrawSoftBodyEdgeConstraints)
+					mp->DrawEdgeConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor);
+
+				if (inDrawSettings.mDrawSoftBodyBendConstraints)
+					mp->DrawBendConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor);
+
+				if (inDrawSettings.mDrawSoftBodyVolumeConstraints)
+					mp->DrawVolumeConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor);
+
+				if (inDrawSettings.mDrawSoftBodySkinConstraints)
+					mp->DrawSkinConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor);
+
+				if (inDrawSettings.mDrawSoftBodyLRAConstraints)
+					mp->DrawLRAConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor);
+
+				if (inDrawSettings.mDrawSoftBodyPredictedBounds)
+					mp->DrawPredictedBounds(inRenderer, com);
+			}
+		}
+
+	UnlockAllBodies();
+}
+#endif // JPH_DEBUG_RENDERER
+
+void BodyManager::InvalidateContactCacheForBody(Body &ioBody)
+{
+	// If this is the first time we flip the collision cache invalid flag, we need to add it to an internal list to ensure we reset the flag at the end of the physics update
+	if (ioBody.InvalidateContactCacheInternal())
+	{
+		lock_guard lock(mBodiesCacheInvalidMutex);
+		mBodiesCacheInvalid.push_back(ioBody.GetID());
+	}
+}
+
+void BodyManager::ValidateContactCacheForAllBodies()
+{
+	lock_guard lock(mBodiesCacheInvalidMutex);
+
+	for (const BodyID &b : mBodiesCacheInvalid)
+	{
+		// The body may have been removed between the call to InvalidateContactCacheForBody and this call, so check if it still exists
+		Body *body = TryGetBody(b);
+		if (body != nullptr)
+			body->ValidateContactCacheInternal();
+	}
+	mBodiesCacheInvalid.clear();
+}
+
+#ifdef JPH_DEBUG
+void BodyManager::ValidateActiveBodyBounds()
+{
+	UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList));
+
+	for (uint type = 0; type < cBodyTypeCount; ++type)
+		for (BodyID *id = mActiveBodies[type], *id_end = mActiveBodies[type] + mNumActiveBodies[type].load(memory_order_relaxed); id < id_end; ++id)
+		{
+			const Body *body = mBodies[id->GetIndex()];
+			AABox cached = body->GetWorldSpaceBounds();
+			AABox calculated = body->GetShape()->GetWorldSpaceBounds(body->GetCenterOfMassTransform(), Vec3::sReplicate(1.0f));
+			JPH_ASSERT(cached == calculated);
+		}
+}
+#endif // JPH_DEBUG
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.h
new file mode 100644
index 0000000000..518574707a
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyManager.h
@@ -0,0 +1,377 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Core/Mutex.h>
+#include <Jolt/Core/MutexArray.h>
+
+JPH_NAMESPACE_BEGIN
+
+// Classes
+class BodyCreationSettings;
+class SoftBodyCreationSettings;
+class BodyActivationListener;
+class StateRecorderFilter;
+struct PhysicsSettings;
+#ifdef JPH_DEBUG_RENDERER
+class DebugRenderer;
+class BodyDrawFilter;
+#endif // JPH_DEBUG_RENDERER
+
+#ifdef JPH_DEBUG_RENDERER
+
+/// Defines how to color soft body constraints
+enum class ESoftBodyConstraintColor
+{
+	ConstraintType,				/// Draw different types of constraints in different colors
+	ConstraintGroup,			/// Draw constraints in the same group in the same color, non-parallel group will be red
+	ConstraintOrder,			/// Draw constraints in the same group in the same color, non-parallel group will be red, and order within each group will be indicated with gradient
+};
+
+#endif // JPH_DEBUG_RENDERER
+
+/// Array of bodies
+using BodyVector = Array<Body *>;
+
+/// Array of body ID's
+using BodyIDVector = Array<BodyID>;
+
+/// Class that contains all bodies
+class JPH_EXPORT BodyManager : public NonCopyable
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Destructor
+									~BodyManager();
+
+	/// Initialize the manager
+	void							Init(uint inMaxBodies, uint inNumBodyMutexes, const BroadPhaseLayerInterface &inLayerInterface);
+
+	/// Gets the current amount of bodies that are in the body manager
+	uint							GetNumBodies() const;
+
+	/// Gets the max bodies that we can support
+	uint							GetMaxBodies() const						{ return uint(mBodies.capacity()); }
+
+	/// Helper struct that counts the number of bodies of each type
+	struct BodyStats
+	{
+		uint						mNumBodies					= 0;			///< Total number of bodies in the body manager
+		uint						mMaxBodies					= 0;			///< Max allowed number of bodies in the body manager (as configured in Init(...))
+
+		uint						mNumBodiesStatic			= 0;			///< Number of static bodies
+
+		uint						mNumBodiesDynamic			= 0;			///< Number of dynamic bodies
+		uint						mNumActiveBodiesDynamic		= 0;			///< Number of dynamic bodies that are currently active
+
+		uint						mNumBodiesKinematic			= 0;			///< Number of kinematic bodies
+		uint						mNumActiveBodiesKinematic	= 0;			///< Number of kinematic bodies that are currently active
+
+		uint						mNumSoftBodies				= 0;			///< Number of soft bodies
+		uint						mNumActiveSoftBodies		= 0;			///< Number of soft bodies that are currently active
+	};
+
+	/// Get stats about the bodies in the body manager (slow, iterates through all bodies)
+	BodyStats						GetBodyStats() const;
+
+	/// Create a body using creation settings. The returned body will not be part of the body manager yet.
+	Body *							AllocateBody(const BodyCreationSettings &inBodyCreationSettings) const;
+
+	/// Create a soft body using creation settings. The returned body will not be part of the body manager yet.
+	Body *							AllocateSoftBody(const SoftBodyCreationSettings &inSoftBodyCreationSettings) const;
+
+	/// Free a body that has not been added to the body manager yet (if it has, use DestroyBodies).
+	void							FreeBody(Body *inBody) const;
+
+	/// Add a body to the body manager, assigning it the next available ID. Returns false if no more IDs are available.
+	bool							AddBody(Body *ioBody);
+
+	/// Add a body to the body manager, assigning it a custom ID. Returns false if the ID is not valid.
+	bool							AddBodyWithCustomID(Body *ioBody, const BodyID &inBodyID);
+
+	/// Remove a list of bodies from the body manager
+	void							RemoveBodies(const BodyID *inBodyIDs, int inNumber, Body **outBodies);
+
+	/// Remove a set of bodies from the body manager and frees them.
+	void							DestroyBodies(const BodyID *inBodyIDs, int inNumber);
+
+	/// Activate a list of bodies.
+	/// This function should only be called when an exclusive lock for the bodies are held.
+	void							ActivateBodies(const BodyID *inBodyIDs, int inNumber);
+
+	/// Deactivate a list of bodies.
+	/// This function should only be called when an exclusive lock for the bodies are held.
+	void							DeactivateBodies(const BodyID *inBodyIDs, int inNumber);
+
+	/// Update the motion quality for a body
+	void							SetMotionQuality(Body &ioBody, EMotionQuality inMotionQuality);
+
+	/// Get copy of the list of active bodies under protection of a lock.
+	void							GetActiveBodies(EBodyType inType, BodyIDVector &outBodyIDs) const;
+
+	/// Get the list of active bodies. Note: Not thread safe. The active bodies list can change at any moment.
+	const BodyID *					GetActiveBodiesUnsafe(EBodyType inType) const { return mActiveBodies[int(inType)]; }
+
+	/// Get the number of active bodies.
+	uint32							GetNumActiveBodies(EBodyType inType) const	{ return mNumActiveBodies[int(inType)].load(memory_order_acquire); }
+
+	/// Get the number of active bodies that are using continuous collision detection
+	uint32							GetNumActiveCCDBodies() const				{ return mNumActiveCCDBodies; }
+
+	/// Listener that is notified whenever a body is activated/deactivated
+	void							SetBodyActivationListener(BodyActivationListener *inListener);
+	BodyActivationListener *		GetBodyActivationListener() const			{ return mActivationListener; }
+
+	/// Check if this is a valid body pointer. When a body is freed the memory that the pointer occupies is reused to store a freelist.
+	static inline bool				sIsValidBodyPointer(const Body *inBody)		{ return (uintptr_t(inBody) & cIsFreedBody) == 0; }
+
+	/// Get all bodies. Note that this can contain invalid body pointers, call sIsValidBodyPointer to check.
+	const BodyVector &				GetBodies() const							{ return mBodies; }
+
+	/// Get all bodies. Note that this can contain invalid body pointers, call sIsValidBodyPointer to check.
+	BodyVector &					GetBodies()									{ return mBodies; }
+
+	/// Get all body IDs under the protection of a lock
+	void							GetBodyIDs(BodyIDVector &outBodies) const;
+
+	/// Access a body (not protected by lock)
+	const Body &					GetBody(const BodyID &inID) const			{ return *mBodies[inID.GetIndex()]; }
+
+	/// Access a body (not protected by lock)
+	Body &							GetBody(const BodyID &inID)					{ return *mBodies[inID.GetIndex()]; }
+
+	/// Access a body, will return a nullptr if the body ID is no longer valid (not protected by lock)
+	const Body *					TryGetBody(const BodyID &inID) const
+	{
+		uint32 idx = inID.GetIndex();
+		if (idx >= mBodies.size())
+			return nullptr;
+
+		const Body *body = mBodies[idx];
+		if (sIsValidBodyPointer(body) && body->GetID() == inID)
+			return body;
+
+		return nullptr;
+	}
+
+	/// Access a body, will return a nullptr if the body ID is no longer valid (not protected by lock)
+	Body *							TryGetBody(const BodyID &inID)
+	{
+		uint32 idx = inID.GetIndex();
+		if (idx >= mBodies.size())
+			return nullptr;
+
+		Body *body = mBodies[idx];
+		if (sIsValidBodyPointer(body) && body->GetID() == inID)
+			return body;
+
+		return nullptr;
+	}
+
+	/// Access the mutex for a single body
+	SharedMutex &					GetMutexForBody(const BodyID &inID) const	{ return mBodyMutexes.GetMutexByObjectIndex(inID.GetIndex()); }
+
+	/// Bodies are protected using an array of mutexes (so a fixed number, not 1 per body). Each bit in this mask indicates a locked mutex.
+	using MutexMask = uint64;
+
+	///@name Batch body mutex access (do not use directly)
+	///@{
+	MutexMask						GetAllBodiesMutexMask() const				{ return mBodyMutexes.GetNumMutexes() == sizeof(MutexMask) * 8? ~MutexMask(0) : (MutexMask(1) << mBodyMutexes.GetNumMutexes()) - 1; }
+	MutexMask						GetMutexMask(const BodyID *inBodies, int inNumber) const;
+	void							LockRead(MutexMask inMutexMask) const;
+	void							UnlockRead(MutexMask inMutexMask) const;
+	void							LockWrite(MutexMask inMutexMask) const;
+	void							UnlockWrite(MutexMask inMutexMask) const;
+	///@}
+
+	/// Lock all bodies. This should only be done during PhysicsSystem::Update().
+	void							LockAllBodies() const;
+
+	/// Unlock all bodies. This should only be done during PhysicsSystem::Update().
+	void							UnlockAllBodies() const;
+
+	/// Function to update body's layer (should only be called by the BodyInterface since it also requires updating the broadphase)
+	inline void						SetBodyObjectLayerInternal(Body &ioBody, ObjectLayer inLayer) const { ioBody.mObjectLayer = inLayer; ioBody.mBroadPhaseLayer = mBroadPhaseLayerInterface->GetBroadPhaseLayer(inLayer); }
+
+	/// Set the Body::EFlags::InvalidateContactCache flag for the specified body. This means that the collision cache is invalid for any body pair involving that body until the next physics step.
+	void							InvalidateContactCacheForBody(Body &ioBody);
+
+	/// Reset the Body::EFlags::InvalidateContactCache flag for all bodies. All contact pairs in the contact cache will now by valid again.
+	void							ValidateContactCacheForAllBodies();
+
+	/// Saving state for replay
+	void							SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const;
+
+	/// Restoring state for replay. Returns false if failed.
+	bool							RestoreState(StateRecorder &inStream);
+
+	/// Save the state of a single body for replay
+	void							SaveBodyState(const Body &inBody, StateRecorder &inStream) const;
+
+	/// Save the state of a single body for replay
+	void							RestoreBodyState(Body &inBody, StateRecorder &inStream);
+
+#ifdef JPH_DEBUG_RENDERER
+	enum class EShapeColor
+	{
+		InstanceColor,				///< Random color per instance
+		ShapeTypeColor,				///< Convex = green, scaled = yellow, compound = orange, mesh = red
+		MotionTypeColor,			///< Static = grey, keyframed = green, dynamic = random color per instance
+		SleepColor,					///< Static = grey, keyframed = green, dynamic = yellow, sleeping = red
+		IslandColor,				///< Static = grey, active = random color per island, sleeping = light grey
+		MaterialColor,				///< Color as defined by the PhysicsMaterial of the shape
+	};
+
+	/// Draw settings
+	struct DrawSettings
+	{
+		bool						mDrawGetSupportFunction = false;				///< Draw the GetSupport() function, used for convex collision detection
+		bool						mDrawSupportDirection = false;					///< When drawing the support function, also draw which direction mapped to a specific support point
+		bool						mDrawGetSupportingFace = false;					///< Draw the faces that were found colliding during collision detection
+		bool						mDrawShape = true;								///< Draw the shapes of all bodies
+		bool						mDrawShapeWireframe = false;					///< When mDrawShape is true and this is true, the shapes will be drawn in wireframe instead of solid.
+		EShapeColor					mDrawShapeColor = EShapeColor::MotionTypeColor; ///< Coloring scheme to use for shapes
+		bool						mDrawBoundingBox = false;						///< Draw a bounding box per body
+		bool						mDrawCenterOfMassTransform = false;				///< Draw the center of mass for each body
+		bool						mDrawWorldTransform = false;					///< Draw the world transform (which can be different than the center of mass) for each body
+		bool						mDrawVelocity = false;							///< Draw the velocity vector for each body
+		bool						mDrawMassAndInertia = false;					///< Draw the mass and inertia (as the box equivalent) for each body
+		bool						mDrawSleepStats = false;						///< Draw stats regarding the sleeping algorithm of each body
+		bool						mDrawSoftBodyVertices = false;					///< Draw the vertices of soft bodies
+		bool						mDrawSoftBodyVertexVelocities = false;			///< Draw the velocities of the vertices of soft bodies
+		bool						mDrawSoftBodyEdgeConstraints = false;			///< Draw the edge constraints of soft bodies
+		bool						mDrawSoftBodyBendConstraints = false;			///< Draw the bend constraints of soft bodies
+		bool						mDrawSoftBodyVolumeConstraints = false;			///< Draw the volume constraints of soft bodies
+		bool						mDrawSoftBodySkinConstraints = false;			///< Draw the skin constraints of soft bodies
+		bool						mDrawSoftBodyLRAConstraints = false;			///< Draw the LRA constraints of soft bodies
+		bool						mDrawSoftBodyPredictedBounds = false;			///< Draw the predicted bounds of soft bodies
+		ESoftBodyConstraintColor	mDrawSoftBodyConstraintColor = ESoftBodyConstraintColor::ConstraintType; ///< Coloring scheme to use for soft body constraints
+	};
+
+	/// Draw the state of the bodies (debugging purposes)
+	void							Draw(const DrawSettings &inSettings, const PhysicsSettings &inPhysicsSettings, DebugRenderer *inRenderer, const BodyDrawFilter *inBodyFilter = nullptr);
+#endif // JPH_DEBUG_RENDERER
+
+#ifdef JPH_ENABLE_ASSERTS
+	/// Lock the active body list, asserts when Activate/DeactivateBody is called.
+	void							SetActiveBodiesLocked(bool inLocked)		{ mActiveBodiesLocked = inLocked; }
+
+	/// Per thread override of the locked state, to be used by the PhysicsSystem only!
+	class GrantActiveBodiesAccess
+	{
+	public:
+		inline GrantActiveBodiesAccess(bool inAllowActivation, bool inAllowDeactivation)
+		{
+			JPH_ASSERT(!sGetOverrideAllowActivation());
+			sSetOverrideAllowActivation(inAllowActivation);
+
+			JPH_ASSERT(!sGetOverrideAllowDeactivation());
+			sSetOverrideAllowDeactivation(inAllowDeactivation);
+		}
+
+		inline ~GrantActiveBodiesAccess()
+		{
+			sSetOverrideAllowActivation(false);
+			sSetOverrideAllowDeactivation(false);
+		}
+	};
+#endif
+
+#ifdef JPH_DEBUG
+	/// Validate if the cached bounding boxes are correct for all active bodies
+	void							ValidateActiveBodyBounds();
+#endif // JPH_DEBUG
+
+private:
+	/// Increment and get the sequence number of the body
+#ifdef JPH_COMPILER_CLANG
+	__attribute__((no_sanitize("implicit-conversion"))) // We intentionally overflow the uint8 sequence number
+#endif
+	inline uint8					GetNextSequenceNumber(int inBodyIndex)		{ return ++mBodySequenceNumbers[inBodyIndex]; }
+
+	/// Add a single body to mActiveBodies, note doesn't lock the active body mutex!
+	inline void						AddBodyToActiveBodies(Body &ioBody);
+
+	/// Remove a single body from mActiveBodies, note doesn't lock the active body mutex!
+	inline void						RemoveBodyFromActiveBodies(Body &ioBody);
+
+	/// Helper function to remove a body from the manager
+	JPH_INLINE Body *				RemoveBodyInternal(const BodyID &inBodyID);
+
+	/// Helper function to delete a body (which could actually be a BodyWithMotionProperties)
+	inline static void				sDeleteBody(Body *inBody);
+
+#if defined(JPH_DEBUG) && defined(JPH_ENABLE_ASSERTS)
+	/// Function to check that the free list is not corrupted
+	void							ValidateFreeList() const;
+#endif // defined(JPH_DEBUG) && _defined(JPH_ENABLE_ASSERTS)
+
+	/// List of pointers to all bodies. Contains invalid pointers for deleted bodies, check with sIsValidBodyPointer. Note that this array is reserved to the max bodies that is passed in the Init function so that adding bodies will not reallocate the array.
+	BodyVector						mBodies;
+
+	/// Current number of allocated bodies
+	uint							mNumBodies = 0;
+
+	/// Indicates that there are no more freed body IDs
+	static constexpr uintptr_t		cBodyIDFreeListEnd = ~uintptr_t(0);
+
+	/// Bit that indicates a pointer in mBodies is actually the index of the next freed body. We use the lowest bit because we know that Bodies need to be 16 byte aligned so addresses can never end in a 1 bit.
+	static constexpr uintptr_t		cIsFreedBody = uintptr_t(1);
+
+	/// Amount of bits to shift to get an index to the next freed body
+	static constexpr uint			cFreedBodyIndexShift = 1;
+
+	/// Index of first entry in mBodies that is unused
+	uintptr_t						mBodyIDFreeListStart = cBodyIDFreeListEnd;
+
+	/// Protects mBodies array (but not the bodies it points to), mNumBodies and mBodyIDFreeListStart
+	mutable Mutex					mBodiesMutex;
+
+	/// An array of mutexes protecting the bodies in the mBodies array
+	using BodyMutexes = MutexArray<SharedMutex>;
+	mutable BodyMutexes				mBodyMutexes;
+
+	/// List of next sequence number for a body ID
+	Array<uint8>					mBodySequenceNumbers;
+
+	/// Mutex that protects the mActiveBodies array
+	mutable Mutex					mActiveBodiesMutex;
+
+	/// List of all active dynamic bodies (size is equal to max amount of bodies)
+	BodyID *						mActiveBodies[cBodyTypeCount] = { };
+
+	/// How many bodies there are in the list of active bodies
+	atomic<uint32>					mNumActiveBodies[cBodyTypeCount] = { };
+
+	/// How many of the active bodies have continuous collision detection enabled
+	uint32							mNumActiveCCDBodies = 0;
+
+	/// Mutex that protects the mBodiesCacheInvalid array
+	mutable Mutex					mBodiesCacheInvalidMutex;
+
+	/// List of all bodies that should have their cache invalidated
+	BodyIDVector					mBodiesCacheInvalid;
+
+	/// Listener that is notified whenever a body is activated/deactivated
+	BodyActivationListener *		mActivationListener = nullptr;
+
+	/// Cached broadphase layer interface
+	const BroadPhaseLayerInterface *mBroadPhaseLayerInterface = nullptr;
+
+#ifdef JPH_ENABLE_ASSERTS
+	static bool						sGetOverrideAllowActivation();
+	static void						sSetOverrideAllowActivation(bool inValue);
+
+	static bool						sGetOverrideAllowDeactivation();
+	static void						sSetOverrideAllowDeactivation(bool inValue);
+
+	/// Debug system that tries to limit changes to active bodies during the PhysicsSystem::Update()
+	bool							mActiveBodiesLocked = false;
+#endif
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyPair.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyPair.h
new file mode 100644
index 0000000000..8ac849a49a
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyPair.h
@@ -0,0 +1,36 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/BodyID.h>
+#include <Jolt/Core/HashCombine.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Structure that holds a body pair
+struct alignas(uint64) BodyPair
+{
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+							BodyPair() = default;
+							BodyPair(BodyID inA, BodyID inB)							: mBodyA(inA), mBodyB(inB) { }
+
+	/// Equals operator
+	bool					operator == (const BodyPair &inRHS) const					{ return *reinterpret_cast<const uint64 *>(this) == *reinterpret_cast<const uint64 *>(&inRHS); }
+
+	/// Smaller than operator, used for consistently ordering body pairs
+	bool					operator < (const BodyPair &inRHS) const					{ return *reinterpret_cast<const uint64 *>(this) < *reinterpret_cast<const uint64 *>(&inRHS); }
+
+	/// Get the hash value of this object
+	uint64					GetHash() const												{ return Hash64(*reinterpret_cast<const uint64 *>(this)); }
+
+	BodyID					mBodyA;
+	BodyID					mBodyB;
+};
+
+static_assert(sizeof(BodyPair) == sizeof(uint64), "Mismatch in class size");
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/BodyType.h b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyType.h
new file mode 100644
index 0000000000..984af06ad2
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Body/BodyType.h
@@ -0,0 +1,19 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Type of body
+enum class EBodyType : uint8
+{
+	RigidBody,				///< Rigid body consisting of a rigid shape
+	SoftBody,				///< Soft body consisting of a deformable shape
+};
+
+/// How many types of bodies there are
+static constexpr uint cBodyTypeCount = 2;
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/MassProperties.cpp b/thirdparty/jolt_physics/Jolt/Physics/Body/MassProperties.cpp
new file mode 100644
index 0000000000..91df6b0df3
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Body/MassProperties.cpp
@@ -0,0 +1,185 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Body/MassProperties.h>
+#include <Jolt/Math/Matrix.h>
+#include <Jolt/Math/Vector.h>
+#include <Jolt/Math/EigenValueSymmetric.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/Core/InsertionSort.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(MassProperties)
+{
+	JPH_ADD_ATTRIBUTE(MassProperties, mMass)
+	JPH_ADD_ATTRIBUTE(MassProperties, mInertia)
+}
+
+bool MassProperties::DecomposePrincipalMomentsOfInertia(Mat44 &outRotation, Vec3 &outDiagonal) const
+{
+	// Using eigendecomposition to get the principal components of the inertia tensor
+	// See: https://en.wikipedia.org/wiki/Eigendecomposition_of_a_matrix
+	Matrix<3, 3> inertia;
+	inertia.CopyPart(mInertia, 0, 0, 3, 3, 0, 0);
+	Matrix<3, 3> eigen_vec = Matrix<3, 3>::sIdentity();
+	Vector<3> eigen_val;
+	if (!EigenValueSymmetric(inertia, eigen_vec, eigen_val))
+		return false;
+
+	// Sort so that the biggest value goes first
+	int indices[] = { 0, 1, 2 };
+	InsertionSort(indices, indices + 3, [&eigen_val](int inLeft, int inRight) { return eigen_val[inLeft] > eigen_val[inRight]; });
+
+	// Convert to a regular Mat44 and Vec3
+	outRotation = Mat44::sIdentity();
+	for (int i = 0; i < 3; ++i)
+	{
+		outRotation.SetColumn3(i, Vec3(reinterpret_cast<Float3 &>(eigen_vec.GetColumn(indices[i]))));
+		outDiagonal.SetComponent(i, eigen_val[indices[i]]);
+	}
+
+	// Make sure that the rotation matrix is a right handed matrix
+	if (outRotation.GetAxisX().Cross(outRotation.GetAxisY()).Dot(outRotation.GetAxisZ()) < 0.0f)
+		outRotation.SetAxisZ(-outRotation.GetAxisZ());
+
+#ifdef JPH_ENABLE_ASSERTS
+	// Validate that the solution is correct, for each axis we want to make sure that the difference in inertia is
+	// smaller than some fraction of the inertia itself in that axis
+	Mat44 new_inertia = outRotation * Mat44::sScale(outDiagonal) * outRotation.Inversed();
+	for (int i = 0; i < 3; ++i)
+		JPH_ASSERT(new_inertia.GetColumn3(i).IsClose(mInertia.GetColumn3(i), mInertia.GetColumn3(i).LengthSq() * 1.0e-10f));
+#endif
+
+	return true;
+}
+
+void MassProperties::SetMassAndInertiaOfSolidBox(Vec3Arg inBoxSize, float inDensity)
+{
+	// Calculate mass
+	mMass = inBoxSize.GetX() * inBoxSize.GetY() * inBoxSize.GetZ() * inDensity;
+
+	// Calculate inertia
+	Vec3 size_sq = inBoxSize * inBoxSize;
+	Vec3 scale = (size_sq.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_X>() + size_sq.Swizzle<SWIZZLE_Z, SWIZZLE_Z, SWIZZLE_Y>()) * (mMass / 12.0f);
+	mInertia = Mat44::sScale(scale);
+}
+
+void MassProperties::ScaleToMass(float inMass)
+{
+	if (mMass > 0.0f)
+	{
+		// Calculate how much we have to scale the inertia tensor
+		float mass_scale = inMass / mMass;
+
+		// Update mass
+		mMass = inMass;
+
+		// Update inertia tensor
+		for (int i = 0; i < 3; ++i)
+			mInertia.SetColumn4(i, mInertia.GetColumn4(i) * mass_scale);
+	}
+	else
+	{
+		// Just set the mass
+		mMass = inMass;
+	}
+}
+
+Vec3 MassProperties::sGetEquivalentSolidBoxSize(float inMass, Vec3Arg inInertiaDiagonal)
+{
+	// Moment of inertia of a solid box has diagonal:
+	// mass / 12 * [size_y^2 + size_z^2, size_x^2 + size_z^2, size_x^2 + size_y^2]
+	// Solving for size_x, size_y and size_y (diagonal and mass are known):
+	Vec3 diagonal = inInertiaDiagonal * (12.0f / inMass);
+	return Vec3(sqrt(0.5f * (-diagonal[0] + diagonal[1] + diagonal[2])), sqrt(0.5f * (diagonal[0] - diagonal[1] + diagonal[2])), sqrt(0.5f * (diagonal[0] + diagonal[1] - diagonal[2])));
+}
+
+void MassProperties::Scale(Vec3Arg inScale)
+{
+	// See: https://en.wikipedia.org/wiki/Moment_of_inertia#Inertia_tensor
+	// The diagonal of the inertia tensor can be calculated like this:
+	// Ixx = sum_{k = 1 to n}(m_k * (y_k^2 + z_k^2))
+	// Iyy = sum_{k = 1 to n}(m_k * (x_k^2 + z_k^2))
+	// Izz = sum_{k = 1 to n}(m_k * (x_k^2 + y_k^2))
+	//
+	// We want to isolate the terms x_k, y_k and z_k:
+	// d = [0.5, 0.5, 0.5].[Ixx, Iyy, Izz]
+	// [sum_{k = 1 to n}(m_k * x_k^2), sum_{k = 1 to n}(m_k * y_k^2), sum_{k = 1 to n}(m_k * z_k^2)] = [d, d, d] - [Ixx, Iyy, Izz]
+	Vec3 diagonal = mInertia.GetDiagonal3();
+	Vec3 xyz_sq = Vec3::sReplicate(Vec3::sReplicate(0.5f).Dot(diagonal)) - diagonal;
+
+	// When scaling a shape these terms change like this:
+	// sum_{k = 1 to n}(m_k * (scale_x * x_k)^2) = scale_x^2 * sum_{k = 1 to n}(m_k * x_k^2)
+	// Same for y_k and z_k
+	// Using these terms we can calculate the new diagonal of the inertia tensor:
+	Vec3 xyz_scaled_sq = inScale * inScale * xyz_sq;
+	float i_xx = xyz_scaled_sq.GetY() + xyz_scaled_sq.GetZ();
+	float i_yy = xyz_scaled_sq.GetX() + xyz_scaled_sq.GetZ();
+	float i_zz = xyz_scaled_sq.GetX() + xyz_scaled_sq.GetY();
+
+	// The off diagonal elements are calculated like:
+	// Ixy = -sum_{k = 1 to n}(x_k y_k)
+	// Ixz = -sum_{k = 1 to n}(x_k z_k)
+	// Iyz = -sum_{k = 1 to n}(y_k z_k)
+	// Scaling these is simple:
+	float i_xy = inScale.GetX() * inScale.GetY() * mInertia(0, 1);
+	float i_xz = inScale.GetX() * inScale.GetZ() * mInertia(0, 2);
+	float i_yz = inScale.GetY() * inScale.GetZ() * mInertia(1, 2);
+
+	// Update inertia tensor
+	mInertia(0, 0) = i_xx;
+	mInertia(0, 1) = i_xy;
+	mInertia(1, 0) = i_xy;
+	mInertia(1, 1) = i_yy;
+	mInertia(0, 2) = i_xz;
+	mInertia(2, 0) = i_xz;
+	mInertia(1, 2) = i_yz;
+	mInertia(2, 1) = i_yz;
+	mInertia(2, 2) = i_zz;
+
+	// Mass scales linear with volume (note that the scaling can be negative and we don't want the mass to become negative)
+	float mass_scale = abs(inScale.GetX() * inScale.GetY() * inScale.GetZ());
+	mMass *= mass_scale;
+
+	// Inertia scales linear with mass. This updates the m_k terms above.
+	mInertia *= mass_scale;
+
+	// Ensure that the bottom right element is a 1 again
+	mInertia(3, 3) = 1.0f;
+}
+
+void MassProperties::Rotate(Mat44Arg inRotation)
+{
+	mInertia = inRotation.Multiply3x3(mInertia).Multiply3x3RightTransposed(inRotation);
+}
+
+void MassProperties::Translate(Vec3Arg inTranslation)
+{
+	// Transform the inertia using the parallel axis theorem: I' = I + m * (translation^2 E - translation translation^T)
+	// Where I is the original body's inertia and E the identity matrix
+	// See: https://en.wikipedia.org/wiki/Parallel_axis_theorem
+	mInertia += mMass * (Mat44::sScale(inTranslation.Dot(inTranslation)) - Mat44::sOuterProduct(inTranslation, inTranslation));
+
+	// Ensure that inertia is a 3x3 matrix, adding inertias causes the bottom right element to change
+	mInertia.SetColumn4(3, Vec4(0, 0, 0, 1));
+}
+
+void MassProperties::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(mMass);
+	inStream.Write(mInertia);
+}
+
+void MassProperties::RestoreBinaryState(StreamIn &inStream)
+{
+	inStream.Read(mMass);
+	inStream.Read(mInertia);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/MassProperties.h b/thirdparty/jolt_physics/Jolt/Physics/Body/MassProperties.h
new file mode 100644
index 0000000000..c2bfbffb2e
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Body/MassProperties.h
@@ -0,0 +1,58 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/ObjectStream/SerializableObject.h>
+
+JPH_NAMESPACE_BEGIN
+
+class StreamIn;
+class StreamOut;
+
+/// Describes the mass and inertia properties of a body. Used during body construction only.
+class JPH_EXPORT MassProperties
+{
+	JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, MassProperties)
+
+public:
+	/// Using eigendecomposition, decompose the inertia tensor into a diagonal matrix D and a right-handed rotation matrix R so that the inertia tensor is \f$R \: D \: R^{-1}\f$.
+	/// @see https://en.wikipedia.org/wiki/Moment_of_inertia section 'Principal axes'
+	/// @param outRotation The rotation matrix R
+	/// @param outDiagonal The diagonal of the diagonal matrix D
+	/// @return True if successful, false if failed
+	bool					DecomposePrincipalMomentsOfInertia(Mat44 &outRotation, Vec3 &outDiagonal) const;
+
+	/// Set the mass and inertia of a box with edge size inBoxSize and density inDensity
+	void					SetMassAndInertiaOfSolidBox(Vec3Arg inBoxSize, float inDensity);
+
+	/// Set the mass and scale the inertia tensor to match the mass
+	void					ScaleToMass(float inMass);
+
+	/// Calculates the size of the solid box that has an inertia tensor diagonal inInertiaDiagonal
+	static Vec3				sGetEquivalentSolidBoxSize(float inMass, Vec3Arg inInertiaDiagonal);
+
+	/// Rotate the inertia by 3x3 matrix inRotation
+	void					Rotate(Mat44Arg inRotation);
+
+	/// Translate the inertia by a vector inTranslation
+	void					Translate(Vec3Arg inTranslation);
+
+	/// Scale the mass and inertia by inScale, note that elements can be < 0 to flip the shape
+	void					Scale(Vec3Arg inScale);
+
+	/// Saves the state of this object in binary form to inStream.
+	void					SaveBinaryState(StreamOut &inStream) const;
+
+	/// Restore the state of this object from inStream.
+	void					RestoreBinaryState(StreamIn &inStream);
+
+	/// Mass of the shape (kg)
+	float					mMass = 0.0f;
+
+	/// Inertia tensor of the shape (kg m^2)
+	Mat44					mInertia = Mat44::sZero();
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.cpp b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.cpp
new file mode 100644
index 0000000000..96aba179e1
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.cpp
@@ -0,0 +1,92 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Body/MotionProperties.h>
+#include <Jolt/Physics/StateRecorder.h>
+
+JPH_NAMESPACE_BEGIN
+
+void MotionProperties::SetMassProperties(EAllowedDOFs inAllowedDOFs, const MassProperties &inMassProperties)
+{
+	// Store allowed DOFs
+	mAllowedDOFs = inAllowedDOFs;
+
+	// Decompose DOFs
+	uint allowed_translation_axis = uint(inAllowedDOFs) & 0b111;
+	uint allowed_rotation_axis = (uint(inAllowedDOFs) >> 3) & 0b111;
+
+	// Set inverse mass
+	if (allowed_translation_axis == 0)
+	{
+		// No translation possible
+		mInvMass = 0.0f;
+	}
+	else
+	{
+		JPH_ASSERT(inMassProperties.mMass > 0.0f, "Invalid mass. "
+			"Some shapes like MeshShape or TriangleShape cannot calculate mass automatically, "
+			"in this case you need to provide it by setting BodyCreationSettings::mOverrideMassProperties and mMassPropertiesOverride.");
+		mInvMass = 1.0f / inMassProperties.mMass;
+	}
+
+	if (allowed_rotation_axis == 0)
+	{
+		// No rotation possible
+		mInvInertiaDiagonal = Vec3::sZero();
+		mInertiaRotation = Quat::sIdentity();
+	}
+	else
+	{
+		// Set inverse inertia
+		Mat44 rotation;
+		Vec3 diagonal;
+		if (inMassProperties.DecomposePrincipalMomentsOfInertia(rotation, diagonal)
+			&& !diagonal.IsNearZero())
+		{
+			mInvInertiaDiagonal = diagonal.Reciprocal();
+			mInertiaRotation = rotation.GetQuaternion();
+		}
+		else
+		{
+			// Failed! Fall back to inertia tensor of sphere with radius 1.
+			mInvInertiaDiagonal = Vec3::sReplicate(2.5f * mInvMass);
+			mInertiaRotation = Quat::sIdentity();
+		}
+	}
+
+	JPH_ASSERT(mInvMass != 0.0f || mInvInertiaDiagonal != Vec3::sZero(), "Can't lock all axes, use a static body for this. This will crash with a division by zero later!");
+}
+
+void MotionProperties::SaveState(StateRecorder &inStream) const
+{
+	// Only write properties that can change at runtime
+	inStream.Write(mLinearVelocity);
+	inStream.Write(mAngularVelocity);
+	inStream.Write(mForce);
+	inStream.Write(mTorque);
+#ifdef JPH_DOUBLE_PRECISION
+	inStream.Write(mSleepTestOffset);
+#endif // JPH_DOUBLE_PRECISION
+	inStream.Write(mSleepTestSpheres);
+	inStream.Write(mSleepTestTimer);
+	inStream.Write(mAllowSleeping);
+}
+
+void MotionProperties::RestoreState(StateRecorder &inStream)
+{
+	inStream.Read(mLinearVelocity);
+	inStream.Read(mAngularVelocity);
+	inStream.Read(mForce);
+	inStream.Read(mTorque);
+#ifdef JPH_DOUBLE_PRECISION
+	inStream.Read(mSleepTestOffset);
+#endif // JPH_DOUBLE_PRECISION
+	inStream.Read(mSleepTestSpheres);
+	inStream.Read(mSleepTestTimer);
+	inStream.Read(mAllowSleeping);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.h b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.h
new file mode 100644
index 0000000000..d8a485b611
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.h
@@ -0,0 +1,282 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/Sphere.h>
+#include <Jolt/Physics/Body/AllowedDOFs.h>
+#include <Jolt/Physics/Body/MotionQuality.h>
+#include <Jolt/Physics/Body/BodyAccess.h>
+#include <Jolt/Physics/Body/MotionType.h>
+#include <Jolt/Physics/Body/BodyType.h>
+#include <Jolt/Physics/Body/MassProperties.h>
+#include <Jolt/Physics/DeterminismLog.h>
+
+JPH_NAMESPACE_BEGIN
+
+class StateRecorder;
+
+/// Enum that determines if an object can go to sleep
+enum class ECanSleep
+{
+	CannotSleep = 0,																		///< Object cannot go to sleep
+	CanSleep = 1,																			///< Object can go to sleep
+};
+
+/// The Body class only keeps track of state for static bodies, the MotionProperties class keeps the additional state needed for a moving Body. It has a 1-on-1 relationship with the body.
+class JPH_EXPORT MotionProperties
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Motion quality, or how well it detects collisions when it has a high velocity
+	EMotionQuality			GetMotionQuality() const										{ return mMotionQuality; }
+
+	/// Get the allowed degrees of freedom that this body has (this can be changed by calling SetMassProperties)
+	inline EAllowedDOFs		GetAllowedDOFs() const											{ return mAllowedDOFs; }
+
+	/// If this body can go to sleep.
+	inline bool				GetAllowSleeping() const										{ return mAllowSleeping; }
+
+	/// Get world space linear velocity of the center of mass
+	inline Vec3				GetLinearVelocity() const										{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::Read)); return mLinearVelocity; }
+
+	/// Set world space linear velocity of the center of mass
+	void					SetLinearVelocity(Vec3Arg inLinearVelocity)						{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); JPH_ASSERT(inLinearVelocity.Length() <= mMaxLinearVelocity); mLinearVelocity = LockTranslation(inLinearVelocity); }
+
+	/// Set world space linear velocity of the center of mass, will make sure the value is clamped against the maximum linear velocity
+	void					SetLinearVelocityClamped(Vec3Arg inLinearVelocity)				{ mLinearVelocity = LockTranslation(inLinearVelocity); ClampLinearVelocity(); }
+
+	/// Get world space angular velocity of the center of mass
+	inline Vec3				GetAngularVelocity() const										{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::Read)); return mAngularVelocity; }
+
+	/// Set world space angular velocity of the center of mass
+	void					SetAngularVelocity(Vec3Arg inAngularVelocity)					{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); JPH_ASSERT(inAngularVelocity.Length() <= mMaxAngularVelocity); mAngularVelocity = LockAngular(inAngularVelocity); }
+
+	/// Set world space angular velocity of the center of mass, will make sure the value is clamped against the maximum angular velocity
+	void					SetAngularVelocityClamped(Vec3Arg inAngularVelocity)			{ mAngularVelocity = LockAngular(inAngularVelocity); ClampAngularVelocity(); }
+
+	/// Set velocity of body such that it will be rotate/translate by inDeltaPosition/Rotation in inDeltaTime seconds.
+	inline void				MoveKinematic(Vec3Arg inDeltaPosition, QuatArg inDeltaRotation, float inDeltaTime);
+
+	///@name Velocity limits
+	///@{
+
+	/// Maximum linear velocity that a body can achieve. Used to prevent the system from exploding.
+	inline float			GetMaxLinearVelocity() const									{ return mMaxLinearVelocity; }
+	inline void				SetMaxLinearVelocity(float inLinearVelocity)					{ JPH_ASSERT(inLinearVelocity >= 0.0f); mMaxLinearVelocity = inLinearVelocity; }
+
+	/// Maximum angular velocity that a body can achieve. Used to prevent the system from exploding.
+	inline float			GetMaxAngularVelocity() const									{ return mMaxAngularVelocity; }
+	inline void				SetMaxAngularVelocity(float inAngularVelocity)					{ JPH_ASSERT(inAngularVelocity >= 0.0f); mMaxAngularVelocity = inAngularVelocity; }
+	///@}
+
+	/// Clamp velocity according to limit
+	inline void				ClampLinearVelocity();
+	inline void				ClampAngularVelocity();
+
+	/// Get linear damping: dv/dt = -c * v. c must be between 0 and 1 but is usually close to 0.
+	inline float			GetLinearDamping() const										{ return mLinearDamping; }
+	void					SetLinearDamping(float inLinearDamping)							{ JPH_ASSERT(inLinearDamping >= 0.0f); mLinearDamping = inLinearDamping; }
+
+	/// Get angular damping: dw/dt = -c * w. c must be between 0 and 1 but is usually close to 0.
+	inline float			GetAngularDamping() const										{ return mAngularDamping; }
+	void					SetAngularDamping(float inAngularDamping)						{ JPH_ASSERT(inAngularDamping >= 0.0f); mAngularDamping = inAngularDamping; }
+
+	/// Get gravity factor (1 = normal gravity, 0 = no gravity)
+	inline float			GetGravityFactor() const										{ return mGravityFactor; }
+	void					SetGravityFactor(float inGravityFactor)							{ mGravityFactor = inGravityFactor; }
+
+	/// Set the mass and inertia tensor
+	void					SetMassProperties(EAllowedDOFs inAllowedDOFs, const MassProperties &inMassProperties);
+
+	/// Get inverse mass (1 / mass). Should only be called on a dynamic object (static or kinematic bodies have infinite mass so should be treated as 1 / mass = 0)
+	inline float			GetInverseMass() const											{ JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); return mInvMass; }
+	inline float			GetInverseMassUnchecked() const									{ return mInvMass; }
+
+	/// Set the inverse mass (1 / mass).
+	/// Note that mass and inertia are linearly related (e.g. inertia of a sphere with mass m and radius r is \f$2/5 \: m \: r^2\f$).
+	/// If you change mass, inertia should probably change as well. You can use ScaleToMass to update mass and inertia at the same time.
+	/// If all your translation degrees of freedom are restricted, make sure this is zero (see EAllowedDOFs).
+	void					SetInverseMass(float inInverseMass)								{ mInvMass = inInverseMass; }
+
+	/// Diagonal of inverse inertia matrix: D. Should only be called on a dynamic object (static or kinematic bodies have infinite mass so should be treated as D = 0)
+	inline Vec3				GetInverseInertiaDiagonal() const								{ JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); return mInvInertiaDiagonal; }
+
+	/// Rotation (R) that takes inverse inertia diagonal to local space: \f$I_{body}^{-1} = R \: D \: R^{-1}\f$
+	inline Quat				GetInertiaRotation() const										{ return mInertiaRotation; }
+
+	/// Set the inverse inertia tensor in local space by setting the diagonal and the rotation: \f$I_{body}^{-1} = R \: D \: R^{-1}\f$.
+	/// Note that mass and inertia are linearly related (e.g. inertia of a sphere with mass m and radius r is \f$2/5 \: m \: r^2\f$).
+	/// If you change inertia, mass should probably change as well. You can use ScaleToMass to update mass and inertia at the same time.
+	/// If all your rotation degrees of freedom are restricted, make sure this is zero (see EAllowedDOFs).
+	void					SetInverseInertia(Vec3Arg inDiagonal, QuatArg inRot)			{ mInvInertiaDiagonal = inDiagonal; mInertiaRotation = inRot; }
+
+	/// Sets the mass to inMass and scale the inertia tensor based on the ratio between the old and new mass.
+	/// Note that this only works when the current mass is finite (i.e. the body is dynamic and translational degrees of freedom are not restricted).
+	void					ScaleToMass(float inMass);
+
+	/// Get inverse inertia matrix (\f$I_{body}^{-1}\f$). Will be a matrix of zeros for a static or kinematic object.
+	inline Mat44			GetLocalSpaceInverseInertia() const;
+
+	/// Same as GetLocalSpaceInverseInertia() but doesn't check if the body is dynamic
+	inline Mat44			GetLocalSpaceInverseInertiaUnchecked() const;
+
+	/// Get inverse inertia matrix (\f$I^{-1}\f$) for a given object rotation (translation will be ignored). Zero if object is static or kinematic.
+	inline Mat44			GetInverseInertiaForRotation(Mat44Arg inRotation) const;
+
+	/// Multiply a vector with the inverse world space inertia tensor (\f$I_{world}^{-1}\f$). Zero if object is static or kinematic.
+	JPH_INLINE Vec3			MultiplyWorldSpaceInverseInertiaByVector(QuatArg inBodyRotation, Vec3Arg inV) const;
+
+	/// Velocity of point inPoint (in center of mass space, e.g. on the surface of the body) of the body (unit: m/s)
+	JPH_INLINE Vec3			GetPointVelocityCOM(Vec3Arg inPointRelativeToCOM) const			{ return mLinearVelocity + mAngularVelocity.Cross(inPointRelativeToCOM); }
+
+	// Get the total amount of force applied to the center of mass this time step (through Body::AddForce calls). Note that it will reset to zero after PhysicsSystem::Update.
+	JPH_INLINE Vec3			GetAccumulatedForce() const										{ return Vec3::sLoadFloat3Unsafe(mForce); }
+
+	// Get the total amount of torque applied to the center of mass this time step (through Body::AddForce/Body::AddTorque calls). Note that it will reset to zero after PhysicsSystem::Update.
+	JPH_INLINE Vec3			GetAccumulatedTorque() const									{ return Vec3::sLoadFloat3Unsafe(mTorque); }
+
+	// Reset the total accumulated force, note that this will be done automatically after every time step.
+	JPH_INLINE void			ResetForce()													{ mForce = Float3(0, 0, 0); }
+
+	// Reset the total accumulated torque, note that this will be done automatically after every time step.
+	JPH_INLINE void			ResetTorque()													{ mTorque = Float3(0, 0, 0); }
+
+	// Reset the current velocity and accumulated force and torque.
+	JPH_INLINE void			ResetMotion()
+	{
+		JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite));
+		mLinearVelocity = mAngularVelocity = Vec3::sZero();
+		mForce = mTorque = Float3(0, 0, 0);
+	}
+
+	/// Returns a vector where the linear components that are not allowed by mAllowedDOFs are set to 0 and the rest to 0xffffffff
+	JPH_INLINE UVec4		GetLinearDOFsMask() const
+	{
+		UVec4 mask(uint32(EAllowedDOFs::TranslationX), uint32(EAllowedDOFs::TranslationY), uint32(EAllowedDOFs::TranslationZ), 0);
+		return UVec4::sEquals(UVec4::sAnd(UVec4::sReplicate(uint32(mAllowedDOFs)), mask), mask);
+	}
+
+	/// Takes a translation vector inV and returns a vector where the components that are not allowed by mAllowedDOFs are set to 0
+	JPH_INLINE Vec3			LockTranslation(Vec3Arg inV) const
+	{
+		return Vec3::sAnd(inV, Vec3(GetLinearDOFsMask().ReinterpretAsFloat()));
+	}
+
+	/// Returns a vector where the angular components that are not allowed by mAllowedDOFs are set to 0 and the rest to 0xffffffff
+	JPH_INLINE UVec4		GetAngularDOFsMask() const
+	{
+		UVec4 mask(uint32(EAllowedDOFs::RotationX), uint32(EAllowedDOFs::RotationY), uint32(EAllowedDOFs::RotationZ), 0);
+		return UVec4::sEquals(UVec4::sAnd(UVec4::sReplicate(uint32(mAllowedDOFs)), mask), mask);
+	}
+
+	/// Takes an angular velocity / torque vector inV and returns a vector where the components that are not allowed by mAllowedDOFs are set to 0
+	JPH_INLINE Vec3			LockAngular(Vec3Arg inV) const
+	{
+		return Vec3::sAnd(inV, Vec3(GetAngularDOFsMask().ReinterpretAsFloat()));
+	}
+
+	/// Used only when this body is dynamic and colliding. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island.
+	void					SetNumVelocityStepsOverride(uint inN)							{ JPH_ASSERT(inN < 256); mNumVelocityStepsOverride = uint8(inN); }
+	uint					GetNumVelocityStepsOverride() const								{ return mNumVelocityStepsOverride; }
+
+	/// Used only when this body is dynamic and colliding. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island.
+	void					SetNumPositionStepsOverride(uint inN)							{ JPH_ASSERT(inN < 256); mNumPositionStepsOverride = uint8(inN); }
+	uint					GetNumPositionStepsOverride() const								{ return mNumPositionStepsOverride; }
+
+	////////////////////////////////////////////////////////////
+	// FUNCTIONS BELOW THIS LINE ARE FOR INTERNAL USE ONLY
+	////////////////////////////////////////////////////////////
+
+	///@name Update linear and angular velocity (used during constraint solving)
+	///@{
+	inline void				AddLinearVelocityStep(Vec3Arg inLinearVelocityChange)			{ JPH_DET_LOG("AddLinearVelocityStep: " << inLinearVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); mLinearVelocity = LockTranslation(mLinearVelocity + inLinearVelocityChange); JPH_ASSERT(!mLinearVelocity.IsNaN()); }
+	inline void				SubLinearVelocityStep(Vec3Arg inLinearVelocityChange)			{ JPH_DET_LOG("SubLinearVelocityStep: " << inLinearVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); mLinearVelocity = LockTranslation(mLinearVelocity - inLinearVelocityChange); JPH_ASSERT(!mLinearVelocity.IsNaN()); }
+	inline void				AddAngularVelocityStep(Vec3Arg inAngularVelocityChange)			{ JPH_DET_LOG("AddAngularVelocityStep: " << inAngularVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); mAngularVelocity += inAngularVelocityChange; JPH_ASSERT(!mAngularVelocity.IsNaN()); }
+	inline void				SubAngularVelocityStep(Vec3Arg inAngularVelocityChange)			{ JPH_DET_LOG("SubAngularVelocityStep: " << inAngularVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite)); mAngularVelocity -= inAngularVelocityChange; JPH_ASSERT(!mAngularVelocity.IsNaN()); }
+	///@}
+
+	/// Apply the gyroscopic force (aka Dzhanibekov effect, see https://en.wikipedia.org/wiki/Tennis_racket_theorem)
+	inline void				ApplyGyroscopicForceInternal(QuatArg inBodyRotation, float inDeltaTime);
+
+	/// Apply all accumulated forces, torques and drag (should only be called by the PhysicsSystem)
+	inline void				ApplyForceTorqueAndDragInternal(QuatArg inBodyRotation, Vec3Arg inGravity, float inDeltaTime);
+
+	/// Access to the island index
+	uint32					GetIslandIndexInternal() const									{ return mIslandIndex; }
+	void					SetIslandIndexInternal(uint32 inIndex)							{ mIslandIndex = inIndex; }
+
+	/// Access to the index in the active bodies array
+	uint32					GetIndexInActiveBodiesInternal() const							{ return mIndexInActiveBodies; }
+
+#ifdef JPH_DOUBLE_PRECISION
+	inline DVec3			GetSleepTestOffset() const										{ return DVec3::sLoadDouble3Unsafe(mSleepTestOffset); }
+#endif // JPH_DOUBLE_PRECISION
+
+	/// Reset spheres to center around inPoints with radius 0
+	inline void				ResetSleepTestSpheres(const RVec3 *inPoints);
+
+	/// Reset the sleep test timer without resetting the sleep test spheres
+	inline void				ResetSleepTestTimer()											{ mSleepTestTimer = 0.0f; }
+
+	/// Accumulate sleep time and return if a body can go to sleep
+	inline ECanSleep		AccumulateSleepTime(float inDeltaTime, float inTimeBeforeSleep);
+
+	/// Saving state for replay
+	void					SaveState(StateRecorder &inStream) const;
+
+	/// Restoring state for replay
+	void					RestoreState(StateRecorder &inStream);
+
+	static constexpr uint32	cInactiveIndex = uint32(-1);									///< Constant indicating that body is not active
+
+private:
+	friend class BodyManager;
+	friend class Body;
+
+	// 1st cache line
+	// 16 byte aligned
+	Vec3					mLinearVelocity { Vec3::sZero() };								///< World space linear velocity of the center of mass (m/s)
+	Vec3					mAngularVelocity { Vec3::sZero() };								///< World space angular velocity (rad/s)
+	Vec3					mInvInertiaDiagonal;											///< Diagonal of inverse inertia matrix: D
+	Quat					mInertiaRotation;												///< Rotation (R) that takes inverse inertia diagonal to local space: Ibody^-1 = R * D * R^-1
+
+	// 2nd cache line
+	// 4 byte aligned
+	Float3					mForce { 0, 0, 0 };												///< Accumulated world space force (N). Note loaded through intrinsics so ensure that the 4 bytes after this are readable!
+	Float3					mTorque { 0, 0, 0 };											///< Accumulated world space torque (N m). Note loaded through intrinsics so ensure that the 4 bytes after this are readable!
+	float					mInvMass;														///< Inverse mass of the object (1/kg)
+	float					mLinearDamping;													///< Linear damping: dv/dt = -c * v. c must be between 0 and 1 but is usually close to 0.
+	float					mAngularDamping;												///< Angular damping: dw/dt = -c * w. c must be between 0 and 1 but is usually close to 0.
+	float					mMaxLinearVelocity;												///< Maximum linear velocity that this body can reach (m/s)
+	float					mMaxAngularVelocity;											///< Maximum angular velocity that this body can reach (rad/s)
+	float					mGravityFactor;													///< Factor to multiply gravity with
+	uint32					mIndexInActiveBodies = cInactiveIndex;							///< If the body is active, this is the index in the active body list or cInactiveIndex if it is not active (note that there are 2 lists, one for rigid and one for soft bodies)
+	uint32					mIslandIndex = cInactiveIndex;									///< Index of the island that this body is part of, when the body has not yet been updated or is not active this is cInactiveIndex
+
+	// 1 byte aligned
+	EMotionQuality			mMotionQuality;													///< Motion quality, or how well it detects collisions when it has a high velocity
+	bool					mAllowSleeping;													///< If this body can go to sleep
+	EAllowedDOFs			mAllowedDOFs = EAllowedDOFs::All;								///< Allowed degrees of freedom for this body
+	uint8					mNumVelocityStepsOverride = 0;									///< Used only when this body is dynamic and colliding. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island.
+	uint8					mNumPositionStepsOverride = 0;									///< Used only when this body is dynamic and colliding. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island.
+
+	// 3rd cache line (least frequently used)
+	// 4 byte aligned (or 8 byte if running in double precision)
+#ifdef JPH_DOUBLE_PRECISION
+	Double3					mSleepTestOffset;												///< mSleepTestSpheres are relative to this offset to prevent floating point inaccuracies. Warning: Loaded using sLoadDouble3Unsafe which will read 8 extra bytes.
+#endif // JPH_DOUBLE_PRECISION
+	Sphere					mSleepTestSpheres[3];											///< Measure motion for 3 points on the body to see if it is resting: COM, COM + largest bounding box axis, COM + second largest bounding box axis
+	float					mSleepTestTimer;												///< How long this body has been within the movement tolerance
+
+#ifdef JPH_ENABLE_ASSERTS
+	EBodyType				mCachedBodyType;												///< Copied from Body::mBodyType and cached for asserting purposes
+	EMotionType				mCachedMotionType;												///< Copied from Body::mMotionType and cached for asserting purposes
+#endif
+};
+
+JPH_NAMESPACE_END
+
+#include "MotionProperties.inl"
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.inl b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.inl
new file mode 100644
index 0000000000..e9521f7444
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.inl
@@ -0,0 +1,178 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+void MotionProperties::MoveKinematic(Vec3Arg inDeltaPosition, QuatArg inDeltaRotation, float inDeltaTime)
+{
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite));
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read));
+	JPH_ASSERT(mCachedBodyType == EBodyType::RigidBody);
+	JPH_ASSERT(mCachedMotionType != EMotionType::Static);
+
+	// Calculate required linear velocity
+	mLinearVelocity = LockTranslation(inDeltaPosition / inDeltaTime);
+
+	// Calculate required angular velocity
+	Vec3 axis;
+	float angle;
+	inDeltaRotation.GetAxisAngle(axis, angle);
+	mAngularVelocity = LockAngular(axis * (angle / inDeltaTime));
+}
+
+void MotionProperties::ClampLinearVelocity()
+{
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite));
+
+	float len_sq = mLinearVelocity.LengthSq();
+	JPH_ASSERT(isfinite(len_sq));
+	if (len_sq > Square(mMaxLinearVelocity))
+		mLinearVelocity *= mMaxLinearVelocity / sqrt(len_sq);
+}
+
+void MotionProperties::ClampAngularVelocity()
+{
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite));
+
+	float len_sq = mAngularVelocity.LengthSq();
+	JPH_ASSERT(isfinite(len_sq));
+	if (len_sq > Square(mMaxAngularVelocity))
+		mAngularVelocity *= mMaxAngularVelocity / sqrt(len_sq);
+}
+
+inline Mat44 MotionProperties::GetLocalSpaceInverseInertiaUnchecked() const
+{
+	Mat44 rotation = Mat44::sRotation(mInertiaRotation);
+	Mat44 rotation_mul_scale_transposed(mInvInertiaDiagonal.SplatX() * rotation.GetColumn4(0), mInvInertiaDiagonal.SplatY() * rotation.GetColumn4(1), mInvInertiaDiagonal.SplatZ() * rotation.GetColumn4(2), Vec4(0, 0, 0, 1));
+	return rotation.Multiply3x3RightTransposed(rotation_mul_scale_transposed);
+}
+
+inline void MotionProperties::ScaleToMass(float inMass)
+{
+	JPH_ASSERT(mInvMass > 0.0f, "Body must have finite mass");
+	JPH_ASSERT(inMass > 0.0f, "New mass cannot be zero");
+
+	float new_inv_mass = 1.0f / inMass;
+	mInvInertiaDiagonal *= new_inv_mass / mInvMass;
+	mInvMass = new_inv_mass;
+}
+
+inline Mat44 MotionProperties::GetLocalSpaceInverseInertia() const
+{
+	JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);
+	return GetLocalSpaceInverseInertiaUnchecked();
+}
+
+Mat44 MotionProperties::GetInverseInertiaForRotation(Mat44Arg inRotation) const
+{
+	JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);
+
+	Mat44 rotation = inRotation.Multiply3x3(Mat44::sRotation(mInertiaRotation));
+	Mat44 rotation_mul_scale_transposed(mInvInertiaDiagonal.SplatX() * rotation.GetColumn4(0), mInvInertiaDiagonal.SplatY() * rotation.GetColumn4(1), mInvInertiaDiagonal.SplatZ() * rotation.GetColumn4(2), Vec4(0, 0, 0, 1));
+	Mat44 inverse_inertia = rotation.Multiply3x3RightTransposed(rotation_mul_scale_transposed);
+
+	// We need to mask out both the rows and columns of DOFs that are not allowed
+	Vec4 angular_dofs_mask = GetAngularDOFsMask().ReinterpretAsFloat();
+	inverse_inertia.SetColumn4(0, Vec4::sAnd(inverse_inertia.GetColumn4(0), Vec4::sAnd(angular_dofs_mask, angular_dofs_mask.SplatX())));
+	inverse_inertia.SetColumn4(1, Vec4::sAnd(inverse_inertia.GetColumn4(1), Vec4::sAnd(angular_dofs_mask, angular_dofs_mask.SplatY())));
+	inverse_inertia.SetColumn4(2, Vec4::sAnd(inverse_inertia.GetColumn4(2), Vec4::sAnd(angular_dofs_mask, angular_dofs_mask.SplatZ())));
+
+	return inverse_inertia;
+}
+
+Vec3 MotionProperties::MultiplyWorldSpaceInverseInertiaByVector(QuatArg inBodyRotation, Vec3Arg inV) const
+{
+	JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);
+
+	// Mask out columns of DOFs that are not allowed
+	Vec3 angular_dofs_mask = Vec3(GetAngularDOFsMask().ReinterpretAsFloat());
+	Vec3 v = Vec3::sAnd(inV, angular_dofs_mask);
+
+	// Multiply vector by inverse inertia
+	Mat44 rotation = Mat44::sRotation(inBodyRotation * mInertiaRotation);
+	Vec3 result = rotation.Multiply3x3(mInvInertiaDiagonal * rotation.Multiply3x3Transposed(v));
+
+	// Mask out rows of DOFs that are not allowed
+	return Vec3::sAnd(result, angular_dofs_mask);
+}
+
+void MotionProperties::ApplyGyroscopicForceInternal(QuatArg inBodyRotation, float inDeltaTime)
+{
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite));
+	JPH_ASSERT(mCachedBodyType == EBodyType::RigidBody);
+	JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);
+
+	// Calculate local space inertia tensor (a diagonal in local space)
+	UVec4 is_zero = Vec3::sEquals(mInvInertiaDiagonal, Vec3::sZero());
+	Vec3 denominator = Vec3::sSelect(mInvInertiaDiagonal, Vec3::sReplicate(1.0f), is_zero);
+	Vec3 nominator = Vec3::sSelect(Vec3::sReplicate(1.0f), Vec3::sZero(), is_zero);
+	Vec3 local_inertia = nominator / denominator; // Avoid dividing by zero, inertia in this axis will be zero
+
+	// Calculate local space angular momentum
+	Quat inertia_space_to_world_space = inBodyRotation * mInertiaRotation;
+	Vec3 local_angular_velocity = inertia_space_to_world_space.Conjugated() * mAngularVelocity;
+	Vec3 local_momentum = local_inertia * local_angular_velocity;
+
+	// The gyroscopic force applies a torque: T = -w x I w where w is angular velocity and I the inertia tensor
+	// Calculate the new angular momentum by applying the gyroscopic force and make sure the new magnitude is the same as the old one
+	// to avoid introducing energy into the system due to the Euler step
+	Vec3 new_local_momentum = local_momentum - inDeltaTime * local_angular_velocity.Cross(local_momentum);
+	float new_local_momentum_len_sq = new_local_momentum.LengthSq();
+	new_local_momentum = new_local_momentum_len_sq > 0.0f? new_local_momentum * sqrt(local_momentum.LengthSq() / new_local_momentum_len_sq) : Vec3::sZero();
+
+	// Convert back to world space angular velocity
+	mAngularVelocity = inertia_space_to_world_space * (mInvInertiaDiagonal * new_local_momentum);
+}
+
+void MotionProperties::ApplyForceTorqueAndDragInternal(QuatArg inBodyRotation, Vec3Arg inGravity, float inDeltaTime)
+{
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess(), BodyAccess::EAccess::ReadWrite));
+	JPH_ASSERT(mCachedBodyType == EBodyType::RigidBody);
+	JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);
+
+	// Update linear velocity
+	mLinearVelocity = LockTranslation(mLinearVelocity + inDeltaTime * (mGravityFactor * inGravity + mInvMass * GetAccumulatedForce()));
+
+	// Update angular velocity
+	mAngularVelocity += inDeltaTime * MultiplyWorldSpaceInverseInertiaByVector(inBodyRotation, GetAccumulatedTorque());
+
+	// Linear damping: dv/dt = -c * v
+	// Solution: v(t) = v(0) * e^(-c * t) or v2 = v1 * e^(-c * dt)
+	// Taylor expansion of e^(-c * dt) = 1 - c * dt + ...
+	// Since dt is usually in the order of 1/60 and c is a low number too this approximation is good enough
+	mLinearVelocity *= max(0.0f, 1.0f - mLinearDamping * inDeltaTime);
+	mAngularVelocity *= max(0.0f, 1.0f - mAngularDamping * inDeltaTime);
+
+	// Clamp velocities
+	ClampLinearVelocity();
+	ClampAngularVelocity();
+}
+
+void MotionProperties::ResetSleepTestSpheres(const RVec3 *inPoints)
+{
+#ifdef JPH_DOUBLE_PRECISION
+	// Make spheres relative to the first point and initialize them to zero radius
+	DVec3 offset = inPoints[0];
+	offset.StoreDouble3(&mSleepTestOffset);
+	mSleepTestSpheres[0] = Sphere(Vec3::sZero(), 0.0f);
+	for (int i = 1; i < 3; ++i)
+		mSleepTestSpheres[i] = Sphere(Vec3(inPoints[i] - offset), 0.0f);
+#else
+	// Initialize the spheres to zero radius around the supplied points
+	for (int i = 0; i < 3; ++i)
+		mSleepTestSpheres[i] = Sphere(inPoints[i], 0.0f);
+#endif
+
+	mSleepTestTimer = 0.0f;
+}
+
+ECanSleep MotionProperties::AccumulateSleepTime(float inDeltaTime, float inTimeBeforeSleep)
+{
+	mSleepTestTimer += inDeltaTime;
+	return mSleepTestTimer >= inTimeBeforeSleep? ECanSleep::CanSleep : ECanSleep::CannotSleep;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/MotionQuality.h b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionQuality.h
new file mode 100644
index 0000000000..b1ba343c06
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionQuality.h
@@ -0,0 +1,31 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Motion quality, or how well it detects collisions when it has a high velocity
+enum class EMotionQuality : uint8
+{
+	/// Update the body in discrete steps. Body will tunnel throuh thin objects if its velocity is high enough.
+	/// This is the cheapest way of simulating a body.
+	Discrete,
+
+	/// Update the body using linear casting. When stepping the body, its collision shape is cast from
+	/// start to destination using the starting rotation. The body will not be able to tunnel through thin
+	/// objects at high velocity, but tunneling is still possible if the body is long and thin and has high
+	/// angular velocity. Time is stolen from the object (which means it will move up to the first collision
+	/// and will not bounce off the surface until the next integration step). This will make the body appear
+	/// to go slower when it collides with high velocity. In order to not get stuck, the body is always
+	/// allowed to move by a fraction of it's inner radius, which may eventually lead it to pass through geometry.
+	///
+	/// Note that if you're using a collision listener, you can receive contact added/persisted notifications of contacts
+	/// that may in the end not happen. This happens between bodies that are using casting: If bodies A and B collide at t1
+	/// and B and C collide at t2 where t2 < t1 and A and C don't collide. In this case you may receive an incorrect contact
+	/// point added callback between A and B (which will be removed the next frame).
+	LinearCast,
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/MotionType.h b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionType.h
new file mode 100644
index 0000000000..6de0d8c8ec
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionType.h
@@ -0,0 +1,17 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Motion type of a physics body
+enum class EMotionType : uint8
+{
+	Static,						///< Non movable
+	Kinematic,					///< Movable using velocities only, does not respond to forces
+	Dynamic,					///< Responds to forces as a normal physics object
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Character/Character.cpp b/thirdparty/jolt_physics/Jolt/Physics/Character/Character.cpp
new file mode 100644
index 0000000000..14b2312870
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Character/Character.cpp
@@ -0,0 +1,323 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Character/Character.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Physics/Body/BodyLock.h>
+#include <Jolt/Physics/Collision/CollideShape.h>
+#include <Jolt/Physics/PhysicsSystem.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+JPH_NAMESPACE_BEGIN
+
+static inline const BodyLockInterface &sGetBodyLockInterface(const PhysicsSystem *inSystem, bool inLockBodies)
+{
+	return inLockBodies? static_cast<const BodyLockInterface &>(inSystem->GetBodyLockInterface()) : static_cast<const BodyLockInterface &>(inSystem->GetBodyLockInterfaceNoLock());
+}
+
+static inline BodyInterface &sGetBodyInterface(PhysicsSystem *inSystem, bool inLockBodies)
+{
+	return inLockBodies? inSystem->GetBodyInterface() : inSystem->GetBodyInterfaceNoLock();
+}
+
+static inline const NarrowPhaseQuery &sGetNarrowPhaseQuery(const PhysicsSystem *inSystem, bool inLockBodies)
+{
+	return inLockBodies? inSystem->GetNarrowPhaseQuery() : inSystem->GetNarrowPhaseQueryNoLock();
+}
+
+Character::Character(const CharacterSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, uint64 inUserData, PhysicsSystem *inSystem) :
+	CharacterBase(inSettings, inSystem),
+	mLayer(inSettings->mLayer)
+{
+	// Construct rigid body
+	BodyCreationSettings settings(mShape, inPosition, inRotation, EMotionType::Dynamic, mLayer);
+	settings.mAllowedDOFs = EAllowedDOFs::TranslationX | EAllowedDOFs::TranslationY | EAllowedDOFs::TranslationZ;
+	settings.mEnhancedInternalEdgeRemoval = inSettings->mEnhancedInternalEdgeRemoval;
+	settings.mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided;
+	settings.mMassPropertiesOverride.mMass = inSettings->mMass;
+	settings.mFriction = inSettings->mFriction;
+	settings.mGravityFactor = inSettings->mGravityFactor;
+	settings.mUserData = inUserData;
+	const Body *body = mSystem->GetBodyInterface().CreateBody(settings);
+	if (body != nullptr)
+		mBodyID = body->GetID();
+}
+
+Character::~Character()
+{
+	// Destroy the body
+	mSystem->GetBodyInterface().DestroyBody(mBodyID);
+}
+
+void Character::AddToPhysicsSystem(EActivation inActivationMode, bool inLockBodies)
+{
+	sGetBodyInterface(mSystem, inLockBodies).AddBody(mBodyID, inActivationMode);
+}
+
+void Character::RemoveFromPhysicsSystem(bool inLockBodies)
+{
+	sGetBodyInterface(mSystem, inLockBodies).RemoveBody(mBodyID);
+}
+
+void Character::Activate(bool inLockBodies)
+{
+	sGetBodyInterface(mSystem, inLockBodies).ActivateBody(mBodyID);
+}
+
+void Character::CheckCollision(RMat44Arg inCenterOfMassTransform, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const
+{
+	// Create query broadphase layer filter
+	DefaultBroadPhaseLayerFilter broadphase_layer_filter = mSystem->GetDefaultBroadPhaseLayerFilter(mLayer);
+
+	// Create query object layer filter
+	DefaultObjectLayerFilter object_layer_filter = mSystem->GetDefaultLayerFilter(mLayer);
+
+	// Ignore my own body
+	IgnoreSingleBodyFilter body_filter(mBodyID);
+
+	// Settings for collide shape
+	CollideShapeSettings settings;
+	settings.mMaxSeparationDistance = inMaxSeparationDistance;
+	settings.mActiveEdgeMode = EActiveEdgeMode::CollideOnlyWithActive;
+	settings.mActiveEdgeMovementDirection = inMovementDirection;
+	settings.mBackFaceMode = EBackFaceMode::IgnoreBackFaces;
+
+	sGetNarrowPhaseQuery(mSystem, inLockBodies).CollideShape(inShape, Vec3::sReplicate(1.0f), inCenterOfMassTransform, settings, inBaseOffset, ioCollector, broadphase_layer_filter, object_layer_filter, body_filter);
+}
+
+void Character::CheckCollision(RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const
+{
+	// Calculate center of mass transform
+	RMat44 center_of_mass = RMat44::sRotationTranslation(inRotation, inPosition).PreTranslated(inShape->GetCenterOfMass());
+
+	CheckCollision(center_of_mass, inMovementDirection, inMaxSeparationDistance, inShape, inBaseOffset, ioCollector, inLockBodies);
+}
+
+void Character::CheckCollision(const Shape *inShape, float inMaxSeparationDistance, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const
+{
+	// Determine position and velocity of body
+	RMat44 query_transform;
+	Vec3 velocity;
+	{
+		BodyLockRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyID);
+		if (!lock.Succeeded())
+			return;
+
+		const Body &body = lock.GetBody();
+
+		// Correct the center of mass transform for the difference between the old and new center of mass shape
+		query_transform = body.GetCenterOfMassTransform().PreTranslated(inShape->GetCenterOfMass() - mShape->GetCenterOfMass());
+		velocity = body.GetLinearVelocity();
+	}
+
+	CheckCollision(query_transform, velocity, inMaxSeparationDistance, inShape, inBaseOffset, ioCollector, inLockBodies);
+}
+
+void Character::PostSimulation(float inMaxSeparationDistance, bool inLockBodies)
+{
+	// Get character position, rotation and velocity
+	RVec3 char_pos;
+	Quat char_rot;
+	Vec3 char_vel;
+	{
+		BodyLockRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyID);
+		if (!lock.Succeeded())
+			return;
+		const Body &body = lock.GetBody();
+		char_pos = body.GetPosition();
+		char_rot = body.GetRotation();
+		char_vel = body.GetLinearVelocity();
+	}
+
+	// Collector that finds the hit with the normal that is the most 'up'
+	class MyCollector : public CollideShapeCollector
+	{
+	public:
+		// Constructor
+		explicit			MyCollector(Vec3Arg inUp, RVec3 inBaseOffset) : mBaseOffset(inBaseOffset), mUp(inUp) { }
+
+		// See: CollectorType::AddHit
+		virtual void		AddHit(const CollideShapeResult &inResult) override
+		{
+			Vec3 normal = -inResult.mPenetrationAxis.Normalized();
+			float dot = normal.Dot(mUp);
+			if (dot > mBestDot) // Find the hit that is most aligned with the up vector
+			{
+				mGroundBodyID = inResult.mBodyID2;
+				mGroundBodySubShapeID = inResult.mSubShapeID2;
+				mGroundPosition = mBaseOffset + inResult.mContactPointOn2;
+				mGroundNormal = normal;
+				mBestDot = dot;
+			}
+		}
+
+		BodyID				mGroundBodyID;
+		SubShapeID			mGroundBodySubShapeID;
+		RVec3				mGroundPosition = RVec3::sZero();
+		Vec3				mGroundNormal = Vec3::sZero();
+
+	private:
+		RVec3				mBaseOffset;
+		Vec3				mUp;
+		float				mBestDot = -FLT_MAX;
+	};
+
+	// Collide shape
+	MyCollector collector(mUp, char_pos);
+	CheckCollision(char_pos, char_rot, char_vel, inMaxSeparationDistance, mShape, char_pos, collector, inLockBodies);
+
+	// Copy results
+	mGroundBodyID = collector.mGroundBodyID;
+	mGroundBodySubShapeID = collector.mGroundBodySubShapeID;
+	mGroundPosition = collector.mGroundPosition;
+	mGroundNormal = collector.mGroundNormal;
+
+	// Get additional data from body
+	BodyLockRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mGroundBodyID);
+	if (lock.Succeeded())
+	{
+		const Body &body = lock.GetBody();
+
+		// Update ground state
+		RMat44 inv_transform = RMat44::sInverseRotationTranslation(char_rot, char_pos);
+		if (mSupportingVolume.SignedDistance(Vec3(inv_transform * mGroundPosition)) > 0.0f)
+			mGroundState = EGroundState::NotSupported;
+		else if (IsSlopeTooSteep(mGroundNormal))
+			mGroundState = EGroundState::OnSteepGround;
+		else
+			mGroundState = EGroundState::OnGround;
+
+		// Copy other body properties
+		mGroundMaterial = body.GetShape()->GetMaterial(mGroundBodySubShapeID);
+		mGroundVelocity = body.GetPointVelocity(mGroundPosition);
+		mGroundUserData = body.GetUserData();
+	}
+	else
+	{
+		mGroundState = EGroundState::InAir;
+		mGroundMaterial = PhysicsMaterial::sDefault;
+		mGroundVelocity = Vec3::sZero();
+		mGroundUserData = 0;
+	}
+}
+
+void Character::SetLinearAndAngularVelocity(Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, bool inLockBodies)
+{
+	sGetBodyInterface(mSystem, inLockBodies).SetLinearAndAngularVelocity(mBodyID, inLinearVelocity, inAngularVelocity);
+}
+
+Vec3 Character::GetLinearVelocity(bool inLockBodies) const
+{
+	return sGetBodyInterface(mSystem, inLockBodies).GetLinearVelocity(mBodyID);
+}
+
+void Character::SetLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies)
+{
+	sGetBodyInterface(mSystem, inLockBodies).SetLinearVelocity(mBodyID, inLinearVelocity);
+}
+
+void Character::AddLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies)
+{
+	sGetBodyInterface(mSystem, inLockBodies).AddLinearVelocity(mBodyID, inLinearVelocity);
+}
+
+void Character::AddImpulse(Vec3Arg inImpulse, bool inLockBodies)
+{
+	sGetBodyInterface(mSystem, inLockBodies).AddImpulse(mBodyID, inImpulse);
+}
+
+void Character::GetPositionAndRotation(RVec3 &outPosition, Quat &outRotation, bool inLockBodies) const
+{
+	sGetBodyInterface(mSystem, inLockBodies).GetPositionAndRotation(mBodyID, outPosition, outRotation);
+}
+
+void Character::SetPositionAndRotation(RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode, bool inLockBodies) const
+{
+	sGetBodyInterface(mSystem, inLockBodies).SetPositionAndRotation(mBodyID, inPosition, inRotation, inActivationMode);
+}
+
+RVec3 Character::GetPosition(bool inLockBodies) const
+{
+	return sGetBodyInterface(mSystem, inLockBodies).GetPosition(mBodyID);
+}
+
+void Character::SetPosition(RVec3Arg inPosition, EActivation inActivationMode, bool inLockBodies)
+{
+	sGetBodyInterface(mSystem, inLockBodies).SetPosition(mBodyID, inPosition, inActivationMode);
+}
+
+Quat Character::GetRotation(bool inLockBodies) const
+{
+	return sGetBodyInterface(mSystem, inLockBodies).GetRotation(mBodyID);
+}
+
+void Character::SetRotation(QuatArg inRotation, EActivation inActivationMode, bool inLockBodies)
+{
+	sGetBodyInterface(mSystem, inLockBodies).SetRotation(mBodyID, inRotation, inActivationMode);
+}
+
+RVec3 Character::GetCenterOfMassPosition(bool inLockBodies) const
+{
+	return sGetBodyInterface(mSystem, inLockBodies).GetCenterOfMassPosition(mBodyID);
+}
+
+RMat44 Character::GetWorldTransform(bool inLockBodies) const
+{
+	return sGetBodyInterface(mSystem, inLockBodies).GetWorldTransform(mBodyID);
+}
+
+void Character::SetLayer(ObjectLayer inLayer, bool inLockBodies)
+{
+	mLayer = inLayer;
+
+	sGetBodyInterface(mSystem, inLockBodies).SetObjectLayer(mBodyID, inLayer);
+}
+
+bool Character::SetShape(const Shape *inShape, float inMaxPenetrationDepth, bool inLockBodies)
+{
+	if (inMaxPenetrationDepth < FLT_MAX)
+	{
+		// Collector that checks if there is anything in the way while switching to inShape
+		class MyCollector : public CollideShapeCollector
+		{
+		public:
+			// Constructor
+			explicit			MyCollector(float inMaxPenetrationDepth) : mMaxPenetrationDepth(inMaxPenetrationDepth) { }
+
+			// See: CollectorType::AddHit
+			virtual void		AddHit(const CollideShapeResult &inResult) override
+			{
+				if (inResult.mPenetrationDepth > mMaxPenetrationDepth)
+				{
+					mHadCollision = true;
+					ForceEarlyOut();
+				}
+			}
+
+			float				mMaxPenetrationDepth;
+			bool				mHadCollision = false;
+		};
+
+		// Test if anything is in the way of switching
+		RVec3 char_pos = GetPosition(inLockBodies);
+		MyCollector collector(inMaxPenetrationDepth);
+		CheckCollision(inShape, 0.0f, char_pos, collector, inLockBodies);
+		if (collector.mHadCollision)
+			return false;
+	}
+
+	// Switch the shape
+	mShape = inShape;
+	sGetBodyInterface(mSystem, inLockBodies).SetShape(mBodyID, mShape, false, EActivation::Activate);
+	return true;
+}
+
+TransformedShape Character::GetTransformedShape(bool inLockBodies) const
+{
+	return sGetBodyInterface(mSystem, inLockBodies).GetTransformedShape(mBodyID);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Character/Character.h b/thirdparty/jolt_physics/Jolt/Physics/Character/Character.h
new file mode 100644
index 0000000000..db67f8b3b6
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Character/Character.h
@@ -0,0 +1,147 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Character/CharacterBase.h>
+#include <Jolt/Physics/Collision/ObjectLayer.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/EActivation.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Contains the configuration of a character
+class JPH_EXPORT CharacterSettings : public CharacterBaseSettings
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Layer that this character will be added to
+	ObjectLayer							mLayer = 0;
+
+	/// Mass of the character
+	float								mMass = 80.0f;
+
+	/// Friction for the character
+	float								mFriction = 0.2f;
+
+	/// Value to multiply gravity with for this character
+	float								mGravityFactor = 1.0f;
+};
+
+/// Runtime character object.
+/// This object usually represents the player or a humanoid AI. It uses a single rigid body,
+/// usually with a capsule shape to simulate movement and collision for the character.
+/// The character is a keyframed object, the application controls it by setting the velocity.
+class JPH_EXPORT Character : public CharacterBase
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+	/// @param inSettings The settings for the character
+	/// @param inPosition Initial position for the character
+	/// @param inRotation Initial rotation for the character (usually only around Y)
+	/// @param inUserData Application specific value
+	/// @param inSystem Physics system that this character will be added to later
+										Character(const CharacterSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, uint64 inUserData, PhysicsSystem *inSystem);
+
+	/// Destructor
+	virtual								~Character() override;
+
+	/// Add bodies and constraints to the system and optionally activate the bodies
+	void								AddToPhysicsSystem(EActivation inActivationMode = EActivation::Activate, bool inLockBodies = true);
+
+	/// Remove bodies and constraints from the system
+	void								RemoveFromPhysicsSystem(bool inLockBodies = true);
+
+	/// Wake up the character
+	void								Activate(bool inLockBodies = true);
+
+	/// Needs to be called after every PhysicsSystem::Update
+	/// @param inMaxSeparationDistance Max distance between the floor and the character to still consider the character standing on the floor
+	/// @param inLockBodies If the collision query should use the locking body interface (true) or the non locking body interface (false)
+	void								PostSimulation(float inMaxSeparationDistance, bool inLockBodies = true);
+
+	/// Control the velocity of the character
+	void								SetLinearAndAngularVelocity(Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, bool inLockBodies = true);
+
+	/// Get the linear velocity of the character (m / s)
+	Vec3								GetLinearVelocity(bool inLockBodies = true) const;
+
+	/// Set the linear velocity of the character (m / s)
+	void								SetLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies = true);
+
+	/// Add world space linear velocity to current velocity (m / s)
+	void								AddLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies = true);
+
+	/// Add impulse to the center of mass of the character
+	void								AddImpulse(Vec3Arg inImpulse, bool inLockBodies = true);
+
+	/// Get the body associated with this character
+	BodyID								GetBodyID() const										{ return mBodyID; }
+
+	/// Get position / rotation of the body
+	void								GetPositionAndRotation(RVec3 &outPosition, Quat &outRotation, bool inLockBodies = true) const;
+
+	/// Set the position / rotation of the body, optionally activating it.
+	void								SetPositionAndRotation(RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode = EActivation::Activate, bool inLockBodies = true) const;
+
+	/// Get the position of the character
+	RVec3								GetPosition(bool inLockBodies = true) const;
+
+	/// Set the position of the character, optionally activating it.
+	void								SetPosition(RVec3Arg inPostion, EActivation inActivationMode = EActivation::Activate, bool inLockBodies = true);
+
+	/// Get the rotation of the character
+	Quat								GetRotation(bool inLockBodies = true) const;
+
+	/// Set the rotation of the character, optionally activating it.
+	void								SetRotation(QuatArg inRotation, EActivation inActivationMode = EActivation::Activate, bool inLockBodies = true);
+
+	/// Position of the center of mass of the underlying rigid body
+	RVec3								GetCenterOfMassPosition(bool inLockBodies = true) const;
+
+	/// Calculate the world transform of the character
+	RMat44								GetWorldTransform(bool inLockBodies = true) const;
+
+	/// Get the layer of the character
+	ObjectLayer							GetLayer() const										{ return mLayer; }
+
+	/// Update the layer of the character
+	void								SetLayer(ObjectLayer inLayer, bool inLockBodies = true);
+
+	/// Switch the shape of the character (e.g. for stance). When inMaxPenetrationDepth is not FLT_MAX, it checks
+	/// if the new shape collides before switching shape. Returns true if the switch succeeded.
+	bool								SetShape(const Shape *inShape, float inMaxPenetrationDepth, bool inLockBodies = true);
+
+	/// Get the transformed shape that represents the volume of the character, can be used for collision checks.
+	TransformedShape					GetTransformedShape(bool inLockBodies = true) const;
+
+	/// @brief Get all contacts for the character at a particular location
+	/// @param inPosition Position to test.
+	/// @param inRotation Rotation at which to test the shape.
+	/// @param inMovementDirection A hint in which direction the character is moving, will be used to calculate a proper normal.
+	/// @param inMaxSeparationDistance How much distance around the character you want to report contacts in (can be 0 to match the character exactly).
+	/// @param inShape Shape to test collision with.
+	/// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. GetPosition() since floats are most accurate near the origin
+	/// @param ioCollector Collision collector that receives the collision results.
+	/// @param inLockBodies If the collision query should use the locking body interface (true) or the non locking body interface (false)
+	void								CheckCollision(RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies = true) const;
+
+private:
+	/// Check collisions between inShape and the world using the center of mass transform
+	void								CheckCollision(RMat44Arg inCenterOfMassTransform, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const;
+
+	/// Check collisions between inShape and the world using the current position / rotation of the character
+	void								CheckCollision(const Shape *inShape, float inMaxSeparationDistance, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const;
+
+	/// The body of this character
+	BodyID								mBodyID;
+
+	/// The layer the body is in
+	ObjectLayer							mLayer;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterBase.cpp b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterBase.cpp
new file mode 100644
index 0000000000..d314250650
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterBase.cpp
@@ -0,0 +1,59 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Character/CharacterBase.h>
+#include <Jolt/Physics/StateRecorder.h>
+
+JPH_NAMESPACE_BEGIN
+
+CharacterBase::CharacterBase(const CharacterBaseSettings *inSettings, PhysicsSystem *inSystem) :
+	mSystem(inSystem),
+	mShape(inSettings->mShape),
+	mUp(inSettings->mUp),
+	mSupportingVolume(inSettings->mSupportingVolume)
+{
+	// Initialize max slope angle
+	SetMaxSlopeAngle(inSettings->mMaxSlopeAngle);
+}
+
+const char *CharacterBase::sToString(EGroundState inState)
+{
+	switch (inState)
+	{
+	case EGroundState::OnGround:		return "OnGround";
+	case EGroundState::OnSteepGround:	return "OnSteepGround";
+	case EGroundState::NotSupported:	return "NotSupported";
+	case EGroundState::InAir:			return "InAir";
+	}
+
+	JPH_ASSERT(false);
+	return "Unknown";
+}
+
+void CharacterBase::SaveState(StateRecorder &inStream) const
+{
+	inStream.Write(mGroundState);
+	inStream.Write(mGroundBodyID);
+	inStream.Write(mGroundBodySubShapeID);
+	inStream.Write(mGroundPosition);
+	inStream.Write(mGroundNormal);
+	inStream.Write(mGroundVelocity);
+	// Can't save user data (may be a pointer) and material
+}
+
+void CharacterBase::RestoreState(StateRecorder &inStream)
+{
+	inStream.Read(mGroundState);
+	inStream.Read(mGroundBodyID);
+	inStream.Read(mGroundBodySubShapeID);
+	inStream.Read(mGroundPosition);
+	inStream.Read(mGroundNormal);
+	inStream.Read(mGroundVelocity);
+	mGroundUserData = 0; // Cannot restore user data
+	mGroundMaterial = PhysicsMaterial::sDefault; // Cannot restore material
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterBase.h b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterBase.h
new file mode 100644
index 0000000000..fc19c3d994
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterBase.h
@@ -0,0 +1,157 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/Reference.h>
+#include <Jolt/Core/NonCopyable.h>
+#include <Jolt/Physics/Body/BodyID.h>
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+#include <Jolt/Physics/Collision/PhysicsMaterial.h>
+
+JPH_NAMESPACE_BEGIN
+
+class PhysicsSystem;
+class StateRecorder;
+
+/// Base class for configuration of a character
+class JPH_EXPORT CharacterBaseSettings : public RefTarget<CharacterBaseSettings>
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+										CharacterBaseSettings() = default;
+										CharacterBaseSettings(const CharacterBaseSettings &inSettings) = default;
+	CharacterBaseSettings &				operator = (const CharacterBaseSettings &inSettings) = default;
+
+	/// Virtual destructor
+	virtual								~CharacterBaseSettings() = default;
+
+	/// Vector indicating the up direction of the character
+	Vec3								mUp = Vec3::sAxisY();
+
+	/// Plane, defined in local space relative to the character. Every contact behind this plane can support the
+	/// character, every contact in front of this plane is treated as only colliding with the player.
+	/// Default: Accept any contact.
+	Plane								mSupportingVolume { Vec3::sAxisY(), -1.0e10f };
+
+	/// Maximum angle of slope that character can still walk on (radians).
+	float								mMaxSlopeAngle = DegreesToRadians(50.0f);
+
+	/// Set to indicate that extra effort should be made to try to remove ghost contacts (collisions with internal edges of a mesh). This is more expensive but makes bodies move smoother over a mesh with convex edges.
+	bool								mEnhancedInternalEdgeRemoval = false;
+
+	/// Initial shape that represents the character's volume.
+	/// Usually this is a capsule, make sure the shape is made so that the bottom of the shape is at (0, 0, 0).
+	RefConst<Shape>						mShape;
+};
+
+/// Base class for character class
+class JPH_EXPORT CharacterBase : public RefTarget<CharacterBase>, public NonCopyable
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+										CharacterBase(const CharacterBaseSettings *inSettings, PhysicsSystem *inSystem);
+
+	/// Destructor
+	virtual								~CharacterBase() = default;
+
+	/// Set the maximum angle of slope that character can still walk on (radians)
+	void								SetMaxSlopeAngle(float inMaxSlopeAngle)					{ mCosMaxSlopeAngle = Cos(inMaxSlopeAngle); }
+	float								GetCosMaxSlopeAngle() const								{ return mCosMaxSlopeAngle; }
+
+	/// Set the up vector for the character
+	void								SetUp(Vec3Arg inUp)										{ mUp = inUp; }
+	Vec3								GetUp() const											{ return mUp; }
+
+	/// Check if the normal of the ground surface is too steep to walk on
+	bool								IsSlopeTooSteep(Vec3Arg inNormal) const
+	{
+		// If cos max slope angle is close to one the system is turned off,
+		// otherwise check the angle between the up and normal vector
+		return mCosMaxSlopeAngle < cNoMaxSlopeAngle && inNormal.Dot(mUp) < mCosMaxSlopeAngle;
+	}
+
+	/// Get the current shape that the character is using.
+	const Shape *						GetShape() const										{ return mShape; }
+
+	enum class EGroundState
+	{
+		OnGround,						///< Character is on the ground and can move freely.
+		OnSteepGround,					///< Character is on a slope that is too steep and can't climb up any further. The caller should start applying downward velocity if sliding from the slope is desired.
+		NotSupported,					///< Character is touching an object, but is not supported by it and should fall. The GetGroundXXX functions will return information about the touched object.
+		InAir,							///< Character is in the air and is not touching anything.
+	};
+
+	/// Debug function to convert enum values to string
+	static const char *					sToString(EGroundState inState);
+
+	///@name Properties of the ground this character is standing on
+
+	/// Current ground state
+	EGroundState						GetGroundState() const									{ return mGroundState; }
+
+	/// Returns true if the player is supported by normal or steep ground
+	bool								IsSupported() const										{ return mGroundState == EGroundState::OnGround || mGroundState == EGroundState::OnSteepGround; }
+
+	/// Get the contact point with the ground
+	RVec3								GetGroundPosition() const								{ return mGroundPosition; }
+
+	/// Get the contact normal with the ground
+	Vec3								GetGroundNormal() const									{ return mGroundNormal; }
+
+	/// Velocity in world space of ground
+	Vec3								GetGroundVelocity() const								{ return mGroundVelocity; }
+
+	/// Material that the character is standing on
+	const PhysicsMaterial *				GetGroundMaterial() const								{ return mGroundMaterial; }
+
+	/// BodyID of the object the character is standing on. Note may have been removed!
+	BodyID								GetGroundBodyID() const									{ return mGroundBodyID; }
+
+	/// Sub part of the body that we're standing on.
+	SubShapeID							GetGroundSubShapeID() const								{ return mGroundBodySubShapeID; }
+
+	/// User data value of the body that we're standing on
+	uint64								GetGroundUserData() const								{ return mGroundUserData; }
+
+	// Saving / restoring state for replay
+	virtual void						SaveState(StateRecorder &inStream) const;
+	virtual void						RestoreState(StateRecorder &inStream);
+
+protected:
+	// Cached physics system
+	PhysicsSystem *						mSystem;
+
+	// The shape that the body currently has
+	RefConst<Shape>						mShape;
+
+	// The character's world space up axis
+	Vec3								mUp;
+
+	// Every contact behind this plane can support the character
+	Plane								mSupportingVolume;
+
+	// Beyond this value there is no max slope
+	static constexpr float				cNoMaxSlopeAngle = 0.9999f;
+
+	// Cosine of the maximum angle of slope that character can still walk on
+	float								mCosMaxSlopeAngle;
+
+	// Ground properties
+	EGroundState						mGroundState = EGroundState::InAir;
+	BodyID								mGroundBodyID;
+	SubShapeID							mGroundBodySubShapeID;
+	RVec3								mGroundPosition = RVec3::sZero();
+	Vec3								mGroundNormal = Vec3::sZero();
+	Vec3								mGroundVelocity = Vec3::sZero();
+	RefConst<PhysicsMaterial>			mGroundMaterial = PhysicsMaterial::sDefault;
+	uint64								mGroundUserData = 0;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.cpp b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.cpp
new file mode 100644
index 0000000000..6c16dc83be
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.cpp
@@ -0,0 +1,1734 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Character/CharacterVirtual.h>
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Physics/PhysicsSystem.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/CollideShape.h>
+#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaledShape.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Core/QuickSort.h>
+#include <Jolt/Geometry/ConvexSupport.h>
+#include <Jolt/Geometry/GJKClosestPoint.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+void CharacterVsCharacterCollisionSimple::Remove(const CharacterVirtual *inCharacter)
+{
+	Array<CharacterVirtual *>::iterator i = std::find(mCharacters.begin(), mCharacters.end(), inCharacter);
+	if (i != mCharacters.end())
+		mCharacters.erase(i);
+}
+
+void CharacterVsCharacterCollisionSimple::CollideCharacter(const CharacterVirtual *inCharacter, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector) const
+{
+	// Make shape 1 relative to inBaseOffset
+	Mat44 transform1 = inCenterOfMassTransform.PostTranslated(-inBaseOffset).ToMat44();
+
+	const Shape *shape = inCharacter->GetShape();
+	CollideShapeSettings settings = inCollideShapeSettings;
+
+	// Iterate over all characters
+	for (const CharacterVirtual *c : mCharacters)
+		if (c != inCharacter
+			&& !ioCollector.ShouldEarlyOut())
+		{
+			// Collector needs to know which character we're colliding with
+			ioCollector.SetUserData(reinterpret_cast<uint64>(c));
+
+			// Make shape 2 relative to inBaseOffset
+			Mat44 transform2 = c->GetCenterOfMassTransform().PostTranslated(-inBaseOffset).ToMat44();
+
+			// We need to add the padding of character 2 so that we will detect collision with its outer shell
+			settings.mMaxSeparationDistance = inCollideShapeSettings.mMaxSeparationDistance + c->GetCharacterPadding();
+
+			// Note that this collides against the character's shape without padding, this will be corrected for in CharacterVirtual::GetContactsAtPosition
+			CollisionDispatch::sCollideShapeVsShape(shape, c->GetShape(), Vec3::sReplicate(1.0f), Vec3::sReplicate(1.0f), transform1, transform2, SubShapeIDCreator(), SubShapeIDCreator(), settings, ioCollector);
+		}
+
+	// Reset the user data
+	ioCollector.SetUserData(0);
+}
+
+void CharacterVsCharacterCollisionSimple::CastCharacter(const CharacterVirtual *inCharacter, RMat44Arg inCenterOfMassTransform, Vec3Arg inDirection, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector) const
+{
+	// Convert shape cast relative to inBaseOffset
+	Mat44 transform1 = inCenterOfMassTransform.PostTranslated(-inBaseOffset).ToMat44();
+	ShapeCast shape_cast(inCharacter->GetShape(), Vec3::sReplicate(1.0f), transform1, inDirection);
+
+	// Iterate over all characters
+	for (const CharacterVirtual *c : mCharacters)
+		if (c != inCharacter
+			&& !ioCollector.ShouldEarlyOut())
+		{
+			// Collector needs to know which character we're colliding with
+			ioCollector.SetUserData(reinterpret_cast<uint64>(c));
+
+			// Make shape 2 relative to inBaseOffset
+			Mat44 transform2 = c->GetCenterOfMassTransform().PostTranslated(-inBaseOffset).ToMat44();
+
+			// Note that this collides against the character's shape without padding, this will be corrected for in CharacterVirtual::GetFirstContactForSweep
+			CollisionDispatch::sCastShapeVsShapeWorldSpace(shape_cast, inShapeCastSettings, c->GetShape(), Vec3::sReplicate(1.0f), { }, transform2, SubShapeIDCreator(), SubShapeIDCreator(), ioCollector);
+		}
+
+	// Reset the user data
+	ioCollector.SetUserData(0);
+}
+
+CharacterVirtual::CharacterVirtual(const CharacterVirtualSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, uint64 inUserData, PhysicsSystem *inSystem) :
+	CharacterBase(inSettings, inSystem),
+	mBackFaceMode(inSettings->mBackFaceMode),
+	mPredictiveContactDistance(inSettings->mPredictiveContactDistance),
+	mMaxCollisionIterations(inSettings->mMaxCollisionIterations),
+	mMaxConstraintIterations(inSettings->mMaxConstraintIterations),
+	mMinTimeRemaining(inSettings->mMinTimeRemaining),
+	mCollisionTolerance(inSettings->mCollisionTolerance),
+	mCharacterPadding(inSettings->mCharacterPadding),
+	mMaxNumHits(inSettings->mMaxNumHits),
+	mHitReductionCosMaxAngle(inSettings->mHitReductionCosMaxAngle),
+	mPenetrationRecoverySpeed(inSettings->mPenetrationRecoverySpeed),
+	mEnhancedInternalEdgeRemoval(inSettings->mEnhancedInternalEdgeRemoval),
+	mShapeOffset(inSettings->mShapeOffset),
+	mPosition(inPosition),
+	mRotation(inRotation),
+	mUserData(inUserData)
+{
+	// Copy settings
+	SetMaxStrength(inSettings->mMaxStrength);
+	SetMass(inSettings->mMass);
+
+	// Create an inner rigid body if requested
+	if (inSettings->mInnerBodyShape != nullptr)
+	{
+		BodyCreationSettings settings(inSettings->mInnerBodyShape, GetInnerBodyPosition(), mRotation, EMotionType::Kinematic, inSettings->mInnerBodyLayer);
+		settings.mAllowSleeping = false; // Disable sleeping so that we will receive sensor callbacks
+		settings.mUserData = inUserData;
+		mInnerBodyID = inSystem->GetBodyInterface().CreateAndAddBody(settings, EActivation::Activate);
+	}
+}
+
+CharacterVirtual::~CharacterVirtual()
+{
+	if (!mInnerBodyID.IsInvalid())
+	{
+		mSystem->GetBodyInterface().RemoveBody(mInnerBodyID);
+		mSystem->GetBodyInterface().DestroyBody(mInnerBodyID);
+	}
+}
+
+void CharacterVirtual::UpdateInnerBodyTransform()
+{
+	if (!mInnerBodyID.IsInvalid())
+		mSystem->GetBodyInterface().SetPositionAndRotation(mInnerBodyID, GetInnerBodyPosition(), mRotation, EActivation::DontActivate);
+}
+
+void CharacterVirtual::GetAdjustedBodyVelocity(const Body& inBody, Vec3 &outLinearVelocity, Vec3 &outAngularVelocity) const
+{
+	// Get real velocity of body
+	if (!inBody.IsStatic())
+	{
+		const MotionProperties *mp = inBody.GetMotionPropertiesUnchecked();
+		outLinearVelocity = mp->GetLinearVelocity();
+		outAngularVelocity = mp->GetAngularVelocity();
+	}
+	else
+	{
+		outLinearVelocity = outAngularVelocity = Vec3::sZero();
+	}
+
+	// Allow application to override
+	if (mListener != nullptr)
+		mListener->OnAdjustBodyVelocity(this, inBody, outLinearVelocity, outAngularVelocity);
+}
+
+Vec3 CharacterVirtual::CalculateCharacterGroundVelocity(RVec3Arg inCenterOfMass, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, float inDeltaTime) const
+{
+	// Get angular velocity
+	float angular_velocity_len_sq = inAngularVelocity.LengthSq();
+	if (angular_velocity_len_sq < 1.0e-12f)
+		return inLinearVelocity;
+	float angular_velocity_len = sqrt(angular_velocity_len_sq);
+
+	// Calculate the rotation that the object will make in the time step
+	Quat rotation = Quat::sRotation(inAngularVelocity / angular_velocity_len, angular_velocity_len * inDeltaTime);
+
+	// Calculate where the new character position will be
+	RVec3 new_position = inCenterOfMass + rotation * Vec3(mPosition - inCenterOfMass);
+
+	// Calculate the velocity
+	return inLinearVelocity + Vec3(new_position - mPosition) / inDeltaTime;
+}
+
+template <class taCollector>
+void CharacterVirtual::sFillContactProperties(const CharacterVirtual *inCharacter, Contact &outContact, const Body &inBody, Vec3Arg inUp, RVec3Arg inBaseOffset, const taCollector &inCollector, const CollideShapeResult &inResult)
+{
+	// Get adjusted body velocity
+	Vec3 linear_velocity, angular_velocity;
+	inCharacter->GetAdjustedBodyVelocity(inBody, linear_velocity, angular_velocity);
+
+	outContact.mPosition = inBaseOffset + inResult.mContactPointOn2;
+	outContact.mLinearVelocity = linear_velocity + angular_velocity.Cross(Vec3(outContact.mPosition - inBody.GetCenterOfMassPosition())); // Calculate point velocity
+	outContact.mContactNormal = -inResult.mPenetrationAxis.NormalizedOr(Vec3::sZero());
+	outContact.mSurfaceNormal = inCollector.GetContext()->GetWorldSpaceSurfaceNormal(inResult.mSubShapeID2, outContact.mPosition);
+	if (outContact.mContactNormal.Dot(outContact.mSurfaceNormal) < 0.0f)
+		outContact.mSurfaceNormal = -outContact.mSurfaceNormal; // Flip surface normal if we're hitting a back face
+	if (outContact.mContactNormal.Dot(inUp) > outContact.mSurfaceNormal.Dot(inUp))
+		outContact.mSurfaceNormal = outContact.mContactNormal; // Replace surface normal with contact normal if the contact normal is pointing more upwards
+	outContact.mDistance = -inResult.mPenetrationDepth;
+	outContact.mBodyB = inResult.mBodyID2;
+	outContact.mSubShapeIDB = inResult.mSubShapeID2;
+	outContact.mMotionTypeB = inBody.GetMotionType();
+	outContact.mIsSensorB = inBody.IsSensor();
+	outContact.mUserData = inBody.GetUserData();
+	outContact.mMaterial = inCollector.GetContext()->GetMaterial(inResult.mSubShapeID2);
+}
+
+void CharacterVirtual::sFillCharacterContactProperties(Contact &outContact, CharacterVirtual *inOtherCharacter, RVec3Arg inBaseOffset, const CollideShapeResult &inResult)
+{
+	outContact.mPosition = inBaseOffset + inResult.mContactPointOn2;
+	outContact.mLinearVelocity = inOtherCharacter->GetLinearVelocity();
+	outContact.mSurfaceNormal = outContact.mContactNormal = -inResult.mPenetrationAxis.NormalizedOr(Vec3::sZero());
+	outContact.mDistance = -inResult.mPenetrationDepth;
+	outContact.mCharacterB = inOtherCharacter;
+	outContact.mSubShapeIDB = inResult.mSubShapeID2;
+	outContact.mMotionTypeB = EMotionType::Kinematic; // Other character is kinematic, we can't directly move it
+	outContact.mIsSensorB = false;
+	outContact.mUserData = inOtherCharacter->GetUserData();
+	outContact.mMaterial = PhysicsMaterial::sDefault;
+}
+
+void CharacterVirtual::ContactCollector::AddHit(const CollideShapeResult &inResult)
+{
+	// If we exceed our contact limit, try to clean up near-duplicate contacts
+	if (mContacts.size() == mMaxHits)
+	{
+		// Flag that we hit this code path
+		mMaxHitsExceeded = true;
+
+		// Check if we can do reduction
+		if (mHitReductionCosMaxAngle > -1.0f)
+		{
+			// Loop all contacts and find similar contacts
+			for (int i = (int)mContacts.size() - 1; i >= 0; --i)
+			{
+				Contact &contact_i = mContacts[i];
+				for (int j = i - 1; j >= 0; --j)
+				{
+					Contact &contact_j = mContacts[j];
+					if (contact_i.IsSameBody(contact_j)
+						&& contact_i.mContactNormal.Dot(contact_j.mContactNormal) > mHitReductionCosMaxAngle) // Very similar contact normals
+					{
+						// Remove the contact with the biggest distance
+						bool i_is_last = i == (int)mContacts.size() - 1;
+						if (contact_i.mDistance > contact_j.mDistance)
+						{
+							// Remove i
+							if (!i_is_last)
+								contact_i = mContacts.back();
+							mContacts.pop_back();
+
+							// Break out of the loop, i is now an element that we already processed
+							break;
+						}
+						else
+						{
+							// Remove j
+							contact_j = mContacts.back();
+							mContacts.pop_back();
+
+							// If i was the last element, we just moved it into position j. Break out of the loop, we'll see it again later.
+							if (i_is_last)
+								break;
+						}
+					}
+				}
+			}
+		}
+
+		if (mContacts.size() == mMaxHits)
+		{
+			// There are still too many hits, give up!
+			ForceEarlyOut();
+			return;
+		}
+	}
+
+	if (inResult.mBodyID2.IsInvalid())
+	{
+		// Assuming this is a hit against another character
+		JPH_ASSERT(mOtherCharacter != nullptr);
+
+		// Create contact with other character
+		mContacts.emplace_back();
+		Contact &contact = mContacts.back();
+		sFillCharacterContactProperties(contact, mOtherCharacter, mBaseOffset, inResult);
+		contact.mFraction = 0.0f;
+	}
+	else
+	{
+		// Create contact with other body
+		BodyLockRead lock(mSystem->GetBodyLockInterface(), inResult.mBodyID2);
+		if (lock.SucceededAndIsInBroadPhase())
+		{
+			mContacts.emplace_back();
+			Contact &contact = mContacts.back();
+			sFillContactProperties(mCharacter, contact, lock.GetBody(), mUp, mBaseOffset, *this, inResult);
+			contact.mFraction = 0.0f;
+		}
+	}
+}
+
+void CharacterVirtual::ContactCastCollector::AddHit(const ShapeCastResult &inResult)
+{
+	if (inResult.mFraction < mContact.mFraction // Since we're doing checks against the world and against characters, we may get a hit with a higher fraction than the previous hit
+		&& inResult.mFraction > 0.0f // Ignore collisions at fraction = 0
+		&& inResult.mPenetrationAxis.Dot(mDisplacement) > 0.0f) // Ignore penetrations that we're moving away from
+	{
+		// Test if this contact should be ignored
+		for (const IgnoredContact &c : mIgnoredContacts)
+			if (c.mBodyID == inResult.mBodyID2 && c.mSubShapeID == inResult.mSubShapeID2)
+				return;
+
+		Contact contact;
+
+		if (inResult.mBodyID2.IsInvalid())
+		{
+			// Assuming this is a hit against another character
+			JPH_ASSERT(mOtherCharacter != nullptr);
+
+			// Create contact with other character
+			sFillCharacterContactProperties(contact, mOtherCharacter, mBaseOffset, inResult);
+		}
+		else
+		{
+			// Lock body only while we fetch contact properties
+			BodyLockRead lock(mSystem->GetBodyLockInterface(), inResult.mBodyID2);
+			if (!lock.SucceededAndIsInBroadPhase())
+				return;
+
+			// Sweeps don't result in OnContactAdded callbacks so we can ignore sensors here
+			const Body &body = lock.GetBody();
+			if (body.IsSensor())
+				return;
+
+			// Convert the hit result into a contact
+			sFillContactProperties(mCharacter, contact, body, mUp, mBaseOffset, *this, inResult);
+		}
+
+		contact.mFraction = inResult.mFraction;
+
+		// Check if the contact that will make us penetrate more than the allowed tolerance
+		if (contact.mDistance + contact.mContactNormal.Dot(mDisplacement) < -mCharacter->mCollisionTolerance
+			&& mCharacter->ValidateContact(contact))
+		{
+			mContact = contact;
+			UpdateEarlyOutFraction(contact.mFraction);
+		}
+	}
+}
+
+void CharacterVirtual::CheckCollision(RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
+{
+	// Query shape transform
+	RMat44 transform = GetCenterOfMassTransform(inPosition, inRotation, inShape);
+
+	// Settings for collide shape
+	CollideShapeSettings settings;
+	settings.mBackFaceMode = mBackFaceMode;
+	settings.mActiveEdgeMovementDirection = inMovementDirection;
+	settings.mMaxSeparationDistance = mCharacterPadding + inMaxSeparationDistance;
+	settings.mActiveEdgeMode = EActiveEdgeMode::CollideOnlyWithActive;
+
+	// Body filter
+	IgnoreSingleBodyFilterChained body_filter(mInnerBodyID, inBodyFilter);
+
+	// Select the right function
+	auto collide_shape_function = mEnhancedInternalEdgeRemoval? &NarrowPhaseQuery::CollideShapeWithInternalEdgeRemoval : &NarrowPhaseQuery::CollideShape;
+
+	// Collide shape
+	(mSystem->GetNarrowPhaseQuery().*collide_shape_function)(inShape, Vec3::sReplicate(1.0f), transform, settings, inBaseOffset, ioCollector, inBroadPhaseLayerFilter, inObjectLayerFilter, body_filter, inShapeFilter);
+
+	// Also collide with other characters
+	if (mCharacterVsCharacterCollision != nullptr)
+	{
+		ioCollector.SetContext(nullptr); // We're no longer colliding with a transformed shape, reset
+		mCharacterVsCharacterCollision->CollideCharacter(this, transform, settings, inBaseOffset, ioCollector);
+	}
+}
+
+void CharacterVirtual::GetContactsAtPosition(RVec3Arg inPosition, Vec3Arg inMovementDirection, const Shape *inShape, TempContactList &outContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
+{
+	// Remove previous results
+	outContacts.clear();
+
+	// Body filter
+	IgnoreSingleBodyFilterChained body_filter(mInnerBodyID, inBodyFilter);
+
+	// Collide shape
+	ContactCollector collector(mSystem, this, mMaxNumHits, mHitReductionCosMaxAngle, mUp, mPosition, outContacts);
+	CheckCollision(inPosition, mRotation, inMovementDirection, mPredictiveContactDistance, inShape, mPosition, collector, inBroadPhaseLayerFilter, inObjectLayerFilter, body_filter, inShapeFilter);
+
+	// The broadphase bounding boxes will not be deterministic, which means that the order in which the contacts are received by the collector is not deterministic.
+	// Therefore we need to sort the contacts to preserve determinism. Note that currently this will fail if we exceed mMaxNumHits hits.
+	QuickSort(outContacts.begin(), outContacts.end(), ContactOrderingPredicate());
+
+	// Flag if we exceeded the max number of hits
+	mMaxHitsExceeded = collector.mMaxHitsExceeded;
+
+	// Reduce distance to contact by padding to ensure we stay away from the object by a little margin
+	// (this will make collision detection cheaper - especially for sweep tests as they won't hit the surface if we're properly sliding)
+	for (Contact &c : outContacts)
+	{
+		c.mDistance -= mCharacterPadding;
+
+		if (c.mCharacterB != nullptr)
+			c.mDistance -= c.mCharacterB->mCharacterPadding;
+	}
+}
+
+void CharacterVirtual::RemoveConflictingContacts(TempContactList &ioContacts, IgnoredContactList &outIgnoredContacts) const
+{
+	// Only use this algorithm if we're penetrating further than this (due to numerical precision issues we can always penetrate a little bit and we don't want to discard contacts if they just have a tiny penetration)
+	// We do need to account for padding (see GetContactsAtPosition) that is removed from the contact distances, to compensate we add it to the cMinRequiredPenetration
+	const float cMinRequiredPenetration = 1.25f * mCharacterPadding;
+
+	// Discard conflicting penetrating contacts
+	for (size_t c1 = 0; c1 < ioContacts.size(); c1++)
+	{
+		Contact &contact1 = ioContacts[c1];
+		if (contact1.mDistance <= -cMinRequiredPenetration) // Only for penetrations
+			for (size_t c2 = c1 + 1; c2 < ioContacts.size(); c2++)
+			{
+				Contact &contact2 = ioContacts[c2];
+				if (contact1.IsSameBody(contact2)
+					&& contact2.mDistance <= -cMinRequiredPenetration // Only for penetrations
+					&& contact1.mContactNormal.Dot(contact2.mContactNormal) < 0.0f) // Only opposing normals
+				{
+					// Discard contacts with the least amount of penetration
+					if (contact1.mDistance < contact2.mDistance)
+					{
+						// Discard the 2nd contact
+						outIgnoredContacts.emplace_back(contact2.mBodyB, contact2.mSubShapeIDB);
+						ioContacts.erase(ioContacts.begin() + c2);
+						c2--;
+					}
+					else
+					{
+						// Discard the first contact
+						outIgnoredContacts.emplace_back(contact1.mBodyB, contact1.mSubShapeIDB);
+						ioContacts.erase(ioContacts.begin() + c1);
+						c1--;
+						break;
+					}
+				}
+			}
+	}
+}
+
+bool CharacterVirtual::ValidateContact(const Contact &inContact) const
+{
+	if (mListener == nullptr)
+		return true;
+
+	if (inContact.mCharacterB != nullptr)
+		return mListener->OnCharacterContactValidate(this, inContact.mCharacterB, inContact.mSubShapeIDB);
+	else
+		return mListener->OnContactValidate(this, inContact.mBodyB, inContact.mSubShapeIDB);
+}
+
+void CharacterVirtual::ContactAdded(const Contact &inContact, CharacterContactSettings &ioSettings) const
+{
+	if (mListener != nullptr)
+	{
+		if (inContact.mCharacterB != nullptr)
+			mListener->OnCharacterContactAdded(this, inContact.mCharacterB, inContact.mSubShapeIDB, inContact.mPosition, -inContact.mContactNormal, ioSettings);
+		else
+			mListener->OnContactAdded(this, inContact.mBodyB, inContact.mSubShapeIDB, inContact.mPosition, -inContact.mContactNormal, ioSettings);
+	}
+}
+
+template <class T>
+inline static bool sCorrectFractionForCharacterPadding(const Shape *inShape, Mat44Arg inStart, Vec3Arg inDisplacement, Vec3Arg inScale, const T &inPolygon, float &ioFraction)
+{
+	if (inShape->GetType() == EShapeType::Convex)
+	{
+		// Get the support function for the shape we're casting
+		const ConvexShape *convex_shape = static_cast<const ConvexShape *>(inShape);
+		ConvexShape::SupportBuffer buffer;
+		const ConvexShape::Support *support = convex_shape->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, inScale);
+
+		// Cast the shape against the polygon
+		GJKClosestPoint gjk;
+		return gjk.CastShape(inStart, inDisplacement, cDefaultCollisionTolerance, *support, inPolygon, ioFraction);
+	}
+	else if (inShape->GetSubType() == EShapeSubType::RotatedTranslated)
+	{
+		const RotatedTranslatedShape *rt_shape = static_cast<const RotatedTranslatedShape *>(inShape);
+		return sCorrectFractionForCharacterPadding(rt_shape->GetInnerShape(), inStart * Mat44::sRotation(rt_shape->GetRotation()), inDisplacement, rt_shape->TransformScale(inScale), inPolygon, ioFraction);
+	}
+	else if (inShape->GetSubType() == EShapeSubType::Scaled)
+	{
+		const ScaledShape *scaled_shape = static_cast<const ScaledShape *>(inShape);
+		return sCorrectFractionForCharacterPadding(scaled_shape->GetInnerShape(), inStart, inDisplacement, inScale * scaled_shape->GetScale(), inPolygon, ioFraction);
+	}
+	else
+	{
+		JPH_ASSERT(false, "Not supported yet!");
+		return false;
+	}
+}
+
+bool CharacterVirtual::GetFirstContactForSweep(RVec3Arg inPosition, Vec3Arg inDisplacement, Contact &outContact, const IgnoredContactList &inIgnoredContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
+{
+	// Too small distance -> skip checking
+	float displacement_len_sq = inDisplacement.LengthSq();
+	if (displacement_len_sq < 1.0e-8f)
+		return false;
+
+	// Calculate start transform
+	RMat44 start = GetCenterOfMassTransform(inPosition, mRotation, mShape);
+
+	// Settings for the cast
+	ShapeCastSettings settings;
+	settings.mBackFaceModeTriangles = mBackFaceMode;
+	settings.mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces;
+	settings.mActiveEdgeMode = EActiveEdgeMode::CollideOnlyWithActive;
+	settings.mUseShrunkenShapeAndConvexRadius = true;
+	settings.mReturnDeepestPoint = false;
+
+	// Calculate how much extra fraction we need to add to the cast to account for the character padding
+	float character_padding_fraction = mCharacterPadding / sqrt(displacement_len_sq);
+
+	// Body filter
+	IgnoreSingleBodyFilterChained body_filter(mInnerBodyID, inBodyFilter);
+
+	// Cast shape
+	Contact contact;
+	contact.mFraction = 1.0f + character_padding_fraction;
+	RVec3 base_offset = start.GetTranslation();
+	ContactCastCollector collector(mSystem, this, inDisplacement, mUp, inIgnoredContacts, base_offset, contact);
+	collector.ResetEarlyOutFraction(contact.mFraction);
+	RShapeCast shape_cast(mShape, Vec3::sReplicate(1.0f), start, inDisplacement);
+	mSystem->GetNarrowPhaseQuery().CastShape(shape_cast, settings, base_offset, collector, inBroadPhaseLayerFilter, inObjectLayerFilter, body_filter, inShapeFilter);
+
+	// Also collide with other characters
+	if (mCharacterVsCharacterCollision != nullptr)
+	{
+		collector.SetContext(nullptr); // We're no longer colliding with a transformed shape, reset
+		mCharacterVsCharacterCollision->CastCharacter(this, start, inDisplacement, settings, base_offset, collector);
+	}
+
+	if (contact.mBodyB.IsInvalid() && contact.mCharacterB == nullptr)
+		return false;
+
+	// Store contact
+	outContact = contact;
+
+	TransformedShape ts;
+	float character_padding = mCharacterPadding;
+	if (outContact.mCharacterB != nullptr)
+	{
+		// Create a transformed shape for the character
+		RMat44 com = outContact.mCharacterB->GetCenterOfMassTransform();
+		ts = TransformedShape(com.GetTranslation(), com.GetQuaternion(), outContact.mCharacterB->GetShape(), BodyID(), SubShapeIDCreator());
+
+		// We need to take the other character's padding into account as well
+		character_padding += outContact.mCharacterB->mCharacterPadding;
+	}
+	else
+	{
+		// Create a transformed shape for the body
+		ts = mSystem->GetBodyInterface().GetTransformedShape(outContact.mBodyB);
+	}
+
+	// Fetch the face we're colliding with
+	Shape::SupportingFace face;
+	ts.GetSupportingFace(outContact.mSubShapeIDB, -outContact.mContactNormal, base_offset, face);
+
+	bool corrected = false;
+	if (face.size() >= 2)
+	{
+		// Inflate the colliding face by the character padding
+		PolygonConvexSupport polygon(face);
+		AddConvexRadius add_cvx(polygon, character_padding);
+
+		// Correct fraction to hit this inflated face instead of the inner shape
+		corrected = sCorrectFractionForCharacterPadding(mShape, start.GetRotation(), inDisplacement, Vec3::sReplicate(1.0f), add_cvx, outContact.mFraction);
+	}
+	if (!corrected)
+	{
+		// When there's only a single contact point or when we were unable to correct the fraction,
+		// we can just move the fraction back so that the character and its padding don't hit the contact point anymore
+		outContact.mFraction = max(0.0f, outContact.mFraction - character_padding_fraction);
+	}
+
+	// Ensure that we never return a fraction that's bigger than 1 (which could happen due to float precision issues).
+	outContact.mFraction = min(outContact.mFraction, 1.0f);
+
+	return true;
+}
+
+void CharacterVirtual::DetermineConstraints(TempContactList &inContacts, float inDeltaTime, ConstraintList &outConstraints) const
+{
+	for (Contact &c : inContacts)
+	{
+		Vec3 contact_velocity = c.mLinearVelocity;
+
+		// Penetrating contact: Add a contact velocity that pushes the character out at the desired speed
+		if (c.mDistance < 0.0f)
+			contact_velocity -= c.mContactNormal * c.mDistance * mPenetrationRecoverySpeed / inDeltaTime;
+
+		// Convert to a constraint
+		outConstraints.emplace_back();
+		Constraint &constraint = outConstraints.back();
+		constraint.mContact = &c;
+		constraint.mLinearVelocity = contact_velocity;
+		constraint.mPlane = Plane(c.mContactNormal, c.mDistance);
+
+		// Next check if the angle is too steep and if it is add an additional constraint that holds the character back
+		if (IsSlopeTooSteep(c.mSurfaceNormal))
+		{
+			// Only take planes that point up.
+			// Note that we use the contact normal to allow for better sliding as the surface normal may be in the opposite direction of movement.
+			float dot = c.mContactNormal.Dot(mUp);
+			if (dot > 1.0e-3f) // Add a little slack, if the normal is perfectly horizontal we already have our vertical plane.
+			{
+				// Mark the slope constraint as steep
+				constraint.mIsSteepSlope = true;
+
+				// Make horizontal normal
+				Vec3 normal = (c.mContactNormal - dot * mUp).Normalized();
+
+				// Create a secondary constraint that blocks horizontal movement
+				outConstraints.emplace_back();
+				Constraint &vertical_constraint = outConstraints.back();
+				vertical_constraint.mContact = &c;
+				vertical_constraint.mLinearVelocity = contact_velocity.Dot(normal) * normal; // Project the contact velocity on the new normal so that both planes push at an equal rate
+				vertical_constraint.mPlane = Plane(normal, c.mDistance / normal.Dot(c.mContactNormal)); // Calculate the distance we have to travel horizontally to hit the contact plane
+			}
+		}
+	}
+}
+
+bool CharacterVirtual::HandleContact(Vec3Arg inVelocity, Constraint &ioConstraint, float inDeltaTime) const
+{
+	Contact &contact = *ioConstraint.mContact;
+
+	// Validate the contact point
+	if (!ValidateContact(contact))
+		return false;
+
+	// Send contact added event
+	CharacterContactSettings settings;
+	ContactAdded(contact, settings);
+	contact.mCanPushCharacter = settings.mCanPushCharacter;
+
+	// We don't have any further interaction with sensors beyond an OnContactAdded notification
+	if (contact.mIsSensorB)
+		return false;
+
+	// If body B cannot receive an impulse, we're done
+	if (!settings.mCanReceiveImpulses || contact.mMotionTypeB != EMotionType::Dynamic)
+		return true;
+
+	// Lock the body we're colliding with
+	BodyLockWrite lock(mSystem->GetBodyLockInterface(), contact.mBodyB);
+	if (!lock.SucceededAndIsInBroadPhase())
+		return false; // Body has been removed, we should not collide with it anymore
+	const Body &body = lock.GetBody();
+
+	// Calculate the velocity that we want to apply at B so that it will start moving at the character's speed at the contact point
+	constexpr float cDamping = 0.9f;
+	constexpr float cPenetrationResolution = 0.4f;
+	Vec3 relative_velocity = inVelocity - contact.mLinearVelocity;
+	float projected_velocity = relative_velocity.Dot(contact.mContactNormal);
+	float delta_velocity = -projected_velocity * cDamping - min(contact.mDistance, 0.0f) * cPenetrationResolution / inDeltaTime;
+
+	// Don't apply impulses if we're separating
+	if (delta_velocity < 0.0f)
+		return true;
+
+	// Determine mass properties of the body we're colliding with
+	const MotionProperties *motion_properties = body.GetMotionProperties();
+	RVec3 center_of_mass = body.GetCenterOfMassPosition();
+	Mat44 inverse_inertia = body.GetInverseInertia();
+	float inverse_mass = motion_properties->GetInverseMass();
+
+	// Calculate the inverse of the mass of body B as seen at the contact point in the direction of the contact normal
+	Vec3 jacobian = Vec3(contact.mPosition - center_of_mass).Cross(contact.mContactNormal);
+	float inv_effective_mass = inverse_inertia.Multiply3x3(jacobian).Dot(jacobian) + inverse_mass;
+
+	// Impulse P = M dv
+	float impulse = delta_velocity / inv_effective_mass;
+
+	// Clamp the impulse according to the character strength, character strength is a force in newtons, P = F dt
+	float max_impulse = mMaxStrength * inDeltaTime;
+	impulse = min(impulse, max_impulse);
+
+	// Calculate the world space impulse to apply
+	Vec3 world_impulse = -impulse * contact.mContactNormal;
+
+	// Cancel impulse in down direction (we apply gravity later)
+	float impulse_dot_up = world_impulse.Dot(mUp);
+	if (impulse_dot_up < 0.0f)
+		world_impulse -= impulse_dot_up * mUp;
+
+	// Now apply the impulse (body is already locked so we use the no-lock interface)
+	mSystem->GetBodyInterfaceNoLock().AddImpulse(contact.mBodyB, world_impulse, contact.mPosition);
+	return true;
+}
+
+void CharacterVirtual::SolveConstraints(Vec3Arg inVelocity, float inDeltaTime, float inTimeRemaining, ConstraintList &ioConstraints, IgnoredContactList &ioIgnoredContacts, float &outTimeSimulated, Vec3 &outDisplacement, TempAllocator &inAllocator
+#ifdef JPH_DEBUG_RENDERER
+	, bool inDrawConstraints
+#endif // JPH_DEBUG_RENDERER
+	) const
+{
+	// If there are no constraints we can immediately move to our target
+	if (ioConstraints.empty())
+	{
+		outDisplacement = inVelocity * inTimeRemaining;
+		outTimeSimulated = inTimeRemaining;
+		return;
+	}
+
+	// Create array that holds the constraints in order of time of impact (sort will happen later)
+	Array<Constraint *, STLTempAllocator<Constraint *>> sorted_constraints(inAllocator);
+	sorted_constraints.resize(ioConstraints.size());
+	for (size_t index = 0; index < sorted_constraints.size(); index++)
+		sorted_constraints[index] = &ioConstraints[index];
+
+	// This is the velocity we use for the displacement, if we hit something it will be shortened
+	Vec3 velocity = inVelocity;
+
+	// Keep track of the last velocity that was applied to the character so that we can detect when the velocity reverses
+	Vec3 last_velocity = inVelocity;
+
+	// Start with no displacement
+	outDisplacement = Vec3::sZero();
+	outTimeSimulated = 0.0f;
+
+	// These are the contacts that we hit previously without moving a significant distance
+	Array<Constraint *, STLTempAllocator<Constraint *>> previous_contacts(inAllocator);
+	previous_contacts.resize(mMaxConstraintIterations);
+	int num_previous_contacts = 0;
+
+	// Loop for a max amount of iterations
+	for (uint iteration = 0; iteration < mMaxConstraintIterations; iteration++)
+	{
+		// Calculate time of impact for all constraints
+		for (Constraint &c : ioConstraints)
+		{
+			// Project velocity on plane direction
+			c.mProjectedVelocity = c.mPlane.GetNormal().Dot(c.mLinearVelocity - velocity);
+			if (c.mProjectedVelocity < 1.0e-6f)
+			{
+				c.mTOI = FLT_MAX;
+			}
+			else
+			{
+				// Distance to plane
+				float dist = c.mPlane.SignedDistance(outDisplacement);
+
+				if (dist - c.mProjectedVelocity * inTimeRemaining > -1.0e-4f)
+				{
+					// Too little penetration, accept the movement
+					c.mTOI = FLT_MAX;
+				}
+				else
+				{
+					// Calculate time of impact
+					c.mTOI = max(0.0f, dist / c.mProjectedVelocity);
+				}
+			}
+		}
+
+		// Sort constraints on proximity
+		QuickSort(sorted_constraints.begin(), sorted_constraints.end(), [](const Constraint *inLHS, const Constraint *inRHS) {
+				// If both constraints hit at t = 0 then order the one that will push the character furthest first
+				// Note that because we add velocity to penetrating contacts, this will also resolve contacts that penetrate the most
+				if (inLHS->mTOI <= 0.0f && inRHS->mTOI <= 0.0f)
+					return inLHS->mProjectedVelocity > inRHS->mProjectedVelocity;
+
+				// Then sort on time of impact
+				if (inLHS->mTOI != inRHS->mTOI)
+					return inLHS->mTOI < inRHS->mTOI;
+
+				// As a tie breaker sort static first so it has the most influence
+				return inLHS->mContact->mMotionTypeB > inRHS->mContact->mMotionTypeB;
+			});
+
+		// Find the first valid constraint
+		Constraint *constraint = nullptr;
+		for (Constraint *c : sorted_constraints)
+		{
+			// Take the first contact and see if we can reach it
+			if (c->mTOI >= inTimeRemaining)
+			{
+				// We can reach our goal!
+				outDisplacement += velocity * inTimeRemaining;
+				outTimeSimulated += inTimeRemaining;
+				return;
+			}
+
+			// Test if this contact was discarded by the contact callback before
+			if (c->mContact->mWasDiscarded)
+				continue;
+
+			// Check if we made contact with this before
+			if (!c->mContact->mHadCollision)
+			{
+				// Handle the contact
+				if (!HandleContact(velocity, *c, inDeltaTime))
+				{
+					// Constraint should be ignored, remove it from the list
+					c->mContact->mWasDiscarded = true;
+
+					// Mark it as ignored for GetFirstContactForSweep
+					ioIgnoredContacts.emplace_back(c->mContact->mBodyB, c->mContact->mSubShapeIDB);
+					continue;
+				}
+
+				c->mContact->mHadCollision = true;
+			}
+
+			// Cancel velocity of constraint if it cannot push the character
+			if (!c->mContact->mCanPushCharacter)
+				c->mLinearVelocity = Vec3::sZero();
+
+			// We found the first constraint that we want to collide with
+			constraint = c;
+			break;
+		}
+
+		if (constraint == nullptr)
+		{
+			// All constraints were discarded, we can reach our goal!
+			outDisplacement += velocity * inTimeRemaining;
+			outTimeSimulated += inTimeRemaining;
+			return;
+		}
+
+		// Move to the contact
+		outDisplacement += velocity * constraint->mTOI;
+		inTimeRemaining -= constraint->mTOI;
+		outTimeSimulated += constraint->mTOI;
+
+		// If there's not enough time left to be simulated, bail
+		if (inTimeRemaining < mMinTimeRemaining)
+			return;
+
+		// If we've moved significantly, clear all previous contacts
+		if (constraint->mTOI > 1.0e-4f)
+			num_previous_contacts = 0;
+
+		// Get the normal of the plane we're hitting
+		Vec3 plane_normal = constraint->mPlane.GetNormal();
+
+		// If we're hitting a steep slope we cancel the velocity towards the slope first so that we don't end up sliding up the slope
+		// (we may hit the slope before the vertical wall constraint we added which will result in a small movement up causing jitter in the character movement)
+		if (constraint->mIsSteepSlope)
+		{
+			// We're hitting a steep slope, create a vertical plane that blocks any further movement up the slope (note: not normalized)
+			Vec3 vertical_plane_normal = plane_normal - plane_normal.Dot(mUp) * mUp;
+
+			// Get the relative velocity between the character and the constraint
+			Vec3 relative_velocity = velocity - constraint->mLinearVelocity;
+
+			// Remove velocity towards the slope
+			velocity = velocity - min(0.0f, relative_velocity.Dot(vertical_plane_normal)) * vertical_plane_normal / vertical_plane_normal.LengthSq();
+		}
+
+		// Get the relative velocity between the character and the constraint
+		Vec3 relative_velocity = velocity - constraint->mLinearVelocity;
+
+		// Calculate new velocity if we cancel the relative velocity in the normal direction
+		Vec3 new_velocity = velocity - relative_velocity.Dot(plane_normal) * plane_normal;
+
+		// Find the normal of the previous contact that we will violate the most if we move in this new direction
+		float highest_penetration = 0.0f;
+		Constraint *other_constraint = nullptr;
+		for (Constraint **c = previous_contacts.data(); c < previous_contacts.data() + num_previous_contacts; ++c)
+			if (*c != constraint)
+			{
+				// Calculate how much we will penetrate if we move in this direction
+				Vec3 other_normal = (*c)->mPlane.GetNormal();
+				float penetration = ((*c)->mLinearVelocity - new_velocity).Dot(other_normal);
+				if (penetration > highest_penetration)
+				{
+					// We don't want parallel or anti-parallel normals as that will cause our cross product below to become zero. Slack is approx 10 degrees.
+					float dot = other_normal.Dot(plane_normal);
+					if (dot < 0.984f && dot > -0.984f)
+					{
+						highest_penetration = penetration;
+						other_constraint = *c;
+					}
+				}
+			}
+
+		// Check if we found a 2nd constraint
+		if (other_constraint != nullptr)
+		{
+			// Calculate the sliding direction and project the new velocity onto that sliding direction
+			Vec3 other_normal = other_constraint->mPlane.GetNormal();
+			Vec3 slide_dir = plane_normal.Cross(other_normal).Normalized();
+			Vec3 velocity_in_slide_dir = new_velocity.Dot(slide_dir) * slide_dir;
+
+			// Cancel the constraint velocity in the other constraint plane's direction so that we won't try to apply it again and keep ping ponging between planes
+			constraint->mLinearVelocity -= min(0.0f, constraint->mLinearVelocity.Dot(other_normal)) * other_normal;
+
+			// Cancel the other constraints velocity in this constraint plane's direction so that we won't try to apply it again and keep ping ponging between planes
+			other_constraint->mLinearVelocity -= min(0.0f, other_constraint->mLinearVelocity.Dot(plane_normal)) * plane_normal;
+
+			// Calculate the velocity of this constraint perpendicular to the slide direction
+			Vec3 perpendicular_velocity = constraint->mLinearVelocity - constraint->mLinearVelocity.Dot(slide_dir) * slide_dir;
+
+			// Calculate the velocity of the other constraint perpendicular to the slide direction
+			Vec3 other_perpendicular_velocity = other_constraint->mLinearVelocity - other_constraint->mLinearVelocity.Dot(slide_dir) * slide_dir;
+
+			// Add all components together
+			new_velocity = velocity_in_slide_dir + perpendicular_velocity + other_perpendicular_velocity;
+		}
+
+		// Allow application to modify calculated velocity
+		if (mListener != nullptr)
+		{
+			if (constraint->mContact->mCharacterB != nullptr)
+				mListener->OnCharacterContactSolve(this, constraint->mContact->mCharacterB, constraint->mContact->mSubShapeIDB, constraint->mContact->mPosition, constraint->mContact->mContactNormal, constraint->mContact->mLinearVelocity, constraint->mContact->mMaterial, velocity, new_velocity);
+			else
+				mListener->OnContactSolve(this, constraint->mContact->mBodyB, constraint->mContact->mSubShapeIDB, constraint->mContact->mPosition, constraint->mContact->mContactNormal, constraint->mContact->mLinearVelocity, constraint->mContact->mMaterial, velocity, new_velocity);
+		}
+
+#ifdef JPH_DEBUG_RENDERER
+		if (inDrawConstraints)
+		{
+			// Calculate where to draw
+			RVec3 offset = mPosition + Vec3(0, 0, 2.5f * (iteration + 1));
+
+			// Draw constraint plane
+			DebugRenderer::sInstance->DrawPlane(offset, constraint->mPlane.GetNormal(), Color::sCyan, 1.0f);
+
+			// Draw 2nd constraint plane
+			if (other_constraint != nullptr)
+				DebugRenderer::sInstance->DrawPlane(offset, other_constraint->mPlane.GetNormal(), Color::sBlue, 1.0f);
+
+			// Draw starting velocity
+			DebugRenderer::sInstance->DrawArrow(offset, offset + velocity, Color::sGreen, 0.05f);
+
+			// Draw resulting velocity
+			DebugRenderer::sInstance->DrawArrow(offset, offset + new_velocity, Color::sRed, 0.05f);
+		}
+#endif // JPH_DEBUG_RENDERER
+
+		// Update the velocity
+		velocity = new_velocity;
+
+		// Add the contact to the list so that next iteration we can avoid violating it again
+		previous_contacts[num_previous_contacts] = constraint;
+		num_previous_contacts++;
+
+		// Check early out
+		if (constraint->mProjectedVelocity < 1.0e-8f // Constraint should not be pushing, otherwise there may be other constraints that are pushing us
+			&& velocity.LengthSq() < 1.0e-8f) // There's not enough velocity left
+			return;
+
+		// If the constraint has velocity we accept the new velocity, otherwise check that we didn't reverse velocity
+		if (!constraint->mLinearVelocity.IsNearZero(1.0e-8f))
+			last_velocity = constraint->mLinearVelocity;
+		else if (velocity.Dot(last_velocity) < 0.0f)
+			return;
+	}
+}
+
+void CharacterVirtual::UpdateSupportingContact(bool inSkipContactVelocityCheck, TempAllocator &inAllocator)
+{
+	// Flag contacts as having a collision if they're close enough but ignore contacts we're moving away from.
+	// Note that if we did MoveShape before we want to preserve any contacts that it marked as colliding
+	for (Contact &c : mActiveContacts)
+		if (!c.mWasDiscarded
+			&& !c.mHadCollision
+			&& c.mDistance < mCollisionTolerance
+			&& (inSkipContactVelocityCheck || c.mSurfaceNormal.Dot(mLinearVelocity - c.mLinearVelocity) <= 1.0e-4f))
+		{
+			if (ValidateContact(c) && !c.mIsSensorB)
+				c.mHadCollision = true;
+			else
+				c.mWasDiscarded = true;
+		}
+
+	// Calculate transform that takes us to character local space
+	RMat44 inv_transform = RMat44::sInverseRotationTranslation(mRotation, mPosition);
+
+	// Determine if we're supported or not
+	int num_supported = 0;
+	int num_sliding = 0;
+	int num_avg_normal = 0;
+	Vec3 avg_normal = Vec3::sZero();
+	Vec3 avg_velocity = Vec3::sZero();
+	const Contact *supporting_contact = nullptr;
+	float max_cos_angle = -FLT_MAX;
+	const Contact *deepest_contact = nullptr;
+	float smallest_distance = FLT_MAX;
+	for (const Contact &c : mActiveContacts)
+		if (c.mHadCollision)
+		{
+			// Calculate the angle between the plane normal and the up direction
+			float cos_angle = c.mSurfaceNormal.Dot(mUp);
+
+			// Find the deepest contact
+			if (c.mDistance < smallest_distance)
+			{
+				deepest_contact = &c;
+				smallest_distance = c.mDistance;
+			}
+
+			// If this contact is in front of our plane, we cannot be supported by it
+			if (mSupportingVolume.SignedDistance(Vec3(inv_transform * c.mPosition)) > 0.0f)
+				continue;
+
+			// Find the contact with the normal that is pointing most upwards and store it
+			if (max_cos_angle < cos_angle)
+			{
+				supporting_contact = &c;
+				max_cos_angle = cos_angle;
+			}
+
+			// Check if this is a sliding or supported contact
+			bool is_supported = mCosMaxSlopeAngle > cNoMaxSlopeAngle || cos_angle >= mCosMaxSlopeAngle;
+			if (is_supported)
+				num_supported++;
+			else
+				num_sliding++;
+
+			// If the angle between the two is less than 85 degrees we also use it to calculate the average normal
+			if (cos_angle >= 0.08f)
+			{
+				avg_normal += c.mSurfaceNormal;
+				num_avg_normal++;
+
+				// For static or dynamic objects or for contacts that don't support us just take the contact velocity
+				if (c.mMotionTypeB != EMotionType::Kinematic || !is_supported)
+					avg_velocity += c.mLinearVelocity;
+				else
+				{
+					// For keyframed objects that support us calculate the velocity at our position rather than at the contact position so that we properly follow the object
+					BodyLockRead lock(mSystem->GetBodyLockInterface(), c.mBodyB);
+					if (lock.SucceededAndIsInBroadPhase())
+					{
+						const Body &body = lock.GetBody();
+
+						// Get adjusted body velocity
+						Vec3 linear_velocity, angular_velocity;
+						GetAdjustedBodyVelocity(body, linear_velocity, angular_velocity);
+
+						// Calculate the ground velocity
+						avg_velocity += CalculateCharacterGroundVelocity(body.GetCenterOfMassPosition(), linear_velocity, angular_velocity, mLastDeltaTime);
+					}
+					else
+					{
+						// Fall back to contact velocity
+						avg_velocity += c.mLinearVelocity;
+					}
+				}
+			}
+		}
+
+	// Take either the most supporting contact or the deepest contact
+	const Contact *best_contact = supporting_contact != nullptr? supporting_contact : deepest_contact;
+
+	// Calculate average normal and velocity
+	if (num_avg_normal >= 1)
+	{
+		mGroundNormal = avg_normal.Normalized();
+		mGroundVelocity = avg_velocity / float(num_avg_normal);
+	}
+	else if (best_contact != nullptr)
+	{
+		mGroundNormal = best_contact->mSurfaceNormal;
+		mGroundVelocity = best_contact->mLinearVelocity;
+	}
+	else
+	{
+		mGroundNormal = Vec3::sZero();
+		mGroundVelocity = Vec3::sZero();
+	}
+
+	// Copy contact properties
+	if (best_contact != nullptr)
+	{
+		mGroundBodyID = best_contact->mBodyB;
+		mGroundBodySubShapeID = best_contact->mSubShapeIDB;
+		mGroundPosition = best_contact->mPosition;
+		mGroundMaterial = best_contact->mMaterial;
+		mGroundUserData = best_contact->mUserData;
+	}
+	else
+	{
+		mGroundBodyID = BodyID();
+		mGroundBodySubShapeID = SubShapeID();
+		mGroundPosition = RVec3::sZero();
+		mGroundMaterial = PhysicsMaterial::sDefault;
+		mGroundUserData = 0;
+	}
+
+	// Determine ground state
+	if (num_supported > 0)
+	{
+		// We made contact with something that supports us
+		mGroundState = EGroundState::OnGround;
+	}
+	else if (num_sliding > 0)
+	{
+		if ((mLinearVelocity - deepest_contact->mLinearVelocity).Dot(mUp) > 1.0e-4f)
+		{
+			// We cannot be on ground if we're moving upwards relative to the ground
+			mGroundState = EGroundState::OnSteepGround;
+		}
+		else
+		{
+			// If we're sliding down, we may actually be standing on multiple sliding contacts in such a way that we can't slide off, in this case we're also supported
+
+			// Convert the contacts into constraints
+			TempContactList contacts(mActiveContacts.begin(), mActiveContacts.end(), inAllocator);
+			ConstraintList constraints(inAllocator);
+			constraints.reserve(contacts.size() * 2);
+			DetermineConstraints(contacts, mLastDeltaTime, constraints);
+
+			// Solve the displacement using these constraints, this is used to check if we didn't move at all because we are supported
+			Vec3 displacement;
+			float time_simulated;
+			IgnoredContactList ignored_contacts(inAllocator);
+			ignored_contacts.reserve(contacts.size());
+			SolveConstraints(-mUp, 1.0f, 1.0f, constraints, ignored_contacts, time_simulated, displacement, inAllocator);
+
+			// If we're blocked then we're supported, otherwise we're sliding
+			float min_required_displacement_sq = Square(0.6f * mLastDeltaTime);
+			if (time_simulated < 0.001f || displacement.LengthSq() < min_required_displacement_sq)
+				mGroundState = EGroundState::OnGround;
+			else
+				mGroundState = EGroundState::OnSteepGround;
+		}
+	}
+	else
+	{
+		// Not supported by anything
+		mGroundState = best_contact != nullptr? EGroundState::NotSupported : EGroundState::InAir;
+	}
+}
+
+void CharacterVirtual::StoreActiveContacts(const TempContactList &inContacts, TempAllocator &inAllocator)
+{
+	mActiveContacts.assign(inContacts.begin(), inContacts.end());
+
+	UpdateSupportingContact(true, inAllocator);
+}
+
+void CharacterVirtual::MoveShape(RVec3 &ioPosition, Vec3Arg inVelocity, float inDeltaTime, ContactList *outActiveContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator
+#ifdef JPH_DEBUG_RENDERER
+	, bool inDrawConstraints
+#endif // JPH_DEBUG_RENDERER
+	) const
+{
+	JPH_DET_LOG("CharacterVirtual::MoveShape: pos: " << ioPosition << " vel: " << inVelocity << " dt: " << inDeltaTime);
+
+	Vec3 movement_direction = inVelocity.NormalizedOr(Vec3::sZero());
+
+	float time_remaining = inDeltaTime;
+	for (uint iteration = 0; iteration < mMaxCollisionIterations && time_remaining >= mMinTimeRemaining; iteration++)
+	{
+		JPH_DET_LOG("iter: " << iteration << " time: " << time_remaining);
+
+		// Determine contacts in the neighborhood
+		TempContactList contacts(inAllocator);
+		contacts.reserve(mMaxNumHits);
+		GetContactsAtPosition(ioPosition, movement_direction, mShape, contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter);
+
+#ifdef JPH_ENABLE_DETERMINISM_LOG
+		for (const Contact &c : contacts)
+			JPH_DET_LOG("contact: " << c.mPosition << " vel: " << c.mLinearVelocity << " cnormal: " << c.mContactNormal << " snormal: " << c.mSurfaceNormal << " dist: " << c.mDistance << " fraction: " << c.mFraction << " body: " << c.mBodyB << " subshape: " << c.mSubShapeIDB);
+#endif // JPH_ENABLE_DETERMINISM_LOG
+
+		// Remove contacts with the same body that have conflicting normals
+		IgnoredContactList ignored_contacts(inAllocator);
+		ignored_contacts.reserve(contacts.size());
+		RemoveConflictingContacts(contacts, ignored_contacts);
+
+		// Convert contacts into constraints
+		ConstraintList constraints(inAllocator);
+		constraints.reserve(contacts.size() * 2);
+		DetermineConstraints(contacts, inDeltaTime, constraints);
+
+#ifdef JPH_DEBUG_RENDERER
+		bool draw_constraints = inDrawConstraints && iteration == 0;
+		if (draw_constraints)
+		{
+			for (const Constraint &c : constraints)
+			{
+				// Draw contact point
+				DebugRenderer::sInstance->DrawMarker(c.mContact->mPosition, Color::sYellow, 0.05f);
+				Vec3 dist_to_plane = -c.mPlane.GetConstant() * c.mPlane.GetNormal();
+
+				// Draw arrow towards surface that we're hitting
+				DebugRenderer::sInstance->DrawArrow(c.mContact->mPosition, c.mContact->mPosition - dist_to_plane, Color::sYellow, 0.05f);
+
+				// Draw plane around the player position indicating the space that we can move
+				DebugRenderer::sInstance->DrawPlane(mPosition + dist_to_plane, c.mPlane.GetNormal(), Color::sCyan, 1.0f);
+				DebugRenderer::sInstance->DrawArrow(mPosition + dist_to_plane, mPosition + dist_to_plane + c.mContact->mSurfaceNormal, Color::sRed, 0.05f);
+			}
+		}
+#endif // JPH_DEBUG_RENDERER
+
+		// Solve the displacement using these constraints
+		Vec3 displacement;
+		float time_simulated;
+		SolveConstraints(inVelocity, inDeltaTime, time_remaining, constraints, ignored_contacts, time_simulated, displacement, inAllocator
+		#ifdef JPH_DEBUG_RENDERER
+			, draw_constraints
+		#endif // JPH_DEBUG_RENDERER
+			);
+
+		// Store the contacts now that the colliding ones have been marked
+		if (outActiveContacts != nullptr)
+			outActiveContacts->assign(contacts.begin(), contacts.end());
+
+		// Do a sweep to test if the path is really unobstructed
+		Contact cast_contact;
+		if (GetFirstContactForSweep(ioPosition, displacement, cast_contact, ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter))
+		{
+			displacement *= cast_contact.mFraction;
+			time_simulated *= cast_contact.mFraction;
+		}
+
+		// Update the position
+		ioPosition += displacement;
+		time_remaining -= time_simulated;
+
+		// If the displacement during this iteration was too small we assume we cannot further progress this update
+		if (displacement.LengthSq() < 1.0e-8f)
+			break;
+	}
+}
+
+void CharacterVirtual::SetUserData(uint64 inUserData)
+{
+	mUserData = inUserData;
+
+	if (!mInnerBodyID.IsInvalid())
+		mSystem->GetBodyInterface().SetUserData(mInnerBodyID, inUserData);
+}
+
+Vec3 CharacterVirtual::CancelVelocityTowardsSteepSlopes(Vec3Arg inDesiredVelocity) const
+{
+	// If we're not pushing against a steep slope, return the desired velocity
+	// Note: This is important as WalkStairs overrides the ground state to OnGround when its first check fails but the second succeeds
+	if (mGroundState == CharacterVirtual::EGroundState::OnGround
+		|| mGroundState == CharacterVirtual::EGroundState::InAir)
+		return inDesiredVelocity;
+
+	Vec3 desired_velocity = inDesiredVelocity;
+	for (const Contact &c : mActiveContacts)
+		if (c.mHadCollision
+			&& IsSlopeTooSteep(c.mSurfaceNormal))
+		{
+			// Note that we use the contact normal to allow for better sliding as the surface normal may be in the opposite direction of movement.
+			Vec3 normal = c.mContactNormal;
+
+			// Remove normal vertical component
+			normal -= normal.Dot(mUp) * mUp;
+
+			// Cancel horizontal movement in opposite direction
+			float dot = normal.Dot(desired_velocity);
+			if (dot < 0.0f)
+				desired_velocity -= (dot * normal) / normal.LengthSq();
+		}
+	return desired_velocity;
+}
+
+void CharacterVirtual::Update(float inDeltaTime, Vec3Arg inGravity, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator)
+{
+	// If there's no delta time, we don't need to do anything
+	if (inDeltaTime <= 0.0f)
+		return;
+
+	// Remember delta time for checking if we're supported by the ground
+	mLastDeltaTime = inDeltaTime;
+
+	// Slide the shape through the world
+	MoveShape(mPosition, mLinearVelocity, inDeltaTime, &mActiveContacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator
+	#ifdef JPH_DEBUG_RENDERER
+		, sDrawConstraints
+	#endif // JPH_DEBUG_RENDERER
+		);
+
+	// Determine the object that we're standing on
+	UpdateSupportingContact(false, inAllocator);
+
+	// Ensure that the rigid body ends up at the new position
+	UpdateInnerBodyTransform();
+
+	// If we're on the ground
+	if (!mGroundBodyID.IsInvalid() && mMass > 0.0f)
+	{
+		// Add the impulse to the ground due to gravity: P = F dt = M g dt
+		float normal_dot_gravity = mGroundNormal.Dot(inGravity);
+		if (normal_dot_gravity < 0.0f)
+		{
+			Vec3 world_impulse = -(mMass * normal_dot_gravity / inGravity.Length() * inDeltaTime) * inGravity;
+			mSystem->GetBodyInterface().AddImpulse(mGroundBodyID, world_impulse, mGroundPosition);
+		}
+	}
+}
+
+void CharacterVirtual::RefreshContacts(const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator)
+{
+	// Determine the contacts
+	TempContactList contacts(inAllocator);
+	contacts.reserve(mMaxNumHits);
+	GetContactsAtPosition(mPosition, mLinearVelocity.NormalizedOr(Vec3::sZero()), mShape, contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter);
+
+	StoreActiveContacts(contacts, inAllocator);
+}
+
+void CharacterVirtual::UpdateGroundVelocity()
+{
+	BodyLockRead lock(mSystem->GetBodyLockInterface(), mGroundBodyID);
+	if (lock.SucceededAndIsInBroadPhase())
+	{
+		const Body &body = lock.GetBody();
+
+		// Get adjusted body velocity
+		Vec3 linear_velocity, angular_velocity;
+		GetAdjustedBodyVelocity(body, linear_velocity, angular_velocity);
+
+		// Calculate the ground velocity
+		mGroundVelocity = CalculateCharacterGroundVelocity(body.GetCenterOfMassPosition(), linear_velocity, angular_velocity, mLastDeltaTime);
+	}
+}
+
+void CharacterVirtual::MoveToContact(RVec3Arg inPosition, const Contact &inContact, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator)
+{
+	// Set the new position
+	SetPosition(inPosition);
+
+	// Trigger contact added callback
+	CharacterContactSettings dummy;
+	ContactAdded(inContact, dummy);
+
+	// Determine the contacts
+	TempContactList contacts(inAllocator);
+	contacts.reserve(mMaxNumHits + 1); // +1 because we can add one extra below
+	GetContactsAtPosition(mPosition, mLinearVelocity.NormalizedOr(Vec3::sZero()), mShape, contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter);
+
+	// Ensure that we mark inContact as colliding
+	bool found_contact = false;
+	for (Contact &c : contacts)
+		if (c.mBodyB == inContact.mBodyB
+			&& c.mSubShapeIDB == inContact.mSubShapeIDB)
+		{
+			c.mHadCollision = true;
+			found_contact = true;
+		}
+	if (!found_contact)
+	{
+		contacts.push_back(inContact);
+
+		Contact &copy = contacts.back();
+		copy.mHadCollision = true;
+	}
+
+	StoreActiveContacts(contacts, inAllocator);
+	JPH_ASSERT(mGroundState != EGroundState::InAir);
+
+	// Ensure that the rigid body ends up at the new position
+	UpdateInnerBodyTransform();
+}
+
+bool CharacterVirtual::SetShape(const Shape *inShape, float inMaxPenetrationDepth, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator)
+{
+	if (mShape == nullptr || mSystem == nullptr)
+	{
+		// It hasn't been initialized yet
+		mShape = inShape;
+		return true;
+	}
+
+	if (inShape != mShape && inShape != nullptr)
+	{
+		if (inMaxPenetrationDepth < FLT_MAX)
+		{
+			// Check collision around the new shape
+			TempContactList contacts(inAllocator);
+			contacts.reserve(mMaxNumHits);
+			GetContactsAtPosition(mPosition, mLinearVelocity.NormalizedOr(Vec3::sZero()), inShape, contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter);
+
+			// Test if this results in penetration, if so cancel the transition
+			for (const Contact &c : contacts)
+				if (c.mDistance < -inMaxPenetrationDepth
+					&& !c.mIsSensorB)
+					return false;
+
+			StoreActiveContacts(contacts, inAllocator);
+		}
+
+		// Set new shape
+		mShape = inShape;
+	}
+
+	return mShape == inShape;
+}
+
+void CharacterVirtual::SetInnerBodyShape(const Shape *inShape)
+{
+	mSystem->GetBodyInterface().SetShape(mInnerBodyID, inShape, false, EActivation::DontActivate);
+}
+
+bool CharacterVirtual::CanWalkStairs(Vec3Arg inLinearVelocity) const
+{
+	// We can only walk stairs if we're supported
+	if (!IsSupported())
+		return false;
+
+	// Check if there's enough horizontal velocity to trigger a stair walk
+	Vec3 horizontal_velocity = inLinearVelocity - inLinearVelocity.Dot(mUp) * mUp;
+	if (horizontal_velocity.IsNearZero(1.0e-6f))
+		return false;
+
+	// Check contacts for steep slopes
+	for (const Contact &c : mActiveContacts)
+		if (c.mHadCollision
+			&& c.mSurfaceNormal.Dot(horizontal_velocity - c.mLinearVelocity) < 0.0f // Pushing into the contact
+			&& IsSlopeTooSteep(c.mSurfaceNormal)) // Slope too steep
+			return true;
+
+	return false;
+}
+
+bool CharacterVirtual::WalkStairs(float inDeltaTime, Vec3Arg inStepUp, Vec3Arg inStepForward, Vec3Arg inStepForwardTest, Vec3Arg inStepDownExtra, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator)
+{
+	// Move up
+	Vec3 up = inStepUp;
+	Contact contact;
+	IgnoredContactList dummy_ignored_contacts(inAllocator);
+	if (GetFirstContactForSweep(mPosition, up, contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter))
+	{
+		if (contact.mFraction < 1.0e-6f)
+			return false; // No movement, cancel
+
+		// Limit up movement to the first contact point
+		up *= contact.mFraction;
+	}
+	RVec3 up_position = mPosition + up;
+
+#ifdef JPH_DEBUG_RENDERER
+	// Draw sweep up
+	if (sDrawWalkStairs)
+		DebugRenderer::sInstance->DrawArrow(mPosition, up_position, Color::sWhite, 0.01f);
+#endif // JPH_DEBUG_RENDERER
+
+	// Collect normals of steep slopes that we would like to walk stairs on.
+	// We need to do this before calling MoveShape because it will update mActiveContacts.
+	Vec3 character_velocity = inStepForward / inDeltaTime;
+	Vec3 horizontal_velocity = character_velocity - character_velocity.Dot(mUp) * mUp;
+	Array<Vec3, STLTempAllocator<Vec3>> steep_slope_normals(inAllocator);
+	steep_slope_normals.reserve(mActiveContacts.size());
+	for (const Contact &c : mActiveContacts)
+		if (c.mHadCollision
+			&& c.mSurfaceNormal.Dot(horizontal_velocity - c.mLinearVelocity) < 0.0f // Pushing into the contact
+			&& IsSlopeTooSteep(c.mSurfaceNormal)) // Slope too steep
+			steep_slope_normals.push_back(c.mSurfaceNormal);
+	if (steep_slope_normals.empty())
+		return false; // No steep slopes, cancel
+
+	// Horizontal movement
+	RVec3 new_position = up_position;
+	MoveShape(new_position, character_velocity, inDeltaTime, nullptr, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator);
+	Vec3 horizontal_movement = Vec3(new_position - up_position);
+	float horizontal_movement_sq = horizontal_movement.LengthSq();
+	if (horizontal_movement_sq < 1.0e-8f)
+		return false; // No movement, cancel
+
+	// Check if we made any progress towards any of the steep slopes, if not we just slid along the slope
+	// so we need to cancel the stair walk or else we will move faster than we should as we've done
+	// normal movement first and then stair walk.
+	bool made_progress = false;
+	float max_dot = -0.05f * inStepForward.Length();
+	for (const Vec3 &normal : steep_slope_normals)
+		if (normal.Dot(horizontal_movement) < max_dot)
+		{
+			// We moved more than 5% of the forward step against a steep slope, accept this as progress
+			made_progress = true;
+			break;
+		}
+	if (!made_progress)
+		return false;
+
+#ifdef JPH_DEBUG_RENDERER
+	// Draw horizontal sweep
+	if (sDrawWalkStairs)
+		DebugRenderer::sInstance->DrawArrow(up_position, new_position, Color::sWhite, 0.01f);
+#endif // JPH_DEBUG_RENDERER
+
+	// Move down towards the floor.
+	// Note that we travel the same amount down as we traveled up with the specified extra
+	Vec3 down = -up + inStepDownExtra;
+	if (!GetFirstContactForSweep(new_position, down, contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter))
+		return false; // No floor found, we're in mid air, cancel stair walk
+
+#ifdef JPH_DEBUG_RENDERER
+	// Draw sweep down
+	if (sDrawWalkStairs)
+	{
+		RVec3 debug_pos = new_position + contact.mFraction * down;
+		DebugRenderer::sInstance->DrawArrow(new_position, debug_pos, Color::sWhite, 0.01f);
+		DebugRenderer::sInstance->DrawArrow(contact.mPosition, contact.mPosition + contact.mSurfaceNormal, Color::sWhite, 0.01f);
+		mShape->Draw(DebugRenderer::sInstance, GetCenterOfMassTransform(debug_pos, mRotation, mShape), Vec3::sReplicate(1.0f), Color::sWhite, false, true);
+	}
+#endif // JPH_DEBUG_RENDERER
+
+	// Test for floor that will support the character
+	if (IsSlopeTooSteep(contact.mSurfaceNormal))
+	{
+		// If no test position was provided, we cancel the stair walk
+		if (inStepForwardTest.IsNearZero())
+			return false;
+
+		// Delta time may be very small, so it may be that we hit the edge of a step and the normal is too horizontal.
+		// In order to judge if the floor is flat further along the sweep, we test again for a floor at inStepForwardTest
+		// and check if the normal is valid there.
+		RVec3 test_position = up_position;
+		MoveShape(test_position, inStepForwardTest / inDeltaTime, inDeltaTime, nullptr, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator);
+		float test_horizontal_movement_sq = Vec3(test_position - up_position).LengthSq();
+		if (test_horizontal_movement_sq <= horizontal_movement_sq + 1.0e-8f)
+			return false; // We didn't move any further than in the previous test
+
+	#ifdef JPH_DEBUG_RENDERER
+		// Draw 2nd sweep horizontal
+		if (sDrawWalkStairs)
+			DebugRenderer::sInstance->DrawArrow(up_position, test_position, Color::sCyan, 0.01f);
+	#endif // JPH_DEBUG_RENDERER
+
+		// Then sweep down
+		Contact test_contact;
+		if (!GetFirstContactForSweep(test_position, down, test_contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter))
+			return false;
+
+	#ifdef JPH_DEBUG_RENDERER
+		// Draw 2nd sweep down
+		if (sDrawWalkStairs)
+		{
+			RVec3 debug_pos = test_position + test_contact.mFraction * down;
+			DebugRenderer::sInstance->DrawArrow(test_position, debug_pos, Color::sCyan, 0.01f);
+			DebugRenderer::sInstance->DrawArrow(test_contact.mPosition, test_contact.mPosition + test_contact.mSurfaceNormal, Color::sCyan, 0.01f);
+			mShape->Draw(DebugRenderer::sInstance, GetCenterOfMassTransform(debug_pos, mRotation, mShape), Vec3::sReplicate(1.0f), Color::sCyan, false, true);
+		}
+	#endif // JPH_DEBUG_RENDERER
+
+		if (IsSlopeTooSteep(test_contact.mSurfaceNormal))
+			return false;
+	}
+
+	// Calculate new down position
+	down *= contact.mFraction;
+	new_position += down;
+
+	// Move the character to the new location
+	MoveToContact(new_position, contact, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator);
+
+	// Override ground state to 'on ground', it is possible that the contact normal is too steep, but in this case the inStepForwardTest has found a contact normal that is not too steep
+	mGroundState = EGroundState::OnGround;
+
+	return true;
+}
+
+bool CharacterVirtual::StickToFloor(Vec3Arg inStepDown, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator)
+{
+	// Try to find the floor
+	Contact contact;
+	IgnoredContactList dummy_ignored_contacts(inAllocator);
+	if (!GetFirstContactForSweep(mPosition, inStepDown, contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter))
+		return false; // If no floor found, don't update our position
+
+	// Calculate new position
+	RVec3 new_position = mPosition + contact.mFraction * inStepDown;
+
+#ifdef JPH_DEBUG_RENDERER
+	// Draw sweep down
+	if (sDrawStickToFloor)
+	{
+		DebugRenderer::sInstance->DrawArrow(mPosition, new_position, Color::sOrange, 0.01f);
+		mShape->Draw(DebugRenderer::sInstance, GetCenterOfMassTransform(new_position, mRotation, mShape), Vec3::sReplicate(1.0f), Color::sOrange, false, true);
+	}
+#endif // JPH_DEBUG_RENDERER
+
+	// Move the character to the new location
+	MoveToContact(new_position, contact, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator);
+	return true;
+}
+
+void CharacterVirtual::ExtendedUpdate(float inDeltaTime, Vec3Arg inGravity, const ExtendedUpdateSettings &inSettings, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator)
+{
+	// Update the velocity
+	Vec3 desired_velocity = mLinearVelocity;
+	mLinearVelocity = CancelVelocityTowardsSteepSlopes(desired_velocity);
+
+	// Remember old position
+	RVec3 old_position = mPosition;
+
+	// Track if on ground before the update
+	bool ground_to_air = IsSupported();
+
+	// Update the character position (instant, do not have to wait for physics update)
+	Update(inDeltaTime, inGravity, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator);
+
+	// ... and that we got into air after
+	if (IsSupported())
+		ground_to_air = false;
+
+	// If stick to floor enabled and we're going from supported to not supported
+	if (ground_to_air && !inSettings.mStickToFloorStepDown.IsNearZero())
+	{
+		// If we're not moving up, stick to the floor
+		float velocity = Vec3(mPosition - old_position).Dot(mUp) / inDeltaTime;
+		if (velocity <= 1.0e-6f)
+			StickToFloor(inSettings.mStickToFloorStepDown, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator);
+	}
+
+	// If walk stairs enabled
+	if (!inSettings.mWalkStairsStepUp.IsNearZero())
+	{
+		// Calculate how much we wanted to move horizontally
+		Vec3 desired_horizontal_step = desired_velocity * inDeltaTime;
+		desired_horizontal_step -= desired_horizontal_step.Dot(mUp) * mUp;
+		float desired_horizontal_step_len = desired_horizontal_step.Length();
+		if (desired_horizontal_step_len > 0.0f)
+		{
+			// Calculate how much we moved horizontally
+			Vec3 achieved_horizontal_step = Vec3(mPosition - old_position);
+			achieved_horizontal_step -= achieved_horizontal_step.Dot(mUp) * mUp;
+
+			// Only count movement in the direction of the desired movement
+			// (otherwise we find it ok if we're sliding downhill while we're trying to climb uphill)
+			Vec3 step_forward_normalized = desired_horizontal_step / desired_horizontal_step_len;
+			achieved_horizontal_step = max(0.0f, achieved_horizontal_step.Dot(step_forward_normalized)) * step_forward_normalized;
+			float achieved_horizontal_step_len = achieved_horizontal_step.Length();
+
+			// If we didn't move as far as we wanted and we're against a slope that's too steep
+			if (achieved_horizontal_step_len + 1.0e-4f < desired_horizontal_step_len
+				&& CanWalkStairs(desired_velocity))
+			{
+				// Calculate how much we should step forward
+				// Note that we clamp the step forward to a minimum distance. This is done because at very high frame rates the delta time
+				// may be very small, causing a very small step forward. If the step becomes small enough, we may not move far enough
+				// horizontally to actually end up at the top of the step.
+				Vec3 step_forward = step_forward_normalized * max(inSettings.mWalkStairsMinStepForward, desired_horizontal_step_len - achieved_horizontal_step_len);
+
+				// Calculate how far to scan ahead for a floor. This is only used in case the floor normal at step_forward is too steep.
+				// In that case an additional check will be performed at this distance to check if that normal is not too steep.
+				// Start with the ground normal in the horizontal plane and normalizing it
+				Vec3 step_forward_test = -mGroundNormal;
+				step_forward_test -= step_forward_test.Dot(mUp) * mUp;
+				step_forward_test = step_forward_test.NormalizedOr(step_forward_normalized);
+
+				// If this normalized vector and the character forward vector is bigger than a preset angle, we use the character forward vector instead of the ground normal
+				// to do our forward test
+				if (step_forward_test.Dot(step_forward_normalized) < inSettings.mWalkStairsCosAngleForwardContact)
+					step_forward_test = step_forward_normalized;
+
+				// Calculate the correct magnitude for the test vector
+				step_forward_test *= inSettings.mWalkStairsStepForwardTest;
+
+				WalkStairs(inDeltaTime, inSettings.mWalkStairsStepUp, step_forward, step_forward_test, inSettings.mWalkStairsStepDownExtra, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator);
+			}
+		}
+	}
+}
+
+void CharacterVirtual::Contact::SaveState(StateRecorder &inStream) const
+{
+	inStream.Write(mPosition);
+	inStream.Write(mLinearVelocity);
+	inStream.Write(mContactNormal);
+	inStream.Write(mSurfaceNormal);
+	inStream.Write(mDistance);
+	inStream.Write(mFraction);
+	inStream.Write(mBodyB);
+	inStream.Write(mSubShapeIDB);
+	inStream.Write(mMotionTypeB);
+	inStream.Write(mHadCollision);
+	inStream.Write(mWasDiscarded);
+	inStream.Write(mCanPushCharacter);
+	// Cannot store user data (may be a pointer) and material
+}
+
+void CharacterVirtual::Contact::RestoreState(StateRecorder &inStream)
+{
+	inStream.Read(mPosition);
+	inStream.Read(mLinearVelocity);
+	inStream.Read(mContactNormal);
+	inStream.Read(mSurfaceNormal);
+	inStream.Read(mDistance);
+	inStream.Read(mFraction);
+	inStream.Read(mBodyB);
+	inStream.Read(mSubShapeIDB);
+	inStream.Read(mMotionTypeB);
+	inStream.Read(mHadCollision);
+	inStream.Read(mWasDiscarded);
+	inStream.Read(mCanPushCharacter);
+	mUserData = 0; // Cannot restore user data
+	mMaterial = PhysicsMaterial::sDefault; // Cannot restore material
+}
+
+void CharacterVirtual::SaveState(StateRecorder &inStream) const
+{
+	CharacterBase::SaveState(inStream);
+
+	inStream.Write(mPosition);
+	inStream.Write(mRotation);
+	inStream.Write(mLinearVelocity);
+	inStream.Write(mLastDeltaTime);
+	inStream.Write(mMaxHitsExceeded);
+
+	// Store contacts that had collision, we're using it at the beginning of the step in CancelVelocityTowardsSteepSlopes
+	uint32 num_contacts = 0;
+	for (const Contact &c : mActiveContacts)
+		if (c.mHadCollision)
+			++num_contacts;
+	inStream.Write(num_contacts);
+	for (const Contact &c : mActiveContacts)
+		if (c.mHadCollision)
+			c.SaveState(inStream);
+}
+
+void CharacterVirtual::RestoreState(StateRecorder &inStream)
+{
+	CharacterBase::RestoreState(inStream);
+
+	inStream.Read(mPosition);
+	inStream.Read(mRotation);
+	inStream.Read(mLinearVelocity);
+	inStream.Read(mLastDeltaTime);
+	inStream.Read(mMaxHitsExceeded);
+
+	// When validating remove contacts that don't have collision since we didn't save them
+	if (inStream.IsValidating())
+		for (int i = (int)mActiveContacts.size() - 1; i >= 0; --i)
+			if (!mActiveContacts[i].mHadCollision)
+				mActiveContacts.erase(mActiveContacts.begin() + i);
+
+	uint32 num_contacts = (uint32)mActiveContacts.size();
+	inStream.Read(num_contacts);
+	mActiveContacts.resize(num_contacts);
+	for (Contact &c : mActiveContacts)
+		c.RestoreState(inStream);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.h b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.h
new file mode 100644
index 0000000000..65b8f89df5
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Character/CharacterVirtual.h
@@ -0,0 +1,642 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Character/CharacterBase.h>
+#include <Jolt/Physics/Body/MotionType.h>
+#include <Jolt/Physics/Body/BodyFilter.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h>
+#include <Jolt/Physics/Collision/ObjectLayer.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Core/STLTempAllocator.h>
+
+JPH_NAMESPACE_BEGIN
+
+class CharacterVirtual;
+class CollideShapeSettings;
+
+/// Contains the configuration of a character
+class JPH_EXPORT CharacterVirtualSettings : public CharacterBaseSettings
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Character mass (kg). Used to push down objects with gravity when the character is standing on top.
+	float								mMass = 70.0f;
+
+	/// Maximum force with which the character can push other bodies (N).
+	float								mMaxStrength = 100.0f;
+
+	/// An extra offset applied to the shape in local space. This allows applying an extra offset to the shape in local space.
+	Vec3								mShapeOffset = Vec3::sZero();
+
+	///@name Movement settings
+	EBackFaceMode						mBackFaceMode = EBackFaceMode::CollideWithBackFaces;	///< When colliding with back faces, the character will not be able to move through back facing triangles. Use this if you have triangles that need to collide on both sides.
+	float								mPredictiveContactDistance = 0.1f;						///< How far to scan outside of the shape for predictive contacts. A value of 0 will most likely cause the character to get stuck as it cannot properly calculate a sliding direction anymore. A value that's too high will cause ghost collisions.
+	uint								mMaxCollisionIterations = 5;							///< Max amount of collision loops
+	uint								mMaxConstraintIterations = 15;							///< How often to try stepping in the constraint solving
+	float								mMinTimeRemaining = 1.0e-4f;							///< Early out condition: If this much time is left to simulate we are done
+	float								mCollisionTolerance = 1.0e-3f;							///< How far we're willing to penetrate geometry
+	float								mCharacterPadding = 0.02f;								///< How far we try to stay away from the geometry, this ensures that the sweep will hit as little as possible lowering the collision cost and reducing the risk of getting stuck
+	uint								mMaxNumHits = 256;										///< Max num hits to collect in order to avoid excess of contact points collection
+	float								mHitReductionCosMaxAngle = 0.999f;						///< Cos(angle) where angle is the maximum angle between two hits contact normals that are allowed to be merged during hit reduction. Default is around 2.5 degrees. Set to -1 to turn off.
+	float								mPenetrationRecoverySpeed = 1.0f;						///< This value governs how fast a penetration will be resolved, 0 = nothing is resolved, 1 = everything in one update
+
+	/// This character can optionally have an inner rigid body. This rigid body can be used to give the character presence in the world. When set it means that:
+	/// - Regular collision checks (e.g. NarrowPhaseQuery::CastRay) will collide with the rigid body (they cannot collide with CharacterVirtual since it is not added to the broad phase)
+	/// - Regular contact callbacks will be called through the ContactListener (next to the ones that will be passed to the CharacterContactListener)
+	/// - Fast moving objects of motion quality LinearCast will not be able to pass through the CharacterVirtual in 1 time step
+	RefConst<Shape>						mInnerBodyShape;
+
+	/// Layer that the inner rigid body will be added to
+	ObjectLayer							mInnerBodyLayer = 0;
+};
+
+/// This class contains settings that allow you to override the behavior of a character's collision response
+class CharacterContactSettings
+{
+public:
+	/// True when the object can push the virtual character.
+	bool								mCanPushCharacter = true;
+
+	/// True when the virtual character can apply impulses (push) the body.
+	/// Note that this only works against rigid bodies. Other CharacterVirtual objects can only be moved in their own update,
+	/// so you must ensure that in their OnCharacterContactAdded mCanPushCharacter is true.
+	bool								mCanReceiveImpulses = true;
+};
+
+/// This class receives callbacks when a virtual character hits something.
+class JPH_EXPORT CharacterContactListener
+{
+public:
+	/// Destructor
+	virtual								~CharacterContactListener() = default;
+
+	/// Callback to adjust the velocity of a body as seen by the character. Can be adjusted to e.g. implement a conveyor belt or an inertial dampener system of a sci-fi space ship.
+	/// Note that inBody2 is locked during the callback so you can read its properties freely.
+	virtual void						OnAdjustBodyVelocity(const CharacterVirtual *inCharacter, const Body &inBody2, Vec3 &ioLinearVelocity, Vec3 &ioAngularVelocity) { /* Do nothing, the linear and angular velocity are already filled in */ }
+
+	/// Checks if a character can collide with specified body. Return true if the contact is valid.
+	virtual bool						OnContactValidate(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2) { return true; }
+
+	/// Same as OnContactValidate but when colliding with a CharacterVirtual
+	virtual bool						OnCharacterContactValidate(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2) { return true; }
+
+	/// Called whenever the character collides with a body.
+	/// @param inCharacter Character that is being solved
+	/// @param inBodyID2 Body ID of body that is being hit
+	/// @param inSubShapeID2 Sub shape ID of shape that is being hit
+	/// @param inContactPosition World space contact position
+	/// @param inContactNormal World space contact normal
+	/// @param ioSettings Settings returned by the contact callback to indicate how the character should behave
+	virtual void						OnContactAdded(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) { /* Default do nothing */ }
+
+	/// Same as OnContactAdded but when colliding with a CharacterVirtual
+	virtual void						OnCharacterContactAdded(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) { /* Default do nothing */ }
+
+	/// Called whenever a contact is being used by the solver. Allows the listener to override the resulting character velocity (e.g. by preventing sliding along certain surfaces).
+	/// @param inCharacter Character that is being solved
+	/// @param inBodyID2 Body ID of body that is being hit
+	/// @param inSubShapeID2 Sub shape ID of shape that is being hit
+	/// @param inContactPosition World space contact position
+	/// @param inContactNormal World space contact normal
+	/// @param inContactVelocity World space velocity of contact point (e.g. for a moving platform)
+	/// @param inContactMaterial Material of contact point
+	/// @param inCharacterVelocity World space velocity of the character prior to hitting this contact
+	/// @param ioNewCharacterVelocity Contains the calculated world space velocity of the character after hitting this contact, this velocity slides along the surface of the contact. Can be modified by the listener to provide an alternative velocity.
+	virtual void						OnContactSolve(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, Vec3Arg inContactVelocity, const PhysicsMaterial *inContactMaterial, Vec3Arg inCharacterVelocity, Vec3 &ioNewCharacterVelocity) { /* Default do nothing */ }
+
+	/// Same as OnContactSolve but when colliding with a CharacterVirtual
+	virtual void						OnCharacterContactSolve(const CharacterVirtual *inCharacter, const CharacterVirtual *inOtherCharacter, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, Vec3Arg inContactVelocity, const PhysicsMaterial *inContactMaterial, Vec3Arg inCharacterVelocity, Vec3 &ioNewCharacterVelocity) { /* Default do nothing */ }
+};
+
+/// Interface class that allows a CharacterVirtual to check collision with other CharacterVirtual instances.
+/// Since CharacterVirtual instances are not registered anywhere, it is up to the application to test collision against relevant characters.
+/// The characters could be stored in a tree structure to make this more efficient.
+class JPH_EXPORT CharacterVsCharacterCollision : public NonCopyable
+{
+public:
+	virtual								~CharacterVsCharacterCollision() = default;
+
+	/// Collide a character against other CharacterVirtuals.
+	/// @param inCharacter The character to collide.
+	/// @param inCenterOfMassTransform Center of mass transform for this character.
+	/// @param inCollideShapeSettings Settings for the collision check.
+	/// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. GetPosition() since floats are most accurate near the origin
+	/// @param ioCollector Collision collector that receives the collision results.
+	virtual void						CollideCharacter(const CharacterVirtual *inCharacter, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector) const = 0;
+
+	/// Cast a character against other CharacterVirtuals.
+	/// @param inCharacter The character to cast.
+	/// @param inCenterOfMassTransform Center of mass transform for this character.
+	/// @param inDirection Direction and length to cast in.
+	/// @param inShapeCastSettings Settings for the shape cast.
+	/// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. GetPosition() since floats are most accurate near the origin
+	/// @param ioCollector Collision collector that receives the collision results.
+	virtual void						CastCharacter(const CharacterVirtual *inCharacter, RMat44Arg inCenterOfMassTransform, Vec3Arg inDirection, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector) const = 0;
+};
+
+/// Simple collision checker that loops over all registered characters.
+/// Note that this is not thread safe, so make sure that only one CharacterVirtual is checking collision at a time.
+class JPH_EXPORT CharacterVsCharacterCollisionSimple : public CharacterVsCharacterCollision
+{
+public:
+	/// Add a character to the list of characters to check collision against.
+	void								Add(CharacterVirtual *inCharacter)						{ mCharacters.push_back(inCharacter); }
+
+	/// Remove a character from the list of characters to check collision against.
+	void								Remove(const CharacterVirtual *inCharacter);
+
+	// See: CharacterVsCharacterCollision
+	virtual void						CollideCharacter(const CharacterVirtual *inCharacter, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector) const override;
+	virtual void						CastCharacter(const CharacterVirtual *inCharacter, RMat44Arg inCenterOfMassTransform, Vec3Arg inDirection, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector) const override;
+
+	Array<CharacterVirtual *>			mCharacters;											///< The list of characters to check collision against
+};
+
+/// Runtime character object.
+/// This object usually represents the player. Contrary to the Character class it doesn't use a rigid body but moves doing collision checks only (hence the name virtual).
+/// The advantage of this is that you can determine when the character moves in the frame (usually this has to happen at a very particular point in the frame)
+/// but the downside is that other objects don't see this virtual character. In order to make this work it is recommended to pair a CharacterVirtual with a Character that
+/// moves along. This Character should be keyframed (or at least have no gravity) and move along with the CharacterVirtual so that other rigid bodies can collide with it.
+class JPH_EXPORT CharacterVirtual : public CharacterBase
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+	/// @param inSettings The settings for the character
+	/// @param inPosition Initial position for the character
+	/// @param inRotation Initial rotation for the character (usually only around the up-axis)
+	/// @param inUserData Application specific value
+	/// @param inSystem Physics system that this character will be added to
+										CharacterVirtual(const CharacterVirtualSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, uint64 inUserData, PhysicsSystem *inSystem);
+
+	/// Constructor without user data
+										CharacterVirtual(const CharacterVirtualSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, PhysicsSystem *inSystem) : CharacterVirtual(inSettings, inPosition, inRotation, 0, inSystem) { }
+
+	/// Destructor
+	virtual								~CharacterVirtual() override;
+
+	/// Set the contact listener
+	void								SetListener(CharacterContactListener *inListener)		{ mListener = inListener; }
+
+	/// Get the current contact listener
+	CharacterContactListener *			GetListener() const										{ return mListener; }
+
+	/// Set the character vs character collision interface
+	void								SetCharacterVsCharacterCollision(CharacterVsCharacterCollision *inCharacterVsCharacterCollision) { mCharacterVsCharacterCollision = inCharacterVsCharacterCollision; }
+
+	/// Get the linear velocity of the character (m / s)
+	Vec3								GetLinearVelocity() const								{ return mLinearVelocity; }
+
+	/// Set the linear velocity of the character (m / s)
+	void								SetLinearVelocity(Vec3Arg inLinearVelocity)				{ mLinearVelocity = inLinearVelocity; }
+
+	/// Get the position of the character
+	RVec3								GetPosition() const										{ return mPosition; }
+
+	/// Set the position of the character
+	void								SetPosition(RVec3Arg inPosition)						{ mPosition = inPosition; UpdateInnerBodyTransform(); }
+
+	/// Get the rotation of the character
+	Quat								GetRotation() const										{ return mRotation; }
+
+	/// Set the rotation of the character
+	void								SetRotation(QuatArg inRotation)							{ mRotation = inRotation; UpdateInnerBodyTransform(); }
+
+	// Get the center of mass position of the shape
+	inline RVec3						GetCenterOfMassPosition() const							{ return mPosition + (mRotation * (mShapeOffset + mShape->GetCenterOfMass()) + mCharacterPadding * mUp); }
+
+	/// Calculate the world transform of the character
+	RMat44								GetWorldTransform() const								{ return RMat44::sRotationTranslation(mRotation, mPosition); }
+
+	/// Calculates the transform for this character's center of mass
+	RMat44								GetCenterOfMassTransform() const						{ return GetCenterOfMassTransform(mPosition, mRotation, mShape); }
+
+	/// Character mass (kg)
+	float								GetMass() const											{ return mMass; }
+	void								SetMass(float inMass)									{ mMass = inMass; }
+
+	/// Maximum force with which the character can push other bodies (N)
+	float								GetMaxStrength() const									{ return mMaxStrength; }
+	void								SetMaxStrength(float inMaxStrength)						{ mMaxStrength = inMaxStrength; }
+
+	/// This value governs how fast a penetration will be resolved, 0 = nothing is resolved, 1 = everything in one update
+	float								GetPenetrationRecoverySpeed() const						{ return mPenetrationRecoverySpeed; }
+	void								SetPenetrationRecoverySpeed(float inSpeed)				{ mPenetrationRecoverySpeed = inSpeed; }
+
+	/// Set to indicate that extra effort should be made to try to remove ghost contacts (collisions with internal edges of a mesh). This is more expensive but makes bodies move smoother over a mesh with convex edges.
+	bool								GetEnhancedInternalEdgeRemoval() const					{ return mEnhancedInternalEdgeRemoval; }
+	void								SetEnhancedInternalEdgeRemoval(bool inApply)			{ mEnhancedInternalEdgeRemoval = inApply; }
+
+	/// Character padding
+	float								GetCharacterPadding() const								{ return mCharacterPadding; }
+
+	/// Max num hits to collect in order to avoid excess of contact points collection
+	uint								GetMaxNumHits() const									{ return mMaxNumHits; }
+	void								SetMaxNumHits(uint inMaxHits)							{ mMaxNumHits = inMaxHits; }
+
+	/// Cos(angle) where angle is the maximum angle between two hits contact normals that are allowed to be merged during hit reduction. Default is around 2.5 degrees. Set to -1 to turn off.
+	float								GetHitReductionCosMaxAngle() const						{ return mHitReductionCosMaxAngle; }
+	void								SetHitReductionCosMaxAngle(float inCosMaxAngle)			{ mHitReductionCosMaxAngle = inCosMaxAngle; }
+
+	/// Returns if we exceeded the maximum number of hits during the last collision check and had to discard hits based on distance.
+	/// This can be used to find areas that have too complex geometry for the character to navigate properly.
+	/// To solve you can either increase the max number of hits or simplify the geometry. Note that the character simulation will
+	/// try to do its best to select the most relevant contacts to avoid the character from getting stuck.
+	bool								GetMaxHitsExceeded() const								{ return mMaxHitsExceeded; }
+
+	/// An extra offset applied to the shape in local space. This allows applying an extra offset to the shape in local space. Note that setting it on the fly can cause the shape to teleport into collision.
+	Vec3								GetShapeOffset() const									{ return mShapeOffset; }
+	void								SetShapeOffset(Vec3Arg inShapeOffset)					{ mShapeOffset = inShapeOffset; UpdateInnerBodyTransform(); }
+
+	/// Access to the user data, can be used for anything by the application
+	uint64								GetUserData() const										{ return mUserData; }
+	void								SetUserData(uint64 inUserData);
+
+	/// Optional inner rigid body that proxies the character in the world. Can be used to update body properties.
+	BodyID								GetInnerBodyID() const									{ return mInnerBodyID; }
+
+	/// This function can be called prior to calling Update() to convert a desired velocity into a velocity that won't make the character move further onto steep slopes.
+	/// This velocity can then be set on the character using SetLinearVelocity()
+	/// @param inDesiredVelocity Velocity to clamp against steep walls
+	/// @return A new velocity vector that won't make the character move up steep slopes
+	Vec3								CancelVelocityTowardsSteepSlopes(Vec3Arg inDesiredVelocity) const;
+
+	/// This is the main update function. It moves the character according to its current velocity (the character is similar to a kinematic body in the sense
+	/// that you set the velocity and the character will follow unless collision is blocking the way). Note it's your own responsibility to apply gravity to the character velocity!
+	/// Different surface materials (like ice) can be emulated by getting the ground material and adjusting the velocity and/or the max slope angle accordingly every frame.
+	/// @param inDeltaTime Time step to simulate.
+	/// @param inGravity Gravity vector (m/s^2). This gravity vector is only used when the character is standing on top of another object to apply downward force.
+	/// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase.
+	/// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer.
+	/// @param inBodyFilter Filter that is used to check if a character collides with a body.
+	/// @param inShapeFilter Filter that is used to check if a character collides with a subshape.
+	/// @param inAllocator An allocator for temporary allocations. All memory will be freed by the time this function returns.
+	void								Update(float inDeltaTime, Vec3Arg inGravity, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator);
+
+	/// This function will return true if the character has moved into a slope that is too steep (e.g. a vertical wall).
+	/// You would call WalkStairs to attempt to step up stairs.
+	/// @param inLinearVelocity The linear velocity that the player desired. This is used to determine if we're pushing into a step.
+	bool								CanWalkStairs(Vec3Arg inLinearVelocity) const;
+
+	/// When stair walking is needed, you can call the WalkStairs function to cast up, forward and down again to try to find a valid position
+	/// @param inDeltaTime Time step to simulate.
+	/// @param inStepUp The direction and distance to step up (this corresponds to the max step height)
+	/// @param inStepForward The direction and distance to step forward after the step up
+	/// @param inStepForwardTest When running at a high frequency, inStepForward can be very small and it's likely that you hit the side of the stairs on the way down. This could produce a normal that violates the max slope angle. If this happens, we test again using this distance from the up position to see if we find a valid slope.
+	/// @param inStepDownExtra An additional translation that is added when stepping down at the end. Allows you to step further down than up. Set to zero if you don't want this. Should be in the opposite direction of up.
+	/// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase.
+	/// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer.
+	/// @param inBodyFilter Filter that is used to check if a character collides with a body.
+	/// @param inShapeFilter Filter that is used to check if a character collides with a subshape.
+	/// @param inAllocator An allocator for temporary allocations. All memory will be freed by the time this function returns.
+	/// @return true if the stair walk was successful
+	bool								WalkStairs(float inDeltaTime, Vec3Arg inStepUp, Vec3Arg inStepForward, Vec3Arg inStepForwardTest, Vec3Arg inStepDownExtra, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator);
+
+	/// This function can be used to artificially keep the character to the floor. Normally when a character is on a small step and starts moving horizontally, the character will
+	/// lose contact with the floor because the initial vertical velocity is zero while the horizontal velocity is quite high. To prevent the character from losing contact with the floor,
+	/// we do an additional collision check downwards and if we find the floor within a certain distance, we project the character onto the floor.
+	/// @param inStepDown Max amount to project the character downwards (if no floor is found within this distance, the function will return false)
+	/// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase.
+	/// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer.
+	/// @param inBodyFilter Filter that is used to check if a character collides with a body.
+	/// @param inShapeFilter Filter that is used to check if a character collides with a subshape.
+	/// @param inAllocator An allocator for temporary allocations. All memory will be freed by the time this function returns.
+	/// @return True if the character was successfully projected onto the floor.
+	bool								StickToFloor(Vec3Arg inStepDown, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator);
+
+	/// Settings struct with settings for ExtendedUpdate
+	struct ExtendedUpdateSettings
+	{
+		Vec3							mStickToFloorStepDown { 0, -0.5f, 0 };									///< See StickToFloor inStepDown parameter. Can be zero to turn off.
+		Vec3							mWalkStairsStepUp { 0, 0.4f, 0 };										///< See WalkStairs inStepUp parameter. Can be zero to turn off.
+		float							mWalkStairsMinStepForward { 0.02f };									///< See WalkStairs inStepForward parameter. Note that the parameter only indicates a magnitude, direction is taken from current velocity.
+		float							mWalkStairsStepForwardTest { 0.15f };									///< See WalkStairs inStepForwardTest parameter. Note that the parameter only indicates a magnitude, direction is taken from current velocity.
+		float							mWalkStairsCosAngleForwardContact { Cos(DegreesToRadians(75.0f)) };		///< Cos(angle) where angle is the maximum angle between the ground normal in the horizontal plane and the character forward vector where we're willing to adjust the step forward test towards the contact normal.
+		Vec3							mWalkStairsStepDownExtra { Vec3::sZero() };								///< See WalkStairs inStepDownExtra
+	};
+
+	/// This function combines Update, StickToFloor and WalkStairs. This function serves as an example of how these functions could be combined.
+	/// Before calling, call SetLinearVelocity to update the horizontal/vertical speed of the character, typically this is:
+	/// - When on OnGround and not moving away from ground: velocity = GetGroundVelocity() + horizontal speed as input by player + optional vertical jump velocity + delta time * gravity
+	/// - Else: velocity = current vertical velocity + horizontal speed as input by player + delta time * gravity
+	/// @param inDeltaTime Time step to simulate.
+	/// @param inGravity Gravity vector (m/s^2). This gravity vector is only used when the character is standing on top of another object to apply downward force.
+	/// @param inSettings A structure containing settings for the algorithm.
+	/// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase.
+	/// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer.
+	/// @param inBodyFilter Filter that is used to check if a character collides with a body.
+	/// @param inShapeFilter Filter that is used to check if a character collides with a subshape.
+	/// @param inAllocator An allocator for temporary allocations. All memory will be freed by the time this function returns.
+	void								ExtendedUpdate(float inDeltaTime, Vec3Arg inGravity, const ExtendedUpdateSettings &inSettings, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator);
+
+	/// This function can be used after a character has teleported to determine the new contacts with the world.
+	void								RefreshContacts(const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator);
+
+	/// Use the ground body ID to get an updated estimate of the ground velocity. This function can be used if the ground body has moved / changed velocity and you want a new estimate of the ground velocity.
+	/// It will not perform collision detection, so is less accurate than RefreshContacts but a lot faster.
+	void								UpdateGroundVelocity();
+
+	/// Switch the shape of the character (e.g. for stance).
+	/// @param inShape The shape to switch to.
+	/// @param inMaxPenetrationDepth When inMaxPenetrationDepth is not FLT_MAX, it checks if the new shape collides before switching shape. This is the max penetration we're willing to accept after the switch.
+	/// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase.
+	/// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer.
+	/// @param inBodyFilter Filter that is used to check if a character collides with a body.
+	/// @param inShapeFilter Filter that is used to check if a character collides with a subshape.
+	/// @param inAllocator An allocator for temporary allocations. All memory will be freed by the time this function returns.
+	/// @return Returns true if the switch succeeded.
+	bool								SetShape(const Shape *inShape, float inMaxPenetrationDepth, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator);
+
+	/// Updates the shape of the inner rigid body. Should be called after a successful call to SetShape.
+	void								SetInnerBodyShape(const Shape *inShape);
+
+	/// Get the transformed shape that represents the volume of the character, can be used for collision checks.
+	TransformedShape					GetTransformedShape() const								{ return TransformedShape(GetCenterOfMassPosition(), mRotation, mShape, mInnerBodyID); }
+
+	/// @brief Get all contacts for the character at a particular location.
+	/// When colliding with another character virtual, this pointer will be provided through CollideShapeCollector::SetUserContext before adding a hit.
+	/// @param inPosition Position to test, note that this position will be corrected for the character padding.
+	/// @param inRotation Rotation at which to test the shape.
+	/// @param inMovementDirection A hint in which direction the character is moving, will be used to calculate a proper normal.
+	/// @param inMaxSeparationDistance How much distance around the character you want to report contacts in (can be 0 to match the character exactly).
+	/// @param inShape Shape to test collision with.
+	/// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. GetPosition() since floats are most accurate near the origin
+	/// @param ioCollector Collision collector that receives the collision results.
+	/// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase.
+	/// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer.
+	/// @param inBodyFilter Filter that is used to check if a character collides with a body.
+	/// @param inShapeFilter Filter that is used to check if a character collides with a subshape.
+	void								CheckCollision(RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const;
+
+	// Saving / restoring state for replay
+	virtual void						SaveState(StateRecorder &inStream) const override;
+	virtual void						RestoreState(StateRecorder &inStream) override;
+
+#ifdef JPH_DEBUG_RENDERER
+	static inline bool					sDrawConstraints = false;								///< Draw the current state of the constraints for iteration 0 when creating them
+	static inline bool					sDrawWalkStairs = false;								///< Draw the state of the walk stairs algorithm
+	static inline bool					sDrawStickToFloor = false;								///< Draw the state of the stick to floor algorithm
+#endif
+
+	// Encapsulates a collision contact
+	struct Contact
+	{
+		// Saving / restoring state for replay
+		void							SaveState(StateRecorder &inStream) const;
+		void							RestoreState(StateRecorder &inStream);
+
+		// Checks if two contacts refer to the same body (or virtual character)
+		inline bool						IsSameBody(const Contact &inOther) const				{ return mBodyB == inOther.mBodyB && mCharacterB == inOther.mCharacterB; }
+
+		RVec3							mPosition;												///< Position where the character makes contact
+		Vec3							mLinearVelocity;										///< Velocity of the contact point
+		Vec3							mContactNormal;											///< Contact normal, pointing towards the character
+		Vec3							mSurfaceNormal;											///< Surface normal of the contact
+		float							mDistance;												///< Distance to the contact <= 0 means that it is an actual contact, > 0 means predictive
+		float							mFraction;												///< Fraction along the path where this contact takes place
+		BodyID							mBodyB;													///< ID of body we're colliding with (if not invalid)
+		CharacterVirtual *				mCharacterB = nullptr;									///< Character we're colliding with (if not null)
+		SubShapeID						mSubShapeIDB;											///< Sub shape ID of body we're colliding with
+		EMotionType						mMotionTypeB;											///< Motion type of B, used to determine the priority of the contact
+		bool							mIsSensorB;												///< If B is a sensor
+		uint64							mUserData;												///< User data of B
+		const PhysicsMaterial *			mMaterial;												///< Material of B
+		bool							mHadCollision = false;									///< If the character actually collided with the contact (can be false if a predictive contact never becomes a real one)
+		bool							mWasDiscarded = false;									///< If the contact validate callback chose to discard this contact
+		bool							mCanPushCharacter = true;								///< When true, the velocity of the contact point can push the character
+	};
+
+	using TempContactList = Array<Contact, STLTempAllocator<Contact>>;
+	using ContactList = Array<Contact>;
+
+	/// Access to the internal list of contacts that the character has found.
+	const ContactList &					GetActiveContacts() const								{ return mActiveContacts; }
+
+	/// Check if the character is currently in contact with or has collided with another body in the last time step
+	bool								HasCollidedWith(const BodyID &inBody) const
+	{
+		for (const CharacterVirtual::Contact &c : mActiveContacts)
+			if (c.mHadCollision && c.mBodyB == inBody)
+				return true;
+		return false;
+	}
+
+	/// Check if the character is currently in contact with or has collided with another character in the last time step
+	bool								HasCollidedWith(const CharacterVirtual *inCharacter) const
+	{
+		for (const CharacterVirtual::Contact &c : mActiveContacts)
+			if (c.mHadCollision && c.mCharacterB == inCharacter)
+				return true;
+		return false;
+	}
+
+private:
+	// Sorting predicate for making contact order deterministic
+	struct ContactOrderingPredicate
+	{
+		inline bool						operator () (const Contact &inLHS, const Contact &inRHS) const
+		{
+			if (inLHS.mBodyB != inRHS.mBodyB)
+				return inLHS.mBodyB < inRHS.mBodyB;
+
+			return inLHS.mSubShapeIDB.GetValue() < inRHS.mSubShapeIDB.GetValue();
+		}
+	};
+
+	// A contact that needs to be ignored
+	struct IgnoredContact
+	{
+										IgnoredContact() = default;
+										IgnoredContact(const BodyID &inBodyID, const SubShapeID &inSubShapeID) : mBodyID(inBodyID), mSubShapeID(inSubShapeID) { }
+
+		BodyID							mBodyID;												///< ID of body we're colliding with
+		SubShapeID						mSubShapeID;											///< Sub shape of body we're colliding with
+	};
+
+	using IgnoredContactList = Array<IgnoredContact, STLTempAllocator<IgnoredContact>>;
+
+	// A constraint that limits the movement of the character
+	struct Constraint
+	{
+		Contact *						mContact;												///< Contact that this constraint was generated from
+		float							mTOI;													///< Calculated time of impact (can be negative if penetrating)
+		float							mProjectedVelocity;										///< Velocity of the contact projected on the contact normal (negative if separating)
+		Vec3							mLinearVelocity;										///< Velocity of the contact (can contain a corrective velocity to resolve penetration)
+		Plane							mPlane;													///< Plane around the origin that describes how far we can displace (from the origin)
+		bool							mIsSteepSlope = false;									///< If this constraint belongs to a steep slope
+	};
+
+	using ConstraintList = Array<Constraint, STLTempAllocator<Constraint>>;
+
+	// Collision collector that collects hits for CollideShape
+	class ContactCollector : public CollideShapeCollector
+	{
+	public:
+										ContactCollector(PhysicsSystem *inSystem, const CharacterVirtual *inCharacter, uint inMaxHits, float inHitReductionCosMaxAngle, Vec3Arg inUp, RVec3Arg inBaseOffset, TempContactList &outContacts) : mBaseOffset(inBaseOffset), mUp(inUp), mSystem(inSystem), mCharacter(inCharacter), mContacts(outContacts), mMaxHits(inMaxHits), mHitReductionCosMaxAngle(inHitReductionCosMaxAngle) { }
+
+		virtual void					SetUserData(uint64 inUserData) override					{ mOtherCharacter = reinterpret_cast<CharacterVirtual *>(inUserData); }
+
+		virtual void					AddHit(const CollideShapeResult &inResult) override;
+
+		RVec3							mBaseOffset;
+		Vec3							mUp;
+		PhysicsSystem *					mSystem;
+		const CharacterVirtual *		mCharacter;
+		CharacterVirtual *				mOtherCharacter = nullptr;
+		TempContactList &				mContacts;
+		uint							mMaxHits;
+		float							mHitReductionCosMaxAngle;
+		bool							mMaxHitsExceeded = false;
+	};
+
+	// A collision collector that collects hits for CastShape
+	class ContactCastCollector : public CastShapeCollector
+	{
+	public:
+										ContactCastCollector(PhysicsSystem *inSystem, const CharacterVirtual *inCharacter, Vec3Arg inDisplacement, Vec3Arg inUp, const IgnoredContactList &inIgnoredContacts, RVec3Arg inBaseOffset, Contact &outContact) : mBaseOffset(inBaseOffset), mDisplacement(inDisplacement), mUp(inUp), mSystem(inSystem), mCharacter(inCharacter), mIgnoredContacts(inIgnoredContacts), mContact(outContact) { }
+
+		virtual void					SetUserData(uint64 inUserData) override					{ mOtherCharacter = reinterpret_cast<CharacterVirtual *>(inUserData); }
+
+		virtual void					AddHit(const ShapeCastResult &inResult) override;
+
+		RVec3							mBaseOffset;
+		Vec3							mDisplacement;
+		Vec3							mUp;
+		PhysicsSystem *					mSystem;
+		const CharacterVirtual *		mCharacter;
+		CharacterVirtual *				mOtherCharacter = nullptr;
+		const IgnoredContactList &		mIgnoredContacts;
+		Contact &						mContact;
+	};
+
+	// Helper function to convert a Jolt collision result into a contact
+	template <class taCollector>
+	inline static void					sFillContactProperties(const CharacterVirtual *inCharacter, Contact &outContact, const Body &inBody, Vec3Arg inUp, RVec3Arg inBaseOffset, const taCollector &inCollector, const CollideShapeResult &inResult);
+	inline static void					sFillCharacterContactProperties(Contact &outContact, CharacterVirtual *inOtherCharacter, RVec3Arg inBaseOffset, const CollideShapeResult &inResult);
+
+	// Move the shape from ioPosition and try to displace it by inVelocity * inDeltaTime, this will try to slide the shape along the world geometry
+	void								MoveShape(RVec3 &ioPosition, Vec3Arg inVelocity, float inDeltaTime, ContactList *outActiveContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator
+	#ifdef JPH_DEBUG_RENDERER
+		, bool inDrawConstraints = false
+	#endif // JPH_DEBUG_RENDERER
+		) const;
+
+	// Ask the callback if inContact is a valid contact point
+	bool								ValidateContact(const Contact &inContact) const;
+
+	// Trigger the contact callback for inContact and get the contact settings
+	void								ContactAdded(const Contact &inContact, CharacterContactSettings &ioSettings) const;
+
+	// Tests the shape for collision around inPosition
+	void								GetContactsAtPosition(RVec3Arg inPosition, Vec3Arg inMovementDirection, const Shape *inShape, TempContactList &outContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const;
+
+	// Remove penetrating contacts with the same body that have conflicting normals, leaving these will make the character mover get stuck
+	void								RemoveConflictingContacts(TempContactList &ioContacts, IgnoredContactList &outIgnoredContacts) const;
+
+	// Convert contacts into constraints. The character is assumed to start at the origin and the constraints are planes around the origin that confine the movement of the character.
+	void								DetermineConstraints(TempContactList &inContacts, float inDeltaTime, ConstraintList &outConstraints) const;
+
+	// Use the constraints to solve the displacement of the character. This will slide the character on the planes around the origin for as far as possible.
+	void								SolveConstraints(Vec3Arg inVelocity, float inDeltaTime, float inTimeRemaining, ConstraintList &ioConstraints, IgnoredContactList &ioIgnoredContacts, float &outTimeSimulated, Vec3 &outDisplacement, TempAllocator &inAllocator
+	#ifdef JPH_DEBUG_RENDERER
+		, bool inDrawConstraints = false
+	#endif // JPH_DEBUG_RENDERER
+		) const;
+
+	// Get the velocity of a body adjusted by the contact listener
+	void								GetAdjustedBodyVelocity(const Body& inBody, Vec3 &outLinearVelocity, Vec3 &outAngularVelocity) const;
+
+	// Calculate the ground velocity of the character assuming it's standing on an object with specified linear and angular velocity and with specified center of mass.
+	// Note that we don't just take the point velocity because a point on an object with angular velocity traces an arc,
+	// so if you just take point velocity * delta time you get an error that accumulates over time
+	Vec3								CalculateCharacterGroundVelocity(RVec3Arg inCenterOfMass, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, float inDeltaTime) const;
+
+	// Handle contact with physics object that we're colliding against
+	bool								HandleContact(Vec3Arg inVelocity, Constraint &ioConstraint, float inDeltaTime) const;
+
+	// Does a swept test of the shape from inPosition with displacement inDisplacement, returns true if there was a collision
+	bool								GetFirstContactForSweep(RVec3Arg inPosition, Vec3Arg inDisplacement, Contact &outContact, const IgnoredContactList &inIgnoredContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const;
+
+	// Store contacts so that we have proper ground information
+	void								StoreActiveContacts(const TempContactList &inContacts, TempAllocator &inAllocator);
+
+	// This function will determine which contacts are touching the character and will calculate the one that is supporting us
+	void								UpdateSupportingContact(bool inSkipContactVelocityCheck, TempAllocator &inAllocator);
+
+	/// This function can be called after moving the character to a new colliding position
+	void								MoveToContact(RVec3Arg inPosition, const Contact &inContact, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator);
+
+	// This function returns the actual center of mass of the shape, not corrected for the character padding
+	inline RMat44						GetCenterOfMassTransform(RVec3Arg inPosition, QuatArg inRotation, const Shape *inShape) const
+	{
+		return RMat44::sRotationTranslation(inRotation, inPosition).PreTranslated(mShapeOffset + inShape->GetCenterOfMass()).PostTranslated(mCharacterPadding * mUp);
+	}
+
+	// This function returns the position of the inner rigid body
+	inline RVec3						GetInnerBodyPosition() const
+	{
+		return mPosition + (mRotation * mShapeOffset + mCharacterPadding * mUp);
+	}
+
+	// Move the inner rigid body to the current position
+	void								UpdateInnerBodyTransform();
+
+	// Our main listener for contacts
+	CharacterContactListener *			mListener = nullptr;
+
+	// Interface to detect collision between characters
+	CharacterVsCharacterCollision *		mCharacterVsCharacterCollision = nullptr;
+
+	// Movement settings
+	EBackFaceMode						mBackFaceMode;											// When colliding with back faces, the character will not be able to move through back facing triangles. Use this if you have triangles that need to collide on both sides.
+	float								mPredictiveContactDistance;								// How far to scan outside of the shape for predictive contacts. A value of 0 will most likely cause the character to get stuck as it cannot properly calculate a sliding direction anymore. A value that's too high will cause ghost collisions.
+	uint								mMaxCollisionIterations;								// Max amount of collision loops
+	uint								mMaxConstraintIterations;								// How often to try stepping in the constraint solving
+	float								mMinTimeRemaining;										// Early out condition: If this much time is left to simulate we are done
+	float								mCollisionTolerance;									// How far we're willing to penetrate geometry
+	float								mCharacterPadding;										// How far we try to stay away from the geometry, this ensures that the sweep will hit as little as possible lowering the collision cost and reducing the risk of getting stuck
+	uint								mMaxNumHits;											// Max num hits to collect in order to avoid excess of contact points collection
+	float								mHitReductionCosMaxAngle;								// Cos(angle) where angle is the maximum angle between two hits contact normals that are allowed to be merged during hit reduction. Default is around 2.5 degrees. Set to -1 to turn off.
+	float								mPenetrationRecoverySpeed;								// This value governs how fast a penetration will be resolved, 0 = nothing is resolved, 1 = everything in one update
+	bool								mEnhancedInternalEdgeRemoval;							// Set to indicate that extra effort should be made to try to remove ghost contacts (collisions with internal edges of a mesh). This is more expensive but makes bodies move smoother over a mesh with convex edges.
+
+	// Character mass (kg)
+	float								mMass;
+
+	// Maximum force with which the character can push other bodies (N)
+	float								mMaxStrength;
+
+	// An extra offset applied to the shape in local space. This allows applying an extra offset to the shape in local space.
+	Vec3								mShapeOffset = Vec3::sZero();
+
+	// Current position (of the base, not the center of mass)
+	RVec3								mPosition = RVec3::sZero();
+
+	// Current rotation (of the base, not of the center of mass)
+	Quat								mRotation = Quat::sIdentity();
+
+	// Current linear velocity
+	Vec3								mLinearVelocity = Vec3::sZero();
+
+	// List of contacts that were active in the last frame
+	ContactList							mActiveContacts;
+
+	// Remembers the delta time of the last update
+	float								mLastDeltaTime = 1.0f / 60.0f;
+
+	// Remember if we exceeded the maximum number of hits and had to remove similar contacts
+	mutable bool						mMaxHitsExceeded = false;
+
+	// User data, can be used for anything by the application
+	uint64								mUserData = 0;
+
+	// The inner rigid body that proxies the character in the world
+	BodyID								mInnerBodyID;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/AABoxCast.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/AABoxCast.h
new file mode 100644
index 0000000000..a1cedf1ad9
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/AABoxCast.h
@@ -0,0 +1,20 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/AABox.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Structure that holds AABox moving linearly through 3d space
+struct AABoxCast
+{
+	JPH_OVERRIDE_NEW_DELETE
+
+	AABox						mBox;						///< Axis aligned box at starting location
+	Vec3						mDirection;					///< Direction and length of the cast (anything beyond this length will not be reported as a hit)
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ActiveEdgeMode.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/ActiveEdgeMode.h
new file mode 100644
index 0000000000..30f96aeb12
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ActiveEdgeMode.h
@@ -0,0 +1,17 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// How to treat active/inactive edges.
+/// An active edge is an edge that either has no neighbouring edge or if the angle between the two connecting faces is too large, see: ActiveEdges
+enum class EActiveEdgeMode : uint8
+{
+	CollideOnlyWithActive,								///< Do not collide with inactive edges. For physics simulation, this gives less ghost collisions.
+	CollideWithAll,										///< Collide with all edges. Use this when you're interested in all collisions.
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ActiveEdges.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/ActiveEdges.h
new file mode 100644
index 0000000000..7e51d2a63b
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ActiveEdges.h
@@ -0,0 +1,114 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/ClosestPoint.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// An active edge is an edge that either has no neighbouring edge or if the angle between the two connecting faces is too large.
+namespace ActiveEdges
+{
+	/// Helper function to check if an edge is active or not
+	/// @param inNormal1 Triangle normal of triangle on the left side of the edge (when looking along the edge from the top)
+	/// @param inNormal2 Triangle normal of triangle on the right side of the edge
+	/// @param inEdgeDirection Vector that points along the edge
+	/// @param inCosThresholdAngle Cosine of the threshold angle (if the angle between the two triangles is bigger than this, the edge is active, note that a concave edge is always inactive)
+	inline static bool					IsEdgeActive(Vec3Arg inNormal1, Vec3Arg inNormal2, Vec3Arg inEdgeDirection, float inCosThresholdAngle)
+	{
+		// If normals are opposite the edges are active (the triangles are back to back)
+		float cos_angle_normals = inNormal1.Dot(inNormal2);
+		if (cos_angle_normals < -0.999848f) // cos(179 degrees)
+			return true;
+
+		// Check if concave edge, if so we are not active
+		if (inNormal1.Cross(inNormal2).Dot(inEdgeDirection) < 0.0f)
+			return false;
+
+		// Convex edge, active when angle bigger than threshold
+		return cos_angle_normals < inCosThresholdAngle;
+	}
+
+	/// Replace normal by triangle normal if a hit is hitting an inactive edge
+	/// @param inV0 , inV1 , inV2 form the triangle
+	/// @param inTriangleNormal is the normal of the provided triangle (does not need to be normalized)
+	/// @param inActiveEdges bit 0 = edge v0..v1 is active, bit 1 = edge v1..v2 is active, bit 2 = edge v2..v0 is active
+	/// @param inPoint Collision point on the triangle
+	/// @param inNormal Collision normal on the triangle (does not need to be normalized)
+	/// @param inMovementDirection Can be zero. This gives an indication of in which direction the motion is to determine if when we hit an inactive edge/triangle we should return the triangle normal.
+	/// @return Returns inNormal if an active edge was hit, otherwise returns inTriangleNormal
+	inline static Vec3					FixNormal(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inTriangleNormal, uint8 inActiveEdges, Vec3Arg inPoint, Vec3Arg inNormal, Vec3Arg inMovementDirection)
+	{
+		// Check: All of the edges are active, we have the correct normal already. No need to call this function!
+		JPH_ASSERT(inActiveEdges != 0b111);
+
+		// If inNormal would affect movement less than inTriangleNormal use inNormal
+		// This is done since it is really hard to make a distinction between sliding over a horizontal triangulated grid and hitting an edge (in this case you want to use the triangle normal)
+		// and sliding over a triangulated grid and grazing a vertical triangle with an inactive edge (in this case using the triangle normal will cause the object to bounce back so we want to use the calculated normal).
+		// To solve this we take a movement hint to give an indication of what direction our object is moving. If the edge normal results in less motion difference than the triangle normal we use the edge normal.
+		float normal_length = inNormal.Length();
+		float triangle_normal_length = inTriangleNormal.Length();
+		if (inMovementDirection.Dot(inNormal) * triangle_normal_length < inMovementDirection.Dot(inTriangleNormal) * normal_length)
+			return inNormal;
+
+		// Check: None of the edges are active, we need to use the triangle normal
+		if (inActiveEdges == 0)
+			return inTriangleNormal;
+
+		// Some edges are active.
+		// If normal is parallel to the triangle normal we don't need to check the active edges.
+		if (inTriangleNormal.Dot(inNormal) > 0.999848f * normal_length * triangle_normal_length) // cos(1 degree)
+			return inNormal;
+
+		const float cEpsilon = 1.0e-4f;
+		const float cOneMinusEpsilon = 1.0f - cEpsilon;
+
+		uint colliding_edge;
+
+		// Test where the contact point is in the triangle
+		float u, v, w;
+		ClosestPoint::GetBaryCentricCoordinates(inV0 - inPoint, inV1 - inPoint, inV2 - inPoint, u, v, w);
+		if (u > cOneMinusEpsilon)
+		{
+			// Colliding with v0, edge 0 or 2 needs to be active
+			colliding_edge = 0b101;
+		}
+		else if (v > cOneMinusEpsilon)
+		{
+			// Colliding with v1, edge 0 or 1 needs to be active
+			colliding_edge = 0b011;
+		}
+		else if (w > cOneMinusEpsilon)
+		{
+			// Colliding with v2, edge 1 or 2 needs to be active
+			colliding_edge = 0b110;
+		}
+		else if (u < cEpsilon)
+		{
+			// Colliding with edge v1, v2, edge 1 needs to be active
+			colliding_edge = 0b010;
+		}
+		else if (v < cEpsilon)
+		{
+			// Colliding with edge v0, v2, edge 2 needs to be active
+			colliding_edge = 0b100;
+		}
+		else if (w < cEpsilon)
+		{
+			// Colliding with edge v0, v1, edge 0 needs to be active
+			colliding_edge = 0b001;
+		}
+		else
+		{
+			// Interior hit
+			return inTriangleNormal;
+		}
+
+		// If this edge is active, use the provided normal instead of the triangle normal
+		return (inActiveEdges & colliding_edge) != 0? inNormal : inTriangleNormal;
+	}
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BackFaceMode.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BackFaceMode.h
new file mode 100644
index 0000000000..441dcd89a5
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BackFaceMode.h
@@ -0,0 +1,16 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// How collision detection functions will treat back facing triangles
+enum class EBackFaceMode : uint8
+{
+	IgnoreBackFaces,									///< Ignore collision with back facing surfaces/triangles
+	CollideWithBackFaces,								///< Collide with back facing surfaces/triangles
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhase.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhase.cpp
new file mode 100644
index 0000000000..1317d13393
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhase.cpp
@@ -0,0 +1,16 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhase.h>
+
+JPH_NAMESPACE_BEGIN
+
+void BroadPhase::Init(BodyManager *inBodyManager, const BroadPhaseLayerInterface &inLayerInterface)
+{
+	mBodyManager = inBodyManager;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhase.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhase.h
new file mode 100644
index 0000000000..8b6506e901
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhase.h
@@ -0,0 +1,112 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h>
+
+JPH_NAMESPACE_BEGIN
+
+// Shorthand function to ifdef out code if broadphase stats tracking is off
+#ifdef JPH_TRACK_BROADPHASE_STATS
+	#define JPH_IF_TRACK_BROADPHASE_STATS(...) __VA_ARGS__
+#else
+	#define JPH_IF_TRACK_BROADPHASE_STATS(...)
+#endif // JPH_TRACK_BROADPHASE_STATS
+
+class BodyManager;
+struct BodyPair;
+
+using BodyPairCollector = CollisionCollector<BodyPair, CollisionCollectorTraitsCollideShape>;
+
+/// Used to do coarse collision detection operations to quickly prune out bodies that will not collide.
+class JPH_EXPORT BroadPhase : public BroadPhaseQuery
+{
+public:
+	/// Initialize the broadphase.
+	/// @param inBodyManager The body manager singleton
+	/// @param inLayerInterface Interface that maps object layers to broadphase layers.
+	/// Note that the broadphase takes a pointer to the data inside inObjectToBroadPhaseLayer so this object should remain static.
+	virtual void		Init(BodyManager *inBodyManager, const BroadPhaseLayerInterface &inLayerInterface);
+
+	/// Should be called after many objects have been inserted to make the broadphase more efficient, usually done on startup only
+	virtual void		Optimize()															{ /* Optionally overridden by implementation */ }
+
+	/// Must be called just before updating the broadphase when none of the body mutexes are locked
+	virtual void		FrameSync()															{ /* Optionally overridden by implementation */ }
+
+	/// Must be called before UpdatePrepare to prevent modifications from being made to the tree
+	virtual void		LockModifications()													{ /* Optionally overridden by implementation */ }
+
+	/// Context used during broadphase update
+	struct UpdateState { void *mData[4]; };
+
+	/// Update the broadphase, needs to be called frequently to update the internal state when bodies have been modified.
+	/// The UpdatePrepare() function can run in a background thread without influencing the broadphase
+	virtual	UpdateState	UpdatePrepare()														{ return UpdateState(); }
+
+	/// Finalizing the update will quickly apply the changes
+	virtual void		UpdateFinalize([[maybe_unused]] const UpdateState &inUpdateState)	{ /* Optionally overridden by implementation */ }
+
+	/// Must be called after UpdateFinalize to allow modifications to the broadphase
+	virtual void		UnlockModifications()												{ /* Optionally overridden by implementation */ }
+
+	/// Handle used during adding bodies to the broadphase
+	using AddState = void *;
+
+	/// Prepare adding inNumber bodies at ioBodies to the broadphase, returns a handle that should be used in AddBodiesFinalize/Abort.
+	/// This can be done on a background thread without influencing the broadphase.
+	/// ioBodies may be shuffled around by this function and should be kept that way until AddBodiesFinalize/Abort is called.
+	virtual AddState	AddBodiesPrepare([[maybe_unused]] BodyID *ioBodies, [[maybe_unused]] int inNumber) { return nullptr; } // By default the broadphase doesn't support this
+
+	/// Finalize adding bodies to the broadphase, supply the return value of AddBodiesPrepare in inAddState.
+	/// Please ensure that the ioBodies array passed to AddBodiesPrepare is unmodified and passed again to this function.
+	virtual void		AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState) = 0;
+
+	/// Abort adding bodies to the broadphase, supply the return value of AddBodiesPrepare in inAddState.
+	/// This can be done on a background thread without influencing the broadphase.
+	/// Please ensure that the ioBodies array passed to AddBodiesPrepare is unmodified and passed again to this function.
+	virtual void		AddBodiesAbort([[maybe_unused]] BodyID *ioBodies, [[maybe_unused]] int inNumber, [[maybe_unused]] AddState inAddState)	{ /* By default nothing needs to be done */ }
+
+	/// Remove inNumber bodies in ioBodies from the broadphase.
+	/// ioBodies may be shuffled around by this function.
+	virtual void		RemoveBodies(BodyID *ioBodies, int inNumber) = 0;
+
+	/// Call whenever the aabb of a body changes (can change order of ioBodies array)
+	/// inTakeLock should be false if we're between LockModifications/UnlockModificiations in which case care needs to be taken to not call this between UpdatePrepare/UpdateFinalize
+	virtual void		NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock = true) = 0;
+
+	/// Call whenever the layer (and optionally the aabb as well) of a body changes (can change order of ioBodies array)
+	virtual void		NotifyBodiesLayerChanged(BodyID *ioBodies, int inNumber) = 0;
+
+	/// Find all colliding pairs between dynamic bodies
+	/// Note that this function is very specifically tailored for the PhysicsSystem::Update function, hence it is not part of the BroadPhaseQuery interface.
+	/// One of the assumptions it can make is that no locking is needed during the query as it will only be called during a very particular part of the update.
+	/// @param ioActiveBodies is a list of bodies for which we need to find colliding pairs (this function can change the order of the ioActiveBodies array). This can be a subset of the set of active bodies in the system.
+	/// @param inNumActiveBodies is the size of the ioActiveBodies array.
+	/// @param inSpeculativeContactDistance Distance at which speculative contact points will be created.
+	/// @param inObjectVsBroadPhaseLayerFilter is the filter that determines if an object can collide with a broadphase layer.
+	/// @param inObjectLayerPairFilter is the filter that determines if two objects can collide.
+	/// @param ioPairCollector receives callbacks for every body pair found.
+	virtual void		FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const = 0;
+
+	/// Same as BroadPhaseQuery::CastAABox but can be implemented in a way to take no broad phase locks.
+	virtual void		CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const = 0;
+
+	/// Get the bounding box of all objects in the broadphase
+	virtual AABox		GetBounds() const = 0;
+
+#ifdef JPH_TRACK_BROADPHASE_STATS
+	/// Trace the collected broadphase stats in CSV form.
+	/// This report can be used to judge and tweak the efficiency of the broadphase.
+	virtual void		ReportStats()														{ /* Can be implemented by derived classes */ }
+#endif // JPH_TRACK_BROADPHASE_STATS
+
+protected:
+	/// Link to the body manager that manages the bodies in this broadphase
+	BodyManager *		mBodyManager = nullptr;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.cpp
new file mode 100644
index 0000000000..fc2332182d
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.cpp
@@ -0,0 +1,313 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/AABoxCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Body/BodyManager.h>
+#include <Jolt/Physics/Body/BodyPair.h>
+#include <Jolt/Geometry/RayAABox.h>
+#include <Jolt/Geometry/OrientedBox.h>
+#include <Jolt/Core/QuickSort.h>
+
+JPH_NAMESPACE_BEGIN
+
+void BroadPhaseBruteForce::AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState)
+{
+	lock_guard lock(mMutex);
+
+	BodyVector &bodies = mBodyManager->GetBodies();
+
+	// Allocate space
+	uint32 idx = (uint32)mBodyIDs.size();
+	mBodyIDs.resize(idx + inNumber);
+
+	// Add bodies
+	for (const BodyID *b = ioBodies, *b_end = ioBodies + inNumber; b < b_end; ++b)
+	{
+		Body &body = *bodies[b->GetIndex()];
+
+		// Validate that body ID is consistent with array index
+		JPH_ASSERT(body.GetID() == *b);
+		JPH_ASSERT(!body.IsInBroadPhase());
+
+		// Add it to the list
+		mBodyIDs[idx] = body.GetID();
+		++idx;
+
+		// Indicate body is in the broadphase
+		body.SetInBroadPhaseInternal(true);
+	}
+
+	// Resort
+	QuickSort(mBodyIDs.begin(), mBodyIDs.end());
+}
+
+void BroadPhaseBruteForce::RemoveBodies(BodyID *ioBodies, int inNumber)
+{
+	lock_guard lock(mMutex);
+
+	BodyVector &bodies = mBodyManager->GetBodies();
+
+	JPH_ASSERT((int)mBodyIDs.size() >= inNumber);
+
+	// Remove bodies
+	for (const BodyID *b = ioBodies, *b_end = ioBodies + inNumber; b < b_end; ++b)
+	{
+		Body &body = *bodies[b->GetIndex()];
+
+		// Validate that body ID is consistent with array index
+		JPH_ASSERT(body.GetID() == *b);
+		JPH_ASSERT(body.IsInBroadPhase());
+
+		// Find body id
+		Array<BodyID>::const_iterator it = std::lower_bound(mBodyIDs.begin(), mBodyIDs.end(), body.GetID());
+		JPH_ASSERT(it != mBodyIDs.end());
+
+		// Remove element
+		mBodyIDs.erase(it);
+
+		// Indicate body is no longer in the broadphase
+		body.SetInBroadPhaseInternal(false);
+	}
+}
+
+void BroadPhaseBruteForce::NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock)
+{
+	// Do nothing, we directly reference the body
+}
+
+void BroadPhaseBruteForce::NotifyBodiesLayerChanged(BodyID * ioBodies, int inNumber)
+{
+	// Do nothing, we directly reference the body
+}
+
+void BroadPhaseBruteForce::CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	shared_lock lock(mMutex);
+
+	// Load ray
+	Vec3 origin(inRay.mOrigin);
+	RayInvDirection inv_direction(inRay.mDirection);
+
+	// For all bodies
+	float early_out_fraction = ioCollector.GetEarlyOutFraction();
+	for (BodyID b : mBodyIDs)
+	{
+		const Body &body = mBodyManager->GetBody(b);
+
+		// Test layer
+		if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer()))
+		{
+			// Test intersection with ray
+			const AABox &bounds = body.GetWorldSpaceBounds();
+			float fraction = RayAABox(origin, inv_direction, bounds.mMin, bounds.mMax);
+			if (fraction < early_out_fraction)
+			{
+				// Store hit
+				BroadPhaseCastResult result { b, fraction };
+				ioCollector.AddHit(result);
+				if (ioCollector.ShouldEarlyOut())
+					break;
+				early_out_fraction = ioCollector.GetEarlyOutFraction();
+			}
+		}
+	}
+}
+
+void BroadPhaseBruteForce::CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	shared_lock lock(mMutex);
+
+	// For all bodies
+	for (BodyID b : mBodyIDs)
+	{
+		const Body &body = mBodyManager->GetBody(b);
+
+		// Test layer
+		if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer()))
+		{
+			// Test intersection with box
+			const AABox &bounds = body.GetWorldSpaceBounds();
+			if (bounds.Overlaps(inBox))
+			{
+				// Store hit
+				ioCollector.AddHit(b);
+				if (ioCollector.ShouldEarlyOut())
+					break;
+			}
+		}
+	}
+}
+
+void BroadPhaseBruteForce::CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	shared_lock lock(mMutex);
+
+	float radius_sq = Square(inRadius);
+
+	// For all bodies
+	for (BodyID b : mBodyIDs)
+	{
+		const Body &body = mBodyManager->GetBody(b);
+
+		// Test layer
+		if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer()))
+		{
+			// Test intersection with box
+			const AABox &bounds = body.GetWorldSpaceBounds();
+			if (bounds.GetSqDistanceTo(inCenter) <= radius_sq)
+			{
+				// Store hit
+				ioCollector.AddHit(b);
+				if (ioCollector.ShouldEarlyOut())
+					break;
+			}
+		}
+	}
+}
+
+void BroadPhaseBruteForce::CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	shared_lock lock(mMutex);
+
+	// For all bodies
+	for (BodyID b : mBodyIDs)
+	{
+		const Body &body = mBodyManager->GetBody(b);
+
+		// Test layer
+		if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer()))
+		{
+			// Test intersection with box
+			const AABox &bounds = body.GetWorldSpaceBounds();
+			if (bounds.Contains(inPoint))
+			{
+				// Store hit
+				ioCollector.AddHit(b);
+				if (ioCollector.ShouldEarlyOut())
+					break;
+			}
+		}
+	}
+}
+
+void BroadPhaseBruteForce::CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	shared_lock lock(mMutex);
+
+	// For all bodies
+	for (BodyID b : mBodyIDs)
+	{
+		const Body &body = mBodyManager->GetBody(b);
+
+		// Test layer
+		if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer()))
+		{
+			// Test intersection with box
+			const AABox &bounds = body.GetWorldSpaceBounds();
+			if (inBox.Overlaps(bounds))
+			{
+				// Store hit
+				ioCollector.AddHit(b);
+				if (ioCollector.ShouldEarlyOut())
+					break;
+			}
+		}
+	}
+}
+
+void BroadPhaseBruteForce::CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	shared_lock lock(mMutex);
+
+	// Load box
+	Vec3 origin(inBox.mBox.GetCenter());
+	Vec3 extent(inBox.mBox.GetExtent());
+	RayInvDirection inv_direction(inBox.mDirection);
+
+	// For all bodies
+	float early_out_fraction = ioCollector.GetPositiveEarlyOutFraction();
+	for (BodyID b : mBodyIDs)
+	{
+		const Body &body = mBodyManager->GetBody(b);
+
+		// Test layer
+		if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer()))
+		{
+			// Test intersection with ray
+			const AABox &bounds = body.GetWorldSpaceBounds();
+			float fraction = RayAABox(origin, inv_direction, bounds.mMin - extent, bounds.mMax + extent);
+			if (fraction < early_out_fraction)
+			{
+				// Store hit
+				BroadPhaseCastResult result { b, fraction };
+				ioCollector.AddHit(result);
+				if (ioCollector.ShouldEarlyOut())
+					break;
+				early_out_fraction = ioCollector.GetPositiveEarlyOutFraction();
+			}
+		}
+	}
+}
+
+void BroadPhaseBruteForce::CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	CastAABoxNoLock(inBox, ioCollector, inBroadPhaseLayerFilter, inObjectLayerFilter);
+}
+
+void BroadPhaseBruteForce::FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const
+{
+	shared_lock lock(mMutex);
+
+	// Loop through all active bodies
+	size_t num_bodies = mBodyIDs.size();
+	for (int b1 = 0; b1 < inNumActiveBodies; ++b1)
+	{
+		BodyID b1_id = ioActiveBodies[b1];
+		const Body &body1 = mBodyManager->GetBody(b1_id);
+		const ObjectLayer layer1 = body1.GetObjectLayer();
+
+		// Expand the bounding box by the speculative contact distance
+		AABox bounds1 = body1.GetWorldSpaceBounds();
+		bounds1.ExpandBy(Vec3::sReplicate(inSpeculativeContactDistance));
+
+		// For all other bodies
+		for (size_t b2 = 0; b2 < num_bodies; ++b2)
+		{
+			// Check if bodies can collide
+			BodyID b2_id = mBodyIDs[b2];
+			const Body &body2 = mBodyManager->GetBody(b2_id);
+			if (!Body::sFindCollidingPairsCanCollide(body1, body2))
+				continue;
+
+			// Check if layers can collide
+			const ObjectLayer layer2 = body2.GetObjectLayer();
+			if (!inObjectLayerPairFilter.ShouldCollide(layer1, layer2))
+				continue;
+
+			// Check if bounds overlap
+			const AABox &bounds2 = body2.GetWorldSpaceBounds();
+			if (!bounds1.Overlaps(bounds2))
+				continue;
+
+			// Store overlapping pair
+			ioPairCollector.AddHit({ b1_id, b2_id });
+		}
+	}
+}
+
+AABox BroadPhaseBruteForce::GetBounds() const
+{
+	shared_lock lock(mMutex);
+
+	AABox bounds;
+	for (BodyID b : mBodyIDs)
+		bounds.Encapsulate(mBodyManager->GetBody(b).GetWorldSpaceBounds());
+	return bounds;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.h
new file mode 100644
index 0000000000..c3e20f5c8b
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.h
@@ -0,0 +1,38 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhase.h>
+#include <Jolt/Core/Mutex.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Test BroadPhase implementation that does not do anything to speed up the operations. Can be used as a reference implementation.
+class JPH_EXPORT BroadPhaseBruteForce final : public BroadPhase
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	// Implementing interface of BroadPhase (see BroadPhase for documentation)
+	virtual void		AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState) override;
+	virtual void		RemoveBodies(BodyID *ioBodies, int inNumber) override;
+	virtual void		NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock) override;
+	virtual void		NotifyBodiesLayerChanged(BodyID *ioBodies, int inNumber) override;
+	virtual void		CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void		CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void		CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void		CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void		CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void		CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void		CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void		FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const override;
+	virtual AABox		GetBounds() const override;
+
+private:
+	Array<BodyID>		mBodyIDs;
+	mutable SharedMutex	mMutex;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h
new file mode 100644
index 0000000000..bc591e733b
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h
@@ -0,0 +1,148 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/NonCopyable.h>
+#include <Jolt/Physics/Collision/ObjectLayer.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// An object layer can be mapped to a broadphase layer. Objects with the same broadphase layer will end up in the same sub structure (usually a tree) of the broadphase.
+/// When there are many layers, this reduces the total amount of sub structures the broad phase needs to manage. Usually you want objects that don't collide with each other
+/// in different broad phase layers, but there could be exceptions if objects layers only contain a minor amount of objects so it is not beneficial to give each layer its
+/// own sub structure in the broadphase.
+/// Note: This class requires explicit casting from and to Type to avoid confusion with ObjectLayer
+class BroadPhaseLayer
+{
+public:
+	using Type = uint8;
+
+	JPH_INLINE						BroadPhaseLayer() = default;
+	JPH_INLINE explicit constexpr	BroadPhaseLayer(Type inValue) : mValue(inValue) { }
+	JPH_INLINE constexpr			BroadPhaseLayer(const BroadPhaseLayer &) = default;
+	JPH_INLINE BroadPhaseLayer &	operator = (const BroadPhaseLayer &) = default;
+
+	JPH_INLINE constexpr bool		operator == (const BroadPhaseLayer &inRHS) const
+	{
+		return mValue == inRHS.mValue;
+	}
+
+	JPH_INLINE constexpr bool		operator != (const BroadPhaseLayer &inRHS) const
+	{
+		return mValue != inRHS.mValue;
+	}
+
+	JPH_INLINE constexpr bool		operator < (const BroadPhaseLayer &inRHS) const
+	{
+		return mValue < inRHS.mValue;
+	}
+
+	JPH_INLINE explicit constexpr	operator Type() const
+	{
+		return mValue;
+	}
+
+	JPH_INLINE Type					GetValue() const
+	{
+		return mValue;
+	}
+
+private:
+	Type							mValue;
+};
+
+/// Constant value used to indicate an invalid broad phase layer
+static constexpr BroadPhaseLayer cBroadPhaseLayerInvalid(0xff);
+
+/// Interface that the application should implement to allow mapping object layers to broadphase layers
+class JPH_EXPORT BroadPhaseLayerInterface : public NonCopyable
+{
+public:
+	/// Destructor
+	virtual							~BroadPhaseLayerInterface() = default;
+
+	/// Return the number of broadphase layers there are
+	virtual uint					GetNumBroadPhaseLayers() const = 0;
+
+	/// Convert an object layer to the corresponding broadphase layer
+	virtual BroadPhaseLayer			GetBroadPhaseLayer(ObjectLayer inLayer) const = 0;
+
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+	/// Get the user readable name of a broadphase layer (debugging purposes)
+	virtual const char *			GetBroadPhaseLayerName(BroadPhaseLayer inLayer) const = 0;
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+};
+
+/// Class to test if an object can collide with a broadphase layer. Used while finding collision pairs.
+class JPH_EXPORT ObjectVsBroadPhaseLayerFilter : public NonCopyable
+{
+public:
+	/// Destructor
+	virtual							~ObjectVsBroadPhaseLayerFilter() = default;
+
+	/// Returns true if an object layer should collide with a broadphase layer
+	virtual bool					ShouldCollide([[maybe_unused]] ObjectLayer inLayer1, [[maybe_unused]] BroadPhaseLayer inLayer2) const
+	{
+		return true;
+	}
+};
+
+/// Filter class for broadphase layers
+class JPH_EXPORT BroadPhaseLayerFilter : public NonCopyable
+{
+public:
+	/// Destructor
+	virtual							~BroadPhaseLayerFilter() = default;
+
+	/// Function to filter out broadphase layers when doing collision query test (return true to allow testing against objects with this layer)
+	virtual bool					ShouldCollide([[maybe_unused]] BroadPhaseLayer inLayer) const
+	{
+		return true;
+	}
+};
+
+/// Default filter class that uses the pair filter in combination with a specified layer to filter layers
+class JPH_EXPORT DefaultBroadPhaseLayerFilter : public BroadPhaseLayerFilter
+{
+public:
+	/// Constructor
+									DefaultBroadPhaseLayerFilter(const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, ObjectLayer inLayer) :
+		mObjectVsBroadPhaseLayerFilter(inObjectVsBroadPhaseLayerFilter),
+		mLayer(inLayer)
+	{
+	}
+
+	// See BroadPhaseLayerFilter::ShouldCollide
+	virtual bool					ShouldCollide(BroadPhaseLayer inLayer) const override
+	{
+		return mObjectVsBroadPhaseLayerFilter.ShouldCollide(mLayer, inLayer);
+	}
+
+private:
+	const ObjectVsBroadPhaseLayerFilter &mObjectVsBroadPhaseLayerFilter;
+	ObjectLayer						mLayer;
+};
+
+/// Allows objects from a specific broad phase layer only
+class JPH_EXPORT SpecifiedBroadPhaseLayerFilter : public BroadPhaseLayerFilter
+{
+public:
+	/// Constructor
+	explicit						SpecifiedBroadPhaseLayerFilter(BroadPhaseLayer inLayer) :
+		mLayer(inLayer)
+	{
+	}
+
+	// See BroadPhaseLayerFilter::ShouldCollide
+	virtual bool					ShouldCollide(BroadPhaseLayer inLayer) const override
+	{
+		return mLayer == inLayer;
+	}
+
+private:
+	BroadPhaseLayer					mLayer;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceMask.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceMask.h
new file mode 100644
index 0000000000..15a894bb12
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceMask.h
@@ -0,0 +1,92 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h>
+#include <Jolt/Physics/Collision/ObjectLayerPairFilterMask.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// BroadPhaseLayerInterface implementation.
+/// This defines a mapping between object and broadphase layers.
+/// This implementation works together with ObjectLayerPairFilterMask and ObjectVsBroadPhaseLayerFilterMask.
+/// A broadphase layer is suitable for an object if its group & inGroupsToInclude is not zero and its group & inGroupsToExclude is zero.
+/// The broadphase layers are iterated from lowest to highest value and the first one that matches is taken. If none match then it takes the last layer.
+class BroadPhaseLayerInterfaceMask : public BroadPhaseLayerInterface
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	explicit				BroadPhaseLayerInterfaceMask(uint inNumBroadPhaseLayers)
+	{
+		JPH_ASSERT(inNumBroadPhaseLayers > 0);
+		mMapping.resize(inNumBroadPhaseLayers);
+
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+		mBroadPhaseLayerNames.resize(inNumBroadPhaseLayers, "Undefined");
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+	}
+
+	// Configures a broadphase layer.
+	void					ConfigureLayer(BroadPhaseLayer inBroadPhaseLayer, uint32 inGroupsToInclude, uint32 inGroupsToExclude)
+	{
+		JPH_ASSERT((BroadPhaseLayer::Type)inBroadPhaseLayer < (uint)mMapping.size());
+		Mapping &m = mMapping[(BroadPhaseLayer::Type)inBroadPhaseLayer];
+		m.mGroupsToInclude = inGroupsToInclude;
+		m.mGroupsToExclude = inGroupsToExclude;
+	}
+
+	virtual uint			GetNumBroadPhaseLayers() const override
+	{
+		return (uint)mMapping.size();
+	}
+
+	virtual BroadPhaseLayer	GetBroadPhaseLayer(ObjectLayer inLayer) const override
+	{
+		// Try to find the first broadphase layer that matches
+		uint32 group = ObjectLayerPairFilterMask::sGetGroup(inLayer);
+		for (const Mapping &m : mMapping)
+			if ((group & m.mGroupsToInclude) != 0 && (group & m.mGroupsToExclude) == 0)
+				return BroadPhaseLayer(BroadPhaseLayer::Type(&m - mMapping.data()));
+
+		// Fall back to the last broadphase layer
+		return BroadPhaseLayer(BroadPhaseLayer::Type(mMapping.size() - 1));
+	}
+
+	/// Returns true if an object layer should collide with a broadphase layer, this function is being called from ObjectVsBroadPhaseLayerFilterMask
+	inline bool				ShouldCollide(ObjectLayer inLayer1, BroadPhaseLayer inLayer2) const
+	{
+		uint32 mask = ObjectLayerPairFilterMask::sGetMask(inLayer1);
+		const Mapping &m = mMapping[(BroadPhaseLayer::Type)inLayer2];
+		return &m == &mMapping.back() // Last layer may collide with anything
+			|| (m.mGroupsToInclude & mask) != 0; // Mask allows it to collide with objects that could reside in this layer
+	}
+
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+	void					SetBroadPhaseLayerName(BroadPhaseLayer inLayer, const char *inName)
+	{
+		mBroadPhaseLayerNames[(BroadPhaseLayer::Type)inLayer] = inName;
+	}
+
+	virtual const char *	GetBroadPhaseLayerName(BroadPhaseLayer inLayer) const override
+	{
+		return mBroadPhaseLayerNames[(BroadPhaseLayer::Type)inLayer];
+	}
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+
+private:
+	struct Mapping
+	{
+		uint32				mGroupsToInclude = 0;
+		uint32				mGroupsToExclude = ~uint32(0);
+	};
+	Array<Mapping>			mMapping;
+
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+	Array<const char *>		mBroadPhaseLayerNames;
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceTable.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceTable.h
new file mode 100644
index 0000000000..e777a08589
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceTable.h
@@ -0,0 +1,64 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// BroadPhaseLayerInterface implementation.
+/// This defines a mapping between object and broadphase layers.
+/// This implementation uses a simple table
+class BroadPhaseLayerInterfaceTable : public BroadPhaseLayerInterface
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+							BroadPhaseLayerInterfaceTable(uint inNumObjectLayers, uint inNumBroadPhaseLayers) :
+		mNumBroadPhaseLayers(inNumBroadPhaseLayers)
+	{
+		mObjectToBroadPhase.resize(inNumObjectLayers, BroadPhaseLayer(0));
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+		mBroadPhaseLayerNames.resize(inNumBroadPhaseLayers, "Undefined");
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+	}
+
+	void					MapObjectToBroadPhaseLayer(ObjectLayer inObjectLayer, BroadPhaseLayer inBroadPhaseLayer)
+	{
+		JPH_ASSERT((BroadPhaseLayer::Type)inBroadPhaseLayer < mNumBroadPhaseLayers);
+		mObjectToBroadPhase[inObjectLayer] = inBroadPhaseLayer;
+	}
+
+	virtual uint			GetNumBroadPhaseLayers() const override
+	{
+		return mNumBroadPhaseLayers;
+	}
+
+	virtual BroadPhaseLayer	GetBroadPhaseLayer(ObjectLayer inLayer) const override
+	{
+		return mObjectToBroadPhase[inLayer];
+	}
+
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+	void					SetBroadPhaseLayerName(BroadPhaseLayer inLayer, const char *inName)
+	{
+		mBroadPhaseLayerNames[(BroadPhaseLayer::Type)inLayer] = inName;
+	}
+
+	virtual const char *	GetBroadPhaseLayerName(BroadPhaseLayer inLayer) const override
+	{
+		return mBroadPhaseLayerNames[(BroadPhaseLayer::Type)inLayer];
+	}
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+
+private:
+	uint					mNumBroadPhaseLayers;
+	Array<BroadPhaseLayer>	mObjectToBroadPhase;
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+	Array<const char *>		mBroadPhaseLayerNames;
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.cpp
new file mode 100644
index 0000000000..717244c023
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.cpp
@@ -0,0 +1,609 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/AABoxCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Core/QuickSort.h>
+
+JPH_NAMESPACE_BEGIN
+
+BroadPhaseQuadTree::~BroadPhaseQuadTree()
+{
+	delete [] mLayers;
+}
+
+void BroadPhaseQuadTree::Init(BodyManager *inBodyManager, const BroadPhaseLayerInterface &inLayerInterface)
+{
+	BroadPhase::Init(inBodyManager, inLayerInterface);
+
+	// Store input parameters
+	mBroadPhaseLayerInterface = &inLayerInterface;
+	mNumLayers = inLayerInterface.GetNumBroadPhaseLayers();
+	JPH_ASSERT(mNumLayers < (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid);
+
+#ifdef JPH_ENABLE_ASSERTS
+	// Store lock context
+	mLockContext = inBodyManager;
+#endif // JPH_ENABLE_ASSERTS
+
+	// Store max bodies
+	mMaxBodies = inBodyManager->GetMaxBodies();
+
+	// Initialize tracking data
+	mTracking.resize(mMaxBodies);
+
+	// Init allocator
+	// Estimate the amount of nodes we're going to need
+	uint32 num_leaves = (uint32)(mMaxBodies + 1) / 2; // Assume 50% fill
+	uint32 num_leaves_plus_internal_nodes = num_leaves + (num_leaves + 2) / 3; // = Sum(num_leaves * 4^-i) with i = [0, Inf].
+	mAllocator.Init(2 * num_leaves_plus_internal_nodes, 256); // We use double the amount of nodes while rebuilding the tree during Update()
+
+	// Init sub trees
+	mLayers = new QuadTree [mNumLayers];
+	for (uint l = 0; l < mNumLayers; ++l)
+	{
+		mLayers[l].Init(mAllocator);
+
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+		// Set the name of the layer
+		mLayers[l].SetName(inLayerInterface.GetBroadPhaseLayerName(BroadPhaseLayer(BroadPhaseLayer::Type(l))));
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+	}
+}
+
+void BroadPhaseQuadTree::FrameSync()
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Take a unique lock on the old query lock so that we know no one is using the old nodes anymore.
+	// Note that nothing should be locked at this point to avoid risking a lock inversion deadlock.
+	// Note that in other places where we lock this mutex we don't use SharedLock to detect lock inversions. As long as
+	// nothing else is locked this is safe. This is why BroadPhaseQuery should be the highest priority lock.
+	UniqueLock root_lock(mQueryLocks[mQueryLockIdx ^ 1] JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseQuery));
+
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+		mLayers[l].DiscardOldTree();
+}
+
+void BroadPhaseQuadTree::Optimize()
+{
+	JPH_PROFILE_FUNCTION();
+
+	FrameSync();
+
+	LockModifications();
+
+	for (uint l = 0; l < mNumLayers; ++l)
+	{
+		QuadTree &tree = mLayers[l];
+		if (tree.HasBodies())
+		{
+			QuadTree::UpdateState update_state;
+			tree.UpdatePrepare(mBodyManager->GetBodies(), mTracking, update_state, true);
+			tree.UpdateFinalize(mBodyManager->GetBodies(), mTracking, update_state);
+		}
+	}
+
+	UnlockModifications();
+
+	mNextLayerToUpdate = 0;
+}
+
+void BroadPhaseQuadTree::LockModifications()
+{
+	// From this point on we prevent modifications to the tree
+	PhysicsLock::sLock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate));
+}
+
+BroadPhase::UpdateState BroadPhaseQuadTree::UpdatePrepare()
+{
+	// LockModifications should have been called
+	JPH_ASSERT(mUpdateMutex.is_locked());
+
+	// Create update state
+	UpdateState update_state;
+	UpdateStateImpl *update_state_impl = reinterpret_cast<UpdateStateImpl *>(&update_state);
+
+	// Loop until we've seen all layers
+	for (uint iteration = 0; iteration < mNumLayers; ++iteration)
+	{
+		// Get the layer
+		QuadTree &tree = mLayers[mNextLayerToUpdate];
+		mNextLayerToUpdate = (mNextLayerToUpdate + 1) % mNumLayers;
+
+		// If it is dirty we update this one
+		if (tree.HasBodies() && tree.IsDirty() && tree.CanBeUpdated())
+		{
+			update_state_impl->mTree = &tree;
+			tree.UpdatePrepare(mBodyManager->GetBodies(), mTracking, update_state_impl->mUpdateState, false);
+			return update_state;
+		}
+	}
+
+	// Nothing to update
+	update_state_impl->mTree = nullptr;
+	return update_state;
+}
+
+void BroadPhaseQuadTree::UpdateFinalize(const UpdateState &inUpdateState)
+{
+	// LockModifications should have been called
+	JPH_ASSERT(mUpdateMutex.is_locked());
+
+	// Test if a tree was updated
+	const UpdateStateImpl *update_state_impl = reinterpret_cast<const UpdateStateImpl *>(&inUpdateState);
+	if (update_state_impl->mTree == nullptr)
+		return;
+
+	update_state_impl->mTree->UpdateFinalize(mBodyManager->GetBodies(), mTracking, update_state_impl->mUpdateState);
+
+	// Make all queries from now on use the new lock
+	mQueryLockIdx = mQueryLockIdx ^ 1;
+}
+
+void BroadPhaseQuadTree::UnlockModifications()
+{
+	// From this point on we allow modifications to the tree again
+	PhysicsLock::sUnlock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate));
+}
+
+BroadPhase::AddState BroadPhaseQuadTree::AddBodiesPrepare(BodyID *ioBodies, int inNumber)
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(inNumber > 0);
+
+	const BodyVector &bodies = mBodyManager->GetBodies();
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	LayerState *state = new LayerState [mNumLayers];
+
+	// Sort bodies on layer
+	Body * const * const bodies_ptr = bodies.data(); // C pointer or else sort is incredibly slow in debug mode
+	QuickSort(ioBodies, ioBodies + inNumber, [bodies_ptr](BodyID inLHS, BodyID inRHS) { return bodies_ptr[inLHS.GetIndex()]->GetBroadPhaseLayer() < bodies_ptr[inRHS.GetIndex()]->GetBroadPhaseLayer(); });
+
+	BodyID *b_start = ioBodies, *b_end = ioBodies + inNumber;
+	while (b_start < b_end)
+	{
+		// Get broadphase layer
+		BroadPhaseLayer::Type broadphase_layer = (BroadPhaseLayer::Type)bodies[b_start->GetIndex()]->GetBroadPhaseLayer();
+		JPH_ASSERT(broadphase_layer < mNumLayers);
+
+		// Find first body with different layer
+		BodyID *b_mid = std::upper_bound(b_start, b_end, broadphase_layer, [bodies_ptr](BroadPhaseLayer::Type inLayer, BodyID inBodyID) { return inLayer < (BroadPhaseLayer::Type)bodies_ptr[inBodyID.GetIndex()]->GetBroadPhaseLayer(); });
+
+		// Keep track of state for this layer
+		LayerState &layer_state = state[broadphase_layer];
+		layer_state.mBodyStart = b_start;
+		layer_state.mBodyEnd = b_mid;
+
+		// Insert all bodies of the same layer
+		mLayers[broadphase_layer].AddBodiesPrepare(bodies, mTracking, b_start, int(b_mid - b_start), layer_state.mAddState);
+
+		// Keep track in which tree we placed the object
+		for (const BodyID *b = b_start; b < b_mid; ++b)
+		{
+			uint32 index = b->GetIndex();
+			JPH_ASSERT(bodies[index]->GetID() == *b, "Provided BodyID doesn't match BodyID in body manager");
+			JPH_ASSERT(!bodies[index]->IsInBroadPhase());
+			Tracking &t = mTracking[index];
+			JPH_ASSERT(t.mBroadPhaseLayer == (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid);
+			t.mBroadPhaseLayer = broadphase_layer;
+			JPH_ASSERT(t.mObjectLayer == cObjectLayerInvalid);
+			t.mObjectLayer = bodies[index]->GetObjectLayer();
+		}
+
+		// Repeat
+		b_start = b_mid;
+	}
+
+	return state;
+}
+
+void BroadPhaseQuadTree::AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// This cannot run concurrently with UpdatePrepare()/UpdateFinalize()
+	SharedLock lock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate));
+
+	BodyVector &bodies = mBodyManager->GetBodies();
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	LayerState *state = (LayerState *)inAddState;
+
+	for (BroadPhaseLayer::Type broadphase_layer = 0; broadphase_layer < mNumLayers; broadphase_layer++)
+	{
+		const LayerState &l = state[broadphase_layer];
+		if (l.mBodyStart != nullptr)
+		{
+			// Insert all bodies of the same layer
+			mLayers[broadphase_layer].AddBodiesFinalize(mTracking, int(l.mBodyEnd - l.mBodyStart), l.mAddState);
+
+			// Mark added to broadphase
+			for (const BodyID *b = l.mBodyStart; b < l.mBodyEnd; ++b)
+			{
+				uint32 index = b->GetIndex();
+				JPH_ASSERT(bodies[index]->GetID() == *b, "Provided BodyID doesn't match BodyID in body manager");
+				JPH_ASSERT(mTracking[index].mBroadPhaseLayer == broadphase_layer);
+				JPH_ASSERT(mTracking[index].mObjectLayer == bodies[index]->GetObjectLayer());
+				JPH_ASSERT(!bodies[index]->IsInBroadPhase());
+				bodies[index]->SetInBroadPhaseInternal(true);
+			}
+		}
+	}
+
+	delete [] state;
+}
+
+void BroadPhaseQuadTree::AddBodiesAbort(BodyID *ioBodies, int inNumber, AddState inAddState)
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_IF_ENABLE_ASSERTS(const BodyVector &bodies = mBodyManager->GetBodies();)
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	LayerState *state = (LayerState *)inAddState;
+
+	for (BroadPhaseLayer::Type broadphase_layer = 0; broadphase_layer < mNumLayers; broadphase_layer++)
+	{
+		const LayerState &l = state[broadphase_layer];
+		if (l.mBodyStart != nullptr)
+		{
+			// Insert all bodies of the same layer
+			mLayers[broadphase_layer].AddBodiesAbort(mTracking, l.mAddState);
+
+			// Reset bookkeeping
+			for (const BodyID *b = l.mBodyStart; b < l.mBodyEnd; ++b)
+			{
+				uint32 index = b->GetIndex();
+				JPH_ASSERT(bodies[index]->GetID() == *b, "Provided BodyID doesn't match BodyID in body manager");
+				JPH_ASSERT(!bodies[index]->IsInBroadPhase());
+				Tracking &t = mTracking[index];
+				JPH_ASSERT(t.mBroadPhaseLayer == broadphase_layer);
+				t.mBroadPhaseLayer = (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid;
+				t.mObjectLayer = cObjectLayerInvalid;
+			}
+		}
+	}
+
+	delete [] state;
+}
+
+void BroadPhaseQuadTree::RemoveBodies(BodyID *ioBodies, int inNumber)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// This cannot run concurrently with UpdatePrepare()/UpdateFinalize()
+	SharedLock lock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate));
+
+	JPH_ASSERT(inNumber > 0);
+
+	BodyVector &bodies = mBodyManager->GetBodies();
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	// Sort bodies on layer
+	Tracking *tracking = mTracking.data(); // C pointer or else sort is incredibly slow in debug mode
+	QuickSort(ioBodies, ioBodies + inNumber, [tracking](BodyID inLHS, BodyID inRHS) { return tracking[inLHS.GetIndex()].mBroadPhaseLayer < tracking[inRHS.GetIndex()].mBroadPhaseLayer; });
+
+	BodyID *b_start = ioBodies, *b_end = ioBodies + inNumber;
+	while (b_start < b_end)
+	{
+		// Get broad phase layer
+		BroadPhaseLayer::Type broadphase_layer = mTracking[b_start->GetIndex()].mBroadPhaseLayer;
+		JPH_ASSERT(broadphase_layer != (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid);
+
+		// Find first body with different layer
+		BodyID *b_mid = std::upper_bound(b_start, b_end, broadphase_layer, [tracking](BroadPhaseLayer::Type inLayer, BodyID inBodyID) { return inLayer < tracking[inBodyID.GetIndex()].mBroadPhaseLayer; });
+
+		// Remove all bodies of the same layer
+		mLayers[broadphase_layer].RemoveBodies(bodies, mTracking, b_start, int(b_mid - b_start));
+
+		for (const BodyID *b = b_start; b < b_mid; ++b)
+		{
+			// Reset bookkeeping
+			uint32 index = b->GetIndex();
+			Tracking &t = tracking[index];
+			t.mBroadPhaseLayer = (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid;
+			t.mObjectLayer = cObjectLayerInvalid;
+
+			// Mark removed from broadphase
+			JPH_ASSERT(bodies[index]->IsInBroadPhase());
+			bodies[index]->SetInBroadPhaseInternal(false);
+		}
+
+		// Repeat
+		b_start = b_mid;
+	}
+}
+
+void BroadPhaseQuadTree::NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock)
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(inNumber > 0);
+
+	// This cannot run concurrently with UpdatePrepare()/UpdateFinalize()
+	if (inTakeLock)
+		PhysicsLock::sLockShared(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate));
+	else
+		JPH_ASSERT(mUpdateMutex.is_locked());
+
+	const BodyVector &bodies = mBodyManager->GetBodies();
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	// Sort bodies on layer
+	const Tracking *tracking = mTracking.data(); // C pointer or else sort is incredibly slow in debug mode
+	QuickSort(ioBodies, ioBodies + inNumber, [tracking](BodyID inLHS, BodyID inRHS) { return tracking[inLHS.GetIndex()].mBroadPhaseLayer < tracking[inRHS.GetIndex()].mBroadPhaseLayer; });
+
+	BodyID *b_start = ioBodies, *b_end = ioBodies + inNumber;
+	while (b_start < b_end)
+	{
+		// Get broadphase layer
+		BroadPhaseLayer::Type broadphase_layer = tracking[b_start->GetIndex()].mBroadPhaseLayer;
+		JPH_ASSERT(broadphase_layer != (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid);
+
+		// Find first body with different layer
+		BodyID *b_mid = std::upper_bound(b_start, b_end, broadphase_layer, [tracking](BroadPhaseLayer::Type inLayer, BodyID inBodyID) { return inLayer < tracking[inBodyID.GetIndex()].mBroadPhaseLayer; });
+
+		// Nodify all bodies of the same layer changed
+		mLayers[broadphase_layer].NotifyBodiesAABBChanged(bodies, mTracking, b_start, int(b_mid - b_start));
+
+		// Repeat
+		b_start = b_mid;
+	}
+
+	if (inTakeLock)
+		PhysicsLock::sUnlockShared(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate));
+}
+
+void BroadPhaseQuadTree::NotifyBodiesLayerChanged(BodyID *ioBodies, int inNumber)
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(inNumber > 0);
+
+	// First sort the bodies that actually changed layer to beginning of the array
+	const BodyVector &bodies = mBodyManager->GetBodies();
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+	for (BodyID *body_id = ioBodies + inNumber - 1; body_id >= ioBodies; --body_id)
+	{
+		uint32 index = body_id->GetIndex();
+		JPH_ASSERT(bodies[index]->GetID() == *body_id, "Provided BodyID doesn't match BodyID in body manager");
+		const Body *body = bodies[index];
+		BroadPhaseLayer::Type broadphase_layer = (BroadPhaseLayer::Type)body->GetBroadPhaseLayer();
+		JPH_ASSERT(broadphase_layer < mNumLayers);
+		if (mTracking[index].mBroadPhaseLayer == broadphase_layer)
+		{
+			// Update tracking information
+			mTracking[index].mObjectLayer = body->GetObjectLayer();
+
+			// Move the body to the end, layer didn't change
+			std::swap(*body_id, ioBodies[inNumber - 1]);
+			--inNumber;
+		}
+	}
+
+	if (inNumber > 0)
+	{
+		// Changing layer requires us to remove from one tree and add to another, so this is equivalent to removing all bodies first and then adding them again
+		RemoveBodies(ioBodies, inNumber);
+		AddState add_state = AddBodiesPrepare(ioBodies, inNumber);
+		AddBodiesFinalize(ioBodies, inNumber, add_state);
+	}
+}
+
+void BroadPhaseQuadTree::CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	// Prevent this from running in parallel with node deletion in FrameSync(), see notes there
+	shared_lock lock(mQueryLocks[mQueryLockIdx]);
+
+	// Loop over all layers and test the ones that could hit
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+	{
+		const QuadTree &tree = mLayers[l];
+		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
+		{
+			JPH_PROFILE(tree.GetName());
+			tree.CastRay(inRay, ioCollector, inObjectLayerFilter, mTracking);
+			if (ioCollector.ShouldEarlyOut())
+				break;
+		}
+	}
+}
+
+void BroadPhaseQuadTree::CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	// Prevent this from running in parallel with node deletion in FrameSync(), see notes there
+	shared_lock lock(mQueryLocks[mQueryLockIdx]);
+
+	// Loop over all layers and test the ones that could hit
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+	{
+		const QuadTree &tree = mLayers[l];
+		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
+		{
+			JPH_PROFILE(tree.GetName());
+			tree.CollideAABox(inBox, ioCollector, inObjectLayerFilter, mTracking);
+			if (ioCollector.ShouldEarlyOut())
+				break;
+		}
+	}
+}
+
+void BroadPhaseQuadTree::CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	// Prevent this from running in parallel with node deletion in FrameSync(), see notes there
+	shared_lock lock(mQueryLocks[mQueryLockIdx]);
+
+	// Loop over all layers and test the ones that could hit
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+	{
+		const QuadTree &tree = mLayers[l];
+		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
+		{
+			JPH_PROFILE(tree.GetName());
+			tree.CollideSphere(inCenter, inRadius, ioCollector, inObjectLayerFilter, mTracking);
+			if (ioCollector.ShouldEarlyOut())
+				break;
+		}
+	}
+}
+
+void BroadPhaseQuadTree::CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	// Prevent this from running in parallel with node deletion in FrameSync(), see notes there
+	shared_lock lock(mQueryLocks[mQueryLockIdx]);
+
+	// Loop over all layers and test the ones that could hit
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+	{
+		const QuadTree &tree = mLayers[l];
+		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
+		{
+			JPH_PROFILE(tree.GetName());
+			tree.CollidePoint(inPoint, ioCollector, inObjectLayerFilter, mTracking);
+			if (ioCollector.ShouldEarlyOut())
+				break;
+		}
+	}
+}
+
+void BroadPhaseQuadTree::CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	// Prevent this from running in parallel with node deletion in FrameSync(), see notes there
+	shared_lock lock(mQueryLocks[mQueryLockIdx]);
+
+	// Loop over all layers and test the ones that could hit
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+	{
+		const QuadTree &tree = mLayers[l];
+		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
+		{
+			JPH_PROFILE(tree.GetName());
+			tree.CollideOrientedBox(inBox, ioCollector, inObjectLayerFilter, mTracking);
+			if (ioCollector.ShouldEarlyOut())
+				break;
+		}
+	}
+}
+
+void BroadPhaseQuadTree::CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	// Loop over all layers and test the ones that could hit
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+	{
+		const QuadTree &tree = mLayers[l];
+		if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l)))
+		{
+			JPH_PROFILE(tree.GetName());
+			tree.CastAABox(inBox, ioCollector, inObjectLayerFilter, mTracking);
+			if (ioCollector.ShouldEarlyOut())
+				break;
+		}
+	}
+}
+
+void BroadPhaseQuadTree::CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const
+{
+	// Prevent this from running in parallel with node deletion in FrameSync(), see notes there
+	shared_lock lock(mQueryLocks[mQueryLockIdx]);
+
+	CastAABoxNoLock(inBox, ioCollector, inBroadPhaseLayerFilter, inObjectLayerFilter);
+}
+
+void BroadPhaseQuadTree::FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	const BodyVector &bodies = mBodyManager->GetBodies();
+	JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies());
+
+	// Note that we don't take any locks at this point. We know that the tree is not going to be swapped or deleted while finding collision pairs due to the way the jobs are scheduled in the PhysicsSystem::Update.
+
+	// Sort bodies on layer
+	const Tracking *tracking = mTracking.data(); // C pointer or else sort is incredibly slow in debug mode
+	QuickSort(ioActiveBodies, ioActiveBodies + inNumActiveBodies, [tracking](BodyID inLHS, BodyID inRHS) { return tracking[inLHS.GetIndex()].mObjectLayer < tracking[inRHS.GetIndex()].mObjectLayer; });
+
+	BodyID *b_start = ioActiveBodies, *b_end = ioActiveBodies + inNumActiveBodies;
+	while (b_start < b_end)
+	{
+		// Get broadphase layer
+		ObjectLayer object_layer = tracking[b_start->GetIndex()].mObjectLayer;
+		JPH_ASSERT(object_layer != cObjectLayerInvalid);
+
+		// Find first body with different layer
+		BodyID *b_mid = std::upper_bound(b_start, b_end, object_layer, [tracking](ObjectLayer inLayer, BodyID inBodyID) { return inLayer < tracking[inBodyID.GetIndex()].mObjectLayer; });
+
+		// Loop over all layers and test the ones that could hit
+		for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+		{
+			const QuadTree &tree = mLayers[l];
+			if (tree.HasBodies() && inObjectVsBroadPhaseLayerFilter.ShouldCollide(object_layer, BroadPhaseLayer(l)))
+			{
+				JPH_PROFILE(tree.GetName());
+				tree.FindCollidingPairs(bodies, b_start, int(b_mid - b_start), inSpeculativeContactDistance, ioPairCollector, inObjectLayerPairFilter);
+			}
+		}
+
+		// Repeat
+		b_start = b_mid;
+	}
+}
+
+AABox BroadPhaseQuadTree::GetBounds() const
+{
+	// Prevent this from running in parallel with node deletion in FrameSync(), see notes there
+	shared_lock lock(mQueryLocks[mQueryLockIdx]);
+
+	AABox bounds;
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+		bounds.Encapsulate(mLayers[l].GetBounds());
+	return bounds;
+}
+
+#ifdef JPH_TRACK_BROADPHASE_STATS
+
+void BroadPhaseQuadTree::ReportStats()
+{
+	Trace("Query Type, Filter Description, Tree Name, Num Queries, Total Time (%%), Total Time Excl. Collector (%%), Nodes Visited, Bodies Visited, Hits Reported, Hits Reported vs Bodies Visited (%%), Hits Reported vs Nodes Visited");
+
+	uint64 total_ticks = 0;
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+		total_ticks += mLayers[l].GetTicks100Pct();
+
+	for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l)
+		mLayers[l].ReportStats(total_ticks);
+}
+
+#endif // JPH_TRACK_BROADPHASE_STATS
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h
new file mode 100644
index 0000000000..ae97c10f0e
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h
@@ -0,0 +1,108 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/BroadPhase/QuadTree.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhase.h>
+#include <Jolt/Physics/PhysicsLock.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Fast SIMD based quad tree BroadPhase that is multithreading aware and tries to do a minimal amount of locking.
+class JPH_EXPORT BroadPhaseQuadTree final : public BroadPhase
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Destructor
+	virtual					~BroadPhaseQuadTree() override;
+
+	// Implementing interface of BroadPhase (see BroadPhase for documentation)
+	virtual void			Init(BodyManager *inBodyManager, const BroadPhaseLayerInterface &inLayerInterface) override;
+	virtual void			Optimize() override;
+	virtual void			FrameSync() override;
+	virtual void			LockModifications() override;
+	virtual	UpdateState		UpdatePrepare() override;
+	virtual void			UpdateFinalize(const UpdateState &inUpdateState) override;
+	virtual void			UnlockModifications() override;
+	virtual AddState		AddBodiesPrepare(BodyID *ioBodies, int inNumber) override;
+	virtual void			AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState) override;
+	virtual void			AddBodiesAbort(BodyID *ioBodies, int inNumber, AddState inAddState) override;
+	virtual void			RemoveBodies(BodyID *ioBodies, int inNumber) override;
+	virtual void			NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock) override;
+	virtual void			NotifyBodiesLayerChanged(BodyID *ioBodies, int inNumber) override;
+	virtual void			CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void			CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void			CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void			CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void			CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void			CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void			CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override;
+	virtual void			FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const override;
+	virtual AABox			GetBounds() const override;
+#ifdef JPH_TRACK_BROADPHASE_STATS
+	virtual void			ReportStats() override;
+#endif // JPH_TRACK_BROADPHASE_STATS
+
+private:
+	/// Helper struct for AddBodies handle
+	struct LayerState
+	{
+		JPH_OVERRIDE_NEW_DELETE
+
+		BodyID *			mBodyStart = nullptr;
+		BodyID *			mBodyEnd;
+		QuadTree::AddState	mAddState;
+	};
+
+	using Tracking = QuadTree::Tracking;
+	using TrackingVector = QuadTree::TrackingVector;
+
+#ifdef JPH_ENABLE_ASSERTS
+	/// Context used to lock a physics lock
+	PhysicsLockContext		mLockContext = nullptr;
+#endif // JPH_ENABLE_ASSERTS
+
+	/// Max amount of bodies we support
+	size_t					mMaxBodies = 0;
+
+	/// Array that for each BodyID keeps track of where it is located in which tree
+	TrackingVector			mTracking;
+
+	/// Node allocator for all trees
+	QuadTree::Allocator		mAllocator;
+
+	/// Information about broad phase layers
+	const BroadPhaseLayerInterface *mBroadPhaseLayerInterface = nullptr;
+
+	/// One tree per object layer
+	QuadTree *				mLayers;
+	uint					mNumLayers;
+
+	/// UpdateState implementation for this tree used during UpdatePrepare/Finalize()
+	struct UpdateStateImpl
+	{
+		QuadTree *				mTree;
+		QuadTree::UpdateState	mUpdateState;
+	};
+
+	static_assert(sizeof(UpdateStateImpl) <= sizeof(UpdateState));
+	static_assert(alignof(UpdateStateImpl) <= alignof(UpdateState));
+
+	/// Mutex that prevents object modification during UpdatePrepare/Finalize()
+	SharedMutex				mUpdateMutex;
+
+	/// We double buffer all trees so that we can query while building the next one and we destroy the old tree the next physics update.
+	/// This structure ensures that we wait for queries that are still using the old tree.
+	mutable SharedMutex		mQueryLocks[2];
+
+	/// This index indicates which lock is currently active, it alternates between 0 and 1
+	atomic<uint32>			mQueryLockIdx { 0 };
+
+	/// This is the next tree to update in UpdatePrepare()
+	uint32					mNextLayerToUpdate = 0;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h
new file mode 100644
index 0000000000..10085e66fe
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h
@@ -0,0 +1,53 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h>
+#include <Jolt/Physics/Collision/ObjectLayer.h>
+#include <Jolt/Physics/Collision/CollisionCollector.h>
+#include <Jolt/Physics/Body/BodyID.h>
+#include <Jolt/Core/NonCopyable.h>
+
+JPH_NAMESPACE_BEGIN
+
+struct RayCast;
+class BroadPhaseCastResult;
+class AABox;
+class OrientedBox;
+struct AABoxCast;
+
+// Various collector configurations
+using RayCastBodyCollector = CollisionCollector<BroadPhaseCastResult, CollisionCollectorTraitsCastRay>;
+using CastShapeBodyCollector = CollisionCollector<BroadPhaseCastResult, CollisionCollectorTraitsCastShape>;
+using CollideShapeBodyCollector = CollisionCollector<BodyID, CollisionCollectorTraitsCollideShape>;
+
+/// Interface to the broadphase that can perform collision queries. These queries will only test the bounding box of the body to quickly determine a potential set of colliding bodies.
+/// The shapes of the bodies are not tested, if you want this then you should use the NarrowPhaseQuery interface.
+class JPH_EXPORT BroadPhaseQuery : public NonCopyable
+{
+public:
+	/// Virtual destructor
+	virtual				~BroadPhaseQuery() = default;
+
+	/// Cast a ray and add any hits to ioCollector
+	virtual void		CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0;
+
+	/// Get bodies intersecting with inBox and any hits to ioCollector
+	virtual void		CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0;
+
+	/// Get bodies intersecting with a sphere and any hits to ioCollector
+	virtual void		CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0;
+
+	/// Get bodies intersecting with a point and any hits to ioCollector
+	virtual void		CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0;
+
+	/// Get bodies intersecting with an oriented box and any hits to ioCollector
+	virtual void		CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0;
+
+	/// Cast a box and add any hits to ioCollector
+	virtual void		CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterMask.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterMask.h
new file mode 100644
index 0000000000..20305a5f0c
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterMask.h
@@ -0,0 +1,35 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceMask.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that determines if an object layer can collide with a broadphase layer.
+/// This implementation works together with BroadPhaseLayerInterfaceMask and ObjectLayerPairFilterMask
+class ObjectVsBroadPhaseLayerFilterMask : public ObjectVsBroadPhaseLayerFilter
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+/// Constructor
+					ObjectVsBroadPhaseLayerFilterMask(const BroadPhaseLayerInterfaceMask &inBroadPhaseLayerInterface) :
+		mBroadPhaseLayerInterface(inBroadPhaseLayerInterface)
+	{
+	}
+
+	/// Returns true if an object layer should collide with a broadphase layer
+	virtual bool	ShouldCollide(ObjectLayer inLayer1, BroadPhaseLayer inLayer2) const override
+	{
+		// Just defer to BroadPhaseLayerInterface
+		return mBroadPhaseLayerInterface.ShouldCollide(inLayer1, inLayer2);
+	}
+
+private:
+	const BroadPhaseLayerInterfaceMask &mBroadPhaseLayerInterface;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterTable.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterTable.h
new file mode 100644
index 0000000000..532ce6da0e
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterTable.h
@@ -0,0 +1,66 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that determines if an object layer can collide with a broadphase layer.
+/// This implementation uses a table and constructs itself from an ObjectLayerPairFilter and a BroadPhaseLayerInterface.
+class ObjectVsBroadPhaseLayerFilterTable : public ObjectVsBroadPhaseLayerFilter
+{
+private:
+	/// Get which bit corresponds to the pair (inLayer1, inLayer2)
+	uint					GetBit(ObjectLayer inLayer1, BroadPhaseLayer inLayer2) const
+	{
+		// Calculate at which bit the entry for this pair resides
+		return inLayer1 * mNumBroadPhaseLayers + (BroadPhaseLayer::Type)inLayer2;
+	}
+
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct the table
+	/// @param inBroadPhaseLayerInterface The broad phase layer interface that maps object layers to broad phase layers
+	/// @param inNumBroadPhaseLayers Number of broad phase layers
+	/// @param inObjectLayerPairFilter The object layer pair filter that determines which object layers can collide
+	/// @param inNumObjectLayers Number of object layers
+							ObjectVsBroadPhaseLayerFilterTable(const BroadPhaseLayerInterface &inBroadPhaseLayerInterface, uint inNumBroadPhaseLayers, const ObjectLayerPairFilter &inObjectLayerPairFilter, uint inNumObjectLayers) :
+		mNumBroadPhaseLayers(inNumBroadPhaseLayers)
+	{
+		// Resize table and set all entries to false
+		mTable.resize((inNumBroadPhaseLayers * inNumObjectLayers + 7) / 8, 0);
+
+		// Loop over all object layer pairs
+		for (ObjectLayer o1 = 0; o1 < inNumObjectLayers; ++o1)
+			for (ObjectLayer o2 = 0; o2 < inNumObjectLayers; ++o2)
+			{
+				// Get the broad phase layer for the second object layer
+				BroadPhaseLayer b2 = inBroadPhaseLayerInterface.GetBroadPhaseLayer(o2);
+				JPH_ASSERT((BroadPhaseLayer::Type)b2 < inNumBroadPhaseLayers);
+
+				// If the object layers collide then so should the object and broadphase layer
+				if (inObjectLayerPairFilter.ShouldCollide(o1, o2))
+				{
+					uint bit = GetBit(o1, b2);
+					mTable[bit >> 3] |= 1 << (bit & 0b111);
+				}
+			}
+	}
+
+	/// Returns true if an object layer should collide with a broadphase layer
+	virtual bool			ShouldCollide(ObjectLayer inLayer1, BroadPhaseLayer inLayer2) const override
+	{
+		uint bit = GetBit(inLayer1, inLayer2);
+		return (mTable[bit >> 3] & (1 << (bit & 0b111))) != 0;
+	}
+
+private:
+	uint					mNumBroadPhaseLayers;						///< The total number of broadphase layers
+	Array<uint8>			mTable;										///< The table of bits that indicates which layers collide
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.cpp
new file mode 100644
index 0000000000..cfcf8ba300
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.cpp
@@ -0,0 +1,1692 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/BroadPhase/QuadTree.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/AABoxCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/SortReverseAndStore.h>
+#include <Jolt/Physics/Body/BodyPair.h>
+#include <Jolt/Physics/PhysicsLock.h>
+#include <Jolt/Geometry/AABox4.h>
+#include <Jolt/Geometry/RayAABox.h>
+#include <Jolt/Geometry/OrientedBox.h>
+
+#ifdef JPH_DUMP_BROADPHASE_TREE
+JPH_SUPPRESS_WARNINGS_STD_BEGIN
+#include <fstream>
+JPH_SUPPRESS_WARNINGS_STD_END
+#endif // JPH_DUMP_BROADPHASE_TREE
+
+JPH_NAMESPACE_BEGIN
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////
+// QuadTree::Node
+////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+QuadTree::Node::Node(bool inIsChanged) :
+	mIsChanged(inIsChanged)
+{
+	// First reset bounds
+	Vec4 val = Vec4::sReplicate(cLargeFloat);
+	val.StoreFloat4((Float4 *)&mBoundsMinX);
+	val.StoreFloat4((Float4 *)&mBoundsMinY);
+	val.StoreFloat4((Float4 *)&mBoundsMinZ);
+	val = Vec4::sReplicate(-cLargeFloat);
+	val.StoreFloat4((Float4 *)&mBoundsMaxX);
+	val.StoreFloat4((Float4 *)&mBoundsMaxY);
+	val.StoreFloat4((Float4 *)&mBoundsMaxZ);
+
+	// Reset child node ids
+	mChildNodeID[0] = NodeID::sInvalid();
+	mChildNodeID[1] = NodeID::sInvalid();
+	mChildNodeID[2] = NodeID::sInvalid();
+	mChildNodeID[3] = NodeID::sInvalid();
+}
+
+void QuadTree::Node::GetChildBounds(int inChildIndex, AABox &outBounds) const
+{
+	// Read bounding box in order min -> max
+	outBounds.mMin = Vec3(mBoundsMinX[inChildIndex], mBoundsMinY[inChildIndex], mBoundsMinZ[inChildIndex]);
+	outBounds.mMax = Vec3(mBoundsMaxX[inChildIndex], mBoundsMaxY[inChildIndex], mBoundsMaxZ[inChildIndex]);
+}
+
+void QuadTree::Node::SetChildBounds(int inChildIndex, const AABox &inBounds)
+{
+	// Set max first (this keeps the bounding box invalid for reading threads)
+	mBoundsMaxZ[inChildIndex] = inBounds.mMax.GetZ();
+	mBoundsMaxY[inChildIndex] = inBounds.mMax.GetY();
+	mBoundsMaxX[inChildIndex] = inBounds.mMax.GetX();
+
+	// Then set min (and make box valid)
+	mBoundsMinZ[inChildIndex] = inBounds.mMin.GetZ();
+	mBoundsMinY[inChildIndex] = inBounds.mMin.GetY();
+	mBoundsMinX[inChildIndex] = inBounds.mMin.GetX(); // Min X becomes valid last
+}
+
+void QuadTree::Node::InvalidateChildBounds(int inChildIndex)
+{
+	// First we make the box invalid by setting the min to cLargeFloat
+	mBoundsMinX[inChildIndex] = cLargeFloat; // Min X becomes invalid first
+	mBoundsMinY[inChildIndex] = cLargeFloat;
+	mBoundsMinZ[inChildIndex] = cLargeFloat;
+
+	// Then we reset the max values too
+	mBoundsMaxX[inChildIndex] = -cLargeFloat;
+	mBoundsMaxY[inChildIndex] = -cLargeFloat;
+	mBoundsMaxZ[inChildIndex] = -cLargeFloat;
+}
+
+void QuadTree::Node::GetNodeBounds(AABox &outBounds) const
+{
+	// Get first child bounds
+	GetChildBounds(0, outBounds);
+
+	// Encapsulate other child bounds
+	for (int child_idx = 1; child_idx < 4; ++child_idx)
+	{
+		AABox tmp;
+		GetChildBounds(child_idx, tmp);
+		outBounds.Encapsulate(tmp);
+	}
+}
+
+bool QuadTree::Node::EncapsulateChildBounds(int inChildIndex, const AABox &inBounds)
+{
+	bool changed = AtomicMin(mBoundsMinX[inChildIndex], inBounds.mMin.GetX());
+	changed |= AtomicMin(mBoundsMinY[inChildIndex], inBounds.mMin.GetY());
+	changed |= AtomicMin(mBoundsMinZ[inChildIndex], inBounds.mMin.GetZ());
+	changed |= AtomicMax(mBoundsMaxX[inChildIndex], inBounds.mMax.GetX());
+	changed |= AtomicMax(mBoundsMaxY[inChildIndex], inBounds.mMax.GetY());
+	changed |= AtomicMax(mBoundsMaxZ[inChildIndex], inBounds.mMax.GetZ());
+	return changed;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////
+// QuadTree
+////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+const float QuadTree::cLargeFloat = 1.0e30f;
+const AABox QuadTree::cInvalidBounds(Vec3::sReplicate(cLargeFloat), Vec3::sReplicate(-cLargeFloat));
+
+void QuadTree::GetBodyLocation(const TrackingVector &inTracking, BodyID inBodyID, uint32 &outNodeIdx, uint32 &outChildIdx) const
+{
+	uint32 body_location = inTracking[inBodyID.GetIndex()].mBodyLocation;
+	JPH_ASSERT(body_location != Tracking::cInvalidBodyLocation);
+	outNodeIdx = body_location & 0x3fffffff;
+	outChildIdx = body_location >> 30;
+	JPH_ASSERT(mAllocator->Get(outNodeIdx).mChildNodeID[outChildIdx] == inBodyID, "Make sure that the body is in the node where it should be");
+}
+
+void QuadTree::SetBodyLocation(TrackingVector &ioTracking, BodyID inBodyID, uint32 inNodeIdx, uint32 inChildIdx) const
+{
+	JPH_ASSERT(inNodeIdx <= 0x3fffffff);
+	JPH_ASSERT(inChildIdx < 4);
+	JPH_ASSERT(mAllocator->Get(inNodeIdx).mChildNodeID[inChildIdx] == inBodyID, "Make sure that the body is in the node where it should be");
+	ioTracking[inBodyID.GetIndex()].mBodyLocation = inNodeIdx + (inChildIdx << 30);
+
+#ifdef JPH_ENABLE_ASSERTS
+	uint32 v1, v2;
+	GetBodyLocation(ioTracking, inBodyID, v1, v2);
+	JPH_ASSERT(v1 == inNodeIdx);
+	JPH_ASSERT(v2 == inChildIdx);
+#endif
+}
+
+void QuadTree::sInvalidateBodyLocation(TrackingVector &ioTracking, BodyID inBodyID)
+{
+	ioTracking[inBodyID.GetIndex()].mBodyLocation = Tracking::cInvalidBodyLocation;
+}
+
+QuadTree::~QuadTree()
+{
+	// Get rid of any nodes that are still to be freed
+	DiscardOldTree();
+
+	// Get the current root node
+	const RootNode &root_node = GetCurrentRoot();
+
+	// Collect all bodies
+	Allocator::Batch free_batch;
+	NodeID node_stack[cStackSize];
+	node_stack[0] = root_node.GetNodeID();
+	JPH_ASSERT(node_stack[0].IsValid());
+	if (node_stack[0].IsNode())
+	{
+		int top = 0;
+		do
+		{
+			// Process node
+			NodeID node_id = node_stack[top];
+			JPH_ASSERT(!node_id.IsBody());
+			uint32 node_idx = node_id.GetNodeIndex();
+			const Node &node = mAllocator->Get(node_idx);
+
+			// Recurse and get all child nodes
+			for (NodeID child_node_id : node.mChildNodeID)
+				if (child_node_id.IsValid() && child_node_id.IsNode())
+				{
+					JPH_ASSERT(top < cStackSize);
+					node_stack[top] = child_node_id;
+					top++;
+				}
+
+			// Mark node to be freed
+			mAllocator->AddObjectToBatch(free_batch, node_idx);
+			--top;
+		}
+		while (top >= 0);
+	}
+
+	// Now free all nodes
+	mAllocator->DestructObjectBatch(free_batch);
+}
+
+uint32 QuadTree::AllocateNode(bool inIsChanged)
+{
+	uint32 index = mAllocator->ConstructObject(inIsChanged);
+	if (index == Allocator::cInvalidObjectIndex)
+	{
+		Trace("QuadTree: Out of nodes!");
+		std::abort();
+	}
+	return index;
+}
+
+void QuadTree::Init(Allocator &inAllocator)
+{
+	// Store allocator
+	mAllocator = &inAllocator;
+
+	// Allocate root node
+	mRootNode[mRootNodeIndex].mIndex = AllocateNode(false);
+}
+
+void QuadTree::DiscardOldTree()
+{
+	// Check if there is an old tree
+	RootNode &old_root_node = mRootNode[mRootNodeIndex ^ 1];
+	if (old_root_node.mIndex != cInvalidNodeIndex)
+	{
+		// Clear the root
+		old_root_node.mIndex = cInvalidNodeIndex;
+
+		// Now free all old nodes
+		mAllocator->DestructObjectBatch(mFreeNodeBatch);
+
+		// Clear the batch
+		mFreeNodeBatch = Allocator::Batch();
+	}
+}
+
+AABox QuadTree::GetBounds() const
+{
+	uint32 node_idx = GetCurrentRoot().mIndex;
+	JPH_ASSERT(node_idx != cInvalidNodeIndex);
+	const Node &node = mAllocator->Get(node_idx);
+
+	AABox bounds;
+	node.GetNodeBounds(bounds);
+	return bounds;
+}
+
+void QuadTree::UpdatePrepare(const BodyVector &inBodies, TrackingVector &ioTracking, UpdateState &outUpdateState, bool inFullRebuild)
+{
+#ifdef JPH_ENABLE_ASSERTS
+	// We only read positions
+	BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::Read);
+#endif
+
+	// Assert we have no nodes pending deletion, this means DiscardOldTree wasn't called yet
+	JPH_ASSERT(mFreeNodeBatch.mNumObjects == 0);
+
+	// Mark tree non-dirty
+	mIsDirty = false;
+
+	// Get the current root node
+	const RootNode &root_node = GetCurrentRoot();
+
+#ifdef JPH_DUMP_BROADPHASE_TREE
+	DumpTree(root_node.GetNodeID(), StringFormat("%s_PRE", mName).c_str());
+#endif
+
+	// Assert sane data
+#ifdef JPH_DEBUG
+	ValidateTree(inBodies, ioTracking, root_node.mIndex, mNumBodies);
+#endif
+
+	// Create space for all body ID's
+	NodeID *node_ids = new NodeID [mNumBodies];
+	NodeID *cur_node_id = node_ids;
+
+	// Collect all bodies
+	NodeID node_stack[cStackSize];
+	node_stack[0] = root_node.GetNodeID();
+	JPH_ASSERT(node_stack[0].IsValid());
+	int top = 0;
+	do
+	{
+		// Check if node is a body
+		NodeID node_id = node_stack[top];
+		if (node_id.IsBody())
+		{
+			// Validate that we're still in the right layer
+		#ifdef JPH_ENABLE_ASSERTS
+			uint32 body_index = node_id.GetBodyID().GetIndex();
+			JPH_ASSERT(ioTracking[body_index].mObjectLayer == inBodies[body_index]->GetObjectLayer());
+		#endif
+
+			// Store body
+			*cur_node_id = node_id;
+			++cur_node_id;
+		}
+		else
+		{
+			// Process normal node
+			uint32 node_idx = node_id.GetNodeIndex();
+			const Node &node = mAllocator->Get(node_idx);
+
+			if (!node.mIsChanged && !inFullRebuild)
+			{
+				// Node is unchanged, treat it as a whole
+				*cur_node_id = node_id;
+				++cur_node_id;
+			}
+			else
+			{
+				// Node is changed, recurse and get all children
+				for (NodeID child_node_id : node.mChildNodeID)
+					if (child_node_id.IsValid())
+					{
+						if (top < cStackSize)
+						{
+							node_stack[top] = child_node_id;
+							top++;
+						}
+						else
+						{
+							JPH_ASSERT(false, "Stack full!\n"
+								"This must be a very deep tree. Are you batch adding bodies through BodyInterface::AddBodiesPrepare/AddBodiesFinalize?\n"
+								"If you add lots of bodies through BodyInterface::AddBody you may need to call PhysicsSystem::OptimizeBroadPhase to rebuild the tree.");
+
+							// Falling back to adding the node as a whole
+							*cur_node_id = child_node_id;
+							++cur_node_id;
+						}
+					}
+
+				// Mark node to be freed
+				mAllocator->AddObjectToBatch(mFreeNodeBatch, node_idx);
+			}
+		}
+		--top;
+	}
+	while (top >= 0);
+
+	// Check that our book keeping matches
+	uint32 num_node_ids = uint32(cur_node_id - node_ids);
+	JPH_ASSERT(inFullRebuild? num_node_ids == mNumBodies : num_node_ids <= mNumBodies);
+
+	// This will be the new root node id
+	NodeID root_node_id;
+
+	if (num_node_ids > 0)
+	{
+		// We mark the first 5 levels (max 1024 nodes) of the newly built tree as 'changed' so that
+		// those nodes get recreated every time when we rebuild the tree. This balances the amount of
+		// time we spend on rebuilding the tree ('unchanged' nodes will be put in the new tree as a whole)
+		// vs the quality of the built tree.
+		constexpr uint cMaxDepthMarkChanged = 5;
+
+		// Build new tree
+		AABox root_bounds;
+		root_node_id = BuildTree(inBodies, ioTracking, node_ids, num_node_ids, cMaxDepthMarkChanged, root_bounds);
+
+		if (root_node_id.IsBody())
+		{
+			// For a single body we need to allocate a new root node
+			uint32 root_idx = AllocateNode(false);
+			Node &root = mAllocator->Get(root_idx);
+			root.SetChildBounds(0, root_bounds);
+			root.mChildNodeID[0] = root_node_id;
+			SetBodyLocation(ioTracking, root_node_id.GetBodyID(), root_idx, 0);
+			root_node_id = NodeID::sFromNodeIndex(root_idx);
+		}
+	}
+	else
+	{
+		// Empty tree, create root node
+		uint32 root_idx = AllocateNode(false);
+		root_node_id = NodeID::sFromNodeIndex(root_idx);
+	}
+
+	// Delete temporary data
+	delete [] node_ids;
+
+	outUpdateState.mRootNodeID = root_node_id;
+}
+
+void QuadTree::UpdateFinalize([[maybe_unused]] const BodyVector &inBodies, [[maybe_unused]] const TrackingVector &inTracking, const UpdateState &inUpdateState)
+{
+	// Tree building is complete, now we switch the old with the new tree
+	uint32 new_root_idx = mRootNodeIndex ^ 1;
+	RootNode &new_root_node = mRootNode[new_root_idx];
+	{
+		// Note: We don't need to lock here as the old tree stays available so any queries
+		// that use it can continue using it until DiscardOldTree is called. This slot
+		// should be empty and unused at this moment.
+		JPH_ASSERT(new_root_node.mIndex == cInvalidNodeIndex);
+		new_root_node.mIndex = inUpdateState.mRootNodeID.GetNodeIndex();
+	}
+
+	// All queries that start from now on will use this new tree
+	mRootNodeIndex = new_root_idx;
+
+#ifdef JPH_DUMP_BROADPHASE_TREE
+	DumpTree(new_root_node.GetNodeID(), StringFormat("%s_POST", mName).c_str());
+#endif
+
+#ifdef JPH_DEBUG
+	ValidateTree(inBodies, inTracking, new_root_node.mIndex, mNumBodies);
+#endif
+}
+
+void QuadTree::sPartition(NodeID *ioNodeIDs, Vec3 *ioNodeCenters, int inNumber, int &outMidPoint)
+{
+	// Handle trivial case
+	if (inNumber <= 4)
+	{
+		outMidPoint = inNumber / 2;
+		return;
+	}
+
+	// Calculate bounding box of box centers
+	Vec3 center_min = Vec3::sReplicate(cLargeFloat);
+	Vec3 center_max = Vec3::sReplicate(-cLargeFloat);
+	for (const Vec3 *c = ioNodeCenters, *c_end = ioNodeCenters + inNumber; c < c_end; ++c)
+	{
+		Vec3 center = *c;
+		center_min = Vec3::sMin(center_min, center);
+		center_max = Vec3::sMax(center_max, center);
+	}
+
+	// Calculate split plane
+	int dimension = (center_max - center_min).GetHighestComponentIndex();
+	float split = 0.5f * (center_min + center_max)[dimension];
+
+	// Divide bodies
+	int start = 0, end = inNumber;
+	while (start < end)
+	{
+		// Search for first element that is on the right hand side of the split plane
+		while (start < end && ioNodeCenters[start][dimension] < split)
+			++start;
+
+		// Search for the first element that is on the left hand side of the split plane
+		while (start < end && ioNodeCenters[end - 1][dimension] >= split)
+			--end;
+
+		if (start < end)
+		{
+			// Swap the two elements
+			std::swap(ioNodeIDs[start], ioNodeIDs[end - 1]);
+			std::swap(ioNodeCenters[start], ioNodeCenters[end - 1]);
+			++start;
+			--end;
+		}
+	}
+	JPH_ASSERT(start == end);
+
+	if (start > 0 && start < inNumber)
+	{
+		// Success!
+		outMidPoint = start;
+	}
+	else
+	{
+		// Failed to divide bodies
+		outMidPoint = inNumber / 2;
+	}
+}
+
+void QuadTree::sPartition4(NodeID *ioNodeIDs, Vec3 *ioNodeCenters, int inBegin, int inEnd, int *outSplit)
+{
+	NodeID *node_ids = ioNodeIDs + inBegin;
+	Vec3 *node_centers = ioNodeCenters + inBegin;
+	int number = inEnd - inBegin;
+
+	// Partition entire range
+	sPartition(node_ids, node_centers, number, outSplit[2]);
+
+	// Partition lower half
+	sPartition(node_ids, node_centers, outSplit[2], outSplit[1]);
+
+	// Partition upper half
+	sPartition(node_ids + outSplit[2], node_centers + outSplit[2], number - outSplit[2], outSplit[3]);
+
+	// Convert to proper range
+	outSplit[0] = inBegin;
+	outSplit[1] += inBegin;
+	outSplit[2] += inBegin;
+	outSplit[3] += outSplit[2];
+	outSplit[4] = inEnd;
+}
+
+AABox QuadTree::GetNodeOrBodyBounds(const BodyVector &inBodies, NodeID inNodeID) const
+{
+	if (inNodeID.IsNode())
+	{
+		// It is a node
+		uint32 node_idx = inNodeID.GetNodeIndex();
+		const Node &node = mAllocator->Get(node_idx);
+
+		AABox bounds;
+		node.GetNodeBounds(bounds);
+		return bounds;
+	}
+	else
+	{
+		// It is a body
+		return inBodies[inNodeID.GetBodyID().GetIndex()]->GetWorldSpaceBounds();
+	}
+}
+
+QuadTree::NodeID QuadTree::BuildTree(const BodyVector &inBodies, TrackingVector &ioTracking, NodeID *ioNodeIDs, int inNumber, uint inMaxDepthMarkChanged, AABox &outBounds)
+{
+	// Trivial case: No bodies in tree
+	if (inNumber == 0)
+	{
+		outBounds = cInvalidBounds;
+		return NodeID::sInvalid();
+	}
+
+	// Trivial case: When we have 1 body or node, return it
+	if (inNumber == 1)
+	{
+		if (ioNodeIDs->IsNode())
+		{
+			// When returning an existing node as root, ensure that no parent has been set
+			Node &node = mAllocator->Get(ioNodeIDs->GetNodeIndex());
+			node.mParentNodeIndex = cInvalidNodeIndex;
+		}
+		outBounds = GetNodeOrBodyBounds(inBodies, *ioNodeIDs);
+		return *ioNodeIDs;
+	}
+
+	// Calculate centers of all bodies that are to be inserted
+	Vec3 *centers = new Vec3 [inNumber];
+	JPH_ASSERT(IsAligned(centers, JPH_VECTOR_ALIGNMENT));
+	Vec3 *c = centers;
+	for (const NodeID *n = ioNodeIDs, *n_end = ioNodeIDs + inNumber; n < n_end; ++n, ++c)
+		*c = GetNodeOrBodyBounds(inBodies, *n).GetCenter();
+
+	// The algorithm is a recursive tree build, but to avoid the call overhead we keep track of a stack here
+	struct StackEntry
+	{
+		uint32			mNodeIdx;					// Node index of node that is generated
+		int				mChildIdx;					// Index of child that we're currently processing
+		int				mSplit[5];					// Indices where the node ID's have been split to form 4 partitions
+		uint32			mDepth;						// Depth of this node in the tree
+		Vec3			mNodeBoundsMin;				// Bounding box of this node, accumulated while iterating over children
+		Vec3			mNodeBoundsMax;
+	};
+	static_assert(sizeof(StackEntry) == 64);
+	StackEntry stack[cStackSize / 4]; // We don't process 4 at a time in this loop but 1, so the stack can be 4x as small
+	int top = 0;
+
+	// Create root node
+	stack[0].mNodeIdx = AllocateNode(inMaxDepthMarkChanged > 0);
+	stack[0].mChildIdx = -1;
+	stack[0].mDepth = 0;
+	stack[0].mNodeBoundsMin = Vec3::sReplicate(cLargeFloat);
+	stack[0].mNodeBoundsMax = Vec3::sReplicate(-cLargeFloat);
+	sPartition4(ioNodeIDs, centers, 0, inNumber, stack[0].mSplit);
+
+	for (;;)
+	{
+		StackEntry &cur_stack = stack[top];
+
+		// Next child
+		cur_stack.mChildIdx++;
+
+		// Check if all children processed
+		if (cur_stack.mChildIdx >= 4)
+		{
+			// Terminate if there's nothing left to pop
+			if (top <= 0)
+				break;
+
+			// Add our bounds to our parents bounds
+			StackEntry &prev_stack = stack[top - 1];
+			prev_stack.mNodeBoundsMin = Vec3::sMin(prev_stack.mNodeBoundsMin, cur_stack.mNodeBoundsMin);
+			prev_stack.mNodeBoundsMax = Vec3::sMax(prev_stack.mNodeBoundsMax, cur_stack.mNodeBoundsMax);
+
+			// Store parent node
+			Node &node = mAllocator->Get(cur_stack.mNodeIdx);
+			node.mParentNodeIndex = prev_stack.mNodeIdx;
+
+			// Store this node's properties in the parent node
+			Node &parent_node = mAllocator->Get(prev_stack.mNodeIdx);
+			parent_node.mChildNodeID[prev_stack.mChildIdx] = NodeID::sFromNodeIndex(cur_stack.mNodeIdx);
+			parent_node.SetChildBounds(prev_stack.mChildIdx, AABox(cur_stack.mNodeBoundsMin, cur_stack.mNodeBoundsMax));
+
+			// Pop entry from stack
+			--top;
+		}
+		else
+		{
+			// Get low and high index to bodies to process
+			int low = cur_stack.mSplit[cur_stack.mChildIdx];
+			int high = cur_stack.mSplit[cur_stack.mChildIdx + 1];
+			int num_bodies = high - low;
+
+			if (num_bodies == 1)
+			{
+				// Get body info
+				NodeID child_node_id = ioNodeIDs[low];
+				AABox bounds = GetNodeOrBodyBounds(inBodies, child_node_id);
+
+				// Update node
+				Node &node = mAllocator->Get(cur_stack.mNodeIdx);
+				node.mChildNodeID[cur_stack.mChildIdx] = child_node_id;
+				node.SetChildBounds(cur_stack.mChildIdx, bounds);
+
+				if (child_node_id.IsNode())
+				{
+					// Update parent for this node
+					Node &child_node = mAllocator->Get(child_node_id.GetNodeIndex());
+					child_node.mParentNodeIndex = cur_stack.mNodeIdx;
+				}
+				else
+				{
+					// Set location in tracking
+					SetBodyLocation(ioTracking, child_node_id.GetBodyID(), cur_stack.mNodeIdx, cur_stack.mChildIdx);
+				}
+
+				// Encapsulate bounding box in parent
+				cur_stack.mNodeBoundsMin = Vec3::sMin(cur_stack.mNodeBoundsMin, bounds.mMin);
+				cur_stack.mNodeBoundsMax = Vec3::sMax(cur_stack.mNodeBoundsMax, bounds.mMax);
+			}
+			else if (num_bodies > 1)
+			{
+				// Allocate new node
+				StackEntry &new_stack = stack[++top];
+				JPH_ASSERT(top < cStackSize / 4);
+				uint32 next_depth = cur_stack.mDepth + 1;
+				new_stack.mNodeIdx = AllocateNode(inMaxDepthMarkChanged > next_depth);
+				new_stack.mChildIdx = -1;
+				new_stack.mDepth = next_depth;
+				new_stack.mNodeBoundsMin = Vec3::sReplicate(cLargeFloat);
+				new_stack.mNodeBoundsMax = Vec3::sReplicate(-cLargeFloat);
+				sPartition4(ioNodeIDs, centers, low, high, new_stack.mSplit);
+			}
+		}
+	}
+
+	// Delete temporary data
+	delete [] centers;
+
+	// Store bounding box of root
+	outBounds.mMin = stack[0].mNodeBoundsMin;
+	outBounds.mMax = stack[0].mNodeBoundsMax;
+
+	// Return root
+	return NodeID::sFromNodeIndex(stack[0].mNodeIdx);
+}
+
+void QuadTree::MarkNodeAndParentsChanged(uint32 inNodeIndex)
+{
+	uint32 node_idx = inNodeIndex;
+
+	do
+	{
+		// If node has changed, parent will be too
+		Node &node = mAllocator->Get(node_idx);
+		if (node.mIsChanged)
+			break;
+
+		// Mark node as changed
+		node.mIsChanged = true;
+
+		// Get our parent
+		node_idx = node.mParentNodeIndex;
+	}
+	while (node_idx != cInvalidNodeIndex);
+}
+
+void QuadTree::WidenAndMarkNodeAndParentsChanged(uint32 inNodeIndex, const AABox &inNewBounds)
+{
+	uint32 node_idx = inNodeIndex;
+
+	for (;;)
+	{
+		// Mark node as changed
+		Node &node = mAllocator->Get(node_idx);
+		node.mIsChanged = true;
+
+		// Get our parent
+		uint32 parent_idx = node.mParentNodeIndex;
+		if (parent_idx == cInvalidNodeIndex)
+			break;
+
+		// Find which child of the parent we're in
+		Node &parent_node = mAllocator->Get(parent_idx);
+		NodeID node_id = NodeID::sFromNodeIndex(node_idx);
+		int child_idx = -1;
+		for (int i = 0; i < 4; ++i)
+			if (parent_node.mChildNodeID[i] == node_id)
+			{
+				// Found one, set the node index and child index and update the bounding box too
+				child_idx = i;
+				break;
+			}
+		JPH_ASSERT(child_idx != -1, "Nodes don't get removed from the tree, we must have found it");
+
+		// To avoid any race conditions with other threads we only enlarge bounding boxes
+		if (!parent_node.EncapsulateChildBounds(child_idx, inNewBounds))
+		{
+			// No changes to bounding box, only marking as changed remains to be done
+			if (!parent_node.mIsChanged)
+				MarkNodeAndParentsChanged(parent_idx);
+			break;
+		}
+
+		// Update node index
+		node_idx = parent_idx;
+	}
+}
+
+bool QuadTree::TryInsertLeaf(TrackingVector &ioTracking, int inNodeIndex, NodeID inLeafID, const AABox &inLeafBounds, int inLeafNumBodies)
+{
+	// Tentively assign the node as parent
+	bool leaf_is_node = inLeafID.IsNode();
+	if (leaf_is_node)
+	{
+		uint32 leaf_idx = inLeafID.GetNodeIndex();
+		mAllocator->Get(leaf_idx).mParentNodeIndex = inNodeIndex;
+	}
+
+	// Fetch node that we're adding to
+	Node &node = mAllocator->Get(inNodeIndex);
+
+	// Find an empty child
+	for (uint32 child_idx = 0; child_idx < 4; ++child_idx)
+		if (node.mChildNodeID[child_idx].CompareExchange(NodeID::sInvalid(), inLeafID)) // Check if we can claim it
+		{
+			// We managed to add it to the node
+
+			// If leaf was a body, we need to update its bookkeeping
+			if (!leaf_is_node)
+				SetBodyLocation(ioTracking, inLeafID.GetBodyID(), inNodeIndex, child_idx);
+
+			// Now set the bounding box making the child valid for queries
+			node.SetChildBounds(child_idx, inLeafBounds);
+
+			// Widen the bounds for our parents too
+			WidenAndMarkNodeAndParentsChanged(inNodeIndex, inLeafBounds);
+
+			// Update body counter
+			mNumBodies += inLeafNumBodies;
+
+			// And we're done
+			return true;
+		}
+
+	return false;
+}
+
+bool QuadTree::TryCreateNewRoot(TrackingVector &ioTracking, atomic<uint32> &ioRootNodeIndex, NodeID inLeafID, const AABox &inLeafBounds, int inLeafNumBodies)
+{
+	// Fetch old root
+	uint32 root_idx = ioRootNodeIndex;
+	Node &root = mAllocator->Get(root_idx);
+
+	// Create new root, mark this new root as changed as we're not creating a very efficient tree at this point
+	uint32 new_root_idx = AllocateNode(true);
+	Node &new_root = mAllocator->Get(new_root_idx);
+
+	// First child is current root, note that since the tree may be modified concurrently we cannot assume that the bounds of our child will be correct so we set a very large bounding box
+	new_root.mChildNodeID[0] = NodeID::sFromNodeIndex(root_idx);
+	new_root.SetChildBounds(0, AABox(Vec3::sReplicate(-cLargeFloat), Vec3::sReplicate(cLargeFloat)));
+
+	// Second child is new leaf
+	new_root.mChildNodeID[1] = inLeafID;
+	new_root.SetChildBounds(1, inLeafBounds);
+
+	// Tentatively assign new root as parent
+	bool leaf_is_node = inLeafID.IsNode();
+	if (leaf_is_node)
+	{
+		uint32 leaf_idx = inLeafID.GetNodeIndex();
+		mAllocator->Get(leaf_idx).mParentNodeIndex = new_root_idx;
+	}
+
+	// Try to swap it
+	if (ioRootNodeIndex.compare_exchange_strong(root_idx, new_root_idx))
+	{
+		// We managed to set the new root
+
+		// If leaf was a body, we need to update its bookkeeping
+		if (!leaf_is_node)
+			SetBodyLocation(ioTracking, inLeafID.GetBodyID(), new_root_idx, 1);
+
+		// Store parent node for old root
+		root.mParentNodeIndex = new_root_idx;
+
+		// Update body counter
+		mNumBodies += inLeafNumBodies;
+
+		// And we're done
+		return true;
+	}
+
+	// Failed to swap, someone else must have created a new root, try again
+	mAllocator->DestructObject(new_root_idx);
+	return false;
+}
+
+void QuadTree::AddBodiesPrepare(const BodyVector &inBodies, TrackingVector &ioTracking, BodyID *ioBodyIDs, int inNumber, AddState &outState)
+{
+	// Assert sane input
+	JPH_ASSERT(ioBodyIDs != nullptr);
+	JPH_ASSERT(inNumber > 0);
+
+#ifdef JPH_ENABLE_ASSERTS
+	// Below we just cast the body ID's to node ID's, check here that that is valid
+	for (const BodyID *b = ioBodyIDs, *b_end = ioBodyIDs + inNumber; b < b_end; ++b)
+		NodeID::sFromBodyID(*b);
+#endif
+
+	// Build subtree for the new bodies, note that we mark all nodes as 'not changed'
+	// so they will stay together as a batch and will make the tree rebuild cheaper
+	outState.mLeafID = BuildTree(inBodies, ioTracking, (NodeID *)ioBodyIDs, inNumber, 0, outState.mLeafBounds);
+
+#ifdef JPH_DEBUG
+	if (outState.mLeafID.IsNode())
+		ValidateTree(inBodies, ioTracking, outState.mLeafID.GetNodeIndex(), inNumber);
+#endif
+}
+
+void QuadTree::AddBodiesFinalize(TrackingVector &ioTracking, int inNumberBodies, const AddState &inState)
+{
+	// Assert sane input
+	JPH_ASSERT(inNumberBodies > 0);
+
+	// Mark tree dirty
+	mIsDirty = true;
+
+	// Get the current root node
+	RootNode &root_node = GetCurrentRoot();
+
+	for (;;)
+	{
+		// Check if we can insert the body in the root
+		if (TryInsertLeaf(ioTracking, root_node.mIndex, inState.mLeafID, inState.mLeafBounds, inNumberBodies))
+			return;
+
+		// Check if we can create a new root
+		if (TryCreateNewRoot(ioTracking, root_node.mIndex, inState.mLeafID, inState.mLeafBounds, inNumberBodies))
+			return;
+	}
+}
+
+void QuadTree::AddBodiesAbort(TrackingVector &ioTracking, const AddState &inState)
+{
+	// Collect all bodies
+	Allocator::Batch free_batch;
+	NodeID node_stack[cStackSize];
+	node_stack[0] = inState.mLeafID;
+	JPH_ASSERT(node_stack[0].IsValid());
+	int top = 0;
+	do
+	{
+		// Check if node is a body
+		NodeID child_node_id = node_stack[top];
+		if (child_node_id.IsBody())
+		{
+			// Reset location of body
+			sInvalidateBodyLocation(ioTracking, child_node_id.GetBodyID());
+		}
+		else
+		{
+			// Process normal node
+			uint32 node_idx = child_node_id.GetNodeIndex();
+			const Node &node = mAllocator->Get(node_idx);
+			for (NodeID sub_child_node_id : node.mChildNodeID)
+				if (sub_child_node_id.IsValid())
+				{
+					JPH_ASSERT(top < cStackSize);
+					node_stack[top] = sub_child_node_id;
+					top++;
+				}
+
+			// Mark it to be freed
+			mAllocator->AddObjectToBatch(free_batch, node_idx);
+		}
+		--top;
+	}
+	while (top >= 0);
+
+	// Now free all nodes as a single batch
+	mAllocator->DestructObjectBatch(free_batch);
+}
+
+void QuadTree::RemoveBodies([[maybe_unused]] const BodyVector &inBodies, TrackingVector &ioTracking, const BodyID *ioBodyIDs, int inNumber)
+{
+	// Assert sane input
+	JPH_ASSERT(ioBodyIDs != nullptr);
+	JPH_ASSERT(inNumber > 0);
+
+	// Mark tree dirty
+	mIsDirty = true;
+
+	for (const BodyID *cur = ioBodyIDs, *end = ioBodyIDs + inNumber; cur < end; ++cur)
+	{
+		// Check if BodyID is correct
+		JPH_ASSERT(inBodies[cur->GetIndex()]->GetID() == *cur, "Provided BodyID doesn't match BodyID in body manager");
+
+		// Get location of body
+		uint32 node_idx, child_idx;
+		GetBodyLocation(ioTracking, *cur, node_idx, child_idx);
+
+		// First we reset our internal bookkeeping
+		sInvalidateBodyLocation(ioTracking, *cur);
+
+		// Then we make the bounding box invalid, no queries can find this node anymore
+		Node &node = mAllocator->Get(node_idx);
+		node.InvalidateChildBounds(child_idx);
+
+		// Finally we reset the child id, this makes the node available for adds again
+		node.mChildNodeID[child_idx] = NodeID::sInvalid();
+
+		// We don't need to bubble up our bounding box changes to our parents since we never make volumes smaller, only bigger
+		// But we do need to mark the nodes as changed so that the tree can be rebuilt
+		MarkNodeAndParentsChanged(node_idx);
+	}
+
+	mNumBodies -= inNumber;
+}
+
+void QuadTree::NotifyBodiesAABBChanged(const BodyVector &inBodies, const TrackingVector &inTracking, const BodyID *ioBodyIDs, int inNumber)
+{
+	// Assert sane input
+	JPH_ASSERT(ioBodyIDs != nullptr);
+	JPH_ASSERT(inNumber > 0);
+
+	for (const BodyID *cur = ioBodyIDs, *end = ioBodyIDs + inNumber; cur < end; ++cur)
+	{
+		// Check if BodyID is correct
+		const Body *body = inBodies[cur->GetIndex()];
+		JPH_ASSERT(body->GetID() == *cur, "Provided BodyID doesn't match BodyID in body manager");
+
+		// Get the new bounding box
+		const AABox &new_bounds = body->GetWorldSpaceBounds();
+
+		// Get location of body
+		uint32 node_idx, child_idx;
+		GetBodyLocation(inTracking, *cur, node_idx, child_idx);
+
+		// Widen bounds for node
+		Node &node = mAllocator->Get(node_idx);
+		if (node.EncapsulateChildBounds(child_idx, new_bounds))
+		{
+			// Mark tree dirty
+			mIsDirty = true;
+
+			// If bounds changed, widen the bounds for our parents too
+			WidenAndMarkNodeAndParentsChanged(node_idx, new_bounds);
+		}
+	}
+}
+
+template <class Visitor>
+JPH_INLINE void QuadTree::WalkTree(const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking, Visitor &ioVisitor JPH_IF_TRACK_BROADPHASE_STATS(, LayerToStats &ioStats)) const
+{
+	// Get the root
+	const RootNode &root_node = GetCurrentRoot();
+
+#ifdef JPH_TRACK_BROADPHASE_STATS
+	// Start tracking stats
+	int bodies_visited = 0;
+	int hits_collected = 0;
+	int nodes_visited = 0;
+	uint64 collector_ticks = 0;
+
+	uint64 start = GetProcessorTickCount();
+#endif // JPH_TRACK_BROADPHASE_STATS
+
+	NodeID node_stack[cStackSize];
+	node_stack[0] = root_node.GetNodeID();
+	int top = 0;
+	do
+	{
+		// Check if node is a body
+		NodeID child_node_id = node_stack[top];
+		if (child_node_id.IsBody())
+		{
+			// Track amount of bodies visited
+			JPH_IF_TRACK_BROADPHASE_STATS(++bodies_visited;)
+
+			BodyID body_id = child_node_id.GetBodyID();
+			ObjectLayer object_layer = inTracking[body_id.GetIndex()].mObjectLayer; // We're not taking a lock on the body, so it may be in the process of being removed so check if the object layer is invalid
+			if (object_layer != cObjectLayerInvalid && inObjectLayerFilter.ShouldCollide(object_layer))
+			{
+				JPH_PROFILE("VisitBody");
+
+				// Track amount of hits
+				JPH_IF_TRACK_BROADPHASE_STATS(++hits_collected;)
+
+				// Start track time the collector takes
+				JPH_IF_TRACK_BROADPHASE_STATS(uint64 collector_start = GetProcessorTickCount();)
+
+				// We found a body we collide with, call our visitor
+				ioVisitor.VisitBody(body_id, top);
+
+				// End track time the collector takes
+				JPH_IF_TRACK_BROADPHASE_STATS(collector_ticks += GetProcessorTickCount() - collector_start;)
+
+				// Check if we're done
+				if (ioVisitor.ShouldAbort())
+					break;
+			}
+		}
+		else if (child_node_id.IsValid())
+		{
+			JPH_IF_TRACK_BROADPHASE_STATS(++nodes_visited;)
+
+			// Check if stack can hold more nodes
+			if (top + 4 < cStackSize)
+			{
+				// Process normal node
+				const Node &node = mAllocator->Get(child_node_id.GetNodeIndex());
+				JPH_ASSERT(IsAligned(&node, JPH_CACHE_LINE_SIZE));
+
+				// Load bounds of 4 children
+				Vec4 bounds_minx = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinX);
+				Vec4 bounds_miny = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinY);
+				Vec4 bounds_minz = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinZ);
+				Vec4 bounds_maxx = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxX);
+				Vec4 bounds_maxy = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxY);
+				Vec4 bounds_maxz = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxZ);
+
+				// Load ids for 4 children
+				UVec4 child_ids = UVec4::sLoadInt4Aligned((const uint32 *)&node.mChildNodeID[0]);
+
+				// Check which sub nodes to visit
+				int num_results = ioVisitor.VisitNodes(bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz, child_ids, top);
+				child_ids.StoreInt4((uint32 *)&node_stack[top]);
+				top += num_results;
+			}
+			else
+				JPH_ASSERT(false, "Stack full!\n"
+					"This must be a very deep tree. Are you batch adding bodies through BodyInterface::AddBodiesPrepare/AddBodiesFinalize?\n"
+					"If you add lots of bodies through BodyInterface::AddBody you may need to call PhysicsSystem::OptimizeBroadPhase to rebuild the tree.");
+		}
+
+		// Fetch next node until we find one that the visitor wants to see
+		do
+			--top;
+		while (top >= 0 && !ioVisitor.ShouldVisitNode(top));
+	}
+	while (top >= 0);
+
+#ifdef JPH_TRACK_BROADPHASE_STATS
+	// Calculate total time the broadphase walk took
+	uint64 total_ticks = GetProcessorTickCount() - start;
+
+	// Update stats under lock protection (slow!)
+	{
+		unique_lock lock(mStatsMutex);
+		Stat &s = ioStats[inObjectLayerFilter.GetDescription()];
+		s.mNumQueries++;
+		s.mNodesVisited += nodes_visited;
+		s.mBodiesVisited += bodies_visited;
+		s.mHitsReported += hits_collected;
+		s.mTotalTicks += total_ticks;
+		s.mCollectorTicks += collector_ticks;
+	}
+#endif // JPH_TRACK_BROADPHASE_STATS
+}
+
+void QuadTree::CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const
+{
+	class Visitor
+	{
+	public:
+		/// Constructor
+		JPH_INLINE				Visitor(const RayCast &inRay, RayCastBodyCollector &ioCollector) :
+			mOrigin(inRay.mOrigin),
+			mInvDirection(inRay.mDirection),
+			mCollector(ioCollector)
+		{
+			mFractionStack[0] = -1;
+		}
+
+		/// Returns true if further processing of the tree should be aborted
+		JPH_INLINE bool			ShouldAbort() const
+		{
+			return mCollector.ShouldEarlyOut();
+		}
+
+		/// Returns true if this node / body should be visited, false if no hit can be generated
+		JPH_INLINE bool			ShouldVisitNode(int inStackTop) const
+		{
+			return mFractionStack[inStackTop] < mCollector.GetEarlyOutFraction();
+		}
+
+		/// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector.
+		JPH_INLINE int			VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop)
+		{
+			// Test the ray against 4 bounding boxes
+			Vec4 fraction = RayAABox4(mOrigin, mInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+
+			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
+			return SortReverseAndStore(fraction, mCollector.GetEarlyOutFraction(), ioChildNodeIDs, &mFractionStack[inStackTop]);
+		}
+
+		/// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore
+		JPH_INLINE void			VisitBody(const BodyID &inBodyID, int inStackTop)
+		{
+			// Store potential hit with body
+			BroadPhaseCastResult result { inBodyID, mFractionStack[inStackTop] };
+			mCollector.AddHit(result);
+		}
+
+	private:
+		Vec3					mOrigin;
+		RayInvDirection			mInvDirection;
+		RayCastBodyCollector &	mCollector;
+		float					mFractionStack[cStackSize];
+	};
+
+	Visitor visitor(inRay, ioCollector);
+	WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCastRayStats));
+}
+
+void QuadTree::CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const
+{
+	class Visitor
+	{
+	public:
+		/// Constructor
+		JPH_INLINE					Visitor(const AABox &inBox, CollideShapeBodyCollector &ioCollector) :
+			mBox(inBox),
+			mCollector(ioCollector)
+		{
+		}
+
+		/// Returns true if further processing of the tree should be aborted
+		JPH_INLINE bool				ShouldAbort() const
+		{
+			return mCollector.ShouldEarlyOut();
+		}
+
+		/// Returns true if this node / body should be visited, false if no hit can be generated
+		JPH_INLINE bool				ShouldVisitNode(int inStackTop) const
+		{
+			return true;
+		}
+
+		/// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector.
+		JPH_INLINE int				VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop) const
+		{
+			// Test the box vs 4 boxes
+			UVec4 hitting = AABox4VsBox(mBox, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+			return CountAndSortTrues(hitting, ioChildNodeIDs);
+		}
+
+		/// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore
+		JPH_INLINE void				VisitBody(const BodyID &inBodyID, int inStackTop)
+		{
+			// Store potential hit with body
+			mCollector.AddHit(inBodyID);
+		}
+
+	private:
+		const AABox &				mBox;
+		CollideShapeBodyCollector &	mCollector;
+	};
+
+	Visitor visitor(inBox, ioCollector);
+	WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCollideAABoxStats));
+}
+
+void QuadTree::CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const
+{
+	class Visitor
+	{
+	public:
+		/// Constructor
+		JPH_INLINE					Visitor(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector) :
+			mCenterX(inCenter.SplatX()),
+			mCenterY(inCenter.SplatY()),
+			mCenterZ(inCenter.SplatZ()),
+			mRadiusSq(Vec4::sReplicate(Square(inRadius))),
+			mCollector(ioCollector)
+		{
+		}
+
+		/// Returns true if further processing of the tree should be aborted
+		JPH_INLINE bool				ShouldAbort() const
+		{
+			return mCollector.ShouldEarlyOut();
+		}
+
+		/// Returns true if this node / body should be visited, false if no hit can be generated
+		JPH_INLINE bool				ShouldVisitNode(int inStackTop) const
+		{
+			return true;
+		}
+
+		/// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector.
+		JPH_INLINE int				VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop) const
+		{
+			// Test 4 boxes vs sphere
+			UVec4 hitting = AABox4VsSphere(mCenterX, mCenterY, mCenterZ, mRadiusSq, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+			return CountAndSortTrues(hitting, ioChildNodeIDs);
+		}
+
+		/// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore
+		JPH_INLINE void				VisitBody(const BodyID &inBodyID, int inStackTop)
+		{
+			// Store potential hit with body
+			mCollector.AddHit(inBodyID);
+		}
+
+	private:
+		Vec4						mCenterX;
+		Vec4						mCenterY;
+		Vec4						mCenterZ;
+		Vec4						mRadiusSq;
+		CollideShapeBodyCollector &	mCollector;
+	};
+
+	Visitor visitor(inCenter, inRadius, ioCollector);
+	WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCollideSphereStats));
+}
+
+void QuadTree::CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const
+{
+	class Visitor
+	{
+	public:
+		/// Constructor
+		JPH_INLINE					Visitor(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector) :
+			mPoint(inPoint),
+			mCollector(ioCollector)
+		{
+		}
+
+		/// Returns true if further processing of the tree should be aborted
+		JPH_INLINE bool				ShouldAbort() const
+		{
+			return mCollector.ShouldEarlyOut();
+		}
+
+		/// Returns true if this node / body should be visited, false if no hit can be generated
+		JPH_INLINE bool				ShouldVisitNode(int inStackTop) const
+		{
+			return true;
+		}
+
+		/// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector.
+		JPH_INLINE int				VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop) const
+		{
+			// Test if point overlaps with box
+			UVec4 hitting = AABox4VsPoint(mPoint, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+			return CountAndSortTrues(hitting, ioChildNodeIDs);
+		}
+
+		/// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore
+		JPH_INLINE void				VisitBody(const BodyID &inBodyID, int inStackTop)
+		{
+			// Store potential hit with body
+			mCollector.AddHit(inBodyID);
+		}
+
+	private:
+		Vec3						mPoint;
+		CollideShapeBodyCollector &	mCollector;
+	};
+
+	Visitor visitor(inPoint, ioCollector);
+	WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCollidePointStats));
+}
+
+void QuadTree::CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const
+{
+	class Visitor
+	{
+	public:
+		/// Constructor
+		JPH_INLINE					Visitor(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector) :
+			mBox(inBox),
+			mCollector(ioCollector)
+		{
+		}
+
+		/// Returns true if further processing of the tree should be aborted
+		JPH_INLINE bool				ShouldAbort() const
+		{
+			return mCollector.ShouldEarlyOut();
+		}
+
+		/// Returns true if this node / body should be visited, false if no hit can be generated
+		JPH_INLINE bool				ShouldVisitNode(int inStackTop) const
+		{
+			return true;
+		}
+
+		/// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector.
+		JPH_INLINE int				VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop) const
+		{
+			// Test if point overlaps with box
+			UVec4 hitting = AABox4VsBox(mBox, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+			return CountAndSortTrues(hitting, ioChildNodeIDs);
+		}
+
+		/// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore
+		JPH_INLINE void				VisitBody(const BodyID &inBodyID, int inStackTop)
+		{
+			// Store potential hit with body
+			mCollector.AddHit(inBodyID);
+		}
+
+	private:
+		OrientedBox					mBox;
+		CollideShapeBodyCollector &	mCollector;
+	};
+
+	Visitor visitor(inBox, ioCollector);
+	WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCollideOrientedBoxStats));
+}
+
+void QuadTree::CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const
+{
+	class Visitor
+	{
+	public:
+		/// Constructor
+		JPH_INLINE					Visitor(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector) :
+			mOrigin(inBox.mBox.GetCenter()),
+			mExtent(inBox.mBox.GetExtent()),
+			mInvDirection(inBox.mDirection),
+			mCollector(ioCollector)
+		{
+			mFractionStack[0] = -1;
+		}
+
+		/// Returns true if further processing of the tree should be aborted
+		JPH_INLINE bool				ShouldAbort() const
+		{
+			return mCollector.ShouldEarlyOut();
+		}
+
+		/// Returns true if this node / body should be visited, false if no hit can be generated
+		JPH_INLINE bool				ShouldVisitNode(int inStackTop) const
+		{
+			return mFractionStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction();
+		}
+
+		/// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector.
+		JPH_INLINE int				VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop)
+		{
+			// Enlarge them by the casted aabox extents
+			Vec4 bounds_min_x = inBoundsMinX, bounds_min_y = inBoundsMinY, bounds_min_z = inBoundsMinZ, bounds_max_x = inBoundsMaxX, bounds_max_y = inBoundsMaxY, bounds_max_z = inBoundsMaxZ;
+			AABox4EnlargeWithExtent(mExtent, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+			// Test 4 children
+			Vec4 fraction = RayAABox4(mOrigin, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
+			return SortReverseAndStore(fraction, mCollector.GetPositiveEarlyOutFraction(), ioChildNodeIDs, &mFractionStack[inStackTop]);
+		}
+
+		/// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore
+		JPH_INLINE void				VisitBody(const BodyID &inBodyID, int inStackTop)
+		{
+			// Store potential hit with body
+			BroadPhaseCastResult result { inBodyID, mFractionStack[inStackTop] };
+			mCollector.AddHit(result);
+		}
+
+	private:
+		Vec3						mOrigin;
+		Vec3						mExtent;
+		RayInvDirection				mInvDirection;
+		CastShapeBodyCollector &	mCollector;
+		float						mFractionStack[cStackSize];
+	};
+
+	Visitor visitor(inBox, ioCollector);
+	WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCastAABoxStats));
+}
+
+void QuadTree::FindCollidingPairs(const BodyVector &inBodies, const BodyID *inActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, BodyPairCollector &ioPairCollector, const ObjectLayerPairFilter &inObjectLayerPairFilter) const
+{
+	// Note that we don't lock the tree at this point. We know that the tree is not going to be swapped or deleted while finding collision pairs due to the way the jobs are scheduled in the PhysicsSystem::Update.
+	// We double check this at the end of the function.
+	const RootNode &root_node = GetCurrentRoot();
+	JPH_ASSERT(root_node.mIndex != cInvalidNodeIndex);
+
+	// Assert sane input
+	JPH_ASSERT(inActiveBodies != nullptr);
+	JPH_ASSERT(inNumActiveBodies > 0);
+
+	NodeID node_stack[cStackSize];
+
+	// Loop over all active bodies
+	for (int b1 = 0; b1 < inNumActiveBodies; ++b1)
+	{
+		BodyID b1_id = inActiveBodies[b1];
+		const Body &body1 = *inBodies[b1_id.GetIndex()];
+		JPH_ASSERT(!body1.IsStatic());
+
+		// Expand the bounding box by the speculative contact distance
+		AABox bounds1 = body1.GetWorldSpaceBounds();
+		bounds1.ExpandBy(Vec3::sReplicate(inSpeculativeContactDistance));
+
+		// Test each body with the tree
+		node_stack[0] = root_node.GetNodeID();
+		int top = 0;
+		do
+		{
+			// Check if node is a body
+			NodeID child_node_id = node_stack[top];
+			if (child_node_id.IsBody())
+			{
+				// Don't collide with self
+				BodyID b2_id = child_node_id.GetBodyID();
+				if (b1_id != b2_id)
+				{
+					// Collision between dynamic pairs need to be picked up only once
+					const Body &body2 = *inBodies[b2_id.GetIndex()];
+					if (inObjectLayerPairFilter.ShouldCollide(body1.GetObjectLayer(), body2.GetObjectLayer())
+						&& Body::sFindCollidingPairsCanCollide(body1, body2)
+						&& bounds1.Overlaps(body2.GetWorldSpaceBounds())) // In the broadphase we widen the bounding box when a body moves, do a final check to see if the bounding boxes actually overlap
+					{
+						// Store potential hit between bodies
+						ioPairCollector.AddHit({ b1_id, b2_id });
+					}
+				}
+			}
+			else if (child_node_id.IsValid())
+			{
+				// Process normal node
+				const Node &node = mAllocator->Get(child_node_id.GetNodeIndex());
+				JPH_ASSERT(IsAligned(&node, JPH_CACHE_LINE_SIZE));
+
+				// Get bounds of 4 children
+				Vec4 bounds_minx = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinX);
+				Vec4 bounds_miny = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinY);
+				Vec4 bounds_minz = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinZ);
+				Vec4 bounds_maxx = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxX);
+				Vec4 bounds_maxy = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxY);
+				Vec4 bounds_maxz = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxZ);
+
+				// Test overlap
+				UVec4 overlap = AABox4VsBox(bounds1, bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz);
+				int num_results = overlap.CountTrues();
+				if (num_results > 0)
+				{
+					// Load ids for 4 children
+					UVec4 child_ids = UVec4::sLoadInt4Aligned((const uint32 *)&node.mChildNodeID[0]);
+
+					// Sort so that overlaps are first
+					child_ids = UVec4::sSort4True(overlap, child_ids);
+
+					// Push them onto the stack
+					if (top + 4 < cStackSize)
+					{
+						child_ids.StoreInt4((uint32 *)&node_stack[top]);
+						top += num_results;
+					}
+					else
+						JPH_ASSERT(false, "Stack full!\n"
+							"This must be a very deep tree. Are you batch adding bodies through BodyInterface::AddBodiesPrepare/AddBodiesFinalize?\n"
+							"If you add lots of bodies through BodyInterface::AddBody you may need to call PhysicsSystem::OptimizeBroadPhase to rebuild the tree.");
+				}
+			}
+			--top;
+		}
+		while (top >= 0);
+	}
+
+	// Test that the root node was not swapped while finding collision pairs.
+	// This would mean that UpdateFinalize/DiscardOldTree ran during collision detection which should not be possible due to the way the jobs are scheduled.
+	JPH_ASSERT(root_node.mIndex != cInvalidNodeIndex);
+	JPH_ASSERT(&root_node == &GetCurrentRoot());
+}
+
+#ifdef JPH_DEBUG
+
+void QuadTree::ValidateTree(const BodyVector &inBodies, const TrackingVector &inTracking, uint32 inNodeIndex, uint32 inNumExpectedBodies) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Root should be valid
+	JPH_ASSERT(inNodeIndex != cInvalidNodeIndex);
+
+	// To avoid call overhead, create a stack in place
+	struct StackEntry
+	{
+		uint32			mNodeIndex;
+		uint32			mParentNodeIndex;
+	};
+	StackEntry stack[cStackSize];
+	stack[0].mNodeIndex = inNodeIndex;
+	stack[0].mParentNodeIndex = cInvalidNodeIndex;
+	int top = 0;
+
+	uint32 num_bodies = 0;
+
+	do
+	{
+		// Copy entry from the stack
+		StackEntry cur_stack = stack[top];
+
+		// Validate parent
+		const Node &node = mAllocator->Get(cur_stack.mNodeIndex);
+		JPH_ASSERT(node.mParentNodeIndex == cur_stack.mParentNodeIndex);
+
+		// Validate that when a parent is not-changed that all of its children are also
+		JPH_ASSERT(cur_stack.mParentNodeIndex == cInvalidNodeIndex || mAllocator->Get(cur_stack.mParentNodeIndex).mIsChanged || !node.mIsChanged);
+
+		// Loop children
+		for (uint32 i = 0; i < 4; ++i)
+		{
+			NodeID child_node_id = node.mChildNodeID[i];
+			if (child_node_id.IsValid())
+			{
+				if (child_node_id.IsNode())
+				{
+					// Child is a node, recurse
+					uint32 child_idx = child_node_id.GetNodeIndex();
+					JPH_ASSERT(top < cStackSize);
+					StackEntry &new_entry = stack[top++];
+					new_entry.mNodeIndex = child_idx;
+					new_entry.mParentNodeIndex = cur_stack.mNodeIndex;
+
+					// Validate that the bounding box is bigger or equal to the bounds in the tree
+					// Bounding box could also be invalid if all children of our child were removed
+					AABox child_bounds;
+					node.GetChildBounds(i, child_bounds);
+					AABox real_child_bounds;
+					mAllocator->Get(child_idx).GetNodeBounds(real_child_bounds);
+					JPH_ASSERT(child_bounds.Contains(real_child_bounds) || !real_child_bounds.IsValid());
+				}
+				else
+				{
+					// Increment number of bodies found
+					++num_bodies;
+
+					// Check if tracker matches position of body
+					uint32 node_idx, child_idx;
+					GetBodyLocation(inTracking, child_node_id.GetBodyID(), node_idx, child_idx);
+					JPH_ASSERT(node_idx == cur_stack.mNodeIndex);
+					JPH_ASSERT(child_idx == i);
+
+					// Validate that the body bounds are bigger or equal to the bounds in the tree
+					AABox body_bounds;
+					node.GetChildBounds(i, body_bounds);
+					const Body *body = inBodies[child_node_id.GetBodyID().GetIndex()];
+					AABox cached_body_bounds = body->GetWorldSpaceBounds();
+					AABox real_body_bounds = body->GetShape()->GetWorldSpaceBounds(body->GetCenterOfMassTransform(), Vec3::sReplicate(1.0f));
+					JPH_ASSERT(cached_body_bounds == real_body_bounds); // Check that cached body bounds are up to date
+					JPH_ASSERT(body_bounds.Contains(real_body_bounds));
+				}
+			}
+		}
+		--top;
+	}
+	while (top >= 0);
+
+	// Check that the amount of bodies in the tree matches our counter
+	JPH_ASSERT(num_bodies == inNumExpectedBodies);
+}
+
+#endif
+
+#ifdef JPH_DUMP_BROADPHASE_TREE
+
+void QuadTree::DumpTree(const NodeID &inRoot, const char *inFileNamePrefix) const
+{
+	// Open DOT file
+	std::ofstream f;
+	f.open(StringFormat("%s.dot", inFileNamePrefix).c_str(), std::ofstream::out | std::ofstream::trunc);
+	if (!f.is_open())
+		return;
+
+	// Write header
+	f << "digraph {\n";
+
+	// Iterate the entire tree
+	NodeID node_stack[cStackSize];
+	node_stack[0] = inRoot;
+	JPH_ASSERT(node_stack[0].IsValid());
+	int top = 0;
+	do
+	{
+		// Check if node is a body
+		NodeID node_id = node_stack[top];
+		if (node_id.IsBody())
+		{
+			// Output body
+			String body_id = ConvertToString(node_id.GetBodyID().GetIndex());
+			f << "body" << body_id << "[label = \"Body " << body_id << "\"]\n";
+		}
+		else
+		{
+			// Process normal node
+			uint32 node_idx = node_id.GetNodeIndex();
+			const Node &node = mAllocator->Get(node_idx);
+
+			// Get bounding box
+			AABox bounds;
+			node.GetNodeBounds(bounds);
+
+			// Output node
+			String node_str = ConvertToString(node_idx);
+			f << "node" << node_str << "[label = \"Node " << node_str << "\nVolume: " << ConvertToString(bounds.GetVolume()) << "\" color=" << (node.mIsChanged? "red" : "black") << "]\n";
+
+			// Recurse and get all children
+			for (NodeID child_node_id : node.mChildNodeID)
+				if (child_node_id.IsValid())
+				{
+					JPH_ASSERT(top < cStackSize);
+					node_stack[top] = child_node_id;
+					top++;
+
+					// Output link
+					f << "node" << node_str << " -> ";
+					if (child_node_id.IsBody())
+						f << "body" << ConvertToString(child_node_id.GetBodyID().GetIndex());
+					else
+						f << "node" << ConvertToString(child_node_id.GetNodeIndex());
+					f << "\n";
+				}
+		}
+		--top;
+	}
+	while (top >= 0);
+
+	// Finish DOT file
+	f << "}\n";
+	f.close();
+
+	// Convert to svg file
+	String cmd = StringFormat("dot %s.dot -Tsvg -o %s.svg", inFileNamePrefix, inFileNamePrefix);
+	system(cmd.c_str());
+}
+
+#endif // JPH_DUMP_BROADPHASE_TREE
+
+#ifdef JPH_TRACK_BROADPHASE_STATS
+
+uint64 QuadTree::GetTicks100Pct(const LayerToStats &inLayer) const
+{
+	uint64 total_ticks = 0;
+	for (const LayerToStats::value_type &kv : inLayer)
+		total_ticks += kv.second.mTotalTicks;
+	return total_ticks;
+}
+
+void QuadTree::ReportStats(const char *inName, const LayerToStats &inLayer, uint64 inTicks100Pct) const
+{
+	for (const LayerToStats::value_type &kv : inLayer)
+	{
+		double total_pct = 100.0 * double(kv.second.mTotalTicks) / double(inTicks100Pct);
+		double total_pct_excl_collector = 100.0 * double(kv.second.mTotalTicks - kv.second.mCollectorTicks) / double(inTicks100Pct);
+		double hits_reported_vs_bodies_visited = kv.second.mBodiesVisited > 0? 100.0 * double(kv.second.mHitsReported) / double(kv.second.mBodiesVisited) : 100.0;
+		double hits_reported_vs_nodes_visited = kv.second.mNodesVisited > 0? double(kv.second.mHitsReported) / double(kv.second.mNodesVisited) : -1.0;
+
+		std::stringstream str;
+		str << inName << ", " << kv.first << ", " << mName << ", " << kv.second.mNumQueries << ", " << total_pct << ", " << total_pct_excl_collector << ", " << kv.second.mNodesVisited << ", " << kv.second.mBodiesVisited << ", " << kv.second.mHitsReported << ", " << hits_reported_vs_bodies_visited << ", " << hits_reported_vs_nodes_visited;
+		Trace(str.str().c_str());
+	}
+}
+
+uint64 QuadTree::GetTicks100Pct() const
+{
+	uint64 total_ticks = 0;
+	total_ticks += GetTicks100Pct(mCastRayStats);
+	total_ticks += GetTicks100Pct(mCollideAABoxStats);
+	total_ticks += GetTicks100Pct(mCollideSphereStats);
+	total_ticks += GetTicks100Pct(mCollidePointStats);
+	total_ticks += GetTicks100Pct(mCollideOrientedBoxStats);
+	total_ticks += GetTicks100Pct(mCastAABoxStats);
+	return total_ticks;
+}
+
+void QuadTree::ReportStats(uint64 inTicks100Pct) const
+{
+	unique_lock lock(mStatsMutex);
+	ReportStats("RayCast", mCastRayStats, inTicks100Pct);
+	ReportStats("CollideAABox", mCollideAABoxStats, inTicks100Pct);
+	ReportStats("CollideSphere", mCollideSphereStats, inTicks100Pct);
+	ReportStats("CollidePoint", mCollidePointStats, inTicks100Pct);
+	ReportStats("CollideOrientedBox", mCollideOrientedBoxStats, inTicks100Pct);
+	ReportStats("CastAABox", mCastAABoxStats, inTicks100Pct);
+}
+
+#endif // JPH_TRACK_BROADPHASE_STATS
+
+uint QuadTree::GetMaxTreeDepth(const NodeID &inNodeID) const
+{
+	// Reached a leaf?
+	if (!inNodeID.IsValid() || inNodeID.IsBody())
+		return 0;
+
+	// Recurse to children
+	uint max_depth = 0;
+	const Node &node = mAllocator->Get(inNodeID.GetNodeIndex());
+	for (NodeID child_node_id : node.mChildNodeID)
+		max_depth = max(max_depth, GetMaxTreeDepth(child_node_id));
+	return max_depth + 1;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.h
new file mode 100644
index 0000000000..d698beb5f3
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/BroadPhase/QuadTree.h
@@ -0,0 +1,390 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/FixedSizeFreeList.h>
+#include <Jolt/Core/Atomics.h>
+#include <Jolt/Core/NonCopyable.h>
+#include <Jolt/Physics/Body/BodyManager.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhase.h>
+
+//#define JPH_DUMP_BROADPHASE_TREE
+
+JPH_NAMESPACE_BEGIN
+
+/// Internal tree structure in broadphase, is essentially a quad AABB tree.
+/// Tree is lockless (except for UpdatePrepare/Finalize() function), modifying objects in the tree will widen the aabbs of parent nodes to make the node fit.
+/// During the UpdatePrepare/Finalize() call the tree is rebuilt to achieve a tight fit again.
+class JPH_EXPORT QuadTree : public NonCopyable
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+private:
+	// Forward declare
+	class AtomicNodeID;
+
+	/// Class that points to either a body or a node in the tree
+	class NodeID
+	{
+	public:
+		JPH_OVERRIDE_NEW_DELETE
+
+		/// Default constructor does not initialize
+		inline					NodeID() = default;
+
+		/// Construct a node ID
+		static inline NodeID	sInvalid()							{ return NodeID(cInvalidNodeIndex); }
+		static inline NodeID	sFromBodyID(BodyID inID)			{ NodeID node_id(inID.GetIndexAndSequenceNumber()); JPH_ASSERT(node_id.IsBody()); return node_id; }
+		static inline NodeID	sFromNodeIndex(uint32 inIdx)		{ NodeID node_id(inIdx | cIsNode); JPH_ASSERT(node_id.IsNode()); return node_id; }
+
+		/// Check what type of ID it is
+		inline bool				IsValid() const						{ return mID != cInvalidNodeIndex; }
+		inline bool				IsBody() const						{ return (mID & cIsNode) == 0; }
+		inline bool				IsNode() const						{ return (mID & cIsNode) != 0; }
+
+		/// Get body or node index
+		inline BodyID			GetBodyID() const					{ JPH_ASSERT(IsBody()); return BodyID(mID); }
+		inline uint32			GetNodeIndex() const				{ JPH_ASSERT(IsNode()); return mID & ~cIsNode; }
+
+		/// Comparison
+		inline bool				operator == (const BodyID &inRHS) const { return mID == inRHS.GetIndexAndSequenceNumber(); }
+		inline bool				operator == (const NodeID &inRHS) const	{ return mID == inRHS.mID; }
+
+	private:
+		friend class AtomicNodeID;
+
+		inline explicit			NodeID(uint32 inID)					: mID(inID) { }
+
+		static const uint32		cIsNode = BodyID::cBroadPhaseBit;	///< If this bit is set it means that the ID refers to a node, otherwise it refers to a body
+
+		uint32					mID;
+	};
+
+	static_assert(sizeof(NodeID) == sizeof(BodyID), "Body id's should have the same size as NodeIDs");
+
+	/// A NodeID that uses atomics to store the value
+	class AtomicNodeID
+	{
+	public:
+		/// Constructor
+								AtomicNodeID() = default;
+		explicit				AtomicNodeID(const NodeID &inRHS)			: mID(inRHS.mID) { }
+
+		/// Assignment
+		inline void				operator = (const NodeID &inRHS)			{ mID = inRHS.mID; }
+
+		/// Getting the value
+		inline					operator NodeID () const					{ return NodeID(mID); }
+
+		/// Check if the ID is valid
+		inline bool				IsValid() const								{ return mID != cInvalidNodeIndex; }
+
+		/// Comparison
+		inline bool				operator == (const BodyID &inRHS) const		{ return mID == inRHS.GetIndexAndSequenceNumber(); }
+		inline bool				operator == (const NodeID &inRHS) const		{ return mID == inRHS.mID; }
+
+		/// Atomically compare and swap value. Expects inOld value, replaces with inNew value or returns false
+		inline bool				CompareExchange(NodeID inOld, NodeID inNew)	{ return mID.compare_exchange_strong(inOld.mID, inNew.mID); }
+
+	private:
+		atomic<uint32>			mID;
+	};
+
+	/// Class that represents a node in the tree
+	class Node
+	{
+	public:
+		/// Construct node
+		explicit				Node(bool inIsChanged);
+
+		/// Get bounding box encapsulating all children
+		void					GetNodeBounds(AABox &outBounds) const;
+
+		/// Get bounding box in a consistent way with the functions below (check outBounds.IsValid() before using the box)
+		void					GetChildBounds(int inChildIndex, AABox &outBounds) const;
+
+		/// Set the bounds in such a way that other threads will either see a fully correct bounding box or a bounding box with no volume
+		void					SetChildBounds(int inChildIndex, const AABox &inBounds);
+
+		/// Invalidate bounding box in such a way that other threads will not temporarily see a very large bounding box
+		void					InvalidateChildBounds(int inChildIndex);
+
+		/// Encapsulate inBounds in node bounds, returns true if there were changes
+		bool					EncapsulateChildBounds(int inChildIndex, const AABox &inBounds);
+
+		/// Bounding box for child nodes or bodies (all initially set to invalid so no collision test will ever traverse to the leaf)
+		atomic<float>			mBoundsMinX[4];
+		atomic<float>			mBoundsMinY[4];
+		atomic<float>			mBoundsMinZ[4];
+		atomic<float>			mBoundsMaxX[4];
+		atomic<float>			mBoundsMaxY[4];
+		atomic<float>			mBoundsMaxZ[4];
+
+		/// Index of child node or body ID.
+		AtomicNodeID			mChildNodeID[4];
+
+		/// Index of the parent node.
+		/// Note: This value is unreliable during the UpdatePrepare/Finalize() function as a node may be relinked to the newly built tree.
+		atomic<uint32>			mParentNodeIndex = cInvalidNodeIndex;
+
+		/// If this part of the tree has changed, if not, we will treat this sub tree as a single body during the UpdatePrepare/Finalize().
+		/// If any changes are made to an object inside this sub tree then the direct path from the body to the top of the tree will become changed.
+		atomic<uint32>			mIsChanged;
+
+		// Padding to align to 124 bytes
+		uint32					mPadding = 0;
+	};
+
+	// Maximum size of the stack during tree walk
+	static constexpr int		cStackSize = 128;
+
+	static_assert(sizeof(atomic<float>) == 4, "Assuming that an atomic doesn't add any additional storage");
+	static_assert(sizeof(atomic<uint32>) == 4, "Assuming that an atomic doesn't add any additional storage");
+	static_assert(std::is_trivially_destructible<Node>(), "Assuming that we don't have a destructor");
+
+public:
+	/// Class that allocates tree nodes, can be shared between multiple trees
+	using Allocator = FixedSizeFreeList<Node>;
+
+	static_assert(Allocator::ObjectStorageSize == 128, "Node should be 128 bytes");
+
+	/// Data to track location of a Body in the tree
+	struct Tracking
+	{
+		/// Constructor to satisfy the vector class
+								Tracking() = default;
+								Tracking(const Tracking &inRHS) : mBroadPhaseLayer(inRHS.mBroadPhaseLayer.load()), mObjectLayer(inRHS.mObjectLayer.load()), mBodyLocation(inRHS.mBodyLocation.load()) { }
+
+		/// Invalid body location identifier
+		static const uint32		cInvalidBodyLocation = 0xffffffff;
+
+		atomic<BroadPhaseLayer::Type> mBroadPhaseLayer = (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid;
+		atomic<ObjectLayer>		mObjectLayer = cObjectLayerInvalid;
+		atomic<uint32>			mBodyLocation { cInvalidBodyLocation };
+	};
+
+	using TrackingVector = Array<Tracking>;
+
+	/// Destructor
+								~QuadTree();
+
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+	/// Name of the tree for debugging purposes
+	void						SetName(const char *inName)			{ mName = inName; }
+	inline const char *			GetName() const						{ return mName; }
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+
+	/// Check if there is anything in the tree
+	inline bool					HasBodies() const					{ return mNumBodies != 0; }
+
+	/// Check if the tree needs an UpdatePrepare/Finalize()
+	inline bool					IsDirty() const						{ return mIsDirty; }
+
+	/// Check if this tree can get an UpdatePrepare/Finalize() or if it needs a DiscardOldTree() first
+	inline bool					CanBeUpdated() const				{ return mFreeNodeBatch.mNumObjects == 0; }
+
+	/// Initialization
+	void						Init(Allocator &inAllocator);
+
+	struct UpdateState
+	{
+		NodeID					mRootNodeID;						///< This will be the new root node id
+	};
+
+	/// Will throw away the previous frame's nodes so that we can start building a new tree in the background
+	void						DiscardOldTree();
+
+	/// Get the bounding box for this tree
+	AABox						GetBounds() const;
+
+	/// Update the broadphase, needs to be called regularly to achieve a tight fit of the tree when bodies have been modified.
+	/// UpdatePrepare() will build the tree, UpdateFinalize() will lock the root of the tree shortly and swap the trees and afterwards clean up temporary data structures.
+	void						UpdatePrepare(const BodyVector &inBodies, TrackingVector &ioTracking, UpdateState &outUpdateState, bool inFullRebuild);
+	void						UpdateFinalize(const BodyVector &inBodies, const TrackingVector &inTracking, const UpdateState &inUpdateState);
+
+	/// Temporary data structure to pass information between AddBodiesPrepare and AddBodiesFinalize/Abort
+	struct AddState
+	{
+		NodeID					mLeafID = NodeID::sInvalid();
+		AABox					mLeafBounds;
+	};
+
+	/// Prepare adding inNumber bodies at ioBodyIDs to the quad tree, returns the state in outState that should be used in AddBodiesFinalize.
+	/// This can be done on a background thread without influencing the broadphase.
+	/// ioBodyIDs may be shuffled around by this function.
+	void						AddBodiesPrepare(const BodyVector &inBodies, TrackingVector &ioTracking, BodyID *ioBodyIDs, int inNumber, AddState &outState);
+
+	/// Finalize adding bodies to the quadtree, supply the same number of bodies as in AddBodiesPrepare.
+	void						AddBodiesFinalize(TrackingVector &ioTracking, int inNumberBodies, const AddState &inState);
+
+	/// Abort adding bodies to the quadtree, supply the same bodies and state as in AddBodiesPrepare.
+	/// This can be done on a background thread without influencing the broadphase.
+	void						AddBodiesAbort(TrackingVector &ioTracking, const AddState &inState);
+
+	/// Remove inNumber bodies in ioBodyIDs from the quadtree.
+	void						RemoveBodies(const BodyVector &inBodies, TrackingVector &ioTracking, const BodyID *ioBodyIDs, int inNumber);
+
+	/// Call whenever the aabb of a body changes.
+	void						NotifyBodiesAABBChanged(const BodyVector &inBodies, const TrackingVector &inTracking, const BodyID *ioBodyIDs, int inNumber);
+
+	/// Cast a ray and get the intersecting bodies in ioCollector.
+	void						CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
+
+	/// Get bodies intersecting with inBox in ioCollector
+	void						CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
+
+	/// Get bodies intersecting with a sphere in ioCollector
+	void						CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
+
+	/// Get bodies intersecting with a point and any hits to ioCollector
+	void						CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
+
+	/// Get bodies intersecting with an oriented box and any hits to ioCollector
+	void						CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
+
+	/// Cast a box and get intersecting bodies in ioCollector
+	void						CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const;
+
+	/// Find all colliding pairs between dynamic bodies, calls ioPairCollector for every pair found
+	void						FindCollidingPairs(const BodyVector &inBodies, const BodyID *inActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, BodyPairCollector &ioPairCollector, const ObjectLayerPairFilter &inObjectLayerPairFilter) const;
+
+#ifdef JPH_TRACK_BROADPHASE_STATS
+	/// Sum up all the ticks spent in the various layers
+	uint64						GetTicks100Pct() const;
+
+	/// Trace the stats of this tree to the TTY
+	void						ReportStats(uint64 inTicks100Pct) const;
+#endif // JPH_TRACK_BROADPHASE_STATS
+
+private:
+	/// Constants
+	static const uint32			cInvalidNodeIndex = 0xffffffff;		///< Value used to indicate node index is invalid
+	static const float			cLargeFloat;						///< A large floating point number that is small enough to not cause any overflows
+	static const AABox			cInvalidBounds;						///< Invalid bounding box using cLargeFloat
+
+	/// We alternate between two trees in order to let collision queries complete in parallel to adding/removing objects to the tree
+	struct RootNode
+	{
+		/// Get the ID of the root node
+		inline NodeID			GetNodeID() const					{ return NodeID::sFromNodeIndex(mIndex); }
+
+		/// Index of the root node of the tree (this is always a node, never a body id)
+		atomic<uint32>			mIndex { cInvalidNodeIndex };
+	};
+
+	/// Caches location of body inBodyID in the tracker, body can be found in mNodes[inNodeIdx].mChildNodeID[inChildIdx]
+	void						GetBodyLocation(const TrackingVector &inTracking, BodyID inBodyID, uint32 &outNodeIdx, uint32 &outChildIdx) const;
+	void						SetBodyLocation(TrackingVector &ioTracking, BodyID inBodyID, uint32 inNodeIdx, uint32 inChildIdx) const;
+	static void					sInvalidateBodyLocation(TrackingVector &ioTracking, BodyID inBodyID);
+
+	/// Get the current root of the tree
+	JPH_INLINE const RootNode &	GetCurrentRoot() const				{ return mRootNode[mRootNodeIndex]; }
+	JPH_INLINE RootNode &		GetCurrentRoot()					{ return mRootNode[mRootNodeIndex]; }
+
+	/// Depending on if inNodeID is a body or tree node return the bounding box
+	inline AABox				GetNodeOrBodyBounds(const BodyVector &inBodies, NodeID inNodeID) const;
+
+	/// Mark node and all of its parents as changed
+	inline void					MarkNodeAndParentsChanged(uint32 inNodeIndex);
+
+	/// Widen parent bounds of node inNodeIndex to encapsulate inNewBounds, also mark node and all of its parents as changed
+	inline void					WidenAndMarkNodeAndParentsChanged(uint32 inNodeIndex, const AABox &inNewBounds);
+
+	/// Allocate a new node
+	inline uint32				AllocateNode(bool inIsChanged);
+
+	/// Try to insert a new leaf to the tree at inNodeIndex
+	inline bool					TryInsertLeaf(TrackingVector &ioTracking, int inNodeIndex, NodeID inLeafID, const AABox &inLeafBounds, int inLeafNumBodies);
+
+	/// Try to replace the existing root with a new root that contains both the existing root and the new leaf
+	inline bool					TryCreateNewRoot(TrackingVector &ioTracking, atomic<uint32> &ioRootNodeIndex, NodeID inLeafID, const AABox &inLeafBounds, int inLeafNumBodies);
+
+	/// Build a tree for ioBodyIDs, returns the NodeID of the root (which will be the ID of a single body if inNumber = 1). All tree levels up to inMaxDepthMarkChanged will be marked as 'changed'.
+	NodeID						BuildTree(const BodyVector &inBodies, TrackingVector &ioTracking, NodeID *ioNodeIDs, int inNumber, uint inMaxDepthMarkChanged, AABox &outBounds);
+
+	/// Sorts ioNodeIDs spatially into 2 groups. Second groups starts at ioNodeIDs + outMidPoint.
+	/// After the function returns ioNodeIDs and ioNodeCenters will be shuffled
+	static void					sPartition(NodeID *ioNodeIDs, Vec3 *ioNodeCenters, int inNumber, int &outMidPoint);
+
+	/// Sorts ioNodeIDs from inBegin to (but excluding) inEnd spatially into 4 groups.
+	/// outSplit needs to be 5 ints long, when the function returns each group runs from outSplit[i] to (but excluding) outSplit[i + 1]
+	/// After the function returns ioNodeIDs and ioNodeCenters will be shuffled
+	static void					sPartition4(NodeID *ioNodeIDs, Vec3 *ioNodeCenters, int inBegin, int inEnd, int *outSplit);
+
+#ifdef JPH_DEBUG
+	/// Validate that the tree is consistent.
+	/// Note: This function only works if the tree is not modified while we're traversing it.
+	void						ValidateTree(const BodyVector &inBodies, const TrackingVector &inTracking, uint32 inNodeIndex, uint32 inNumExpectedBodies) const;
+#endif
+
+#ifdef JPH_DUMP_BROADPHASE_TREE
+	/// Dump the tree in DOT format (see: https://graphviz.org/)
+	void						DumpTree(const NodeID &inRoot, const char *inFileNamePrefix) const;
+#endif
+
+	/// Allocator that controls adding / freeing nodes
+	Allocator *					mAllocator = nullptr;
+
+	/// This is a list of nodes that must be deleted after the trees are swapped and the old tree is no longer in use
+	Allocator::Batch			mFreeNodeBatch;
+
+	/// Number of bodies currently in the tree
+	/// This is aligned to be in a different cache line from the `Allocator` pointer to prevent cross-thread syncs
+	/// when reading nodes.
+	alignas(JPH_CACHE_LINE_SIZE) atomic<uint32> mNumBodies { 0 };
+
+	/// We alternate between two tree root nodes. When updating, we activate the new tree and we keep the old tree alive.
+	/// for queries that are in progress until the next time DiscardOldTree() is called.
+	RootNode					mRootNode[2];
+	atomic<uint32>				mRootNodeIndex { 0 };
+
+	/// Flag to keep track of changes to the broadphase, if false, we don't need to UpdatePrepare/Finalize()
+	atomic<bool>				mIsDirty = false;
+
+#ifdef JPH_TRACK_BROADPHASE_STATS
+	/// Mutex protecting the various LayerToStats members
+	mutable Mutex				mStatsMutex;
+
+	struct Stat
+	{
+		uint64					mNumQueries = 0;
+		uint64					mNodesVisited = 0;
+		uint64					mBodiesVisited = 0;
+		uint64					mHitsReported = 0;
+		uint64					mTotalTicks = 0;
+		uint64					mCollectorTicks = 0;
+	};
+
+	using LayerToStats = UnorderedMap<String, Stat>;
+
+	/// Sum up all the ticks in a layer
+	uint64						GetTicks100Pct(const LayerToStats &inLayer) const;
+
+	/// Trace the stats of a single query type to the TTY
+	void						ReportStats(const char *inName, const LayerToStats &inLayer, uint64 inTicks100Pct) const;
+
+	mutable LayerToStats		mCastRayStats;
+	mutable LayerToStats		mCollideAABoxStats;
+	mutable LayerToStats		mCollideSphereStats;
+	mutable LayerToStats		mCollidePointStats;
+	mutable LayerToStats		mCollideOrientedBoxStats;
+	mutable LayerToStats		mCastAABoxStats;
+#endif // JPH_TRACK_BROADPHASE_STATS
+
+	/// Debug function to get the depth of the tree from node inNodeID
+	uint						GetMaxTreeDepth(const NodeID &inNodeID) const;
+
+	/// Walk the node tree calling the Visitor::VisitNodes for each node encountered and Visitor::VisitBody for each body encountered
+	template <class Visitor>
+	JPH_INLINE void				WalkTree(const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking, Visitor &ioVisitor JPH_IF_TRACK_BROADPHASE_STATS(, LayerToStats &ioStats)) const;
+
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+	/// Name of this tree for debugging purposes
+	const char *				mName = "Layer";
+#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.cpp
new file mode 100644
index 0000000000..a69208a04d
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.cpp
@@ -0,0 +1,109 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/CastConvexVsTriangles.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/ActiveEdges.h>
+#include <Jolt/Physics/Collision/NarrowPhaseStats.h>
+#include <Jolt/Geometry/EPAPenetrationDepth.h>
+
+JPH_NAMESPACE_BEGIN
+
+CastConvexVsTriangles::CastConvexVsTriangles(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, CastShapeCollector &ioCollector) :
+	mShapeCast(inShapeCast),
+	mShapeCastSettings(inShapeCastSettings),
+	mCenterOfMassTransform2(inCenterOfMassTransform2),
+	mScale(inScale),
+	mSubShapeIDCreator1(inSubShapeIDCreator1),
+	mCollector(ioCollector)
+{
+	JPH_ASSERT(inShapeCast.mShape->GetType() == EShapeType::Convex);
+
+	// Determine if shape is inside out or not
+	mScaleSign = ScaleHelpers::IsInsideOut(inScale)? -1.0f : 1.0f;
+}
+
+void CastConvexVsTriangles::Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Scale triangle
+	Vec3 v0 = mScale * inV0;
+	Vec3 v1 = mScale * inV1;
+	Vec3 v2 = mScale * inV2;
+
+	// Calculate triangle normal
+	Vec3 triangle_normal = mScaleSign * (v1 - v0).Cross(v2 - v0);
+
+	// Backface check
+	bool back_facing = triangle_normal.Dot(mShapeCast.mDirection) > 0.0f;
+	if (mShapeCastSettings.mBackFaceModeTriangles == EBackFaceMode::IgnoreBackFaces && back_facing)
+		return;
+
+	// Create triangle support function
+	TriangleConvexSupport triangle { v0, v1, v2 };
+
+	// Check if we already created the cast shape support function
+	if (mSupport == nullptr)
+	{
+		// Determine if we want to use the actual shape or a shrunken shape with convex radius
+		ConvexShape::ESupportMode support_mode = mShapeCastSettings.mUseShrunkenShapeAndConvexRadius? ConvexShape::ESupportMode::ExcludeConvexRadius : ConvexShape::ESupportMode::Default;
+
+		// Create support function
+		mSupport = static_cast<const ConvexShape *>(mShapeCast.mShape)->GetSupportFunction(support_mode, mSupportBuffer, mShapeCast.mScale);
+	}
+
+	EPAPenetrationDepth epa;
+	float fraction = mCollector.GetEarlyOutFraction();
+	Vec3 contact_point_a, contact_point_b, contact_normal;
+	if (epa.CastShape(mShapeCast.mCenterOfMassStart, mShapeCast.mDirection, mShapeCastSettings.mCollisionTolerance, mShapeCastSettings.mPenetrationTolerance, *mSupport, triangle, mSupport->GetConvexRadius(), 0.0f, mShapeCastSettings.mReturnDeepestPoint, fraction, contact_point_a, contact_point_b, contact_normal))
+	{
+		// Check if we have enabled active edge detection
+		if (mShapeCastSettings.mActiveEdgeMode == EActiveEdgeMode::CollideOnlyWithActive && inActiveEdges != 0b111)
+		{
+			// Convert the active edge velocity hint to local space
+			Vec3 active_edge_movement_direction = mCenterOfMassTransform2.Multiply3x3Transposed(mShapeCastSettings.mActiveEdgeMovementDirection);
+
+			// Update the contact normal to account for active edges
+			// Note that we flip the triangle normal as the penetration axis is pointing towards the triangle instead of away
+			contact_normal = ActiveEdges::FixNormal(v0, v1, v2, back_facing? triangle_normal : -triangle_normal, inActiveEdges, contact_point_b, contact_normal, active_edge_movement_direction);
+		}
+
+		// Convert to world space
+		contact_point_a = mCenterOfMassTransform2 * contact_point_a;
+		contact_point_b = mCenterOfMassTransform2 * contact_point_b;
+		Vec3 contact_normal_world = mCenterOfMassTransform2.Multiply3x3(contact_normal);
+
+		// Its a hit, store the sub shape id's
+		ShapeCastResult result(fraction, contact_point_a, contact_point_b, contact_normal_world, back_facing, mSubShapeIDCreator1.GetID(), inSubShapeID2, TransformedShape::sGetBodyID(mCollector.GetContext()));
+
+		// Early out if this hit is deeper than the collector's early out value
+		if (fraction == 0.0f && -result.mPenetrationDepth >= mCollector.GetEarlyOutFraction())
+			return;
+
+		// Gather faces
+		if (mShapeCastSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces)
+		{
+			// Get supporting face of shape 1
+			Mat44 transform_1_to_2 = mShapeCast.mCenterOfMassStart;
+			transform_1_to_2.SetTranslation(transform_1_to_2.GetTranslation() + fraction * mShapeCast.mDirection);
+			static_cast<const ConvexShape *>(mShapeCast.mShape)->GetSupportingFace(SubShapeID(), transform_1_to_2.Multiply3x3Transposed(-contact_normal), mShapeCast.mScale, mCenterOfMassTransform2 * transform_1_to_2, result.mShape1Face);
+
+			// Get face of the triangle
+			triangle.GetSupportingFace(contact_normal, result.mShape2Face);
+
+			// Convert to world space
+			for (Vec3 &p : result.mShape2Face)
+				p = mCenterOfMassTransform2 * p;
+		}
+
+		JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;)
+		mCollector.AddHit(result);
+	}
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.h
new file mode 100644
index 0000000000..6a6c161cd2
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.h
@@ -0,0 +1,46 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/ConvexShape.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Collision detection helper that casts a convex object vs one or more triangles
+class JPH_EXPORT CastConvexVsTriangles
+{
+public:
+	/// Constructor
+	/// @param inShapeCast The shape to cast against the triangles and its start and direction
+	/// @param inShapeCastSettings Settings for performing the cast
+	/// @param inScale Local space scale for the shape to cast against (scales relative to its center of mass).
+	/// @param inCenterOfMassTransform2 Is the center of mass transform of shape 2 (excluding scale), this is used to provide a transform to the shape cast result so that local quantities can be transformed into world space.
+	/// @param inSubShapeIDCreator1 Class that tracks the current sub shape ID for the casting shape
+	/// @param ioCollector The collector that receives the results.
+									CastConvexVsTriangles(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, CastShapeCollector &ioCollector);
+
+	/// Cast convex object with a single triangle
+	/// @param inV0 , inV1 , inV2: CCW triangle vertices
+	/// @param inActiveEdges bit 0 = edge v0..v1 is active, bit 1 = edge v1..v2 is active, bit 2 = edge v2..v0 is active
+	/// An active edge is an edge that is not connected to another triangle in such a way that it is impossible to collide with the edge
+	/// @param inSubShapeID2 The sub shape ID for the triangle
+	void							Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2);
+
+protected:
+	const ShapeCast &				mShapeCast;
+	const ShapeCastSettings &		mShapeCastSettings;
+	const Mat44 &					mCenterOfMassTransform2;
+	Vec3							mScale;
+	SubShapeIDCreator				mSubShapeIDCreator1;
+	CastShapeCollector &			mCollector;
+
+private:
+	ConvexShape::SupportBuffer		mSupportBuffer;						///< Buffer that holds the support function of the cast shape
+	const ConvexShape::Support *	mSupport = nullptr;					///< Support function of the cast shape
+	float							mScaleSign;							///< Sign of the scale, -1 if object is inside out, 1 if not
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CastResult.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastResult.h
new file mode 100644
index 0000000000..6bb49feb7c
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastResult.h
@@ -0,0 +1,37 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/BodyID.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Structure that holds a ray cast or other object cast hit
+class BroadPhaseCastResult
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Function required by the CollisionCollector. A smaller fraction is considered to be a 'better hit'. For rays/cast shapes we can just use the collision fraction.
+	inline float	GetEarlyOutFraction() const			{ return mFraction; }
+
+	/// Reset this result so it can be reused for a new cast.
+	inline void		Reset()								{ mBodyID = BodyID(); mFraction = 1.0f + FLT_EPSILON; }
+
+	BodyID			mBodyID;							///< Body that was hit
+	float			mFraction = 1.0f + FLT_EPSILON;		///< Hit fraction of the ray/object [0, 1], HitPoint = Start + mFraction * (End - Start)
+};
+
+/// Specialization of cast result against a shape
+class RayCastResult : public BroadPhaseCastResult
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	SubShapeID		mSubShapeID2;						///< Sub shape ID of shape that we collided against
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CastSphereVsTriangles.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastSphereVsTriangles.cpp
new file mode 100644
index 0000000000..166883be8d
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastSphereVsTriangles.cpp
@@ -0,0 +1,223 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/CastSphereVsTriangles.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/ActiveEdges.h>
+#include <Jolt/Physics/Collision/NarrowPhaseStats.h>
+#include <Jolt/Geometry/ClosestPoint.h>
+#include <Jolt/Geometry/RaySphere.h>
+#include <Jolt/Core/Profiler.h>
+
+JPH_NAMESPACE_BEGIN
+
+CastSphereVsTriangles::CastSphereVsTriangles(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, CastShapeCollector &ioCollector) :
+	mStart(inShapeCast.mCenterOfMassStart.GetTranslation()),
+	mDirection(inShapeCast.mDirection),
+	mShapeCastSettings(inShapeCastSettings),
+	mCenterOfMassTransform2(inCenterOfMassTransform2),
+	mScale(inScale),
+	mSubShapeIDCreator1(inSubShapeIDCreator1),
+	mCollector(ioCollector)
+{
+	// Cast to sphere shape
+	JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::Sphere);
+	const SphereShape *sphere = static_cast<const SphereShape *>(inShapeCast.mShape);
+
+	// Scale the radius
+	mRadius = sphere->GetRadius() * abs(inShapeCast.mScale.GetX());
+
+	// Determine if shape is inside out or not
+	mScaleSign = ScaleHelpers::IsInsideOut(inScale)? -1.0f : 1.0f;
+}
+
+void CastSphereVsTriangles::AddHit(bool inBackFacing, const SubShapeID &inSubShapeID2, float inFraction, Vec3Arg inContactPointA, Vec3Arg inContactPointB, Vec3Arg inContactNormal)
+{
+	// Convert to world space
+	Vec3 contact_point_a = mCenterOfMassTransform2 * (mStart + inContactPointA);
+	Vec3 contact_point_b = mCenterOfMassTransform2 * (mStart + inContactPointB);
+	Vec3 contact_normal_world = mCenterOfMassTransform2.Multiply3x3(inContactNormal);
+
+	// Its a hit, store the sub shape id's
+	ShapeCastResult result(inFraction, contact_point_a, contact_point_b, contact_normal_world, inBackFacing, mSubShapeIDCreator1.GetID(), inSubShapeID2, TransformedShape::sGetBodyID(mCollector.GetContext()));
+
+	// Note: We don't gather faces here because that's only useful if both shapes have a face. Since the sphere always has only 1 contact point, the manifold is always a point.
+
+	JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;)
+	mCollector.AddHit(result);
+}
+
+void CastSphereVsTriangles::AddHitWithActiveEdgeDetection(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, bool inBackFacing, Vec3Arg inTriangleNormal, uint8 inActiveEdges, const SubShapeID &inSubShapeID2, float inFraction, Vec3Arg inContactPointA, Vec3Arg inContactPointB, Vec3Arg inContactNormal)
+{
+	// Check if we have enabled active edge detection
+	Vec3 contact_normal = inContactNormal;
+	if (mShapeCastSettings.mActiveEdgeMode == EActiveEdgeMode::CollideOnlyWithActive && inActiveEdges != 0b111)
+	{
+		// Convert the active edge velocity hint to local space
+		Vec3 active_edge_movement_direction = mCenterOfMassTransform2.Multiply3x3Transposed(mShapeCastSettings.mActiveEdgeMovementDirection);
+
+		// Update the contact normal to account for active edges
+		// Note that we flip the triangle normal as the penetration axis is pointing towards the triangle instead of away
+		contact_normal = ActiveEdges::FixNormal(inV0, inV1, inV2, inBackFacing? inTriangleNormal : -inTriangleNormal, inActiveEdges, inContactPointB, inContactNormal, active_edge_movement_direction);
+	}
+
+	AddHit(inBackFacing, inSubShapeID2, inFraction, inContactPointA, inContactPointB, contact_normal);
+}
+
+// This is a simplified version of the ray cylinder test from: Real Time Collision Detection - Christer Ericson
+// Chapter 5.3.7, page 194-197. Some conditions have been removed as we're not interested in hitting the caps of the cylinder.
+// Note that the ray origin is assumed to be the origin here.
+float CastSphereVsTriangles::RayCylinder(Vec3Arg inRayDirection, Vec3Arg inCylinderA, Vec3Arg inCylinderB, float inRadius) const
+{
+	// Calculate cylinder axis
+	Vec3 axis = inCylinderB - inCylinderA;
+
+	// Make ray start relative to cylinder side A (moving cylinder A to the origin)
+	Vec3 start = -inCylinderA;
+
+	// Test if segment is fully on the A side of the cylinder
+	float start_dot_axis = start.Dot(axis);
+	float direction_dot_axis = inRayDirection.Dot(axis);
+	float end_dot_axis = start_dot_axis + direction_dot_axis;
+	if (start_dot_axis < 0.0f && end_dot_axis < 0.0f)
+		return FLT_MAX;
+
+	// Test if segment is fully on the B side of the cylinder
+	float axis_len_sq = axis.LengthSq();
+	if (start_dot_axis > axis_len_sq && end_dot_axis > axis_len_sq)
+		return FLT_MAX;
+
+	// Calculate a, b and c, the factors for quadratic equation
+	// We're basically solving the ray: x = start + direction * t
+	// The closest point to x on the segment A B is: w = (x . axis) * axis / (axis . axis)
+	// The distance between x and w should be radius: (x - w) . (x - w) = radius^2
+	// Solving this gives the following:
+	float a = axis_len_sq * inRayDirection.LengthSq() - Square(direction_dot_axis);
+	if (abs(a) < 1.0e-6f)
+		return FLT_MAX; // Segment runs parallel to cylinder axis, stop processing, we will either hit at fraction = 0 or we'll hit a vertex
+	float b = axis_len_sq * start.Dot(inRayDirection) - direction_dot_axis * start_dot_axis; // should be multiplied by 2, instead we'll divide a and c by 2 when we solve the quadratic equation
+	float c = axis_len_sq * (start.LengthSq() - Square(inRadius)) - Square(start_dot_axis);
+	float det = Square(b) - a * c; // normally 4 * a * c but since both a and c need to be divided by 2 we lose the 4
+	if (det < 0.0f)
+		return FLT_MAX; // No solution to quadractic equation
+
+	// Solve fraction t where the ray hits the cylinder
+	float t = -(b + sqrt(det)) / a; // normally divided by 2 * a but since a should be divided by 2 we lose the 2
+	if (t < 0.0f || t > 1.0f)
+		return FLT_MAX; // Intersection lies outside segment
+	if (start_dot_axis + t * direction_dot_axis < 0.0f || start_dot_axis + t * direction_dot_axis > axis_len_sq)
+		return FLT_MAX; // Intersection outside the end point of the cylinder, stop processing, we will possibly hit a vertex
+	return t;
+}
+
+void CastSphereVsTriangles::Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Scale triangle and make it relative to the start of the cast
+	Vec3 v0 = mScale * inV0 - mStart;
+	Vec3 v1 = mScale * inV1 - mStart;
+	Vec3 v2 = mScale * inV2 - mStart;
+
+	// Calculate triangle normal
+	Vec3 triangle_normal = mScaleSign * (v1 - v0).Cross(v2 - v0);
+	float triangle_normal_len = triangle_normal.Length();
+	if (triangle_normal_len == 0.0f)
+		return; // Degenerate triangle
+	triangle_normal /= triangle_normal_len;
+
+	// Backface check
+	float normal_dot_direction = triangle_normal.Dot(mDirection);
+	bool back_facing = normal_dot_direction > 0.0f;
+	if (mShapeCastSettings.mBackFaceModeTriangles == EBackFaceMode::IgnoreBackFaces && back_facing)
+		return;
+
+	// Test if distance between the sphere and plane of triangle is smaller or equal than the radius
+	if (abs(v0.Dot(triangle_normal)) <= mRadius)
+	{
+		// Check if the sphere intersects at the start of the cast
+		uint32 closest_feature;
+		Vec3 q = ClosestPoint::GetClosestPointOnTriangle(v0, v1, v2, closest_feature);
+		float q_len_sq = q.LengthSq();
+		if (q_len_sq <= Square(mRadius))
+		{
+			// Early out if this hit is deeper than the collector's early out value
+			float q_len = sqrt(q_len_sq);
+			float penetration_depth = mRadius - q_len;
+			if (-penetration_depth >= mCollector.GetEarlyOutFraction())
+				return;
+
+			// Generate contact point
+			Vec3 contact_normal = q_len > 0.0f? q / q_len : Vec3::sAxisY();
+			Vec3 contact_point_a = q + contact_normal * penetration_depth;
+			Vec3 contact_point_b = q;
+			AddHitWithActiveEdgeDetection(v0, v1, v2, back_facing, triangle_normal, inActiveEdges, inSubShapeID2, 0.0f, contact_point_a, contact_point_b, contact_normal);
+			return;
+		}
+	}
+	else
+	{
+		// Check if cast is not parallel to the plane of the triangle
+		float abs_normal_dot_direction = abs(normal_dot_direction);
+		if (abs_normal_dot_direction > 1.0e-6f)
+		{
+			// Calculate the point on the sphere that will hit the triangle's plane first and calculate a fraction where it will do so
+			Vec3 d = Sign(normal_dot_direction) * mRadius * triangle_normal;
+			float plane_intersection = (v0 - d).Dot(triangle_normal) / normal_dot_direction;
+
+			// Check if sphere will hit in the interval that we're interested in
+			if (plane_intersection * abs_normal_dot_direction < -mRadius	// Sphere hits the plane before the sweep, cannot intersect
+				|| plane_intersection >= mCollector.GetEarlyOutFraction())	// Sphere hits the plane after the sweep / early out fraction, cannot intersect
+				return;
+
+			// We can only report an interior hit if we're hitting the plane during our sweep and not before
+			if (plane_intersection >= 0.0f)
+			{
+				// Calculate the point of contact on the plane
+				Vec3 p = d + plane_intersection * mDirection;
+
+				// Check if this is an interior point
+				float u, v, w;
+				if (ClosestPoint::GetBaryCentricCoordinates(v0 - p, v1 - p, v2 - p, u, v, w)
+					&& u >= 0.0f && v >= 0.0f && w >= 0.0f)
+				{
+					// Interior point, we found the collision point. We don't need to check active edges.
+					AddHit(back_facing, inSubShapeID2, plane_intersection, p, p, back_facing? triangle_normal : -triangle_normal);
+					return;
+				}
+			}
+		}
+	}
+
+	// Test 3 edges
+	float fraction = RayCylinder(mDirection, v0, v1, mRadius);
+	fraction = min(fraction, RayCylinder(mDirection, v1, v2, mRadius));
+	fraction = min(fraction, RayCylinder(mDirection, v2, v0, mRadius));
+
+	// Test 3 vertices
+	fraction = min(fraction, RaySphere(Vec3::sZero(), mDirection, v0, mRadius));
+	fraction = min(fraction, RaySphere(Vec3::sZero(), mDirection, v1, mRadius));
+	fraction = min(fraction, RaySphere(Vec3::sZero(), mDirection, v2, mRadius));
+
+	// Check if we have a collision
+	JPH_ASSERT(fraction >= 0.0f);
+	if (fraction < mCollector.GetEarlyOutFraction())
+	{
+		// Calculate the center of the sphere at the point of contact
+		Vec3 p = fraction * mDirection;
+
+		// Get contact point and normal
+		uint32 closest_feature;
+		Vec3 q = ClosestPoint::GetClosestPointOnTriangle(v0 - p, v1 - p, v2 - p, closest_feature);
+		Vec3 contact_normal = q.Normalized();
+		Vec3 contact_point_ab = p + q;
+		AddHitWithActiveEdgeDetection(v0, v1, v2, back_facing, triangle_normal, inActiveEdges, inSubShapeID2, fraction, contact_point_ab, contact_point_ab, contact_normal);
+	}
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CastSphereVsTriangles.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastSphereVsTriangles.h
new file mode 100644
index 0000000000..e37bf68293
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastSphereVsTriangles.h
@@ -0,0 +1,49 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/ShapeCast.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Collision detection helper that casts a sphere vs one or more triangles
+class JPH_EXPORT CastSphereVsTriangles
+{
+public:
+	/// Constructor
+	/// @param inShapeCast The sphere to cast against the triangles and its start and direction
+	/// @param inShapeCastSettings Settings for performing the cast
+	/// @param inScale Local space scale for the shape to cast against (scales relative to its center of mass).
+	/// @param inCenterOfMassTransform2 Is the center of mass transform of shape 2 (excluding scale), this is used to provide a transform to the shape cast result so that local quantities can be transformed into world space.
+	/// @param inSubShapeIDCreator1 Class that tracks the current sub shape ID for the casting shape
+	/// @param ioCollector The collector that receives the results.
+									CastSphereVsTriangles(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, CastShapeCollector &ioCollector);
+
+	/// Cast sphere with a single triangle
+	/// @param inV0 , inV1 , inV2: CCW triangle vertices
+	/// @param inActiveEdges bit 0 = edge v0..v1 is active, bit 1 = edge v1..v2 is active, bit 2 = edge v2..v0 is active
+	/// An active edge is an edge that is not connected to another triangle in such a way that it is impossible to collide with the edge
+	/// @param inSubShapeID2 The sub shape ID for the triangle
+	void							Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2);
+
+protected:
+	Vec3							mStart;								///< Starting location of the sphere
+	Vec3							mDirection;							///< Direction and length of movement of sphere
+	float							mRadius;							///< Scaled radius of sphere
+	const ShapeCastSettings &		mShapeCastSettings;
+	const Mat44 &					mCenterOfMassTransform2;
+	Vec3							mScale;
+	SubShapeIDCreator				mSubShapeIDCreator1;
+	CastShapeCollector &			mCollector;
+
+private:
+	void							AddHit(bool inBackFacing, const SubShapeID &inSubShapeID2, float inFraction, Vec3Arg inContactPointA, Vec3Arg inContactPointB, Vec3Arg inContactNormal);
+	void							AddHitWithActiveEdgeDetection(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, bool inBackFacing, Vec3Arg inTriangleNormal, uint8 inActiveEdges, const SubShapeID &inSubShapeID2, float inFraction, Vec3Arg inContactPointA, Vec3Arg inContactPointB, Vec3Arg inContactNormal);
+	float							RayCylinder(Vec3Arg inRayDirection, Vec3Arg inCylinderA, Vec3Arg inCylinderB, float inRadius) const;
+
+	float							mScaleSign;							///< Sign of the scale, -1 if object is inside out, 1 if not
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollectFacesMode.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollectFacesMode.h
new file mode 100644
index 0000000000..4cac6256db
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollectFacesMode.h
@@ -0,0 +1,16 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Whether or not to collect faces, used by CastShape and CollideShape
+enum class ECollectFacesMode : uint8
+{
+	CollectFaces,										///< mShape1/2Face is desired
+	NoFaces												///< mShape1/2Face is not desired
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp
new file mode 100644
index 0000000000..f00b0023aa
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp
@@ -0,0 +1,150 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/CollideConvexVsTriangles.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/CollideShape.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/ActiveEdges.h>
+#include <Jolt/Physics/Collision/NarrowPhaseStats.h>
+#include <Jolt/Geometry/EPAPenetrationDepth.h>
+#include <Jolt/Geometry/Plane.h>
+
+JPH_NAMESPACE_BEGIN
+
+CollideConvexVsTriangles::CollideConvexVsTriangles(const ConvexShape *inShape1, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeID &inSubShapeID1, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector) :
+	mCollideShapeSettings(inCollideShapeSettings),
+	mCollector(ioCollector),
+	mShape1(inShape1),
+	mScale1(inScale1),
+	mScale2(inScale2),
+	mTransform1(inCenterOfMassTransform1),
+	mSubShapeID1(inSubShapeID1)
+{
+	// Get transforms
+	Mat44 inverse_transform2 = inCenterOfMassTransform2.InversedRotationTranslation();
+	Mat44 transform1_to_2 = inverse_transform2 * inCenterOfMassTransform1;
+	mTransform2To1 = transform1_to_2.InversedRotationTranslation();
+
+	// Calculate bounds
+	mBoundsOf1 = inShape1->GetLocalBounds().Scaled(inScale1);
+	mBoundsOf1.ExpandBy(Vec3::sReplicate(inCollideShapeSettings.mMaxSeparationDistance));
+	mBoundsOf1InSpaceOf2 = mBoundsOf1.Transformed(transform1_to_2);	// Convert bounding box of 1 into space of 2
+
+	// Determine if shape 2 is inside out or not
+	mScaleSign2 = ScaleHelpers::IsInsideOut(inScale2)? -1.0f : 1.0f;
+}
+
+void CollideConvexVsTriangles::Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Scale triangle and transform it to the space of 1
+	Vec3 v0 = mTransform2To1 * (mScale2 * inV0);
+	Vec3 v1 = mTransform2To1 * (mScale2 * inV1);
+	Vec3 v2 = mTransform2To1 * (mScale2 * inV2);
+
+	// Calculate triangle normal
+	Vec3 triangle_normal = mScaleSign2 * (v1 - v0).Cross(v2 - v0);
+
+	// Backface check
+	bool back_facing = triangle_normal.Dot(v0) > 0.0f;
+	if (mCollideShapeSettings.mBackFaceMode == EBackFaceMode::IgnoreBackFaces && back_facing)
+		return;
+
+	// Get bounding box for triangle
+	AABox triangle_bbox = AABox::sFromTwoPoints(v0, v1);
+	triangle_bbox.Encapsulate(v2);
+
+	// Get intersection between triangle and shape box, if there is none, we're done
+	if (!triangle_bbox.Overlaps(mBoundsOf1))
+		return;
+
+	// Create triangle support function
+	TriangleConvexSupport triangle(v0, v1, v2);
+
+	// Perform collision detection
+	// Note: As we don't remember the penetration axis from the last iteration, and it is likely that the shape (A) we're colliding the triangle (B) against is in front of the triangle,
+	// and the penetration axis is the shortest distance along to push B out of collision, we use the inverse of the triangle normal as an initial penetration axis. This has been seen
+	// to improve performance by approx. 5% over using a fixed axis like (1, 0, 0).
+	Vec3 penetration_axis = -triangle_normal, point1, point2;
+	EPAPenetrationDepth pen_depth;
+	EPAPenetrationDepth::EStatus status;
+
+	// Get the support function
+	if (mShape1ExCvxRadius == nullptr)
+		mShape1ExCvxRadius = mShape1->GetSupportFunction(ConvexShape::ESupportMode::ExcludeConvexRadius, mBufferExCvxRadius, mScale1);
+
+	// Perform GJK step
+	status = pen_depth.GetPenetrationDepthStepGJK(*mShape1ExCvxRadius, mShape1ExCvxRadius->GetConvexRadius() + mCollideShapeSettings.mMaxSeparationDistance, triangle, 0.0f, mCollideShapeSettings.mCollisionTolerance, penetration_axis, point1, point2);
+
+	// Check result of collision detection
+	if (status == EPAPenetrationDepth::EStatus::NotColliding)
+		return;
+	else if (status == EPAPenetrationDepth::EStatus::Indeterminate)
+	{
+		// Need to run expensive EPA algorithm
+
+		// Get the support function
+		if (mShape1IncCvxRadius == nullptr)
+			mShape1IncCvxRadius = mShape1->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, mBufferIncCvxRadius, mScale1);
+
+		// Add convex radius
+		AddConvexRadius<ConvexShape::Support> shape1_add_max_separation_distance(*mShape1IncCvxRadius, mCollideShapeSettings.mMaxSeparationDistance);
+
+		// Perform EPA step
+		if (!pen_depth.GetPenetrationDepthStepEPA(shape1_add_max_separation_distance, triangle, mCollideShapeSettings.mPenetrationTolerance, penetration_axis, point1, point2))
+			return;
+	}
+
+	// Check if the penetration is bigger than the early out fraction
+	float penetration_depth = (point2 - point1).Length() - mCollideShapeSettings.mMaxSeparationDistance;
+	if (-penetration_depth >= mCollector.GetEarlyOutFraction())
+		return;
+
+	// Correct point1 for the added separation distance
+	float penetration_axis_len = penetration_axis.Length();
+	if (penetration_axis_len > 0.0f)
+		point1 -= penetration_axis * (mCollideShapeSettings.mMaxSeparationDistance / penetration_axis_len);
+
+	// Check if we have enabled active edge detection
+	if (mCollideShapeSettings.mActiveEdgeMode == EActiveEdgeMode::CollideOnlyWithActive && inActiveEdges != 0b111)
+	{
+		// Convert the active edge velocity hint to local space
+		Vec3 active_edge_movement_direction = mTransform1.Multiply3x3Transposed(mCollideShapeSettings.mActiveEdgeMovementDirection);
+
+		// Update the penetration axis to account for active edges
+		// Note that we flip the triangle normal as the penetration axis is pointing towards the triangle instead of away
+		penetration_axis = ActiveEdges::FixNormal(v0, v1, v2, back_facing? triangle_normal : -triangle_normal, inActiveEdges, point2, penetration_axis, active_edge_movement_direction);
+	}
+
+	// Convert to world space
+	point1 = mTransform1 * point1;
+	point2 = mTransform1 * point2;
+	Vec3 penetration_axis_world = mTransform1.Multiply3x3(penetration_axis);
+
+	// Create collision result
+	CollideShapeResult result(point1, point2, penetration_axis_world, penetration_depth, mSubShapeID1, inSubShapeID2, TransformedShape::sGetBodyID(mCollector.GetContext()));
+
+	// Gather faces
+	if (mCollideShapeSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces)
+	{
+		// Get supporting face of shape 1
+		mShape1->GetSupportingFace(SubShapeID(), -penetration_axis, mScale1, mTransform1, result.mShape1Face);
+
+		// Get face of the triangle
+		result.mShape2Face.resize(3);
+		result.mShape2Face[0] = mTransform1 * v0;
+		result.mShape2Face[1] = mTransform1 * v1;
+		result.mShape2Face[2] = mTransform1 * v2;
+	}
+
+	// Notify the collector
+	JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;)
+	mCollector.AddHit(result);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.h
new file mode 100644
index 0000000000..8adfa3b410
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.h
@@ -0,0 +1,56 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/AABox.h>
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+#include <Jolt/Physics/Collision/Shape/ConvexShape.h>
+
+JPH_NAMESPACE_BEGIN
+
+class CollideShapeSettings;
+
+/// Collision detection helper that collides a convex object vs one or more triangles
+class JPH_EXPORT CollideConvexVsTriangles
+{
+public:
+	/// Constructor
+	/// @param inShape1 The convex shape to collide against triangles
+	/// @param inScale1 Local space scale for the convex object (scales relative to its center of mass)
+	/// @param inScale2 Local space scale for the triangles
+	/// @param inCenterOfMassTransform1 Transform that takes the center of mass of 1 into world space
+	/// @param inCenterOfMassTransform2 Transform that takes the center of mass of 2 into world space
+	/// @param inSubShapeID1 Sub shape ID of the convex object
+	/// @param inCollideShapeSettings Settings for the collide shape query
+	/// @param ioCollector The collector that will receive the results
+									CollideConvexVsTriangles(const ConvexShape *inShape1, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeID &inSubShapeID1, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector);
+
+	/// Collide convex object with a single triangle
+	/// @param inV0 , inV1 , inV2: CCW triangle vertices
+	/// @param inActiveEdges bit 0 = edge v0..v1 is active, bit 1 = edge v1..v2 is active, bit 2 = edge v2..v0 is active
+	/// An active edge is an edge that is not connected to another triangle in such a way that it is impossible to collide with the edge
+	/// @param inSubShapeID2 The sub shape ID for the triangle
+	void							Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2);
+
+protected:
+	const CollideShapeSettings &	mCollideShapeSettings;					///< Settings for this collision operation
+	CollideShapeCollector &			mCollector;								///< The collector that will receive the results
+	const ConvexShape *				mShape1;								///< The shape that we're colliding with
+	Vec3							mScale1;								///< The scale of the shape (in shape local space) of the shape we're colliding with
+	Vec3							mScale2;								///< The scale of the shape (in shape local space) of the shape we're colliding against
+	Mat44							mTransform1;							///< Transform of the shape we're colliding with
+	Mat44							mTransform2To1;							///< Transform that takes a point in space of the colliding shape to the shape we're colliding with
+	AABox							mBoundsOf1;								///< Bounds of the colliding shape in local space
+	AABox							mBoundsOf1InSpaceOf2;					///< Bounds of the colliding shape in space of shape we're colliding with
+	SubShapeID						mSubShapeID1;							///< Sub shape ID of colliding shape
+	float							mScaleSign2;							///< Sign of the scale of object 2, -1 if object is inside out, 1 if not
+	ConvexShape::SupportBuffer		mBufferExCvxRadius;						///< Buffer that holds the support function data excluding convex radius
+	ConvexShape::SupportBuffer		mBufferIncCvxRadius;					///< Buffer that holds the support function data including convex radius
+	const ConvexShape::Support *	mShape1ExCvxRadius = nullptr;			///< Actual support function object excluding convex radius
+	const ConvexShape::Support *	mShape1IncCvxRadius = nullptr;			///< Actual support function object including convex radius
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollidePointResult.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollidePointResult.h
new file mode 100644
index 0000000000..8601b3c40a
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollidePointResult.h
@@ -0,0 +1,25 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/BodyID.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Structure that holds the result of colliding a point against a shape
+class CollidePointResult
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Function required by the CollisionCollector. A smaller fraction is considered to be a 'better hit'. For point queries there is no sensible return value.
+	inline float	GetEarlyOutFraction() const			{ return 0.0f; }
+
+	BodyID			mBodyID;							///< Body that was hit
+	SubShapeID		mSubShapeID2;						///< Sub shape ID of shape that we collided against
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideShape.h
new file mode 100644
index 0000000000..2c5f8f217f
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideShape.h
@@ -0,0 +1,105 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/StaticArray.h>
+#include <Jolt/Physics/Collision/BackFaceMode.h>
+#include <Jolt/Physics/Collision/ActiveEdgeMode.h>
+#include <Jolt/Physics/Collision/CollectFacesMode.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+#include <Jolt/Physics/Body/BodyID.h>
+#include <Jolt/Physics/PhysicsSettings.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that contains all information of two colliding shapes
+class CollideShapeResult
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Default constructor
+								CollideShapeResult() = default;
+
+	/// Constructor
+								CollideShapeResult(Vec3Arg inContactPointOn1, Vec3Arg inContactPointOn2, Vec3Arg inPenetrationAxis, float inPenetrationDepth, const SubShapeID &inSubShapeID1, const SubShapeID &inSubShapeID2, const BodyID &inBodyID2) :
+		mContactPointOn1(inContactPointOn1),
+		mContactPointOn2(inContactPointOn2),
+		mPenetrationAxis(inPenetrationAxis),
+		mPenetrationDepth(inPenetrationDepth),
+		mSubShapeID1(inSubShapeID1),
+		mSubShapeID2(inSubShapeID2),
+		mBodyID2(inBodyID2)
+	{
+	}
+
+	/// Function required by the CollisionCollector. A smaller fraction is considered to be a 'better hit'. We use -penetration depth to get the hit with the biggest penetration depth
+	inline float				GetEarlyOutFraction() const	{ return -mPenetrationDepth; }
+
+	/// Reverses the hit result, swapping contact point 1 with contact point 2 etc.
+	inline CollideShapeResult	Reversed() const
+	{
+		CollideShapeResult result;
+		result.mContactPointOn2 = mContactPointOn1;
+		result.mContactPointOn1 = mContactPointOn2;
+		result.mPenetrationAxis = -mPenetrationAxis;
+		result.mPenetrationDepth = mPenetrationDepth;
+		result.mSubShapeID2 = mSubShapeID1;
+		result.mSubShapeID1 = mSubShapeID2;
+		result.mBodyID2 = mBodyID2;
+		result.mShape2Face = mShape1Face;
+		result.mShape1Face = mShape2Face;
+		return result;
+	}
+
+	using Face = StaticArray<Vec3, 32>;
+
+	Vec3						mContactPointOn1;			///< Contact point on the surface of shape 1 (in world space or relative to base offset)
+	Vec3						mContactPointOn2;			///< Contact point on the surface of shape 2 (in world space or relative to base offset). If the penetration depth is 0, this will be the same as mContactPointOn1.
+	Vec3						mPenetrationAxis;			///< Direction to move shape 2 out of collision along the shortest path (magnitude is meaningless, in world space). You can use -mPenetrationAxis.Normalized() as contact normal.
+	float						mPenetrationDepth;			///< Penetration depth (move shape 2 by this distance to resolve the collision)
+	SubShapeID					mSubShapeID1;				///< Sub shape ID that identifies the face on shape 1
+	SubShapeID					mSubShapeID2;				///< Sub shape ID that identifies the face on shape 2
+	BodyID						mBodyID2;					///< BodyID to which shape 2 belongs to
+	Face						mShape1Face;				///< Colliding face on shape 1 (optional result, in world space or relative to base offset)
+	Face						mShape2Face;				///< Colliding face on shape 2 (optional result, in world space or relative to base offset)
+};
+
+/// Settings to be passed with a collision query
+class CollideSettingsBase
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// How active edges (edges that a moving object should bump into) are handled
+	EActiveEdgeMode				mActiveEdgeMode				= EActiveEdgeMode::CollideOnlyWithActive;
+
+	/// If colliding faces should be collected or only the collision point
+	ECollectFacesMode			mCollectFacesMode			= ECollectFacesMode::NoFaces;
+
+	/// If objects are closer than this distance, they are considered to be colliding (used for GJK) (unit: meter)
+	float						mCollisionTolerance			= cDefaultCollisionTolerance;
+
+	/// A factor that determines the accuracy of the penetration depth calculation. If the change of the squared distance is less than tolerance * current_penetration_depth^2 the algorithm will terminate. (unit: dimensionless)
+	float						mPenetrationTolerance		= cDefaultPenetrationTolerance;
+
+	/// When mActiveEdgeMode is CollideOnlyWithActive a movement direction can be provided. When hitting an inactive edge, the system will select the triangle normal as penetration depth only if it impedes the movement less than with the calculated penetration depth.
+	Vec3						mActiveEdgeMovementDirection = Vec3::sZero();
+};
+
+/// Settings to be passed with a collision query
+class CollideShapeSettings : public CollideSettingsBase
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// When > 0 contacts in the vicinity of the query shape can be found. All nearest contacts that are not further away than this distance will be found (unit: meter)
+	float						mMaxSeparationDistance		= 0.0f;
+
+	/// How backfacing triangles should be treated
+	EBackFaceMode				mBackFaceMode				= EBackFaceMode::IgnoreBackFaces;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h
new file mode 100644
index 0000000000..e976aa9de2
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h
@@ -0,0 +1,110 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/SoftBody/SoftBodyVertex.h>
+#include <Jolt/Core/StridedPtr.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that allows iterating over the vertices of a soft body.
+/// It tracks the largest penetration and allows storing the resulting collision in a different structure than the soft body vertex itself.
+class CollideSoftBodyVertexIterator
+{
+public:
+	/// Default constructor
+									CollideSoftBodyVertexIterator() = default;
+									CollideSoftBodyVertexIterator(const CollideSoftBodyVertexIterator &) = default;
+
+	/// Construct using (strided) pointers
+									CollideSoftBodyVertexIterator(const StridedPtr<const Vec3> &inPosition, const StridedPtr<const float> &inInvMass, const StridedPtr<Plane> &inCollisionPlane, const StridedPtr<float> &inLargestPenetration, const StridedPtr<int> &inCollidingShapeIndex) :
+		mPosition(inPosition),
+		mInvMass(inInvMass),
+		mCollisionPlane(inCollisionPlane),
+		mLargestPenetration(inLargestPenetration),
+		mCollidingShapeIndex(inCollidingShapeIndex)
+	{
+	}
+
+	/// Construct using a soft body vertex
+	explicit						CollideSoftBodyVertexIterator(SoftBodyVertex *inVertices) :
+		mPosition(&inVertices->mPosition, sizeof(SoftBodyVertex)),
+		mInvMass(&inVertices->mInvMass, sizeof(SoftBodyVertex)),
+		mCollisionPlane(&inVertices->mCollisionPlane, sizeof(SoftBodyVertex)),
+		mLargestPenetration(&inVertices->mLargestPenetration, sizeof(SoftBodyVertex)),
+		mCollidingShapeIndex(&inVertices->mCollidingShapeIndex, sizeof(SoftBodyVertex))
+	{
+	}
+
+	/// Default assignment
+	CollideSoftBodyVertexIterator &	operator = (const CollideSoftBodyVertexIterator &) = default;
+
+	/// Equality operator.
+	/// Note: Only used to determine end iterator, so we only compare position.
+	bool							operator != (const CollideSoftBodyVertexIterator &inRHS) const
+	{
+		return mPosition != inRHS.mPosition;
+	}
+
+	/// Next vertex
+	CollideSoftBodyVertexIterator &	operator ++ ()
+	{
+		++mPosition;
+		++mInvMass;
+		++mCollisionPlane;
+		++mLargestPenetration;
+		++mCollidingShapeIndex;
+		return *this;
+	}
+
+	/// Add an offset
+	/// Note: Only used to determine end iterator, so we only set position.
+	CollideSoftBodyVertexIterator	operator + (int inOffset) const
+	{
+		return CollideSoftBodyVertexIterator(mPosition + inOffset, StridedPtr<const float>(), StridedPtr<Plane>(), StridedPtr<float>(), StridedPtr<int>());
+	}
+
+	/// Get the position of the current vertex
+	Vec3							GetPosition() const
+	{
+		return *mPosition;
+	}
+
+	/// Get the inverse mass of the current vertex
+	float							GetInvMass() const
+	{
+		return *mInvMass;
+	}
+
+	/// Update penetration of the current vertex
+	/// @return Returns true if the vertex has the largest penetration so far, this means you need to follow up by calling SetCollision
+	bool							UpdatePenetration(float inLargestPenetration) const
+	{
+		float &penetration = *mLargestPenetration;
+		if (penetration >= inLargestPenetration)
+			return false;
+		penetration = inLargestPenetration;
+		return true;
+	}
+
+	/// Update the collision of the current vertex
+	void							SetCollision(const Plane &inCollisionPlane, int inCollidingShapeIndex) const
+	{
+		*mCollisionPlane = inCollisionPlane;
+		*mCollidingShapeIndex = inCollidingShapeIndex;
+	}
+
+private:
+	/// Input data
+	StridedPtr<const Vec3>			mPosition;
+	StridedPtr<const float>			mInvMass;
+
+	/// Output data
+	StridedPtr<Plane>				mCollisionPlane;
+	StridedPtr<float>				mLargestPenetration;
+	StridedPtr<int>					mCollidingShapeIndex;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h
new file mode 100644
index 0000000000..c91becb642
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h
@@ -0,0 +1,90 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
+#include <Jolt/Geometry/ClosestPoint.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Collision detection helper that collides soft body vertices vs triangles
+class JPH_EXPORT CollideSoftBodyVerticesVsTriangles
+{
+public:
+						CollideSoftBodyVerticesVsTriangles(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) :
+		mTransform(inCenterOfMassTransform * Mat44::sScale(inScale)),
+		mInvTransform(mTransform.Inversed()),
+		mNormalSign(ScaleHelpers::IsInsideOut(inScale)? -1.0f : 1.0f)
+	{
+	}
+
+	JPH_INLINE void		StartVertex(const CollideSoftBodyVertexIterator &inVertex)
+	{
+		mLocalPosition = mInvTransform * inVertex.GetPosition();
+		mClosestDistanceSq = FLT_MAX;
+	}
+
+	JPH_INLINE void		ProcessTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
+	{
+		// Get the closest point from the vertex to the triangle
+		uint32 set;
+		Vec3 closest_point = ClosestPoint::GetClosestPointOnTriangle(inV0 - mLocalPosition, inV1 - mLocalPosition, inV2 - mLocalPosition, set);
+		float dist_sq = closest_point.LengthSq();
+		if (dist_sq < mClosestDistanceSq)
+		{
+			mV0 = inV0;
+			mV1 = inV1;
+			mV2 = inV2;
+			mClosestPoint = closest_point;
+			mClosestDistanceSq = dist_sq;
+			mSet = set;
+		}
+	}
+
+	JPH_INLINE void		FinishVertex(const CollideSoftBodyVertexIterator &ioVertex, int inCollidingShapeIndex) const
+	{
+		if (mClosestDistanceSq < FLT_MAX)
+		{
+			// Convert triangle to world space
+			Vec3 v0 = mTransform * mV0;
+			Vec3 v1 = mTransform * mV1;
+			Vec3 v2 = mTransform * mV2;
+			Vec3 triangle_normal = mNormalSign * (v1 - v0).Cross(v2 - v0).NormalizedOr(Vec3::sAxisY());
+
+			if (mSet == 0b111)
+			{
+				// Closest is interior to the triangle, use plane as collision plane but don't allow more than 0.1 m penetration
+				// because otherwise a triangle half a level a way will have a huge penetration if it is back facing
+				float penetration = min(triangle_normal.Dot(v0 - ioVertex.GetPosition()), 0.1f);
+				if (ioVertex.UpdatePenetration(penetration))
+					ioVertex.SetCollision(Plane::sFromPointAndNormal(v0, triangle_normal), inCollidingShapeIndex);
+			}
+			else
+			{
+				// Closest point is on an edge or vertex, use closest point as collision plane
+				Vec3 closest_point = mTransform * (mLocalPosition + mClosestPoint);
+				Vec3 normal = ioVertex.GetPosition() - closest_point;
+				if (normal.Dot(triangle_normal) > 0.0f) // Ignore back facing edges
+				{
+					float normal_length = normal.Length();
+					float penetration = -normal_length;
+					if (ioVertex.UpdatePenetration(penetration))
+						ioVertex.SetCollision(Plane::sFromPointAndNormal(closest_point, normal_length > 0.0f? normal / normal_length : triangle_normal), inCollidingShapeIndex);
+				}
+			}
+		}
+	}
+
+	Mat44				mTransform;
+	Mat44				mInvTransform;
+	Vec3				mLocalPosition;
+	Vec3				mV0, mV1, mV2;
+	Vec3				mClosestPoint;
+	float				mNormalSign;
+	float				mClosestDistanceSq;
+	uint32				mSet;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.cpp
new file mode 100644
index 0000000000..92ed28a7b4
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.cpp
@@ -0,0 +1,123 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/CollideSphereVsTriangles.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/CollideShape.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/ActiveEdges.h>
+#include <Jolt/Physics/Collision/NarrowPhaseStats.h>
+#include <Jolt/Core/Profiler.h>
+
+JPH_NAMESPACE_BEGIN
+
+static constexpr uint8 sClosestFeatureToActiveEdgesMask[] = {
+	0b000,		// 0b000: Invalid, guarded by an assert
+	0b101,		// 0b001: Vertex 1 -> edge 1 or 3
+	0b011,		// 0b010: Vertex 2 -> edge 1 or 2
+	0b001,		// 0b011: Vertex 1 & 2 -> edge 1
+	0b110,		// 0b100: Vertex 3 -> edge 2 or 3
+	0b100,		// 0b101: Vertex 1 & 3 -> edge 3
+	0b010,		// 0b110: Vertex 2 & 3 -> edge 2
+	// 0b111: Vertex 1, 2 & 3 -> interior, guarded by an if
+};
+
+CollideSphereVsTriangles::CollideSphereVsTriangles(const SphereShape *inShape1, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeID &inSubShapeID1, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector) :
+	mCollideShapeSettings(inCollideShapeSettings),
+	mCollector(ioCollector),
+	mShape1(inShape1),
+	mScale2(inScale2),
+	mTransform2(inCenterOfMassTransform2),
+	mSubShapeID1(inSubShapeID1)
+{
+	// Calculate the center of the sphere in the space of 2
+	mSphereCenterIn2 = inCenterOfMassTransform2.Multiply3x3Transposed(inCenterOfMassTransform1.GetTranslation() - inCenterOfMassTransform2.GetTranslation());
+
+	// Determine if shape 2 is inside out or not
+	mScaleSign2 = ScaleHelpers::IsInsideOut(inScale2)? -1.0f : 1.0f;
+
+	// Check that the sphere is uniformly scaled
+	JPH_ASSERT(ScaleHelpers::IsUniformScale(inScale1.Abs()));
+	mRadius = abs(inScale1.GetX()) * inShape1->GetRadius();
+	mRadiusPlusMaxSeparationSq = Square(mRadius + inCollideShapeSettings.mMaxSeparationDistance);
+}
+
+void CollideSphereVsTriangles::Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Scale triangle and make it relative to the center of the sphere
+	Vec3 v0 = mScale2 * inV0 - mSphereCenterIn2;
+	Vec3 v1 = mScale2 * inV1 - mSphereCenterIn2;
+	Vec3 v2 = mScale2 * inV2 - mSphereCenterIn2;
+
+	// Calculate triangle normal
+	Vec3 triangle_normal = mScaleSign2 * (v1 - v0).Cross(v2 - v0);
+
+	// Backface check
+	bool back_facing = triangle_normal.Dot(v0) > 0.0f;
+	if (mCollideShapeSettings.mBackFaceMode == EBackFaceMode::IgnoreBackFaces && back_facing)
+		return;
+
+	// Check if we collide with the sphere
+	uint32 closest_feature;
+	Vec3 point2 = ClosestPoint::GetClosestPointOnTriangle(v0, v1, v2, closest_feature);
+	float point2_len_sq = point2.LengthSq();
+	if (point2_len_sq > mRadiusPlusMaxSeparationSq)
+		return;
+
+	// Calculate penetration depth
+	float penetration_depth = mRadius - sqrt(point2_len_sq);
+	if (-penetration_depth >= mCollector.GetEarlyOutFraction())
+		return;
+
+	// Calculate penetration axis, direction along which to push 2 to move it out of collision (this is always away from the sphere center)
+	Vec3 penetration_axis = point2.NormalizedOr(Vec3::sAxisY());
+
+	// Calculate the point on the sphere
+	Vec3 point1 = mRadius * penetration_axis;
+
+	// Check if we have enabled active edge detection
+	JPH_ASSERT(closest_feature != 0);
+	if (mCollideShapeSettings.mActiveEdgeMode == EActiveEdgeMode::CollideOnlyWithActive
+		&& closest_feature != 0b111 // For an interior hit we should already have the right normal
+		&& (inActiveEdges & sClosestFeatureToActiveEdgesMask[closest_feature]) == 0) // If we didn't hit an active edge we should take the triangle normal
+	{
+		// Convert the active edge velocity hint to local space
+		Vec3 active_edge_movement_direction = mTransform2.Multiply3x3Transposed(mCollideShapeSettings.mActiveEdgeMovementDirection);
+
+		// See ActiveEdges::FixNormal. If penetration_axis affects the movement less than the triangle normal we keep penetration_axis.
+		Vec3 new_penetration_axis = back_facing? triangle_normal : -triangle_normal;
+		if (active_edge_movement_direction.Dot(penetration_axis) * new_penetration_axis.Length() >= active_edge_movement_direction.Dot(new_penetration_axis))
+			penetration_axis = new_penetration_axis;
+	}
+
+	// Convert to world space
+	point1 = mTransform2 * (mSphereCenterIn2 + point1);
+	point2 = mTransform2 * (mSphereCenterIn2 + point2);
+	Vec3 penetration_axis_world = mTransform2.Multiply3x3(penetration_axis);
+
+	// Create collision result
+	CollideShapeResult result(point1, point2, penetration_axis_world, penetration_depth, mSubShapeID1, inSubShapeID2, TransformedShape::sGetBodyID(mCollector.GetContext()));
+
+	// Gather faces
+	if (mCollideShapeSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces)
+	{
+		// The sphere doesn't have a supporting face
+
+		// Get face of triangle 2
+		result.mShape2Face.resize(3);
+		result.mShape2Face[0] = mTransform2 * (mSphereCenterIn2 + v0);
+		result.mShape2Face[1] = mTransform2 * (mSphereCenterIn2 + v1);
+		result.mShape2Face[2] = mTransform2 * (mSphereCenterIn2 + v2);
+	}
+
+	// Notify the collector
+	JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;)
+	mCollector.AddHit(result);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.h
new file mode 100644
index 0000000000..5d16d9a884
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.h
@@ -0,0 +1,50 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+
+JPH_NAMESPACE_BEGIN
+
+class CollideShapeSettings;
+
+/// Collision detection helper that collides a sphere vs one or more triangles
+class JPH_EXPORT CollideSphereVsTriangles
+{
+public:
+	/// Constructor
+	/// @param inShape1 The sphere to collide against triangles
+	/// @param inScale1 Local space scale for the sphere (scales relative to its center of mass)
+	/// @param inScale2 Local space scale for the triangles
+	/// @param inCenterOfMassTransform1 Transform that takes the center of mass of 1 into world space
+	/// @param inCenterOfMassTransform2 Transform that takes the center of mass of 2 into world space
+	/// @param inSubShapeID1 Sub shape ID of the convex object
+	/// @param inCollideShapeSettings Settings for the collide shape query
+	/// @param ioCollector The collector that will receive the results
+									CollideSphereVsTriangles(const SphereShape *inShape1, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeID &inSubShapeID1, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector);
+
+	/// Collide sphere with a single triangle
+	/// @param inV0 , inV1 , inV2: CCW triangle vertices
+	/// @param inActiveEdges bit 0 = edge v0..v1 is active, bit 1 = edge v1..v2 is active, bit 2 = edge v2..v0 is active
+	/// An active edge is an edge that is not connected to another triangle in such a way that it is impossible to collide with the edge
+	/// @param inSubShapeID2 The sub shape ID for the triangle
+	void							Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2);
+
+protected:
+	const CollideShapeSettings &	mCollideShapeSettings;					///< Settings for this collision operation
+	CollideShapeCollector &			mCollector;								///< The collector that will receive the results
+	const SphereShape *				mShape1;								///< The shape that we're colliding with
+	Vec3							mScale2;								///< The scale of the shape (in shape local space) of the shape we're colliding against
+	Mat44							mTransform2;							///< Transform of the shape we're colliding against
+	Vec3							mSphereCenterIn2;						///< The center of the sphere in the space of 2
+	SubShapeID						mSubShapeID1;							///< Sub shape ID of colliding shape
+	float							mScaleSign2;							///< Sign of the scale of object 2, -1 if object is inside out, 1 if not
+	float							mRadius;								///< Radius of the sphere
+	float							mRadiusPlusMaxSeparationSq;				///< (Radius + Max SeparationDistance)^2
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionCollector.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionCollector.h
new file mode 100644
index 0000000000..ec60850271
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionCollector.h
@@ -0,0 +1,105 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+class Body;
+class TransformedShape;
+
+/// Traits to use for CastRay
+class CollisionCollectorTraitsCastRay
+{
+public:
+	/// For rays the early out fraction is the fraction along the line to order hits.
+	static constexpr float InitialEarlyOutFraction = 1.0f + FLT_EPSILON;	///< Furthest hit: Fraction is 1 + epsilon
+	static constexpr float ShouldEarlyOutFraction = 0.0f;					///< Closest hit: Fraction is 0
+};
+
+/// Traits to use for CastShape
+class CollisionCollectorTraitsCastShape
+{
+public:
+	/// For rays the early out fraction is the fraction along the line to order hits.
+	static constexpr float InitialEarlyOutFraction = 1.0f + FLT_EPSILON;	///< Furthest hit: Fraction is 1 + epsilon
+	static constexpr float ShouldEarlyOutFraction = -FLT_MAX;				///< Deepest hit: Penetration is infinite
+};
+
+/// Traits to use for CollideShape
+class CollisionCollectorTraitsCollideShape
+{
+public:
+	/// For shape collisions we use -penetration depth to order hits.
+	static constexpr float InitialEarlyOutFraction = FLT_MAX;				///< Most shallow hit: Separation is infinite
+	static constexpr float ShouldEarlyOutFraction = -FLT_MAX;				///< Deepest hit: Penetration is infinite
+};
+
+/// Traits to use for CollidePoint
+using CollisionCollectorTraitsCollidePoint = CollisionCollectorTraitsCollideShape;
+
+/// Virtual interface that allows collecting multiple collision results
+template <class ResultTypeArg, class TraitsType>
+class CollisionCollector
+{
+public:
+	/// Declare ResultType so that derived classes can use it
+	using ResultType = ResultTypeArg;
+
+	/// Default constructor
+							CollisionCollector() = default;
+
+	/// Constructor to initialize from another collector
+	template <class ResultTypeArg2>
+	explicit				CollisionCollector(const CollisionCollector<ResultTypeArg2, TraitsType> &inRHS) : mEarlyOutFraction(inRHS.GetEarlyOutFraction()), mContext(inRHS.GetContext()) { }
+							CollisionCollector(const CollisionCollector<ResultTypeArg, TraitsType> &inRHS) = default;
+
+	/// Destructor
+	virtual					~CollisionCollector() = default;
+
+	/// If you want to reuse this collector, call Reset()
+	virtual void			Reset()											{ mEarlyOutFraction = TraitsType::InitialEarlyOutFraction; }
+
+	/// When running a query through the NarrowPhaseQuery class, this will be called for every body that is potentially colliding.
+	/// It allows collecting additional information needed by the collision collector implementation from the body under lock protection
+	/// before AddHit is called (e.g. the user data pointer or the velocity of the body).
+	virtual void			OnBody([[maybe_unused]] const Body &inBody)		{ /* Collects nothing by default */ }
+
+	/// Set by the collision detection functions to the current TransformedShape that we're colliding against before calling the AddHit function
+	void					SetContext(const TransformedShape *inContext)	{ mContext = inContext; }
+	const TransformedShape *GetContext() const								{ return mContext; }
+
+	/// This function can be used to set some user data on the collision collector
+	virtual void			SetUserData(uint64 inUserData)					{ /* Does nothing by default */ }
+
+	/// This function will be called for every hit found, it's up to the application to decide how to store the hit
+	virtual void			AddHit(const ResultType &inResult) = 0;
+
+	/// Update the early out fraction (should be lower than before)
+	inline void				UpdateEarlyOutFraction(float inFraction)		{ JPH_ASSERT(inFraction <= mEarlyOutFraction); mEarlyOutFraction = inFraction; }
+
+	/// Reset the early out fraction to a specific value
+	inline void				ResetEarlyOutFraction(float inFraction = TraitsType::InitialEarlyOutFraction) { mEarlyOutFraction = inFraction; }
+
+	/// Force the collision detection algorithm to terminate as soon as possible. Call this from the AddHit function when a satisfying hit is found.
+	inline void				ForceEarlyOut()									{ mEarlyOutFraction = TraitsType::ShouldEarlyOutFraction; }
+
+	/// When true, the collector will no longer accept any additional hits and the collision detection routine should early out as soon as possible
+	inline bool				ShouldEarlyOut() const							{ return mEarlyOutFraction <= TraitsType::ShouldEarlyOutFraction; }
+
+	/// Get the current early out value
+	inline float			GetEarlyOutFraction() const						{ return mEarlyOutFraction; }
+
+	/// Get the current early out value but make sure it's bigger than zero, this is used for shape casting as negative values are used for penetration
+	inline float			GetPositiveEarlyOutFraction() const				{ return max(FLT_MIN, mEarlyOutFraction); }
+
+private:
+	/// The early out fraction determines the fraction below which the collector is still accepting a hit (can be used to reduce the amount of work)
+	float					mEarlyOutFraction = TraitsType::InitialEarlyOutFraction;
+
+	/// Set by the collision detection functions to the current TransformedShape of the body that we're colliding against before calling the AddHit function
+	const TransformedShape *mContext = nullptr;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionCollectorImpl.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionCollectorImpl.h
new file mode 100644
index 0000000000..7bdd5da279
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionCollectorImpl.h
@@ -0,0 +1,134 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/CollisionCollector.h>
+#include <Jolt/Core/QuickSort.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Simple implementation that collects all hits and optionally sorts them on distance
+template <class CollectorType>
+class AllHitCollisionCollector : public CollectorType
+{
+public:
+	/// Redeclare ResultType
+	using ResultType = typename CollectorType::ResultType;
+
+	// See: CollectorType::Reset
+	virtual void		Reset() override
+	{
+		CollectorType::Reset();
+
+		mHits.clear();
+	}
+
+	// See: CollectorType::AddHit
+	virtual void		AddHit(const ResultType &inResult) override
+	{
+		mHits.push_back(inResult);
+	}
+
+	/// Order hits on closest first
+	void				Sort()
+	{
+		QuickSort(mHits.begin(), mHits.end(), [](const ResultType &inLHS, const ResultType &inRHS) { return inLHS.GetEarlyOutFraction() < inRHS.GetEarlyOutFraction(); });
+	}
+
+	/// Check if any hits were collected
+	inline bool			HadHit() const
+	{
+		return !mHits.empty();
+	}
+
+	Array<ResultType>	mHits;
+};
+
+/// Simple implementation that collects the closest / deepest hit
+template <class CollectorType>
+class ClosestHitCollisionCollector : public CollectorType
+{
+public:
+	/// Redeclare ResultType
+	using ResultType = typename CollectorType::ResultType;
+
+	// See: CollectorType::Reset
+	virtual void		Reset() override
+	{
+		CollectorType::Reset();
+
+		mHadHit = false;
+	}
+
+	// See: CollectorType::AddHit
+	virtual void		AddHit(const ResultType &inResult) override
+	{
+		float early_out = inResult.GetEarlyOutFraction();
+		if (!mHadHit || early_out < mHit.GetEarlyOutFraction())
+		{
+			// Update early out fraction
+			CollectorType::UpdateEarlyOutFraction(early_out);
+
+			// Store hit
+			mHit = inResult;
+			mHadHit = true;
+		}
+	}
+
+	/// Check if this collector has had a hit
+	inline bool			HadHit() const
+	{
+		return mHadHit;
+	}
+
+	ResultType			mHit;
+
+private:
+	bool				mHadHit = false;
+};
+
+/// Simple implementation that collects any hit
+template <class CollectorType>
+class AnyHitCollisionCollector : public CollectorType
+{
+public:
+	/// Redeclare ResultType
+	using ResultType = typename CollectorType::ResultType;
+
+	// See: CollectorType::Reset
+	virtual void		Reset() override
+	{
+		CollectorType::Reset();
+
+		mHadHit = false;
+	}
+
+	// See: CollectorType::AddHit
+	virtual void		AddHit(const ResultType &inResult) override
+	{
+		// Test that the collector is not collecting more hits after forcing an early out
+		JPH_ASSERT(!mHadHit);
+
+		// Abort any further testing
+		CollectorType::ForceEarlyOut();
+
+		// Store hit
+		mHit = inResult;
+		mHadHit = true;
+	}
+
+	/// Check if this collector has had a hit
+	inline bool			HadHit() const
+	{
+		return mHadHit;
+	}
+
+	ResultType			mHit;
+
+private:
+	bool				mHadHit = false;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionDispatch.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionDispatch.cpp
new file mode 100644
index 0000000000..259a9526e8
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionDispatch.cpp
@@ -0,0 +1,107 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+
+JPH_NAMESPACE_BEGIN
+
+CollisionDispatch::CollideShape CollisionDispatch::sCollideShape[NumSubShapeTypes][NumSubShapeTypes];
+CollisionDispatch::CastShape CollisionDispatch::sCastShape[NumSubShapeTypes][NumSubShapeTypes];
+
+void CollisionDispatch::sInit()
+{
+	for (uint i = 0; i < NumSubShapeTypes; ++i)
+		for (uint j = 0; j < NumSubShapeTypes; ++j)
+		{
+			if (sCollideShape[i][j] == nullptr)
+				sCollideShape[i][j] = [](const Shape *, const Shape *, Vec3Arg, Vec3Arg, Mat44Arg, Mat44Arg, const SubShapeIDCreator &, const SubShapeIDCreator &, const CollideShapeSettings &, CollideShapeCollector &, const ShapeFilter &)
+				{
+					JPH_ASSERT(false, "Unsupported shape pair");
+				};
+
+			if (sCastShape[i][j] == nullptr)
+				sCastShape[i][j] = [](const ShapeCast &, const ShapeCastSettings &, const Shape *, Vec3Arg, const ShapeFilter &, Mat44Arg, const SubShapeIDCreator &, const SubShapeIDCreator &, CastShapeCollector &)
+				{
+					JPH_ASSERT(false, "Unsupported shape pair");
+				};
+		}
+}
+
+void CollisionDispatch::sReversedCollideShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+{
+	// A collision collector that flips the collision results
+	class ReversedCollector : public CollideShapeCollector
+	{
+	public:
+		explicit				ReversedCollector(CollideShapeCollector &ioCollector) :
+			CollideShapeCollector(ioCollector),
+			mCollector(ioCollector)
+		{
+		}
+
+		virtual void			AddHit(const CollideShapeResult &inResult) override
+		{
+			// Add the reversed hit
+			mCollector.AddHit(inResult.Reversed());
+
+			// If our chained collector updated its early out fraction, we need to follow
+			UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
+		}
+
+	private:
+		CollideShapeCollector &	mCollector;
+	};
+
+	ReversedShapeFilter shape_filter(inShapeFilter);
+	ReversedCollector collector(ioCollector);
+	sCollideShapeVsShape(inShape2, inShape1, inScale2, inScale1, inCenterOfMassTransform2, inCenterOfMassTransform1, inSubShapeIDCreator2, inSubShapeIDCreator1, inCollideShapeSettings, collector, shape_filter);
+}
+
+void CollisionDispatch::sReversedCastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	// A collision collector that flips the collision results
+	class ReversedCollector : public CastShapeCollector
+	{
+	public:
+		explicit				ReversedCollector(CastShapeCollector &ioCollector, Vec3Arg inWorldDirection) :
+			CastShapeCollector(ioCollector),
+			mCollector(ioCollector),
+			mWorldDirection(inWorldDirection)
+		{
+		}
+
+		virtual void			AddHit(const ShapeCastResult &inResult) override
+		{
+			// Add the reversed hit
+			mCollector.AddHit(inResult.Reversed(mWorldDirection));
+
+			// If our chained collector updated its early out fraction, we need to follow
+			UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
+		}
+
+	private:
+		CastShapeCollector &	mCollector;
+		Vec3					mWorldDirection;
+	};
+
+	// Reverse the shape cast (shape cast is in local space to shape 2)
+	Mat44 com_start_inv = inShapeCast.mCenterOfMassStart.InversedRotationTranslation();
+	ShapeCast local_shape_cast(inShape, inScale, com_start_inv, -com_start_inv.Multiply3x3(inShapeCast.mDirection));
+
+	// Calculate the center of mass of shape 1 at start of sweep
+	Mat44 shape1_com = inCenterOfMassTransform2 * inShapeCast.mCenterOfMassStart;
+
+	// Calculate the world space direction vector of the shape cast
+	Vec3 world_direction = -inCenterOfMassTransform2.Multiply3x3(inShapeCast.mDirection);
+
+	// Forward the cast
+	ReversedShapeFilter shape_filter(inShapeFilter);
+	ReversedCollector collector(ioCollector, world_direction);
+	sCastShapeVsShapeLocalSpace(local_shape_cast, inShapeCastSettings, inShapeCast.mShape, inShapeCast.mScale, shape_filter, shape1_com, inSubShapeIDCreator2, inSubShapeIDCreator1, collector);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionDispatch.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionDispatch.h
new file mode 100644
index 0000000000..6842c8154a
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionDispatch.h
@@ -0,0 +1,97 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/ShapeFilter.h>
+#include <Jolt/Physics/Collision/NarrowPhaseStats.h>
+
+JPH_NAMESPACE_BEGIN
+
+class CollideShapeSettings;
+
+/// Dispatch function, main function to handle collisions between shapes
+class JPH_EXPORT CollisionDispatch
+{
+public:
+	/// Collide 2 shapes and pass any collision on to ioCollector
+	/// @param inShape1 The first shape
+	/// @param inShape2 The second shape
+	/// @param inScale1 Local space scale of shape 1 (scales relative to its center of mass)
+	/// @param inScale2 Local space scale of shape 2 (scales relative to its center of mass)
+	/// @param inCenterOfMassTransform1 Transform to transform center of mass of shape 1 into world space
+	/// @param inCenterOfMassTransform2 Transform to transform center of mass of shape 2 into world space
+	/// @param inSubShapeIDCreator1 Class that tracks the current sub shape ID for shape 1
+	/// @param inSubShapeIDCreator2 Class that tracks the current sub shape ID for shape 2
+	/// @param inCollideShapeSettings Options for the CollideShape test
+	/// @param ioCollector The collector that receives the results.
+	/// @param inShapeFilter allows selectively disabling collisions between pairs of (sub) shapes.
+	static inline void		sCollideShapeVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter = { })
+	{
+		JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseStat track(NarrowPhaseStat::sCollideShape[(int)inShape1->GetSubType()][(int)inShape2->GetSubType()]);)
+
+		// Only test shape if it passes the shape filter
+		if (inShapeFilter.ShouldCollide(inShape1, inSubShapeIDCreator1.GetID(), inShape2, inSubShapeIDCreator2.GetID()))
+			sCollideShape[(int)inShape1->GetSubType()][(int)inShape2->GetSubType()](inShape1, inShape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
+	}
+
+	/// Cast a shape against this shape, passes any hits found to ioCollector.
+	/// Note: This version takes the shape cast in local space relative to the center of mass of inShape, take a look at sCastShapeVsShapeWorldSpace if you have a shape cast in world space.
+	/// @param inShapeCastLocal The shape to cast against the other shape and its start and direction.
+	/// @param inShapeCastSettings Settings for performing the cast
+	/// @param inShape The shape to cast against.
+	/// @param inScale Local space scale for the shape to cast against (scales relative to its center of mass).
+	/// @param inShapeFilter allows selectively disabling collisions between pairs of (sub) shapes.
+	/// @param inCenterOfMassTransform2 Is the center of mass transform of shape 2 (excluding scale), this is used to provide a transform to the shape cast result so that local hit result quantities can be transformed into world space.
+	/// @param inSubShapeIDCreator1 Class that tracks the current sub shape ID for the casting shape
+	/// @param inSubShapeIDCreator2 Class that tracks the current sub shape ID for the shape we're casting against
+	/// @param ioCollector The collector that receives the results.
+	static inline void		sCastShapeVsShapeLocalSpace(const ShapeCast &inShapeCastLocal, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+	{
+		JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseStat track(NarrowPhaseStat::sCastShape[(int)inShapeCastLocal.mShape->GetSubType()][(int)inShape->GetSubType()]);)
+
+		// Only test shape if it passes the shape filter
+		if (inShapeFilter.ShouldCollide(inShapeCastLocal.mShape, inSubShapeIDCreator1.GetID(), inShape, inSubShapeIDCreator2.GetID()))
+			sCastShape[(int)inShapeCastLocal.mShape->GetSubType()][(int)inShape->GetSubType()](inShapeCastLocal, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+	}
+
+	/// See: sCastShapeVsShapeLocalSpace.
+	/// The only difference is that the shape cast (inShapeCastWorld) is provided in world space.
+	/// Note: A shape cast contains the center of mass start of the shape, if you have the world transform of the shape you probably want to construct it using ShapeCast::sFromWorldTransform.
+	static inline void		sCastShapeVsShapeWorldSpace(const ShapeCast &inShapeCastWorld, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+	{
+		ShapeCast local_shape_cast = inShapeCastWorld.PostTransformed(inCenterOfMassTransform2.InversedRotationTranslation());
+		sCastShapeVsShapeLocalSpace(local_shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+	}
+
+	/// Function that collides 2 shapes (see sCollideShapeVsShape)
+	using CollideShape = void (*)(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+
+	/// Function that casts a shape vs another shape (see sCastShapeVsShapeLocalSpace)
+	using CastShape = void (*)(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+
+	/// Initialize all collision functions with a function that asserts and returns no collision
+	static void				sInit();
+
+	/// Register a collide shape function in the collision table
+	static void				sRegisterCollideShape(EShapeSubType inType1, EShapeSubType inType2, CollideShape inFunction)	{ sCollideShape[(int)inType1][(int)inType2] = inFunction; }
+
+	/// Register a cast shape function in the collision table
+	static void				sRegisterCastShape(EShapeSubType inType1, EShapeSubType inType2, CastShape inFunction)			{ sCastShape[(int)inType1][(int)inType2] = inFunction; }
+
+	/// An implementation of CollideShape that swaps inShape1 and inShape2 and swaps the result back, can be registered if the collision function only exists the other way around
+	static void				sReversedCollideShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+
+	/// An implementation of CastShape that swaps inShape1 and inShape2 and swaps the result back, can be registered if the collision function only exists the other way around
+	static void				sReversedCastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+
+private:
+	static CollideShape		sCollideShape[NumSubShapeTypes][NumSubShapeTypes];
+	static CastShape		sCastShape[NumSubShapeTypes][NumSubShapeTypes];
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionGroup.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionGroup.cpp
new file mode 100644
index 0000000000..a8c98b6dc4
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionGroup.cpp
@@ -0,0 +1,33 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/CollisionGroup.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(CollisionGroup)
+{
+	JPH_ADD_ATTRIBUTE(CollisionGroup, mGroupFilter)
+	JPH_ADD_ATTRIBUTE(CollisionGroup, mGroupID)
+	JPH_ADD_ATTRIBUTE(CollisionGroup, mSubGroupID)
+}
+
+void CollisionGroup::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(mGroupID);
+	inStream.Write(mSubGroupID);
+}
+
+void CollisionGroup::RestoreBinaryState(StreamIn &inStream)
+{
+	inStream.Read(mGroupID);
+	inStream.Read(mSubGroupID);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionGroup.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionGroup.h
new file mode 100644
index 0000000000..5e8c6cf1f5
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollisionGroup.h
@@ -0,0 +1,94 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/GroupFilter.h>
+#include <Jolt/ObjectStream/SerializableObject.h>
+
+JPH_NAMESPACE_BEGIN
+
+class StreamIn;
+class StreamOut;
+
+/// Two objects collide with each other if:
+/// - Both don't have a group filter
+/// - The first group filter says that the objects can collide
+/// - Or if there's no filter for the first object, the second group filter says the objects can collide
+class JPH_EXPORT CollisionGroup
+{
+	JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, CollisionGroup)
+
+public:
+	using GroupID			= uint32;
+	using SubGroupID		= uint32;
+
+	static const GroupID	cInvalidGroup = ~GroupID(0);
+	static const SubGroupID	cInvalidSubGroup = ~SubGroupID(0);
+
+	/// Default constructor
+							CollisionGroup() = default;
+
+	/// Construct with all properties
+							CollisionGroup(const GroupFilter *inFilter, GroupID inGroupID, SubGroupID inSubGroupID) : mGroupFilter(inFilter), mGroupID(inGroupID), mSubGroupID(inSubGroupID) { }
+
+	/// Set the collision group filter
+	inline void				SetGroupFilter(const GroupFilter *inFilter)
+	{
+		mGroupFilter = inFilter;
+	}
+
+	/// Get the collision group filter
+	inline const GroupFilter *GetGroupFilter() const
+	{
+		return mGroupFilter;
+	}
+
+	/// Set the main group id for this object
+	inline void				SetGroupID(GroupID inID)
+	{
+		mGroupID = inID;
+	}
+
+	inline GroupID			GetGroupID() const
+	{
+		return mGroupID;
+	}
+
+	/// Add this object to a sub group
+	inline void				SetSubGroupID(SubGroupID inID)
+	{
+		mSubGroupID = inID;
+	}
+
+	inline SubGroupID		GetSubGroupID() const
+	{
+		return mSubGroupID;
+	}
+
+	/// Check if this object collides with another object
+	bool					CanCollide(const CollisionGroup &inOther) const
+	{
+		// Call the CanCollide function of the first group filter that's not null
+		if (mGroupFilter != nullptr)
+			return mGroupFilter->CanCollide(*this, inOther);
+		else if (inOther.mGroupFilter != nullptr)
+			return inOther.mGroupFilter->CanCollide(inOther, *this);
+		else
+			return true;
+	}
+
+	/// Saves the state of this object in binary form to inStream. Does not save group filter.
+	void					SaveBinaryState(StreamOut &inStream) const;
+
+	/// Restore the state of this object from inStream. Does not save group filter.
+	void					RestoreBinaryState(StreamIn &inStream);
+
+private:
+	RefConst<GroupFilter>	mGroupFilter;
+	GroupID					mGroupID = cInvalidGroup;
+	SubGroupID				mSubGroupID = cInvalidSubGroup;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ContactListener.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/ContactListener.h
new file mode 100644
index 0000000000..1a24b4fb88
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ContactListener.h
@@ -0,0 +1,119 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/SubShapeIDPair.h>
+#include <Jolt/Core/StaticArray.h>
+
+JPH_NAMESPACE_BEGIN
+
+class Body;
+class CollideShapeResult;
+
+/// Array of contact points
+using ContactPoints = StaticArray<Vec3, 64>;
+
+/// Manifold class, describes the contact surface between two bodies
+class ContactManifold
+{
+public:
+	/// Swaps shape 1 and 2
+	ContactManifold			SwapShapes() const					{ return { mBaseOffset, -mWorldSpaceNormal, mPenetrationDepth, mSubShapeID2, mSubShapeID1, mRelativeContactPointsOn2, mRelativeContactPointsOn1 }; }
+
+	/// Access to the world space contact positions
+	inline RVec3			GetWorldSpaceContactPointOn1(uint inIndex) const { return mBaseOffset + mRelativeContactPointsOn1[inIndex]; }
+	inline RVec3			GetWorldSpaceContactPointOn2(uint inIndex) const { return mBaseOffset + mRelativeContactPointsOn2[inIndex]; }
+
+	RVec3					mBaseOffset;						///< Offset to which all the contact points are relative
+	Vec3					mWorldSpaceNormal;					///< Normal for this manifold, direction along which to move body 2 out of collision along the shortest path
+	float					mPenetrationDepth;					///< Penetration depth (move shape 2 by this distance to resolve the collision). If this value is negative, this is a speculative contact point and may not actually result in a velocity change as during solving the bodies may not actually collide.
+	SubShapeID				mSubShapeID1;						///< Sub shapes that formed this manifold (note that when multiple manifolds are combined because they're coplanar, we lose some information here because we only keep track of one sub shape pair that we encounter, see description at Body::SetUseManifoldReduction)
+	SubShapeID				mSubShapeID2;
+	ContactPoints			mRelativeContactPointsOn1;			///< Contact points on the surface of shape 1 relative to mBaseOffset.
+	ContactPoints			mRelativeContactPointsOn2;			///< Contact points on the surface of shape 2 relative to mBaseOffset. If there's no penetration, this will be the same as mRelativeContactPointsOn1. If there is penetration they will be different.
+};
+
+/// When a contact point is added or persisted, the callback gets a chance to override certain properties of the contact constraint.
+/// The values are filled in with their defaults by the system so the callback doesn't need to modify anything, but it can if it wants to.
+class ContactSettings
+{
+public:
+	float					mCombinedFriction;					///< Combined friction for the body pair (see: PhysicsSystem::SetCombineFriction)
+	float					mCombinedRestitution;				///< Combined restitution for the body pair (see: PhysicsSystem::SetCombineRestitution)
+	float					mInvMassScale1 = 1.0f;				///< Scale factor for the inverse mass of body 1 (0 = infinite mass, 1 = use original mass, 2 = body has half the mass). For the same contact pair, you should strive to keep the value the same over time.
+	float					mInvInertiaScale1 = 1.0f;			///< Scale factor for the inverse inertia of body 1 (usually same as mInvMassScale1)
+	float					mInvMassScale2 = 1.0f;				///< Scale factor for the inverse mass of body 2 (0 = infinite mass, 1 = use original mass, 2 = body has half the mass). For the same contact pair, you should strive to keep the value the same over time.
+	float					mInvInertiaScale2 = 1.0f;			///< Scale factor for the inverse inertia of body 2 (usually same as mInvMassScale2)
+	bool					mIsSensor;							///< If the contact should be treated as a sensor vs body contact (no collision response)
+	Vec3					mRelativeLinearSurfaceVelocity = Vec3::sZero(); ///< Relative linear surface velocity between the bodies (world space surface velocity of body 2 - world space surface velocity of body 1), can be used to create a conveyor belt effect
+	Vec3					mRelativeAngularSurfaceVelocity = Vec3::sZero(); ///< Relative angular surface velocity between the bodies (world space angular surface velocity of body 2 - world space angular surface velocity of body 1). Note that this angular velocity is relative to the center of mass of body 1, so if you want it relative to body 2's center of mass you need to add body 2 angular velocity x (body 1 world space center of mass - body 2 world space center of mass) to mRelativeLinearSurfaceVelocity.
+};
+
+/// Return value for the OnContactValidate callback. Determines if the contact is being processed or not.
+/// Results are ordered so that the strongest accept has the lowest number and the strongest reject the highest number (which allows for easy combining of results)
+enum class ValidateResult
+{
+	AcceptAllContactsForThisBodyPair,							///< Accept this and any further contact points for this body pair
+	AcceptContact,												///< Accept this contact only (and continue calling this callback for every contact manifold for the same body pair)
+	RejectContact,												///< Reject this contact only (but process any other contact manifolds for the same body pair)
+	RejectAllContactsForThisBodyPair							///< Rejects this and any further contact points for this body pair
+};
+
+/// A listener class that receives collision contact events.
+/// It can be registered with the ContactConstraintManager (or PhysicsSystem).
+/// Note that contact listener callbacks are called from multiple threads at the same time when all bodies are locked, you're only allowed to read from the bodies and you can't change physics state.
+/// During OnContactRemoved you cannot access the bodies at all, see the comments at that function.
+class ContactListener
+{
+public:
+	/// Ensure virtual destructor
+	virtual					~ContactListener() = default;
+
+	/// Called after detecting a collision between a body pair, but before calling OnContactAdded and before adding the contact constraint.
+	/// If the function rejects the contact, the contact will not be added and any other contacts between this body pair will not be processed.
+	/// This function will only be called once per PhysicsSystem::Update per body pair and may not be called again the next update
+	/// if a contact persists and no new contact pairs between sub shapes are found.
+	/// This is a rather expensive time to reject a contact point since a lot of the collision detection has happened already, make sure you
+	/// filter out the majority of undesired body pairs through the ObjectLayerPairFilter that is registered on the PhysicsSystem.
+	/// Note that this callback is called when all bodies are locked, so don't use any locking functions!
+	/// Body 1 will have a motion type that is larger or equal than body 2's motion type (order from large to small: dynamic -> kinematic -> static). When motion types are equal, they are ordered by BodyID.
+	/// The collision result (inCollisionResult) is reported relative to inBaseOffset.
+	virtual ValidateResult	OnContactValidate([[maybe_unused]] const Body &inBody1, [[maybe_unused]] const Body &inBody2, [[maybe_unused]] RVec3Arg inBaseOffset, [[maybe_unused]] const CollideShapeResult &inCollisionResult) { return ValidateResult::AcceptAllContactsForThisBodyPair; }
+
+	/// Called whenever a new contact point is detected.
+	/// Note that this callback is called when all bodies are locked, so don't use any locking functions!
+	/// Body 1 and 2 will be sorted such that body 1 ID < body 2 ID, so body 1 may not be dynamic.
+	/// Note that only active bodies will report contacts, as soon as a body goes to sleep the contacts between that body and all other
+	/// bodies will receive an OnContactRemoved callback, if this is the case then Body::IsActive() will return false during the callback.
+	/// When contacts are added, the constraint solver has not run yet, so the collision impulse is unknown at that point.
+	/// The velocities of inBody1 and inBody2 are the velocities before the contact has been resolved, so you can use this to
+	/// estimate the collision impulse to e.g. determine the volume of the impact sound to play (see: EstimateCollisionResponse).
+	virtual void			OnContactAdded([[maybe_unused]] const Body &inBody1, [[maybe_unused]] const Body &inBody2, [[maybe_unused]] const ContactManifold &inManifold, [[maybe_unused]] ContactSettings &ioSettings) { /* Do nothing */ }
+
+	/// Called whenever a contact is detected that was also detected last update.
+	/// Note that this callback is called when all bodies are locked, so don't use any locking functions!
+	/// Body 1 and 2 will be sorted such that body 1 ID < body 2 ID, so body 1 may not be dynamic.
+	/// If the structure of the shape of a body changes between simulation steps (e.g. by adding/removing a child shape of a compound shape),
+	/// it is possible that the same sub shape ID used to identify the removed child shape is now reused for a different child shape. The physics
+	/// system cannot detect this, so may send a 'contact persisted' callback even though the contact is now on a different child shape. You can
+	/// detect this by keeping the old shape (before adding/removing a part) around until the next PhysicsSystem::Update (when the OnContactPersisted
+	/// callbacks are triggered) and resolving the sub shape ID against both the old and new shape to see if they still refer to the same child shape.
+	virtual void			OnContactPersisted([[maybe_unused]] const Body &inBody1, [[maybe_unused]] const Body &inBody2, [[maybe_unused]] const ContactManifold &inManifold, [[maybe_unused]] ContactSettings &ioSettings) { /* Do nothing */ }
+
+	/// Called whenever a contact was detected last update but is not detected anymore.
+	/// You cannot access the bodies at the time of this callback because:
+	/// - All bodies are locked at the time of this callback.
+	/// - Some properties of the bodies are being modified from another thread at the same time.
+	/// - The body may have been removed and destroyed (you'll receive an OnContactRemoved callback in the PhysicsSystem::Update after the body has been removed).
+	/// Cache what you need in the OnContactAdded and OnContactPersisted callbacks and store it in a separate structure to use during this callback.
+	/// Alternatively, you could just record that the contact was removed and process it after PhysicsSimulation::Update.
+	/// Body 1 and 2 will be sorted such that body 1 ID < body 2 ID, so body 1 may not be dynamic.
+	/// The sub shape ID were created in the previous simulation step too, so if the structure of a shape changes (e.g. by adding/removing a child shape of a compound shape),
+	/// the sub shape ID may not be valid / may not point to the same sub shape anymore.
+	/// If you want to know if this is the last contact between the two bodies, use PhysicsSystem::WereBodiesInContact.
+	virtual void			OnContactRemoved([[maybe_unused]] const SubShapeIDPair &inSubShapePair) { /* Do nothing */ }
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/EstimateCollisionResponse.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/EstimateCollisionResponse.cpp
new file mode 100644
index 0000000000..53cf12b5d4
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/EstimateCollisionResponse.cpp
@@ -0,0 +1,213 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/EstimateCollisionResponse.h>
+#include <Jolt/Physics/Body/Body.h>
+
+JPH_NAMESPACE_BEGIN
+
+void EstimateCollisionResponse(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, CollisionEstimationResult &outResult, float inCombinedFriction, float inCombinedRestitution, float inMinVelocityForRestitution, uint inNumIterations)
+{
+	// Note this code is based on AxisConstraintPart, see that class for more comments on the math
+
+	ContactPoints::size_type num_points = inManifold.mRelativeContactPointsOn1.size();
+	JPH_ASSERT(num_points == inManifold.mRelativeContactPointsOn2.size());
+
+	// Start with zero impulses
+	outResult.mImpulses.resize(num_points);
+	memset(outResult.mImpulses.data(), 0, num_points * sizeof(CollisionEstimationResult::Impulse));
+
+	// Calculate friction directions
+	outResult.mTangent1 = inManifold.mWorldSpaceNormal.GetNormalizedPerpendicular();
+	outResult.mTangent2 = inManifold.mWorldSpaceNormal.Cross(outResult.mTangent1);
+
+	// Get body velocities
+	EMotionType motion_type1 = inBody1.GetMotionType();
+	const MotionProperties *motion_properties1 = inBody1.GetMotionPropertiesUnchecked();
+	if (motion_type1 != EMotionType::Static)
+	{
+		outResult.mLinearVelocity1 = motion_properties1->GetLinearVelocity();
+		outResult.mAngularVelocity1 = motion_properties1->GetAngularVelocity();
+	}
+	else
+		outResult.mLinearVelocity1 = outResult.mAngularVelocity1 = Vec3::sZero();
+
+	EMotionType motion_type2 = inBody2.GetMotionType();
+	const MotionProperties *motion_properties2 = inBody2.GetMotionPropertiesUnchecked();
+	if (motion_type2 != EMotionType::Static)
+	{
+		outResult.mLinearVelocity2 = motion_properties2->GetLinearVelocity();
+		outResult.mAngularVelocity2 = motion_properties2->GetAngularVelocity();
+	}
+	else
+		outResult.mLinearVelocity2 = outResult.mAngularVelocity2 = Vec3::sZero();
+
+	// Get inverse mass and inertia
+	float inv_m1, inv_m2;
+	Mat44 inv_i1, inv_i2;
+	if (motion_type1 == EMotionType::Dynamic)
+	{
+		inv_m1 = motion_properties1->GetInverseMass();
+		inv_i1 = inBody1.GetInverseInertia();
+	}
+	else
+	{
+		inv_m1 = 0.0f;
+		inv_i1 = Mat44::sZero();
+	}
+
+	if (motion_type2 == EMotionType::Dynamic)
+	{
+		inv_m2 = motion_properties2->GetInverseMass();
+		inv_i2 = inBody2.GetInverseInertia();
+	}
+	else
+	{
+		inv_m2 = 0.0f;
+		inv_i2 = Mat44::sZero();
+	}
+
+	// Get center of masses relative to the base offset
+	Vec3 com1 = Vec3(inBody1.GetCenterOfMassPosition() - inManifold.mBaseOffset);
+	Vec3 com2 = Vec3(inBody2.GetCenterOfMassPosition() - inManifold.mBaseOffset);
+
+	struct AxisConstraint
+	{
+		inline void		Initialize(Vec3Arg inR1, Vec3Arg inR2, Vec3Arg inWorldSpaceNormal, float inInvM1, float inInvM2, Mat44Arg inInvI1, Mat44Arg inInvI2)
+		{
+			// Calculate effective mass: K^-1 = (J M^-1 J^T)^-1
+			mR1PlusUxAxis = inR1.Cross(inWorldSpaceNormal);
+			mR2xAxis = inR2.Cross(inWorldSpaceNormal);
+			mInvI1_R1PlusUxAxis = inInvI1.Multiply3x3(mR1PlusUxAxis);
+			mInvI2_R2xAxis = inInvI2.Multiply3x3(mR2xAxis);
+			mEffectiveMass = 1.0f / (inInvM1 + mInvI1_R1PlusUxAxis.Dot(mR1PlusUxAxis) + inInvM2 + mInvI2_R2xAxis.Dot(mR2xAxis));
+			mBias = 0.0f;
+		}
+
+		inline float	SolveGetLambda(Vec3Arg inWorldSpaceNormal, const CollisionEstimationResult &inResult) const
+		{
+			// Calculate jacobian multiplied by linear/angular velocity
+			float jv = inWorldSpaceNormal.Dot(inResult.mLinearVelocity1 - inResult.mLinearVelocity2) + mR1PlusUxAxis.Dot(inResult.mAngularVelocity1) - mR2xAxis.Dot(inResult.mAngularVelocity2);
+
+			// Lagrange multiplier is:
+			//
+			// lambda = -K^-1 (J v + b)
+			return mEffectiveMass * (jv - mBias);
+		}
+
+		inline void		SolveApplyLambda(Vec3Arg inWorldSpaceNormal, float inInvM1, float inInvM2, float inLambda, CollisionEstimationResult &ioResult) const
+		{
+			// Apply impulse to body velocities
+			ioResult.mLinearVelocity1 -= (inLambda * inInvM1) * inWorldSpaceNormal;
+			ioResult.mAngularVelocity1 -= inLambda * mInvI1_R1PlusUxAxis;
+			ioResult.mLinearVelocity2 += (inLambda * inInvM2) * inWorldSpaceNormal;
+			ioResult.mAngularVelocity2 += inLambda * mInvI2_R2xAxis;
+		}
+
+		inline void		Solve(Vec3Arg inWorldSpaceNormal, float inInvM1, float inInvM2, float inMinLambda, float inMaxLambda, float &ioTotalLambda, CollisionEstimationResult &ioResult) const
+		{
+			// Calculate new total lambda
+			float total_lambda = ioTotalLambda + SolveGetLambda(inWorldSpaceNormal, ioResult);
+
+			// Clamp impulse
+			total_lambda = Clamp(total_lambda, inMinLambda, inMaxLambda);
+
+			SolveApplyLambda(inWorldSpaceNormal, inInvM1, inInvM2, total_lambda - ioTotalLambda, ioResult);
+
+			ioTotalLambda = total_lambda;
+		}
+
+		Vec3			mR1PlusUxAxis;
+		Vec3			mR2xAxis;
+		Vec3			mInvI1_R1PlusUxAxis;
+		Vec3			mInvI2_R2xAxis;
+		float			mEffectiveMass;
+		float			mBias;
+	};
+
+	struct Constraint
+	{
+		AxisConstraint	mContact;
+		AxisConstraint	mFriction1;
+		AxisConstraint	mFriction2;
+	};
+
+	// Initialize the constraint properties
+	Constraint constraints[ContactPoints::Capacity];
+	for (uint c = 0; c < num_points; ++c)
+	{
+		Constraint &constraint = constraints[c];
+
+		// Calculate contact points relative to body 1 and 2
+		Vec3 p = 0.5f * (inManifold.mRelativeContactPointsOn1[c] + inManifold.mRelativeContactPointsOn2[c]);
+		Vec3 r1 = p - com1;
+		Vec3 r2 = p - com2;
+
+		// Initialize contact constraint
+		constraint.mContact.Initialize(r1, r2, inManifold.mWorldSpaceNormal, inv_m1, inv_m2, inv_i1, inv_i2);
+
+		// Handle elastic collisions
+		if (inCombinedRestitution > 0.0f)
+		{
+			// Calculate velocity of contact point
+			Vec3 relative_velocity = outResult.mLinearVelocity2 + outResult.mAngularVelocity2.Cross(r2) - outResult.mLinearVelocity1 - outResult.mAngularVelocity1.Cross(r1);
+			float normal_velocity = relative_velocity.Dot(inManifold.mWorldSpaceNormal);
+
+			// If it is big enough, apply restitution
+			if (normal_velocity < -inMinVelocityForRestitution)
+				constraint.mContact.mBias = inCombinedRestitution * normal_velocity;
+		}
+
+		if (inCombinedFriction > 0.0f)
+		{
+			// Initialize friction constraints
+			constraint.mFriction1.Initialize(r1, r2, outResult.mTangent1, inv_m1, inv_m2, inv_i1, inv_i2);
+			constraint.mFriction2.Initialize(r1, r2, outResult.mTangent2, inv_m1, inv_m2, inv_i1, inv_i2);
+		}
+	}
+
+	// If there's only 1 contact point, we only need 1 iteration
+	int num_iterations = inCombinedFriction <= 0.0f && num_points == 1? 1 : inNumIterations;
+
+	// Solve iteratively
+	for (int iteration = 0; iteration < num_iterations; ++iteration)
+	{
+		// Solve friction constraints first
+		if (inCombinedFriction > 0.0f && iteration > 0) // For first iteration the contact impulse is zero so there's no point in applying friction
+			for (uint c = 0; c < num_points; ++c)
+			{
+				const Constraint &constraint = constraints[c];
+				CollisionEstimationResult::Impulse &impulse = outResult.mImpulses[c];
+
+				float lambda1 = impulse.mFrictionImpulse1 + constraint.mFriction1.SolveGetLambda(outResult.mTangent1, outResult);
+				float lambda2 = impulse.mFrictionImpulse2 + constraint.mFriction2.SolveGetLambda(outResult.mTangent2, outResult);
+
+				// Calculate max impulse based on contact impulse
+				float max_impulse = inCombinedFriction * impulse.mContactImpulse;
+
+				// If the total lambda that we will apply is too large, scale it back
+				float total_lambda_sq = Square(lambda1) + Square(lambda2);
+				if (total_lambda_sq > Square(max_impulse))
+				{
+					float scale = max_impulse / sqrt(total_lambda_sq);
+					lambda1 *= scale;
+					lambda2 *= scale;
+				}
+
+				constraint.mFriction1.SolveApplyLambda(outResult.mTangent1, inv_m1, inv_m2, lambda1 - impulse.mFrictionImpulse1, outResult);
+				constraint.mFriction2.SolveApplyLambda(outResult.mTangent2, inv_m1, inv_m2, lambda2 - impulse.mFrictionImpulse2, outResult);
+
+				impulse.mFrictionImpulse1 = lambda1;
+				impulse.mFrictionImpulse2 = lambda2;
+			}
+
+		// Solve contact constraints last
+		for (uint c = 0; c < num_points; ++c)
+			constraints[c].mContact.Solve(inManifold.mWorldSpaceNormal, inv_m1, inv_m2, 0.0f, FLT_MAX, outResult.mImpulses[c].mContactImpulse, outResult);
+	}
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/EstimateCollisionResponse.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/EstimateCollisionResponse.h
new file mode 100644
index 0000000000..45098d1477
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/EstimateCollisionResponse.h
@@ -0,0 +1,48 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/ContactListener.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// A structure that contains the estimated contact and friction impulses and the resulting body velocities
+struct CollisionEstimationResult
+{
+	Vec3			mLinearVelocity1;				///< The estimated linear velocity of body 1 after collision
+	Vec3			mAngularVelocity1;				///< The estimated angular velocity of body 1 after collision
+	Vec3			mLinearVelocity2;				///< The estimated linear velocity of body 2 after collision
+	Vec3			mAngularVelocity2;				///< The estimated angular velocity of body 2 after collision
+
+	Vec3			mTangent1;						///< Normalized tangent of contact normal
+	Vec3			mTangent2;						///< Second normalized tangent of contact normal (forms a basis with mTangent1 and mWorldSpaceNormal)
+
+	struct Impulse
+	{
+		float		mContactImpulse;				///< Estimated contact impulses (kg m / s)
+		float		mFrictionImpulse1;				///< Estimated friction impulses in the direction of tangent 1 (kg m / s)
+		float		mFrictionImpulse2;				///< Estimated friction impulses in the direction of tangent 2 (kg m / s)
+	};
+
+	using Impulses = StaticArray<Impulse, ContactPoints::Capacity>;
+
+	Impulses		mImpulses;
+};
+
+/// This function estimates the contact impulses and body velocity changes as a result of a collision.
+/// It can be used in the ContactListener::OnContactAdded to determine the strength of the collision to e.g. play a sound or trigger a particle system.
+/// This function is accurate when two bodies collide but will not be accurate when more than 2 bodies collide at the same time as it does not know about these other collisions.
+///
+/// @param inBody1 Colliding body 1
+/// @param inBody2 Colliding body 2
+/// @param inManifold The collision manifold
+/// @param outResult A structure that contains the estimated contact and friction impulses and the resulting body velocities
+/// @param inCombinedFriction The combined friction of body 1 and body 2 (see ContactSettings::mCombinedFriction)
+/// @param inCombinedRestitution The combined restitution of body 1 and body 2 (see ContactSettings::mCombinedRestitution)
+/// @param inMinVelocityForRestitution Minimal velocity required for restitution to be applied (see PhysicsSettings::mMinVelocityForRestitution)
+/// @param inNumIterations Number of iterations to use for the impulse estimation (see PhysicsSettings::mNumVelocitySteps, note you can probably use a lower number for a decent estimate). If you set the number of iterations to 1 then no friction will be calculated.
+JPH_EXPORT void EstimateCollisionResponse(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, CollisionEstimationResult &outResult, float inCombinedFriction, float inCombinedRestitution, float inMinVelocityForRestitution = 1.0f, uint inNumIterations = 10);
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilter.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilter.cpp
new file mode 100644
index 0000000000..80c7620fda
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilter.cpp
@@ -0,0 +1,32 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/GroupFilter.h>
+#include <Jolt/Core/StreamUtils.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT_BASE(GroupFilter)
+{
+	JPH_ADD_BASE_CLASS(GroupFilter, SerializableObject)
+}
+
+void GroupFilter::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(GetRTTI()->GetHash());
+}
+
+void GroupFilter::RestoreBinaryState(StreamIn &inStream)
+{
+	// RTTI hash is read in sRestoreFromBinaryState
+}
+
+GroupFilter::GroupFilterResult GroupFilter::sRestoreFromBinaryState(StreamIn &inStream)
+{
+	return StreamUtils::RestoreObject<GroupFilter>(inStream, &GroupFilter::RestoreBinaryState);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilter.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilter.h
new file mode 100644
index 0000000000..5e01043a8f
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilter.h
@@ -0,0 +1,41 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/Result.h>
+#include <Jolt/ObjectStream/SerializableObject.h>
+
+JPH_NAMESPACE_BEGIN
+
+class CollisionGroup;
+class StreamIn;
+class StreamOut;
+
+/// Abstract class that checks if two CollisionGroups collide
+class JPH_EXPORT GroupFilter : public SerializableObject, public RefTarget<GroupFilter>
+{
+	JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, GroupFilter)
+
+public:
+	/// Virtual destructor
+	virtual						~GroupFilter() override = default;
+
+	/// Check if two groups collide
+	virtual bool				CanCollide(const CollisionGroup &inGroup1, const CollisionGroup &inGroup2) const = 0;
+
+	/// Saves the contents of the group filter in binary form to inStream.
+	virtual void				SaveBinaryState(StreamOut &inStream) const;
+
+	using GroupFilterResult = Result<Ref<GroupFilter>>;
+
+	/// Creates a GroupFilter of the correct type and restores its contents from the binary stream inStream.
+	static GroupFilterResult	sRestoreFromBinaryState(StreamIn &inStream);
+
+protected:
+	/// This function should not be called directly, it is used by sRestoreFromBinaryState.
+	virtual void				RestoreBinaryState(StreamIn &inStream);
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilterTable.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilterTable.cpp
new file mode 100644
index 0000000000..dd69d27c85
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilterTable.cpp
@@ -0,0 +1,38 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/GroupFilterTable.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(GroupFilterTable)
+{
+	JPH_ADD_BASE_CLASS(GroupFilterTable, GroupFilter)
+
+	JPH_ADD_ATTRIBUTE(GroupFilterTable, mNumSubGroups)
+	JPH_ADD_ATTRIBUTE(GroupFilterTable, mTable)
+}
+
+void GroupFilterTable::SaveBinaryState(StreamOut &inStream) const
+{
+	GroupFilter::SaveBinaryState(inStream);
+
+	inStream.Write(mNumSubGroups);
+	inStream.Write(mTable);
+}
+
+void GroupFilterTable::RestoreBinaryState(StreamIn &inStream)
+{
+	GroupFilter::RestoreBinaryState(inStream);
+
+	inStream.Read(mNumSubGroups);
+	inStream.Read(mTable);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilterTable.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilterTable.h
new file mode 100644
index 0000000000..76cae277da
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/GroupFilterTable.h
@@ -0,0 +1,130 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/GroupFilter.h>
+#include <Jolt/Physics/Collision/CollisionGroup.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Implementation of GroupFilter that stores a bit table with one bit per sub shape ID pair to determine if they collide or not
+///
+/// The collision rules:
+/// - If one of the objects is in the cInvalidGroup the objects will collide.
+/// - If the objects are in different groups they will collide.
+/// - If they're in the same group but their collision filter is different they will not collide.
+/// - If they're in the same group and their collision filters match, we'll use the SubGroupID and the table below.
+///
+/// For N = 6 sub groups the table will look like:
+///
+///		            sub group 1 --->
+///		sub group 2 x.....
+///		     |      ox....
+///		     |      oox...
+///		     V      ooox..
+///		            oooox.
+///		            ooooox
+///
+/// * 'x' means sub group 1 == sub group 2 and we define this to never collide.
+/// * 'o' is a bit that we have to store that defines if the sub groups collide or not.
+/// * '.' is a bit we don't need to store because the table is symmetric, we take care that group 2 > group 1 by swapping sub group 1 and sub group 2 if needed.
+///
+/// The total number of bits we need to store is (N * (N - 1)) / 2
+class JPH_EXPORT GroupFilterTable final : public GroupFilter
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, GroupFilterTable)
+
+private:
+	using GroupID = CollisionGroup::GroupID;
+	using SubGroupID = CollisionGroup::SubGroupID;
+
+	/// Get which bit corresponds to the pair (inSubGroup1, inSubGroup2)
+	int						GetBit(SubGroupID inSubGroup1, SubGroupID inSubGroup2) const
+	{
+		JPH_ASSERT(inSubGroup1 != inSubGroup2);
+
+		// We store the lower left half only, so swap the inputs when trying to access the top right half
+		if (inSubGroup1 > inSubGroup2)
+			std::swap(inSubGroup1, inSubGroup2);
+
+		JPH_ASSERT(inSubGroup2 < mNumSubGroups);
+
+		// Calculate at which bit the entry for this pair resides
+		// We use the fact that a row always starts at inSubGroup2 * (inSubGroup2 - 1) / 2
+		// (this is the amount of bits needed to store a table of inSubGroup2 entries)
+		return (inSubGroup2 * (inSubGroup2 - 1)) / 2 + inSubGroup1;
+	}
+
+public:
+	/// Constructs the table with inNumSubGroups subgroups, initially all collision pairs are enabled except when the sub group ID is the same
+	explicit				GroupFilterTable(uint inNumSubGroups = 0) :
+		mNumSubGroups(inNumSubGroups)
+	{
+		// By default everything collides
+		int table_size = ((inNumSubGroups * (inNumSubGroups - 1)) / 2 + 7) / 8;
+		mTable.resize(table_size, 0xff);
+	}
+
+	/// Copy constructor
+							GroupFilterTable(const GroupFilterTable &inRHS) : mNumSubGroups(inRHS.mNumSubGroups), mTable(inRHS.mTable) { }
+
+	/// Disable collision between two sub groups
+	void					DisableCollision(SubGroupID inSubGroup1, SubGroupID inSubGroup2)
+	{
+		int bit = GetBit(inSubGroup1, inSubGroup2);
+		mTable[bit >> 3] &= (0xff ^ (1 << (bit & 0b111)));
+	}
+
+	/// Enable collision between two sub groups
+	void					EnableCollision(SubGroupID inSubGroup1, SubGroupID inSubGroup2)
+	{
+		int bit = GetBit(inSubGroup1, inSubGroup2);
+		mTable[bit >> 3] |= 1 << (bit & 0b111);
+	}
+
+	/// Check if the collision between two subgroups is enabled
+	inline bool				IsCollisionEnabled(SubGroupID inSubGroup1, SubGroupID inSubGroup2) const
+	{
+		// Test if the bit is set for this group pair
+		int bit = GetBit(inSubGroup1, inSubGroup2);
+		return (mTable[bit >> 3] & (1 << (bit & 0b111))) != 0;
+	}
+
+	/// Checks if two CollisionGroups collide
+	virtual bool			CanCollide(const CollisionGroup &inGroup1, const CollisionGroup &inGroup2) const override
+	{
+		// If one of the groups is cInvalidGroup the objects will collide (note that the if following this if will ensure that group2 is not cInvalidGroup)
+		if (inGroup1.GetGroupID() == CollisionGroup::cInvalidGroup)
+			return true;
+
+		// If the objects are in different groups, they collide
+		if (inGroup1.GetGroupID() != inGroup2.GetGroupID())
+			return true;
+
+		// If the collision filters do not match, but they're in the same group we ignore the collision
+		if (inGroup1.GetGroupFilter() != inGroup2.GetGroupFilter())
+			return false;
+
+		// If they are in the same sub group, they don't collide
+		if (inGroup1.GetSubGroupID() == inGroup2.GetSubGroupID())
+			return false;
+
+		// Check the bit table
+		return IsCollisionEnabled(inGroup1.GetSubGroupID(), inGroup2.GetSubGroupID());
+	}
+
+	// See: GroupFilter::SaveBinaryState
+	virtual void			SaveBinaryState(StreamOut &inStream) const override;
+
+protected:
+	// See: GroupFilter::RestoreBinaryState
+	virtual void			RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	uint					mNumSubGroups;									///< The number of subgroups that this group filter supports
+	Array<uint8>			mTable;											///< The table of bits that indicates which pairs collide
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/InternalEdgeRemovingCollector.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/InternalEdgeRemovingCollector.h
new file mode 100644
index 0000000000..0d67729125
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/InternalEdgeRemovingCollector.h
@@ -0,0 +1,250 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/QuickSort.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+
+//#define JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
+
+#ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
+#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
+
+JPH_NAMESPACE_BEGIN
+
+/// Removes internal edges from collision results. Can be used to filter out 'ghost collisions'.
+/// Based on: Contact generation for meshes - Pierre Terdiman (https://www.codercorner.com/MeshContacts.pdf)
+class InternalEdgeRemovingCollector : public CollideShapeCollector
+{
+	static constexpr uint cMaxDelayedResults = 16;
+	static constexpr uint cMaxVoidedFeatures = 128;
+
+	/// Check if a vertex is voided
+	inline bool				IsVoided(const SubShapeID &inSubShapeID, Vec3 inV) const
+	{
+		for (const Voided &vf : mVoidedFeatures)
+			if (vf.mSubShapeID == inSubShapeID
+				&& inV.IsClose(Vec3::sLoadFloat3Unsafe(vf.mFeature), 1.0e-8f))
+				return true;
+		return false;
+	}
+
+	/// Add all vertices of a face to the voided features
+	inline void				VoidFeatures(const CollideShapeResult &inResult)
+	{
+		if (mVoidedFeatures.size() < cMaxVoidedFeatures)
+			for (const Vec3 &v : inResult.mShape2Face)
+				if (!IsVoided(inResult.mSubShapeID1, v))
+				{
+					Voided vf;
+					v.StoreFloat3(&vf.mFeature);
+					vf.mSubShapeID = inResult.mSubShapeID1;
+					mVoidedFeatures.push_back(vf);
+					if (mVoidedFeatures.size() == cMaxVoidedFeatures)
+						break;
+				}
+	}
+
+	/// Call the chained collector
+	inline void				Chain(const CollideShapeResult &inResult)
+	{
+		// Make sure the chained collector has the same context as we do
+		mChainedCollector.SetContext(GetContext());
+
+		// Forward the hit
+		mChainedCollector.AddHit(inResult);
+
+		// If our chained collector updated its early out fraction, we need to follow
+		UpdateEarlyOutFraction(mChainedCollector.GetEarlyOutFraction());
+	}
+
+	/// Call the chained collector and void all features of inResult
+	inline void				ChainAndVoid(const CollideShapeResult &inResult)
+	{
+		Chain(inResult);
+		VoidFeatures(inResult);
+
+	#ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
+		DebugRenderer::sInstance->DrawWirePolygon(RMat44::sIdentity(), inResult.mShape2Face, Color::sGreen);
+		DebugRenderer::sInstance->DrawArrow(RVec3(inResult.mContactPointOn2), RVec3(inResult.mContactPointOn2) + inResult.mPenetrationAxis.NormalizedOr(Vec3::sZero()), Color::sGreen, 0.1f);
+	#endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
+	}
+
+public:
+	/// Constructor, configures a collector to be called with all the results that do not hit internal edges
+	explicit				InternalEdgeRemovingCollector(CollideShapeCollector &inChainedCollector) :
+		mChainedCollector(inChainedCollector)
+	{
+	}
+
+	// See: CollideShapeCollector::Reset
+	virtual void			Reset() override
+	{
+		CollideShapeCollector::Reset();
+
+		mChainedCollector.Reset();
+
+		mVoidedFeatures.clear();
+		mDelayedResults.clear();
+	}
+
+	// See: CollideShapeCollector::OnBody
+	virtual void			OnBody(const Body &inBody) override
+	{
+		// Just forward the call to our chained collector
+		mChainedCollector.OnBody(inBody);
+	}
+
+	// See: CollideShapeCollector::AddHit
+	virtual void			AddHit(const CollideShapeResult &inResult) override
+	{
+		// We only support welding when the shape is a triangle or has more vertices so that we can calculate a normal
+		if (inResult.mShape2Face.size() < 3)
+			return ChainAndVoid(inResult);
+
+		// Get the triangle normal of shape 2 face
+		Vec3 triangle_normal = (inResult.mShape2Face[1] - inResult.mShape2Face[0]).Cross(inResult.mShape2Face[2] - inResult.mShape2Face[0]);
+		float triangle_normal_len = triangle_normal.Length();
+		if (triangle_normal_len < 1e-6f)
+			return ChainAndVoid(inResult);
+
+		// If the triangle normal matches the contact normal within 1 degree, we can process the contact immediately
+		// We make the assumption here that if the contact normal and the triangle normal align that the we're dealing with a 'face contact'
+		Vec3 contact_normal = -inResult.mPenetrationAxis;
+		float contact_normal_len = inResult.mPenetrationAxis.Length();
+		if (triangle_normal.Dot(contact_normal) > 0.999848f * contact_normal_len * triangle_normal_len) // cos(1 degree)
+			return ChainAndVoid(inResult);
+
+		// Delayed processing
+		if (mDelayedResults.size() == cMaxDelayedResults)
+			return ChainAndVoid(inResult);
+		mDelayedResults.push_back(inResult);
+	}
+
+	/// After all hits have been added, call this function to process the delayed results
+	void					Flush()
+	{
+		// Sort on biggest penetration depth first
+		uint sorted_indices[cMaxDelayedResults];
+		for (uint i = 0; i < uint(mDelayedResults.size()); ++i)
+			sorted_indices[i] = i;
+		QuickSort(sorted_indices, sorted_indices + mDelayedResults.size(), [this](uint inLHS, uint inRHS) { return mDelayedResults[inLHS].mPenetrationDepth > mDelayedResults[inRHS].mPenetrationDepth; });
+
+		// Loop over all results
+		for (uint i = 0; i < uint(mDelayedResults.size()); ++i)
+		{
+			const CollideShapeResult &r = mDelayedResults[sorted_indices[i]];
+
+			// Determine which vertex or which edge is the closest to the contact point
+			float best_dist_sq = FLT_MAX;
+			uint best_v1_idx = 0;
+			uint best_v2_idx = 0;
+			uint num_v = uint(r.mShape2Face.size());
+			uint v1_idx = num_v - 1;
+			Vec3 v1 = r.mShape2Face[v1_idx] - r.mContactPointOn2;
+			for (uint v2_idx = 0; v2_idx < num_v; ++v2_idx)
+			{
+				Vec3 v2 = r.mShape2Face[v2_idx] - r.mContactPointOn2;
+				Vec3 v1_v2 = v2 - v1;
+				float denominator = v1_v2.LengthSq();
+				if (denominator < Square(FLT_EPSILON))
+				{
+					// Degenerate, assume v1 is closest, v2 will be tested in a later iteration
+					float v1_len_sq = v1.LengthSq();
+					if (v1_len_sq < best_dist_sq)
+					{
+						best_dist_sq = v1_len_sq;
+						best_v1_idx = v1_idx;
+						best_v2_idx = v1_idx;
+					}
+				}
+				else
+				{
+					// Taken from ClosestPoint::GetBaryCentricCoordinates
+					float fraction = -v1.Dot(v1_v2) / denominator;
+					if (fraction < 1.0e-6f)
+					{
+						// Closest lies on v1
+						float v1_len_sq = v1.LengthSq();
+						if (v1_len_sq < best_dist_sq)
+						{
+							best_dist_sq = v1_len_sq;
+							best_v1_idx = v1_idx;
+							best_v2_idx = v1_idx;
+						}
+					}
+					else if (fraction < 1.0f - 1.0e-6f)
+					{
+						// Closest lies on the line segment v1, v2
+						Vec3 closest = v1 + fraction * v1_v2;
+						float closest_len_sq = closest.LengthSq();
+						if (closest_len_sq < best_dist_sq)
+						{
+							best_dist_sq = closest_len_sq;
+							best_v1_idx = v1_idx;
+							best_v2_idx = v2_idx;
+						}
+					}
+					// else closest is v2, but v2 will be tested in a later iteration
+				}
+
+				v1_idx = v2_idx;
+				v1 = v2;
+			}
+
+			// Check if this vertex/edge is voided
+			bool voided = IsVoided(r.mSubShapeID1, r.mShape2Face[best_v1_idx])
+				&& (best_v1_idx == best_v2_idx || IsVoided(r.mSubShapeID1, r.mShape2Face[best_v2_idx]));
+
+		#ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
+			Color color = voided? Color::sRed : Color::sYellow;
+			DebugRenderer::sInstance->DrawText3D(RVec3(r.mContactPointOn2), StringFormat("%d: %g", i, r.mPenetrationDepth), color, 0.1f);
+			DebugRenderer::sInstance->DrawWirePolygon(RMat44::sIdentity(), r.mShape2Face, color);
+			DebugRenderer::sInstance->DrawArrow(RVec3(r.mContactPointOn2), RVec3(r.mContactPointOn2) + r.mPenetrationAxis.NormalizedOr(Vec3::sZero()), color, 0.1f);
+			DebugRenderer::sInstance->DrawMarker(RVec3(r.mShape2Face[best_v1_idx]), IsVoided(r.mSubShapeID1, r.mShape2Face[best_v1_idx])? Color::sRed : Color::sYellow, 0.1f);
+			DebugRenderer::sInstance->DrawMarker(RVec3(r.mShape2Face[best_v2_idx]), IsVoided(r.mSubShapeID1, r.mShape2Face[best_v2_idx])? Color::sRed : Color::sYellow, 0.1f);
+		#endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
+
+			// No voided features, accept the contact
+			if (!voided)
+				Chain(r);
+
+			// Void the features of this face
+			VoidFeatures(r);
+		}
+
+		// All delayed results have been processed
+		mVoidedFeatures.clear();
+		mDelayedResults.clear();
+	}
+
+	/// Version of CollisionDispatch::sCollideShapeVsShape that removes internal edges
+	static void				sCollideShapeVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter = { })
+	{
+		JPH_ASSERT(inCollideShapeSettings.mActiveEdgeMode == EActiveEdgeMode::CollideWithAll); // Won't work without colliding with all edges
+		JPH_ASSERT(inCollideShapeSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces); // Won't work without collecting faces
+
+		InternalEdgeRemovingCollector wrapper(ioCollector);
+		CollisionDispatch::sCollideShapeVsShape(inShape1, inShape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, wrapper, inShapeFilter);
+		wrapper.Flush();
+	}
+
+private:
+	// This algorithm tests a convex shape (shape 1) against a set of polygons (shape 2).
+	// This assumption doesn't hold if the shape we're testing is a compound shape, so we must also
+	// store the sub shape ID and ignore voided features that belong to another sub shape ID.
+	struct Voided
+	{
+		Float3				mFeature;				// Feature that is voided (of shape 2). Read with Vec3::sLoadFloat3Unsafe so must not be the last member.
+		SubShapeID			mSubShapeID;			// Sub shape ID of the shape that is colliding against the feature (of shape 1).
+	};
+
+	CollideShapeCollector &	mChainedCollector;
+	StaticArray<Voided, cMaxVoidedFeatures> mVoidedFeatures;
+	StaticArray<CollideShapeResult, cMaxDelayedResults> mDelayedResults;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.cpp
new file mode 100644
index 0000000000..3331f5d7a0
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.cpp
@@ -0,0 +1,237 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/ManifoldBetweenTwoFaces.h>
+#include <Jolt/Physics/Constraints/ContactConstraintManager.h>
+#include <Jolt/Geometry/ClipPoly.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+void PruneContactPoints(Vec3Arg inPenetrationAxis, ContactPoints &ioContactPointsOn1, ContactPoints &ioContactPointsOn2 JPH_IF_DEBUG_RENDERER(, RVec3Arg inCenterOfMass))
+{
+	// Makes no sense to call this with 4 or less points
+	JPH_ASSERT(ioContactPointsOn1.size() > 4);
+
+	// Both arrays should have the same size
+	JPH_ASSERT(ioContactPointsOn1.size() == ioContactPointsOn2.size());
+
+	// Penetration axis must be normalized
+	JPH_ASSERT(inPenetrationAxis.IsNormalized());
+
+	// We use a heuristic of (distance to center of mass) * (penetration depth) to find the contact point that we should keep
+	// Neither of those two terms should ever become zero, so we clamp against this minimum value
+	constexpr float cMinDistanceSq = 1.0e-6f; // 1 mm
+
+	ContactPoints projected;
+	StaticArray<float, 64> penetration_depth_sq;
+	for (ContactPoints::size_type i = 0; i < ioContactPointsOn1.size(); ++i)
+	{
+		// Project contact points on the plane through inCenterOfMass with normal inPenetrationAxis and center around the center of mass of body 1
+		// (note that since all points are relative to inCenterOfMass we can project onto the plane through the origin)
+		Vec3 v1 = ioContactPointsOn1[i];
+		projected.push_back(v1 - v1.Dot(inPenetrationAxis) * inPenetrationAxis);
+
+		// Calculate penetration depth^2 of each point and clamp against the minimal distance
+		Vec3 v2 = ioContactPointsOn2[i];
+		penetration_depth_sq.push_back(max(cMinDistanceSq, (v2 - v1).LengthSq()));
+	}
+
+	// Find the point that is furthest away from the center of mass (its torque will have the biggest influence)
+	// and the point that has the deepest penetration depth. Use the heuristic (distance to center of mass) * (penetration depth) for this.
+	uint point1 = 0;
+	float val = max(cMinDistanceSq, projected[0].LengthSq()) * penetration_depth_sq[0];
+	for (uint i = 0; i < projected.size(); ++i)
+	{
+		float v = max(cMinDistanceSq, projected[i].LengthSq()) * penetration_depth_sq[i];
+		if (v > val)
+		{
+			val = v;
+			point1 = i;
+		}
+	}
+	Vec3 point1v = projected[point1];
+
+	// Find point furthest from the first point forming a line segment with point1. Again combine this with the heuristic
+	// for deepest point as per above.
+	uint point2 = uint(-1);
+	val = -FLT_MAX;
+	for (uint i = 0; i < projected.size(); ++i)
+		if (i != point1)
+		{
+			float v = max(cMinDistanceSq, (projected[i] - point1v).LengthSq()) * penetration_depth_sq[i];
+			if (v > val)
+			{
+				val = v;
+				point2 = i;
+			}
+		}
+	JPH_ASSERT(point2 != uint(-1));
+	Vec3 point2v = projected[point2];
+
+	// Find furthest points on both sides of the line segment in order to maximize the area
+	uint point3 = uint(-1);
+	uint point4 = uint(-1);
+	float min_val = 0.0f;
+	float max_val = 0.0f;
+	Vec3 perp = (point2v - point1v).Cross(inPenetrationAxis);
+	for (uint i = 0; i < projected.size(); ++i)
+		if (i != point1 && i != point2)
+		{
+			float v = perp.Dot(projected[i] - point1v);
+			if (v < min_val)
+			{
+				min_val = v;
+				point3 = i;
+			}
+			else if (v > max_val)
+			{
+				max_val = v;
+				point4 = i;
+			}
+		}
+
+	// Add points to array (in order so they form a polygon)
+	StaticArray<Vec3, 4> points_to_keep_on_1, points_to_keep_on_2;
+	points_to_keep_on_1.push_back(ioContactPointsOn1[point1]);
+	points_to_keep_on_2.push_back(ioContactPointsOn2[point1]);
+	if (point3 != uint(-1))
+	{
+		points_to_keep_on_1.push_back(ioContactPointsOn1[point3]);
+		points_to_keep_on_2.push_back(ioContactPointsOn2[point3]);
+	}
+	points_to_keep_on_1.push_back(ioContactPointsOn1[point2]);
+	points_to_keep_on_2.push_back(ioContactPointsOn2[point2]);
+	if (point4 != uint(-1))
+	{
+		JPH_ASSERT(point3 != point4);
+		points_to_keep_on_1.push_back(ioContactPointsOn1[point4]);
+		points_to_keep_on_2.push_back(ioContactPointsOn2[point4]);
+	}
+
+#ifdef JPH_DEBUG_RENDERER
+	if (ContactConstraintManager::sDrawContactPointReduction)
+	{
+		// Draw input polygon
+		DebugRenderer::sInstance->DrawWirePolygon(RMat44::sTranslation(inCenterOfMass), ioContactPointsOn1, Color::sOrange, 0.05f);
+
+		// Draw primary axis
+		DebugRenderer::sInstance->DrawArrow(inCenterOfMass + ioContactPointsOn1[point1], inCenterOfMass + ioContactPointsOn1[point2], Color::sRed, 0.05f);
+
+		// Draw contact points we kept
+		for (Vec3 p : points_to_keep_on_1)
+			DebugRenderer::sInstance->DrawMarker(inCenterOfMass + p, Color::sGreen, 0.1f);
+	}
+#endif // JPH_DEBUG_RENDERER
+
+	// Copy the points back to the input buffer
+	ioContactPointsOn1 = points_to_keep_on_1;
+	ioContactPointsOn2 = points_to_keep_on_2;
+}
+
+void ManifoldBetweenTwoFaces(Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, Vec3Arg inPenetrationAxis, float inMaxContactDistanceSq	, const ConvexShape::SupportingFace &inShape1Face, const ConvexShape::SupportingFace &inShape2Face, ContactPoints &outContactPoints1, ContactPoints &outContactPoints2 JPH_IF_DEBUG_RENDERER(, RVec3Arg inCenterOfMass))
+{
+#ifdef JPH_DEBUG_RENDERER
+	if (ContactConstraintManager::sDrawContactPoint)
+	{
+		RVec3 cp1 = inCenterOfMass + inContactPoint1;
+		RVec3 cp2 = inCenterOfMass + inContactPoint2;
+
+		// Draw contact points
+		DebugRenderer::sInstance->DrawMarker(cp1, Color::sRed, 0.1f);
+		DebugRenderer::sInstance->DrawMarker(cp2, Color::sGreen, 0.1f);
+
+		// Draw contact normal
+		DebugRenderer::sInstance->DrawArrow(cp1, cp1 + inPenetrationAxis.Normalized(), Color::sRed, 0.05f);
+	}
+#endif // JPH_DEBUG_RENDERER
+
+	// Remember size before adding new points, to check at the end if we added some
+	ContactPoints::size_type old_size = outContactPoints1.size();
+
+	// Check if both shapes have polygon faces
+	if (inShape1Face.size() >= 2 // The dynamic shape needs to have at least 2 points or else there can never be more than 1 contact point
+		&& inShape2Face.size() >= 3) // The dynamic/static shape needs to have at least 3 points (in the case that it has 2 points only if the edges match exactly you can have 2 contact points, but this situation is unstable anyhow)
+	{
+		// Clip the polygon of face 2 against that of 1
+		ConvexShape::SupportingFace clipped_face;
+		if (inShape1Face.size() >= 3)
+			ClipPolyVsPoly(inShape2Face, inShape1Face, inPenetrationAxis, clipped_face);
+		else if (inShape1Face.size() == 2)
+			ClipPolyVsEdge(inShape2Face, inShape1Face[0], inShape1Face[1], inPenetrationAxis, clipped_face);
+
+		// Project the points back onto the plane of shape 1 face and only keep those that are behind the plane
+		Vec3 plane_origin = inShape1Face[0];
+		Vec3 plane_normal;
+		Vec3 first_edge = inShape1Face[1] - plane_origin;
+		if (inShape1Face.size() >= 3)
+		{
+			// Three vertices, can just calculate the normal
+			plane_normal = first_edge.Cross(inShape1Face[2] - plane_origin);
+		}
+		else
+		{
+			// Two vertices, first find a perpendicular to the edge and penetration axis and then use the perpendicular together with the edge to form a normal
+			plane_normal = first_edge.Cross(inPenetrationAxis).Cross(first_edge);
+		}
+
+		// Check if the plane normal has any length, if not the clipped shape is so small that we'll just use the contact points
+		float plane_normal_len_sq = plane_normal.LengthSq();
+		if (plane_normal_len_sq > 0.0f)
+		{
+			// Discard points of faces that are too far away to collide
+			for (Vec3 p2 : clipped_face)
+			{
+				float distance = (p2 - plane_origin).Dot(plane_normal); // Note should divide by length of plane_normal (unnormalized here)
+				if (distance <= 0.0f || Square(distance) < inMaxContactDistanceSq * plane_normal_len_sq) // Must be close enough to plane, note we correct for not dividing by plane normal length here
+				{
+					// Project point back on shape 1 using the normal, note we correct for not dividing by plane normal length here:
+					// p1 = p2 - (distance / sqrt(plane_normal_len_sq)) * (plane_normal / sqrt(plane_normal_len_sq));
+					Vec3 p1 = p2 - (distance / plane_normal_len_sq) * plane_normal;
+
+					outContactPoints1.push_back(p1);
+					outContactPoints2.push_back(p2);
+				}
+			}
+		}
+
+	#ifdef JPH_DEBUG_RENDERER
+		if (ContactConstraintManager::sDrawSupportingFaces)
+		{
+			RMat44 com = RMat44::sTranslation(inCenterOfMass);
+
+			// Draw clipped poly
+			DebugRenderer::sInstance->DrawWirePolygon(com, clipped_face, Color::sOrange);
+
+			// Draw supporting faces
+			DebugRenderer::sInstance->DrawWirePolygon(com, inShape1Face, Color::sRed, 0.05f);
+			DebugRenderer::sInstance->DrawWirePolygon(com, inShape2Face, Color::sGreen, 0.05f);
+
+			// Draw normal
+			if (plane_normal_len_sq > 0.0f)
+			{
+				RVec3 plane_origin_ws = inCenterOfMass + plane_origin;
+				DebugRenderer::sInstance->DrawArrow(plane_origin_ws, plane_origin_ws + plane_normal / sqrt(plane_normal_len_sq), Color::sYellow, 0.05f);
+			}
+
+			// Draw contact points that remain after distance check
+			for (ContactPoints::size_type p = old_size; p < outContactPoints1.size(); ++p)
+				DebugRenderer::sInstance->DrawMarker(inCenterOfMass + outContactPoints1[p], Color::sYellow, 0.1f);
+		}
+	#endif // JPH_DEBUG_RENDERER
+	}
+
+	// If the clipping result is empty, use the contact point itself
+	if (outContactPoints1.size() == old_size)
+	{
+		outContactPoints1.push_back(inContactPoint1);
+		outContactPoints2.push_back(inContactPoint2);
+	}
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.h
new file mode 100644
index 0000000000..72a5b8f0c1
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.h
@@ -0,0 +1,44 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/ConvexShape.h>
+#include <Jolt/Physics/Collision/ContactListener.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Remove contact points if there are > 4 (no more than 4 are needed for a stable solution)
+/// @param inPenetrationAxis is the world space penetration axis (must be normalized)
+/// @param ioContactPointsOn1 The contact points on shape 1 relative to inCenterOfMass
+/// @param ioContactPointsOn2 The contact points on shape 2 relative to inCenterOfMass
+/// On output ioContactPointsOn1/2 are reduced to 4 or less points
+#ifdef JPH_DEBUG_RENDERER
+/// @param inCenterOfMass Center of mass position of body 1
+#endif
+JPH_EXPORT void PruneContactPoints(Vec3Arg inPenetrationAxis, ContactPoints &ioContactPointsOn1, ContactPoints &ioContactPointsOn2
+#ifdef JPH_DEBUG_RENDERER
+	, RVec3Arg inCenterOfMass
+#endif
+	);
+
+/// Determine contact points between 2 faces of 2 shapes and return them in outContactPoints 1 & 2
+/// @param inContactPoint1 The contact point on shape 1 relative to inCenterOfMass
+/// @param inContactPoint2 The contact point on shape 2 relative to inCenterOfMass
+/// @param inPenetrationAxis The local space penetration axis in world space
+/// @param inMaxContactDistanceSq After face 2 is clipped against face 1, each remaining point on face 2 is tested against the plane of face 1. If the distance^2 on the positive side of the plane is larger than this distance, the point will be discarded as a contact point.
+/// @param inShape1Face The supporting faces on shape 1 relative to inCenterOfMass
+/// @param inShape2Face The supporting faces on shape 2 relative to inCenterOfMass
+/// @param outContactPoints1 Returns the contact points between the two shapes for shape 1 relative to inCenterOfMass (any existing points in the output array are left as is)
+/// @param outContactPoints2 Returns the contact points between the two shapes for shape 2 relative to inCenterOfMass (any existing points in the output array are left as is)
+#ifdef JPH_DEBUG_RENDERER
+/// @param inCenterOfMass Center of mass position of body 1
+#endif
+JPH_EXPORT void ManifoldBetweenTwoFaces(Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, Vec3Arg inPenetrationAxis, float inMaxContactDistanceSq, const ConvexShape::SupportingFace &inShape1Face, const ConvexShape::SupportingFace &inShape2Face, ContactPoints &outContactPoints1, ContactPoints &outContactPoints2
+#ifdef JPH_DEBUG_RENDERER
+	, RVec3Arg inCenterOfMass
+#endif
+	);
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseQuery.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseQuery.cpp
new file mode 100644
index 0000000000..4431a1047e
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseQuery.cpp
@@ -0,0 +1,493 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/NarrowPhaseQuery.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/AABoxCast.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/CollideShape.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/InternalEdgeRemovingCollector.h>
+
+JPH_NAMESPACE_BEGIN
+
+bool NarrowPhaseQuery::CastRay(const RRayCast &inRay, RayCastResult &ioHit, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	class MyCollector : public RayCastBodyCollector
+	{
+	public:
+							MyCollector(const RRayCast &inRay, RayCastResult &ioHit, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter) :
+			mRay(inRay),
+			mHit(ioHit),
+			mBodyLockInterface(inBodyLockInterface),
+			mBodyFilter(inBodyFilter)
+		{
+			UpdateEarlyOutFraction(ioHit.mFraction);
+		}
+
+		virtual void		AddHit(const ResultType &inResult) override
+		{
+			JPH_ASSERT(inResult.mFraction < mHit.mFraction, "This hit should not have been passed on to the collector");
+
+			// Only test shape if it passes the body filter
+			if (mBodyFilter.ShouldCollide(inResult.mBodyID))
+			{
+				// Lock the body
+				BodyLockRead lock(mBodyLockInterface, inResult.mBodyID);
+				if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks
+				{
+					const Body &body = lock.GetBody();
+
+					// Check body filter again now that we've locked the body
+					if (mBodyFilter.ShouldCollideLocked(body))
+					{
+						// Collect the transformed shape
+						TransformedShape ts = body.GetTransformedShape();
+
+						// Release the lock now, we have all the info we need in the transformed shape
+						lock.ReleaseLock();
+
+						// Do narrow phase collision check
+						if (ts.CastRay(mRay, mHit))
+						{
+							// Test that we didn't find a further hit by accident
+							JPH_ASSERT(mHit.mFraction >= 0.0f && mHit.mFraction < GetEarlyOutFraction());
+
+							// Update early out fraction based on narrow phase collector
+							UpdateEarlyOutFraction(mHit.mFraction);
+						}
+					}
+				}
+			}
+		}
+
+		RRayCast					mRay;
+		RayCastResult &				mHit;
+		const BodyLockInterface &	mBodyLockInterface;
+		const BodyFilter &			mBodyFilter;
+	};
+
+	// Do broadphase test, note that the broadphase uses floats so we drop precision here
+	MyCollector collector(inRay, ioHit, *mBodyLockInterface, inBodyFilter);
+	mBroadPhaseQuery->CastRay(RayCast(inRay), collector, inBroadPhaseLayerFilter, inObjectLayerFilter);
+	return ioHit.mFraction <= 1.0f;
+}
+
+void NarrowPhaseQuery::CastRay(const RRayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	class MyCollector : public RayCastBodyCollector
+	{
+	public:
+							MyCollector(const RRayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) :
+			RayCastBodyCollector(ioCollector),
+			mRay(inRay),
+			mRayCastSettings(inRayCastSettings),
+			mCollector(ioCollector),
+			mBodyLockInterface(inBodyLockInterface),
+			mBodyFilter(inBodyFilter),
+			mShapeFilter(inShapeFilter)
+		{
+		}
+
+		virtual void		AddHit(const ResultType &inResult) override
+		{
+			JPH_ASSERT(inResult.mFraction < mCollector.GetEarlyOutFraction(), "This hit should not have been passed on to the collector");
+
+			// Only test shape if it passes the body filter
+			if (mBodyFilter.ShouldCollide(inResult.mBodyID))
+			{
+				// Lock the body
+				BodyLockRead lock(mBodyLockInterface, inResult.mBodyID);
+				if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks
+				{
+					const Body &body = lock.GetBody();
+
+					// Check body filter again now that we've locked the body
+					if (mBodyFilter.ShouldCollideLocked(body))
+					{
+						// Collect the transformed shape
+						TransformedShape ts = body.GetTransformedShape();
+
+						// Notify collector of new body
+						mCollector.OnBody(body);
+
+						// Release the lock now, we have all the info we need in the transformed shape
+						lock.ReleaseLock();
+
+						// Do narrow phase collision check
+						ts.CastRay(mRay, mRayCastSettings, mCollector, mShapeFilter);
+
+						// Update early out fraction based on narrow phase collector
+						UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
+					}
+				}
+			}
+		}
+
+		RRayCast					mRay;
+		RayCastSettings				mRayCastSettings;
+		CastRayCollector &			mCollector;
+		const BodyLockInterface &	mBodyLockInterface;
+		const BodyFilter &			mBodyFilter;
+		const ShapeFilter &			mShapeFilter;
+	};
+
+	// Do broadphase test, note that the broadphase uses floats so we drop precision here
+	MyCollector collector(inRay, inRayCastSettings, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter);
+	mBroadPhaseQuery->CastRay(RayCast(inRay), collector, inBroadPhaseLayerFilter, inObjectLayerFilter);
+}
+
+void NarrowPhaseQuery::CollidePoint(RVec3Arg inPoint, CollidePointCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	class MyCollector : public CollideShapeBodyCollector
+	{
+	public:
+							MyCollector(RVec3Arg inPoint, CollidePointCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) :
+			CollideShapeBodyCollector(ioCollector),
+			mPoint(inPoint),
+			mCollector(ioCollector),
+			mBodyLockInterface(inBodyLockInterface),
+			mBodyFilter(inBodyFilter),
+			mShapeFilter(inShapeFilter)
+		{
+		}
+
+		virtual void		AddHit(const ResultType &inResult) override
+		{
+			// Only test shape if it passes the body filter
+			if (mBodyFilter.ShouldCollide(inResult))
+			{
+				// Lock the body
+				BodyLockRead lock(mBodyLockInterface, inResult);
+				if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks
+				{
+					const Body &body = lock.GetBody();
+
+					// Check body filter again now that we've locked the body
+					if (mBodyFilter.ShouldCollideLocked(body))
+					{
+						// Collect the transformed shape
+						TransformedShape ts = body.GetTransformedShape();
+
+						// Notify collector of new body
+						mCollector.OnBody(body);
+
+						// Release the lock now, we have all the info we need in the transformed shape
+						lock.ReleaseLock();
+
+						// Do narrow phase collision check
+						ts.CollidePoint(mPoint, mCollector, mShapeFilter);
+
+						// Update early out fraction based on narrow phase collector
+						UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
+					}
+				}
+			}
+		}
+
+		RVec3							mPoint;
+		CollidePointCollector &			mCollector;
+		const BodyLockInterface &		mBodyLockInterface;
+		const BodyFilter &				mBodyFilter;
+		const ShapeFilter &				mShapeFilter;
+	};
+
+	// Do broadphase test (note: truncates double to single precision since the broadphase uses single precision)
+	MyCollector collector(inPoint, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter);
+	mBroadPhaseQuery->CollidePoint(Vec3(inPoint), collector, inBroadPhaseLayerFilter, inObjectLayerFilter);
+}
+
+void NarrowPhaseQuery::CollideShape(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	class MyCollector : public CollideShapeBodyCollector
+	{
+	public:
+							MyCollector(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) :
+			CollideShapeBodyCollector(ioCollector),
+			mShape(inShape),
+			mShapeScale(inShapeScale),
+			mCenterOfMassTransform(inCenterOfMassTransform),
+			mCollideShapeSettings(inCollideShapeSettings),
+			mBaseOffset(inBaseOffset),
+			mCollector(ioCollector),
+			mBodyLockInterface(inBodyLockInterface),
+			mBodyFilter(inBodyFilter),
+			mShapeFilter(inShapeFilter)
+		{
+		}
+
+		virtual void		AddHit(const ResultType &inResult) override
+		{
+			// Only test shape if it passes the body filter
+			if (mBodyFilter.ShouldCollide(inResult))
+			{
+				// Lock the body
+				BodyLockRead lock(mBodyLockInterface, inResult);
+				if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks
+				{
+					const Body &body = lock.GetBody();
+
+					// Check body filter again now that we've locked the body
+					if (mBodyFilter.ShouldCollideLocked(body))
+					{
+						// Collect the transformed shape
+						TransformedShape ts = body.GetTransformedShape();
+
+						// Notify collector of new body
+						mCollector.OnBody(body);
+
+						// Release the lock now, we have all the info we need in the transformed shape
+						lock.ReleaseLock();
+
+						// Do narrow phase collision check
+						ts.CollideShape(mShape, mShapeScale, mCenterOfMassTransform, mCollideShapeSettings, mBaseOffset, mCollector, mShapeFilter);
+
+						// Update early out fraction based on narrow phase collector
+						UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
+					}
+				}
+			}
+		}
+
+		const Shape *					mShape;
+		Vec3							mShapeScale;
+		RMat44							mCenterOfMassTransform;
+		const CollideShapeSettings &	mCollideShapeSettings;
+		RVec3							mBaseOffset;
+		CollideShapeCollector &			mCollector;
+		const BodyLockInterface &		mBodyLockInterface;
+		const BodyFilter &				mBodyFilter;
+		const ShapeFilter &				mShapeFilter;
+	};
+
+	// Calculate bounds for shape and expand by max separation distance
+	AABox bounds = inShape->GetWorldSpaceBounds(inCenterOfMassTransform, inShapeScale);
+	bounds.ExpandBy(Vec3::sReplicate(inCollideShapeSettings.mMaxSeparationDistance));
+
+	// Do broadphase test
+	MyCollector collector(inShape, inShapeScale, inCenterOfMassTransform, inCollideShapeSettings, inBaseOffset, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter);
+	mBroadPhaseQuery->CollideAABox(bounds, collector, inBroadPhaseLayerFilter, inObjectLayerFilter);
+}
+
+void NarrowPhaseQuery::CollideShapeWithInternalEdgeRemoval(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	class MyCollector : public CollideShapeBodyCollector
+	{
+	public:
+							MyCollector(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) :
+			CollideShapeBodyCollector(ioCollector),
+			mShape(inShape),
+			mShapeScale(inShapeScale),
+			mCenterOfMassTransform(inCenterOfMassTransform),
+			mBaseOffset(inBaseOffset),
+			mBodyLockInterface(inBodyLockInterface),
+			mBodyFilter(inBodyFilter),
+			mShapeFilter(inShapeFilter),
+			mCollideShapeSettings(inCollideShapeSettings),
+			mCollector(ioCollector)
+		{
+			// We require these settings for internal edge removal to work
+			mCollideShapeSettings.mActiveEdgeMode = EActiveEdgeMode::CollideWithAll;
+			mCollideShapeSettings.mCollectFacesMode = ECollectFacesMode::CollectFaces;
+		}
+
+		virtual void		AddHit(const ResultType &inResult) override
+		{
+			// Only test shape if it passes the body filter
+			if (mBodyFilter.ShouldCollide(inResult))
+			{
+				// Lock the body
+				BodyLockRead lock(mBodyLockInterface, inResult);
+				if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks
+				{
+					const Body &body = lock.GetBody();
+
+					// Check body filter again now that we've locked the body
+					if (mBodyFilter.ShouldCollideLocked(body))
+					{
+						// Collect the transformed shape
+						TransformedShape ts = body.GetTransformedShape();
+
+						// Notify collector of new body
+						mCollector.OnBody(body);
+
+						// Release the lock now, we have all the info we need in the transformed shape
+						lock.ReleaseLock();
+
+						// Do narrow phase collision check
+						ts.CollideShape(mShape, mShapeScale, mCenterOfMassTransform, mCollideShapeSettings, mBaseOffset, mCollector, mShapeFilter);
+
+						// After each body, we need to flush the InternalEdgeRemovingCollector because it uses 'ts' as context and it will go out of scope at the end of this block
+						mCollector.Flush();
+
+						// Update early out fraction based on narrow phase collector
+						UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
+					}
+				}
+			}
+		}
+
+		const Shape *					mShape;
+		Vec3							mShapeScale;
+		RMat44							mCenterOfMassTransform;
+		RVec3							mBaseOffset;
+		const BodyLockInterface &		mBodyLockInterface;
+		const BodyFilter &				mBodyFilter;
+		const ShapeFilter &				mShapeFilter;
+		CollideShapeSettings			mCollideShapeSettings;
+		InternalEdgeRemovingCollector	mCollector;
+	};
+
+	// Calculate bounds for shape and expand by max separation distance
+	AABox bounds = inShape->GetWorldSpaceBounds(inCenterOfMassTransform, inShapeScale);
+	bounds.ExpandBy(Vec3::sReplicate(inCollideShapeSettings.mMaxSeparationDistance));
+
+	// Do broadphase test
+	MyCollector collector(inShape, inShapeScale, inCenterOfMassTransform, inCollideShapeSettings, inBaseOffset, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter);
+	mBroadPhaseQuery->CollideAABox(bounds, collector, inBroadPhaseLayerFilter, inObjectLayerFilter);
+}
+
+void NarrowPhaseQuery::CastShape(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	class MyCollector : public CastShapeBodyCollector
+	{
+	public:
+							MyCollector(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) :
+			CastShapeBodyCollector(ioCollector),
+			mShapeCast(inShapeCast),
+			mShapeCastSettings(inShapeCastSettings),
+			mBaseOffset(inBaseOffset),
+			mCollector(ioCollector),
+			mBodyLockInterface(inBodyLockInterface),
+			mBodyFilter(inBodyFilter),
+			mShapeFilter(inShapeFilter)
+		{
+		}
+
+		virtual void		AddHit(const ResultType &inResult) override
+		{
+			JPH_ASSERT(inResult.mFraction <= max(0.0f, mCollector.GetEarlyOutFraction()), "This hit should not have been passed on to the collector");
+
+			// Only test shape if it passes the body filter
+			if (mBodyFilter.ShouldCollide(inResult.mBodyID))
+			{
+				// Lock the body
+				BodyLockRead lock(mBodyLockInterface, inResult.mBodyID);
+				if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks
+				{
+					const Body &body = lock.GetBody();
+
+					// Check body filter again now that we've locked the body
+					if (mBodyFilter.ShouldCollideLocked(body))
+					{
+						// Collect the transformed shape
+						TransformedShape ts = body.GetTransformedShape();
+
+						// Notify collector of new body
+						mCollector.OnBody(body);
+
+						// Release the lock now, we have all the info we need in the transformed shape
+						lock.ReleaseLock();
+
+						// Do narrow phase collision check
+						ts.CastShape(mShapeCast, mShapeCastSettings, mBaseOffset, mCollector, mShapeFilter);
+
+						// Update early out fraction based on narrow phase collector
+						UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
+					}
+				}
+			}
+		}
+
+		RShapeCast					mShapeCast;
+		const ShapeCastSettings &	mShapeCastSettings;
+		RVec3						mBaseOffset;
+		CastShapeCollector &		mCollector;
+		const BodyLockInterface &	mBodyLockInterface;
+		const BodyFilter &			mBodyFilter;
+		const ShapeFilter &			mShapeFilter;
+	};
+
+	// Do broadphase test
+	MyCollector collector(inShapeCast, inShapeCastSettings, inBaseOffset, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter);
+	mBroadPhaseQuery->CastAABox({ inShapeCast.mShapeWorldBounds, inShapeCast.mDirection }, collector, inBroadPhaseLayerFilter, inObjectLayerFilter);
+}
+
+void NarrowPhaseQuery::CollectTransformedShapes(const AABox &inBox, TransformedShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
+{
+	class MyCollector : public CollideShapeBodyCollector
+	{
+	public:
+							MyCollector(const AABox &inBox, TransformedShapeCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) :
+			CollideShapeBodyCollector(ioCollector),
+			mBox(inBox),
+			mCollector(ioCollector),
+			mBodyLockInterface(inBodyLockInterface),
+			mBodyFilter(inBodyFilter),
+			mShapeFilter(inShapeFilter)
+		{
+		}
+
+		virtual void		AddHit(const ResultType &inResult) override
+		{
+			// Only test shape if it passes the body filter
+			if (mBodyFilter.ShouldCollide(inResult))
+			{
+				// Lock the body
+				BodyLockRead lock(mBodyLockInterface, inResult);
+				if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks
+				{
+					const Body &body = lock.GetBody();
+
+					// Check body filter again now that we've locked the body
+					if (mBodyFilter.ShouldCollideLocked(body))
+					{
+						// Collect the transformed shape
+						TransformedShape ts = body.GetTransformedShape();
+
+						// Notify collector of new body
+						mCollector.OnBody(body);
+
+						// Release the lock now, we have all the info we need in the transformed shape
+						lock.ReleaseLock();
+
+						// Do narrow phase collision check
+						ts.CollectTransformedShapes(mBox, mCollector, mShapeFilter);
+
+						// Update early out fraction based on narrow phase collector
+						UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
+					}
+				}
+			}
+		}
+
+		const AABox &					mBox;
+		TransformedShapeCollector &		mCollector;
+		const BodyLockInterface &		mBodyLockInterface;
+		const BodyFilter &				mBodyFilter;
+		const ShapeFilter &				mShapeFilter;
+	};
+
+	// Do broadphase test
+	MyCollector collector(inBox, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter);
+	mBroadPhaseQuery->CollideAABox(inBox, collector, inBroadPhaseLayerFilter, inObjectLayerFilter);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseQuery.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseQuery.h
new file mode 100644
index 0000000000..087cc6ca3f
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseQuery.h
@@ -0,0 +1,77 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/BodyFilter.h>
+#include <Jolt/Physics/Body/BodyLock.h>
+#include <Jolt/Physics/Body/BodyLockInterface.h>
+#include <Jolt/Physics/Collision/ShapeFilter.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h>
+#include <Jolt/Physics/Collision/BackFaceMode.h>
+
+JPH_NAMESPACE_BEGIN
+
+class Shape;
+class CollideShapeSettings;
+class RayCastResult;
+
+/// Class that provides an interface for doing precise collision detection against the broad and then the narrow phase.
+/// Unlike a BroadPhaseQuery, the NarrowPhaseQuery will test against shapes and will return collision information against triangles, spheres etc.
+class JPH_EXPORT NarrowPhaseQuery : public NonCopyable
+{
+public:
+	/// Initialize the interface (should only be called by PhysicsSystem)
+	void						Init(BodyLockInterface &inBodyLockInterface, BroadPhaseQuery &inBroadPhaseQuery) { mBodyLockInterface = &inBodyLockInterface; mBroadPhaseQuery = &inBroadPhaseQuery; }
+
+	/// Cast a ray and find the closest hit. Returns true if it finds a hit. Hits further than ioHit.mFraction will not be considered and in this case ioHit will remain unmodified (and the function will return false).
+	/// Convex objects will be treated as solid (meaning if the ray starts inside, you'll get a hit fraction of 0) and back face hits against triangles are returned.
+	/// If you want the surface normal of the hit use Body::GetWorldSpaceSurfaceNormal(ioHit.mSubShapeID2, inRay.GetPointOnRay(ioHit.mFraction)) on body with ID ioHit.mBodyID.
+	bool						CastRay(const RRayCast &inRay, RayCastResult &ioHit, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }) const;
+
+	/// Cast a ray, allows collecting multiple hits. Note that this version is more flexible but also slightly slower than the CastRay function that returns only a single hit.
+	/// If you want the surface normal of the hit use Body::GetWorldSpaceSurfaceNormal(collected sub shape ID, inRay.GetPointOnRay(collected fraction)) on body with collected body ID.
+	void						CastRay(const RRayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const;
+
+	/// Check if inPoint is inside any shapes. For this tests all shapes are treated as if they were solid.
+	/// For a mesh shape, this test will only provide sensible information if the mesh is a closed manifold.
+	/// For each shape that collides, ioCollector will receive a hit
+	void						CollidePoint(RVec3Arg inPoint, CollidePointCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const;
+
+	/// Collide a shape with the system
+	/// @param inShape Shape to test
+	/// @param inShapeScale Scale in local space of shape
+	/// @param inCenterOfMassTransform Center of mass transform for the shape
+	/// @param inCollideShapeSettings Settings
+	/// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. inCenterOfMassTransform.GetTranslation() since floats are most accurate near the origin
+	/// @param ioCollector Collector that receives the hits
+	/// @param inBroadPhaseLayerFilter Filter that filters at broadphase level
+	/// @param inObjectLayerFilter Filter that filters at layer level
+	/// @param inBodyFilter Filter that filters at body level
+	/// @param inShapeFilter Filter that filters at shape level
+	void						CollideShape(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const;
+
+	/// Same as CollideShape, but uses InternalEdgeRemovingCollector to remove internal edges from the collision results (a.k.a. ghost collisions)
+	void						CollideShapeWithInternalEdgeRemoval(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const;
+
+	/// Cast a shape and report any hits to ioCollector
+	/// @param inShapeCast The shape cast and its position and direction
+	/// @param inShapeCastSettings Settings for the shape cast
+	/// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. inShapeCast.mCenterOfMassStart.GetTranslation() since floats are most accurate near the origin
+	/// @param ioCollector Collector that receives the hits
+	/// @param inBroadPhaseLayerFilter Filter that filters at broadphase level
+	/// @param inObjectLayerFilter Filter that filters at layer level
+	/// @param inBodyFilter Filter that filters at body level
+	/// @param inShapeFilter Filter that filters at shape level
+	void						CastShape(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const;
+
+	/// Collect all leaf transformed shapes that fall inside world space box inBox
+	void						CollectTransformedShapes(const AABox &inBox, TransformedShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const;
+
+private:
+	BodyLockInterface *			mBodyLockInterface = nullptr;
+	BroadPhaseQuery *			mBroadPhaseQuery = nullptr;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseStats.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseStats.cpp
new file mode 100644
index 0000000000..69c6113725
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseStats.cpp
@@ -0,0 +1,62 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/NarrowPhaseStats.h>
+
+#ifdef JPH_TRACK_NARROWPHASE_STATS
+
+JPH_NAMESPACE_BEGIN
+
+NarrowPhaseStat	NarrowPhaseStat::sCollideShape[NumSubShapeTypes][NumSubShapeTypes];
+NarrowPhaseStat	NarrowPhaseStat::sCastShape[NumSubShapeTypes][NumSubShapeTypes];
+
+thread_local TrackNarrowPhaseStat *TrackNarrowPhaseStat::sRoot = nullptr;
+
+void NarrowPhaseStat::ReportStats(const char *inName, EShapeSubType inType1, EShapeSubType inType2, uint64 inTicks100Pct) const
+{
+	double total_pct = 100.0 * double(mTotalTicks) / double(inTicks100Pct);
+	double total_pct_excl_children = 100.0 * double(mTotalTicks - mChildTicks) / double(inTicks100Pct);
+
+	std::stringstream str;
+	str << inName << ", " << sSubShapeTypeNames[(int)inType1] << ", " << sSubShapeTypeNames[(int)inType2] << ", " << mNumQueries << ", " << total_pct << ", " << total_pct_excl_children << ", " << total_pct_excl_children / mNumQueries << ", " << mHitsReported;
+	Trace(str.str().c_str());
+}
+
+void NarrowPhaseStat::sReportStats()
+{
+	Trace("Query Type, Shape Type 1, Shape Type 2, Num Queries, Total Time (%%), Total Time Excl Children (%%), Total Time Excl. Children / Query (%%), Hits Reported");
+
+	uint64 total_ticks = 0;
+	for (EShapeSubType t1 : sAllSubShapeTypes)
+		for (EShapeSubType t2 : sAllSubShapeTypes)
+		{
+			const NarrowPhaseStat &collide_stat = sCollideShape[(int)t1][(int)t2];
+			total_ticks += collide_stat.mTotalTicks - collide_stat.mChildTicks;
+
+			const NarrowPhaseStat &cast_stat = sCastShape[(int)t1][(int)t2];
+			total_ticks += cast_stat.mTotalTicks - cast_stat.mChildTicks;
+		}
+
+	for (EShapeSubType t1 : sAllSubShapeTypes)
+		for (EShapeSubType t2 : sAllSubShapeTypes)
+		{
+			const NarrowPhaseStat &stat = sCollideShape[(int)t1][(int)t2];
+			if (stat.mNumQueries > 0)
+				stat.ReportStats("CollideShape", t1, t2, total_ticks);
+		}
+
+	for (EShapeSubType t1 : sAllSubShapeTypes)
+		for (EShapeSubType t2 : sAllSubShapeTypes)
+		{
+			const NarrowPhaseStat &stat = sCastShape[(int)t1][(int)t2];
+			if (stat.mNumQueries > 0)
+				stat.ReportStats("CastShape", t1, t2, total_ticks);
+		}
+}
+
+JPH_NAMESPACE_END
+
+#endif // JPH_TRACK_NARROWPHASE_STATS
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseStats.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseStats.h
new file mode 100644
index 0000000000..813933bb75
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/NarrowPhaseStats.h
@@ -0,0 +1,110 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/TickCounter.h>
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+
+JPH_SUPPRESS_WARNING_PUSH
+JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat-pedantic")
+
+// Shorthand function to ifdef out code if narrow phase stats tracking is off
+#ifdef JPH_TRACK_NARROWPHASE_STATS
+	#define JPH_IF_TRACK_NARROWPHASE_STATS(...) __VA_ARGS__
+#else
+	#define JPH_IF_TRACK_NARROWPHASE_STATS(...)
+#endif // JPH_TRACK_NARROWPHASE_STATS
+
+JPH_SUPPRESS_WARNING_POP
+
+#ifdef JPH_TRACK_NARROWPHASE_STATS
+
+JPH_NAMESPACE_BEGIN
+
+/// Structure that tracks narrow phase timing information for a particular combination of shapes
+class NarrowPhaseStat
+{
+public:
+	/// Trace an individual stat in CSV form.
+	void					ReportStats(const char *inName, EShapeSubType inType1, EShapeSubType inType2, uint64 inTicks100Pct) const;
+
+	/// Trace the collected broadphase stats in CSV form.
+	/// This report can be used to judge and tweak the efficiency of the broadphase.
+	static void				sReportStats();
+
+	atomic<uint64>			mNumQueries = 0;
+	atomic<uint64>			mHitsReported = 0;
+	atomic<uint64>			mTotalTicks = 0;
+	atomic<uint64>			mChildTicks = 0;
+
+	static NarrowPhaseStat	sCollideShape[NumSubShapeTypes][NumSubShapeTypes];
+	static NarrowPhaseStat	sCastShape[NumSubShapeTypes][NumSubShapeTypes];
+};
+
+/// Object that tracks the start and end of a narrow phase operation
+class TrackNarrowPhaseStat
+{
+public:
+							TrackNarrowPhaseStat(NarrowPhaseStat &inStat) :
+		mStat(inStat),
+		mParent(sRoot),
+		mStart(GetProcessorTickCount())
+	{
+		// Make this the new root of the chain
+		sRoot = this;
+	}
+
+							~TrackNarrowPhaseStat()
+	{
+		uint64 delta_ticks = GetProcessorTickCount() - mStart;
+
+		// Notify parent of time spent in child
+		if (mParent != nullptr)
+			mParent->mStat.mChildTicks += delta_ticks;
+
+		// Increment stats at this level
+		mStat.mNumQueries++;
+		mStat.mTotalTicks += delta_ticks;
+
+		// Restore root pointer
+		JPH_ASSERT(sRoot == this);
+		sRoot = mParent;
+	}
+
+	NarrowPhaseStat &		mStat;
+	TrackNarrowPhaseStat *	mParent;
+	uint64					mStart;
+
+	static thread_local TrackNarrowPhaseStat *sRoot;
+};
+
+/// Object that tracks the start and end of a hit being processed by a collision collector
+class TrackNarrowPhaseCollector
+{
+public:
+							TrackNarrowPhaseCollector() :
+		mStart(GetProcessorTickCount())
+	{
+	}
+
+							~TrackNarrowPhaseCollector()
+	{
+		// Mark time spent in collector as 'child' time for the parent
+		uint64 delta_ticks = GetProcessorTickCount() - mStart;
+		if (TrackNarrowPhaseStat::sRoot != nullptr)
+			TrackNarrowPhaseStat::sRoot->mStat.mChildTicks += delta_ticks;
+
+		// Notify all parents of a hit
+		for (TrackNarrowPhaseStat *track = TrackNarrowPhaseStat::sRoot; track != nullptr; track = track->mParent)
+			track->mStat.mHitsReported++;
+	}
+
+private:
+	uint64					mStart;
+};
+
+JPH_NAMESPACE_END
+
+#endif // JPH_TRACK_NARROWPHASE_STATS
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayer.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayer.h
new file mode 100644
index 0000000000..bfb9e6c9a8
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayer.h
@@ -0,0 +1,111 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/NonCopyable.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Layer that objects can be in, determines which other objects it can collide with
+#ifndef JPH_OBJECT_LAYER_BITS
+	#define JPH_OBJECT_LAYER_BITS 16
+#endif // JPH_OBJECT_LAYER_BITS
+#if JPH_OBJECT_LAYER_BITS == 16
+	using ObjectLayer = uint16;
+#elif JPH_OBJECT_LAYER_BITS == 32
+	using ObjectLayer = uint32;
+#else
+	#error "JPH_OBJECT_LAYER_BITS must be 16 or 32"
+#endif
+
+/// Constant value used to indicate an invalid object layer
+static constexpr ObjectLayer cObjectLayerInvalid = ObjectLayer(~ObjectLayer(0U));
+
+/// Filter class for object layers
+class JPH_EXPORT ObjectLayerFilter : public NonCopyable
+{
+public:
+	/// Destructor
+	virtual					~ObjectLayerFilter() = default;
+
+	/// Function to filter out object layers when doing collision query test (return true to allow testing against objects with this layer)
+	virtual bool			ShouldCollide([[maybe_unused]] ObjectLayer inLayer) const
+	{
+		return true;
+	}
+
+#ifdef JPH_TRACK_BROADPHASE_STATS
+	/// Get a string that describes this filter for stat tracking purposes
+	virtual String			GetDescription() const
+	{
+		return "No Description";
+	}
+#endif // JPH_TRACK_BROADPHASE_STATS
+};
+
+/// Filter class to test if two objects can collide based on their object layer. Used while finding collision pairs.
+class JPH_EXPORT ObjectLayerPairFilter : public NonCopyable
+{
+public:
+	/// Destructor
+	virtual					~ObjectLayerPairFilter() = default;
+
+	/// Returns true if two layers can collide
+	virtual bool			ShouldCollide([[maybe_unused]] ObjectLayer inLayer1, [[maybe_unused]] ObjectLayer inLayer2) const
+	{
+		return true;
+	}
+};
+
+/// Default filter class that uses the pair filter in combination with a specified layer to filter layers
+class JPH_EXPORT DefaultObjectLayerFilter : public ObjectLayerFilter
+{
+public:
+	/// Constructor
+							DefaultObjectLayerFilter(const ObjectLayerPairFilter &inObjectLayerPairFilter, ObjectLayer inLayer) :
+		mObjectLayerPairFilter(inObjectLayerPairFilter),
+		mLayer(inLayer)
+	{
+	}
+
+	/// Copy constructor
+							DefaultObjectLayerFilter(const DefaultObjectLayerFilter &inRHS) :
+		mObjectLayerPairFilter(inRHS.mObjectLayerPairFilter),
+		mLayer(inRHS.mLayer)
+	{
+	}
+
+	// See ObjectLayerFilter::ShouldCollide
+	virtual bool			ShouldCollide(ObjectLayer inLayer) const override
+	{
+		return mObjectLayerPairFilter.ShouldCollide(mLayer, inLayer);
+	}
+
+private:
+	const ObjectLayerPairFilter & mObjectLayerPairFilter;
+	ObjectLayer				mLayer;
+};
+
+/// Allows objects from a specific layer only
+class JPH_EXPORT SpecifiedObjectLayerFilter : public ObjectLayerFilter
+{
+public:
+	/// Constructor
+	explicit				SpecifiedObjectLayerFilter(ObjectLayer inLayer) :
+		mLayer(inLayer)
+	{
+	}
+
+	// See ObjectLayerFilter::ShouldCollide
+	virtual bool			ShouldCollide(ObjectLayer inLayer) const override
+	{
+		return mLayer == inLayer;
+	}
+
+private:
+	ObjectLayer				mLayer;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayerPairFilterMask.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayerPairFilterMask.h
new file mode 100644
index 0000000000..dc3494c2ee
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayerPairFilterMask.h
@@ -0,0 +1,52 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/ObjectLayer.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Filter class to test if two objects can collide based on their object layer. Used while finding collision pairs.
+/// Uses group bits and mask bits. Two layers can collide if Object1.Group & Object2.Mask is non-zero and Object2.Group & Object1.Mask is non-zero.
+/// The behavior is similar to that in e.g. Bullet.
+/// This implementation works together with BroadPhaseLayerInterfaceMask and ObjectVsBroadPhaseLayerFilterMask
+class ObjectLayerPairFilterMask : public ObjectLayerPairFilter
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Number of bits for the group and mask bits
+	static constexpr uint32 cNumBits = JPH_OBJECT_LAYER_BITS / 2;
+	static constexpr uint32	cMask = (1 << cNumBits) - 1;
+
+	/// Construct an ObjectLayer from a group and mask bits
+	static ObjectLayer		sGetObjectLayer(uint32 inGroup, uint32 inMask = cMask)
+	{
+		JPH_ASSERT((inGroup & ~cMask) == 0);
+		JPH_ASSERT((inMask & ~cMask) == 0);
+		return ObjectLayer((inGroup & cMask) | (inMask << cNumBits));
+	}
+
+	/// Get the group bits from an ObjectLayer
+	static inline uint32	sGetGroup(ObjectLayer inObjectLayer)
+	{
+		return uint32(inObjectLayer) & cMask;
+	}
+
+	/// Get the mask bits from an ObjectLayer
+	static inline uint32	sGetMask(ObjectLayer inObjectLayer)
+	{
+		return uint32(inObjectLayer) >> cNumBits;
+	}
+
+	/// Returns true if two layers can collide
+	virtual bool			ShouldCollide(ObjectLayer inObject1, ObjectLayer inObject2) const override
+	{
+		return (sGetGroup(inObject1) & sGetMask(inObject2)) != 0
+			&& (sGetGroup(inObject2) & sGetMask(inObject1)) != 0;
+	}
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayerPairFilterTable.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayerPairFilterTable.h
new file mode 100644
index 0000000000..cb17f3c5da
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ObjectLayerPairFilterTable.h
@@ -0,0 +1,78 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/ObjectLayer.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Filter class to test if two objects can collide based on their object layer. Used while finding collision pairs.
+/// This implementation uses a table to determine if two layers can collide.
+class ObjectLayerPairFilterTable : public ObjectLayerPairFilter
+{
+private:
+	/// Get which bit corresponds to the pair (inLayer1, inLayer2)
+	uint					GetBit(ObjectLayer inLayer1, ObjectLayer inLayer2) const
+	{
+		// We store the lower left half only, so swap the inputs when trying to access the top right half
+		if (inLayer1 > inLayer2)
+			std::swap(inLayer1, inLayer2);
+
+		JPH_ASSERT(inLayer2 < mNumObjectLayers);
+
+		// Calculate at which bit the entry for this pair resides
+		// We use the fact that a row always starts at inLayer2 * (inLayer2 + 1) / 2
+		// (this is the amount of bits needed to store a table of inLayer2 entries)
+		return (inLayer2 * (inLayer2 + 1)) / 2 + inLayer1;
+	}
+
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructs the table with inNumObjectLayers Layers, initially all layer pairs are disabled
+	explicit				ObjectLayerPairFilterTable(uint inNumObjectLayers) :
+		mNumObjectLayers(inNumObjectLayers)
+	{
+		// By default nothing collides
+		// For the first layer we only need to store 1 bit, for the second 2 bits, for the third 3 bits, etc.
+		// We use the formula Sum_i=1^N i = N * (N + 1) / 2 to calculate the size of the table
+		int table_size = (inNumObjectLayers * (inNumObjectLayers + 1) / 2 + 7) / 8;
+		mTable.resize(table_size, 0);
+	}
+
+	/// Get the number of object layers
+	uint					GetNumObjectLayers() const
+	{
+		return mNumObjectLayers;
+	}
+
+	/// Disable collision between two object layers
+	void					DisableCollision(ObjectLayer inLayer1, ObjectLayer inLayer2)
+	{
+		uint bit = GetBit(inLayer1, inLayer2);
+		mTable[bit >> 3] &= (0xff ^ (1 << (bit & 0b111)));
+	}
+
+	/// Enable collision between two object layers
+	void					EnableCollision(ObjectLayer inLayer1, ObjectLayer inLayer2)
+	{
+		uint bit = GetBit(inLayer1, inLayer2);
+		mTable[bit >> 3] |= 1 << (bit & 0b111);
+	}
+
+	/// Returns true if two layers can collide
+	virtual bool			ShouldCollide(ObjectLayer inObject1, ObjectLayer inObject2) const override
+	{
+		// Test if the bit is set for this group pair
+		uint bit = GetBit(inObject1, inObject2);
+		return (mTable[bit >> 3] & (1 << (bit & 0b111))) != 0;
+	}
+
+private:
+	uint					mNumObjectLayers;							///< The number of layers that this table supports
+	Array<uint8>			mTable;										///< The table of bits that indicates which layers collide
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterial.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterial.cpp
new file mode 100644
index 0000000000..17a982e749
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterial.cpp
@@ -0,0 +1,35 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/PhysicsMaterial.h>
+#include <Jolt/Physics/Collision/PhysicsMaterialSimple.h>
+#include <Jolt/Core/StreamUtils.h>
+
+JPH_NAMESPACE_BEGIN
+
+RefConst<PhysicsMaterial> PhysicsMaterial::sDefault;
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PhysicsMaterial)
+{
+	JPH_ADD_BASE_CLASS(PhysicsMaterial, SerializableObject)
+}
+
+void PhysicsMaterial::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(GetRTTI()->GetHash());
+}
+
+void PhysicsMaterial::RestoreBinaryState(StreamIn &inStream)
+{
+	// RTTI hash is read in sRestoreFromBinaryState
+}
+
+PhysicsMaterial::PhysicsMaterialResult PhysicsMaterial::sRestoreFromBinaryState(StreamIn &inStream)
+{
+	return StreamUtils::RestoreObject<PhysicsMaterial>(inStream, &PhysicsMaterial::RestoreBinaryState);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterial.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterial.h
new file mode 100644
index 0000000000..d13c9644b6
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterial.h
@@ -0,0 +1,52 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/Reference.h>
+#include <Jolt/Core/Color.h>
+#include <Jolt/Core/Result.h>
+#include <Jolt/ObjectStream/SerializableObject.h>
+
+JPH_NAMESPACE_BEGIN
+
+class StreamIn;
+class StreamOut;
+
+/// This structure describes the surface of (part of) a shape. You should inherit from it to define additional
+/// information that is interesting for the simulation. The 2 materials involved in a contact could be used
+/// to decide which sound or particle effects to play.
+///
+/// If you inherit from this material, don't forget to create a suitable default material in sDefault
+class JPH_EXPORT PhysicsMaterial : public SerializableObject, public RefTarget<PhysicsMaterial>
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PhysicsMaterial)
+
+public:
+	/// Virtual destructor
+	virtual									~PhysicsMaterial() override = default;
+
+	/// Default material that is used when a shape has no materials defined
+	static RefConst<PhysicsMaterial>		sDefault;
+
+	// Properties
+	virtual const char *					GetDebugName() const			{ return "Unknown"; }
+	virtual Color							GetDebugColor() const			{ return Color::sGrey; }
+
+	/// Saves the contents of the material in binary form to inStream.
+	virtual void							SaveBinaryState(StreamOut &inStream) const;
+
+	using PhysicsMaterialResult = Result<Ref<PhysicsMaterial>>;
+
+	/// Creates a PhysicsMaterial of the correct type and restores its contents from the binary stream inStream.
+	static PhysicsMaterialResult			sRestoreFromBinaryState(StreamIn &inStream);
+
+protected:
+	/// This function should not be called directly, it is used by sRestoreFromBinaryState.
+	virtual void							RestoreBinaryState(StreamIn &inStream);
+};
+
+using PhysicsMaterialList = Array<RefConst<PhysicsMaterial>>;
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterialSimple.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterialSimple.cpp
new file mode 100644
index 0000000000..02a569822c
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterialSimple.cpp
@@ -0,0 +1,38 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/PhysicsMaterialSimple.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PhysicsMaterialSimple)
+{
+	JPH_ADD_BASE_CLASS(PhysicsMaterialSimple, PhysicsMaterial)
+
+	JPH_ADD_ATTRIBUTE(PhysicsMaterialSimple, mDebugName)
+	JPH_ADD_ATTRIBUTE(PhysicsMaterialSimple, mDebugColor)
+}
+
+void PhysicsMaterialSimple::SaveBinaryState(StreamOut &inStream) const
+{
+	PhysicsMaterial::SaveBinaryState(inStream);
+
+	inStream.Write(mDebugName);
+	inStream.Write(mDebugColor);
+}
+
+void PhysicsMaterialSimple::RestoreBinaryState(StreamIn &inStream)
+{
+	PhysicsMaterial::RestoreBinaryState(inStream);
+
+	inStream.Read(mDebugName);
+	inStream.Read(mDebugColor);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterialSimple.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterialSimple.h
new file mode 100644
index 0000000000..63ffff72d6
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/PhysicsMaterialSimple.h
@@ -0,0 +1,37 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/PhysicsMaterial.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Sample implementation of PhysicsMaterial that just holds the needed properties directly
+class JPH_EXPORT PhysicsMaterialSimple : public PhysicsMaterial
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PhysicsMaterialSimple)
+
+public:
+	/// Constructor
+											PhysicsMaterialSimple() = default;
+											PhysicsMaterialSimple(const string_view &inName, ColorArg inColor) : mDebugName(inName), mDebugColor(inColor) { }
+
+	// Properties
+	virtual const char *					GetDebugName() const override		{ return mDebugName.c_str(); }
+	virtual Color							GetDebugColor() const override		{ return mDebugColor; }
+
+	// See: PhysicsMaterial::SaveBinaryState
+	virtual void							SaveBinaryState(StreamOut &inStream) const override;
+
+protected:
+	// See: PhysicsMaterial::RestoreBinaryState
+	virtual void							RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	String									mDebugName;							///< Name of the material, used for debugging purposes
+	Color									mDebugColor = Color::sGrey;			///< Color of the material, used to render the shapes
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/RayCast.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/RayCast.h
new file mode 100644
index 0000000000..c0a7ea66a6
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/RayCast.h
@@ -0,0 +1,87 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/BackFaceMode.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Structure that holds a single ray cast
+template <class Vec, class Mat, class RayCastType>
+struct RayCastT
+{
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructors
+								RayCastT() = default; // Allow raycast to be created uninitialized
+								RayCastT(typename Vec::ArgType inOrigin, Vec3Arg inDirection) : mOrigin(inOrigin), mDirection(inDirection) { }
+								RayCastT(const RayCastT<Vec, Mat, RayCastType> &) = default;
+
+	/// Transform this ray using inTransform
+	RayCastType					Transformed(typename Mat::ArgType inTransform) const
+	{
+		Vec ray_origin = inTransform * mOrigin;
+		Vec3 ray_direction(inTransform * (mOrigin + mDirection) - ray_origin);
+		return { ray_origin, ray_direction };
+	}
+
+	/// Translate ray using inTranslation
+	RayCastType					Translated(typename Vec::ArgType inTranslation) const
+	{
+		return { inTranslation + mOrigin, mDirection };
+	}
+
+	/// Get point with fraction inFraction on ray (0 = start of ray, 1 = end of ray)
+	inline Vec					GetPointOnRay(float inFraction) const
+	{
+		return mOrigin + inFraction * mDirection;
+	}
+
+	Vec							mOrigin;					///< Origin of the ray
+	Vec3						mDirection;					///< Direction and length of the ray (anything beyond this length will not be reported as a hit)
+};
+
+struct RayCast : public RayCastT<Vec3, Mat44, RayCast>
+{
+	using RayCastT<Vec3, Mat44, RayCast>::RayCastT;
+};
+
+struct RRayCast : public RayCastT<RVec3, RMat44, RRayCast>
+{
+	using RayCastT<RVec3, RMat44, RRayCast>::RayCastT;
+
+	/// Convert from RayCast, converts single to double precision
+	explicit					RRayCast(const RayCast &inRay) :
+		RRayCast(RVec3(inRay.mOrigin), inRay.mDirection)
+	{
+	}
+
+	/// Convert to RayCast, which implies casting from double precision to single precision
+	explicit					operator RayCast() const
+	{
+		return RayCast(Vec3(mOrigin), mDirection);
+	}
+};
+
+/// Settings to be passed with a ray cast
+class RayCastSettings
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Set the backfacing mode for all shapes
+	void						SetBackFaceMode(EBackFaceMode inMode) { mBackFaceModeTriangles = mBackFaceModeConvex = inMode; }
+
+	/// How backfacing triangles should be treated (should we report back facing hits for triangle based shapes, e.g. MeshShape/HeightFieldShape?)
+	EBackFaceMode				mBackFaceModeTriangles		= EBackFaceMode::IgnoreBackFaces;
+
+	/// How backfacing convex objects should be treated (should we report back facing hits for convex shapes?)
+	EBackFaceMode				mBackFaceModeConvex			= EBackFaceMode::IgnoreBackFaces;
+
+	/// If convex shapes should be treated as solid. When true, a ray starting inside a convex shape will generate a hit at fraction 0.
+	bool						mTreatConvexAsSolid			= true;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.cpp
new file mode 100644
index 0000000000..dc7ea48285
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.cpp
@@ -0,0 +1,312 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/Shape/GetTrianglesContext.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
+#include <Jolt/Geometry/RayAABox.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(BoxShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(BoxShapeSettings, ConvexShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(BoxShapeSettings, mHalfExtent)
+	JPH_ADD_ATTRIBUTE(BoxShapeSettings, mConvexRadius)
+}
+
+static const Vec3 sUnitBoxTriangles[] = {
+	Vec3(-1, 1, -1),	Vec3(-1, 1, 1),		Vec3(1, 1, 1),
+	Vec3(-1, 1, -1),	Vec3(1, 1, 1),		Vec3(1, 1, -1),
+	Vec3(-1, -1, -1),	Vec3(1, -1, -1),	Vec3(1, -1, 1),
+	Vec3(-1, -1, -1),	Vec3(1, -1, 1),		Vec3(-1, -1, 1),
+	Vec3(-1, 1, -1),	Vec3(-1, -1, -1),	Vec3(-1, -1, 1),
+	Vec3(-1, 1, -1),	Vec3(-1, -1, 1),	Vec3(-1, 1, 1),
+	Vec3(1, 1, 1),		Vec3(1, -1, 1),		Vec3(1, -1, -1),
+	Vec3(1, 1, 1),		Vec3(1, -1, -1),	Vec3(1, 1, -1),
+	Vec3(-1, 1, 1),		Vec3(-1, -1, 1),	Vec3(1, -1, 1),
+	Vec3(-1, 1, 1),		Vec3(1, -1, 1),		Vec3(1, 1, 1),
+	Vec3(-1, 1, -1),	Vec3(1, 1, -1),		Vec3(1, -1, -1),
+	Vec3(-1, 1, -1),	Vec3(1, -1, -1),	Vec3(-1, -1, -1)
+};
+
+ShapeSettings::ShapeResult BoxShapeSettings::Create() const
+{
+	if (mCachedResult.IsEmpty())
+		Ref<Shape> shape = new BoxShape(*this, mCachedResult);
+	return mCachedResult;
+}
+
+BoxShape::BoxShape(const BoxShapeSettings &inSettings, ShapeResult &outResult) :
+	ConvexShape(EShapeSubType::Box, inSettings, outResult),
+	mHalfExtent(inSettings.mHalfExtent),
+	mConvexRadius(inSettings.mConvexRadius)
+{
+	// Check convex radius
+	if (inSettings.mConvexRadius < 0.0f
+		|| inSettings.mHalfExtent.ReduceMin() <= inSettings.mConvexRadius)
+	{
+		outResult.SetError("Invalid convex radius");
+		return;
+	}
+
+	// Result is valid
+	outResult.Set(this);
+}
+
+class BoxShape::Box final : public Support
+{
+public:
+					Box(const AABox &inBox, float inConvexRadius) :
+		mBox(inBox),
+		mConvexRadius(inConvexRadius)
+	{
+		static_assert(sizeof(Box) <= sizeof(SupportBuffer), "Buffer size too small");
+		JPH_ASSERT(IsAligned(this, alignof(Box)));
+	}
+
+	virtual Vec3	GetSupport(Vec3Arg inDirection) const override
+	{
+		return mBox.GetSupport(inDirection);
+	}
+
+	virtual float	GetConvexRadius() const override
+	{
+		return mConvexRadius;
+	}
+
+private:
+	AABox			mBox;
+	float			mConvexRadius;
+};
+
+const ConvexShape::Support *BoxShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const
+{
+	// Scale our half extents
+	Vec3 scaled_half_extent = inScale.Abs() * mHalfExtent;
+
+	switch (inMode)
+	{
+	case ESupportMode::IncludeConvexRadius:
+	case ESupportMode::Default:
+		{
+			// Make box out of our half extents
+			AABox box = AABox(-scaled_half_extent, scaled_half_extent);
+			JPH_ASSERT(box.IsValid());
+			return new (&inBuffer) Box(box, 0.0f);
+		}
+
+	case ESupportMode::ExcludeConvexRadius:
+		{
+			// Reduce the box by our convex radius
+			float convex_radius = ScaleHelpers::ScaleConvexRadius(mConvexRadius, inScale);
+			Vec3 convex_radius3 = Vec3::sReplicate(convex_radius);
+			Vec3 reduced_half_extent = scaled_half_extent - convex_radius3;
+			AABox box = AABox(-reduced_half_extent, reduced_half_extent);
+			JPH_ASSERT(box.IsValid());
+			return new (&inBuffer) Box(box, convex_radius);
+		}
+	}
+
+	JPH_ASSERT(false);
+	return nullptr;
+}
+
+void BoxShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
+
+	Vec3 scaled_half_extent = inScale.Abs() * mHalfExtent;
+	AABox box(-scaled_half_extent, scaled_half_extent);
+	box.GetSupportingFace(inDirection, outVertices);
+
+	// Transform to world space
+	for (Vec3 &v : outVertices)
+		v = inCenterOfMassTransform * v;
+}
+
+MassProperties BoxShape::GetMassProperties() const
+{
+	MassProperties p;
+	p.SetMassAndInertiaOfSolidBox(2.0f * mHalfExtent, GetDensity());
+	return p;
+}
+
+Vec3 BoxShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
+
+	// Get component that is closest to the surface of the box
+	int index = (inLocalSurfacePosition.Abs() - mHalfExtent).Abs().GetLowestComponentIndex();
+
+	// Calculate normal
+	Vec3 normal = Vec3::sZero();
+	normal.SetComponent(index, inLocalSurfacePosition[index] > 0.0f? 1.0f : -1.0f);
+	return normal;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void BoxShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid;
+	inRenderer->DrawBox(inCenterOfMassTransform * Mat44::sScale(inScale.Abs()), GetLocalBounds(), inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor, DebugRenderer::ECastShadow::On, draw_mode);
+}
+#endif // JPH_DEBUG_RENDERER
+
+bool BoxShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	// Test hit against box
+	float fraction = max(RayAABox(inRay.mOrigin, RayInvDirection(inRay.mDirection), -mHalfExtent, mHalfExtent), 0.0f);
+	if (fraction < ioHit.mFraction)
+	{
+		ioHit.mFraction = fraction;
+		ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID();
+		return true;
+	}
+	return false;
+}
+
+void BoxShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	float min_fraction, max_fraction;
+	RayAABox(inRay.mOrigin, RayInvDirection(inRay.mDirection), -mHalfExtent, mHalfExtent, min_fraction, max_fraction);
+	if (min_fraction <= max_fraction // Ray should intersect
+		&& max_fraction >= 0.0f // End of ray should be inside box
+		&& min_fraction < ioCollector.GetEarlyOutFraction()) // Start of ray should be before early out fraction
+	{
+		// Better hit than the current hit
+		RayCastResult hit;
+		hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext());
+		hit.mSubShapeID2 = inSubShapeIDCreator.GetID();
+
+		// Check front side
+		if (inRayCastSettings.mTreatConvexAsSolid || min_fraction > 0.0f)
+		{
+			hit.mFraction = max(0.0f, min_fraction);
+			ioCollector.AddHit(hit);
+		}
+
+		// Check back side hit
+		if (inRayCastSettings.mBackFaceModeConvex == EBackFaceMode::CollideWithBackFaces
+			&& max_fraction < ioCollector.GetEarlyOutFraction())
+		{
+			hit.mFraction = max_fraction;
+			ioCollector.AddHit(hit);
+		}
+	}
+}
+
+void BoxShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	if (Vec3::sLessOrEqual(inPoint.Abs(), mHalfExtent).TestAllXYZTrue())
+		ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
+}
+
+void BoxShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
+{
+	Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation();
+	Vec3 half_extent = inScale.Abs() * mHalfExtent;
+
+	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
+		if (v.GetInvMass() > 0.0f)
+		{
+			// Convert to local space
+			Vec3 local_pos = inverse_transform * v.GetPosition();
+
+			// Clamp point to inside box
+			Vec3 clamped_point = Vec3::sMax(Vec3::sMin(local_pos, half_extent), -half_extent);
+
+			// Test if point was inside
+			if (clamped_point == local_pos)
+			{
+				// Calculate closest distance to surface
+				Vec3 delta = half_extent - local_pos.Abs();
+				int index = delta.GetLowestComponentIndex();
+				float penetration = delta[index];
+				if (v.UpdatePenetration(penetration))
+				{
+					// Calculate contact point and normal
+					Vec3 possible_normals[] = { Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ() };
+					Vec3 normal = local_pos.GetSign() * possible_normals[index];
+					Vec3 point = normal * half_extent;
+
+					// Store collision
+					v.SetCollision(Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex);
+				}
+			}
+			else
+			{
+				// Calculate normal
+				Vec3 normal = local_pos - clamped_point;
+				float normal_length = normal.Length();
+
+				// Penetration will be negative since we're not penetrating
+				float penetration = -normal_length;
+				if (v.UpdatePenetration(penetration))
+				{
+					normal /= normal_length;
+
+					// Store collision
+					v.SetCollision(Plane::sFromPointAndNormal(clamped_point, normal).GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex);
+				}
+			}
+		}
+}
+
+void BoxShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const
+{
+	new (&ioContext) GetTrianglesContextVertexList(inPositionCOM, inRotation, inScale, Mat44::sScale(mHalfExtent), sUnitBoxTriangles, sizeof(sUnitBoxTriangles) / sizeof(Vec3), GetMaterial());
+}
+
+int BoxShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const
+{
+	return ((GetTrianglesContextVertexList &)ioContext).GetTrianglesNext(inMaxTrianglesRequested, outTriangleVertices, outMaterials);
+}
+
+void BoxShape::SaveBinaryState(StreamOut &inStream) const
+{
+	ConvexShape::SaveBinaryState(inStream);
+
+	inStream.Write(mHalfExtent);
+	inStream.Write(mConvexRadius);
+}
+
+void BoxShape::RestoreBinaryState(StreamIn &inStream)
+{
+	ConvexShape::RestoreBinaryState(inStream);
+
+	inStream.Read(mHalfExtent);
+	inStream.Read(mConvexRadius);
+}
+
+void BoxShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Box);
+	f.mConstruct = []() -> Shape * { return new BoxShape; };
+	f.mColor = Color::sGreen;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.h
new file mode 100644
index 0000000000..030dd2db80
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/BoxShape.h
@@ -0,0 +1,115 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/ConvexShape.h>
+#include <Jolt/Physics/PhysicsSettings.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that constructs a BoxShape
+class JPH_EXPORT BoxShapeSettings final : public ConvexShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, BoxShapeSettings)
+
+public:
+	/// Default constructor for deserialization
+							BoxShapeSettings() = default;
+
+	/// Create a box with half edge length inHalfExtent and convex radius inConvexRadius.
+	/// (internally the convex radius will be subtracted from the half extent so the total box will not grow with the convex radius).
+							BoxShapeSettings(Vec3Arg inHalfExtent, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mHalfExtent(inHalfExtent), mConvexRadius(inConvexRadius) { }
+
+	// See: ShapeSettings
+	virtual ShapeResult		Create() const override;
+
+	Vec3					mHalfExtent = Vec3::sZero();								///< Half the size of the box (including convex radius)
+	float					mConvexRadius = 0.0f;
+};
+
+/// A box, centered around the origin
+class JPH_EXPORT BoxShape final : public ConvexShape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+							BoxShape() : ConvexShape(EShapeSubType::Box) { }
+							BoxShape(const BoxShapeSettings &inSettings, ShapeResult &outResult);
+
+	/// Create a box with half edge length inHalfExtent and convex radius inConvexRadius.
+	/// (internally the convex radius will be subtracted from the half extent so the total box will not grow with the convex radius).
+							BoxShape(Vec3Arg inHalfExtent, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShape(EShapeSubType::Box, inMaterial), mHalfExtent(inHalfExtent), mConvexRadius(inConvexRadius) { JPH_ASSERT(inConvexRadius >= 0.0f); JPH_ASSERT(inHalfExtent.ReduceMin() >= inConvexRadius); }
+
+	/// Get half extent of box
+	Vec3					GetHalfExtent() const										{ return mHalfExtent; }
+
+	// See Shape::GetLocalBounds
+	virtual AABox			GetLocalBounds() const override								{ return AABox(-mHalfExtent, mHalfExtent); }
+
+	// See Shape::GetInnerRadius
+	virtual float			GetInnerRadius() const override								{ return mHalfExtent.ReduceMin(); }
+
+	// See Shape::GetMassProperties
+	virtual MassProperties	GetMassProperties() const override;
+
+	// See Shape::GetSurfaceNormal
+	virtual Vec3			GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override;
+
+	// See Shape::GetSupportingFace
+	virtual void			GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
+
+	// See ConvexShape::GetSupportFunction
+	virtual const Support *	GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override;
+
+#ifdef JPH_DEBUG_RENDERER
+	// See Shape::Draw
+	virtual void			Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// See Shape::CastRay
+	virtual bool			CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+	virtual void			CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollidePoint
+	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollideSoftBodyVertices
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
+
+	// See Shape::GetTrianglesStart
+	virtual void			GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
+
+	// See Shape::GetTrianglesNext
+	virtual int				GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override;
+
+	// See Shape
+	virtual void			SaveBinaryState(StreamOut &inStream) const override;
+
+	// See Shape::GetStats
+	virtual Stats			GetStats() const override									{ return Stats(sizeof(*this), 12); }
+
+	// See Shape::GetVolume
+	virtual float			GetVolume() const override									{ return GetLocalBounds().GetVolume(); }
+
+	/// Get the convex radius of this box
+	float					GetConvexRadius() const										{ return mConvexRadius; }
+
+	// Register shape functions with the registry
+	static void				sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void			RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	// Class for GetSupportFunction
+	class					Box;
+
+	Vec3					mHalfExtent = Vec3::sZero();								///< Half the size of the box (including convex radius)
+	float					mConvexRadius = 0.0f;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CapsuleShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CapsuleShape.cpp
new file mode 100644
index 0000000000..48a6cba3e6
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CapsuleShape.cpp
@@ -0,0 +1,438 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/Shape/GetTrianglesContext.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
+#include <Jolt/Geometry/RayCapsule.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(CapsuleShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(CapsuleShapeSettings, ConvexShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(CapsuleShapeSettings, mRadius)
+	JPH_ADD_ATTRIBUTE(CapsuleShapeSettings, mHalfHeightOfCylinder)
+}
+
+static const int cCapsuleDetailLevel = 2;
+
+static const StaticArray<Vec3, 192> sCapsuleTopTriangles = []() {
+	StaticArray<Vec3, 192> verts;
+	GetTrianglesContextVertexList::sCreateHalfUnitSphereTop(verts, cCapsuleDetailLevel);
+	return verts;
+}();
+
+static const StaticArray<Vec3, 96> sCapsuleMiddleTriangles = []() {
+	StaticArray<Vec3, 96> verts;
+	GetTrianglesContextVertexList::sCreateUnitOpenCylinder(verts, cCapsuleDetailLevel);
+	return verts;
+}();
+
+static const StaticArray<Vec3, 192> sCapsuleBottomTriangles = []() {
+	StaticArray<Vec3, 192> verts;
+	GetTrianglesContextVertexList::sCreateHalfUnitSphereBottom(verts, cCapsuleDetailLevel);
+	return verts;
+}();
+
+ShapeSettings::ShapeResult CapsuleShapeSettings::Create() const
+{
+	if (mCachedResult.IsEmpty())
+	{
+		Ref<Shape> shape;
+		if (IsValid() && IsSphere())
+		{
+			// If the capsule has no height, use a sphere instead
+			shape = new SphereShape(mRadius, mMaterial);
+			mCachedResult.Set(shape);
+		}
+		else
+			shape = new CapsuleShape(*this, mCachedResult);
+	}
+	return mCachedResult;
+}
+
+CapsuleShape::CapsuleShape(const CapsuleShapeSettings &inSettings, ShapeResult &outResult) :
+	ConvexShape(EShapeSubType::Capsule, inSettings, outResult),
+	mRadius(inSettings.mRadius),
+	mHalfHeightOfCylinder(inSettings.mHalfHeightOfCylinder)
+{
+	if (inSettings.mHalfHeightOfCylinder <= 0.0f)
+	{
+		outResult.SetError("Invalid height");
+		return;
+	}
+
+	if (inSettings.mRadius <= 0.0f)
+	{
+		outResult.SetError("Invalid radius");
+		return;
+	}
+
+	outResult.Set(this);
+}
+
+class CapsuleShape::CapsuleNoConvex final : public Support
+{
+public:
+					CapsuleNoConvex(Vec3Arg inHalfHeightOfCylinder, float inConvexRadius) :
+		mHalfHeightOfCylinder(inHalfHeightOfCylinder),
+		mConvexRadius(inConvexRadius)
+	{
+		static_assert(sizeof(CapsuleNoConvex) <= sizeof(SupportBuffer), "Buffer size too small");
+		JPH_ASSERT(IsAligned(this, alignof(CapsuleNoConvex)));
+	}
+
+	virtual Vec3	GetSupport(Vec3Arg inDirection) const override
+	{
+		if (inDirection.GetY() > 0)
+			return mHalfHeightOfCylinder;
+		else
+			return -mHalfHeightOfCylinder;
+	}
+
+	virtual float	GetConvexRadius() const override
+	{
+		return mConvexRadius;
+	}
+
+private:
+	Vec3			mHalfHeightOfCylinder;
+	float			mConvexRadius;
+};
+
+class CapsuleShape::CapsuleWithConvex final : public Support
+{
+public:
+					CapsuleWithConvex(Vec3Arg inHalfHeightOfCylinder, float inRadius) :
+		mHalfHeightOfCylinder(inHalfHeightOfCylinder),
+		mRadius(inRadius)
+	{
+		static_assert(sizeof(CapsuleWithConvex) <= sizeof(SupportBuffer), "Buffer size too small");
+		JPH_ASSERT(IsAligned(this, alignof(CapsuleWithConvex)));
+	}
+
+	virtual Vec3	GetSupport(Vec3Arg inDirection) const override
+	{
+		float len = inDirection.Length();
+		Vec3 radius = len > 0.0f? inDirection * (mRadius / len) : Vec3::sZero();
+
+		if (inDirection.GetY() > 0)
+			return radius + mHalfHeightOfCylinder;
+		else
+			return radius - mHalfHeightOfCylinder;
+	}
+
+	virtual float	GetConvexRadius() const override
+	{
+		return 0.0f;
+	}
+
+private:
+	Vec3			mHalfHeightOfCylinder;
+	float			mRadius;
+};
+
+const ConvexShape::Support *CapsuleShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const
+{
+	JPH_ASSERT(IsValidScale(inScale));
+
+	// Get scaled capsule
+	Vec3 abs_scale = inScale.Abs();
+	float scale = abs_scale.GetX();
+	Vec3 scaled_half_height_of_cylinder = Vec3(0, scale * mHalfHeightOfCylinder, 0);
+	float scaled_radius = scale * mRadius;
+
+	switch (inMode)
+	{
+	case ESupportMode::IncludeConvexRadius:
+		return new (&inBuffer) CapsuleWithConvex(scaled_half_height_of_cylinder, scaled_radius);
+
+	case ESupportMode::ExcludeConvexRadius:
+	case ESupportMode::Default:
+		return new (&inBuffer) CapsuleNoConvex(scaled_half_height_of_cylinder, scaled_radius);
+	}
+
+	JPH_ASSERT(false);
+	return nullptr;
+}
+
+void CapsuleShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
+	JPH_ASSERT(IsValidScale(inScale));
+
+	// Get direction in horizontal plane
+	Vec3 direction = inDirection;
+	direction.SetComponent(1, 0.0f);
+
+	// Check zero vector, in this case we're hitting from top/bottom so there's no supporting face
+	float len = direction.Length();
+	if (len == 0.0f)
+		return;
+
+	// Get scaled capsule
+	Vec3 abs_scale = inScale.Abs();
+	float scale = abs_scale.GetX();
+	Vec3 scaled_half_height_of_cylinder = Vec3(0, scale * mHalfHeightOfCylinder, 0);
+	float scaled_radius = scale * mRadius;
+
+	// Get support point for top and bottom sphere in the opposite of 'direction' (including convex radius)
+	Vec3 support = (scaled_radius / len) * direction;
+	Vec3 support_top = scaled_half_height_of_cylinder - support;
+	Vec3 support_bottom = -scaled_half_height_of_cylinder - support;
+
+	// Get projection on inDirection
+	// Note that inDirection is not normalized, so we need to divide by inDirection.Length() to get the actual projection
+	// We've multiplied both sides of the if below with inDirection.Length()
+	float proj_top = support_top.Dot(inDirection);
+	float proj_bottom = support_bottom.Dot(inDirection);
+
+	// If projection is roughly equal then return line, otherwise we return nothing as there's only 1 point
+	if (abs(proj_top - proj_bottom) < cCapsuleProjectionSlop * inDirection.Length())
+	{
+		outVertices.push_back(inCenterOfMassTransform * support_top);
+		outVertices.push_back(inCenterOfMassTransform * support_bottom);
+	}
+}
+
+MassProperties CapsuleShape::GetMassProperties() const
+{
+	MassProperties p;
+
+	float density = GetDensity();
+
+	// Calculate inertia and mass according to:
+	// https://www.gamedev.net/resources/_/technical/math-and-physics/capsule-inertia-tensor-r3856
+	// Note that there is an error in eq 14, H^2/2 should be H^2/4 in Ixx and Izz, eq 12 does contain the correct value
+	float radius_sq = Square(mRadius);
+	float height = 2.0f * mHalfHeightOfCylinder;
+	float cylinder_mass = JPH_PI * height * radius_sq * density;
+	float hemisphere_mass = (2.0f * JPH_PI / 3.0f) * radius_sq * mRadius * density;
+
+	// From cylinder
+	float height_sq = Square(height);
+	float inertia_y = radius_sq * cylinder_mass * 0.5f;
+	float inertia_xz = inertia_y * 0.5f + cylinder_mass * height_sq / 12.0f;
+
+	// From hemispheres
+	float temp = hemisphere_mass * 4.0f * radius_sq / 5.0f;
+	inertia_y += temp;
+	inertia_xz += temp + hemisphere_mass * (0.5f * height_sq + (3.0f / 4.0f) * height * mRadius);
+
+	// Mass is cylinder + hemispheres
+	p.mMass = cylinder_mass + hemisphere_mass * 2.0f;
+
+	// Set inertia
+	p.mInertia = Mat44::sScale(Vec3(inertia_xz, inertia_y, inertia_xz));
+
+	return p;
+}
+
+Vec3 CapsuleShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
+
+	if (inLocalSurfacePosition.GetY() > mHalfHeightOfCylinder)
+		return (inLocalSurfacePosition - Vec3(0, mHalfHeightOfCylinder, 0)).Normalized();
+	else if (inLocalSurfacePosition.GetY() < -mHalfHeightOfCylinder)
+		return (inLocalSurfacePosition - Vec3(0, -mHalfHeightOfCylinder, 0)).Normalized();
+	else
+		return Vec3(inLocalSurfacePosition.GetX(), 0, inLocalSurfacePosition.GetZ()).NormalizedOr(Vec3::sAxisX());
+}
+
+AABox CapsuleShape::GetLocalBounds() const
+{
+	Vec3 extent = Vec3::sReplicate(mRadius) + Vec3(0, mHalfHeightOfCylinder, 0);
+	return AABox(-extent, extent);
+}
+
+AABox CapsuleShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
+	JPH_ASSERT(IsValidScale(inScale));
+
+	Vec3 abs_scale = inScale.Abs();
+	float scale = abs_scale.GetX();
+	Vec3 extent = Vec3::sReplicate(scale * mRadius);
+	Vec3 height = Vec3(0, scale * mHalfHeightOfCylinder, 0);
+	Vec3 p1 = inCenterOfMassTransform * -height;
+	Vec3 p2 = inCenterOfMassTransform * height;
+	return AABox(Vec3::sMin(p1, p2) - extent, Vec3::sMax(p1, p2) + extent);
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void CapsuleShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid;
+	inRenderer->DrawCapsule(inCenterOfMassTransform * Mat44::sScale(inScale.Abs().GetX()), mHalfHeightOfCylinder, mRadius, inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor, DebugRenderer::ECastShadow::On, draw_mode);
+}
+#endif // JPH_DEBUG_RENDERER
+
+bool CapsuleShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	// Test ray against capsule
+	float fraction = RayCapsule(inRay.mOrigin, inRay.mDirection, mHalfHeightOfCylinder, mRadius);
+	if (fraction < ioHit.mFraction)
+	{
+		ioHit.mFraction = fraction;
+		ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID();
+		return true;
+	}
+	return false;
+}
+
+void CapsuleShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	float radius_sq = Square(mRadius);
+
+	// Get vertical distance to the top/bottom sphere centers
+	float delta_y = abs(inPoint.GetY()) - mHalfHeightOfCylinder;
+
+	// Get distance in horizontal plane
+	float xz_sq = Square(inPoint.GetX()) + Square(inPoint.GetZ());
+
+	// Check if the point is in one of the two spheres
+	bool in_sphere = xz_sq + Square(delta_y) <= radius_sq;
+
+	// Check if the point is in the cylinder in the middle
+	bool in_cylinder = delta_y <= 0.0f && xz_sq <= radius_sq;
+
+	if (in_sphere || in_cylinder)
+		ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
+}
+
+void CapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
+{
+	JPH_ASSERT(IsValidScale(inScale));
+
+	Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation();
+
+	// Get scaled capsule
+	float scale = abs(inScale.GetX());
+	float half_height_of_cylinder = scale * mHalfHeightOfCylinder;
+	float radius = scale * mRadius;
+
+	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
+		if (v.GetInvMass() > 0.0f)
+		{
+			// Calculate penetration
+			Vec3 local_pos = inverse_transform * v.GetPosition();
+			if (abs(local_pos.GetY()) <= half_height_of_cylinder)
+			{
+				// Near cylinder
+				Vec3 normal = local_pos;
+				normal.SetY(0.0f);
+				float normal_length = normal.Length();
+				float penetration = radius - normal_length;
+				if (v.UpdatePenetration(penetration))
+				{
+					// Calculate contact point and normal
+					normal = normal_length > 0.0f? normal / normal_length : Vec3::sAxisX();
+					Vec3 point = radius * normal;
+
+					// Store collision
+					v.SetCollision(Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex);
+				}
+			}
+			else
+			{
+				// Near cap
+				Vec3 center = Vec3(0, Sign(local_pos.GetY()) * half_height_of_cylinder, 0);
+				Vec3 delta = local_pos - center;
+				float distance = delta.Length();
+				float penetration = radius - distance;
+				if (v.UpdatePenetration(penetration))
+				{
+					// Calculate contact point and normal
+					Vec3 normal = delta / distance;
+					Vec3 point = center + radius * normal;
+
+					// Store collision
+					v.SetCollision(Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex);
+				}
+			}
+		}
+}
+
+void CapsuleShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const
+{
+	JPH_ASSERT(IsValidScale(inScale));
+
+	Vec3 abs_scale = inScale.Abs();
+	float scale = abs_scale.GetX();
+
+	GetTrianglesContextMultiVertexList *context = new (&ioContext) GetTrianglesContextMultiVertexList(false, GetMaterial());
+
+	Mat44 world_matrix = Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(scale);
+
+	Mat44 top_matrix = world_matrix * Mat44(Vec4(mRadius, 0, 0, 0), Vec4(0, mRadius, 0, 0), Vec4(0, 0, mRadius, 0), Vec4(0, mHalfHeightOfCylinder, 0, 1));
+	context->AddPart(top_matrix, sCapsuleTopTriangles.data(), sCapsuleTopTriangles.size());
+
+	Mat44 middle_matrix = world_matrix * Mat44::sScale(Vec3(mRadius, mHalfHeightOfCylinder, mRadius));
+	context->AddPart(middle_matrix, sCapsuleMiddleTriangles.data(), sCapsuleMiddleTriangles.size());
+
+	Mat44 bottom_matrix = world_matrix * Mat44(Vec4(mRadius, 0, 0, 0), Vec4(0, mRadius, 0, 0), Vec4(0, 0, mRadius, 0), Vec4(0, -mHalfHeightOfCylinder, 0, 1));
+	context->AddPart(bottom_matrix, sCapsuleBottomTriangles.data(), sCapsuleBottomTriangles.size());
+}
+
+int CapsuleShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const
+{
+	return ((GetTrianglesContextMultiVertexList &)ioContext).GetTrianglesNext(inMaxTrianglesRequested, outTriangleVertices, outMaterials);
+}
+
+void CapsuleShape::SaveBinaryState(StreamOut &inStream) const
+{
+	ConvexShape::SaveBinaryState(inStream);
+
+	inStream.Write(mRadius);
+	inStream.Write(mHalfHeightOfCylinder);
+}
+
+void CapsuleShape::RestoreBinaryState(StreamIn &inStream)
+{
+	ConvexShape::RestoreBinaryState(inStream);
+
+	inStream.Read(mRadius);
+	inStream.Read(mHalfHeightOfCylinder);
+}
+
+bool CapsuleShape::IsValidScale(Vec3Arg inScale) const
+{
+	return ConvexShape::IsValidScale(inScale) && ScaleHelpers::IsUniformScale(inScale.Abs());
+}
+
+Vec3 CapsuleShape::MakeScaleValid(Vec3Arg inScale) const
+{
+	Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale);
+
+	return scale.GetSign() * ScaleHelpers::MakeUniformScale(scale.Abs());
+}
+
+void CapsuleShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Capsule);
+	f.mConstruct = []() -> Shape * { return new CapsuleShape; };
+	f.mColor = Color::sGreen;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CapsuleShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CapsuleShape.h
new file mode 100644
index 0000000000..77af7eceb6
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CapsuleShape.h
@@ -0,0 +1,129 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/ConvexShape.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that constructs a CapsuleShape
+class JPH_EXPORT CapsuleShapeSettings final : public ConvexShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, CapsuleShapeSettings)
+
+public:
+	/// Default constructor for deserialization
+							CapsuleShapeSettings() = default;
+
+	/// Create a capsule centered around the origin with one sphere cap at (0, -inHalfHeightOfCylinder, 0) and the other at (0, inHalfHeightOfCylinder, 0)
+							CapsuleShapeSettings(float inHalfHeightOfCylinder, float inRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mRadius(inRadius), mHalfHeightOfCylinder(inHalfHeightOfCylinder) { }
+
+	/// Check if this is a valid capsule shape
+	bool					IsValid() const															{ return mRadius > 0.0f && mHalfHeightOfCylinder >= 0.0f; }
+
+	/// Checks if the settings of this capsule make this shape a sphere
+	bool					IsSphere() const														{ return mHalfHeightOfCylinder == 0.0f; }
+
+	// See: ShapeSettings
+	virtual ShapeResult		Create() const override;
+
+	float					mRadius = 0.0f;
+	float					mHalfHeightOfCylinder = 0.0f;
+};
+
+/// A capsule, implemented as a line segment with convex radius
+class JPH_EXPORT CapsuleShape final : public ConvexShape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+							CapsuleShape() : ConvexShape(EShapeSubType::Capsule) { }
+							CapsuleShape(const CapsuleShapeSettings &inSettings, ShapeResult &outResult);
+
+	/// Create a capsule centered around the origin with one sphere cap at (0, -inHalfHeightOfCylinder, 0) and the other at (0, inHalfHeightOfCylinder, 0)
+							CapsuleShape(float inHalfHeightOfCylinder, float inRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShape(EShapeSubType::Capsule, inMaterial), mRadius(inRadius), mHalfHeightOfCylinder(inHalfHeightOfCylinder) { JPH_ASSERT(inHalfHeightOfCylinder > 0.0f); JPH_ASSERT(inRadius > 0.0f); }
+
+	/// Radius of the cylinder
+	float					GetRadius() const														{ return mRadius; }
+
+	/// Get half of the height of the cylinder
+	float					GetHalfHeightOfCylinder() const											{ return mHalfHeightOfCylinder; }
+
+	// See Shape::GetLocalBounds
+	virtual AABox			GetLocalBounds() const override;
+
+	// See Shape::GetWorldSpaceBounds
+	virtual AABox			GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+	using Shape::GetWorldSpaceBounds;
+
+	// See Shape::GetInnerRadius
+	virtual float			GetInnerRadius() const override											{ return mRadius; }
+
+	// See Shape::GetMassProperties
+	virtual MassProperties	GetMassProperties() const override;
+
+	// See Shape::GetSurfaceNormal
+	virtual Vec3			GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override;
+
+	// See Shape::GetSupportingFace
+	virtual void			GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
+
+	// See ConvexShape::GetSupportFunction
+	virtual const Support *	GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override;
+
+#ifdef JPH_DEBUG_RENDERER
+	// See Shape::Draw
+	virtual void			Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// See Shape::CastRay
+	using ConvexShape::CastRay;
+	virtual bool			CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+
+	// See: Shape::CollidePoint
+	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollideSoftBodyVertices
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
+
+	// See Shape::GetTrianglesStart
+	virtual void			GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
+
+	// See Shape::GetTrianglesNext
+	virtual int				GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override;
+
+	// See Shape
+	virtual void			SaveBinaryState(StreamOut &inStream) const override;
+
+	// See Shape::GetStats
+	virtual Stats			GetStats() const override												{ return Stats(sizeof(*this), 0); }
+
+	// See Shape::GetVolume
+	virtual float			GetVolume() const override												{ return 4.0f / 3.0f * JPH_PI * Cubed(mRadius) + 2.0f * JPH_PI * mHalfHeightOfCylinder * Square(mRadius); }
+
+	// See Shape::IsValidScale
+	virtual bool			IsValidScale(Vec3Arg inScale) const override;
+
+	// See Shape::MakeScaleValid
+	virtual Vec3			MakeScaleValid(Vec3Arg inScale) const override;
+
+	// Register shape functions with the registry
+	static void				sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void			RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	// Classes for GetSupportFunction
+	class					CapsuleNoConvex;
+	class					CapsuleWithConvex;
+
+	float					mRadius = 0.0f;
+	float					mHalfHeightOfCylinder = 0.0f;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShape.cpp
new file mode 100644
index 0000000000..f5fe04ecf2
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShape.cpp
@@ -0,0 +1,428 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/Shape/CompoundShape.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Core/Profiler.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(CompoundShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(CompoundShapeSettings, ShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(CompoundShapeSettings, mSubShapes)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(CompoundShapeSettings::SubShapeSettings)
+{
+	JPH_ADD_ATTRIBUTE(CompoundShapeSettings::SubShapeSettings, mShape)
+	JPH_ADD_ATTRIBUTE(CompoundShapeSettings::SubShapeSettings, mPosition)
+	JPH_ADD_ATTRIBUTE(CompoundShapeSettings::SubShapeSettings, mRotation)
+	JPH_ADD_ATTRIBUTE(CompoundShapeSettings::SubShapeSettings, mUserData)
+}
+
+void CompoundShapeSettings::AddShape(Vec3Arg inPosition, QuatArg inRotation, const ShapeSettings *inShape, uint32 inUserData)
+{
+	// Add shape
+	SubShapeSettings shape;
+	shape.mPosition = inPosition;
+	shape.mRotation = inRotation;
+	shape.mShape = inShape;
+	shape.mUserData = inUserData;
+	mSubShapes.push_back(shape);
+}
+
+void CompoundShapeSettings::AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData)
+{
+	// Add shape
+	SubShapeSettings shape;
+	shape.mPosition = inPosition;
+	shape.mRotation = inRotation;
+	shape.mShapePtr = inShape;
+	shape.mUserData = inUserData;
+	mSubShapes.push_back(shape);
+}
+
+bool CompoundShape::MustBeStatic() const
+{
+	for (const SubShape &shape : mSubShapes)
+		if (shape.mShape->MustBeStatic())
+			return true;
+
+	return false;
+}
+
+MassProperties CompoundShape::GetMassProperties() const
+{
+	MassProperties p;
+
+	// Calculate mass and inertia
+	p.mMass = 0.0f;
+	p.mInertia = Mat44::sZero();
+	for (const SubShape &shape : mSubShapes)
+	{
+		// Rotate and translate inertia of child into place
+		MassProperties child = shape.mShape->GetMassProperties();
+		child.Rotate(Mat44::sRotation(shape.GetRotation()));
+		child.Translate(shape.GetPositionCOM());
+
+		// Accumulate mass and inertia
+		p.mMass += child.mMass;
+		p.mInertia += child.mInertia;
+	}
+
+	// Ensure that inertia is a 3x3 matrix, adding inertias causes the bottom right element to change
+	p.mInertia.SetColumn4(3, Vec4(0, 0, 0, 1));
+
+	return p;
+}
+
+AABox CompoundShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
+	if (mSubShapes.size() <= 10)
+	{
+		AABox bounds;
+		for (const SubShape &shape : mSubShapes)
+		{
+			Mat44 transform = inCenterOfMassTransform * shape.GetLocalTransformNoScale(inScale);
+			bounds.Encapsulate(shape.mShape->GetWorldSpaceBounds(transform, shape.TransformScale(inScale)));
+		}
+		return bounds;
+	}
+	else
+	{
+		// If there are too many shapes, use the base class function (this will result in a slightly wider bounding box)
+		return Shape::GetWorldSpaceBounds(inCenterOfMassTransform, inScale);
+	}
+}
+
+uint CompoundShape::GetSubShapeIDBitsRecursive() const
+{
+	// Add max of child bits to our bits
+	uint child_bits = 0;
+	for (const SubShape &shape : mSubShapes)
+		child_bits = max(child_bits, shape.mShape->GetSubShapeIDBitsRecursive());
+	return child_bits + GetSubShapeIDBits();
+}
+
+const PhysicsMaterial *CompoundShape::GetMaterial(const SubShapeID &inSubShapeID) const
+{
+	// Decode sub shape index
+	SubShapeID remainder;
+	uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder);
+
+	// Pass call on
+	return mSubShapes[index].mShape->GetMaterial(remainder);
+}
+
+const Shape *CompoundShape::GetLeafShape(const SubShapeID &inSubShapeID, SubShapeID &outRemainder) const
+{
+	// Decode sub shape index
+	SubShapeID remainder;
+	uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder);
+	if (index >= mSubShapes.size())
+	{
+		// No longer valid index
+		outRemainder = SubShapeID();
+		return nullptr;
+	}
+
+	// Pass call on
+	return mSubShapes[index].mShape->GetLeafShape(remainder, outRemainder);
+}
+
+uint64 CompoundShape::GetSubShapeUserData(const SubShapeID &inSubShapeID) const
+{
+	// Decode sub shape index
+	SubShapeID remainder;
+	uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder);
+	if (index >= mSubShapes.size())
+		return 0; // No longer valid index
+
+	// Pass call on
+	return mSubShapes[index].mShape->GetSubShapeUserData(remainder);
+}
+
+TransformedShape CompoundShape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const
+{
+	// Get the sub shape
+	const SubShape &sub_shape = mSubShapes[GetSubShapeIndexFromID(inSubShapeID, outRemainder)];
+
+	// Calculate transform for sub shape
+	Vec3 position = inPositionCOM + inRotation * (inScale * sub_shape.GetPositionCOM());
+	Quat rotation = inRotation * sub_shape.GetRotation();
+	Vec3 scale = sub_shape.TransformScale(inScale);
+
+	// Return transformed shape
+	TransformedShape ts(RVec3(position), rotation, sub_shape.mShape, BodyID());
+	ts.SetShapeScale(scale);
+	return ts;
+}
+
+Vec3 CompoundShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	// Decode sub shape index
+	SubShapeID remainder;
+	uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder);
+
+	// Transform surface position to local space and pass call on
+	const SubShape &shape = mSubShapes[index];
+	Mat44 transform = Mat44::sInverseRotationTranslation(shape.GetRotation(), shape.GetPositionCOM());
+	Vec3 normal = shape.mShape->GetSurfaceNormal(remainder, transform * inLocalSurfacePosition);
+
+	// Transform normal to this shape's space
+	return transform.Multiply3x3Transposed(normal);
+}
+
+void CompoundShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
+	// Decode sub shape index
+	SubShapeID remainder;
+	uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder);
+
+	// Apply transform and pass on to sub shape
+	const SubShape &shape = mSubShapes[index];
+	Mat44 transform = shape.GetLocalTransformNoScale(inScale);
+	shape.mShape->GetSupportingFace(remainder, transform.Multiply3x3Transposed(inDirection), shape.TransformScale(inScale), inCenterOfMassTransform * transform, outVertices);
+}
+
+void CompoundShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const
+{
+	outTotalVolume = 0.0f;
+	outSubmergedVolume = 0.0f;
+	outCenterOfBuoyancy = Vec3::sZero();
+
+	for (const SubShape &shape : mSubShapes)
+	{
+		// Get center of mass transform of child
+		Mat44 transform = inCenterOfMassTransform * shape.GetLocalTransformNoScale(inScale);
+
+		// Recurse to child
+		float total_volume, submerged_volume;
+		Vec3 center_of_buoyancy;
+		shape.mShape->GetSubmergedVolume(transform, shape.TransformScale(inScale), inSurface, total_volume, submerged_volume, center_of_buoyancy JPH_IF_DEBUG_RENDERER(, inBaseOffset));
+
+		// Accumulate volumes
+		outTotalVolume += total_volume;
+		outSubmergedVolume += submerged_volume;
+
+		// The center of buoyancy is the weighted average of the center of buoyancy of our child shapes
+		outCenterOfBuoyancy += submerged_volume * center_of_buoyancy;
+	}
+
+	if (outSubmergedVolume > 0.0f)
+		outCenterOfBuoyancy /= outSubmergedVolume;
+
+#ifdef JPH_DEBUG_RENDERER
+	// Draw center of buoyancy
+	if (sDrawSubmergedVolumes)
+		DebugRenderer::sInstance->DrawWireSphere(inBaseOffset + outCenterOfBuoyancy, 0.05f, Color::sRed, 1);
+#endif // JPH_DEBUG_RENDERER
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void CompoundShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	for (const SubShape &shape : mSubShapes)
+	{
+		Mat44 transform = shape.GetLocalTransformNoScale(inScale);
+		shape.mShape->Draw(inRenderer, inCenterOfMassTransform * transform, shape.TransformScale(inScale), inColor, inUseMaterialColors, inDrawWireframe);
+	}
+}
+
+void CompoundShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const
+{
+	for (const SubShape &shape : mSubShapes)
+	{
+		Mat44 transform = shape.GetLocalTransformNoScale(inScale);
+		shape.mShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform * transform, shape.TransformScale(inScale), inColor, inDrawSupportDirection);
+	}
+}
+
+void CompoundShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
+	for (const SubShape &shape : mSubShapes)
+	{
+		Mat44 transform = shape.GetLocalTransformNoScale(inScale);
+		shape.mShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform * transform, shape.TransformScale(inScale));
+	}
+}
+#endif // JPH_DEBUG_RENDERER
+
+void CompoundShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
+{
+	for (const SubShape &shape : mSubShapes)
+	{
+		Mat44 transform = shape.GetLocalTransformNoScale(inScale);
+		shape.mShape->CollideSoftBodyVertices(inCenterOfMassTransform * transform, shape.TransformScale(inScale), inVertices, inNumVertices, inCollidingShapeIndex);
+	}
+}
+
+void CompoundShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const
+{
+	for (const SubShape &shape : mSubShapes)
+		shape.mShape->TransformShape(inCenterOfMassTransform * Mat44::sRotationTranslation(shape.GetRotation(), shape.GetPositionCOM()), ioCollector);
+}
+
+void CompoundShape::sCastCompoundVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Fetch compound shape from cast shape
+	JPH_ASSERT(inShapeCast.mShape->GetType() == EShapeType::Compound);
+	const CompoundShape *compound = static_cast<const CompoundShape *>(inShapeCast.mShape);
+
+	// Number of sub shapes
+	int n = (int)compound->mSubShapes.size();
+
+	// Determine amount of bits for sub shape
+	uint sub_shape_bits = compound->GetSubShapeIDBits();
+
+	// Recurse to sub shapes
+	for (int i = 0; i < n; ++i)
+	{
+		const SubShape &shape = compound->mSubShapes[i];
+
+		// Create ID for sub shape
+		SubShapeIDCreator shape1_sub_shape_id = inSubShapeIDCreator1.PushID(i, sub_shape_bits);
+
+		// Transform the shape cast and update the shape
+		Mat44 transform = inShapeCast.mCenterOfMassStart * shape.GetLocalTransformNoScale(inShapeCast.mScale);
+		Vec3 scale = shape.TransformScale(inShapeCast.mScale);
+		ShapeCast shape_cast(shape.mShape, scale, transform, inShapeCast.mDirection);
+
+		CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, shape1_sub_shape_id, inSubShapeIDCreator2, ioCollector);
+
+		if (ioCollector.ShouldEarlyOut())
+			break;
+	}
+}
+
+void CompoundShape::SaveBinaryState(StreamOut &inStream) const
+{
+	Shape::SaveBinaryState(inStream);
+
+	inStream.Write(mCenterOfMass);
+	inStream.Write(mLocalBounds.mMin);
+	inStream.Write(mLocalBounds.mMax);
+	inStream.Write(mInnerRadius);
+
+	// Write sub shapes
+	inStream.Write(mSubShapes, [](const SubShape &inElement, StreamOut &inS) {
+		inS.Write(inElement.mUserData);
+		inS.Write(inElement.mPositionCOM);
+		inS.Write(inElement.mRotation);
+	});
+}
+
+void CompoundShape::RestoreBinaryState(StreamIn &inStream)
+{
+	Shape::RestoreBinaryState(inStream);
+
+	inStream.Read(mCenterOfMass);
+	inStream.Read(mLocalBounds.mMin);
+	inStream.Read(mLocalBounds.mMax);
+	inStream.Read(mInnerRadius);
+
+	// Read sub shapes
+	inStream.Read(mSubShapes, [](StreamIn &inS, SubShape &outElement) {
+		inS.Read(outElement.mUserData);
+		inS.Read(outElement.mPositionCOM);
+		inS.Read(outElement.mRotation);
+		outElement.mIsRotationIdentity = outElement.mRotation == Float3(0, 0, 0);
+	});
+}
+
+void CompoundShape::SaveSubShapeState(ShapeList &outSubShapes) const
+{
+	outSubShapes.clear();
+	outSubShapes.reserve(mSubShapes.size());
+	for (const SubShape &shape : mSubShapes)
+		outSubShapes.push_back(shape.mShape);
+}
+
+void CompoundShape::RestoreSubShapeState(const ShapeRefC *inSubShapes, uint inNumShapes)
+{
+	JPH_ASSERT(mSubShapes.size() == inNumShapes);
+	for (uint i = 0; i < inNumShapes; ++i)
+		mSubShapes[i].mShape = inSubShapes[i];
+}
+
+Shape::Stats CompoundShape::GetStatsRecursive(VisitedShapes &ioVisitedShapes) const
+{
+	// Get own stats
+	Stats stats = Shape::GetStatsRecursive(ioVisitedShapes);
+
+	// Add child stats
+	for (const SubShape &shape : mSubShapes)
+	{
+		Stats child_stats = shape.mShape->GetStatsRecursive(ioVisitedShapes);
+		stats.mSizeBytes += child_stats.mSizeBytes;
+		stats.mNumTriangles += child_stats.mNumTriangles;
+	}
+
+	return stats;
+}
+
+float CompoundShape::GetVolume() const
+{
+	float volume = 0.0f;
+	for (const SubShape &shape : mSubShapes)
+		volume += shape.mShape->GetVolume();
+	return volume;
+}
+
+bool CompoundShape::IsValidScale(Vec3Arg inScale) const
+{
+	if (!Shape::IsValidScale(inScale))
+		return false;
+
+	for (const SubShape &shape : mSubShapes)
+	{
+		// Test if the scale is non-uniform and the shape is rotated
+		if (!shape.IsValidScale(inScale))
+			return false;
+
+		// Test the child shape
+		if (!shape.mShape->IsValidScale(shape.TransformScale(inScale)))
+			return false;
+	}
+
+	return true;
+}
+
+Vec3 CompoundShape::MakeScaleValid(Vec3Arg inScale) const
+{
+	Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale);
+	if (CompoundShape::IsValidScale(scale))
+		return scale;
+
+	Vec3 abs_uniform_scale = ScaleHelpers::MakeUniformScale(scale.Abs());
+	Vec3 uniform_scale = scale.GetSign() * abs_uniform_scale;
+	if (CompoundShape::IsValidScale(uniform_scale))
+		return uniform_scale;
+
+	return Sign(scale.GetX()) * abs_uniform_scale;
+}
+
+void CompoundShape::sRegister()
+{
+	for (EShapeSubType s1 : sCompoundSubShapeTypes)
+		for (EShapeSubType s2 : sAllSubShapeTypes)
+			CollisionDispatch::sRegisterCastShape(s1, s2, sCastCompoundVsShape);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShape.h
new file mode 100644
index 0000000000..0bbd7c4d7a
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShape.h
@@ -0,0 +1,350 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+
+JPH_NAMESPACE_BEGIN
+
+class CollideShapeSettings;
+class OrientedBox;
+
+/// Base class settings to construct a compound shape
+class JPH_EXPORT CompoundShapeSettings : public ShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, CompoundShapeSettings)
+
+public:
+	/// Constructor. Use AddShape to add the parts.
+									CompoundShapeSettings() = default;
+
+	/// Add a shape to the compound.
+	void							AddShape(Vec3Arg inPosition, QuatArg inRotation, const ShapeSettings *inShape, uint32 inUserData = 0);
+
+	/// Add a shape to the compound. Variant that uses a concrete shape, which means this object cannot be serialized.
+	void							AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData = 0);
+
+	struct SubShapeSettings
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SubShapeSettings)
+
+		RefConst<ShapeSettings>		mShape;													///< Sub shape (either this or mShapePtr needs to be filled up)
+		RefConst<Shape>				mShapePtr;												///< Sub shape (either this or mShape needs to be filled up)
+		Vec3						mPosition;												///< Position of the sub shape
+		Quat						mRotation;												///< Rotation of the sub shape
+		uint32						mUserData = 0;											///< User data value (can be used by the application for any purpose)
+	};
+
+	using SubShapes = Array<SubShapeSettings>;
+
+	SubShapes						mSubShapes;
+};
+
+/// Base class for a compound shape
+class JPH_EXPORT CompoundShape : public Shape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+	explicit						CompoundShape(EShapeSubType inSubType) : Shape(EShapeType::Compound, inSubType) { }
+									CompoundShape(EShapeSubType inSubType, const ShapeSettings &inSettings, ShapeResult &outResult) : Shape(EShapeType::Compound, inSubType, inSettings, outResult) { }
+
+	// See Shape::GetCenterOfMass
+	virtual Vec3					GetCenterOfMass() const override						{ return mCenterOfMass; }
+
+	// See Shape::MustBeStatic
+	virtual bool					MustBeStatic() const override;
+
+	// See Shape::GetLocalBounds
+	virtual AABox					GetLocalBounds() const override							{ return mLocalBounds; }
+
+	// See Shape::GetSubShapeIDBitsRecursive
+	virtual uint					GetSubShapeIDBitsRecursive() const override;
+
+	// See Shape::GetWorldSpaceBounds
+	virtual AABox					GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+	using Shape::GetWorldSpaceBounds;
+
+	// See Shape::GetInnerRadius
+	virtual float					GetInnerRadius() const override							{ return mInnerRadius; }
+
+	// See Shape::GetMassProperties
+	virtual MassProperties			GetMassProperties() const override;
+
+	// See Shape::GetMaterial
+	virtual const PhysicsMaterial *	GetMaterial(const SubShapeID &inSubShapeID) const override;
+
+	// See Shape::GetLeafShape
+	virtual const Shape *			GetLeafShape(const SubShapeID &inSubShapeID, SubShapeID &outRemainder) const override;
+
+	// See Shape::GetSubShapeUserData
+	virtual uint64					GetSubShapeUserData(const SubShapeID &inSubShapeID) const override;
+
+	// See Shape::GetSubShapeTransformedShape
+	virtual TransformedShape		GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const override;
+
+	// See Shape::GetSurfaceNormal
+	virtual Vec3					GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override;
+
+	// See Shape::GetSupportingFace
+	virtual void					GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
+
+	// See Shape::GetSubmergedVolume
+	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override;
+
+#ifdef JPH_DEBUG_RENDERER
+	// See Shape::Draw
+	virtual void					Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+
+	// See Shape::DrawGetSupportFunction
+	virtual void					DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override;
+
+	// See Shape::DrawGetSupportingFace
+	virtual void					DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// See: Shape::CollideSoftBodyVertices
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
+
+	// See Shape::TransformShape
+	virtual void					TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override;
+
+	// See Shape::GetTrianglesStart
+	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); }
+
+	// See Shape::GetTrianglesNext
+	virtual int						GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); return 0; }
+
+	/// Get which sub shape's bounding boxes overlap with an axis aligned box
+	/// @param inBox The axis aligned box to test against (relative to the center of mass of this shape)
+	/// @param outSubShapeIndices Buffer where to place the indices of the sub shapes that intersect
+	/// @param inMaxSubShapeIndices How many indices will fit in the buffer (normally you'd provide a buffer of GetNumSubShapes() indices)
+	/// @return How many indices were placed in outSubShapeIndices
+	virtual int						GetIntersectingSubShapes(const AABox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const = 0;
+
+	/// Get which sub shape's bounding boxes overlap with an axis aligned box
+	/// @param inBox The axis aligned box to test against (relative to the center of mass of this shape)
+	/// @param outSubShapeIndices Buffer where to place the indices of the sub shapes that intersect
+	/// @param inMaxSubShapeIndices How many indices will fit in the buffer (normally you'd provide a buffer of GetNumSubShapes() indices)
+	/// @return How many indices were placed in outSubShapeIndices
+	virtual int						GetIntersectingSubShapes(const OrientedBox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const = 0;
+
+	struct SubShape
+	{
+		/// Initialize sub shape from sub shape settings
+		/// @param inSettings Settings object
+		/// @param outResult Result object, only used in case of error
+		/// @return True on success, false on failure
+		bool						FromSettings(const CompoundShapeSettings::SubShapeSettings &inSettings, ShapeResult &outResult)
+		{
+			if (inSettings.mShapePtr != nullptr)
+			{
+				// Use provided shape
+				mShape = inSettings.mShapePtr;
+			}
+			else
+			{
+				// Create child shape
+				ShapeResult child_result = inSettings.mShape->Create();
+				if (!child_result.IsValid())
+				{
+					outResult = child_result;
+					return false;
+				}
+				mShape = child_result.Get();
+			}
+
+			// Copy user data
+			mUserData = inSettings.mUserData;
+
+			SetTransform(inSettings.mPosition, inSettings.mRotation, Vec3::sZero() /* Center of mass not yet calculated */);
+			return true;
+		}
+
+		/// Update the transform of this sub shape
+		/// @param inPosition New position
+		/// @param inRotation New orientation
+		/// @param inCenterOfMass The center of mass of the compound shape
+		JPH_INLINE void				SetTransform(Vec3Arg inPosition, QuatArg inRotation, Vec3Arg inCenterOfMass)
+		{
+			SetPositionCOM(inPosition - inCenterOfMass + inRotation * mShape->GetCenterOfMass());
+
+			mIsRotationIdentity = inRotation.IsClose(Quat::sIdentity()) || inRotation.IsClose(-Quat::sIdentity());
+			SetRotation(mIsRotationIdentity? Quat::sIdentity() : inRotation);
+		}
+
+		/// Get the local transform for this shape given the scale of the child shape
+		/// The total transform of the child shape will be GetLocalTransformNoScale(inScale) * Mat44::sScaling(TransformScale(inScale))
+		/// @param inScale The scale of the child shape (in local space of this shape)
+		JPH_INLINE Mat44			GetLocalTransformNoScale(Vec3Arg inScale) const
+		{
+			JPH_ASSERT(IsValidScale(inScale));
+			return Mat44::sRotationTranslation(GetRotation(), inScale * GetPositionCOM());
+		}
+
+		/// Test if inScale is valid for this sub shape
+		inline bool					IsValidScale(Vec3Arg inScale) const
+		{
+			// We can always handle uniform scale or identity rotations
+			if (mIsRotationIdentity || ScaleHelpers::IsUniformScale(inScale))
+				return true;
+
+			return ScaleHelpers::CanScaleBeRotated(GetRotation(), inScale);
+		}
+
+		/// Transform the scale to the local space of the child shape
+		inline Vec3					TransformScale(Vec3Arg inScale) const
+		{
+			// We don't need to transform uniform scale or if the rotation is identity
+			if (mIsRotationIdentity || ScaleHelpers::IsUniformScale(inScale))
+				return inScale;
+
+			return ScaleHelpers::RotateScale(GetRotation(), inScale);
+		}
+
+		/// Compress the center of mass position
+		JPH_INLINE void				SetPositionCOM(Vec3Arg inPositionCOM)
+		{
+			inPositionCOM.StoreFloat3(&mPositionCOM);
+		}
+
+		/// Uncompress the center of mass position
+		JPH_INLINE Vec3				GetPositionCOM() const
+		{
+			return Vec3::sLoadFloat3Unsafe(mPositionCOM);
+		}
+
+		/// Compress the rotation
+		JPH_INLINE void				SetRotation(QuatArg inRotation)
+		{
+			inRotation.StoreFloat3(&mRotation);
+		}
+
+		/// Uncompress the rotation
+		JPH_INLINE Quat				GetRotation() const
+		{
+			return mIsRotationIdentity? Quat::sIdentity() : Quat::sLoadFloat3Unsafe(mRotation);
+		}
+
+		RefConst<Shape>				mShape;
+		Float3						mPositionCOM;											///< Note: Position of center of mass of sub shape!
+		Float3						mRotation;												///< Note: X, Y, Z of rotation quaternion - note we read 4 bytes beyond this so make sure there's something there
+		uint32						mUserData;												///< User data value (put here because it falls in padding bytes)
+		bool						mIsRotationIdentity;									///< If mRotation is close to identity (put here because it falls in padding bytes)
+		// 3 padding bytes left
+	};
+
+	static_assert(sizeof(SubShape) == (JPH_CPU_ADDRESS_BITS == 64? 40 : 36), "Compiler added unexpected padding");
+
+	using SubShapes = Array<SubShape>;
+
+	/// Access to the sub shapes of this compound
+	const SubShapes &				GetSubShapes() const									{ return mSubShapes; }
+
+	/// Get the total number of sub shapes
+	uint							GetNumSubShapes() const									{ return uint(mSubShapes.size()); }
+
+	/// Access to a particular sub shape
+	const SubShape &				GetSubShape(uint inIdx) const							{ return mSubShapes[inIdx]; }
+
+	/// Get the user data associated with a shape in this compound
+	uint32							GetCompoundUserData(uint inIdx) const					{ return mSubShapes[inIdx].mUserData; }
+
+	/// Set the user data associated with a shape in this compound
+	void							SetCompoundUserData(uint inIdx, uint32 inUserData)		{ mSubShapes[inIdx].mUserData = inUserData; }
+
+	/// Check if a sub shape ID is still valid for this shape
+	/// @param inSubShapeID Sub shape id that indicates the leaf shape relative to this shape
+	/// @return True if the ID is valid, false if not
+	inline bool						IsSubShapeIDValid(SubShapeID inSubShapeID) const
+	{
+		SubShapeID remainder;
+		return inSubShapeID.PopID(GetSubShapeIDBits(), remainder) < mSubShapes.size();
+	}
+
+	/// Convert SubShapeID to sub shape index
+	/// @param inSubShapeID Sub shape id that indicates the leaf shape relative to this shape
+	/// @param outRemainder This is the sub shape ID for the sub shape of the compound after popping off the index
+	/// @return The index of the sub shape of this compound
+	inline uint32					GetSubShapeIndexFromID(SubShapeID inSubShapeID, SubShapeID &outRemainder) const
+	{
+		uint32 idx = inSubShapeID.PopID(GetSubShapeIDBits(), outRemainder);
+		JPH_ASSERT(idx < mSubShapes.size(), "Invalid SubShapeID");
+		return idx;
+	}
+
+	/// @brief Convert a sub shape index to a sub shape ID
+	/// @param inIdx Index of the sub shape of this compound
+	/// @param inParentSubShapeID Parent SubShapeID (describing the path to the compound shape)
+	/// @return A sub shape ID creator that contains the full path to the sub shape with index inIdx
+	inline SubShapeIDCreator		GetSubShapeIDFromIndex(int inIdx, const SubShapeIDCreator &inParentSubShapeID) const
+	{
+		return inParentSubShapeID.PushID(inIdx, GetSubShapeIDBits());
+	}
+
+	// See Shape
+	virtual void					SaveBinaryState(StreamOut &inStream) const override;
+	virtual void					SaveSubShapeState(ShapeList &outSubShapes) const override;
+	virtual void					RestoreSubShapeState(const ShapeRefC *inSubShapes, uint inNumShapes) override;
+
+	// See Shape::GetStatsRecursive
+	virtual Stats					GetStatsRecursive(VisitedShapes &ioVisitedShapes) const override;
+
+	// See Shape::GetVolume
+	virtual float					GetVolume() const override;
+
+	// See Shape::IsValidScale
+	virtual bool					IsValidScale(Vec3Arg inScale) const override;
+
+	// See Shape::MakeScaleValid
+	virtual Vec3					MakeScaleValid(Vec3Arg inScale) const override;
+
+	// Register shape functions with the registry
+	static void						sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void					RestoreBinaryState(StreamIn &inStream) override;
+
+	// Visitors for collision detection
+	struct CastRayVisitor;
+	struct CastRayVisitorCollector;
+	struct CollidePointVisitor;
+	struct CastShapeVisitor;
+	struct CollectTransformedShapesVisitor;
+	struct CollideCompoundVsShapeVisitor;
+	struct CollideShapeVsCompoundVisitor;
+	template <class BoxType> struct GetIntersectingSubShapesVisitor;
+
+	/// Determine amount of bits needed to encode sub shape id
+	inline uint						GetSubShapeIDBits() const
+	{
+		// Ensure we have enough bits to encode our shape [0, n - 1]
+		uint32 n = uint32(mSubShapes.size()) - 1;
+		return 32 - CountLeadingZeros(n);
+	}
+
+	/// Determine the inner radius of this shape
+	inline void						CalculateInnerRadius()
+	{
+		mInnerRadius = FLT_MAX;
+		for (const SubShape &s : mSubShapes)
+			mInnerRadius = min(mInnerRadius, s.mShape->GetInnerRadius());
+	}
+
+	Vec3							mCenterOfMass { Vec3::sZero() };						///< Center of mass of the compound
+	AABox							mLocalBounds;
+	SubShapes						mSubShapes;
+	float							mInnerRadius = FLT_MAX;									///< Smallest radius of GetInnerRadius() of child shapes
+
+private:
+	// Helper functions called by CollisionDispatch
+	static void						sCastCompoundVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShapeVisitors.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShapeVisitors.h
new file mode 100644
index 0000000000..1b1e3867b0
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CompoundShapeVisitors.h
@@ -0,0 +1,460 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/CompoundShape.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Geometry/RayAABox.h>
+#include <Jolt/Geometry/AABox4.h>
+#include <Jolt/Geometry/OrientedBox.h>
+
+JPH_NAMESPACE_BEGIN
+
+struct CompoundShape::CastRayVisitor
+{
+	JPH_INLINE			CastRayVisitor(const RayCast &inRay, const CompoundShape *inShape, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) :
+		mRay(inRay),
+		mHit(ioHit),
+		mSubShapeIDCreator(inSubShapeIDCreator),
+		mSubShapeBits(inShape->GetSubShapeIDBits())
+	{
+		// Determine ray properties of cast
+		mInvDirection.Set(inRay.mDirection);
+	}
+
+	/// Returns true when collision detection should abort because it's not possible to find a better hit
+	JPH_INLINE bool		ShouldAbort() const
+	{
+		return mHit.mFraction <= 0.0f;
+	}
+
+	/// Test ray against 4 bounding boxes and returns the distance where the ray enters the bounding box
+	JPH_INLINE Vec4		TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+	{
+		return RayAABox4(mRay.mOrigin, mInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+	}
+
+	/// Test the ray against a single subshape
+	JPH_INLINE void		VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex)
+	{
+		// Create ID for sub shape
+		SubShapeIDCreator shape2_sub_shape_id = mSubShapeIDCreator.PushID(inSubShapeIndex, mSubShapeBits);
+
+		// Transform the ray
+		Mat44 transform = Mat44::sInverseRotationTranslation(inSubShape.GetRotation(), inSubShape.GetPositionCOM());
+		RayCast ray = mRay.Transformed(transform);
+		if (inSubShape.mShape->CastRay(ray, shape2_sub_shape_id, mHit))
+			mReturnValue = true;
+	}
+
+	RayInvDirection		mInvDirection;
+	const RayCast &		mRay;
+	RayCastResult &		mHit;
+	SubShapeIDCreator	mSubShapeIDCreator;
+	uint				mSubShapeBits;
+	bool				mReturnValue = false;
+};
+
+struct CompoundShape::CastRayVisitorCollector
+{
+	JPH_INLINE			CastRayVisitorCollector(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const CompoundShape *inShape, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) :
+		mRay(inRay),
+		mCollector(ioCollector),
+		mSubShapeIDCreator(inSubShapeIDCreator),
+		mSubShapeBits(inShape->GetSubShapeIDBits()),
+		mRayCastSettings(inRayCastSettings),
+		mShapeFilter(inShapeFilter)
+	{
+		// Determine ray properties of cast
+		mInvDirection.Set(inRay.mDirection);
+	}
+
+	/// Returns true when collision detection should abort because it's not possible to find a better hit
+	JPH_INLINE bool		ShouldAbort() const
+	{
+		return mCollector.ShouldEarlyOut();
+	}
+
+	/// Test ray against 4 bounding boxes and returns the distance where the ray enters the bounding box
+	JPH_INLINE Vec4		TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+	{
+		return RayAABox4(mRay.mOrigin, mInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+	}
+
+	/// Test the ray against a single subshape
+	JPH_INLINE void		VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex)
+	{
+		// Create ID for sub shape
+		SubShapeIDCreator shape2_sub_shape_id = mSubShapeIDCreator.PushID(inSubShapeIndex, mSubShapeBits);
+
+		// Transform the ray
+		Mat44 transform = Mat44::sInverseRotationTranslation(inSubShape.GetRotation(), inSubShape.GetPositionCOM());
+		RayCast ray = mRay.Transformed(transform);
+		inSubShape.mShape->CastRay(ray, mRayCastSettings, shape2_sub_shape_id, mCollector, mShapeFilter);
+	}
+
+	RayInvDirection		mInvDirection;
+	const RayCast &		mRay;
+	CastRayCollector &	mCollector;
+	SubShapeIDCreator	mSubShapeIDCreator;
+	uint				mSubShapeBits;
+	RayCastSettings		mRayCastSettings;
+	const ShapeFilter &	mShapeFilter;
+};
+
+struct CompoundShape::CollidePointVisitor
+{
+	JPH_INLINE			CollidePointVisitor(Vec3Arg inPoint, const CompoundShape *inShape, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) :
+		mPoint(inPoint),
+		mSubShapeIDCreator(inSubShapeIDCreator),
+		mCollector(ioCollector),
+		mSubShapeBits(inShape->GetSubShapeIDBits()),
+		mShapeFilter(inShapeFilter)
+	{
+	}
+
+	/// Returns true when collision detection should abort because it's not possible to find a better hit
+	JPH_INLINE bool		ShouldAbort() const
+	{
+		return mCollector.ShouldEarlyOut();
+	}
+
+	/// Test if point overlaps with 4 boxes, returns true for the ones that do
+	JPH_INLINE UVec4	TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+	{
+		return AABox4VsPoint(mPoint, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+	}
+
+	/// Test the point against a single subshape
+	JPH_INLINE void		VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex)
+	{
+		// Create ID for sub shape
+		SubShapeIDCreator shape2_sub_shape_id = mSubShapeIDCreator.PushID(inSubShapeIndex, mSubShapeBits);
+
+		// Transform the point
+		Mat44 transform = Mat44::sInverseRotationTranslation(inSubShape.GetRotation(), inSubShape.GetPositionCOM());
+		inSubShape.mShape->CollidePoint(transform * mPoint, shape2_sub_shape_id, mCollector, mShapeFilter);
+	}
+
+	Vec3						mPoint;
+	SubShapeIDCreator			mSubShapeIDCreator;
+	CollidePointCollector &		mCollector;
+	uint						mSubShapeBits;
+	const ShapeFilter &			mShapeFilter;
+};
+
+struct CompoundShape::CastShapeVisitor
+{
+	JPH_INLINE			CastShapeVisitor(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const CompoundShape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) :
+		mBoxCenter(inShapeCast.mShapeWorldBounds.GetCenter()),
+		mBoxExtent(inShapeCast.mShapeWorldBounds.GetExtent()),
+		mScale(inScale),
+		mShapeCast(inShapeCast),
+		mShapeCastSettings(inShapeCastSettings),
+		mShapeFilter(inShapeFilter),
+		mCollector(ioCollector),
+		mCenterOfMassTransform2(inCenterOfMassTransform2),
+		mSubShapeIDCreator1(inSubShapeIDCreator1),
+		mSubShapeIDCreator2(inSubShapeIDCreator2),
+		mSubShapeBits(inShape->GetSubShapeIDBits())
+	{
+		// Determine ray properties of cast
+		mInvDirection.Set(inShapeCast.mDirection);
+	}
+
+	/// Returns true when collision detection should abort because it's not possible to find a better hit
+	JPH_INLINE bool		ShouldAbort() const
+	{
+		return mCollector.ShouldEarlyOut();
+	}
+
+	/// Tests the shape cast against 4 bounding boxes, returns the distance along the shape cast where the shape first enters the bounding box
+	JPH_INLINE Vec4		TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+	{
+		// Scale the bounding boxes
+		Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
+		AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+		// Enlarge them by the casted shape's box extents
+		AABox4EnlargeWithExtent(mBoxExtent, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+		// Test ray against the bounding boxes
+		return RayAABox4(mBoxCenter, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+	}
+
+	/// Test the cast shape against a single subshape
+	JPH_INLINE void		VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex)
+	{
+		JPH_ASSERT(inSubShape.IsValidScale(mScale));
+
+		// Create ID for sub shape
+		SubShapeIDCreator shape2_sub_shape_id = mSubShapeIDCreator2.PushID(inSubShapeIndex, mSubShapeBits);
+
+		// Calculate the local transform for this sub shape
+		Mat44 local_transform = Mat44::sRotationTranslation(inSubShape.GetRotation(), mScale * inSubShape.GetPositionCOM());
+
+		// Transform the center of mass of 2
+		Mat44 center_of_mass_transform2 = mCenterOfMassTransform2 * local_transform;
+
+		// Transform the shape cast
+		ShapeCast shape_cast = mShapeCast.PostTransformed(local_transform.InversedRotationTranslation());
+
+		CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, mShapeCastSettings, inSubShape.mShape, inSubShape.TransformScale(mScale), mShapeFilter, center_of_mass_transform2, mSubShapeIDCreator1, shape2_sub_shape_id, mCollector);
+	}
+
+	RayInvDirection				mInvDirection;
+	Vec3						mBoxCenter;
+	Vec3						mBoxExtent;
+	Vec3						mScale;
+	const ShapeCast &			mShapeCast;
+	const ShapeCastSettings &	mShapeCastSettings;
+	const ShapeFilter &			mShapeFilter;
+	CastShapeCollector &		mCollector;
+	Mat44						mCenterOfMassTransform2;
+	SubShapeIDCreator			mSubShapeIDCreator1;
+	SubShapeIDCreator			mSubShapeIDCreator2;
+	uint						mSubShapeBits;
+};
+
+struct CompoundShape::CollectTransformedShapesVisitor
+{
+	JPH_INLINE			CollectTransformedShapesVisitor(const AABox &inBox, const CompoundShape *inShape, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) :
+		mBox(inBox),
+		mLocalBox(Mat44::sInverseRotationTranslation(inRotation, inPositionCOM), inBox),
+		mPositionCOM(inPositionCOM),
+		mRotation(inRotation),
+		mScale(inScale),
+		mSubShapeIDCreator(inSubShapeIDCreator),
+		mCollector(ioCollector),
+		mSubShapeBits(inShape->GetSubShapeIDBits()),
+		mShapeFilter(inShapeFilter)
+	{
+	}
+
+	/// Returns true when collision detection should abort because it's not possible to find a better hit
+	JPH_INLINE bool		ShouldAbort() const
+	{
+		return mCollector.ShouldEarlyOut();
+	}
+
+	/// Tests 4 bounding boxes against the query box, returns true for the ones that collide
+	JPH_INLINE UVec4	TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+	{
+		// Scale the bounding boxes of this node
+		Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
+		AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+		// Test which nodes collide
+		return AABox4VsBox(mLocalBox, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+	}
+
+	/// Collect the transformed sub shapes for a single subshape
+	JPH_INLINE void		VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex)
+	{
+		JPH_ASSERT(inSubShape.IsValidScale(mScale));
+
+		// Create ID for sub shape
+		SubShapeIDCreator sub_shape_id = mSubShapeIDCreator.PushID(inSubShapeIndex, mSubShapeBits);
+
+		// Calculate world transform for sub shape
+		Vec3 position = mPositionCOM + mRotation * (mScale * inSubShape.GetPositionCOM());
+		Quat rotation = mRotation * inSubShape.GetRotation();
+
+		// Recurse to sub shape
+		inSubShape.mShape->CollectTransformedShapes(mBox, position, rotation, inSubShape.TransformScale(mScale), sub_shape_id, mCollector, mShapeFilter);
+	}
+
+	AABox							mBox;
+	OrientedBox						mLocalBox;
+	Vec3							mPositionCOM;
+	Quat							mRotation;
+	Vec3							mScale;
+	SubShapeIDCreator				mSubShapeIDCreator;
+	TransformedShapeCollector &		mCollector;
+	uint							mSubShapeBits;
+	const ShapeFilter &				mShapeFilter;
+};
+
+struct CompoundShape::CollideCompoundVsShapeVisitor
+{
+	JPH_INLINE			CollideCompoundVsShapeVisitor(const CompoundShape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) :
+		mCollideShapeSettings(inCollideShapeSettings),
+		mCollector(ioCollector),
+		mShape2(inShape2),
+		mScale1(inScale1),
+		mScale2(inScale2),
+		mTransform1(inCenterOfMassTransform1),
+		mTransform2(inCenterOfMassTransform2),
+		mSubShapeIDCreator1(inSubShapeIDCreator1),
+		mSubShapeIDCreator2(inSubShapeIDCreator2),
+		mSubShapeBits(inShape1->GetSubShapeIDBits()),
+		mShapeFilter(inShapeFilter)
+	{
+		// Get transform from shape 2 to shape 1
+		Mat44 transform2_to_1 = inCenterOfMassTransform1.InversedRotationTranslation() * inCenterOfMassTransform2;
+
+		// Convert bounding box of 2 into space of 1
+		mBoundsOf2InSpaceOf1 = inShape2->GetLocalBounds().Scaled(inScale2).Transformed(transform2_to_1);
+	}
+
+	/// Returns true when collision detection should abort because it's not possible to find a better hit
+	JPH_INLINE bool		ShouldAbort() const
+	{
+		return mCollector.ShouldEarlyOut();
+	}
+
+	/// Tests the bounds of shape 2 vs 4 bounding boxes, returns true for the ones that intersect
+	JPH_INLINE UVec4	TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+	{
+		// Scale the bounding boxes
+		Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
+		AABox4Scale(mScale1, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+		// Test which boxes collide
+		return AABox4VsBox(mBoundsOf2InSpaceOf1, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+	}
+
+	/// Test the shape against a single subshape
+	JPH_INLINE void		VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex)
+	{
+		// Get world transform of 1
+		Mat44 transform1 = mTransform1 * inSubShape.GetLocalTransformNoScale(mScale1);
+
+		// Create ID for sub shape
+		SubShapeIDCreator shape1_sub_shape_id = mSubShapeIDCreator1.PushID(inSubShapeIndex, mSubShapeBits);
+
+		CollisionDispatch::sCollideShapeVsShape(inSubShape.mShape, mShape2, inSubShape.TransformScale(mScale1), mScale2, transform1, mTransform2, shape1_sub_shape_id, mSubShapeIDCreator2, mCollideShapeSettings, mCollector, mShapeFilter);
+	}
+
+	const CollideShapeSettings &	mCollideShapeSettings;
+	CollideShapeCollector &			mCollector;
+	const Shape *					mShape2;
+	Vec3							mScale1;
+	Vec3							mScale2;
+	Mat44							mTransform1;
+	Mat44							mTransform2;
+	AABox							mBoundsOf2InSpaceOf1;
+	SubShapeIDCreator				mSubShapeIDCreator1;
+	SubShapeIDCreator				mSubShapeIDCreator2;
+	uint							mSubShapeBits;
+	const ShapeFilter &				mShapeFilter;
+};
+
+struct CompoundShape::CollideShapeVsCompoundVisitor
+{
+	JPH_INLINE			CollideShapeVsCompoundVisitor(const Shape *inShape1, const CompoundShape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) :
+		mCollideShapeSettings(inCollideShapeSettings),
+		mCollector(ioCollector),
+		mShape1(inShape1),
+		mScale1(inScale1),
+		mScale2(inScale2),
+		mTransform1(inCenterOfMassTransform1),
+		mTransform2(inCenterOfMassTransform2),
+		mSubShapeIDCreator1(inSubShapeIDCreator1),
+		mSubShapeIDCreator2(inSubShapeIDCreator2),
+		mSubShapeBits(inShape2->GetSubShapeIDBits()),
+		mShapeFilter(inShapeFilter)
+	{
+		// Get transform from shape 1 to shape 2
+		Mat44 transform1_to_2 = inCenterOfMassTransform2.InversedRotationTranslation() * inCenterOfMassTransform1;
+
+		// Convert bounding box of 1 into space of 2
+		mBoundsOf1InSpaceOf2 = inShape1->GetLocalBounds().Scaled(inScale1).Transformed(transform1_to_2);
+		mBoundsOf1InSpaceOf2.ExpandBy(Vec3::sReplicate(inCollideShapeSettings.mMaxSeparationDistance));
+	}
+
+	/// Returns true when collision detection should abort because it's not possible to find a better hit
+	JPH_INLINE bool		ShouldAbort() const
+	{
+		return mCollector.ShouldEarlyOut();
+	}
+
+	/// Tests the bounds of shape 1 vs 4 bounding boxes, returns true for the ones that intersect
+	JPH_INLINE UVec4	TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+	{
+		// Scale the bounding boxes
+		Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
+		AABox4Scale(mScale2, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+		// Test which bounding boxes collide
+		return AABox4VsBox(mBoundsOf1InSpaceOf2, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+	}
+
+	/// Test the shape against a single subshape
+	JPH_INLINE void		VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex)
+	{
+		// Create ID for sub shape
+		SubShapeIDCreator shape2_sub_shape_id = mSubShapeIDCreator2.PushID(inSubShapeIndex, mSubShapeBits);
+
+		// Get world transform of 2
+		Mat44 transform2 = mTransform2 * inSubShape.GetLocalTransformNoScale(mScale2);
+
+		CollisionDispatch::sCollideShapeVsShape(mShape1, inSubShape.mShape, mScale1, inSubShape.TransformScale(mScale2), mTransform1, transform2, mSubShapeIDCreator1, shape2_sub_shape_id, mCollideShapeSettings, mCollector, mShapeFilter);
+	}
+
+	const CollideShapeSettings &	mCollideShapeSettings;
+	CollideShapeCollector &			mCollector;
+	const Shape *					mShape1;
+	Vec3							mScale1;
+	Vec3							mScale2;
+	Mat44							mTransform1;
+	Mat44							mTransform2;
+	AABox							mBoundsOf1InSpaceOf2;
+	SubShapeIDCreator				mSubShapeIDCreator1;
+	SubShapeIDCreator				mSubShapeIDCreator2;
+	uint							mSubShapeBits;
+	const ShapeFilter &				mShapeFilter;
+};
+
+template <class BoxType>
+struct CompoundShape::GetIntersectingSubShapesVisitor
+{
+	JPH_INLINE			GetIntersectingSubShapesVisitor(const BoxType &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) :
+		mBox(inBox),
+		mSubShapeIndices(outSubShapeIndices),
+		mMaxSubShapeIndices(inMaxSubShapeIndices)
+	{
+	}
+
+	/// Returns true when collision detection should abort because the buffer is full
+	JPH_INLINE bool		ShouldAbort() const
+	{
+		return mNumResults >= mMaxSubShapeIndices;
+	}
+
+	/// Tests the box vs 4 bounding boxes, returns true for the ones that intersect
+	JPH_INLINE UVec4	TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+	{
+		// Test which bounding boxes collide
+		return AABox4VsBox(mBox, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+	}
+
+	/// Records a hit
+	JPH_INLINE void		VisitShape([[maybe_unused]] const SubShape &inSubShape, uint32 inSubShapeIndex)
+	{
+		JPH_ASSERT(mNumResults < mMaxSubShapeIndices);
+		*mSubShapeIndices++ = inSubShapeIndex;
+		mNumResults++;
+	}
+
+	/// Get the number of indices that were found
+	JPH_INLINE int		GetNumResults() const
+	{
+		return mNumResults;
+	}
+
+private:
+	BoxType				mBox;
+	uint *				mSubShapeIndices;
+	int					mMaxSubShapeIndices;
+	int					mNumResults = 0;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.cpp
new file mode 100644
index 0000000000..1a1126e190
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.cpp
@@ -0,0 +1,1308 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/Shape/ConvexHullShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/Shape/PolyhedronSubmergedVolumeCalculator.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
+#include <Jolt/Geometry/ConvexHullBuilder.h>
+#include <Jolt/Geometry/ClosestPoint.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StringTools.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/Core/UnorderedMap.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(ConvexHullShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(ConvexHullShapeSettings, ConvexShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(ConvexHullShapeSettings, mPoints)
+	JPH_ADD_ATTRIBUTE(ConvexHullShapeSettings, mMaxConvexRadius)
+	JPH_ADD_ATTRIBUTE(ConvexHullShapeSettings, mMaxErrorConvexRadius)
+	JPH_ADD_ATTRIBUTE(ConvexHullShapeSettings, mHullTolerance)
+}
+
+ShapeSettings::ShapeResult ConvexHullShapeSettings::Create() const
+{
+	if (mCachedResult.IsEmpty())
+		Ref<Shape> shape = new ConvexHullShape(*this, mCachedResult);
+	return mCachedResult;
+}
+
+ConvexHullShape::ConvexHullShape(const ConvexHullShapeSettings &inSettings, ShapeResult &outResult) :
+	ConvexShape(EShapeSubType::ConvexHull, inSettings, outResult),
+	mConvexRadius(inSettings.mMaxConvexRadius)
+{
+	using BuilderFace = ConvexHullBuilder::Face;
+	using Edge = ConvexHullBuilder::Edge;
+	using Faces = Array<BuilderFace *>;
+
+	// Check convex radius
+	if (mConvexRadius < 0.0f)
+	{
+		outResult.SetError("Invalid convex radius");
+		return;
+	}
+
+	// Build convex hull
+	const char *error = nullptr;
+	ConvexHullBuilder builder(inSettings.mPoints);
+	ConvexHullBuilder::EResult result = builder.Initialize(cMaxPointsInHull, inSettings.mHullTolerance, error);
+	if (result != ConvexHullBuilder::EResult::Success && result != ConvexHullBuilder::EResult::MaxVerticesReached)
+	{
+		outResult.SetError(error);
+		return;
+	}
+	const Faces &builder_faces = builder.GetFaces();
+
+	// Check the consistency of the resulting hull if we fully built it
+	if (result == ConvexHullBuilder::EResult::Success)
+	{
+		ConvexHullBuilder::Face *max_error_face;
+		float max_error_distance, coplanar_distance;
+		int max_error_idx;
+		builder.DetermineMaxError(max_error_face, max_error_distance, max_error_idx, coplanar_distance);
+		if (max_error_distance > 4.0f * max(coplanar_distance, inSettings.mHullTolerance)) // Coplanar distance could be bigger than the allowed tolerance if the points are far apart
+		{
+			outResult.SetError(StringFormat("Hull building failed, point %d had an error of %g (relative to tolerance: %g)", max_error_idx, (double)max_error_distance, double(max_error_distance / inSettings.mHullTolerance)));
+			return;
+		}
+	}
+
+	// Calculate center of mass and volume
+	builder.GetCenterOfMassAndVolume(mCenterOfMass, mVolume);
+
+	// Calculate covariance matrix
+	// See:
+	// - Why the inertia tensor is the inertia tensor - Jonathan Blow (http://number-none.com/blow/inertia/deriving_i.html)
+	// - How to find the inertia tensor (or other mass properties) of a 3D solid body represented by a triangle mesh (Draft) - Jonathan Blow, Atman J Binstock (http://number-none.com/blow/inertia/bb_inertia.doc)
+	Mat44 covariance_canonical(Vec4(1.0f / 60.0f, 1.0f / 120.0f, 1.0f / 120.0f, 0), Vec4(1.0f / 120.0f, 1.0f / 60.0f, 1.0f / 120.0f, 0), Vec4(1.0f / 120.0f, 1.0f / 120.0f, 1.0f / 60.0f, 0), Vec4(0, 0, 0, 1));
+	Mat44 covariance_matrix = Mat44::sZero();
+	for (BuilderFace *f : builder_faces)
+	{
+		// Fourth point of the tetrahedron is at the center of mass, we subtract it from the other points so we get a tetrahedron with one vertex at zero
+		// The first point on the face will be used to form a triangle fan
+		Edge *e = f->mFirstEdge;
+		Vec3 v1 = inSettings.mPoints[e->mStartIdx] - mCenterOfMass;
+
+		// Get the 2nd point
+		e = e->mNextEdge;
+		Vec3 v2 = inSettings.mPoints[e->mStartIdx] - mCenterOfMass;
+
+		// Loop over the triangle fan
+		for (e = e->mNextEdge; e != f->mFirstEdge; e = e->mNextEdge)
+		{
+			Vec3 v3 = inSettings.mPoints[e->mStartIdx] - mCenterOfMass;
+
+			// Affine transform that transforms a unit tetrahedon (with vertices (0, 0, 0), (1, 0, 0), (0, 1, 0) and (0, 0, 1) to this tetrahedron
+			Mat44 a(Vec4(v1, 0), Vec4(v2, 0), Vec4(v3, 0), Vec4(0, 0, 0, 1));
+
+			// Calculate covariance matrix for this tetrahedron
+			float det_a = a.GetDeterminant3x3();
+			Mat44 c = det_a * (a * covariance_canonical * a.Transposed());
+
+			// Add it
+			covariance_matrix += c;
+
+			// Prepare for next triangle
+			v2 = v3;
+		}
+	}
+
+	// Calculate inertia matrix assuming density is 1, note that element (3, 3) is garbage
+	mInertia = Mat44::sIdentity() * (covariance_matrix(0, 0) + covariance_matrix(1, 1) + covariance_matrix(2, 2)) - covariance_matrix;
+
+	// Convert polygons from the builder to our internal representation
+	using VtxMap = UnorderedMap<int, uint8>;
+	VtxMap vertex_map;
+	vertex_map.reserve(VtxMap::size_type(inSettings.mPoints.size()));
+	for (BuilderFace *builder_face : builder_faces)
+	{
+		// Determine where the vertices go
+		JPH_ASSERT(mVertexIdx.size() <= 0xFFFF);
+		uint16 first_vertex = (uint16)mVertexIdx.size();
+		uint16 num_vertices = 0;
+
+		// Loop over vertices in face
+		Edge *edge = builder_face->mFirstEdge;
+		do
+		{
+			// Remap to new index, not all points in the original input set are required to form the hull
+			uint8 new_idx;
+			int original_idx = edge->mStartIdx;
+			VtxMap::iterator m = vertex_map.find(original_idx);
+			if (m != vertex_map.end())
+			{
+				// Found, reuse
+				new_idx = m->second;
+			}
+			else
+			{
+				// This is a new point
+				// Make relative to center of mass
+				Vec3 p = inSettings.mPoints[original_idx] - mCenterOfMass;
+
+				// Update local bounds
+				mLocalBounds.Encapsulate(p);
+
+				// Add to point list
+				JPH_ASSERT(mPoints.size() <= 0xff);
+				new_idx = (uint8)mPoints.size();
+				mPoints.push_back({ p });
+				vertex_map[original_idx] = new_idx;
+			}
+
+			// Append to vertex list
+			JPH_ASSERT(mVertexIdx.size() < 0xffff);
+			mVertexIdx.push_back(new_idx);
+			num_vertices++;
+
+			edge = edge->mNextEdge;
+		} while (edge != builder_face->mFirstEdge);
+
+		// Add face
+		mFaces.push_back({ first_vertex, num_vertices });
+
+		// Add plane
+		Plane plane = Plane::sFromPointAndNormal(builder_face->mCentroid - mCenterOfMass, builder_face->mNormal.Normalized());
+		mPlanes.push_back(plane);
+	}
+
+	// Test if GetSupportFunction can support this many points
+	if (mPoints.size() > cMaxPointsInHull)
+	{
+		outResult.SetError(StringFormat("Internal error: Too many points in hull (%u), max allowed %d", (uint)mPoints.size(), cMaxPointsInHull));
+		return;
+	}
+
+	for (int p = 0; p < (int)mPoints.size(); ++p)
+	{
+		// For each point, find faces that use the point
+		Array<int> faces;
+		for (int f = 0; f < (int)mFaces.size(); ++f)
+		{
+			const Face &face = mFaces[f];
+			for (int v = 0; v < face.mNumVertices; ++v)
+				if (mVertexIdx[face.mFirstVertex + v] == p)
+				{
+					faces.push_back(f);
+					break;
+				}
+		}
+
+		if (faces.size() < 2)
+		{
+			outResult.SetError("A point must be connected to 2 or more faces!");
+			return;
+		}
+
+		// Find the 3 normals that form the largest tetrahedron
+		// The largest tetrahedron we can get is ((1, 0, 0) x (0, 1, 0)) . (0, 0, 1) = 1, if the volume is only 5% of that,
+		// the three vectors are too coplanar and we fall back to using only 2 plane normals
+		float biggest_volume = 0.05f;
+		int best3[3] = { -1, -1, -1 };
+
+		// When using 2 normals, we get the two with the biggest angle between them with a minimal difference of 1 degree
+		// otherwise we fall back to just using 1 plane normal
+		float smallest_dot = Cos(DegreesToRadians(1.0f));
+		int best2[2] = { -1, -1 };
+
+		for (int face1 = 0; face1 < (int)faces.size(); ++face1)
+		{
+			Vec3 normal1 = mPlanes[faces[face1]].GetNormal();
+			for (int face2 = face1 + 1; face2 < (int)faces.size(); ++face2)
+			{
+				Vec3 normal2 = mPlanes[faces[face2]].GetNormal();
+				Vec3 cross = normal1.Cross(normal2);
+
+				// Determine the 2 face normals that are most apart
+				float dot = normal1.Dot(normal2);
+				if (dot < smallest_dot)
+				{
+					smallest_dot = dot;
+					best2[0] = faces[face1];
+					best2[1] = faces[face2];
+				}
+
+				// Determine the 3 face normals that form the largest tetrahedron
+				for (int face3 = face2 + 1; face3 < (int)faces.size(); ++face3)
+				{
+					Vec3 normal3 = mPlanes[faces[face3]].GetNormal();
+					float volume = abs(cross.Dot(normal3));
+					if (volume > biggest_volume)
+					{
+						biggest_volume = volume;
+						best3[0] = faces[face1];
+						best3[1] = faces[face2];
+						best3[2] = faces[face3];
+					}
+				}
+			}
+		}
+
+		// If we didn't find 3 planes, use 2, if we didn't find 2 use 1
+		if (best3[0] != -1)
+			faces = { best3[0], best3[1], best3[2] };
+		else if (best2[0] != -1)
+			faces = { best2[0], best2[1] };
+		else
+			faces = { faces[0] };
+
+		// Copy the faces to the points buffer
+		Point &point = mPoints[p];
+		point.mNumFaces = (int)faces.size();
+		for (int i = 0; i < (int)faces.size(); ++i)
+			point.mFaces[i] = faces[i];
+	}
+
+	// If the convex radius is already zero, there's no point in further reducing it
+	if (mConvexRadius > 0.0f)
+	{
+		// Find out how thin the hull is by walking over all planes and checking the thickness of the hull in that direction
+		float min_size = FLT_MAX;
+		for (const Plane &plane : mPlanes)
+		{
+			// Take the point that is furthest away from the plane as thickness of this hull
+			float max_dist = 0.0f;
+			for (const Point &point : mPoints)
+			{
+				float dist = -plane.SignedDistance(point.mPosition); // Point is always behind plane, so we need to negate
+				if (dist > max_dist)
+					max_dist = dist;
+			}
+			min_size = min(min_size, max_dist);
+		}
+
+		// We need to fit in 2x the convex radius in min_size, so reduce the convex radius if it's bigger than that
+		mConvexRadius = min(mConvexRadius, 0.5f * min_size);
+	}
+
+	// Now walk over all points and see if we have to further reduce the convex radius because of sharp edges
+	if (mConvexRadius > 0.0f)
+	{
+		for (const Point &point : mPoints)
+			if (point.mNumFaces != 1) // If we have a single face, shifting back is easy and we don't need to reduce the convex radius
+			{
+				// Get first two planes
+				Plane p1 = mPlanes[point.mFaces[0]];
+				Plane p2 = mPlanes[point.mFaces[1]];
+				Plane p3;
+				Vec3 offset_mask;
+
+				if (point.mNumFaces == 3)
+				{
+					// Get third plane
+					p3 = mPlanes[point.mFaces[2]];
+
+					// All 3 planes will be offset by the convex radius
+					offset_mask = Vec3::sReplicate(1);
+				}
+				else
+				{
+					// Third plane has normal perpendicular to the other two planes and goes through the vertex position
+					JPH_ASSERT(point.mNumFaces == 2);
+					p3 = Plane::sFromPointAndNormal(point.mPosition, p1.GetNormal().Cross(p2.GetNormal()));
+
+					// Only the first and 2nd plane will be offset, the 3rd plane is only there to guide the intersection point
+					offset_mask = Vec3(1, 1, 0);
+				}
+
+				// Plane equation: point . normal + constant = 0
+				// Offsetting the plane backwards with convex radius r: point . normal + constant + r = 0
+				// To find the intersection 'point' of 3 planes we solve:
+				// |n1x n1y n1z| |x|     | r + c1 |
+				// |n2x n2y n2z| |y| = - | r + c2 | <=> n point = -r (1, 1, 1) - (c1, c2, c3)
+				// |n3x n3y n3z| |z|     | r + c3 |
+				// Where point = (x, y, z), n1x is the x component of the first plane, c1 = plane constant of plane 1, etc.
+				// The relation between how much the intersection point shifts as a function of r is: -r * n^-1 (1, 1, 1) = r * offset
+				// Where offset = -n^-1 (1, 1, 1) or -n^-1 (1, 1, 0) in case only the first 2 planes are offset
+				// The error that is introduced by a convex radius r is: error = r * |offset| - r
+				// So the max convex radius given error is: r = error / (|offset| - 1)
+				Mat44 n = Mat44(Vec4(p1.GetNormal(), 0), Vec4(p2.GetNormal(), 0), Vec4(p3.GetNormal(), 0), Vec4(0, 0, 0, 1)).Transposed();
+				float det_n = n.GetDeterminant3x3();
+				if (det_n == 0.0f)
+				{
+					// If the determinant is zero, the matrix is not invertible so no solution exists to move the point backwards and we have to choose a convex radius of zero
+					mConvexRadius = 0.0f;
+					break;
+				}
+				Mat44 adj_n = n.Adjointed3x3();
+				float offset = ((adj_n * offset_mask) / det_n).Length();
+				JPH_ASSERT(offset > 1.0f);
+				float max_convex_radius = inSettings.mMaxErrorConvexRadius / (offset - 1.0f);
+				mConvexRadius = min(mConvexRadius, max_convex_radius);
+			}
+		}
+
+	// Calculate the inner radius by getting the minimum distance from the origin to the planes of the hull
+	mInnerRadius = FLT_MAX;
+	for (const Plane &p : mPlanes)
+		mInnerRadius = min(mInnerRadius, -p.GetConstant());
+	mInnerRadius = max(0.0f, mInnerRadius); // Clamp against zero, this should do nothing as the shape is centered around the center of mass but for flat convex hulls there may be numerical round off issues
+
+	outResult.Set(this);
+}
+
+MassProperties ConvexHullShape::GetMassProperties() const
+{
+	MassProperties p;
+
+	float density = GetDensity();
+
+	// Calculate mass
+	p.mMass = density * mVolume;
+
+	// Calculate inertia matrix
+	p.mInertia = density * mInertia;
+	p.mInertia(3, 3) = 1.0f;
+
+	return p;
+}
+
+Vec3 ConvexHullShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
+
+	const Plane &first_plane = mPlanes[0];
+	Vec3 best_normal = first_plane.GetNormal();
+	float best_dist = abs(first_plane.SignedDistance(inLocalSurfacePosition));
+
+	// Find the face that has the shortest distance to the surface point
+	for (Array<Face>::size_type i = 1; i < mFaces.size(); ++i)
+	{
+		const Plane &plane = mPlanes[i];
+		Vec3 plane_normal = plane.GetNormal();
+		float dist = abs(plane.SignedDistance(inLocalSurfacePosition));
+		if (dist < best_dist)
+		{
+			best_dist = dist;
+			best_normal = plane_normal;
+		}
+	}
+
+	return best_normal;
+}
+
+class ConvexHullShape::HullNoConvex final : public Support
+{
+public:
+	explicit				HullNoConvex(float inConvexRadius) :
+		mConvexRadius(inConvexRadius)
+	{
+		static_assert(sizeof(HullNoConvex) <= sizeof(SupportBuffer), "Buffer size too small");
+		JPH_ASSERT(IsAligned(this, alignof(HullNoConvex)));
+	}
+
+	virtual Vec3			GetSupport(Vec3Arg inDirection) const override
+	{
+		// Find the point with the highest projection on inDirection
+		float best_dot = -FLT_MAX;
+		Vec3 best_point = Vec3::sZero();
+
+		for (Vec3 point : mPoints)
+		{
+			// Check if its support is bigger than the current max
+			float dot = point.Dot(inDirection);
+			if (dot > best_dot)
+			{
+				best_dot = dot;
+				best_point = point;
+			}
+		}
+
+		return best_point;
+	}
+
+	virtual float			GetConvexRadius() const override
+	{
+		return mConvexRadius;
+	}
+
+	using PointsArray = StaticArray<Vec3, cMaxPointsInHull>;
+
+	inline PointsArray &	GetPoints()
+	{
+		return mPoints;
+	}
+
+	const PointsArray &		GetPoints() const
+	{
+		return mPoints;
+	}
+
+private:
+	float					mConvexRadius;
+	PointsArray				mPoints;
+};
+
+class ConvexHullShape::HullWithConvex final : public Support
+{
+public:
+	explicit				HullWithConvex(const ConvexHullShape *inShape) :
+		mShape(inShape)
+	{
+		static_assert(sizeof(HullWithConvex) <= sizeof(SupportBuffer), "Buffer size too small");
+		JPH_ASSERT(IsAligned(this, alignof(HullWithConvex)));
+	}
+
+	virtual Vec3			GetSupport(Vec3Arg inDirection) const override
+	{
+		// Find the point with the highest projection on inDirection
+		float best_dot = -FLT_MAX;
+		Vec3 best_point = Vec3::sZero();
+
+		for (const Point &point : mShape->mPoints)
+		{
+			// Check if its support is bigger than the current max
+			float dot = point.mPosition.Dot(inDirection);
+			if (dot > best_dot)
+			{
+				best_dot = dot;
+				best_point = point.mPosition;
+			}
+		}
+
+		return best_point;
+	}
+
+	virtual float			GetConvexRadius() const override
+	{
+		return 0.0f;
+	}
+
+private:
+	const ConvexHullShape *	mShape;
+};
+
+class ConvexHullShape::HullWithConvexScaled final : public Support
+{
+public:
+							HullWithConvexScaled(const ConvexHullShape *inShape, Vec3Arg inScale) :
+		mShape(inShape),
+		mScale(inScale)
+	{
+		static_assert(sizeof(HullWithConvexScaled) <= sizeof(SupportBuffer), "Buffer size too small");
+		JPH_ASSERT(IsAligned(this, alignof(HullWithConvexScaled)));
+	}
+
+	virtual Vec3			GetSupport(Vec3Arg inDirection) const override
+	{
+		// Find the point with the highest projection on inDirection
+		float best_dot = -FLT_MAX;
+		Vec3 best_point = Vec3::sZero();
+
+		for (const Point &point : mShape->mPoints)
+		{
+			// Calculate scaled position
+			Vec3 pos = mScale * point.mPosition;
+
+			// Check if its support is bigger than the current max
+			float dot = pos.Dot(inDirection);
+			if (dot > best_dot)
+			{
+				best_dot = dot;
+				best_point = pos;
+			}
+		}
+
+		return best_point;
+	}
+
+	virtual float			GetConvexRadius() const override
+	{
+		return 0.0f;
+	}
+
+private:
+	const ConvexHullShape *	mShape;
+	Vec3					mScale;
+};
+
+const ConvexShape::Support *ConvexHullShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const
+{
+	// If there's no convex radius, we don't need to shrink the hull
+	if (mConvexRadius == 0.0f)
+	{
+		if (ScaleHelpers::IsNotScaled(inScale))
+			return new (&inBuffer) HullWithConvex(this);
+		else
+			return new (&inBuffer) HullWithConvexScaled(this, inScale);
+	}
+
+	switch (inMode)
+	{
+	case ESupportMode::IncludeConvexRadius:
+	case ESupportMode::Default:
+		if (ScaleHelpers::IsNotScaled(inScale))
+			return new (&inBuffer) HullWithConvex(this);
+		else
+			return new (&inBuffer) HullWithConvexScaled(this, inScale);
+
+	case ESupportMode::ExcludeConvexRadius:
+		if (ScaleHelpers::IsNotScaled(inScale))
+		{
+			// Create support function
+			HullNoConvex *hull = new (&inBuffer) HullNoConvex(mConvexRadius);
+			HullNoConvex::PointsArray &transformed_points = hull->GetPoints();
+			JPH_ASSERT(mPoints.size() <= cMaxPointsInHull, "Not enough space, this should have been caught during shape creation!");
+
+			for (const Point &point : mPoints)
+			{
+				Vec3 new_point;
+
+				if (point.mNumFaces == 1)
+				{
+					// Simply shift back by the convex radius using our 1 plane
+					new_point = point.mPosition - mPlanes[point.mFaces[0]].GetNormal() * mConvexRadius;
+				}
+				else
+				{
+					// Get first two planes and offset inwards by convex radius
+					Plane p1 = mPlanes[point.mFaces[0]].Offset(-mConvexRadius);
+					Plane p2 = mPlanes[point.mFaces[1]].Offset(-mConvexRadius);
+					Plane p3;
+
+					if (point.mNumFaces == 3)
+					{
+						// Get third plane and offset inwards by convex radius
+						p3 = mPlanes[point.mFaces[2]].Offset(-mConvexRadius);
+					}
+					else
+					{
+						// Third plane has normal perpendicular to the other two planes and goes through the vertex position
+						JPH_ASSERT(point.mNumFaces == 2);
+						p3 = Plane::sFromPointAndNormal(point.mPosition, p1.GetNormal().Cross(p2.GetNormal()));
+					}
+
+					// Find intersection point between the three planes
+					if (!Plane::sIntersectPlanes(p1, p2, p3, new_point))
+					{
+						// Fallback: Just push point back using the first plane
+						new_point = point.mPosition - p1.GetNormal() * mConvexRadius;
+					}
+				}
+
+				// Add point
+				transformed_points.push_back(new_point);
+			}
+
+			return hull;
+		}
+		else
+		{
+			// Calculate scaled convex radius
+			float convex_radius = ScaleHelpers::ScaleConvexRadius(mConvexRadius, inScale);
+
+			// Create new support function
+			HullNoConvex *hull = new (&inBuffer) HullNoConvex(convex_radius);
+			HullNoConvex::PointsArray &transformed_points = hull->GetPoints();
+			JPH_ASSERT(mPoints.size() <= cMaxPointsInHull, "Not enough space, this should have been caught during shape creation!");
+
+			// Precalculate inverse scale
+			Vec3 inv_scale = inScale.Reciprocal();
+
+			for (const Point &point : mPoints)
+			{
+				// Calculate scaled position
+				Vec3 pos = inScale * point.mPosition;
+
+				// Transform normals for plane 1 with scale
+				Vec3 n1 = (inv_scale * mPlanes[point.mFaces[0]].GetNormal()).Normalized();
+
+				Vec3 new_point;
+
+				if (point.mNumFaces == 1)
+				{
+					// Simply shift back by the convex radius using our 1 plane
+					new_point = pos - n1 * convex_radius;
+				}
+				else
+				{
+					// Transform normals for plane 2 with scale
+					Vec3 n2 = (inv_scale * mPlanes[point.mFaces[1]].GetNormal()).Normalized();
+
+					// Get first two planes and offset inwards by convex radius
+					Plane p1 = Plane::sFromPointAndNormal(pos, n1).Offset(-convex_radius);
+					Plane p2 = Plane::sFromPointAndNormal(pos, n2).Offset(-convex_radius);
+					Plane p3;
+
+					if (point.mNumFaces == 3)
+					{
+						// Transform last normal with scale
+						Vec3 n3 = (inv_scale * mPlanes[point.mFaces[2]].GetNormal()).Normalized();
+
+						// Get third plane and offset inwards by convex radius
+						p3 = Plane::sFromPointAndNormal(pos, n3).Offset(-convex_radius);
+					}
+					else
+					{
+						// Third plane has normal perpendicular to the other two planes and goes through the vertex position
+						JPH_ASSERT(point.mNumFaces == 2);
+						p3 = Plane::sFromPointAndNormal(pos, n1.Cross(n2));
+					}
+
+					// Find intersection point between the three planes
+					if (!Plane::sIntersectPlanes(p1, p2, p3, new_point))
+					{
+						// Fallback: Just push point back using the first plane
+						new_point = pos - n1 * convex_radius;
+					}
+				}
+
+				// Add point
+				transformed_points.push_back(new_point);
+			}
+
+			return hull;
+		}
+	}
+
+	JPH_ASSERT(false);
+	return nullptr;
+}
+
+void ConvexHullShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
+
+	Vec3 inv_scale = inScale.Reciprocal();
+
+	// Need to transform the plane normals using inScale
+	// Transforming a direction with matrix M is done through multiplying by (M^-1)^T
+	// In this case M is a diagonal matrix with the scale vector, so we need to multiply our normal by 1 / scale and renormalize afterwards
+	Vec3 plane0_normal = inv_scale * mPlanes[0].GetNormal();
+	float best_dot = plane0_normal.Dot(inDirection) / plane0_normal.Length();
+	int best_face_idx = 0;
+
+	for (Array<Plane>::size_type i = 1; i < mPlanes.size(); ++i)
+	{
+		Vec3 plane_normal = inv_scale * mPlanes[i].GetNormal();
+		float dot = plane_normal.Dot(inDirection) / plane_normal.Length();
+		if (dot < best_dot)
+		{
+			best_dot = dot;
+			best_face_idx = (int)i;
+		}
+	}
+
+	// Get vertices
+	const Face &best_face = mFaces[best_face_idx];
+	const uint8 *first_vtx = mVertexIdx.data() + best_face.mFirstVertex;
+	const uint8 *end_vtx = first_vtx + best_face.mNumVertices;
+
+	// If we have more than 1/2 the capacity of outVertices worth of vertices, we start skipping vertices (note we can't fill the buffer completely since extra edges will be generated by clipping).
+	// TODO: This really needs a better algorithm to determine which vertices are important!
+	int max_vertices_to_return = outVertices.capacity() / 2;
+	int delta_vtx = (int(best_face.mNumVertices) + max_vertices_to_return) / max_vertices_to_return;
+
+	// Calculate transform with scale
+	Mat44 transform = inCenterOfMassTransform.PreScaled(inScale);
+
+	if (ScaleHelpers::IsInsideOut(inScale))
+	{
+		// Flip winding of supporting face
+		for (const uint8 *v = end_vtx - 1; v >= first_vtx; v -= delta_vtx)
+			outVertices.push_back(transform * mPoints[*v].mPosition);
+	}
+	else
+	{
+		// Normal winding of supporting face
+		for (const uint8 *v = first_vtx; v < end_vtx; v += delta_vtx)
+			outVertices.push_back(transform * mPoints[*v].mPosition);
+	}
+}
+
+void ConvexHullShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const
+{
+	// Trivially calculate total volume
+	Vec3 abs_scale = inScale.Abs();
+	outTotalVolume = mVolume * abs_scale.GetX() * abs_scale.GetY() * abs_scale.GetZ();
+
+	// Check if shape has been scaled inside out
+	bool is_inside_out = ScaleHelpers::IsInsideOut(inScale);
+
+	// Convert the points to world space and determine the distance to the surface
+	int num_points = int(mPoints.size());
+	PolyhedronSubmergedVolumeCalculator::Point *buffer = (PolyhedronSubmergedVolumeCalculator::Point *)JPH_STACK_ALLOC(num_points * sizeof(PolyhedronSubmergedVolumeCalculator::Point));
+	PolyhedronSubmergedVolumeCalculator submerged_vol_calc(inCenterOfMassTransform * Mat44::sScale(inScale), &mPoints[0].mPosition, sizeof(Point), num_points, inSurface, buffer JPH_IF_DEBUG_RENDERER(, inBaseOffset));
+
+	if (submerged_vol_calc.AreAllAbove())
+	{
+		// We're above the water
+		outSubmergedVolume = 0.0f;
+		outCenterOfBuoyancy = Vec3::sZero();
+	}
+	else if (submerged_vol_calc.AreAllBelow())
+	{
+		// We're fully submerged
+		outSubmergedVolume = outTotalVolume;
+		outCenterOfBuoyancy = inCenterOfMassTransform.GetTranslation();
+	}
+	else
+	{
+		// Calculate submerged volume
+		int reference_point_idx = submerged_vol_calc.GetReferencePointIdx();
+		for (const Face &f : mFaces)
+		{
+			const uint8 *first_vtx = mVertexIdx.data() + f.mFirstVertex;
+			const uint8 *end_vtx = first_vtx + f.mNumVertices;
+
+			// If any of the vertices of this face are the reference point, the volume will be zero so we can skip this face
+			bool degenerate = false;
+			for (const uint8 *v = first_vtx; v < end_vtx; ++v)
+				if (*v == reference_point_idx)
+				{
+					degenerate = true;
+					break;
+				}
+			if (degenerate)
+				continue;
+
+			// Triangulate the face
+			int i1 = *first_vtx;
+			if (is_inside_out)
+			{
+				// Reverse winding
+				for (const uint8 *v = first_vtx + 2; v < end_vtx; ++v)
+				{
+					int i2 = *(v - 1);
+					int i3 = *v;
+					submerged_vol_calc.AddFace(i1, i3, i2);
+				}
+			}
+			else
+			{
+				// Normal winding
+				for (const uint8 *v = first_vtx + 2; v < end_vtx; ++v)
+				{
+					int i2 = *(v - 1);
+					int i3 = *v;
+					submerged_vol_calc.AddFace(i1, i2, i3);
+				}
+			}
+		}
+
+		// Get the results
+		submerged_vol_calc.GetResult(outSubmergedVolume, outCenterOfBuoyancy);
+	}
+
+#ifdef JPH_DEBUG_RENDERER
+	// Draw center of buoyancy
+	if (sDrawSubmergedVolumes)
+		DebugRenderer::sInstance->DrawWireSphere(inBaseOffset + outCenterOfBuoyancy, 0.05f, Color::sRed, 1);
+#endif // JPH_DEBUG_RENDERER
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void ConvexHullShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	if (mGeometry == nullptr)
+	{
+		Array<DebugRenderer::Triangle> triangles;
+		for (const Face &f : mFaces)
+		{
+			const uint8 *first_vtx = mVertexIdx.data() + f.mFirstVertex;
+			const uint8 *end_vtx = first_vtx + f.mNumVertices;
+
+			// Draw first triangle of polygon
+			Vec3 v0 = mPoints[first_vtx[0]].mPosition;
+			Vec3 v1 = mPoints[first_vtx[1]].mPosition;
+			Vec3 v2 = mPoints[first_vtx[2]].mPosition;
+			Vec3 uv_direction = (v1 - v0).Normalized();
+			triangles.push_back({ v0, v1, v2, Color::sWhite, v0, uv_direction });
+
+			// Draw any other triangles in this polygon
+			for (const uint8 *v = first_vtx + 3; v < end_vtx; ++v)
+				triangles.push_back({ v0, mPoints[*(v - 1)].mPosition, mPoints[*v].mPosition, Color::sWhite, v0, uv_direction });
+		}
+		mGeometry = new DebugRenderer::Geometry(inRenderer->CreateTriangleBatch(triangles), GetLocalBounds());
+	}
+
+	// Test if the shape is scaled inside out
+	DebugRenderer::ECullMode cull_mode = ScaleHelpers::IsInsideOut(inScale)? DebugRenderer::ECullMode::CullFrontFace : DebugRenderer::ECullMode::CullBackFace;
+
+	// Determine the draw mode
+	DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid;
+
+	// Draw the geometry
+	Color color = inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor;
+	RMat44 transform = inCenterOfMassTransform.PreScaled(inScale);
+	inRenderer->DrawGeometry(transform, color, mGeometry, cull_mode, DebugRenderer::ECastShadow::On, draw_mode);
+
+	// Draw the outline if requested
+	if (sDrawFaceOutlines)
+		for (const Face &f : mFaces)
+		{
+			const uint8 *first_vtx = mVertexIdx.data() + f.mFirstVertex;
+			const uint8 *end_vtx = first_vtx + f.mNumVertices;
+
+			// Draw edges of face
+			inRenderer->DrawLine(transform * mPoints[*(end_vtx - 1)].mPosition, transform * mPoints[*first_vtx].mPosition, Color::sGrey);
+			for (const uint8 *v = first_vtx + 1; v < end_vtx; ++v)
+				inRenderer->DrawLine(transform * mPoints[*(v - 1)].mPosition, transform * mPoints[*v].mPosition, Color::sGrey);
+		}
+}
+
+void ConvexHullShape::DrawShrunkShape(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
+	// Get the shrunk points
+	SupportBuffer buffer;
+	const HullNoConvex *support = mConvexRadius > 0.0f? static_cast<const HullNoConvex *>(GetSupportFunction(ESupportMode::ExcludeConvexRadius, buffer, inScale)) : nullptr;
+
+	RMat44 transform = inCenterOfMassTransform * Mat44::sScale(inScale);
+
+	for (int p = 0; p < (int)mPoints.size(); ++p)
+	{
+		const Point &point = mPoints[p];
+		RVec3 position = transform * point.mPosition;
+		RVec3 shrunk_point = support != nullptr? transform * support->GetPoints()[p] : position;
+
+		// Draw difference between shrunk position and position
+		inRenderer->DrawLine(position, shrunk_point, Color::sGreen);
+
+		// Draw face normals that are contributing
+		for (int i = 0; i < point.mNumFaces; ++i)
+			inRenderer->DrawLine(position, position + 0.1f * mPlanes[point.mFaces[i]].GetNormal(), Color::sYellow);
+
+		// Draw point index
+		inRenderer->DrawText3D(position, ConvertToString(p), Color::sWhite, 0.1f);
+	}
+}
+#endif // JPH_DEBUG_RENDERER
+
+bool ConvexHullShape::CastRayHelper(const RayCast &inRay, float &outMinFraction, float &outMaxFraction) const
+{
+	if (mFaces.size() == 2)
+	{
+		// If we have only 2 faces, we're a flat convex hull and we need to test edges instead of planes
+
+		// Check if plane is parallel to ray
+		const Plane &p = mPlanes.front();
+		Vec3 plane_normal = p.GetNormal();
+		float direction_projection = inRay.mDirection.Dot(plane_normal);
+		if (abs(direction_projection) >= 1.0e-12f)
+		{
+			// Calculate intersection point
+			float distance_to_plane = inRay.mOrigin.Dot(plane_normal) + p.GetConstant();
+			float fraction = -distance_to_plane / direction_projection;
+			if (fraction < 0.0f || fraction > 1.0f)
+			{
+				// Does not hit plane, no hit
+				outMinFraction = 0.0f;
+				outMaxFraction = 1.0f + FLT_EPSILON;
+				return false;
+			}
+			Vec3 intersection_point = inRay.mOrigin + fraction * inRay.mDirection;
+
+			// Test all edges to see if point is inside polygon
+			const Face &f = mFaces.front();
+			const uint8 *first_vtx = mVertexIdx.data() + f.mFirstVertex;
+			const uint8 *end_vtx = first_vtx + f.mNumVertices;
+			Vec3 p1 = mPoints[*end_vtx].mPosition;
+			for (const uint8 *v = first_vtx; v < end_vtx; ++v)
+			{
+				Vec3 p2 = mPoints[*v].mPosition;
+				if ((p2 - p1).Cross(intersection_point - p1).Dot(plane_normal) < 0.0f)
+				{
+					// Outside polygon, no hit
+					outMinFraction = 0.0f;
+					outMaxFraction = 1.0f + FLT_EPSILON;
+					return false;
+				}
+				p1 = p2;
+			}
+
+			// Inside polygon, a hit
+			outMinFraction = fraction;
+			outMaxFraction = fraction;
+			return true;
+		}
+		else
+		{
+			// Parallel ray doesn't hit
+			outMinFraction = 0.0f;
+			outMaxFraction = 1.0f + FLT_EPSILON;
+			return false;
+		}
+	}
+	else
+	{
+		// Clip ray against all planes
+		int fractions_set = 0;
+		bool all_inside = true;
+		float min_fraction = 0.0f, max_fraction = 1.0f + FLT_EPSILON;
+		for (const Plane &p : mPlanes)
+		{
+			// Check if the ray origin is behind this plane
+			Vec3 plane_normal = p.GetNormal();
+			float distance_to_plane = inRay.mOrigin.Dot(plane_normal) + p.GetConstant();
+			bool is_outside = distance_to_plane > 0.0f;
+			all_inside &= !is_outside;
+
+			// Check if plane is parallel to ray
+			float direction_projection = inRay.mDirection.Dot(plane_normal);
+			if (abs(direction_projection) >= 1.0e-12f)
+			{
+				// Get intersection fraction between ray and plane
+				float fraction = -distance_to_plane / direction_projection;
+
+				// Update interval of ray that is inside the hull
+				if (direction_projection < 0.0f)
+				{
+					min_fraction = max(fraction, min_fraction);
+					fractions_set |= 1;
+				}
+				else
+				{
+					max_fraction = min(fraction, max_fraction);
+					fractions_set |= 2;
+				}
+			}
+			else if (is_outside)
+				return false; // Outside the plane and parallel, no hit!
+		}
+
+		// Test if both min and max have been set
+		if (fractions_set == 3)
+		{
+			// Output fractions
+			outMinFraction = min_fraction;
+			outMaxFraction = max_fraction;
+
+			// Test if the infinite ray intersects with the hull (the length will be checked later)
+			return min_fraction <= max_fraction && max_fraction >= 0.0f;
+		}
+		else
+		{
+			// Degenerate case, either the ray is parallel to all planes or the ray has zero length
+			outMinFraction = 0.0f;
+			outMaxFraction = 1.0f + FLT_EPSILON;
+
+			// Return if the origin is inside the hull
+			return all_inside;
+		}
+	}
+}
+
+bool ConvexHullShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	// Determine if ray hits the shape
+	float min_fraction, max_fraction;
+	if (CastRayHelper(inRay, min_fraction, max_fraction)
+		&& min_fraction < ioHit.mFraction) // Check if this is a closer hit
+	{
+		// Better hit than the current hit
+		ioHit.mFraction = min_fraction;
+		ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID();
+		return true;
+	}
+	return false;
+}
+
+void ConvexHullShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	// Determine if ray hits the shape
+	float min_fraction, max_fraction;
+	if (CastRayHelper(inRay, min_fraction, max_fraction)
+		&& min_fraction < ioCollector.GetEarlyOutFraction()) // Check if this is closer than the early out fraction
+	{
+		// Better hit than the current hit
+		RayCastResult hit;
+		hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext());
+		hit.mSubShapeID2 = inSubShapeIDCreator.GetID();
+
+		// Check front side hit
+		if (inRayCastSettings.mTreatConvexAsSolid || min_fraction > 0.0f)
+		{
+			hit.mFraction = min_fraction;
+			ioCollector.AddHit(hit);
+		}
+
+		// Check back side hit
+		if (inRayCastSettings.mBackFaceModeConvex == EBackFaceMode::CollideWithBackFaces
+			&& max_fraction < ioCollector.GetEarlyOutFraction())
+		{
+			hit.mFraction = max_fraction;
+			ioCollector.AddHit(hit);
+		}
+	}
+}
+
+void ConvexHullShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	// Check if point is behind all planes
+	for (const Plane &p : mPlanes)
+		if (p.SignedDistance(inPoint) > 0.0f)
+			return;
+
+	// Point is inside
+	ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
+}
+
+void ConvexHullShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
+{
+	Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation();
+
+	Vec3 inv_scale = inScale.Reciprocal();
+	bool is_not_scaled = ScaleHelpers::IsNotScaled(inScale);
+	float scale_flip = ScaleHelpers::IsInsideOut(inScale)? -1.0f : 1.0f;
+
+	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
+		if (v.GetInvMass() > 0.0f)
+		{
+			Vec3 local_pos = inverse_transform * v.GetPosition();
+
+			// Find most facing plane
+			float max_distance = -FLT_MAX;
+			Vec3 max_plane_normal = Vec3::sZero();
+			uint max_plane_idx = 0;
+			if (is_not_scaled)
+			{
+				// Without scale, it is trivial to calculate the distance to the hull
+				for (const Plane &p : mPlanes)
+				{
+					float distance = p.SignedDistance(local_pos);
+					if (distance > max_distance)
+					{
+						max_distance = distance;
+						max_plane_normal = p.GetNormal();
+						max_plane_idx = uint(&p - mPlanes.data());
+					}
+				}
+			}
+			else
+			{
+				// When there's scale we need to calculate the planes first
+				for (uint i = 0; i < (uint)mPlanes.size(); ++i)
+				{
+					// Calculate plane normal and point by scaling the original plane
+					Vec3 plane_normal = (inv_scale * mPlanes[i].GetNormal()).Normalized();
+					Vec3 plane_point = inScale * mPoints[mVertexIdx[mFaces[i].mFirstVertex]].mPosition;
+
+					float distance = plane_normal.Dot(local_pos - plane_point);
+					if (distance > max_distance)
+					{
+						max_distance = distance;
+						max_plane_normal = plane_normal;
+						max_plane_idx = i;
+					}
+				}
+			}
+			bool is_outside = max_distance > 0.0f;
+
+			// Project point onto that plane
+			Vec3 closest_point = local_pos - max_distance * max_plane_normal;
+
+			// Check edges if we're outside the hull (when inside we know the closest face is also the closest point to the surface)
+			if (is_outside)
+			{
+				// Loop over edges
+				float closest_point_dist_sq = FLT_MAX;
+				const Face &face = mFaces[max_plane_idx];
+				for (const uint8 *v_start = &mVertexIdx[face.mFirstVertex], *v1 = v_start, *v_end = v_start + face.mNumVertices; v1 < v_end; ++v1)
+				{
+					// Find second point
+					const uint8 *v2 = v1 + 1;
+					if (v2 == v_end)
+						v2 = v_start;
+
+					// Get edge points
+					Vec3 p1 = inScale * mPoints[*v1].mPosition;
+					Vec3 p2 = inScale * mPoints[*v2].mPosition;
+
+					// Check if the position is outside the edge (if not, the face will be closer)
+					Vec3 edge_normal = (p2 - p1).Cross(max_plane_normal);
+					if (scale_flip * edge_normal.Dot(local_pos - p1) > 0.0f)
+					{
+						// Get closest point on edge
+						uint32 set;
+						Vec3 closest = ClosestPoint::GetClosestPointOnLine(p1 - local_pos, p2 - local_pos, set);
+						float distance_sq = closest.LengthSq();
+						if (distance_sq < closest_point_dist_sq)
+							closest_point = local_pos + closest;
+					}
+				}
+			}
+
+			// Check if this is the largest penetration
+			Vec3 normal = local_pos - closest_point;
+			float normal_length = normal.Length();
+			float penetration = normal_length;
+			if (is_outside)
+				penetration = -penetration;
+			else
+				normal = -normal;
+			if (v.UpdatePenetration(penetration))
+			{
+				// Calculate contact plane
+				normal = normal_length > 0.0f? normal / normal_length : max_plane_normal;
+				Plane plane = Plane::sFromPointAndNormal(closest_point, normal);
+
+				// Store collision
+				v.SetCollision(plane.GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex);
+			}
+		}
+}
+
+class ConvexHullShape::CHSGetTrianglesContext
+{
+public:
+				CHSGetTrianglesContext(Mat44Arg inTransform, bool inIsInsideOut) : mTransform(inTransform), mIsInsideOut(inIsInsideOut) { }
+
+	Mat44		mTransform;
+	bool		mIsInsideOut;
+	size_t		mCurrentFace = 0;
+};
+
+void ConvexHullShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const
+{
+	static_assert(sizeof(CHSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small");
+	JPH_ASSERT(IsAligned(&ioContext, alignof(CHSGetTrianglesContext)));
+
+	new (&ioContext) CHSGetTrianglesContext(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale), ScaleHelpers::IsInsideOut(inScale));
+}
+
+int ConvexHullShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const
+{
+	static_assert(cGetTrianglesMinTrianglesRequested >= 12, "cGetTrianglesMinTrianglesRequested is too small");
+	JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested);
+
+	CHSGetTrianglesContext &context = (CHSGetTrianglesContext &)ioContext;
+
+	int total_num_triangles = 0;
+	for (; context.mCurrentFace < mFaces.size(); ++context.mCurrentFace)
+	{
+		const Face &f = mFaces[context.mCurrentFace];
+
+		const uint8 *first_vtx = mVertexIdx.data() + f.mFirstVertex;
+		const uint8 *end_vtx = first_vtx + f.mNumVertices;
+
+		// Check if there is still room in the output buffer for this face
+		int num_triangles = f.mNumVertices - 2;
+		inMaxTrianglesRequested -= num_triangles;
+		if (inMaxTrianglesRequested < 0)
+			break;
+		total_num_triangles += num_triangles;
+
+		// Get first triangle of polygon
+		Vec3 v0 = context.mTransform * mPoints[first_vtx[0]].mPosition;
+		Vec3 v1 = context.mTransform * mPoints[first_vtx[1]].mPosition;
+		Vec3 v2 = context.mTransform * mPoints[first_vtx[2]].mPosition;
+		v0.StoreFloat3(outTriangleVertices++);
+		if (context.mIsInsideOut)
+		{
+			// Store first triangle in this polygon flipped
+			v2.StoreFloat3(outTriangleVertices++);
+			v1.StoreFloat3(outTriangleVertices++);
+
+			// Store other triangles in this polygon flipped
+			for (const uint8 *v = first_vtx + 3; v < end_vtx; ++v)
+			{
+				v0.StoreFloat3(outTriangleVertices++);
+				(context.mTransform * mPoints[*v].mPosition).StoreFloat3(outTriangleVertices++);
+				(context.mTransform * mPoints[*(v - 1)].mPosition).StoreFloat3(outTriangleVertices++);
+			}
+		}
+		else
+		{
+			// Store first triangle in this polygon
+			v1.StoreFloat3(outTriangleVertices++);
+			v2.StoreFloat3(outTriangleVertices++);
+
+			// Store other triangles in this polygon
+			for (const uint8 *v = first_vtx + 3; v < end_vtx; ++v)
+			{
+				v0.StoreFloat3(outTriangleVertices++);
+				(context.mTransform * mPoints[*(v - 1)].mPosition).StoreFloat3(outTriangleVertices++);
+				(context.mTransform * mPoints[*v].mPosition).StoreFloat3(outTriangleVertices++);
+			}
+		}
+	}
+
+	// Store materials
+	if (outMaterials != nullptr)
+	{
+		const PhysicsMaterial *material = GetMaterial();
+		for (const PhysicsMaterial **m = outMaterials, **m_end = outMaterials + total_num_triangles; m < m_end; ++m)
+			*m = material;
+	}
+
+	return total_num_triangles;
+}
+
+void ConvexHullShape::SaveBinaryState(StreamOut &inStream) const
+{
+	ConvexShape::SaveBinaryState(inStream);
+
+	inStream.Write(mCenterOfMass);
+	inStream.Write(mInertia);
+	inStream.Write(mLocalBounds.mMin);
+	inStream.Write(mLocalBounds.mMax);
+	inStream.Write(mPoints);
+	inStream.Write(mFaces);
+	inStream.Write(mPlanes);
+	inStream.Write(mVertexIdx);
+	inStream.Write(mConvexRadius);
+	inStream.Write(mVolume);
+	inStream.Write(mInnerRadius);
+}
+
+void ConvexHullShape::RestoreBinaryState(StreamIn &inStream)
+{
+	ConvexShape::RestoreBinaryState(inStream);
+
+	inStream.Read(mCenterOfMass);
+	inStream.Read(mInertia);
+	inStream.Read(mLocalBounds.mMin);
+	inStream.Read(mLocalBounds.mMax);
+	inStream.Read(mPoints);
+	inStream.Read(mFaces);
+	inStream.Read(mPlanes);
+	inStream.Read(mVertexIdx);
+	inStream.Read(mConvexRadius);
+	inStream.Read(mVolume);
+	inStream.Read(mInnerRadius);
+}
+
+Shape::Stats ConvexHullShape::GetStats() const
+{
+	// Count number of triangles
+	uint triangle_count = 0;
+	for (const Face &f : mFaces)
+		triangle_count += f.mNumVertices - 2;
+
+	return Stats(
+		sizeof(*this)
+			+ mPoints.size() * sizeof(Point)
+			+ mFaces.size() * sizeof(Face)
+			+ mPlanes.size() * sizeof(Plane)
+			+ mVertexIdx.size() * sizeof(uint8),
+		triangle_count);
+}
+
+void ConvexHullShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::ConvexHull);
+	f.mConstruct = []() -> Shape * { return new ConvexHullShape; };
+	f.mColor = Color::sGreen;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.h
new file mode 100644
index 0000000000..4068791f43
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexHullShape.h
@@ -0,0 +1,202 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/ConvexShape.h>
+#include <Jolt/Physics/PhysicsSettings.h>
+#include <Jolt/Geometry/Plane.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that constructs a ConvexHullShape
+class JPH_EXPORT ConvexHullShapeSettings final : public ConvexShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, ConvexHullShapeSettings)
+
+public:
+	/// Default constructor for deserialization
+							ConvexHullShapeSettings() = default;
+
+	/// Create a convex hull from inPoints and maximum convex radius inMaxConvexRadius, the radius is automatically lowered if the hull requires it.
+	/// (internally this will be subtracted so the total size will not grow with the convex radius).
+							ConvexHullShapeSettings(const Vec3 *inPoints, int inNumPoints, float inMaxConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mPoints(inPoints, inPoints + inNumPoints), mMaxConvexRadius(inMaxConvexRadius) { }
+							ConvexHullShapeSettings(const Array<Vec3> &inPoints, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mPoints(inPoints), mMaxConvexRadius(inConvexRadius) { }
+
+	// See: ShapeSettings
+	virtual ShapeResult		Create() const override;
+
+	Array<Vec3>				mPoints;															///< Points to create the hull from
+	float					mMaxConvexRadius = 0.0f;											///< Convex radius as supplied by the constructor. Note that during hull creation the convex radius can be made smaller if the value is too big for the hull.
+	float					mMaxErrorConvexRadius = 0.05f;										///< Maximum distance between the shrunk hull + convex radius and the actual hull.
+	float					mHullTolerance = 1.0e-3f;											///< Points are allowed this far outside of the hull (increasing this yields a hull with less vertices). Note that the actual used value can be larger if the points of the hull are far apart.
+};
+
+/// A convex hull
+class JPH_EXPORT ConvexHullShape final : public ConvexShape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Maximum amount of points supported in a convex hull. Note that while constructing a hull, interior points are discarded so you can provide more points.
+	/// The ConvexHullShapeSettings::Create function will return an error when too many points are provided.
+	static constexpr int	cMaxPointsInHull = 256;
+
+	/// Constructor
+							ConvexHullShape() : ConvexShape(EShapeSubType::ConvexHull) { }
+							ConvexHullShape(const ConvexHullShapeSettings &inSettings, ShapeResult &outResult);
+
+	// See Shape::GetCenterOfMass
+	virtual Vec3			GetCenterOfMass() const override									{ return mCenterOfMass; }
+
+	// See Shape::GetLocalBounds
+	virtual AABox			GetLocalBounds() const override										{ return mLocalBounds; }
+
+	// See Shape::GetInnerRadius
+	virtual float			GetInnerRadius() const override										{ return mInnerRadius; }
+
+	// See Shape::GetMassProperties
+	virtual MassProperties	GetMassProperties() const override;
+
+	// See Shape::GetSurfaceNormal
+	virtual Vec3			GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override;
+
+	// See Shape::GetSupportingFace
+	virtual void			GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
+
+	// See ConvexShape::GetSupportFunction
+	virtual const Support *	GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override;
+
+	// See Shape::GetSubmergedVolume
+	virtual void			GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override;
+
+#ifdef JPH_DEBUG_RENDERER
+	// See Shape::Draw
+	virtual void			Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+
+	/// Debugging helper draw function that draws how all points are moved when a shape is shrunk by the convex radius
+	void					DrawShrunkShape(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const;
+#endif // JPH_DEBUG_RENDERER
+
+	// See Shape::CastRay
+	virtual bool			CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+	virtual void			CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollidePoint
+	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollideSoftBodyVertices
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
+
+	// See Shape::GetTrianglesStart
+	virtual void			GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
+
+	// See Shape::GetTrianglesNext
+	virtual int				GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override;
+
+	// See Shape
+	virtual void			SaveBinaryState(StreamOut &inStream) const override;
+
+	// See Shape::GetStats
+	virtual Stats			GetStats() const override;
+
+	// See Shape::GetVolume
+	virtual float			GetVolume() const override											{ return mVolume; }
+
+	/// Get the convex radius of this convex hull
+	float					GetConvexRadius() const												{ return mConvexRadius; }
+
+	/// Get the planes of this convex hull
+	const Array<Plane> &	GetPlanes() const													{ return mPlanes; }
+
+	/// Get the number of vertices in this convex hull
+	inline uint				GetNumPoints() const												{ return uint(mPoints.size()); }
+
+	/// Get a vertex of this convex hull relative to the center of mass
+	inline Vec3				GetPoint(uint inIndex) const										{ return mPoints[inIndex].mPosition; }
+
+	/// Get the number of faces in this convex hull
+	inline uint				GetNumFaces() const													{ return uint(mFaces.size()); }
+
+	/// Get the number of vertices in a face
+	inline uint				GetNumVerticesInFace(uint inFaceIndex) const						{ return mFaces[inFaceIndex].mNumVertices; }
+
+	/// Get the vertices indices of a face
+	/// @param inFaceIndex Index of the face.
+	/// @param inMaxVertices Maximum number of vertices to return.
+	/// @param outVertices Array of vertices indices, must be at least inMaxVertices in size, the vertices are returned in counter clockwise order and the positions can be obtained using GetPoint(index).
+	/// @return Number of vertices in face, if this is bigger than inMaxVertices, not all vertices were retrieved.
+	inline uint				GetFaceVertices(uint inFaceIndex, uint inMaxVertices, uint *outVertices) const
+	{
+		const Face &face = mFaces[inFaceIndex];
+		const uint8 *first_vertex = mVertexIdx.data() + face.mFirstVertex;
+		uint num_vertices = min<uint>(face.mNumVertices, inMaxVertices);
+		for (uint i = 0; i < num_vertices; ++i)
+			outVertices[i] = first_vertex[i];
+		return face.mNumVertices;
+	}
+
+	// Register shape functions with the registry
+	static void				sRegister();
+
+#ifdef JPH_DEBUG_RENDERER
+	/// Draw the outlines of the faces of the convex hull when drawing the shape
+	inline static bool		sDrawFaceOutlines = false;
+#endif // JPH_DEBUG_RENDERER
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void			RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	/// Helper function that returns the min and max fraction along the ray that hits the convex hull. Returns false if there is no hit.
+	bool					CastRayHelper(const RayCast &inRay, float &outMinFraction, float &outMaxFraction) const;
+
+	/// Class for GetTrianglesStart/Next
+	class					CHSGetTrianglesContext;
+
+	/// Classes for GetSupportFunction
+	class					HullNoConvex;
+	class					HullWithConvex;
+	class					HullWithConvexScaled;
+
+	struct Face
+	{
+		uint16				mFirstVertex;				///< First index in mVertexIdx to use
+		uint16				mNumVertices = 0;			///< Number of vertices in the mVertexIdx to use
+	};
+
+	static_assert(sizeof(Face) == 4, "Unexpected size");
+	static_assert(alignof(Face) == 2, "Unexpected alignment");
+
+	struct Point
+	{
+		Vec3				mPosition;					///< Position of vertex
+		int					mNumFaces = 0;				///< Number of faces in the face array below
+		int					mFaces[3] = { -1, -1, -1 };	///< Indices of 3 neighboring faces with the biggest difference in normal (used to shift vertices for convex radius)
+	};
+
+	static_assert(sizeof(Point) == 32, "Unexpected size");
+	static_assert(alignof(Point) == JPH_VECTOR_ALIGNMENT, "Unexpected alignment");
+
+	Vec3					mCenterOfMass;				///< Center of mass of this convex hull
+	Mat44					mInertia;					///< Inertia matrix assuming density is 1 (needs to be multiplied by density)
+	AABox					mLocalBounds;				///< Local bounding box for the convex hull
+	Array<Point>			mPoints;					///< Points on the convex hull surface
+	Array<Face>				mFaces;						///< Faces of the convex hull surface
+	Array<Plane>			mPlanes;					///< Planes for the faces (1-on-1 with mFaces array, separate because they need to be 16 byte aligned)
+	Array<uint8>			mVertexIdx;					///< A list of vertex indices (indexing in mPoints) for each of the faces
+	float					mConvexRadius = 0.0f;		///< Convex radius
+	float					mVolume;					///< Total volume of the convex hull
+	float					mInnerRadius = FLT_MAX;		///< Radius of the biggest sphere that fits entirely in the convex hull
+
+#ifdef JPH_DEBUG_RENDERER
+	mutable DebugRenderer::GeometryRef mGeometry;
+#endif // JPH_DEBUG_RENDERER
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexShape.cpp
new file mode 100644
index 0000000000..35e23ee01c
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexShape.cpp
@@ -0,0 +1,559 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/Shape/ConvexShape.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/CollideShape.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/Shape/GetTrianglesContext.h>
+#include <Jolt/Physics/Collision/Shape/PolyhedronSubmergedVolumeCalculator.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Physics/Collision/NarrowPhaseStats.h>
+#include <Jolt/Physics/PhysicsSettings.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/Geometry/EPAPenetrationDepth.h>
+#include <Jolt/Geometry/OrientedBox.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(ConvexShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(ConvexShapeSettings, ShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(ConvexShapeSettings, mDensity)
+	JPH_ADD_ATTRIBUTE(ConvexShapeSettings, mMaterial)
+}
+
+const StaticArray<Vec3, 384> ConvexShape::sUnitSphereTriangles = []() {
+	const int level = 2;
+
+	StaticArray<Vec3, 384> verts;
+	GetTrianglesContextVertexList::sCreateHalfUnitSphereTop(verts, level);
+	GetTrianglesContextVertexList::sCreateHalfUnitSphereBottom(verts, level);
+	return verts;
+}();
+
+void ConvexShape::sCollideConvexVsConvex(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Get the shapes
+	JPH_ASSERT(inShape1->GetType() == EShapeType::Convex);
+	JPH_ASSERT(inShape2->GetType() == EShapeType::Convex);
+	const ConvexShape *shape1 = static_cast<const ConvexShape *>(inShape1);
+	const ConvexShape *shape2 = static_cast<const ConvexShape *>(inShape2);
+
+	// Get transforms
+	Mat44 inverse_transform1 = inCenterOfMassTransform1.InversedRotationTranslation();
+	Mat44 transform_2_to_1 = inverse_transform1 * inCenterOfMassTransform2;
+
+	// Get bounding boxes
+	AABox shape1_bbox = shape1->GetLocalBounds().Scaled(inScale1);
+	shape1_bbox.ExpandBy(Vec3::sReplicate(inCollideShapeSettings.mMaxSeparationDistance));
+	AABox shape2_bbox = shape2->GetLocalBounds().Scaled(inScale2);
+
+	// Check if they overlap
+	if (!OrientedBox(transform_2_to_1, shape2_bbox).Overlaps(shape1_bbox))
+		return;
+
+	// Note: As we don't remember the penetration axis from the last iteration, and it is likely that shape2 is pushed out of
+	// collision relative to shape1 by comparing their COM's, we use that as an initial penetration axis: shape2.com - shape1.com
+	// This has been seen to improve performance by approx. 1% over using a fixed axis like (1, 0, 0).
+	Vec3 penetration_axis = transform_2_to_1.GetTranslation();
+
+	// Ensure that we do not pass in a near zero penetration axis
+	if (penetration_axis.IsNearZero())
+		penetration_axis = Vec3::sAxisX();
+
+	Vec3 point1, point2;
+	EPAPenetrationDepth pen_depth;
+	EPAPenetrationDepth::EStatus status;
+
+	// Scope to limit lifetime of SupportBuffer
+	{
+		// Create support function
+		SupportBuffer buffer1_excl_cvx_radius, buffer2_excl_cvx_radius;
+		const Support *shape1_excl_cvx_radius = shape1->GetSupportFunction(ConvexShape::ESupportMode::ExcludeConvexRadius, buffer1_excl_cvx_radius, inScale1);
+		const Support *shape2_excl_cvx_radius = shape2->GetSupportFunction(ConvexShape::ESupportMode::ExcludeConvexRadius, buffer2_excl_cvx_radius, inScale2);
+
+		// Transform shape 2 in the space of shape 1
+		TransformedConvexObject<Support> transformed2_excl_cvx_radius(transform_2_to_1, *shape2_excl_cvx_radius);
+
+		// Perform GJK step
+		status = pen_depth.GetPenetrationDepthStepGJK(*shape1_excl_cvx_radius, shape1_excl_cvx_radius->GetConvexRadius() + inCollideShapeSettings.mMaxSeparationDistance, transformed2_excl_cvx_radius, shape2_excl_cvx_radius->GetConvexRadius(), inCollideShapeSettings.mCollisionTolerance, penetration_axis, point1, point2);
+	}
+
+	// Check result of collision detection
+	switch (status)
+	{
+	case EPAPenetrationDepth::EStatus::Colliding:
+		break;
+
+	case EPAPenetrationDepth::EStatus::NotColliding:
+		return;
+
+	case EPAPenetrationDepth::EStatus::Indeterminate:
+		{
+			// Need to run expensive EPA algorithm
+
+			// Create support function
+			SupportBuffer buffer1_incl_cvx_radius, buffer2_incl_cvx_radius;
+			const Support *shape1_incl_cvx_radius = shape1->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer1_incl_cvx_radius, inScale1);
+			const Support *shape2_incl_cvx_radius = shape2->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer2_incl_cvx_radius, inScale2);
+
+			// Add separation distance
+			AddConvexRadius<Support> shape1_add_max_separation_distance(*shape1_incl_cvx_radius, inCollideShapeSettings.mMaxSeparationDistance);
+
+			// Transform shape 2 in the space of shape 1
+			TransformedConvexObject<Support> transformed2_incl_cvx_radius(transform_2_to_1, *shape2_incl_cvx_radius);
+
+			// Perform EPA step
+			if (!pen_depth.GetPenetrationDepthStepEPA(shape1_add_max_separation_distance, transformed2_incl_cvx_radius, inCollideShapeSettings.mPenetrationTolerance, penetration_axis, point1, point2))
+				return;
+			break;
+		}
+	}
+
+	// Check if the penetration is bigger than the early out fraction
+	float penetration_depth = (point2 - point1).Length() - inCollideShapeSettings.mMaxSeparationDistance;
+	if (-penetration_depth >= ioCollector.GetEarlyOutFraction())
+		return;
+
+	// Correct point1 for the added separation distance
+	float penetration_axis_len = penetration_axis.Length();
+	if (penetration_axis_len > 0.0f)
+		point1 -= penetration_axis * (inCollideShapeSettings.mMaxSeparationDistance / penetration_axis_len);
+
+	// Convert to world space
+	point1 = inCenterOfMassTransform1 * point1;
+	point2 = inCenterOfMassTransform1 * point2;
+	Vec3 penetration_axis_world = inCenterOfMassTransform1.Multiply3x3(penetration_axis);
+
+	// Create collision result
+	CollideShapeResult result(point1, point2, penetration_axis_world, penetration_depth, inSubShapeIDCreator1.GetID(), inSubShapeIDCreator2.GetID(), TransformedShape::sGetBodyID(ioCollector.GetContext()));
+
+	// Gather faces
+	if (inCollideShapeSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces)
+	{
+		// Get supporting face of shape 1
+		shape1->GetSupportingFace(SubShapeID(), -penetration_axis, inScale1, inCenterOfMassTransform1, result.mShape1Face);
+
+		// Get supporting face of shape 2
+		shape2->GetSupportingFace(SubShapeID(), transform_2_to_1.Multiply3x3Transposed(penetration_axis), inScale2, inCenterOfMassTransform2, result.mShape2Face);
+	}
+
+	// Notify the collector
+	JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;)
+	ioCollector.AddHit(result);
+}
+
+bool ConvexShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	// Note: This is a fallback routine, most convex shapes should implement a more performant version!
+
+	JPH_PROFILE_FUNCTION();
+
+	// Create support function
+	SupportBuffer buffer;
+	const Support *support = GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sReplicate(1.0f));
+
+	// Cast ray
+	GJKClosestPoint gjk;
+	if (gjk.CastRay(inRay.mOrigin, inRay.mDirection, cDefaultCollisionTolerance, *support, ioHit.mFraction))
+	{
+		ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID();
+		return true;
+	}
+
+	return false;
+}
+
+void ConvexShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Note: This is a fallback routine, most convex shapes should implement a more performant version!
+
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	// First do a normal raycast, limited to the early out fraction
+	RayCastResult hit;
+	hit.mFraction = ioCollector.GetEarlyOutFraction();
+	if (CastRay(inRay, inSubShapeIDCreator, hit))
+	{
+		// Check front side
+		if (inRayCastSettings.mTreatConvexAsSolid || hit.mFraction > 0.0f)
+		{
+			hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext());
+			ioCollector.AddHit(hit);
+		}
+
+		// Check if we want back facing hits and the collector still accepts additional hits
+		if (inRayCastSettings.mBackFaceModeConvex == EBackFaceMode::CollideWithBackFaces && !ioCollector.ShouldEarlyOut())
+		{
+			// Invert the ray, going from the early out fraction back to the fraction where we found our forward hit
+			float start_fraction = min(1.0f, ioCollector.GetEarlyOutFraction());
+			float delta_fraction = hit.mFraction - start_fraction;
+			if (delta_fraction < 0.0f)
+			{
+				RayCast inverted_ray { inRay.mOrigin + start_fraction * inRay.mDirection, delta_fraction * inRay.mDirection };
+
+				// Cast another ray
+				RayCastResult inverted_hit;
+				inverted_hit.mFraction = 1.0f;
+				if (CastRay(inverted_ray, inSubShapeIDCreator, inverted_hit)
+					&& inverted_hit.mFraction > 0.0f) // Ignore hits with fraction 0, this means the ray ends inside the object and we don't want to report it as a back facing hit
+				{
+					// Invert fraction and rescale it to the fraction of the original ray
+					inverted_hit.mFraction = hit.mFraction + (inverted_hit.mFraction - 1.0f) * delta_fraction;
+					inverted_hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext());
+					ioCollector.AddHit(inverted_hit);
+				}
+			}
+		}
+	}
+}
+
+void ConvexShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	// First test bounding box
+	if (GetLocalBounds().Contains(inPoint))
+	{
+		// Create support function
+		SupportBuffer buffer;
+		const Support *support = GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sReplicate(1.0f));
+
+		// Create support function for point
+		PointConvexSupport point { inPoint };
+
+		// Test intersection
+		GJKClosestPoint gjk;
+		Vec3 v = inPoint;
+		if (gjk.Intersects(*support, point, cDefaultCollisionTolerance, v))
+			ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
+	}
+}
+
+void ConvexShape::sCastConvexVsConvex(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Only supported for convex shapes
+	JPH_ASSERT(inShapeCast.mShape->GetType() == EShapeType::Convex);
+	const ConvexShape *cast_shape = static_cast<const ConvexShape *>(inShapeCast.mShape);
+
+	JPH_ASSERT(inShape->GetType() == EShapeType::Convex);
+	const ConvexShape *shape = static_cast<const ConvexShape *>(inShape);
+
+	// Determine if we want to use the actual shape or a shrunken shape with convex radius
+	ConvexShape::ESupportMode support_mode = inShapeCastSettings.mUseShrunkenShapeAndConvexRadius? ConvexShape::ESupportMode::ExcludeConvexRadius : ConvexShape::ESupportMode::Default;
+
+	// Create support function for shape to cast
+	SupportBuffer cast_buffer;
+	const Support *cast_support = cast_shape->GetSupportFunction(support_mode, cast_buffer, inShapeCast.mScale);
+
+	// Create support function for target shape
+	SupportBuffer target_buffer;
+	const Support *target_support = shape->GetSupportFunction(support_mode, target_buffer, inScale);
+
+	// Do a raycast against the result
+	EPAPenetrationDepth epa;
+	float fraction = ioCollector.GetEarlyOutFraction();
+	Vec3 contact_point_a, contact_point_b, contact_normal;
+	if (epa.CastShape(inShapeCast.mCenterOfMassStart, inShapeCast.mDirection, inShapeCastSettings.mCollisionTolerance, inShapeCastSettings.mPenetrationTolerance, *cast_support, *target_support, cast_support->GetConvexRadius(), target_support->GetConvexRadius(), inShapeCastSettings.mReturnDeepestPoint, fraction, contact_point_a, contact_point_b, contact_normal)
+		&& (inShapeCastSettings.mBackFaceModeConvex == EBackFaceMode::CollideWithBackFaces
+			|| contact_normal.Dot(inShapeCast.mDirection) > 0.0f)) // Test if backfacing
+	{
+		// Convert to world space
+		contact_point_a = inCenterOfMassTransform2 * contact_point_a;
+		contact_point_b = inCenterOfMassTransform2 * contact_point_b;
+		Vec3 contact_normal_world = inCenterOfMassTransform2.Multiply3x3(contact_normal);
+
+		ShapeCastResult result(fraction, contact_point_a, contact_point_b, contact_normal_world, false, inSubShapeIDCreator1.GetID(), inSubShapeIDCreator2.GetID(), TransformedShape::sGetBodyID(ioCollector.GetContext()));
+
+		// Early out if this hit is deeper than the collector's early out value
+		if (fraction == 0.0f && -result.mPenetrationDepth >= ioCollector.GetEarlyOutFraction())
+			return;
+
+		// Gather faces
+		if (inShapeCastSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces)
+		{
+			// Get supporting face of shape 1
+			Mat44 transform_1_to_2 = inShapeCast.mCenterOfMassStart;
+			transform_1_to_2.SetTranslation(transform_1_to_2.GetTranslation() + fraction * inShapeCast.mDirection);
+			cast_shape->GetSupportingFace(SubShapeID(), transform_1_to_2.Multiply3x3Transposed(-contact_normal), inShapeCast.mScale, inCenterOfMassTransform2 * transform_1_to_2, result.mShape1Face);
+
+			// Get supporting face of shape 2
+			shape->GetSupportingFace(SubShapeID(), contact_normal, inScale, inCenterOfMassTransform2, result.mShape2Face);
+		}
+
+		JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;)
+		ioCollector.AddHit(result);
+	}
+}
+
+class ConvexShape::CSGetTrianglesContext
+{
+public:
+				CSGetTrianglesContext(const ConvexShape *inShape, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) :
+		mLocalToWorld(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale)),
+		mIsInsideOut(ScaleHelpers::IsInsideOut(inScale))
+	{
+		mSupport = inShape->GetSupportFunction(ESupportMode::IncludeConvexRadius, mSupportBuffer, Vec3::sReplicate(1.0f));
+	}
+
+	SupportBuffer		mSupportBuffer;
+	const Support *		mSupport;
+	Mat44				mLocalToWorld;
+	bool				mIsInsideOut;
+	size_t				mCurrentVertex = 0;
+};
+
+void ConvexShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const
+{
+	static_assert(sizeof(CSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small");
+	JPH_ASSERT(IsAligned(&ioContext, alignof(CSGetTrianglesContext)));
+
+	new (&ioContext) CSGetTrianglesContext(this, inPositionCOM, inRotation, inScale);
+}
+
+int ConvexShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const
+{
+	JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested);
+
+	CSGetTrianglesContext &context = (CSGetTrianglesContext &)ioContext;
+
+	int total_num_vertices = min(inMaxTrianglesRequested * 3, int(sUnitSphereTriangles.size() - context.mCurrentVertex));
+
+	if (context.mIsInsideOut)
+	{
+		// Store triangles flipped
+		for (const Vec3 *v = sUnitSphereTriangles.data() + context.mCurrentVertex, *v_end = v + total_num_vertices; v < v_end; v += 3)
+		{
+			(context.mLocalToWorld * context.mSupport->GetSupport(v[0])).StoreFloat3(outTriangleVertices++);
+			(context.mLocalToWorld * context.mSupport->GetSupport(v[2])).StoreFloat3(outTriangleVertices++);
+			(context.mLocalToWorld * context.mSupport->GetSupport(v[1])).StoreFloat3(outTriangleVertices++);
+		}
+	}
+	else
+	{
+		// Store triangles
+		for (const Vec3 *v = sUnitSphereTriangles.data() + context.mCurrentVertex, *v_end = v + total_num_vertices; v < v_end; v += 3)
+		{
+			(context.mLocalToWorld * context.mSupport->GetSupport(v[0])).StoreFloat3(outTriangleVertices++);
+			(context.mLocalToWorld * context.mSupport->GetSupport(v[1])).StoreFloat3(outTriangleVertices++);
+			(context.mLocalToWorld * context.mSupport->GetSupport(v[2])).StoreFloat3(outTriangleVertices++);
+		}
+	}
+
+	context.mCurrentVertex += total_num_vertices;
+	int total_num_triangles = total_num_vertices / 3;
+
+	// Store materials
+	if (outMaterials != nullptr)
+	{
+		const PhysicsMaterial *material = GetMaterial();
+		for (const PhysicsMaterial **m = outMaterials, **m_end = outMaterials + total_num_triangles; m < m_end; ++m)
+			*m = material;
+	}
+
+	return total_num_triangles;
+}
+
+void ConvexShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const
+{
+	// Calculate total volume
+	Vec3 abs_scale = inScale.Abs();
+	Vec3 extent = GetLocalBounds().GetExtent() * abs_scale;
+	outTotalVolume = 8.0f * extent.GetX() * extent.GetY() * extent.GetZ();
+
+	// Points of the bounding box
+	Vec3 points[] =
+	{
+		Vec3(-1, -1, -1),
+		Vec3( 1, -1, -1),
+		Vec3(-1,  1, -1),
+		Vec3( 1,  1, -1),
+		Vec3(-1, -1,  1),
+		Vec3( 1, -1,  1),
+		Vec3(-1,  1,  1),
+		Vec3( 1,  1,  1),
+	};
+
+	// Faces of the bounding box
+	using Face = int[5];
+	#define MAKE_FACE(a, b, c, d) { a, b, c, d, ((1 << a) | (1 << b) | (1 << c) | (1 << d)) } // Last int is a bit mask that indicates which indices are used
+	Face faces[] =
+	{
+		MAKE_FACE(0, 2, 3, 1),
+		MAKE_FACE(4, 6, 2, 0),
+		MAKE_FACE(4, 5, 7, 6),
+		MAKE_FACE(1, 3, 7, 5),
+		MAKE_FACE(2, 6, 7, 3),
+		MAKE_FACE(0, 1, 5, 4),
+	};
+
+	PolyhedronSubmergedVolumeCalculator::Point *buffer = (PolyhedronSubmergedVolumeCalculator::Point *)JPH_STACK_ALLOC(8 * sizeof(PolyhedronSubmergedVolumeCalculator::Point));
+	PolyhedronSubmergedVolumeCalculator submerged_vol_calc(inCenterOfMassTransform * Mat44::sScale(extent), points, sizeof(Vec3), 8, inSurface, buffer JPH_IF_DEBUG_RENDERER(, inBaseOffset));
+
+	if (submerged_vol_calc.AreAllAbove())
+	{
+		// We're above the water
+		outSubmergedVolume = 0.0f;
+		outCenterOfBuoyancy = Vec3::sZero();
+	}
+	else if (submerged_vol_calc.AreAllBelow())
+	{
+		// We're fully submerged
+		outSubmergedVolume = outTotalVolume;
+		outCenterOfBuoyancy = inCenterOfMassTransform.GetTranslation();
+	}
+	else
+	{
+		// Calculate submerged volume
+		int reference_point_bit = 1 << submerged_vol_calc.GetReferencePointIdx();
+		for (const Face &f : faces)
+		{
+			// Test if this face includes the reference point
+			if ((f[4] & reference_point_bit) == 0)
+			{
+				// Triangulate the face (a quad)
+				submerged_vol_calc.AddFace(f[0], f[1], f[2]);
+				submerged_vol_calc.AddFace(f[0], f[2], f[3]);
+			}
+		}
+
+		submerged_vol_calc.GetResult(outSubmergedVolume, outCenterOfBuoyancy);
+	}
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void ConvexShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const
+{
+	// Get the support function with convex radius
+	SupportBuffer buffer;
+	const Support *support = GetSupportFunction(ESupportMode::ExcludeConvexRadius, buffer, inScale);
+	AddConvexRadius<Support> add_convex(*support, support->GetConvexRadius());
+
+	// Draw the shape
+	DebugRenderer::GeometryRef geometry = inRenderer->CreateTriangleGeometryForConvex([&add_convex](Vec3Arg inDirection) { return add_convex.GetSupport(inDirection); });
+	AABox bounds = geometry->mBounds.Transformed(inCenterOfMassTransform);
+	float lod_scale_sq = geometry->mBounds.GetExtent().LengthSq();
+	inRenderer->DrawGeometry(inCenterOfMassTransform, bounds, lod_scale_sq, inColor, geometry);
+
+	if (inDrawSupportDirection)
+	{
+		// Iterate on all directions and draw the support point and an arrow in the direction that was sampled to test if the support points make sense
+		for (Vec3 v : Vec3::sUnitSphere)
+		{
+			Vec3 direction = 0.05f * v;
+			Vec3 pos = add_convex.GetSupport(direction);
+			RVec3 from = inCenterOfMassTransform * pos;
+			RVec3 to = inCenterOfMassTransform * (pos + direction);
+			inRenderer->DrawMarker(from, Color::sWhite, 0.001f);
+			inRenderer->DrawArrow(from, to, Color::sWhite, 0.001f);
+		}
+	}
+}
+
+void ConvexShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
+	// Sample directions and map which faces belong to which directions
+	using FaceToDirection = UnorderedMap<SupportingFace, Array<Vec3>>;
+	FaceToDirection faces;
+	for (Vec3 v : Vec3::sUnitSphere)
+	{
+		Vec3 direction = 0.05f * v;
+
+		SupportingFace face;
+		GetSupportingFace(SubShapeID(), direction, inScale, Mat44::sIdentity(), face);
+
+		if (!face.empty())
+		{
+			JPH_ASSERT(face.size() >= 2, "The GetSupportingFace function should either return nothing or at least an edge");
+			faces[face].push_back(direction);
+		}
+	}
+
+	// Draw each face in a unique color and draw corresponding directions
+	int color_it = 0;
+	for (FaceToDirection::value_type &ftd : faces)
+	{
+		Color color = Color::sGetDistinctColor(color_it++);
+
+		// Create copy of face (key in map is read only)
+		SupportingFace face = ftd.first;
+
+		// Displace the face a little bit forward so it is easier to see
+		Vec3 normal = face.size() >= 3? (face[2] - face[1]).Cross(face[0] - face[1]).Normalized() : Vec3::sZero();
+		Vec3 displacement = 0.001f * normal;
+
+		// Transform face to world space and calculate center of mass
+		Vec3 com_ls = Vec3::sZero();
+		for (Vec3 &v : face)
+		{
+			v = inCenterOfMassTransform.Multiply3x3(v + displacement);
+			com_ls += v;
+		}
+		RVec3 com = inCenterOfMassTransform.GetTranslation() + com_ls / (float)face.size();
+
+		// Draw the polygon and directions
+		inRenderer->DrawWirePolygon(RMat44::sTranslation(inCenterOfMassTransform.GetTranslation()), face, color, face.size() >= 3? 0.001f : 0.0f);
+		if (face.size() >= 3)
+			inRenderer->DrawArrow(com, com + inCenterOfMassTransform.Multiply3x3(normal), color, 0.01f);
+		for (Vec3 &v : ftd.second)
+			inRenderer->DrawArrow(com, com + inCenterOfMassTransform.Multiply3x3(-v), color, 0.001f);
+	}
+}
+#endif // JPH_DEBUG_RENDERER
+
+void ConvexShape::SaveBinaryState(StreamOut &inStream) const
+{
+	Shape::SaveBinaryState(inStream);
+
+	inStream.Write(mDensity);
+}
+
+void ConvexShape::RestoreBinaryState(StreamIn &inStream)
+{
+	Shape::RestoreBinaryState(inStream);
+
+	inStream.Read(mDensity);
+}
+
+void ConvexShape::SaveMaterialState(PhysicsMaterialList &outMaterials) const
+{
+	outMaterials.clear();
+	outMaterials.push_back(mMaterial);
+}
+
+void ConvexShape::RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials)
+{
+	JPH_ASSERT(inNumMaterials == 1);
+	mMaterial = inMaterials[0];
+}
+
+void ConvexShape::sRegister()
+{
+	for (EShapeSubType s1 : sConvexSubShapeTypes)
+		for (EShapeSubType s2 : sConvexSubShapeTypes)
+		{
+			CollisionDispatch::sRegisterCollideShape(s1, s2, sCollideConvexVsConvex);
+			CollisionDispatch::sRegisterCastShape(s1, s2, sCastConvexVsConvex);
+		}
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexShape.h
new file mode 100644
index 0000000000..a4eab87553
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ConvexShape.h
@@ -0,0 +1,150 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/StaticArray.h>
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+#include <Jolt/Physics/Collision/PhysicsMaterial.h>
+
+JPH_NAMESPACE_BEGIN
+
+class CollideShapeSettings;
+
+/// Class that constructs a ConvexShape (abstract)
+class JPH_EXPORT ConvexShapeSettings : public ShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, ConvexShapeSettings)
+
+public:
+	/// Constructor
+									ConvexShapeSettings() = default;
+	explicit						ConvexShapeSettings(const PhysicsMaterial *inMaterial)		: mMaterial(inMaterial) { }
+
+	/// Set the density of the object in kg / m^3
+	void							SetDensity(float inDensity)									{ mDensity = inDensity; }
+
+	// Properties
+	RefConst<PhysicsMaterial>		mMaterial;													///< Material assigned to this shape
+	float							mDensity = 1000.0f;											///< Uniform density of the interior of the convex object (kg / m^3)
+};
+
+/// Base class for all convex shapes. Defines a virtual interface.
+class JPH_EXPORT ConvexShape : public Shape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+	explicit						ConvexShape(EShapeSubType inSubType) : Shape(EShapeType::Convex, inSubType) { }
+									ConvexShape(EShapeSubType inSubType, const ConvexShapeSettings &inSettings, ShapeResult &outResult) : Shape(EShapeType::Convex, inSubType, inSettings, outResult), mMaterial(inSettings.mMaterial), mDensity(inSettings.mDensity) { }
+									ConvexShape(EShapeSubType inSubType, const PhysicsMaterial *inMaterial) : Shape(EShapeType::Convex, inSubType), mMaterial(inMaterial) { }
+
+	// See Shape::GetSubShapeIDBitsRecursive
+	virtual uint					GetSubShapeIDBitsRecursive() const override					{ return 0; } // Convex shapes don't have sub shapes
+
+	// See Shape::GetMaterial
+	virtual const PhysicsMaterial *	GetMaterial([[maybe_unused]] const SubShapeID &inSubShapeID) const override	{ JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); return GetMaterial(); }
+
+	// See Shape::CastRay
+	virtual bool					CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+	virtual void					CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollidePoint
+	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See Shape::GetTrianglesStart
+	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
+
+	// See Shape::GetTrianglesNext
+	virtual int						GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override;
+
+	// See Shape::GetSubmergedVolume
+	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override;
+
+	/// Function that provides an interface for GJK
+	class Support
+	{
+	public:
+		/// Warning: Virtual destructor will not be called on this object!
+		virtual						~Support() = default;
+
+		/// Calculate the support vector for this convex shape (includes / excludes the convex radius depending on how this was obtained).
+		/// Support vector is relative to the center of mass of the shape.
+		virtual Vec3				GetSupport(Vec3Arg inDirection) const = 0;
+
+		/// Convex radius of shape. Collision detection on penetrating shapes is much more expensive,
+		/// so you can add a radius around objects to increase the shape. This makes it far less likely that they will actually penetrate.
+		virtual float				GetConvexRadius() const = 0;
+	};
+
+	/// Buffer to hold a Support object, used to avoid dynamic memory allocations
+	class alignas(16) SupportBuffer
+	{
+	public:
+		uint8						mData[4160];
+	};
+
+	/// How the GetSupport function should behave
+	enum class ESupportMode
+	{
+		ExcludeConvexRadius,		///< Return the shape excluding the convex radius, Support::GetConvexRadius will return the convex radius if there is one, but adding this radius may not result in the most accurate/efficient representation of shapes with sharp edges
+		IncludeConvexRadius,		///< Return the shape including the convex radius, Support::GetSupport includes the convex radius if there is one, Support::GetConvexRadius will return 0
+		Default,					///< Use both Support::GetSupport add Support::GetConvexRadius to get a support point that matches the original shape as accurately/efficiently as possible
+	};
+
+	/// Returns an object that provides the GetSupport function for this shape.
+	/// inMode determines if this support function includes or excludes the convex radius.
+	/// of the values returned by the GetSupport function. This improves numerical accuracy of the results.
+	/// inScale scales this shape in local space.
+	virtual const Support *			GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const = 0;
+
+	/// Material of the shape
+	void							SetMaterial(const PhysicsMaterial *inMaterial)				{ mMaterial = inMaterial; }
+	const PhysicsMaterial *			GetMaterial() const											{ return mMaterial != nullptr? mMaterial : PhysicsMaterial::sDefault; }
+
+	/// Set density of the shape (kg / m^3)
+	void							SetDensity(float inDensity)									{ mDensity = inDensity; }
+
+	/// Get density of the shape (kg / m^3)
+	float							GetDensity() const											{ return mDensity; }
+
+#ifdef JPH_DEBUG_RENDERER
+	// See Shape::DrawGetSupportFunction
+	virtual void					DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override;
+
+	// See Shape::DrawGetSupportingFace
+	virtual void					DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// See Shape
+	virtual void					SaveBinaryState(StreamOut &inStream) const override;
+	virtual void					SaveMaterialState(PhysicsMaterialList &outMaterials) const override;
+	virtual void					RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) override;
+
+	// Register shape functions with the registry
+	static void						sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void					RestoreBinaryState(StreamIn &inStream) override;
+
+	/// Vertex list that forms a unit sphere
+	static const StaticArray<Vec3, 384> sUnitSphereTriangles;
+
+private:
+	// Class for GetTrianglesStart/Next
+	class							CSGetTrianglesContext;
+
+	// Helper functions called by CollisionDispatch
+	static void						sCollideConvexVsConvex(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCastConvexVsConvex(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+
+	// Properties
+	RefConst<PhysicsMaterial>		mMaterial;													///< Material assigned to this shape
+	float							mDensity = 1000.0f;											///< Uniform density of the interior of the convex object (kg / m^3)
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CylinderShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CylinderShape.cpp
new file mode 100644
index 0000000000..06fc5ec718
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CylinderShape.cpp
@@ -0,0 +1,404 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/Shape/CylinderShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/Shape/GetTrianglesContext.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
+#include <Jolt/Geometry/RayCylinder.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(CylinderShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(CylinderShapeSettings, ConvexShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(CylinderShapeSettings, mHalfHeight)
+	JPH_ADD_ATTRIBUTE(CylinderShapeSettings, mRadius)
+	JPH_ADD_ATTRIBUTE(CylinderShapeSettings, mConvexRadius)
+}
+
+// Approximation of top face with 8 vertices
+static const Vec3 cCylinderTopFace[] =
+{
+	Vec3(0.0f,			1.0f,	1.0f),
+	Vec3(0.707106769f,	1.0f,	0.707106769f),
+	Vec3(1.0f,			1.0f,	0.0f),
+	Vec3(0.707106769f,	1.0f,	-0.707106769f),
+	Vec3(-0.0f,			1.0f,	-1.0f),
+	Vec3(-0.707106769f,	1.0f,	-0.707106769f),
+	Vec3(-1.0f,			1.0f,	0.0f),
+	Vec3(-0.707106769f,	1.0f,	0.707106769f)
+};
+
+static const StaticArray<Vec3, 96> sUnitCylinderTriangles = []() {
+	StaticArray<Vec3, 96> verts;
+
+	const Vec3 bottom_offset(0.0f, -2.0f, 0.0f);
+
+	int num_verts = sizeof(cCylinderTopFace) / sizeof(Vec3);
+	for (int i = 0; i < num_verts; ++i)
+	{
+		Vec3 t1 = cCylinderTopFace[i];
+		Vec3 t2 = cCylinderTopFace[(i + 1) % num_verts];
+		Vec3 b1 = cCylinderTopFace[i] + bottom_offset;
+		Vec3 b2 = cCylinderTopFace[(i + 1) % num_verts] + bottom_offset;
+
+		// Top
+		verts.emplace_back(0.0f, 1.0f, 0.0f);
+		verts.push_back(t1);
+		verts.push_back(t2);
+
+		// Bottom
+		verts.emplace_back(0.0f, -1.0f, 0.0f);
+		verts.push_back(b2);
+		verts.push_back(b1);
+
+		// Side
+		verts.push_back(t1);
+		verts.push_back(b1);
+		verts.push_back(t2);
+
+		verts.push_back(t2);
+		verts.push_back(b1);
+		verts.push_back(b2);
+	}
+
+	return verts;
+}();
+
+ShapeSettings::ShapeResult CylinderShapeSettings::Create() const
+{
+	if (mCachedResult.IsEmpty())
+		Ref<Shape> shape = new CylinderShape(*this, mCachedResult);
+	return mCachedResult;
+}
+
+CylinderShape::CylinderShape(const CylinderShapeSettings &inSettings, ShapeResult &outResult) :
+	ConvexShape(EShapeSubType::Cylinder, inSettings, outResult),
+	mHalfHeight(inSettings.mHalfHeight),
+	mRadius(inSettings.mRadius),
+	mConvexRadius(inSettings.mConvexRadius)
+{
+	if (inSettings.mHalfHeight < inSettings.mConvexRadius)
+	{
+		outResult.SetError("Invalid height");
+		return;
+	}
+
+	if (inSettings.mRadius < inSettings.mConvexRadius)
+	{
+		outResult.SetError("Invalid radius");
+		return;
+	}
+
+	if (inSettings.mConvexRadius < 0.0f)
+	{
+		outResult.SetError("Invalid convex radius");
+		return;
+	}
+
+	outResult.Set(this);
+}
+
+CylinderShape::CylinderShape(float inHalfHeight, float inRadius, float inConvexRadius, const PhysicsMaterial *inMaterial) :
+	ConvexShape(EShapeSubType::Cylinder, inMaterial),
+	mHalfHeight(inHalfHeight),
+	mRadius(inRadius),
+	mConvexRadius(inConvexRadius)
+{
+	JPH_ASSERT(inHalfHeight >= inConvexRadius);
+	JPH_ASSERT(inRadius >= inConvexRadius);
+	JPH_ASSERT(inConvexRadius >= 0.0f);
+}
+
+class CylinderShape::Cylinder final : public Support
+{
+public:
+					Cylinder(float inHalfHeight, float inRadius, float inConvexRadius) :
+		mHalfHeight(inHalfHeight),
+		mRadius(inRadius),
+		mConvexRadius(inConvexRadius)
+	{
+		static_assert(sizeof(Cylinder) <= sizeof(SupportBuffer), "Buffer size too small");
+		JPH_ASSERT(IsAligned(this, alignof(Cylinder)));
+	}
+
+	virtual Vec3	GetSupport(Vec3Arg inDirection) const override
+	{
+		// Support mapping, taken from:
+		// A Fast and Robust GJK Implementation for Collision Detection of Convex Objects - Gino van den Bergen
+		// page 8
+		float x = inDirection.GetX(), y = inDirection.GetY(), z = inDirection.GetZ();
+		float o = sqrt(Square(x) + Square(z));
+		if (o > 0.0f)
+			return Vec3((mRadius * x) / o, Sign(y) * mHalfHeight, (mRadius * z) / o);
+		else
+			return Vec3(0, Sign(y) * mHalfHeight, 0);
+	}
+
+	virtual float	GetConvexRadius() const override
+	{
+		return mConvexRadius;
+	}
+
+private:
+	float			mHalfHeight;
+	float			mRadius;
+	float			mConvexRadius;
+};
+
+const ConvexShape::Support *CylinderShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const
+{
+	JPH_ASSERT(IsValidScale(inScale));
+
+	// Get scaled cylinder
+	Vec3 abs_scale = inScale.Abs();
+	float scale_xz = abs_scale.GetX();
+	float scale_y = abs_scale.GetY();
+	float scaled_half_height = scale_y * mHalfHeight;
+	float scaled_radius = scale_xz * mRadius;
+	float scaled_convex_radius = ScaleHelpers::ScaleConvexRadius(mConvexRadius, inScale);
+
+	switch (inMode)
+	{
+	case ESupportMode::IncludeConvexRadius:
+	case ESupportMode::Default:
+		return new (&inBuffer) Cylinder(scaled_half_height, scaled_radius, 0.0f);
+
+	case ESupportMode::ExcludeConvexRadius:
+		return new (&inBuffer) Cylinder(scaled_half_height - scaled_convex_radius, scaled_radius - scaled_convex_radius, scaled_convex_radius);
+	}
+
+	JPH_ASSERT(false);
+	return nullptr;
+}
+
+void CylinderShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
+	JPH_ASSERT(IsValidScale(inScale));
+
+	// Get scaled cylinder
+	Vec3 abs_scale = inScale.Abs();
+	float scale_xz = abs_scale.GetX();
+	float scale_y = abs_scale.GetY();
+	float scaled_half_height = scale_y * mHalfHeight;
+	float scaled_radius = scale_xz * mRadius;
+
+	float x = inDirection.GetX(), y = inDirection.GetY(), z = inDirection.GetZ();
+	float o = sqrt(Square(x) + Square(z));
+
+	// If o / |y| > scaled_radius / scaled_half_height, we're hitting the side
+	if (o * scaled_half_height > scaled_radius * abs(y))
+	{
+		// Hitting side
+		float f = -scaled_radius / o;
+		float vx = x * f;
+		float vz = z * f;
+		outVertices.push_back(inCenterOfMassTransform * Vec3(vx, scaled_half_height, vz));
+		outVertices.push_back(inCenterOfMassTransform * Vec3(vx, -scaled_half_height, vz));
+	}
+	else
+	{
+		// Hitting top or bottom
+		Vec3 multiplier = y < 0.0f? Vec3(scaled_radius, scaled_half_height, scaled_radius) : Vec3(-scaled_radius, -scaled_half_height, scaled_radius);
+		Mat44 transform = inCenterOfMassTransform.PreScaled(multiplier);
+		for (const Vec3 &v : cCylinderTopFace)
+			outVertices.push_back(transform * v);
+	}
+}
+
+MassProperties CylinderShape::GetMassProperties() const
+{
+	MassProperties p;
+
+	// Mass is surface of circle * height
+	float radius_sq = Square(mRadius);
+	float height = 2.0f * mHalfHeight;
+	p.mMass = JPH_PI * radius_sq * height * GetDensity();
+
+	// Inertia according to https://en.wikipedia.org/wiki/List_of_moments_of_inertia:
+	float inertia_y = radius_sq * p.mMass * 0.5f;
+	float inertia_x = inertia_y * 0.5f + p.mMass * height * height / 12.0f;
+	float inertia_z = inertia_x;
+
+	// Set inertia
+	p.mInertia = Mat44::sScale(Vec3(inertia_x, inertia_y, inertia_z));
+
+	return p;
+}
+
+Vec3 CylinderShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
+
+	// Calculate distance to infinite cylinder surface
+	Vec3 local_surface_position_xz(inLocalSurfacePosition.GetX(), 0, inLocalSurfacePosition.GetZ());
+	float local_surface_position_xz_len = local_surface_position_xz.Length();
+	float distance_to_curved_surface = abs(local_surface_position_xz_len - mRadius);
+
+	// Calculate distance to top or bottom plane
+	float distance_to_top_or_bottom = abs(abs(inLocalSurfacePosition.GetY()) - mHalfHeight);
+
+	// Return normal according to closest surface
+	if (distance_to_curved_surface < distance_to_top_or_bottom)
+		return local_surface_position_xz / local_surface_position_xz_len;
+	else
+		return inLocalSurfacePosition.GetY() > 0.0f? Vec3::sAxisY() : -Vec3::sAxisY();
+}
+
+AABox CylinderShape::GetLocalBounds() const
+{
+	Vec3 extent = Vec3(mRadius, mHalfHeight, mRadius);
+	return AABox(-extent, extent);
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void CylinderShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid;
+	inRenderer->DrawCylinder(inCenterOfMassTransform * Mat44::sScale(inScale.Abs()), mHalfHeight, mRadius, inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor, DebugRenderer::ECastShadow::On, draw_mode);
+}
+#endif // JPH_DEBUG_RENDERER
+
+bool CylinderShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	// Test ray against capsule
+	float fraction = RayCylinder(inRay.mOrigin, inRay.mDirection, mHalfHeight, mRadius);
+	if (fraction < ioHit.mFraction)
+	{
+		ioHit.mFraction = fraction;
+		ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID();
+		return true;
+	}
+	return false;
+}
+
+void CylinderShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	// Check if the point is in the cylinder
+	if (abs(inPoint.GetY()) <= mHalfHeight											// Within the height
+		&& Square(inPoint.GetX()) + Square(inPoint.GetZ()) <= Square(mRadius))		// Within the radius
+		ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
+}
+
+void CylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
+{
+	JPH_ASSERT(IsValidScale(inScale));
+
+	Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation();
+
+	// Get scaled cylinder
+	Vec3 abs_scale = inScale.Abs();
+	float half_height = abs_scale.GetY() * mHalfHeight;
+	float radius = abs_scale.GetX() * mRadius;
+
+	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
+		if (v.GetInvMass() > 0.0f)
+		{
+			Vec3 local_pos = inverse_transform * v.GetPosition();
+
+			// Calculate penetration into side surface
+			Vec3 side_normal = local_pos;
+			side_normal.SetY(0.0f);
+			float side_normal_length = side_normal.Length();
+			float side_penetration = radius - side_normal_length;
+
+			// Calculate penetration into top or bottom plane
+			float top_penetration = half_height - abs(local_pos.GetY());
+
+			Vec3 point, normal;
+			if (side_penetration < 0.0f && top_penetration < 0.0f)
+			{
+				// We're outside the cylinder height and radius
+				point = side_normal * (radius / side_normal_length) + Vec3(0, half_height * Sign(local_pos.GetY()), 0);
+				normal = (local_pos - point).NormalizedOr(Vec3::sAxisY());
+			}
+			else if (side_penetration < top_penetration)
+			{
+				// Side surface is closest
+				normal = side_normal_length > 0.0f? side_normal / side_normal_length : Vec3::sAxisX();
+				point = radius * normal;
+			}
+			else
+			{
+				// Top or bottom plane is closest
+				normal = Vec3(0, Sign(local_pos.GetY()), 0);
+				point = half_height * normal;
+			}
+
+			// Calculate penetration
+			Plane plane = Plane::sFromPointAndNormal(point, normal);
+			float penetration = -plane.SignedDistance(local_pos);
+			if (v.UpdatePenetration(penetration))
+				v.SetCollision(plane.GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex);
+		}
+}
+
+void CylinderShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const
+{
+	Mat44 unit_cylinder_transform(Vec4(mRadius, 0, 0, 0), Vec4(0, mHalfHeight, 0, 0), Vec4(0, 0, mRadius, 0), Vec4(0, 0, 0, 1));
+	new (&ioContext) GetTrianglesContextVertexList(inPositionCOM, inRotation, inScale, unit_cylinder_transform, sUnitCylinderTriangles.data(), sUnitCylinderTriangles.size(), GetMaterial());
+}
+
+int CylinderShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const
+{
+	return ((GetTrianglesContextVertexList &)ioContext).GetTrianglesNext(inMaxTrianglesRequested, outTriangleVertices, outMaterials);
+}
+
+void CylinderShape::SaveBinaryState(StreamOut &inStream) const
+{
+	ConvexShape::SaveBinaryState(inStream);
+
+	inStream.Write(mHalfHeight);
+	inStream.Write(mRadius);
+	inStream.Write(mConvexRadius);
+}
+
+void CylinderShape::RestoreBinaryState(StreamIn &inStream)
+{
+	ConvexShape::RestoreBinaryState(inStream);
+
+	inStream.Read(mHalfHeight);
+	inStream.Read(mRadius);
+	inStream.Read(mConvexRadius);
+}
+
+bool CylinderShape::IsValidScale(Vec3Arg inScale) const
+{
+	return ConvexShape::IsValidScale(inScale) && ScaleHelpers::IsUniformScaleXZ(inScale.Abs());
+}
+
+Vec3 CylinderShape::MakeScaleValid(Vec3Arg inScale) const
+{
+	Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale);
+
+	return scale.GetSign() * ScaleHelpers::MakeUniformScaleXZ(scale.Abs());
+}
+
+void CylinderShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Cylinder);
+	f.mConstruct = []() -> Shape * { return new CylinderShape; };
+	f.mColor = Color::sGreen;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CylinderShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CylinderShape.h
new file mode 100644
index 0000000000..302c97565b
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/CylinderShape.h
@@ -0,0 +1,126 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/ConvexShape.h>
+#include <Jolt/Physics/PhysicsSettings.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that constructs a CylinderShape
+class JPH_EXPORT CylinderShapeSettings final : public ConvexShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, CylinderShapeSettings)
+
+public:
+	/// Default constructor for deserialization
+							CylinderShapeSettings() = default;
+
+	/// Create a shape centered around the origin with one top at (0, -inHalfHeight, 0) and the other at (0, inHalfHeight, 0) and radius inRadius.
+	/// (internally the convex radius will be subtracted from the cylinder the total cylinder will not grow with the convex radius, but the edges of the cylinder will be rounded a bit).
+							CylinderShapeSettings(float inHalfHeight, float inRadius, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mHalfHeight(inHalfHeight), mRadius(inRadius), mConvexRadius(inConvexRadius) { }
+
+	// See: ShapeSettings
+	virtual ShapeResult		Create() const override;
+
+	float					mHalfHeight = 0.0f;
+	float					mRadius = 0.0f;
+	float					mConvexRadius = 0.0f;
+};
+
+/// A cylinder
+class JPH_EXPORT CylinderShape final : public ConvexShape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+							CylinderShape() : ConvexShape(EShapeSubType::Cylinder) { }
+							CylinderShape(const CylinderShapeSettings &inSettings, ShapeResult &outResult);
+
+	/// Create a shape centered around the origin with one top at (0, -inHalfHeight, 0) and the other at (0, inHalfHeight, 0) and radius inRadius.
+	/// (internally the convex radius will be subtracted from the cylinder the total cylinder will not grow with the convex radius, but the edges of the cylinder will be rounded a bit).
+							CylinderShape(float inHalfHeight, float inRadius, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr);
+
+	/// Get half height of cylinder
+	float					GetHalfHeight() const														{ return mHalfHeight; }
+
+	/// Get radius of cylinder
+	float					GetRadius() const															{ return mRadius; }
+
+	// See Shape::GetLocalBounds
+	virtual AABox			GetLocalBounds() const override;
+
+	// See Shape::GetInnerRadius
+	virtual float			GetInnerRadius() const override												{ return min(mHalfHeight, mRadius); }
+
+	// See Shape::GetMassProperties
+	virtual MassProperties	GetMassProperties() const override;
+
+	// See Shape::GetSurfaceNormal
+	virtual Vec3			GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override;
+
+	// See Shape::GetSupportingFace
+	virtual void			GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
+
+	// See ConvexShape::GetSupportFunction
+	virtual const Support *	GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override;
+
+#ifdef JPH_DEBUG_RENDERER
+	// See Shape::Draw
+	virtual void			Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// See Shape::CastRay
+	using ConvexShape::CastRay;
+	virtual bool			CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+
+	// See: Shape::CollidePoint
+	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollideSoftBodyVertices
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
+
+	// See Shape::GetTrianglesStart
+	virtual void			GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
+
+	// See Shape::GetTrianglesNext
+	virtual int				GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override;
+
+	// See Shape
+	virtual void			SaveBinaryState(StreamOut &inStream) const override;
+
+	// See Shape::GetStats
+	virtual Stats			GetStats() const override												{ return Stats(sizeof(*this), 0); }
+
+	// See Shape::GetVolume
+	virtual float			GetVolume() const override												{ return 2.0f * JPH_PI * mHalfHeight * Square(mRadius); }
+
+	/// Get the convex radius of this cylinder
+	float					GetConvexRadius() const													{ return mConvexRadius; }
+
+	// See Shape::IsValidScale
+	virtual bool			IsValidScale(Vec3Arg inScale) const override;
+
+	// See Shape::MakeScaleValid
+	virtual Vec3			MakeScaleValid(Vec3Arg inScale) const override;
+
+	// Register shape functions with the registry
+	static void				sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void			RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	// Class for GetSupportFunction
+	class					Cylinder;
+
+	float					mHalfHeight = 0.0f;
+	float					mRadius = 0.0f;
+	float					mConvexRadius = 0.0f;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/DecoratedShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/DecoratedShape.cpp
new file mode 100644
index 0000000000..339f783637
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/DecoratedShape.cpp
@@ -0,0 +1,87 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/Shape/DecoratedShape.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(DecoratedShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(DecoratedShapeSettings, ShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(DecoratedShapeSettings, mInnerShape)
+}
+
+DecoratedShape::DecoratedShape(EShapeSubType inSubType, const DecoratedShapeSettings &inSettings, ShapeResult &outResult) :
+	Shape(EShapeType::Decorated, inSubType, inSettings, outResult)
+{
+	// Check that there's a shape
+	if (inSettings.mInnerShape == nullptr && inSettings.mInnerShapePtr == nullptr)
+	{
+		outResult.SetError("Inner shape is null!");
+		return;
+	}
+
+	if (inSettings.mInnerShapePtr != nullptr)
+	{
+		// Use provided shape
+		mInnerShape = inSettings.mInnerShapePtr;
+	}
+	else
+	{
+		// Create child shape
+		ShapeResult child_result = inSettings.mInnerShape->Create();
+		if (!child_result.IsValid())
+		{
+			outResult = child_result;
+			return;
+		}
+		mInnerShape = child_result.Get();
+	}
+}
+
+const PhysicsMaterial *DecoratedShape::GetMaterial(const SubShapeID &inSubShapeID) const
+{
+	return mInnerShape->GetMaterial(inSubShapeID);
+}
+
+void DecoratedShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
+	mInnerShape->GetSupportingFace(inSubShapeID, inDirection, inScale, inCenterOfMassTransform, outVertices);
+}
+
+uint64 DecoratedShape::GetSubShapeUserData(const SubShapeID &inSubShapeID) const
+{
+	return mInnerShape->GetSubShapeUserData(inSubShapeID);
+}
+
+void DecoratedShape::SaveSubShapeState(ShapeList &outSubShapes) const
+{
+	outSubShapes.clear();
+	outSubShapes.push_back(mInnerShape);
+}
+
+void DecoratedShape::RestoreSubShapeState(const ShapeRefC *inSubShapes, uint inNumShapes)
+{
+	JPH_ASSERT(inNumShapes == 1);
+	mInnerShape = inSubShapes[0];
+}
+
+Shape::Stats DecoratedShape::GetStatsRecursive(VisitedShapes &ioVisitedShapes) const
+{
+	// Get own stats
+	Stats stats = Shape::GetStatsRecursive(ioVisitedShapes);
+
+	// Add child stats
+	Stats child_stats = mInnerShape->GetStatsRecursive(ioVisitedShapes);
+	stats.mSizeBytes += child_stats.mSizeBytes;
+	stats.mNumTriangles += child_stats.mNumTriangles;
+
+	return stats;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/DecoratedShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/DecoratedShape.h
new file mode 100644
index 0000000000..5ab8748fc9
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/DecoratedShape.h
@@ -0,0 +1,80 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that constructs a DecoratedShape
+class JPH_EXPORT DecoratedShapeSettings : public ShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, DecoratedShapeSettings)
+
+public:
+	/// Default constructor for deserialization
+									DecoratedShapeSettings() = default;
+
+	/// Constructor that decorates another shape
+	explicit						DecoratedShapeSettings(const ShapeSettings *inShape)	: mInnerShape(inShape) { }
+	explicit						DecoratedShapeSettings(const Shape *inShape)			: mInnerShapePtr(inShape) { }
+
+	RefConst<ShapeSettings>			mInnerShape;											///< Sub shape (either this or mShapePtr needs to be filled up)
+	RefConst<Shape>					mInnerShapePtr;											///< Sub shape (either this or mShape needs to be filled up)
+};
+
+/// Base class for shapes that decorate another shape with extra functionality (e.g. scale, translation etc.)
+class JPH_EXPORT DecoratedShape : public Shape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+	explicit						DecoratedShape(EShapeSubType inSubType) : Shape(EShapeType::Decorated, inSubType) { }
+									DecoratedShape(EShapeSubType inSubType, const Shape *inInnerShape) : Shape(EShapeType::Decorated, inSubType), mInnerShape(inInnerShape) { }
+									DecoratedShape(EShapeSubType inSubType, const DecoratedShapeSettings &inSettings, ShapeResult &outResult);
+
+	/// Access to the decorated inner shape
+	const Shape *					GetInnerShape() const									{ return mInnerShape; }
+
+	// See Shape::MustBeStatic
+	virtual bool					MustBeStatic() const override							{ return mInnerShape->MustBeStatic(); }
+
+	// See Shape::GetCenterOfMass
+	virtual Vec3					GetCenterOfMass() const override						{ return mInnerShape->GetCenterOfMass(); }
+
+	// See Shape::GetSubShapeIDBitsRecursive
+	virtual uint					GetSubShapeIDBitsRecursive() const override				{ return mInnerShape->GetSubShapeIDBitsRecursive(); }
+
+	// See Shape::GetLeafShape
+	virtual const Shape *			GetLeafShape(const SubShapeID &inSubShapeID, SubShapeID &outRemainder) const override { return mInnerShape->GetLeafShape(inSubShapeID, outRemainder); }
+
+	// See Shape::GetMaterial
+	virtual const PhysicsMaterial *	GetMaterial(const SubShapeID &inSubShapeID) const override;
+
+	// See Shape::GetSupportingFace
+	virtual void					GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
+
+	// See Shape::GetSubShapeUserData
+	virtual uint64					GetSubShapeUserData(const SubShapeID &inSubShapeID) const override;
+
+	// See Shape
+	virtual void					SaveSubShapeState(ShapeList &outSubShapes) const override;
+	virtual void					RestoreSubShapeState(const ShapeRefC *inSubShapes, uint inNumShapes) override;
+
+	// See Shape::GetStatsRecursive
+	virtual Stats					GetStatsRecursive(VisitedShapes &ioVisitedShapes) const override;
+
+	// See Shape::IsValidScale
+	virtual bool					IsValidScale(Vec3Arg inScale) const override			{ return mInnerShape->IsValidScale(inScale); }
+
+	// See Shape::MakeScaleValid
+	virtual Vec3					MakeScaleValid(Vec3Arg inScale) const override			{ return mInnerShape->MakeScaleValid(inScale); }
+
+protected:
+	RefConst<Shape>					mInnerShape;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/EmptyShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/EmptyShape.cpp
new file mode 100644
index 0000000000..8cb81f68d0
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/EmptyShape.cpp
@@ -0,0 +1,65 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/Shape/EmptyShape.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(EmptyShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(EmptyShapeSettings, ShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(EmptyShapeSettings, mCenterOfMass)
+}
+
+ShapeSettings::ShapeResult EmptyShapeSettings::Create() const
+{
+	if (mCachedResult.IsEmpty())
+		new EmptyShape(*this, mCachedResult);
+
+	return mCachedResult;
+}
+
+MassProperties EmptyShape::GetMassProperties() const
+{
+	MassProperties mass_properties;
+	mass_properties.mMass = 1.0f;
+	mass_properties.mInertia = Mat44::sIdentity();
+	return mass_properties;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void EmptyShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, [[maybe_unused]] bool inUseMaterialColors, [[maybe_unused]] bool inDrawWireframe) const
+{
+	inRenderer->DrawMarker(inCenterOfMassTransform.GetTranslation(), inColor, abs(inScale.GetX()) * 0.1f);
+}
+#endif // JPH_DEBUG_RENDERER
+
+void EmptyShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Empty);
+	f.mConstruct = []() -> Shape * { return new EmptyShape; };
+	f.mColor = Color::sBlack;
+
+	auto collide_empty = []([[maybe_unused]] const Shape *inShape1, [[maybe_unused]] const Shape *inShape2, [[maybe_unused]] Vec3Arg inScale1, [[maybe_unused]] Vec3Arg inScale2, [[maybe_unused]] Mat44Arg inCenterOfMassTransform1, [[maybe_unused]] Mat44Arg inCenterOfMassTransform2, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator1, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator2, [[maybe_unused]] const CollideShapeSettings &inCollideShapeSettings, [[maybe_unused]] CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter) { /* Do Nothing */ };
+	auto cast_empty = []([[maybe_unused]] const ShapeCast &inShapeCast, [[maybe_unused]] const ShapeCastSettings &inShapeCastSettings, [[maybe_unused]] const Shape *inShape, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, [[maybe_unused]] Mat44Arg inCenterOfMassTransform2, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator1, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator2, [[maybe_unused]] CastShapeCollector &ioCollector) { /* Do nothing */ };
+
+	for (const EShapeSubType s : sAllSubShapeTypes)
+	{
+		CollisionDispatch::sRegisterCollideShape(EShapeSubType::Empty, s, collide_empty);
+		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::Empty, collide_empty);
+
+		CollisionDispatch::sRegisterCastShape(EShapeSubType::Empty, s, cast_empty);
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Empty, cast_empty);
+	}
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/EmptyShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/EmptyShape.h
new file mode 100644
index 0000000000..f3e7bdcf6a
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/EmptyShape.h
@@ -0,0 +1,75 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+#include <Jolt/Physics/Collision/PhysicsMaterial.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that constructs an EmptyShape
+class JPH_EXPORT EmptyShapeSettings final : public ShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, EmptyShapeSettings)
+
+public:
+							EmptyShapeSettings() = default;
+	explicit				EmptyShapeSettings(Vec3Arg inCenterOfMass) : mCenterOfMass(inCenterOfMass) { }
+
+	ShapeResult				Create() const override;
+
+	Vec3					mCenterOfMass = Vec3::sZero();									///< Determines the center of mass for this shape
+};
+
+/// An empty shape that has no volume and collides with nothing.
+///
+/// Possible use cases:
+/// - As a placeholder for a shape that will be created later. E.g. if you first need to create a body and only then know what shape it will have.
+/// - If you need a kinematic body to attach a constraint to, but you don't want the body to collide with anything.
+///
+/// Note that, if possible, you should also put your body in an ObjectLayer that doesn't collide with anything.
+/// This ensures that collisions will be filtered out at broad phase level instead of at narrow phase level, this is more efficient.
+class JPH_EXPORT EmptyShape final : public Shape
+{
+public:
+	// Constructor
+							EmptyShape() : Shape(EShapeType::Empty, EShapeSubType::Empty) { }
+	explicit				EmptyShape(Vec3Arg inCenterOfMass) : Shape(EShapeType::Empty, EShapeSubType::Empty), mCenterOfMass(inCenterOfMass) { }
+							EmptyShape(const EmptyShapeSettings &inSettings, ShapeResult &outResult) : Shape(EShapeType::Empty, EShapeSubType::Empty, inSettings, outResult), mCenterOfMass(inSettings.mCenterOfMass) { outResult.Set(this); }
+
+	// See: Shape
+	Vec3					GetCenterOfMass() const override								{ return mCenterOfMass; }
+	AABox					GetLocalBounds() const override									{ return { Vec3::sZero(), Vec3::sZero() }; }
+	uint					GetSubShapeIDBitsRecursive() const override						{ return 0; }
+	float					GetInnerRadius() const override									{ return 0.0f; }
+	MassProperties			GetMassProperties() const override;
+	const PhysicsMaterial *	GetMaterial([[maybe_unused]] const SubShapeID &inSubShapeID) const override { return PhysicsMaterial::sDefault; }
+	virtual Vec3			GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override { return Vec3::sZero(); }
+	virtual void			GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy
+#ifdef JPH_DEBUG_RENDERER // Not using JPH_IF_DEBUG_RENDERER for Doxygen
+		, RVec3Arg inBaseOffset
+#endif
+		) const override																	{ outTotalVolume = 0.0f; outSubmergedVolume = 0.0f; outCenterOfBuoyancy = Vec3::sZero(); }
+#ifdef JPH_DEBUG_RENDERER
+	virtual void			Draw([[maybe_unused]] DebugRenderer *inRenderer, [[maybe_unused]] RMat44Arg inCenterOfMassTransform, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] ColorArg inColor, [[maybe_unused]] bool inUseMaterialColors, [[maybe_unused]] bool inDrawWireframe) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual bool			CastRay([[maybe_unused]] const RayCast &inRay, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator, [[maybe_unused]] RayCastResult &ioHit) const override { return false; }
+	virtual void			CastRay([[maybe_unused]] const RayCast &inRay, [[maybe_unused]] const RayCastSettings &inRayCastSettings, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator, [[maybe_unused]] CastRayCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter = { }) const override { /* Do nothing */ }
+	virtual void			CollidePoint([[maybe_unused]] Vec3Arg inPoint, [[maybe_unused]] const SubShapeIDCreator &inSubShapeIDCreator, [[maybe_unused]] CollidePointCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter = { }) const override { /* Do nothing */ }
+	virtual void			CollideSoftBodyVertices([[maybe_unused]] Mat44Arg inCenterOfMassTransform, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] const CollideSoftBodyVertexIterator &inVertices, [[maybe_unused]] uint inNumVertices, [[maybe_unused]] int inCollidingShapeIndex) const override { /* Do nothing */ }
+	virtual void			GetTrianglesStart([[maybe_unused]] GetTrianglesContext &ioContext, [[maybe_unused]] const AABox &inBox, [[maybe_unused]] Vec3Arg inPositionCOM, [[maybe_unused]] QuatArg inRotation, [[maybe_unused]] Vec3Arg inScale) const override { /* Do nothing */ }
+	virtual int				GetTrianglesNext([[maybe_unused]] GetTrianglesContext &ioContext, [[maybe_unused]] int inMaxTrianglesRequested, [[maybe_unused]] Float3 *outTriangleVertices, [[maybe_unused]] const PhysicsMaterial **outMaterials = nullptr) const override { return 0; }
+	Stats					GetStats() const override										{ return { sizeof(*this), 0 }; }
+	float					GetVolume() const override										{ return 0.0f; }
+	bool					IsValidScale([[maybe_unused]] Vec3Arg inScale) const override	{ return true; }
+
+	// Register shape functions with the registry
+	static void				sRegister();
+
+private:
+	Vec3					mCenterOfMass = Vec3::sZero();
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/GetTrianglesContext.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/GetTrianglesContext.h
new file mode 100644
index 0000000000..df68ae2f55
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/GetTrianglesContext.h
@@ -0,0 +1,248 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+
+JPH_NAMESPACE_BEGIN
+
+class PhysicsMaterial;
+
+/// Implementation of GetTrianglesStart/Next that uses a fixed list of vertices for the triangles. These are transformed into world space when getting the triangles.
+class GetTrianglesContextVertexList
+{
+public:
+	/// Constructor, to be called in GetTrianglesStart
+					GetTrianglesContextVertexList(Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, Mat44Arg inLocalTransform, const Vec3 *inTriangleVertices, size_t inNumTriangleVertices, const PhysicsMaterial *inMaterial) :
+		mLocalToWorld(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale) * inLocalTransform),
+		mTriangleVertices(inTriangleVertices),
+		mNumTriangleVertices(inNumTriangleVertices),
+		mMaterial(inMaterial),
+		mIsInsideOut(ScaleHelpers::IsInsideOut(inScale))
+	{
+		static_assert(sizeof(GetTrianglesContextVertexList) <= sizeof(Shape::GetTrianglesContext), "GetTrianglesContext too small");
+		JPH_ASSERT(IsAligned(this, alignof(GetTrianglesContextVertexList)));
+		JPH_ASSERT(inNumTriangleVertices % 3 == 0);
+	}
+
+	/// @see Shape::GetTrianglesNext
+	int				GetTrianglesNext(int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials)
+	{
+		JPH_ASSERT(inMaxTrianglesRequested >= Shape::cGetTrianglesMinTrianglesRequested);
+
+		int total_num_vertices = min(inMaxTrianglesRequested * 3, int(mNumTriangleVertices - mCurrentVertex));
+
+		if (mIsInsideOut)
+		{
+			// Store triangles flipped
+			for (const Vec3 *v = mTriangleVertices + mCurrentVertex, *v_end = v + total_num_vertices; v < v_end; v += 3)
+			{
+				(mLocalToWorld * v[0]).StoreFloat3(outTriangleVertices++);
+				(mLocalToWorld * v[2]).StoreFloat3(outTriangleVertices++);
+				(mLocalToWorld * v[1]).StoreFloat3(outTriangleVertices++);
+			}
+		}
+		else
+		{
+			// Store triangles
+			for (const Vec3 *v = mTriangleVertices + mCurrentVertex, *v_end = v + total_num_vertices; v < v_end; v += 3)
+			{
+				(mLocalToWorld * v[0]).StoreFloat3(outTriangleVertices++);
+				(mLocalToWorld * v[1]).StoreFloat3(outTriangleVertices++);
+				(mLocalToWorld * v[2]).StoreFloat3(outTriangleVertices++);
+			}
+		}
+
+		// Update the current vertex to point to the next vertex to get
+		mCurrentVertex += total_num_vertices;
+		int total_num_triangles = total_num_vertices / 3;
+
+		// Store materials
+		if (outMaterials != nullptr)
+			for (const PhysicsMaterial **m = outMaterials, **m_end = outMaterials + total_num_triangles; m < m_end; ++m)
+				*m = mMaterial;
+
+		return total_num_triangles;
+	}
+
+	/// Helper function that creates a vertex list of a half unit sphere (top part)
+	template <class A>
+	static void		sCreateHalfUnitSphereTop(A &ioVertices, int inDetailLevel)
+	{
+		sCreateUnitSphereHelper(ioVertices,  Vec3::sAxisX(),  Vec3::sAxisY(),  Vec3::sAxisZ(), inDetailLevel);
+		sCreateUnitSphereHelper(ioVertices,  Vec3::sAxisY(), -Vec3::sAxisX(),  Vec3::sAxisZ(), inDetailLevel);
+		sCreateUnitSphereHelper(ioVertices,  Vec3::sAxisY(),  Vec3::sAxisX(), -Vec3::sAxisZ(), inDetailLevel);
+		sCreateUnitSphereHelper(ioVertices, -Vec3::sAxisX(),  Vec3::sAxisY(), -Vec3::sAxisZ(), inDetailLevel);
+	}
+
+	/// Helper function that creates a vertex list of a half unit sphere (bottom part)
+	template <class A>
+	static void		sCreateHalfUnitSphereBottom(A &ioVertices, int inDetailLevel)
+	{
+		sCreateUnitSphereHelper(ioVertices, -Vec3::sAxisX(), -Vec3::sAxisY(),  Vec3::sAxisZ(), inDetailLevel);
+		sCreateUnitSphereHelper(ioVertices, -Vec3::sAxisY(),  Vec3::sAxisX(),  Vec3::sAxisZ(), inDetailLevel);
+		sCreateUnitSphereHelper(ioVertices,  Vec3::sAxisX(), -Vec3::sAxisY(), -Vec3::sAxisZ(), inDetailLevel);
+		sCreateUnitSphereHelper(ioVertices, -Vec3::sAxisY(), -Vec3::sAxisX(), -Vec3::sAxisZ(), inDetailLevel);
+	}
+
+	/// Helper function that creates an open cylinder of half height 1 and radius 1
+	template <class A>
+	static void		sCreateUnitOpenCylinder(A &ioVertices, int inDetailLevel)
+	{
+		const Vec3 bottom_offset(0.0f, -2.0f, 0.0f);
+		int num_verts = 4 * (1 << inDetailLevel);
+		for (int i = 0; i < num_verts; ++i)
+		{
+			float angle1 = 2.0f * JPH_PI * (float(i) / num_verts);
+			float angle2 = 2.0f * JPH_PI * (float(i + 1) / num_verts);
+
+			Vec3 t1(Sin(angle1), 1.0f, Cos(angle1));
+			Vec3 t2(Sin(angle2), 1.0f, Cos(angle2));
+			Vec3 b1 = t1 + bottom_offset;
+			Vec3 b2 = t2 + bottom_offset;
+
+			ioVertices.push_back(t1);
+			ioVertices.push_back(b1);
+			ioVertices.push_back(t2);
+
+			ioVertices.push_back(t2);
+			ioVertices.push_back(b1);
+			ioVertices.push_back(b2);
+		}
+	}
+
+private:
+	/// Recursive helper function for creating a sphere
+	template <class A>
+	static void		sCreateUnitSphereHelper(A &ioVertices, Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, int inLevel)
+	{
+		Vec3 center1 = (inV1 + inV2).Normalized();
+		Vec3 center2 = (inV2 + inV3).Normalized();
+		Vec3 center3 = (inV3 + inV1).Normalized();
+
+		if (inLevel > 0)
+		{
+			int new_level = inLevel - 1;
+			sCreateUnitSphereHelper(ioVertices, inV1, center1, center3, new_level);
+			sCreateUnitSphereHelper(ioVertices, center1, center2, center3, new_level);
+			sCreateUnitSphereHelper(ioVertices, center1, inV2, center2, new_level);
+			sCreateUnitSphereHelper(ioVertices, center3, center2, inV3, new_level);
+		}
+		else
+		{
+			ioVertices.push_back(inV1);
+			ioVertices.push_back(inV2);
+			ioVertices.push_back(inV3);
+		}
+	}
+
+	Mat44					mLocalToWorld;
+	const Vec3 *			mTriangleVertices;
+	size_t					mNumTriangleVertices;
+	size_t					mCurrentVertex = 0;
+	const PhysicsMaterial *	mMaterial;
+	bool					mIsInsideOut;
+};
+
+/// Implementation of GetTrianglesStart/Next that uses a multiple fixed lists of vertices for the triangles. These are transformed into world space when getting the triangles.
+class GetTrianglesContextMultiVertexList
+{
+public:
+	/// Constructor, to be called in GetTrianglesStart
+					GetTrianglesContextMultiVertexList(bool inIsInsideOut, const PhysicsMaterial *inMaterial) :
+		mMaterial(inMaterial),
+		mIsInsideOut(inIsInsideOut)
+	{
+		static_assert(sizeof(GetTrianglesContextMultiVertexList) <= sizeof(Shape::GetTrianglesContext), "GetTrianglesContext too small");
+		JPH_ASSERT(IsAligned(this, alignof(GetTrianglesContextMultiVertexList)));
+	}
+
+	/// Add a mesh part and its transform
+	void			AddPart(Mat44Arg inLocalToWorld, const Vec3 *inTriangleVertices, size_t inNumTriangleVertices)
+	{
+		JPH_ASSERT(inNumTriangleVertices % 3 == 0);
+
+		mParts.push_back({ inLocalToWorld, inTriangleVertices, inNumTriangleVertices });
+	}
+
+	/// @see Shape::GetTrianglesNext
+	int				GetTrianglesNext(int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials)
+	{
+		JPH_ASSERT(inMaxTrianglesRequested >= Shape::cGetTrianglesMinTrianglesRequested);
+
+		int total_num_vertices = 0;
+		int max_vertices_requested = inMaxTrianglesRequested * 3;
+
+		// Loop over parts
+		for (; mCurrentPart < mParts.size(); ++mCurrentPart)
+		{
+			const Part &part = mParts[mCurrentPart];
+
+			// Calculate how many vertices to take from this part
+			int part_num_vertices = min(max_vertices_requested, int(part.mNumTriangleVertices - mCurrentVertex));
+			if (part_num_vertices == 0)
+				break;
+
+			max_vertices_requested -= part_num_vertices;
+			total_num_vertices += part_num_vertices;
+
+			if (mIsInsideOut)
+			{
+				// Store triangles flipped
+				for (const Vec3 *v = part.mTriangleVertices + mCurrentVertex, *v_end = v + part_num_vertices; v < v_end; v += 3)
+				{
+					(part.mLocalToWorld * v[0]).StoreFloat3(outTriangleVertices++);
+					(part.mLocalToWorld * v[2]).StoreFloat3(outTriangleVertices++);
+					(part.mLocalToWorld * v[1]).StoreFloat3(outTriangleVertices++);
+				}
+			}
+			else
+			{
+				// Store triangles
+				for (const Vec3 *v = part.mTriangleVertices + mCurrentVertex, *v_end = v + part_num_vertices; v < v_end; v += 3)
+				{
+					(part.mLocalToWorld * v[0]).StoreFloat3(outTriangleVertices++);
+					(part.mLocalToWorld * v[1]).StoreFloat3(outTriangleVertices++);
+					(part.mLocalToWorld * v[2]).StoreFloat3(outTriangleVertices++);
+				}
+			}
+
+			// Update the current vertex to point to the next vertex to get
+			mCurrentVertex += part_num_vertices;
+
+			// Check if we completed this part
+			if (mCurrentVertex < part.mNumTriangleVertices)
+				break;
+
+			// Reset current vertex for the next part
+			mCurrentVertex = 0;
+		}
+
+		int total_num_triangles = total_num_vertices / 3;
+
+		// Store materials
+		if (outMaterials != nullptr)
+			for (const PhysicsMaterial **m = outMaterials, **m_end = outMaterials + total_num_triangles; m < m_end; ++m)
+				*m = mMaterial;
+
+		return total_num_triangles;
+	}
+
+private:
+	struct Part
+	{
+		Mat44				mLocalToWorld;
+		const Vec3 *		mTriangleVertices;
+		size_t				mNumTriangleVertices;
+	};
+
+	StaticArray<Part, 3>	mParts;
+	uint					mCurrentPart = 0;
+	size_t					mCurrentVertex = 0;
+	const PhysicsMaterial *	mMaterial;
+	bool					mIsInsideOut;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/HeightFieldShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/HeightFieldShape.cpp
new file mode 100644
index 0000000000..fcecdaf92d
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/HeightFieldShape.cpp
@@ -0,0 +1,2714 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/Shape/HeightFieldShape.h>
+#include <Jolt/Physics/Collision/Shape/ConvexShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+#include <Jolt/Physics/Collision/ShapeFilter.h>
+#include <Jolt/Physics/Collision/CastConvexVsTriangles.h>
+#include <Jolt/Physics/Collision/CastSphereVsTriangles.h>
+#include <Jolt/Physics/Collision/CollideConvexVsTriangles.h>
+#include <Jolt/Physics/Collision/CollideSphereVsTriangles.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/ActiveEdges.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Physics/Collision/SortReverseAndStore.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h>
+#include <Jolt/Core/Profiler.h>
+#include <Jolt/Core/StringTools.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/Core/TempAllocator.h>
+#include <Jolt/Core/ScopeExit.h>
+#include <Jolt/Geometry/AABox4.h>
+#include <Jolt/Geometry/RayTriangle.h>
+#include <Jolt/Geometry/RayAABox.h>
+#include <Jolt/Geometry/OrientedBox.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+//#define JPH_DEBUG_HEIGHT_FIELD
+
+JPH_NAMESPACE_BEGIN
+
+#ifdef JPH_DEBUG_RENDERER
+bool HeightFieldShape::sDrawTriangleOutlines = false;
+#endif // JPH_DEBUG_RENDERER
+
+using namespace HeightFieldShapeConstants;
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(HeightFieldShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(HeightFieldShapeSettings, ShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mHeightSamples)
+	JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mOffset)
+	JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mScale)
+	JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMinHeightValue)
+	JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMaxHeightValue)
+	JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMaterialsCapacity)
+	JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mSampleCount)
+	JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mBlockSize)
+	JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mBitsPerSample)
+	JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMaterialIndices)
+	JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMaterials)
+	JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mActiveEdgeCosThresholdAngle)
+}
+
+const uint HeightFieldShape::sGridOffsets[] =
+{
+	0,			// level:  0, max x/y:     0, offset: 0
+	1,			// level:  1, max x/y:     1, offset: 1
+	5,			// level:  2, max x/y:     3, offset: 1 + 4
+	21,			// level:  3, max x/y:     7, offset: 1 + 4 + 16
+	85,			// level:  4, max x/y:    15, offset: 1 + 4 + 16 + 64
+	341,		// level:  5, max x/y:    31, offset: 1 + 4 + 16 + 64 + 256
+	1365,		// level:  6, max x/y:    63, offset: 1 + 4 + 16 + 64 + 256 + 1024
+	5461,		// level:  7, max x/y:   127, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096
+	21845,		// level:  8, max x/y:   255, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ...
+	87381,		// level:  9, max x/y:   511, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ...
+	349525,		// level: 10, max x/y:  1023, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ...
+	1398101,	// level: 11, max x/y:  2047, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ...
+	5592405,	// level: 12, max x/y:  4095, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ...
+	22369621,	// level: 13, max x/y:  8191, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ...
+	89478485,	// level: 14, max x/y: 16383, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ...
+};
+
+HeightFieldShapeSettings::HeightFieldShapeSettings(const float *inSamples, Vec3Arg inOffset, Vec3Arg inScale, uint32 inSampleCount, const uint8 *inMaterialIndices, const PhysicsMaterialList &inMaterialList) :
+	mOffset(inOffset),
+	mScale(inScale),
+	mSampleCount(inSampleCount)
+{
+	mHeightSamples.assign(inSamples, inSamples + Square(inSampleCount));
+
+	if (!inMaterialList.empty() && inMaterialIndices != nullptr)
+	{
+		mMaterialIndices.assign(inMaterialIndices, inMaterialIndices + Square(inSampleCount - 1));
+		mMaterials = inMaterialList;
+	}
+	else
+	{
+		JPH_ASSERT(inMaterialList.empty());
+		JPH_ASSERT(inMaterialIndices == nullptr);
+	}
+}
+
+ShapeSettings::ShapeResult HeightFieldShapeSettings::Create() const
+{
+	if (mCachedResult.IsEmpty())
+		Ref<Shape> shape = new HeightFieldShape(*this, mCachedResult);
+	return mCachedResult;
+}
+
+void HeightFieldShapeSettings::DetermineMinAndMaxSample(float &outMinValue, float &outMaxValue, float &outQuantizationScale) const
+{
+	// Determine min and max value
+	outMinValue = mMinHeightValue;
+	outMaxValue = mMaxHeightValue;
+	for (float h : mHeightSamples)
+		if (h != cNoCollisionValue)
+		{
+			outMinValue = min(outMinValue, h);
+			outMaxValue = max(outMaxValue, h);
+		}
+
+	// Prevent dividing by zero by setting a minimal height difference
+	float height_diff = max(outMaxValue - outMinValue, 1.0e-6f);
+
+	// Calculate the scale factor to quantize to 16 bits
+	outQuantizationScale = float(cMaxHeightValue16) / height_diff;
+}
+
+uint32 HeightFieldShapeSettings::CalculateBitsPerSampleForError(float inMaxError) const
+{
+	// Start with 1 bit per sample
+	uint32 bits_per_sample = 1;
+
+	// Determine total range
+	float min_value, max_value, scale;
+	DetermineMinAndMaxSample(min_value, max_value, scale);
+	if (min_value < max_value)
+	{
+		// Loop over all blocks
+		for (uint y = 0; y < mSampleCount; y += mBlockSize)
+			for (uint x = 0; x < mSampleCount; x += mBlockSize)
+			{
+				// Determine min and max block value + take 1 sample border just like we do while building the hierarchical grids
+				float block_min_value = FLT_MAX, block_max_value = -FLT_MAX;
+				for (uint bx = x; bx < min(x + mBlockSize + 1, mSampleCount); ++bx)
+					for (uint by = y; by < min(y + mBlockSize + 1, mSampleCount); ++by)
+					{
+						float h = mHeightSamples[by * mSampleCount + bx];
+						if (h != cNoCollisionValue)
+						{
+							block_min_value = min(block_min_value, h);
+							block_max_value = max(block_max_value, h);
+						}
+					}
+
+				if (block_min_value < block_max_value)
+				{
+					// Quantize then dequantize block min/max value
+					block_min_value = min_value + floor((block_min_value - min_value) * scale) / scale;
+					block_max_value = min_value + ceil((block_max_value - min_value) * scale) / scale;
+					float block_height = block_max_value - block_min_value;
+
+					// Loop over the block again
+					for (uint bx = x; bx < x + mBlockSize; ++bx)
+						for (uint by = y; by < y + mBlockSize; ++by)
+						{
+							// Get the height
+							float height = mHeightSamples[by * mSampleCount + bx];
+							if (height != cNoCollisionValue)
+							{
+								for (;;)
+								{
+									// Determine bitmask for sample
+									uint32 sample_mask = (1 << bits_per_sample) - 1;
+
+									// Quantize
+									float quantized_height = floor((height - block_min_value) * float(sample_mask) / block_height);
+									quantized_height = Clamp(quantized_height, 0.0f, float(sample_mask - 1));
+
+									// Dequantize and check error
+									float dequantized_height = block_min_value + (quantized_height + 0.5f) * block_height / float(sample_mask);
+									if (abs(dequantized_height - height) <= inMaxError)
+										break;
+
+									// Not accurate enough, increase bits per sample
+									bits_per_sample++;
+
+									// Don't go above 8 bits per sample
+									if (bits_per_sample == 8)
+										return bits_per_sample;
+								}
+							}
+						}
+				}
+			}
+
+	}
+
+	return bits_per_sample;
+}
+
+void HeightFieldShape::CalculateActiveEdges(uint inX, uint inY, uint inSizeX, uint inSizeY, const float *inHeights, uint inHeightsStartX, uint inHeightsStartY, intptr_t inHeightsStride, float inHeightsScale, float inActiveEdgeCosThresholdAngle, TempAllocator &inAllocator)
+{
+	// Allocate temporary buffer for normals
+	uint normals_size = 2 * inSizeX * inSizeY * sizeof(Vec3);
+	Vec3 *normals = (Vec3 *)inAllocator.Allocate(normals_size);
+	JPH_SCOPE_EXIT([&inAllocator, normals, normals_size]{ inAllocator.Free(normals, normals_size); });
+
+	// Calculate triangle normals and make normals zero for triangles that are missing
+	Vec3 *out_normal = normals;
+	for (uint y = 0; y < inSizeY; ++y)
+		for (uint x = 0; x < inSizeX; ++x)
+		{
+			// Get height on diagonal
+			const float *height_samples = inHeights + (inY - inHeightsStartY + y) * inHeightsStride + (inX - inHeightsStartX + x);
+			float x1y1_h = height_samples[0];
+			float x2y2_h = height_samples[inHeightsStride + 1];
+			if (x1y1_h != cNoCollisionValue && x2y2_h != cNoCollisionValue)
+			{
+				// Calculate normal for lower left triangle (e.g. T1A)
+				float x1y2_h = height_samples[inHeightsStride];
+				if (x1y2_h != cNoCollisionValue)
+				{
+					Vec3 x2y2_minus_x1y2(mScale.GetX(), inHeightsScale * (x2y2_h - x1y2_h), 0);
+					Vec3 x1y1_minus_x1y2(0, inHeightsScale * (x1y1_h - x1y2_h), -mScale.GetZ());
+					out_normal[0] = x2y2_minus_x1y2.Cross(x1y1_minus_x1y2).Normalized();
+				}
+				else
+					out_normal[0] = Vec3::sZero();
+
+				// Calculate normal for upper right triangle (e.g. T1B)
+				float x2y1_h = height_samples[1];
+				if (x2y1_h != cNoCollisionValue)
+				{
+					Vec3 x1y1_minus_x2y1(-mScale.GetX(), inHeightsScale * (x1y1_h - x2y1_h), 0);
+					Vec3 x2y2_minus_x2y1(0, inHeightsScale * (x2y2_h - x2y1_h), mScale.GetZ());
+					out_normal[1] = x1y1_minus_x2y1.Cross(x2y2_minus_x2y1).Normalized();
+				}
+				else
+					out_normal[1] = Vec3::sZero();
+			}
+			else
+			{
+				out_normal[0] = Vec3::sZero();
+				out_normal[1] = Vec3::sZero();
+			}
+
+			out_normal += 2;
+		}
+
+	// Calculate active edges
+	const Vec3 *in_normal = normals;
+	uint global_bit_pos = 3 * (inY * (mSampleCount - 1) + inX);
+	for (uint y = 0; y < inSizeY; ++y)
+	{
+		for (uint x = 0; x < inSizeX; ++x)
+		{
+			// Get vertex heights
+			const float *height_samples = inHeights + (inY - inHeightsStartY + y) * inHeightsStride + (inX - inHeightsStartX + x);
+			float x1y1_h = height_samples[0];
+			float x1y2_h = height_samples[inHeightsStride];
+			float x2y2_h = height_samples[inHeightsStride + 1];
+			bool x1y1_valid = x1y1_h != cNoCollisionValue;
+			bool x1y2_valid = x1y2_h != cNoCollisionValue;
+			bool x2y2_valid = x2y2_h != cNoCollisionValue;
+
+			// Calculate the edge flags (3 bits)
+			// See diagram in the next function for the edge numbering
+			uint16 edge_mask = 0b111;
+			uint16 edge_flags = 0;
+
+			// Edge 0
+			if (x == 0)
+				edge_mask &= 0b110; // We need normal x - 1 which we didn't calculate, don't update this edge
+			else if (x1y1_valid && x1y2_valid)
+			{
+				Vec3 edge0_direction(0, inHeightsScale * (x1y2_h - x1y1_h), mScale.GetZ());
+				if (ActiveEdges::IsEdgeActive(in_normal[0], in_normal[-1], edge0_direction, inActiveEdgeCosThresholdAngle))
+					edge_flags |= 0b001;
+			}
+
+			// Edge 1
+			if (y == inSizeY - 1)
+				edge_mask &= 0b101; // We need normal y + 1 which we didn't calculate, don't update this edge
+			else if (x1y2_valid && x2y2_valid)
+			{
+				Vec3 edge1_direction(mScale.GetX(), inHeightsScale * (x2y2_h - x1y2_h), 0);
+				if (ActiveEdges::IsEdgeActive(in_normal[0], in_normal[2 * inSizeX + 1], edge1_direction, inActiveEdgeCosThresholdAngle))
+					edge_flags |= 0b010;
+			}
+
+			// Edge 2
+			if (x1y1_valid && x2y2_valid)
+			{
+				Vec3 edge2_direction(-mScale.GetX(), inHeightsScale * (x1y1_h - x2y2_h), -mScale.GetZ());
+				if (ActiveEdges::IsEdgeActive(in_normal[0], in_normal[1], edge2_direction, inActiveEdgeCosThresholdAngle))
+					edge_flags |= 0b100;
+			}
+
+			// Store the edge flags in the array
+			uint byte_pos = global_bit_pos >> 3;
+			uint bit_pos = global_bit_pos & 0b111;
+			JPH_ASSERT(byte_pos < mActiveEdgesSize);
+			uint8 *edge_flags_ptr = &mActiveEdges[byte_pos];
+			uint16 combined_edge_flags = uint16(edge_flags_ptr[0]) | uint16(uint16(edge_flags_ptr[1]) << 8);
+			combined_edge_flags &= ~(edge_mask << bit_pos);
+			combined_edge_flags |= edge_flags << bit_pos;
+			edge_flags_ptr[0] = uint8(combined_edge_flags);
+			edge_flags_ptr[1] = uint8(combined_edge_flags >> 8);
+
+			in_normal += 2;
+			global_bit_pos += 3;
+		}
+
+		global_bit_pos += 3 * (mSampleCount - 1 - inSizeX);
+	}
+}
+
+void HeightFieldShape::CalculateActiveEdges(const HeightFieldShapeSettings &inSettings)
+{
+	/*
+		Store active edges. The triangles are organized like this:
+			x --->
+
+		y   +       +
+			| \ T1B | \ T2B
+		|  e0   e2  |   \
+		|   | T1A \ | T2A \
+		V   +--e1---+-------+
+			| \ T3B | \ T4B
+			|   \   |   \
+			| T3A \ | T4A \
+			+-------+-------+
+		We store active edges e0 .. e2 as bits 0 .. 2.
+		We store triangles horizontally then vertically (order T1A, T2A, T3A and T4A).
+		The top edge and right edge of the heightfield are always active so we do not need to store them,
+		therefore we only need to store (mSampleCount - 1)^2 * 3-bit
+		The triangles T1B, T2B, T3B and T4B do not need to be stored, their active edges can be constructed from adjacent triangles.
+		Add 1 byte padding so we can always read 1 uint16 to get the bits that cross an 8 bit boundary
+	*/
+
+	// Make all edges active (if mSampleCount is bigger than inSettings.mSampleCount we need to fill up the padding,
+	// also edges at x = 0 and y = inSettings.mSampleCount - 1 are not updated)
+	memset(mActiveEdges, 0xff, mActiveEdgesSize);
+
+	// Now clear the edges that are not active
+	TempAllocatorMalloc allocator;
+	CalculateActiveEdges(0, 0, inSettings.mSampleCount - 1, inSettings.mSampleCount - 1, inSettings.mHeightSamples.data(), 0, 0, inSettings.mSampleCount, inSettings.mScale.GetY(), inSettings.mActiveEdgeCosThresholdAngle, allocator);
+}
+
+void HeightFieldShape::StoreMaterialIndices(const HeightFieldShapeSettings &inSettings)
+{
+	// We need to account for any rounding of the sample count to the nearest block size
+	uint in_count_min_1 = inSettings.mSampleCount - 1;
+	uint out_count_min_1 = mSampleCount - 1;
+
+	mNumBitsPerMaterialIndex = 32 - CountLeadingZeros(max((uint32)mMaterials.size(), inSettings.mMaterialsCapacity) - 1);
+	mMaterialIndices.resize(((Square(out_count_min_1) * mNumBitsPerMaterialIndex + 7) >> 3) + 1, 0); // Add 1 byte so we don't read out of bounds when reading an uint16
+
+	if (mMaterials.size() > 1)
+		for (uint y = 0; y < out_count_min_1; ++y)
+			for (uint x = 0; x < out_count_min_1; ++x)
+			{
+				// Read material
+				uint16 material_index = x < in_count_min_1 && y < in_count_min_1? uint16(inSettings.mMaterialIndices[x + y * in_count_min_1]) : 0;
+
+				// Calculate byte and bit position where the material index needs to go
+				uint sample_pos = x + y * out_count_min_1;
+				uint bit_pos = sample_pos * mNumBitsPerMaterialIndex;
+				uint byte_pos = bit_pos >> 3;
+				bit_pos &= 0b111;
+
+				// Write the material index
+				material_index <<= bit_pos;
+				JPH_ASSERT(byte_pos + 1 < mMaterialIndices.size());
+				mMaterialIndices[byte_pos] |= uint8(material_index);
+				mMaterialIndices[byte_pos + 1] |= uint8(material_index >> 8);
+			}
+}
+
+void HeightFieldShape::CacheValues()
+{
+	mSampleMask = uint8((uint32(1) << mBitsPerSample) - 1);
+}
+
+void HeightFieldShape::AllocateBuffers()
+{
+	uint num_blocks = GetNumBlocks();
+	uint max_stride = (num_blocks + 1) >> 1;
+	mRangeBlocksSize = sGridOffsets[sGetMaxLevel(num_blocks) - 1] + Square(max_stride);
+	mHeightSamplesSize = (mSampleCount * mSampleCount * mBitsPerSample + 7) / 8 + 1;
+	mActiveEdgesSize = (Square(mSampleCount - 1) * 3 + 7) / 8 + 1; // See explanation at HeightFieldShape::CalculateActiveEdges
+
+	JPH_ASSERT(mRangeBlocks == nullptr && mHeightSamples == nullptr && mActiveEdges == nullptr);
+	void *data = AlignedAllocate(mRangeBlocksSize * sizeof(RangeBlock) + mHeightSamplesSize + mActiveEdgesSize, alignof(RangeBlock));
+	mRangeBlocks = reinterpret_cast<RangeBlock *>(data);
+	mHeightSamples = reinterpret_cast<uint8 *>(mRangeBlocks + mRangeBlocksSize);
+	mActiveEdges = mHeightSamples + mHeightSamplesSize;
+}
+
+HeightFieldShape::HeightFieldShape(const HeightFieldShapeSettings &inSettings, ShapeResult &outResult) :
+	Shape(EShapeType::HeightField, EShapeSubType::HeightField, inSettings, outResult),
+	mOffset(inSettings.mOffset),
+	mScale(inSettings.mScale),
+	mSampleCount(((inSettings.mSampleCount + inSettings.mBlockSize - 1) / inSettings.mBlockSize) * inSettings.mBlockSize), // Round sample count to nearest block size
+	mBlockSize(inSettings.mBlockSize),
+	mBitsPerSample(uint8(inSettings.mBitsPerSample))
+{
+	CacheValues();
+
+	// Reserve a bigger materials list if requested
+	if (inSettings.mMaterialsCapacity > 0)
+		mMaterials.reserve(inSettings.mMaterialsCapacity);
+	mMaterials = inSettings.mMaterials;
+
+	// Check block size
+	if (mBlockSize < 2 || mBlockSize > 8)
+	{
+		outResult.SetError("HeightFieldShape: Block size must be in the range [2, 8]!");
+		return;
+	}
+
+	// Check bits per sample
+	if (inSettings.mBitsPerSample < 1 || inSettings.mBitsPerSample > 8)
+	{
+		outResult.SetError("HeightFieldShape: Bits per sample must be in the range [1, 8]!");
+		return;
+	}
+
+	// We stop at mBlockSize x mBlockSize height sample blocks
+	uint num_blocks = GetNumBlocks();
+
+	// We want at least 1 grid layer
+	if (num_blocks < 2)
+	{
+		outResult.SetError("HeightFieldShape: Sample count too low!");
+		return;
+	}
+
+	// Check that we don't overflow our 32 bit 'properties'
+	if (num_blocks > (1 << cNumBitsXY))
+	{
+		outResult.SetError("HeightFieldShape: Sample count too high!");
+		return;
+	}
+
+	// Check if we're not exceeding the amount of sub shape id bits
+	if (GetSubShapeIDBitsRecursive() > SubShapeID::MaxBits)
+	{
+		outResult.SetError("HeightFieldShape: Size exceeds the amount of available sub shape ID bits!");
+		return;
+	}
+
+	if (!mMaterials.empty())
+	{
+		// Validate materials
+		if (mMaterials.size() > 256)
+		{
+			outResult.SetError("Supporting max 256 materials per height field");
+			return;
+		}
+		for (uint8 s : inSettings.mMaterialIndices)
+			if (s >= mMaterials.size())
+			{
+				outResult.SetError(StringFormat("Material %u is beyond material list (size: %u)", s, (uint)mMaterials.size()));
+				return;
+			}
+	}
+	else
+	{
+		// No materials assigned, validate that no materials have been specified
+		if (!inSettings.mMaterialIndices.empty())
+		{
+			outResult.SetError("No materials present, mMaterialIndices should be empty");
+			return;
+		}
+	}
+
+	// Determine range
+	float min_value, max_value, scale;
+	inSettings.DetermineMinAndMaxSample(min_value, max_value, scale);
+	if (min_value > max_value)
+	{
+		// If there is no collision with this heightmap, leave everything empty
+		mMaterials.clear();
+		outResult.Set(this);
+		return;
+	}
+
+	// Allocate space for this shape
+	AllocateBuffers();
+
+	// Quantize to uint16
+	Array<uint16> quantized_samples;
+	quantized_samples.reserve(mSampleCount * mSampleCount);
+	for (uint y = 0; y < inSettings.mSampleCount; ++y)
+	{
+		for (uint x = 0; x < inSettings.mSampleCount; ++x)
+		{
+			float h = inSettings.mHeightSamples[x + y * inSettings.mSampleCount];
+			if (h == cNoCollisionValue)
+			{
+				quantized_samples.push_back(cNoCollisionValue16);
+			}
+			else
+			{
+				// Floor the quantized height to get a lower bound for the quantized value
+				int quantized_height = (int)floor(scale * (h - min_value));
+
+				// Ensure that the height says below the max height value so we can safely add 1 to get the upper bound for the quantized value
+				quantized_height = Clamp(quantized_height, 0, int(cMaxHeightValue16 - 1));
+
+				quantized_samples.push_back(uint16(quantized_height));
+			}
+		}
+		// Pad remaining columns with no collision
+		for (uint x = inSettings.mSampleCount; x < mSampleCount; ++x)
+			quantized_samples.push_back(cNoCollisionValue16);
+	}
+	// Pad remaining rows with no collision
+	for (uint y = inSettings.mSampleCount; y < mSampleCount; ++y)
+		for (uint x = 0; x < mSampleCount; ++x)
+			quantized_samples.push_back(cNoCollisionValue16);
+
+	// Update offset and scale to account for the compression to uint16
+	if (min_value <= max_value) // Only when there was collision
+	{
+		// In GetPosition we always add 0.5 to the quantized sample in order to reduce the average error.
+		// We want to be able to exactly quantize min_value (this is important in case the heightfield is entirely flat) so we subtract that value from min_value.
+		min_value -= 0.5f / (scale * mSampleMask);
+
+		mOffset.SetY(mOffset.GetY() + mScale.GetY() * min_value);
+	}
+	mScale.SetY(mScale.GetY() / scale);
+
+	// Calculate amount of grids
+	uint max_level = sGetMaxLevel(num_blocks);
+
+	// Temporary data structure used during creating of a hierarchy of grids
+	struct Range
+	{
+		uint16	mMin;
+		uint16	mMax;
+	};
+
+	// Reserve size for temporary range data + reserve 1 extra for a 1x1 grid that we won't store but use for calculating the bounding box
+	Array<Array<Range>> ranges;
+	ranges.resize(max_level + 1);
+
+	// Calculate highest detail grid by combining mBlockSize x mBlockSize height samples
+	Array<Range> *cur_range_vector = &ranges.back();
+	uint num_blocks_pow2 = GetNextPowerOf2(num_blocks); // We calculate the range blocks as if the heightfield was a power of 2, when we save the range blocks we'll ignore the extra samples (this makes downsampling easier)
+	cur_range_vector->resize(num_blocks_pow2 * num_blocks_pow2);
+	Range *range_dst = &cur_range_vector->front();
+	for (uint y = 0; y < num_blocks_pow2; ++y)
+		for (uint x = 0; x < num_blocks_pow2; ++x)
+		{
+			range_dst->mMin = 0xffff;
+			range_dst->mMax = 0;
+			uint max_bx = x == num_blocks_pow2 - 1? mBlockSize : mBlockSize + 1; // for interior blocks take 1 more because the triangles connect to the next block so we must include their height too
+			uint max_by = y == num_blocks_pow2 - 1? mBlockSize : mBlockSize + 1;
+			for (uint by = 0; by < max_by; ++by)
+				for (uint bx = 0; bx < max_bx; ++bx)
+				{
+					uint sx = x * mBlockSize + bx;
+					uint sy = y * mBlockSize + by;
+					if (sx < mSampleCount && sy < mSampleCount)
+					{
+						uint16 h = quantized_samples[sy * mSampleCount + sx];
+						if (h != cNoCollisionValue16)
+						{
+							range_dst->mMin = min(range_dst->mMin, h);
+							range_dst->mMax = max(range_dst->mMax, uint16(h + 1)); // Add 1 to the max so we know the real value is between mMin and mMax
+						}
+					}
+				}
+			++range_dst;
+		}
+
+	// Calculate remaining grids
+	for (uint n = num_blocks_pow2 >> 1; n >= 1; n >>= 1)
+	{
+		// Get source buffer
+		const Range *range_src = &cur_range_vector->front();
+
+		// Previous array element
+		--cur_range_vector;
+
+		// Make space for this grid
+		cur_range_vector->resize(n * n);
+
+		// Get target buffer
+		range_dst = &cur_range_vector->front();
+
+		// Combine the results of 2x2 ranges
+		for (uint y = 0; y < n; ++y)
+			for (uint x = 0; x < n; ++x)
+			{
+				range_dst->mMin = 0xffff;
+				range_dst->mMax = 0;
+				for (uint by = 0; by < 2; ++by)
+					for (uint bx = 0; bx < 2; ++bx)
+					{
+						const Range &r = range_src[(y * 2 + by) * n * 2 + x * 2 + bx];
+						range_dst->mMin = min(range_dst->mMin, r.mMin);
+						range_dst->mMax = max(range_dst->mMax, r.mMax);
+					}
+				++range_dst;
+			}
+	}
+	JPH_ASSERT(cur_range_vector == &ranges.front());
+
+	// Store global range for bounding box calculation
+	mMinSample = ranges[0][0].mMin;
+	mMaxSample = ranges[0][0].mMax;
+
+#ifdef JPH_ENABLE_ASSERTS
+	// Validate that we did not lose range along the way
+	uint16 minv = 0xffff, maxv = 0;
+	for (uint16 v : quantized_samples)
+		if (v != cNoCollisionValue16)
+		{
+			minv = min(minv, v);
+			maxv = max(maxv, uint16(v + 1));
+		}
+	JPH_ASSERT(mMinSample == minv && mMaxSample == maxv);
+#endif
+
+	// Now erase the first element, we need a 2x2 grid to start with
+	ranges.erase(ranges.begin());
+
+	// Create blocks
+	uint max_stride = (num_blocks + 1) >> 1;
+	RangeBlock *current_block = mRangeBlocks;
+	for (uint level = 0; level < ranges.size(); ++level)
+	{
+		JPH_ASSERT(uint(current_block - mRangeBlocks) == sGridOffsets[level]);
+
+		uint in_n = 1 << level;
+		uint out_n = min(in_n, max_stride); // At the most detailed level we store a non-power of 2 number of blocks
+
+		for (uint y = 0; y < out_n; ++y)
+			for (uint x = 0; x < out_n; ++x)
+			{
+				// Convert from 2x2 Range structure to 1 RangeBlock structure
+				RangeBlock &rb = *current_block++;
+				for (uint by = 0; by < 2; ++by)
+					for (uint bx = 0; bx < 2; ++bx)
+					{
+						uint src_pos = (y * 2 + by) * 2 * in_n + (x * 2 + bx);
+						uint dst_pos = by * 2 + bx;
+						rb.mMin[dst_pos] = ranges[level][src_pos].mMin;
+						rb.mMax[dst_pos] = ranges[level][src_pos].mMax;
+					}
+			}
+	}
+	JPH_ASSERT(uint32(current_block - mRangeBlocks) == mRangeBlocksSize);
+
+	// Quantize height samples
+	memset(mHeightSamples, 0, mHeightSamplesSize);
+	int sample = 0;
+	for (uint y = 0; y < mSampleCount; ++y)
+		for (uint x = 0; x < mSampleCount; ++x)
+		{
+			uint32 output_value;
+
+			float h = x < inSettings.mSampleCount && y < inSettings.mSampleCount? inSettings.mHeightSamples[x + y * inSettings.mSampleCount] : cNoCollisionValue;
+			if (h == cNoCollisionValue)
+			{
+				// No collision
+				output_value = mSampleMask;
+			}
+			else
+			{
+				// Get range of block so we know what range to compress to
+				uint bx = x / mBlockSize;
+				uint by = y / mBlockSize;
+				const Range &range = ranges.back()[by * num_blocks_pow2 + bx];
+				JPH_ASSERT(range.mMin < range.mMax);
+
+				// Quantize to mBitsPerSample bits, note that mSampleMask is reserved for indicating that there's no collision.
+				// We divide the range into mSampleMask segments and use the mid points of these segments as the quantized values.
+				// This results in a lower error than if we had quantized our data using the lowest point of all these segments.
+				float h_min = min_value + range.mMin / scale;
+				float h_delta = float(range.mMax - range.mMin) / scale;
+				float quantized_height = floor((h - h_min) * float(mSampleMask) / h_delta);
+				output_value = uint32(Clamp((int)quantized_height, 0, int(mSampleMask) - 1)); // mSampleMask is reserved as 'no collision value'
+			}
+
+			// Store the sample
+			uint byte_pos = sample >> 3;
+			uint bit_pos = sample & 0b111;
+			output_value <<= bit_pos;
+			JPH_ASSERT(byte_pos + 1 < mHeightSamplesSize);
+			mHeightSamples[byte_pos] |= uint8(output_value);
+			mHeightSamples[byte_pos + 1] |= uint8(output_value >> 8);
+			sample += inSettings.mBitsPerSample;
+		}
+
+	// Calculate the active edges
+	CalculateActiveEdges(inSettings);
+
+	// Compress material indices
+	if (mMaterials.size() > 1 || inSettings.mMaterialsCapacity > 1)
+		StoreMaterialIndices(inSettings);
+
+	outResult.Set(this);
+}
+
+HeightFieldShape::~HeightFieldShape()
+{
+	if (mRangeBlocks != nullptr)
+		AlignedFree(mRangeBlocks);
+}
+
+Ref<HeightFieldShape> HeightFieldShape::Clone() const
+{
+	Ref<HeightFieldShape> clone = new HeightFieldShape;
+	clone->SetUserData(GetUserData());
+
+	clone->mOffset = mOffset;
+	clone->mScale = mScale;
+	clone->mSampleCount = mSampleCount;
+	clone->mBlockSize = mBlockSize;
+	clone->mBitsPerSample = mBitsPerSample;
+	clone->mSampleMask = mSampleMask;
+	clone->mMinSample = mMinSample;
+	clone->mMaxSample = mMaxSample;
+
+	clone->AllocateBuffers();
+	memcpy(clone->mRangeBlocks, mRangeBlocks, mRangeBlocksSize * sizeof(RangeBlock) + mHeightSamplesSize + mActiveEdgesSize); // Copy the entire buffer in 1 go
+
+	clone->mMaterials.reserve(mMaterials.capacity()); // Ensure we keep the capacity of the original
+	clone->mMaterials = mMaterials;
+	clone->mMaterialIndices = mMaterialIndices;
+	clone->mNumBitsPerMaterialIndex = mNumBitsPerMaterialIndex;
+
+#ifdef JPH_DEBUG_RENDERER
+	clone->mGeometry = mGeometry;
+	clone->mCachedUseMaterialColors = mCachedUseMaterialColors;
+#endif // JPH_DEBUG_RENDERER
+
+	return clone;
+}
+
+inline void HeightFieldShape::sGetRangeBlockOffsetAndStride(uint inNumBlocks, uint inMaxLevel, uint &outRangeBlockOffset, uint &outRangeBlockStride)
+{
+	outRangeBlockOffset = sGridOffsets[inMaxLevel - 1];
+	outRangeBlockStride = (inNumBlocks + 1) >> 1;
+}
+
+inline void HeightFieldShape::GetRangeBlock(uint inBlockX, uint inBlockY, uint inRangeBlockOffset, uint inRangeBlockStride, RangeBlock *&outBlock, uint &outIndexInBlock)
+{
+	JPH_ASSERT(inBlockX < GetNumBlocks() && inBlockY < GetNumBlocks());
+
+	// Convert to location of range block
+	uint rbx = inBlockX >> 1;
+	uint rby = inBlockY >> 1;
+	outIndexInBlock = ((inBlockY & 1) << 1) + (inBlockX & 1);
+
+	uint offset = inRangeBlockOffset + rby * inRangeBlockStride + rbx;
+	JPH_ASSERT(offset < mRangeBlocksSize);
+	outBlock = mRangeBlocks + offset;
+}
+
+inline void HeightFieldShape::GetBlockOffsetAndScale(uint inBlockX, uint inBlockY, uint inRangeBlockOffset, uint inRangeBlockStride, float &outBlockOffset, float &outBlockScale) const
+{
+	JPH_ASSERT(inBlockX < GetNumBlocks() && inBlockY < GetNumBlocks());
+
+	// Convert to location of range block
+	uint rbx = inBlockX >> 1;
+	uint rby = inBlockY >> 1;
+	uint n = ((inBlockY & 1) << 1) + (inBlockX & 1);
+
+	// Calculate offset and scale
+	uint offset = inRangeBlockOffset + rby * inRangeBlockStride + rbx;
+	JPH_ASSERT(offset < mRangeBlocksSize);
+	const RangeBlock &block = mRangeBlocks[offset];
+	outBlockOffset = float(block.mMin[n]);
+	outBlockScale = float(block.mMax[n] - block.mMin[n]) / float(mSampleMask);
+}
+
+inline uint8 HeightFieldShape::GetHeightSample(uint inX, uint inY) const
+{
+	JPH_ASSERT(inX < mSampleCount);
+	JPH_ASSERT(inY < mSampleCount);
+
+	// Determine bit position of sample
+	uint sample = (inY * mSampleCount + inX) * uint(mBitsPerSample);
+	uint byte_pos = sample >> 3;
+	uint bit_pos = sample & 0b111;
+
+	// Fetch the height sample value
+	JPH_ASSERT(byte_pos + 1 < mHeightSamplesSize);
+	const uint8 *height_samples = mHeightSamples + byte_pos;
+	uint16 height_sample = uint16(height_samples[0]) | uint16(uint16(height_samples[1]) << 8);
+	return uint8(height_sample >> bit_pos) & mSampleMask;
+}
+
+inline Vec3 HeightFieldShape::GetPosition(uint inX, uint inY, float inBlockOffset, float inBlockScale, bool &outNoCollision) const
+{
+	// Get quantized value
+	uint8 height_sample = GetHeightSample(inX, inY);
+	outNoCollision = height_sample == mSampleMask;
+
+	// Add 0.5 to the quantized value to minimize the error (see constructor)
+	return mOffset + mScale * Vec3(float(inX), inBlockOffset + (0.5f + height_sample) * inBlockScale, float(inY));
+}
+
+Vec3 HeightFieldShape::GetPosition(uint inX, uint inY) const
+{
+	// Test if there are any samples
+	if (mHeightSamplesSize == 0)
+		return mOffset + mScale * Vec3(float(inX), 0.0f, float(inY));
+
+	// Get block location
+	uint bx = inX / mBlockSize;
+	uint by = inY / mBlockSize;
+
+	// Calculate offset and stride
+	uint num_blocks = GetNumBlocks();
+	uint range_block_offset, range_block_stride;
+	sGetRangeBlockOffsetAndStride(num_blocks, sGetMaxLevel(num_blocks), range_block_offset, range_block_stride);
+
+	float offset, scale;
+	GetBlockOffsetAndScale(bx, by, range_block_offset, range_block_stride, offset, scale);
+
+	bool no_collision;
+	return GetPosition(inX, inY, offset, scale, no_collision);
+}
+
+bool HeightFieldShape::IsNoCollision(uint inX, uint inY) const
+{
+	return mHeightSamplesSize == 0 || GetHeightSample(inX, inY) == mSampleMask;
+}
+
+bool HeightFieldShape::ProjectOntoSurface(Vec3Arg inLocalPosition, Vec3 &outSurfacePosition, SubShapeID &outSubShapeID) const
+{
+	// Check if we have collision
+	if (mHeightSamplesSize == 0)
+		return false;
+
+	// Convert coordinate to integer space
+	Vec3 integer_space = (inLocalPosition - mOffset) / mScale;
+
+	// Get x coordinate and fraction
+	float x_frac = integer_space.GetX();
+	if (x_frac < 0.0f || x_frac >= mSampleCount - 1)
+		return false;
+	uint x = (uint)floor(x_frac);
+	x_frac -= x;
+
+	// Get y coordinate and fraction
+	float y_frac = integer_space.GetZ();
+	if (y_frac < 0.0f || y_frac >= mSampleCount - 1)
+		return false;
+	uint y = (uint)floor(y_frac);
+	y_frac -= y;
+
+	// If one of the diagonal points doesn't have collision, we don't have a height at this location
+	if (IsNoCollision(x, y) || IsNoCollision(x + 1, y + 1))
+		return false;
+
+	if (y_frac >= x_frac)
+	{
+		// Left bottom triangle, test the 3rd point
+		if (IsNoCollision(x, y + 1))
+			return false;
+
+		// Interpolate height value
+		Vec3 v1 = GetPosition(x, y);
+		Vec3 v2 = GetPosition(x, y + 1);
+		Vec3 v3 = GetPosition(x + 1, y + 1);
+		outSurfacePosition = v1 + y_frac * (v2 - v1) + x_frac * (v3 - v2);
+		SubShapeIDCreator creator;
+		outSubShapeID = EncodeSubShapeID(creator, x, y, 0);
+		return true;
+	}
+	else
+	{
+		// Right top triangle, test the third point
+		if (IsNoCollision(x + 1, y))
+			return false;
+
+		// Interpolate height value
+		Vec3 v1 = GetPosition(x, y);
+		Vec3 v2 = GetPosition(x + 1, y + 1);
+		Vec3 v3 = GetPosition(x + 1, y);
+		outSurfacePosition = v1 + y_frac * (v2 - v3) + x_frac * (v3 - v1);
+		SubShapeIDCreator creator;
+		outSubShapeID = EncodeSubShapeID(creator, x, y, 1);
+		return true;
+	}
+}
+
+void HeightFieldShape::GetHeights(uint inX, uint inY, uint inSizeX, uint inSizeY, float *outHeights, intptr_t inHeightsStride) const
+{
+	if (inSizeX == 0 || inSizeY == 0)
+		return;
+
+	JPH_ASSERT(inX % mBlockSize == 0 && inY % mBlockSize == 0);
+	JPH_ASSERT(inX < mSampleCount && inY < mSampleCount);
+	JPH_ASSERT(inX + inSizeX <= mSampleCount && inY + inSizeY <= mSampleCount);
+
+	// Test if there are any samples
+	if (mHeightSamplesSize == 0)
+	{
+		// No samples, return the offset
+		float offset = mOffset.GetY();
+		for (uint y = 0; y < inSizeY; ++y, outHeights += inHeightsStride)
+			for (uint x = 0; x < inSizeX; ++x)
+				outHeights[x] = offset;
+	}
+	else
+	{
+		// Calculate offset and stride
+		uint num_blocks = GetNumBlocks();
+		uint range_block_offset, range_block_stride;
+		sGetRangeBlockOffsetAndStride(num_blocks, sGetMaxLevel(num_blocks), range_block_offset, range_block_stride);
+
+		// Loop over blocks
+		uint block_start_x = inX / mBlockSize;
+		uint block_start_y = inY / mBlockSize;
+		uint num_blocks_x = inSizeX / mBlockSize;
+		uint num_blocks_y = inSizeY / mBlockSize;
+		for (uint block_y = 0; block_y < num_blocks_y; ++block_y)
+			for (uint block_x = 0; block_x < num_blocks_x; ++block_x)
+			{
+				// Get offset and scale for block
+				float offset, scale;
+				GetBlockOffsetAndScale(block_start_x + block_x, block_start_y + block_y, range_block_offset, range_block_stride, offset, scale);
+
+				// Adjust by global offset and scale
+				// Note: This is the math applied in GetPosition() written out to reduce calculations in the inner loop
+				scale *= mScale.GetY();
+				offset = mOffset.GetY() + mScale.GetY() * offset + 0.5f * scale;
+
+				// Loop over samples in block
+				for (uint sample_y = 0; sample_y < mBlockSize; ++sample_y)
+					for (uint sample_x = 0; sample_x < mBlockSize; ++sample_x)
+					{
+						// Calculate output coordinate
+						uint output_x = block_x * mBlockSize + sample_x;
+						uint output_y = block_y * mBlockSize + sample_y;
+
+						// Get quantized value
+						uint8 height_sample = GetHeightSample(inX + output_x, inY + output_y);
+
+						// Dequantize
+						float h = height_sample != mSampleMask? offset + height_sample * scale : cNoCollisionValue;
+						outHeights[output_y * inHeightsStride + output_x] = h;
+					}
+			}
+	}
+}
+
+void HeightFieldShape::SetHeights(uint inX, uint inY, uint inSizeX, uint inSizeY, const float *inHeights, intptr_t inHeightsStride, TempAllocator &inAllocator, float inActiveEdgeCosThresholdAngle)
+{
+	if (inSizeX == 0 || inSizeY == 0)
+		return;
+
+	JPH_ASSERT(mHeightSamplesSize > 0);
+	JPH_ASSERT(inX % mBlockSize == 0 && inY % mBlockSize == 0);
+	JPH_ASSERT(inX < mSampleCount && inY < mSampleCount);
+	JPH_ASSERT(inX + inSizeX <= mSampleCount && inY + inSizeY <= mSampleCount);
+
+	// If we have a block in negative x/y direction, we will affect its range so we need to take it into account
+	bool need_temp_heights = false;
+	uint affected_x = inX;
+	uint affected_y = inY;
+	uint affected_size_x = inSizeX;
+	uint affected_size_y = inSizeY;
+	if (inX > 0) { affected_x -= mBlockSize; affected_size_x += mBlockSize; need_temp_heights = true; }
+	if (inY > 0) { affected_y -= mBlockSize; affected_size_y += mBlockSize; need_temp_heights = true; }
+
+	// If we have a block in positive x/y direction, our ranges are affected by it so we need to take it into account
+	uint heights_size_x = affected_size_x;
+	uint heights_size_y = affected_size_y;
+	if (inX + inSizeX < mSampleCount) { heights_size_x += mBlockSize; need_temp_heights = true; }
+	if (inY + inSizeY < mSampleCount) { heights_size_y += mBlockSize; need_temp_heights = true; }
+
+	// Get heights for affected area
+	const float *heights;
+	intptr_t heights_stride;
+	float *temp_heights;
+	if (need_temp_heights)
+	{
+		// Fetch the surrounding height data (note we're forced to recompress this data with a potentially different range so there will be some precision loss here)
+		temp_heights = (float *)inAllocator.Allocate(heights_size_x * heights_size_y * sizeof(float));
+		heights = temp_heights;
+		heights_stride = heights_size_x;
+
+		// We need to fill in the following areas:
+		//
+		// +-----------------+
+		// |        2        |
+		// |---+---------+---|
+		// |   |         |   |
+		// | 3 |    1    | 4 |
+		// |   |         |   |
+		// |---+---------+---|
+		// |        5        |
+		// +-----------------+
+		//
+		// 1. The area that is affected by the new heights (we just copy these)
+		// 2-5. These areas are either needed to calculate the range of the affected blocks or they need to be recompressed with a different range
+		uint offset_x = inX - affected_x;
+		uint offset_y = inY - affected_y;
+
+		// Area 2
+		GetHeights(affected_x, affected_y, heights_size_x, offset_y, temp_heights, heights_size_x);
+		float *area3_start = temp_heights + offset_y * heights_size_x;
+
+		// Area 3
+		GetHeights(affected_x, inY, offset_x, inSizeY, area3_start, heights_size_x);
+
+		// Area 1
+		float *area1_start = area3_start + offset_x;
+		for (uint y = 0; y < inSizeY; ++y, area1_start += heights_size_x, inHeights += inHeightsStride)
+			memcpy(area1_start, inHeights, inSizeX * sizeof(float));
+
+		// Area 4
+		uint area4_x = inX + inSizeX;
+		GetHeights(area4_x, inY, affected_x + heights_size_x - area4_x, inSizeY, area3_start + area4_x - affected_x, heights_size_x);
+
+		// Area 5
+		uint area5_y = inY + inSizeY;
+		float *area5_start = temp_heights + (area5_y - affected_y) * heights_size_x;
+		GetHeights(affected_x, area5_y, heights_size_x, affected_y + heights_size_y - area5_y, area5_start, heights_size_x);
+	}
+	else
+	{
+		// We can directly use the input buffer because there are no extra edges to take into account
+		heights = inHeights;
+		heights_stride = inHeightsStride;
+		temp_heights = nullptr;
+	}
+
+	// Calculate offset and stride
+	uint num_blocks = GetNumBlocks();
+	uint range_block_offset, range_block_stride;
+	uint max_level = sGetMaxLevel(num_blocks);
+	sGetRangeBlockOffsetAndStride(num_blocks, max_level, range_block_offset, range_block_stride);
+
+	// Loop over blocks
+	uint block_start_x = affected_x / mBlockSize;
+	uint block_start_y = affected_y / mBlockSize;
+	uint num_blocks_x = affected_size_x / mBlockSize;
+	uint num_blocks_y = affected_size_y / mBlockSize;
+	for (uint block_y = 0, sample_start_y = 0; block_y < num_blocks_y; ++block_y, sample_start_y += mBlockSize)
+		for (uint block_x = 0, sample_start_x = 0; block_x < num_blocks_x; ++block_x, sample_start_x += mBlockSize)
+		{
+			// Determine quantized min and max value for block
+			// Note that we need to include 1 extra row in the positive x/y direction to account for connecting triangles
+			int min_value = 0xffff;
+			int max_value = 0;
+			uint sample_x_end = min(sample_start_x + mBlockSize + 1, mSampleCount - affected_x);
+			uint sample_y_end = min(sample_start_y + mBlockSize + 1, mSampleCount - affected_y);
+			for (uint sample_y = sample_start_y; sample_y < sample_y_end; ++sample_y)
+				for (uint sample_x = sample_start_x; sample_x < sample_x_end; ++sample_x)
+				{
+					float h = heights[sample_y * heights_stride + sample_x];
+					if (h != cNoCollisionValue)
+					{
+						int quantized_height = Clamp((int)floor((h - mOffset.GetY()) / mScale.GetY()), 0, int(cMaxHeightValue16 - 1));
+						min_value = min(min_value, quantized_height);
+						max_value = max(max_value, quantized_height + 1);
+					}
+				}
+			if (min_value > max_value)
+				min_value = max_value = cNoCollisionValue16;
+
+			// Update range for block
+			RangeBlock *range_block;
+			uint index_in_block;
+			GetRangeBlock(block_start_x + block_x, block_start_y + block_y, range_block_offset, range_block_stride, range_block, index_in_block);
+			range_block->mMin[index_in_block] = uint16(min_value);
+			range_block->mMax[index_in_block] = uint16(max_value);
+
+			// Get offset and scale for block
+			float offset_block = float(min_value);
+			float scale_block = float(max_value - min_value) / float(mSampleMask);
+
+			// Calculate scale and offset using the formula used in GetPosition() solved for the quantized height (excluding 0.5 because we round down while quantizing)
+			float scale = scale_block * mScale.GetY();
+			float offset = mOffset.GetY() + offset_block * mScale.GetY();
+
+			// Loop over samples in block
+			sample_x_end = sample_start_x + mBlockSize;
+			sample_y_end = sample_start_y + mBlockSize;
+			for (uint sample_y = sample_start_y; sample_y < sample_y_end; ++sample_y)
+				for (uint sample_x = sample_start_x; sample_x < sample_x_end; ++sample_x)
+				{
+					// Quantize height
+					float h = heights[sample_y * heights_stride + sample_x];
+					uint8 quantized_height = h != cNoCollisionValue? uint8(Clamp((int)floor((h - offset) / scale), 0, int(mSampleMask) - 1)) : mSampleMask;
+
+					// Determine bit position of sample
+					uint sample = ((affected_y + sample_y) * mSampleCount + affected_x + sample_x) * uint(mBitsPerSample);
+					uint byte_pos = sample >> 3;
+					uint bit_pos = sample & 0b111;
+
+					// Update the height value sample
+					JPH_ASSERT(byte_pos + 1 < mHeightSamplesSize);
+					uint8 *height_samples = mHeightSamples + byte_pos;
+					uint16 height_sample = uint16(height_samples[0]) | uint16(uint16(height_samples[1]) << 8);
+					height_sample &= ~(uint16(mSampleMask) << bit_pos);
+					height_sample |= uint16(quantized_height) << bit_pos;
+					height_samples[0] = uint8(height_sample);
+					height_samples[1] = uint8(height_sample >> 8);
+				}
+		}
+
+	// Update active edges
+	// Note that we must take an extra row on all sides to account for connecting triangles
+	uint ae_x = inX > 1? inX - 2 : 0;
+	uint ae_y = inY > 1? inY - 2 : 0;
+	uint ae_sx = min(inX + inSizeX + 1, mSampleCount - 1) - ae_x;
+	uint ae_sy = min(inY + inSizeY + 1, mSampleCount - 1) - ae_y;
+	CalculateActiveEdges(ae_x, ae_y, ae_sx, ae_sy, heights, affected_x, affected_y, heights_stride, 1.0f, inActiveEdgeCosThresholdAngle, inAllocator);
+
+	// Free temporary buffer
+	if (temp_heights != nullptr)
+		inAllocator.Free(temp_heights, heights_size_x * heights_size_y * sizeof(float));
+
+	// Update hierarchy of range blocks
+	while (max_level > 1)
+	{
+		// Get offset and stride for destination blocks
+		uint dst_range_block_offset, dst_range_block_stride;
+		sGetRangeBlockOffsetAndStride(num_blocks >> 1, max_level - 1, dst_range_block_offset, dst_range_block_stride);
+
+		// We'll be processing 2x2 blocks below so we need the start coordinates to be even and we extend the number of blocks to correct for that
+		if (block_start_x & 1) { --block_start_x; ++num_blocks_x; }
+		if (block_start_y & 1) { --block_start_y; ++num_blocks_y; }
+
+		// Loop over all affected blocks
+		uint block_end_x = block_start_x + num_blocks_x;
+		uint block_end_y = block_start_y + num_blocks_y;
+		for (uint block_y = block_start_y; block_y < block_end_y; block_y += 2)
+			for (uint block_x = block_start_x; block_x < block_end_x; block_x += 2)
+			{
+				// Get source range block
+				RangeBlock *src_range_block;
+				uint index_in_src_block;
+				GetRangeBlock(block_x, block_y, range_block_offset, range_block_stride, src_range_block, index_in_src_block);
+
+				// Determine quantized min and max value for the entire 2x2 block
+				uint16 min_value = 0xffff;
+				uint16 max_value = 0;
+				for (uint i = 0; i < 4; ++i)
+					if (src_range_block->mMin[i] != cNoCollisionValue16)
+					{
+						min_value = min(min_value, src_range_block->mMin[i]);
+						max_value = max(max_value, src_range_block->mMax[i]);
+					}
+
+				// Write to destination block
+				RangeBlock *dst_range_block;
+				uint index_in_dst_block;
+				GetRangeBlock(block_x >> 1, block_y >> 1, dst_range_block_offset, dst_range_block_stride, dst_range_block, index_in_dst_block);
+				dst_range_block->mMin[index_in_dst_block] = uint16(min_value);
+				dst_range_block->mMax[index_in_dst_block] = uint16(max_value);
+			}
+
+		// Go up one level
+		--max_level;
+		num_blocks >>= 1;
+		block_start_x >>= 1;
+		block_start_y >>= 1;
+		num_blocks_x = min((num_blocks_x + 1) >> 1, num_blocks);
+		num_blocks_y = min((num_blocks_y + 1) >> 1, num_blocks);
+
+		// Update stride and offset for source to old destination
+		range_block_offset = dst_range_block_offset;
+		range_block_stride = dst_range_block_stride;
+	}
+
+	// Calculate new min and max sample for the entire height field
+	mMinSample = 0xffff;
+	mMaxSample = 0;
+	for (uint i = 0; i < 4; ++i)
+		if (mRangeBlocks[0].mMin[i] != cNoCollisionValue16)
+		{
+			mMinSample = min(mMinSample, mRangeBlocks[0].mMin[i]);
+			mMaxSample = max(mMaxSample, mRangeBlocks[0].mMax[i]);
+		}
+
+#ifdef JPH_DEBUG_RENDERER
+	// Invalidate temporary rendering data
+	mGeometry.clear();
+#endif
+}
+
+void HeightFieldShape::GetMaterials(uint inX, uint inY, uint inSizeX, uint inSizeY, uint8 *outMaterials, intptr_t inMaterialsStride) const
+{
+	if (inSizeX == 0 || inSizeY == 0)
+		return;
+
+	if (mMaterialIndices.empty())
+	{
+		// Return all 0's
+		for (uint y = 0; y < inSizeY; ++y)
+		{
+			uint8 *out_indices = outMaterials + y * inMaterialsStride;
+			for (uint x = 0; x < inSizeX; ++x)
+				*out_indices++ = 0;
+		}
+		return;
+	}
+
+	JPH_ASSERT(inX < mSampleCount && inY < mSampleCount);
+	JPH_ASSERT(inX + inSizeX < mSampleCount && inY + inSizeY < mSampleCount);
+
+	uint count_min_1 = mSampleCount - 1;
+	uint16 material_index_mask = uint16((1 << mNumBitsPerMaterialIndex) - 1);
+
+	for (uint y = 0; y < inSizeY; ++y)
+	{
+		// Calculate input position
+		uint bit_pos = (inX + (inY + y) * count_min_1) * mNumBitsPerMaterialIndex;
+		const uint8 *in_indices = mMaterialIndices.data() + (bit_pos >> 3);
+		bit_pos &= 0b111;
+
+		// Calculate output position
+		uint8 *out_indices = outMaterials + y * inMaterialsStride;
+
+		for (uint x = 0; x < inSizeX; ++x)
+		{
+			// Get material index
+			uint16 material_index = uint16(in_indices[0]) + uint16(uint16(in_indices[1]) << 8);
+			material_index >>= bit_pos;
+			material_index &= material_index_mask;
+			*out_indices = uint8(material_index);
+
+			// Go to the next index
+			bit_pos += mNumBitsPerMaterialIndex;
+			in_indices += bit_pos >> 3;
+			bit_pos &= 0b111;
+			++out_indices;
+		}
+	}
+}
+
+bool HeightFieldShape::SetMaterials(uint inX, uint inY, uint inSizeX, uint inSizeY, const uint8 *inMaterials, intptr_t inMaterialsStride, const PhysicsMaterialList *inMaterialList, TempAllocator &inAllocator)
+{
+	if (inSizeX == 0 || inSizeY == 0)
+		return true;
+
+	JPH_ASSERT(inX < mSampleCount && inY < mSampleCount);
+	JPH_ASSERT(inX + inSizeX < mSampleCount && inY + inSizeY < mSampleCount);
+
+	// Remap materials
+	uint material_remap_table_size = uint(inMaterialList != nullptr? inMaterialList->size() : mMaterials.size());
+	uint8 *material_remap_table = (uint8 *)inAllocator.Allocate(material_remap_table_size);
+	JPH_SCOPE_EXIT([&inAllocator, material_remap_table, material_remap_table_size]{ inAllocator.Free(material_remap_table, material_remap_table_size); });
+	if (inMaterialList != nullptr)
+	{
+		// Conservatively reserve more space if the incoming material list is bigger
+		if (inMaterialList->size() > mMaterials.size())
+			mMaterials.reserve(inMaterialList->size());
+
+		// Create a remap table
+		uint8 *remap_entry = material_remap_table;
+		for (const PhysicsMaterial *material : *inMaterialList)
+		{
+			// Try to find it in the existing list
+			PhysicsMaterialList::const_iterator it = std::find(mMaterials.begin(), mMaterials.end(), material);
+			if (it != mMaterials.end())
+			{
+				// Found it, calculate index
+				*remap_entry = uint8(it - mMaterials.begin());
+			}
+			else
+			{
+				// Not found, add it
+				if (mMaterials.size() >= 256)
+				{
+					// We can't have more than 256 materials since we use uint8 as indices
+					return false;
+				}
+				*remap_entry = uint8(mMaterials.size());
+				mMaterials.push_back(material);
+			}
+			++remap_entry;
+		}
+	}
+	else
+	{
+		// No remapping
+		for (uint i = 0; i < material_remap_table_size; ++i)
+			material_remap_table[i] = uint8(i);
+	}
+
+	if (mMaterials.size() == 1)
+	{
+		// Only 1 material, we don't need to store the material indices
+		return true;
+	}
+
+	// Check if we need to resize the material indices array
+	uint count_min_1 = mSampleCount - 1;
+	uint32 new_bits_per_material_index = 32 - CountLeadingZeros((uint32)mMaterials.size() - 1);
+	JPH_ASSERT(mNumBitsPerMaterialIndex <= 8 && new_bits_per_material_index <= 8);
+	if (new_bits_per_material_index > mNumBitsPerMaterialIndex)
+	{
+		// Resize the material indices array
+		mMaterialIndices.resize(((Square(count_min_1) * new_bits_per_material_index + 7) >> 3) + 1, 0); // Add 1 byte so we don't read out of bounds when reading an uint16
+
+		// Calculate old and new mask
+		uint16 old_material_index_mask = uint16((1 << mNumBitsPerMaterialIndex) - 1);
+		uint16 new_material_index_mask = uint16((1 << new_bits_per_material_index) - 1);
+
+		// Loop through the array backwards to avoid overwriting data
+		int in_bit_pos = (count_min_1 * count_min_1 - 1) * mNumBitsPerMaterialIndex;
+		const uint8 *in_indices = mMaterialIndices.data() + (in_bit_pos >> 3);
+		in_bit_pos &= 0b111;
+		int out_bit_pos = (count_min_1 * count_min_1 - 1) * new_bits_per_material_index;
+		uint8 *out_indices = mMaterialIndices.data() + (out_bit_pos >> 3);
+		out_bit_pos &= 0b111;
+
+		while (out_indices >= mMaterialIndices.data())
+		{
+			// Read the material index
+			uint16 material_index = uint16(in_indices[0]) + uint16(uint16(in_indices[1]) << 8);
+			material_index >>= in_bit_pos;
+			material_index &= old_material_index_mask;
+
+			// Write the material index
+			uint16 output_data = uint16(out_indices[0]) + uint16(uint16(out_indices[1]) << 8);
+			output_data &= ~(new_material_index_mask << out_bit_pos);
+			output_data |= material_index << out_bit_pos;
+			out_indices[0] = uint8(output_data);
+			out_indices[1] = uint8(output_data >> 8);
+
+			// Go to the previous index
+			in_bit_pos -= int(mNumBitsPerMaterialIndex);
+			in_indices += in_bit_pos >> 3;
+			in_bit_pos &= 0b111;
+			out_bit_pos -= int(new_bits_per_material_index);
+			out_indices += out_bit_pos >> 3;
+			out_bit_pos &= 0b111;
+		}
+
+		// Accept the new bits per material index
+		mNumBitsPerMaterialIndex = new_bits_per_material_index;
+	}
+
+	uint16 material_index_mask = uint16((1 << mNumBitsPerMaterialIndex) - 1);
+	for (uint y = 0; y < inSizeY; ++y)
+	{
+		// Calculate input position
+		const uint8 *in_indices = inMaterials + y * inMaterialsStride;
+
+		// Calculate output position
+		uint bit_pos = (inX + (inY + y) * count_min_1) * mNumBitsPerMaterialIndex;
+		uint8 *out_indices = mMaterialIndices.data() + (bit_pos >> 3);
+		bit_pos &= 0b111;
+
+		for (uint x = 0; x < inSizeX; ++x)
+		{
+			// Update material
+			uint16 output_data = uint16(out_indices[0]) + uint16(uint16(out_indices[1]) << 8);
+			output_data &= ~(material_index_mask << bit_pos);
+			output_data |= material_remap_table[*in_indices] << bit_pos;
+			out_indices[0] = uint8(output_data);
+			out_indices[1] = uint8(output_data >> 8);
+
+			// Go to the next index
+			in_indices++;
+			bit_pos += mNumBitsPerMaterialIndex;
+			out_indices += bit_pos >> 3;
+			bit_pos &= 0b111;
+		}
+	}
+
+	return true;
+}
+
+MassProperties HeightFieldShape::GetMassProperties() const
+{
+	// Object should always be static, return default mass properties
+	return MassProperties();
+}
+
+const PhysicsMaterial *HeightFieldShape::GetMaterial(uint inX, uint inY) const
+{
+	if (mMaterials.empty())
+		return PhysicsMaterial::sDefault;
+	if (mMaterials.size() == 1)
+		return mMaterials[0];
+
+	uint count_min_1 = mSampleCount - 1;
+	JPH_ASSERT(inX < count_min_1);
+	JPH_ASSERT(inY < count_min_1);
+
+	// Calculate at which bit the material index starts
+	uint bit_pos = (inX + inY * count_min_1) * mNumBitsPerMaterialIndex;
+	uint byte_pos = bit_pos >> 3;
+	bit_pos &= 0b111;
+
+	// Read the material index
+	JPH_ASSERT(byte_pos + 1 < mMaterialIndices.size());
+	const uint8 *material_indices = mMaterialIndices.data() + byte_pos;
+	uint16 material_index = uint16(material_indices[0]) + uint16(uint16(material_indices[1]) << 8);
+	material_index >>= bit_pos;
+	material_index &= (1 << mNumBitsPerMaterialIndex) - 1;
+
+	// Return the material
+	return mMaterials[material_index];
+}
+
+uint HeightFieldShape::GetSubShapeIDBits() const
+{
+	// Need to store X, Y and 1 extra bit to specify the triangle number in the quad
+	return 2 * (32 - CountLeadingZeros(mSampleCount - 1)) + 1;
+}
+
+SubShapeID HeightFieldShape::EncodeSubShapeID(const SubShapeIDCreator &inCreator, uint inX, uint inY, uint inTriangle) const
+{
+	return inCreator.PushID((inX + inY * mSampleCount) * 2 + inTriangle, GetSubShapeIDBits()).GetID();
+}
+
+void HeightFieldShape::DecodeSubShapeID(const SubShapeID &inSubShapeID, uint &outX, uint &outY, uint &outTriangle) const
+{
+	// Decode sub shape id
+	SubShapeID remainder;
+	uint32 id = inSubShapeID.PopID(GetSubShapeIDBits(), remainder);
+	JPH_ASSERT(remainder.IsEmpty(), "Invalid subshape ID");
+
+	// Get triangle index
+	outTriangle = id & 1;
+	id >>= 1;
+
+	// Fetch the x and y coordinate
+	outX = id % mSampleCount;
+	outY = id / mSampleCount;
+}
+
+void HeightFieldShape::GetSubShapeCoordinates(const SubShapeID &inSubShapeID, uint &outX, uint &outY, uint &outTriangleIndex) const
+{
+	DecodeSubShapeID(inSubShapeID, outX, outY, outTriangleIndex);
+}
+
+const PhysicsMaterial *HeightFieldShape::GetMaterial(const SubShapeID &inSubShapeID) const
+{
+	// Decode ID
+	uint x, y, triangle;
+	DecodeSubShapeID(inSubShapeID, x, y, triangle);
+
+	// Fetch the material
+	return GetMaterial(x, y);
+}
+
+Vec3 HeightFieldShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	// Decode ID
+	uint x, y, triangle;
+	DecodeSubShapeID(inSubShapeID, x, y, triangle);
+
+	// Fetch vertices that both triangles share
+	Vec3 x1y1 = GetPosition(x, y);
+	Vec3 x2y2 = GetPosition(x + 1, y + 1);
+
+	// Get normal depending on which triangle was selected
+	Vec3 normal;
+	if (triangle == 0)
+	{
+		Vec3 x1y2 = GetPosition(x, y + 1);
+		normal = (x2y2 - x1y2).Cross(x1y1 - x1y2);
+	}
+	else
+	{
+		Vec3 x2y1 = GetPosition(x + 1, y);
+		normal = (x1y1 - x2y1).Cross(x2y2 - x2y1);
+	}
+
+	return normal.Normalized();
+}
+
+void HeightFieldShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
+	// Decode ID
+	uint x, y, triangle;
+	DecodeSubShapeID(inSubShapeID, x, y, triangle);
+
+	// Fetch the triangle
+	outVertices.resize(3);
+	outVertices[0] = GetPosition(x, y);
+	Vec3 v2 = GetPosition(x + 1, y + 1);
+	if (triangle == 0)
+	{
+		outVertices[1] = GetPosition(x, y + 1);
+		outVertices[2] = v2;
+	}
+	else
+	{
+		outVertices[1] = v2;
+		outVertices[2] = GetPosition(x + 1, y);
+	}
+
+	// Flip triangle if scaled inside out
+	if (ScaleHelpers::IsInsideOut(inScale))
+		std::swap(outVertices[1], outVertices[2]);
+
+	// Transform to world space
+	Mat44 transform = inCenterOfMassTransform.PreScaled(inScale);
+	for (Vec3 &v : outVertices)
+		v = transform * v;
+}
+
+inline uint8 HeightFieldShape::GetEdgeFlags(uint inX, uint inY, uint inTriangle) const
+{
+	JPH_ASSERT(inX < mSampleCount - 1 && inY < mSampleCount - 1);
+
+	if (inTriangle == 0)
+	{
+		// The edge flags for this triangle are directly stored, find the right 3 bits
+		uint bit_pos = 3 * (inX + inY * (mSampleCount - 1));
+		uint byte_pos = bit_pos >> 3;
+		bit_pos &= 0b111;
+		JPH_ASSERT(byte_pos + 1 < mActiveEdgesSize);
+		const uint8 *active_edges = mActiveEdges + byte_pos;
+		uint16 edge_flags = uint16(active_edges[0]) + uint16(uint16(active_edges[1]) << 8);
+		return uint8(edge_flags >> bit_pos) & 0b111;
+	}
+	else
+	{
+		// We don't store this triangle directly, we need to look at our three neighbours to construct the edge flags
+		uint8 edge0 = (GetEdgeFlags(inX, inY, 0) & 0b100) != 0? 0b001 : 0; // Diagonal edge
+		uint8 edge1 = inX == mSampleCount - 2 || (GetEdgeFlags(inX + 1, inY, 0) & 0b001) != 0? 0b010 : 0; // Vertical edge
+		uint8 edge2 = inY == 0 || (GetEdgeFlags(inX, inY - 1, 0) & 0b010) != 0? 0b100 : 0; // Horizontal edge
+		return edge0 | edge1 | edge2;
+	}
+}
+
+AABox HeightFieldShape::GetLocalBounds() const
+{
+	if (mMinSample == cNoCollisionValue16)
+	{
+		// This whole height field shape doesn't have any collision, return the center point
+		Vec3 center = mOffset + 0.5f * mScale * Vec3(float(mSampleCount - 1), 0.0f, float(mSampleCount - 1));
+		return AABox(center, center);
+	}
+	else
+	{
+		// Bounding box based on min and max sample height
+		Vec3 bmin = mOffset + mScale * Vec3(0.0f, float(mMinSample), 0.0f);
+		Vec3 bmax = mOffset + mScale * Vec3(float(mSampleCount - 1), float(mMaxSample), float(mSampleCount - 1));
+		return AABox(bmin, bmax);
+	}
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void HeightFieldShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	// Don't draw anything if we don't have any collision
+	if (mHeightSamplesSize == 0)
+		return;
+
+	// Reset the batch if we switch coloring mode
+	if (mCachedUseMaterialColors != inUseMaterialColors)
+	{
+		mGeometry.clear();
+		mCachedUseMaterialColors = inUseMaterialColors;
+	}
+
+	if (mGeometry.empty())
+	{
+		// Divide terrain in triangle batches of max 64x64x2 triangles to allow better culling of the terrain
+		uint32 block_size = min<uint32>(mSampleCount, 64);
+		for (uint32 by = 0; by < mSampleCount; by += block_size)
+			for (uint32 bx = 0; bx < mSampleCount; bx += block_size)
+			{
+				// Create vertices for a block
+				Array<DebugRenderer::Triangle> triangles;
+				triangles.resize(block_size * block_size * 2);
+				DebugRenderer::Triangle *out_tri = &triangles[0];
+				for (uint32 y = by, max_y = min(by + block_size, mSampleCount - 1); y < max_y; ++y)
+					for (uint32 x = bx, max_x = min(bx + block_size, mSampleCount - 1); x < max_x; ++x)
+						if (!IsNoCollision(x, y) && !IsNoCollision(x + 1, y + 1))
+						{
+							Vec3 x1y1 = GetPosition(x, y);
+							Vec3 x2y2 = GetPosition(x + 1, y + 1);
+							Color color = inUseMaterialColors? GetMaterial(x, y)->GetDebugColor() : Color::sWhite;
+
+							if (!IsNoCollision(x, y + 1))
+							{
+								Vec3 x1y2 = GetPosition(x, y + 1);
+
+								x1y1.StoreFloat3(&out_tri->mV[0].mPosition);
+								x1y2.StoreFloat3(&out_tri->mV[1].mPosition);
+								x2y2.StoreFloat3(&out_tri->mV[2].mPosition);
+
+								Vec3 normal = (x2y2 - x1y2).Cross(x1y1 - x1y2).Normalized();
+								for (DebugRenderer::Vertex &v : out_tri->mV)
+								{
+									v.mColor = color;
+									v.mUV = Float2(0, 0);
+									normal.StoreFloat3(&v.mNormal);
+								}
+
+								++out_tri;
+							}
+
+							if (!IsNoCollision(x + 1, y))
+							{
+								Vec3 x2y1 = GetPosition(x + 1, y);
+
+								x1y1.StoreFloat3(&out_tri->mV[0].mPosition);
+								x2y2.StoreFloat3(&out_tri->mV[1].mPosition);
+								x2y1.StoreFloat3(&out_tri->mV[2].mPosition);
+
+								Vec3 normal = (x1y1 - x2y1).Cross(x2y2 - x2y1).Normalized();
+								for (DebugRenderer::Vertex &v : out_tri->mV)
+								{
+									v.mColor = color;
+									v.mUV = Float2(0, 0);
+									normal.StoreFloat3(&v.mNormal);
+								}
+
+								++out_tri;
+							}
+						}
+
+				// Resize triangles array to actual amount of triangles written
+				size_t num_triangles = out_tri - &triangles[0];
+				triangles.resize(num_triangles);
+
+				// Create batch
+				if (num_triangles > 0)
+					mGeometry.push_back(new DebugRenderer::Geometry(inRenderer->CreateTriangleBatch(triangles), DebugRenderer::sCalculateBounds(&triangles[0].mV[0], int(3 * num_triangles))));
+			}
+	}
+
+	// Get transform including scale
+	RMat44 transform = inCenterOfMassTransform.PreScaled(inScale);
+
+	// Test if the shape is scaled inside out
+	DebugRenderer::ECullMode cull_mode = ScaleHelpers::IsInsideOut(inScale)? DebugRenderer::ECullMode::CullFrontFace : DebugRenderer::ECullMode::CullBackFace;
+
+	// Determine the draw mode
+	DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid;
+
+	// Draw the geometry
+	for (const DebugRenderer::GeometryRef &b : mGeometry)
+		inRenderer->DrawGeometry(transform, inColor, b, cull_mode, DebugRenderer::ECastShadow::On, draw_mode);
+
+	if (sDrawTriangleOutlines)
+	{
+		struct Visitor
+		{
+			JPH_INLINE explicit		Visitor(const HeightFieldShape *inShape, DebugRenderer *inRenderer, RMat44Arg inTransform) :
+				mShape(inShape),
+				mRenderer(inRenderer),
+				mTransform(inTransform)
+			{
+			}
+
+			JPH_INLINE bool			ShouldAbort() const
+			{
+				return false;
+			}
+
+			JPH_INLINE bool			ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const
+			{
+				return true;
+			}
+
+			JPH_INLINE int			VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const
+			{
+				UVec4 valid = Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY);
+				return CountAndSortTrues(valid, ioProperties);
+			}
+
+			JPH_INLINE void			VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) const
+			{
+				// Determine active edges
+				uint8 active_edges = mShape->GetEdgeFlags(inX, inY, inTriangle);
+
+				// Loop through edges
+				Vec3 v[] = { inV0, inV1, inV2 };
+				for (uint edge_idx = 0; edge_idx < 3; ++edge_idx)
+				{
+					RVec3 v1 = mTransform * v[edge_idx];
+					RVec3 v2 = mTransform * v[(edge_idx + 1) % 3];
+
+					// Draw active edge as a green arrow, other edges as grey
+					if (active_edges & (1 << edge_idx))
+						mRenderer->DrawArrow(v1, v2, Color::sGreen, 0.01f);
+					else
+						mRenderer->DrawLine(v1, v2, Color::sGrey);
+				}
+			}
+
+			const HeightFieldShape *mShape;
+			DebugRenderer *			mRenderer;
+			RMat44					mTransform;
+		};
+
+		Visitor visitor(this, inRenderer, inCenterOfMassTransform.PreScaled(inScale));
+		WalkHeightField(visitor);
+	}
+}
+#endif // JPH_DEBUG_RENDERER
+
+class HeightFieldShape::DecodingContext
+{
+public:
+	JPH_INLINE explicit			DecodingContext(const HeightFieldShape *inShape) :
+		mShape(inShape)
+	{
+		static_assert(sizeof(sGridOffsets) / sizeof(uint) == cNumBitsXY + 1, "Offsets array is not long enough");
+
+		// Construct root stack entry
+		mPropertiesStack[0] = 0; // level: 0, x: 0, y: 0
+	}
+
+	template <class Visitor>
+	JPH_INLINE void				WalkHeightField(Visitor &ioVisitor)
+	{
+		// Early out if there's no collision
+		if (mShape->mHeightSamplesSize == 0)
+			return;
+
+		// Assert that an inside-out bounding box does not collide
+		JPH_IF_ENABLE_ASSERTS(UVec4 dummy = UVec4::sReplicate(0);)
+		JPH_ASSERT(ioVisitor.VisitRangeBlock(Vec4::sReplicate(-1.0e6f), Vec4::sReplicate(1.0e6f), Vec4::sReplicate(-1.0e6f), Vec4::sReplicate(1.0e6f), Vec4::sReplicate(-1.0e6f), Vec4::sReplicate(1.0e6f), dummy, 0) == 0);
+
+		// Precalculate values relating to sample count
+		uint32 sample_count = mShape->mSampleCount;
+		UVec4 sample_count_min_1 = UVec4::sReplicate(sample_count - 1);
+
+		// Precalculate values relating to block size
+		uint32 block_size = mShape->mBlockSize;
+		uint32 block_size_plus_1 = block_size + 1;
+		uint num_blocks = mShape->GetNumBlocks();
+		uint num_blocks_min_1 = num_blocks - 1;
+		uint max_level = HeightFieldShape::sGetMaxLevel(num_blocks);
+		uint32 max_stride = (num_blocks + 1) >> 1;
+
+		// Precalculate range block offset and stride for GetBlockOffsetAndScale
+		uint range_block_offset, range_block_stride;
+		sGetRangeBlockOffsetAndStride(num_blocks, max_level, range_block_offset, range_block_stride);
+
+		// Allocate space for vertices and 'no collision' flags
+		int array_size = Square(block_size_plus_1);
+		Vec3 *vertices = reinterpret_cast<Vec3 *>(JPH_STACK_ALLOC(array_size * sizeof(Vec3)));
+		bool *no_collision = reinterpret_cast<bool *>(JPH_STACK_ALLOC(array_size * sizeof(bool)));
+
+		// Splat offsets
+		Vec4 ox = mShape->mOffset.SplatX();
+		Vec4 oy = mShape->mOffset.SplatY();
+		Vec4 oz = mShape->mOffset.SplatZ();
+
+		// Splat scales
+		Vec4 sx = mShape->mScale.SplatX();
+		Vec4 sy = mShape->mScale.SplatY();
+		Vec4 sz = mShape->mScale.SplatZ();
+
+		do
+		{
+			// Decode properties
+			uint32 properties_top = mPropertiesStack[mTop];
+			uint32 x = properties_top & cMaskBitsXY;
+			uint32 y = (properties_top >> cNumBitsXY) & cMaskBitsXY;
+			uint32 level = properties_top >> cLevelShift;
+
+			if (level >= max_level)
+			{
+				// Determine actual range of samples (minus one because we eventually want to iterate over the triangles, not the samples)
+				uint32 min_x = x * block_size;
+				uint32 max_x = min_x + block_size;
+				uint32 min_y = y * block_size;
+				uint32 max_y = min_y + block_size;
+
+				// Decompress vertices of block at (x, y)
+				Vec3 *dst_vertex = vertices;
+				bool *dst_no_collision = no_collision;
+				float block_offset, block_scale;
+				mShape->GetBlockOffsetAndScale(x, y, range_block_offset, range_block_stride, block_offset, block_scale);
+				for (uint32 v_y = min_y; v_y < max_y; ++v_y)
+				{
+					for (uint32 v_x = min_x; v_x < max_x; ++v_x)
+					{
+						*dst_vertex = mShape->GetPosition(v_x, v_y, block_offset, block_scale, *dst_no_collision);
+						++dst_vertex;
+						++dst_no_collision;
+					}
+
+					// Skip last column, these values come from a different block
+					++dst_vertex;
+					++dst_no_collision;
+				}
+
+				// Decompress block (x + 1, y)
+				uint32 max_x_decrement = 0;
+				if (x < num_blocks_min_1)
+				{
+					dst_vertex = vertices + block_size;
+					dst_no_collision = no_collision + block_size;
+					mShape->GetBlockOffsetAndScale(x + 1, y, range_block_offset, range_block_stride, block_offset, block_scale);
+					for (uint32 v_y = min_y; v_y < max_y; ++v_y)
+					{
+						*dst_vertex = mShape->GetPosition(max_x, v_y, block_offset, block_scale, *dst_no_collision);
+						dst_vertex += block_size_plus_1;
+						dst_no_collision += block_size_plus_1;
+					}
+				}
+				else
+					max_x_decrement = 1; // We don't have a next block, one less triangle to test
+
+				// Decompress block (x, y + 1)
+				if (y < num_blocks_min_1)
+				{
+					uint start = block_size * block_size_plus_1;
+					dst_vertex = vertices + start;
+					dst_no_collision = no_collision + start;
+					mShape->GetBlockOffsetAndScale(x, y + 1, range_block_offset, range_block_stride, block_offset, block_scale);
+					for (uint32 v_x = min_x; v_x < max_x; ++v_x)
+					{
+						*dst_vertex = mShape->GetPosition(v_x, max_y, block_offset, block_scale, *dst_no_collision);
+						++dst_vertex;
+						++dst_no_collision;
+					}
+
+					// Decompress single sample of block at (x + 1, y + 1)
+					if (x < num_blocks_min_1)
+					{
+						mShape->GetBlockOffsetAndScale(x + 1, y + 1, range_block_offset, range_block_stride, block_offset, block_scale);
+						*dst_vertex = mShape->GetPosition(max_x, max_y, block_offset, block_scale, *dst_no_collision);
+					}
+				}
+				else
+					--max_y; // We don't have a next block, one less triangle to test
+
+				// Update max_x (we've been using it so we couldn't update it earlier)
+				max_x -= max_x_decrement;
+
+				// We're going to divide the vertices in 4 blocks to do one more runtime sub-division, calculate the ranges of those blocks
+				struct Range
+				{
+					uint32 mMinX, mMinY, mNumTrianglesX, mNumTrianglesY;
+				};
+				uint32 half_block_size = block_size >> 1;
+				uint32 block_size_x = max_x - min_x - half_block_size;
+				uint32 block_size_y = max_y - min_y - half_block_size;
+				Range ranges[] =
+				{
+					{ 0, 0,									half_block_size, half_block_size },
+					{ half_block_size, 0,					block_size_x, half_block_size },
+					{ 0, half_block_size,					half_block_size, block_size_y },
+					{ half_block_size, half_block_size,		block_size_x, block_size_y },
+				};
+
+				// Calculate the min and max of each of the blocks
+				Mat44 block_min, block_max;
+				for (int block = 0; block < 4; ++block)
+				{
+					// Get the range for this block
+					const Range &range = ranges[block];
+					uint32 start = range.mMinX + range.mMinY * block_size_plus_1;
+					uint32 size_x_plus_1 = range.mNumTrianglesX + 1;
+					uint32 size_y_plus_1 = range.mNumTrianglesY + 1;
+
+					// Calculate where to start reading
+					const Vec3 *src_vertex = vertices + start;
+					const bool *src_no_collision = no_collision + start;
+					uint32 stride = block_size_plus_1 - size_x_plus_1;
+
+					// Start range with a very large inside-out box
+					Vec3 value_min = Vec3::sReplicate(1.0e30f);
+					Vec3 value_max = Vec3::sReplicate(-1.0e30f);
+
+					// Loop over the samples to determine the min and max of this block
+					for (uint32 block_y = 0; block_y < size_y_plus_1; ++block_y)
+					{
+						for (uint32 block_x = 0; block_x < size_x_plus_1; ++block_x)
+						{
+							if (!*src_no_collision)
+							{
+								value_min = Vec3::sMin(value_min, *src_vertex);
+								value_max = Vec3::sMax(value_max, *src_vertex);
+							}
+							++src_vertex;
+							++src_no_collision;
+						}
+						src_vertex += stride;
+						src_no_collision += stride;
+					}
+					block_min.SetColumn4(block, Vec4(value_min));
+					block_max.SetColumn4(block, Vec4(value_max));
+				}
+
+			#ifdef JPH_DEBUG_HEIGHT_FIELD
+				// Draw the bounding boxes of the sub-nodes
+				for (int block = 0; block < 4; ++block)
+				{
+					AABox bounds(block_min.GetColumn3(block), block_max.GetColumn3(block));
+					if (bounds.IsValid())
+						DebugRenderer::sInstance->DrawWireBox(bounds, Color::sYellow);
+				}
+			#endif // JPH_DEBUG_HEIGHT_FIELD
+
+				// Transpose so we have the mins and maxes of each of the blocks in rows instead of columns
+				Mat44 transposed_min = block_min.Transposed();
+				Mat44 transposed_max = block_max.Transposed();
+
+				// Check which blocks collide
+				// Note: At this point we don't use our own stack but we do allow the visitor to use its own stack
+				// to store collision distances so that we can still early out when no closer hits have been found.
+				UVec4 colliding_blocks(0, 1, 2, 3);
+				int num_results = ioVisitor.VisitRangeBlock(transposed_min.GetColumn4(0), transposed_min.GetColumn4(1), transposed_min.GetColumn4(2), transposed_max.GetColumn4(0), transposed_max.GetColumn4(1), transposed_max.GetColumn4(2), colliding_blocks, mTop);
+
+				// Loop through the results backwards (closest first)
+				int result = num_results - 1;
+				while (result >= 0)
+				{
+					// Calculate the min and max of this block
+					uint32 block = colliding_blocks[result];
+					const Range &range = ranges[block];
+					uint32 block_min_x = min_x + range.mMinX;
+					uint32 block_max_x = block_min_x + range.mNumTrianglesX;
+					uint32 block_min_y = min_y + range.mMinY;
+					uint32 block_max_y = block_min_y + range.mNumTrianglesY;
+
+					// Loop triangles
+					for (uint32 v_y = block_min_y; v_y < block_max_y; ++v_y)
+						for (uint32 v_x = block_min_x; v_x < block_max_x; ++v_x)
+						{
+							// Get first vertex
+							const int offset = (v_y - min_y) * block_size_plus_1 + (v_x - min_x);
+							const Vec3 *start_vertex = vertices + offset;
+							const bool *start_no_collision = no_collision + offset;
+
+							// Check if vertices shared by both triangles have collision
+							if (!start_no_collision[0] && !start_no_collision[block_size_plus_1 + 1])
+							{
+								// Loop 2 triangles
+								for (uint t = 0; t < 2; ++t)
+								{
+									// Determine triangle vertices
+									Vec3 v0, v1, v2;
+									if (t == 0)
+									{
+										// Check third vertex
+										if (start_no_collision[block_size_plus_1])
+											continue;
+
+										// Get vertices for triangle
+										v0 = start_vertex[0];
+										v1 = start_vertex[block_size_plus_1];
+										v2 = start_vertex[block_size_plus_1 + 1];
+									}
+									else
+									{
+										// Check third vertex
+										if (start_no_collision[1])
+											continue;
+
+										// Get vertices for triangle
+										v0 = start_vertex[0];
+										v1 = start_vertex[block_size_plus_1 + 1];
+										v2 = start_vertex[1];
+									}
+
+								#ifdef JPH_DEBUG_HEIGHT_FIELD
+									DebugRenderer::sInstance->DrawWireTriangle(RVec3(v0), RVec3(v1), RVec3(v2), Color::sWhite);
+								#endif
+
+									// Call visitor
+									ioVisitor.VisitTriangle(v_x, v_y, t, v0, v1, v2);
+
+									// Check if we're done
+									if (ioVisitor.ShouldAbort())
+										return;
+								}
+							}
+						}
+
+					// Fetch next block until we find one that the visitor wants to see
+					do
+						--result;
+					while (result >= 0 && !ioVisitor.ShouldVisitRangeBlock(mTop + result));
+				}
+			}
+			else
+			{
+				// Visit child grid
+				uint32 stride = min(1U << level, max_stride); // At the most detailed level we store a non-power of 2 number of blocks
+				uint32 offset = sGridOffsets[level] + stride * y + x;
+
+				// Decode min/max height
+				JPH_ASSERT(offset < mShape->mRangeBlocksSize);
+				UVec4 block = UVec4::sLoadInt4Aligned(reinterpret_cast<const uint32 *>(&mShape->mRangeBlocks[offset]));
+				Vec4 bounds_miny = oy + sy * block.Expand4Uint16Lo().ToFloat();
+				Vec4 bounds_maxy = oy + sy * block.Expand4Uint16Hi().ToFloat();
+
+				// Calculate size of one cell at this grid level
+				UVec4 internal_cell_size = UVec4::sReplicate(block_size << (max_level - level - 1)); // subtract 1 from level because we have an internal grid of 2x2
+
+				// Calculate min/max x and z
+				UVec4 two_x = UVec4::sReplicate(2 * x); // multiply by two because we have an internal grid of 2x2
+				Vec4 bounds_minx = ox + sx * (internal_cell_size * (two_x + UVec4(0, 1, 0, 1))).ToFloat();
+				Vec4 bounds_maxx = ox + sx * UVec4::sMin(internal_cell_size * (two_x + UVec4(1, 2, 1, 2)), sample_count_min_1).ToFloat();
+
+				UVec4 two_y = UVec4::sReplicate(2 * y);
+				Vec4 bounds_minz = oz + sz * (internal_cell_size * (two_y + UVec4(0, 0, 1, 1))).ToFloat();
+				Vec4 bounds_maxz = oz + sz * UVec4::sMin(internal_cell_size * (two_y + UVec4(1, 1, 2, 2)), sample_count_min_1).ToFloat();
+
+				// Calculate properties of child blocks
+				UVec4 properties = UVec4::sReplicate(((level + 1) << cLevelShift) + (y << (cNumBitsXY + 1)) + (x << 1)) + UVec4(0, 1, 1 << cNumBitsXY, (1 << cNumBitsXY) + 1);
+
+			#ifdef JPH_DEBUG_HEIGHT_FIELD
+				// Draw boxes
+				for (int i = 0; i < 4; ++i)
+				{
+					AABox b(Vec3(bounds_minx[i], bounds_miny[i], bounds_minz[i]), Vec3(bounds_maxx[i], bounds_maxy[i], bounds_maxz[i]));
+					if (b.IsValid())
+						DebugRenderer::sInstance->DrawWireBox(b, Color::sGreen);
+				}
+			#endif
+
+				// Check which sub nodes to visit
+				int num_results = ioVisitor.VisitRangeBlock(bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz, properties, mTop);
+
+				// Push them onto the stack
+				JPH_ASSERT(mTop + 4 < cStackSize);
+				properties.StoreInt4(&mPropertiesStack[mTop]);
+				mTop += num_results;
+			}
+
+			// Check if we're done
+			if (ioVisitor.ShouldAbort())
+				return;
+
+			// Fetch next node until we find one that the visitor wants to see
+			do
+				--mTop;
+			while (mTop >= 0 && !ioVisitor.ShouldVisitRangeBlock(mTop));
+		}
+		while (mTop >= 0);
+	}
+
+	// This can be used to have the visitor early out (ioVisitor.ShouldAbort() returns true) and later continue again (call WalkHeightField() again)
+	JPH_INLINE bool				IsDoneWalking() const
+	{
+		return mTop < 0;
+	}
+
+private:
+	const HeightFieldShape *	mShape;
+	int							mTop = 0;
+	uint32						mPropertiesStack[cStackSize];
+};
+
+template <class Visitor>
+void HeightFieldShape::WalkHeightField(Visitor &ioVisitor) const
+{
+	DecodingContext ctx(this);
+	ctx.WalkHeightField(ioVisitor);
+}
+
+bool HeightFieldShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	struct Visitor
+	{
+		JPH_INLINE explicit		Visitor(const HeightFieldShape *inShape, const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) :
+			mHit(ioHit),
+			mRayOrigin(inRay.mOrigin),
+			mRayDirection(inRay.mDirection),
+			mRayInvDirection(inRay.mDirection),
+			mShape(inShape),
+			mSubShapeIDCreator(inSubShapeIDCreator)
+		{
+		}
+
+		JPH_INLINE bool			ShouldAbort() const
+		{
+			return mHit.mFraction <= 0.0f;
+		}
+
+		JPH_INLINE bool			ShouldVisitRangeBlock(int inStackTop) const
+		{
+			return mDistanceStack[inStackTop] < mHit.mFraction;
+		}
+
+		JPH_INLINE int			VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
+		{
+			// Test bounds of 4 children
+			Vec4 distance = RayAABox4(mRayOrigin, mRayInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+
+			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
+			return SortReverseAndStore(distance, mHit.mFraction, ioProperties, &mDistanceStack[inStackTop]);
+		}
+
+		JPH_INLINE void			VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
+		{
+			float fraction = RayTriangle(mRayOrigin, mRayDirection, inV0, inV1, inV2);
+			if (fraction < mHit.mFraction)
+			{
+				// It's a closer hit
+				mHit.mFraction = fraction;
+				mHit.mSubShapeID2 = mShape->EncodeSubShapeID(mSubShapeIDCreator, inX, inY, inTriangle);
+				mReturnValue = true;
+			}
+		}
+
+		RayCastResult &			mHit;
+		Vec3					mRayOrigin;
+		Vec3					mRayDirection;
+		RayInvDirection			mRayInvDirection;
+		const HeightFieldShape *mShape;
+		SubShapeIDCreator		mSubShapeIDCreator;
+		bool					mReturnValue = false;
+		float					mDistanceStack[cStackSize];
+	};
+
+	Visitor visitor(this, inRay, inSubShapeIDCreator, ioHit);
+	WalkHeightField(visitor);
+
+	return visitor.mReturnValue;
+}
+
+void HeightFieldShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	struct Visitor
+	{
+		JPH_INLINE explicit		Visitor(const HeightFieldShape *inShape, const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector) :
+			mCollector(ioCollector),
+			mRayOrigin(inRay.mOrigin),
+			mRayDirection(inRay.mDirection),
+			mRayInvDirection(inRay.mDirection),
+			mBackFaceMode(inRayCastSettings.mBackFaceModeTriangles),
+			mShape(inShape),
+			mSubShapeIDCreator(inSubShapeIDCreator)
+		{
+		}
+
+		JPH_INLINE bool			ShouldAbort() const
+		{
+			return mCollector.ShouldEarlyOut();
+		}
+
+		JPH_INLINE bool			ShouldVisitRangeBlock(int inStackTop) const
+		{
+			return mDistanceStack[inStackTop] < mCollector.GetEarlyOutFraction();
+		}
+
+		JPH_INLINE int			VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
+		{
+			// Test bounds of 4 children
+			Vec4 distance = RayAABox4(mRayOrigin, mRayInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+
+			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
+			return SortReverseAndStore(distance, mCollector.GetEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
+		}
+
+		JPH_INLINE void			VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) const
+		{
+			// Back facing check
+			if (mBackFaceMode == EBackFaceMode::IgnoreBackFaces && (inV2 - inV0).Cross(inV1 - inV0).Dot(mRayDirection) < 0)
+				return;
+
+			// Check the triangle
+			float fraction = RayTriangle(mRayOrigin, mRayDirection, inV0, inV1, inV2);
+			if (fraction < mCollector.GetEarlyOutFraction())
+			{
+				RayCastResult hit;
+				hit.mBodyID = TransformedShape::sGetBodyID(mCollector.GetContext());
+				hit.mFraction = fraction;
+				hit.mSubShapeID2 = mShape->EncodeSubShapeID(mSubShapeIDCreator, inX, inY, inTriangle);
+				mCollector.AddHit(hit);
+			}
+		}
+
+		CastRayCollector &		mCollector;
+		Vec3					mRayOrigin;
+		Vec3					mRayDirection;
+		RayInvDirection			mRayInvDirection;
+		EBackFaceMode			mBackFaceMode;
+		const HeightFieldShape *mShape;
+		SubShapeIDCreator		mSubShapeIDCreator;
+		float					mDistanceStack[cStackSize];
+	};
+
+	Visitor visitor(this, inRay, inRayCastSettings, inSubShapeIDCreator, ioCollector);
+	WalkHeightField(visitor);
+}
+
+void HeightFieldShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// A height field doesn't have volume, so we can't test insideness
+}
+
+void HeightFieldShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	struct Visitor : public CollideSoftBodyVerticesVsTriangles
+	{
+		using CollideSoftBodyVerticesVsTriangles::CollideSoftBodyVerticesVsTriangles;
+
+		JPH_INLINE bool	ShouldAbort() const
+		{
+			return false;
+		}
+
+		JPH_INLINE bool	ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const
+		{
+			return mDistanceStack[inStackTop] < mClosestDistanceSq;
+		}
+
+		JPH_INLINE int	VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
+		{
+			// Get distance to vertex
+			Vec4 dist_sq = AABox4DistanceSqToPoint(mLocalPosition, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+
+			// Clear distance for invalid bounds
+			dist_sq = Vec4::sSelect(Vec4::sReplicate(FLT_MAX), dist_sq, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY));
+
+			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
+			return SortReverseAndStore(dist_sq, mClosestDistanceSq, ioProperties, &mDistanceStack[inStackTop]);
+		}
+
+		JPH_INLINE void	VisitTriangle([[maybe_unused]] uint inX, [[maybe_unused]] uint inY, [[maybe_unused]] uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
+		{
+			ProcessTriangle(inV0, inV1, inV2);
+		}
+
+		float			mDistanceStack[cStackSize];
+	};
+
+	Visitor visitor(inCenterOfMassTransform, inScale);
+
+	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
+		if (v.GetInvMass() > 0.0f)
+		{
+			visitor.StartVertex(v);
+			WalkHeightField(visitor);
+			visitor.FinishVertex(v, inCollidingShapeIndex);
+		}
+}
+
+void HeightFieldShape::sCastConvexVsHeightField(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_PROFILE_FUNCTION();
+
+	struct Visitor : public CastConvexVsTriangles
+	{
+		using CastConvexVsTriangles::CastConvexVsTriangles;
+
+		JPH_INLINE bool				ShouldAbort() const
+		{
+			return mCollector.ShouldEarlyOut();
+		}
+
+		JPH_INLINE bool				ShouldVisitRangeBlock(int inStackTop) const
+		{
+			return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction();
+		}
+
+		JPH_INLINE int				VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
+		{
+			// Scale the bounding boxes of this node
+			Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
+			AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+			// Enlarge them by the casted shape's box extents
+			AABox4EnlargeWithExtent(mBoxExtent, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+			// Test bounds of 4 children
+			Vec4 distance = RayAABox4(mBoxCenter, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+			// Clear distance for invalid bounds
+			distance = Vec4::sSelect(Vec4::sReplicate(FLT_MAX), distance, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY));
+
+			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
+			return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
+		}
+
+		JPH_INLINE void				VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
+		{
+			// Create sub shape id for this part
+			SubShapeID triangle_sub_shape_id = mShape2->EncodeSubShapeID(mSubShapeIDCreator2, inX, inY, inTriangle);
+
+			// Determine active edges
+			uint8 active_edges = mShape2->GetEdgeFlags(inX, inY, inTriangle);
+
+			Cast(inV0, inV1, inV2, active_edges, triangle_sub_shape_id);
+		}
+
+		const HeightFieldShape *	mShape2;
+		RayInvDirection				mInvDirection;
+		Vec3						mBoxCenter;
+		Vec3						mBoxExtent;
+		SubShapeIDCreator			mSubShapeIDCreator2;
+		float						mDistanceStack[cStackSize];
+	};
+
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::HeightField);
+	const HeightFieldShape *shape = static_cast<const HeightFieldShape *>(inShape);
+
+	Visitor visitor(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector);
+	visitor.mShape2 = shape;
+	visitor.mInvDirection.Set(inShapeCast.mDirection);
+	visitor.mBoxCenter = inShapeCast.mShapeWorldBounds.GetCenter();
+	visitor.mBoxExtent = inShapeCast.mShapeWorldBounds.GetExtent();
+	visitor.mSubShapeIDCreator2 = inSubShapeIDCreator2;
+	shape->WalkHeightField(visitor);
+}
+
+void HeightFieldShape::sCastSphereVsHeightField(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_PROFILE_FUNCTION();
+
+	struct Visitor : public CastSphereVsTriangles
+	{
+		using CastSphereVsTriangles::CastSphereVsTriangles;
+
+		JPH_INLINE bool				ShouldAbort() const
+		{
+			return mCollector.ShouldEarlyOut();
+		}
+
+		JPH_INLINE bool				ShouldVisitRangeBlock(int inStackTop) const
+		{
+			return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction();
+		}
+
+		JPH_INLINE int				VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
+		{
+			// Scale the bounding boxes of this node
+			Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
+			AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+			// Enlarge them by the radius of the sphere
+			AABox4EnlargeWithExtent(Vec3::sReplicate(mRadius), bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+			// Test bounds of 4 children
+			Vec4 distance = RayAABox4(mStart, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+			// Clear distance for invalid bounds
+			distance = Vec4::sSelect(Vec4::sReplicate(FLT_MAX), distance, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY));
+
+			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
+			return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
+		}
+
+		JPH_INLINE void				VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
+		{
+			// Create sub shape id for this part
+			SubShapeID triangle_sub_shape_id = mShape2->EncodeSubShapeID(mSubShapeIDCreator2, inX, inY, inTriangle);
+
+			// Determine active edges
+			uint8 active_edges = mShape2->GetEdgeFlags(inX, inY, inTriangle);
+
+			Cast(inV0, inV1, inV2, active_edges, triangle_sub_shape_id);
+		}
+
+		const HeightFieldShape *	mShape2;
+		RayInvDirection				mInvDirection;
+		SubShapeIDCreator			mSubShapeIDCreator2;
+		float						mDistanceStack[cStackSize];
+	};
+
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::HeightField);
+	const HeightFieldShape *shape = static_cast<const HeightFieldShape *>(inShape);
+
+	Visitor visitor(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector);
+	visitor.mShape2 = shape;
+	visitor.mInvDirection.Set(inShapeCast.mDirection);
+	visitor.mSubShapeIDCreator2 = inSubShapeIDCreator2;
+	shape->WalkHeightField(visitor);
+}
+
+struct HeightFieldShape::HSGetTrianglesContext
+{
+			HSGetTrianglesContext(const HeightFieldShape *inShape, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) :
+		mDecodeCtx(inShape),
+		mShape(inShape),
+		mLocalBox(Mat44::sInverseRotationTranslation(inRotation, inPositionCOM), inBox),
+		mHeightFieldScale(inScale),
+		mLocalToWorld(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale)),
+		mIsInsideOut(ScaleHelpers::IsInsideOut(inScale))
+	{
+	}
+
+	bool	ShouldAbort() const
+	{
+		return mShouldAbort;
+	}
+
+	bool	ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const
+	{
+		return true;
+	}
+
+	int		VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const
+	{
+		// Scale the bounding boxes of this node
+		Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
+		AABox4Scale(mHeightFieldScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+		// Test which nodes collide
+		UVec4 collides = AABox4VsBox(mLocalBox, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+		// Filter out invalid bounding boxes
+		collides = UVec4::sAnd(collides, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY));
+
+		return CountAndSortTrues(collides, ioProperties);
+	}
+
+	void	VisitTriangle(uint inX, uint inY, [[maybe_unused]] uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
+	{
+		// When the buffer is full and we cannot process the triangles, abort the height field walk. The next time GetTrianglesNext is called we will continue here.
+		if (mNumTrianglesFound + 1 > mMaxTrianglesRequested)
+		{
+			mShouldAbort = true;
+			return;
+		}
+
+		// Store vertices as Float3
+		if (mIsInsideOut)
+		{
+			// Reverse vertices
+			(mLocalToWorld * inV0).StoreFloat3(mTriangleVertices++);
+			(mLocalToWorld * inV2).StoreFloat3(mTriangleVertices++);
+			(mLocalToWorld * inV1).StoreFloat3(mTriangleVertices++);
+		}
+		else
+		{
+			// Normal scale
+			(mLocalToWorld * inV0).StoreFloat3(mTriangleVertices++);
+			(mLocalToWorld * inV1).StoreFloat3(mTriangleVertices++);
+			(mLocalToWorld * inV2).StoreFloat3(mTriangleVertices++);
+		}
+
+		// Decode material
+		if (mMaterials != nullptr)
+			*mMaterials++ = mShape->GetMaterial(inX, inY);
+
+		// Accumulate triangles found
+		mNumTrianglesFound++;
+	}
+
+	DecodingContext				mDecodeCtx;
+	const HeightFieldShape *	mShape;
+	OrientedBox					mLocalBox;
+	Vec3						mHeightFieldScale;
+	Mat44						mLocalToWorld;
+	int							mMaxTrianglesRequested;
+	Float3 *					mTriangleVertices;
+	int							mNumTrianglesFound;
+	const PhysicsMaterial **	mMaterials;
+	bool						mShouldAbort;
+	bool						mIsInsideOut;
+};
+
+void HeightFieldShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const
+{
+	static_assert(sizeof(HSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small");
+	JPH_ASSERT(IsAligned(&ioContext, alignof(HSGetTrianglesContext)));
+
+	new (&ioContext) HSGetTrianglesContext(this, inBox, inPositionCOM, inRotation, inScale);
+}
+
+int HeightFieldShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const
+{
+	static_assert(cGetTrianglesMinTrianglesRequested >= 1, "cGetTrianglesMinTrianglesRequested is too small");
+	JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested);
+
+	// Check if we're done
+	HSGetTrianglesContext &context = (HSGetTrianglesContext &)ioContext;
+	if (context.mDecodeCtx.IsDoneWalking())
+		return 0;
+
+	// Store parameters on context
+	context.mMaxTrianglesRequested = inMaxTrianglesRequested;
+	context.mTriangleVertices = outTriangleVertices;
+	context.mMaterials = outMaterials;
+	context.mShouldAbort = false; // Reset the abort flag
+	context.mNumTrianglesFound = 0;
+
+	// Continue (or start) walking the height field
+	context.mDecodeCtx.WalkHeightField(context);
+	return context.mNumTrianglesFound;
+}
+
+void HeightFieldShape::sCollideConvexVsHeightField(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Get the shapes
+	JPH_ASSERT(inShape1->GetType() == EShapeType::Convex);
+	JPH_ASSERT(inShape2->GetType() == EShapeType::HeightField);
+	const ConvexShape *shape1 = static_cast<const ConvexShape *>(inShape1);
+	const HeightFieldShape *shape2 = static_cast<const HeightFieldShape *>(inShape2);
+
+	struct Visitor : public CollideConvexVsTriangles
+	{
+		using CollideConvexVsTriangles::CollideConvexVsTriangles;
+
+		JPH_INLINE bool				ShouldAbort() const
+		{
+			return mCollector.ShouldEarlyOut();
+		}
+
+		JPH_INLINE bool				ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const
+		{
+			return true;
+		}
+
+		JPH_INLINE int				VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const
+		{
+			// Scale the bounding boxes of this node
+			Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
+			AABox4Scale(mScale2, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+			// Test which nodes collide
+			UVec4 collides = AABox4VsBox(mBoundsOf1InSpaceOf2, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+			// Filter out invalid bounding boxes
+			collides = UVec4::sAnd(collides, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY));
+
+			return CountAndSortTrues(collides, ioProperties);
+		}
+
+		JPH_INLINE void				VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
+		{
+			// Create ID for triangle
+			SubShapeID triangle_sub_shape_id = mShape2->EncodeSubShapeID(mSubShapeIDCreator2, inX, inY, inTriangle);
+
+			// Determine active edges
+			uint8 active_edges = mShape2->GetEdgeFlags(inX, inY, inTriangle);
+
+			Collide(inV0, inV1, inV2, active_edges, triangle_sub_shape_id);
+		}
+
+		const HeightFieldShape *	mShape2;
+		SubShapeIDCreator			mSubShapeIDCreator2;
+	};
+
+	Visitor visitor(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector);
+	visitor.mShape2 = shape2;
+	visitor.mSubShapeIDCreator2 = inSubShapeIDCreator2;
+	shape2->WalkHeightField(visitor);
+}
+
+void HeightFieldShape::sCollideSphereVsHeightField(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Get the shapes
+	JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::Sphere);
+	JPH_ASSERT(inShape2->GetType() == EShapeType::HeightField);
+	const SphereShape *shape1 = static_cast<const SphereShape *>(inShape1);
+	const HeightFieldShape *shape2 = static_cast<const HeightFieldShape *>(inShape2);
+
+	struct Visitor : public CollideSphereVsTriangles
+	{
+		using CollideSphereVsTriangles::CollideSphereVsTriangles;
+
+		JPH_INLINE bool				ShouldAbort() const
+		{
+			return mCollector.ShouldEarlyOut();
+		}
+
+		JPH_INLINE bool				ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const
+		{
+			return true;
+		}
+
+		JPH_INLINE int				VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const
+		{
+			// Scale the bounding boxes of this node
+			Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
+			AABox4Scale(mScale2, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+			// Test which nodes collide
+			UVec4 collides = AABox4VsSphere(mSphereCenterIn2, mRadiusPlusMaxSeparationSq, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+			// Filter out invalid bounding boxes
+			collides = UVec4::sAnd(collides, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY));
+
+			return CountAndSortTrues(collides, ioProperties);
+		}
+
+		JPH_INLINE void				VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
+		{
+			// Create ID for triangle
+			SubShapeID triangle_sub_shape_id = mShape2->EncodeSubShapeID(mSubShapeIDCreator2, inX, inY, inTriangle);
+
+			// Determine active edges
+			uint8 active_edges = mShape2->GetEdgeFlags(inX, inY, inTriangle);
+
+			Collide(inV0, inV1, inV2, active_edges, triangle_sub_shape_id);
+		}
+
+		const HeightFieldShape *	mShape2;
+		SubShapeIDCreator			mSubShapeIDCreator2;
+	};
+
+	Visitor visitor(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector);
+	visitor.mShape2 = shape2;
+	visitor.mSubShapeIDCreator2 = inSubShapeIDCreator2;
+	shape2->WalkHeightField(visitor);
+}
+
+void HeightFieldShape::SaveBinaryState(StreamOut &inStream) const
+{
+	Shape::SaveBinaryState(inStream);
+
+	inStream.Write(mOffset);
+	inStream.Write(mScale);
+	inStream.Write(mSampleCount);
+	inStream.Write(mBlockSize);
+	inStream.Write(mBitsPerSample);
+	inStream.Write(mMinSample);
+	inStream.Write(mMaxSample);
+	inStream.Write(mMaterialIndices);
+	inStream.Write(mNumBitsPerMaterialIndex);
+
+	if (mRangeBlocks != nullptr)
+	{
+		inStream.Write(true);
+		inStream.WriteBytes(mRangeBlocks, mRangeBlocksSize * sizeof(RangeBlock) + mHeightSamplesSize + mActiveEdgesSize);
+	}
+	else
+	{
+		inStream.Write(false);
+	}
+}
+
+void HeightFieldShape::RestoreBinaryState(StreamIn &inStream)
+{
+	Shape::RestoreBinaryState(inStream);
+
+	inStream.Read(mOffset);
+	inStream.Read(mScale);
+	inStream.Read(mSampleCount);
+	inStream.Read(mBlockSize);
+	inStream.Read(mBitsPerSample);
+	inStream.Read(mMinSample);
+	inStream.Read(mMaxSample);
+	inStream.Read(mMaterialIndices);
+	inStream.Read(mNumBitsPerMaterialIndex);
+
+	// We don't have the exact number of reserved materials anymore, but ensure that our array is big enough
+	// TODO: Next time when we bump the binary serialization format of this class we should store the capacity and allocate the right amount, for now we accept a little bit of waste
+	mMaterials.reserve(PhysicsMaterialList::size_type(1) << mNumBitsPerMaterialIndex);
+
+	CacheValues();
+
+	bool has_heights = false;
+	inStream.Read(has_heights);
+	if (has_heights)
+	{
+		AllocateBuffers();
+		inStream.ReadBytes(mRangeBlocks, mRangeBlocksSize * sizeof(RangeBlock) + mHeightSamplesSize + mActiveEdgesSize);
+	}
+}
+
+void HeightFieldShape::SaveMaterialState(PhysicsMaterialList &outMaterials) const
+{
+	outMaterials = mMaterials;
+}
+
+void HeightFieldShape::RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials)
+{
+	mMaterials.assign(inMaterials, inMaterials + inNumMaterials);
+}
+
+Shape::Stats HeightFieldShape::GetStats() const
+{
+	return Stats(
+		sizeof(*this)
+			+ mMaterials.size() * sizeof(Ref<PhysicsMaterial>)
+			+ mRangeBlocksSize * sizeof(RangeBlock)
+			+ mHeightSamplesSize * sizeof(uint8)
+			+ mActiveEdgesSize * sizeof(uint8)
+			+ mMaterialIndices.size() * sizeof(uint8),
+		mHeightSamplesSize == 0? 0 : Square(mSampleCount - 1) * 2);
+}
+
+void HeightFieldShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::HeightField);
+	f.mConstruct = []() -> Shape * { return new HeightFieldShape; };
+	f.mColor = Color::sPurple;
+
+	for (EShapeSubType s : sConvexSubShapeTypes)
+	{
+		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::HeightField, sCollideConvexVsHeightField);
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::HeightField, sCastConvexVsHeightField);
+
+		CollisionDispatch::sRegisterCastShape(EShapeSubType::HeightField, s, CollisionDispatch::sReversedCastShape);
+		CollisionDispatch::sRegisterCollideShape(EShapeSubType::HeightField, s, CollisionDispatch::sReversedCollideShape);
+	}
+
+	// Specialized collision functions
+	CollisionDispatch::sRegisterCollideShape(EShapeSubType::Sphere, EShapeSubType::HeightField, sCollideSphereVsHeightField);
+	CollisionDispatch::sRegisterCastShape(EShapeSubType::Sphere, EShapeSubType::HeightField, sCastSphereVsHeightField);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/HeightFieldShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/HeightFieldShape.h
new file mode 100644
index 0000000000..16cbb76367
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/HeightFieldShape.h
@@ -0,0 +1,380 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+#include <Jolt/Physics/Collision/PhysicsMaterial.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+class ConvexShape;
+class CollideShapeSettings;
+class TempAllocator;
+
+/// Constants for HeightFieldShape, this was moved out of the HeightFieldShape because of a linker bug
+namespace HeightFieldShapeConstants
+{
+	/// Value used to create gaps in the height field
+	constexpr float					cNoCollisionValue = FLT_MAX;
+
+	/// Stack size to use during WalkHeightField
+	constexpr int					cStackSize = 128;
+
+	/// A position in the hierarchical grid is defined by a level (which grid), x and y position. We encode this in a single uint32 as: level << 28 | y << 14 | x
+	constexpr uint					cNumBitsXY = 14;
+	constexpr uint					cMaskBitsXY = (1 << cNumBitsXY) - 1;
+	constexpr uint					cLevelShift = 2 * cNumBitsXY;
+
+	/// When height samples are converted to 16 bit:
+	constexpr uint16				cNoCollisionValue16 = 0xffff;				///< This is the magic value for 'no collision'
+	constexpr uint16				cMaxHeightValue16 = 0xfffe;					///< This is the maximum allowed height value
+};
+
+/// Class that constructs a HeightFieldShape
+class JPH_EXPORT HeightFieldShapeSettings final : public ShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, HeightFieldShapeSettings)
+
+public:
+	/// Default constructor for deserialization
+									HeightFieldShapeSettings() = default;
+
+	/// Create a height field shape of inSampleCount * inSampleCount vertices.
+	/// The height field is a surface defined by: inOffset + inScale * (x, inSamples[y * inSampleCount + x], y).
+	/// where x and y are integers in the range x and y e [0, inSampleCount - 1].
+	/// inSampleCount: inSampleCount / mBlockSize must be minimally 2 and a power of 2 is the most efficient in terms of performance and storage.
+	/// inSamples: inSampleCount^2 vertices.
+	/// inMaterialIndices: (inSampleCount - 1)^2 indices that index into inMaterialList.
+									HeightFieldShapeSettings(const float *inSamples, Vec3Arg inOffset, Vec3Arg inScale, uint32 inSampleCount, const uint8 *inMaterialIndices = nullptr, const PhysicsMaterialList &inMaterialList = PhysicsMaterialList());
+
+	// See: ShapeSettings
+	virtual ShapeResult				Create() const override;
+
+	/// Determine the minimal and maximal value of mHeightSamples (will ignore cNoCollisionValue)
+	/// @param outMinValue The minimal value of mHeightSamples or FLT_MAX if no samples have collision
+	/// @param outMaxValue The maximal value of mHeightSamples or -FLT_MAX if no samples have collision
+	/// @param outQuantizationScale (value - outMinValue) * outQuantizationScale quantizes a height sample to 16 bits
+	void							DetermineMinAndMaxSample(float &outMinValue, float &outMaxValue, float &outQuantizationScale) const;
+
+	/// Given mBlockSize, mSampleCount and mHeightSamples, calculate the amount of bits needed to stay below absolute error inMaxError
+	/// @param inMaxError Maximum allowed error in mHeightSamples after compression (note that this does not take mScale.Y into account)
+	/// @return Needed bits per sample in the range [1, 8].
+	uint32							CalculateBitsPerSampleForError(float inMaxError) const;
+
+	/// The height field is a surface defined by: mOffset + mScale * (x, mHeightSamples[y * mSampleCount + x], y).
+	/// where x and y are integers in the range x and y e [0, mSampleCount - 1].
+	Vec3							mOffset = Vec3::sZero();
+	Vec3							mScale = Vec3::sReplicate(1.0f);
+	uint32							mSampleCount = 0;
+
+	/// Artificial minimal value of mHeightSamples, used for compression and can be used to update the terrain after creating with lower height values. If there are any lower values in mHeightSamples, this value will be ignored.
+	float							mMinHeightValue = FLT_MAX;
+
+	/// Artificial maximum value of mHeightSamples, used for compression and can be used to update the terrain after creating with higher height values. If there are any higher values in mHeightSamples, this value will be ignored.
+	float							mMaxHeightValue = -FLT_MAX;
+
+	/// When bigger than mMaterials.size() the internal material list will be preallocated to support this number of materials.
+	/// This avoids reallocations when calling HeightFieldShape::SetMaterials with new materials later.
+	uint32							mMaterialsCapacity = 0;
+
+	/// The heightfield is divided in blocks of mBlockSize * mBlockSize * 2 triangles and the acceleration structure culls blocks only,
+	/// bigger block sizes reduce memory consumption but also reduce query performance. Sensible values are [2, 8], does not need to be
+	/// a power of 2. Note that at run-time we'll perform one more grid subdivision, so the effective block size is half of what is provided here.
+	uint32							mBlockSize = 2;
+
+	/// How many bits per sample to use to compress the height field. Can be in the range [1, 8].
+	/// Note that each sample is compressed relative to the min/max value of its block of mBlockSize * mBlockSize pixels so the effective precision is higher.
+	/// Also note that increasing mBlockSize saves more memory than reducing the amount of bits per sample.
+	uint32							mBitsPerSample = 8;
+
+	/// An array of mSampleCount^2 height samples. Samples are stored in row major order, so the sample at (x, y) is at index y * mSampleCount + x.
+	Array<float>					mHeightSamples;
+
+	/// An array of (mSampleCount - 1)^2 material indices.
+	Array<uint8>					mMaterialIndices;
+
+	/// The materials of square at (x, y) is: mMaterials[mMaterialIndices[x + y * (mSampleCount - 1)]]
+	PhysicsMaterialList				mMaterials;
+
+	/// Cosine of the threshold angle (if the angle between the two triangles is bigger than this, the edge is active, note that a concave edge is always inactive).
+	/// Setting this value too small can cause ghost collisions with edges, setting it too big can cause depenetration artifacts (objects not depenetrating quickly).
+	/// Valid ranges are between cos(0 degrees) and cos(90 degrees). The default value is cos(5 degrees).
+	float							mActiveEdgeCosThresholdAngle = 0.996195f;	// cos(5 degrees)
+};
+
+/// A height field shape. Cannot be used as a dynamic object.
+///
+/// Note: If you're using HeightFieldShape and are querying data while modifying the shape you'll have a race condition.
+/// In this case it is best to create a new HeightFieldShape using the Clone function. You replace the shape on a body using BodyInterface::SetShape.
+/// If a query is still working on the old shape, it will have taken a reference and keep the old shape alive until the query finishes.
+class JPH_EXPORT HeightFieldShape final : public Shape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+									HeightFieldShape() : Shape(EShapeType::HeightField, EShapeSubType::HeightField) { }
+									HeightFieldShape(const HeightFieldShapeSettings &inSettings, ShapeResult &outResult);
+	virtual							~HeightFieldShape() override;
+
+	/// Clone this shape. Can be used to avoid race conditions. See the documentation of this class for more information.
+	Ref<HeightFieldShape>			Clone() const;
+
+	// See Shape::MustBeStatic
+	virtual bool					MustBeStatic() const override				{ return true; }
+
+	/// Get the size of the height field. Note that this will always be rounded up to the nearest multiple of GetBlockSize().
+	inline uint						GetSampleCount() const						{ return mSampleCount; }
+
+	/// Get the size of a block
+	inline uint						GetBlockSize() const						{ return mBlockSize; }
+
+	// See Shape::GetLocalBounds
+	virtual AABox					GetLocalBounds() const override;
+
+	// See Shape::GetSubShapeIDBitsRecursive
+	virtual uint					GetSubShapeIDBitsRecursive() const override	{ return GetSubShapeIDBits(); }
+
+	// See Shape::GetInnerRadius
+	virtual float					GetInnerRadius() const override				{ return 0.0f; }
+
+	// See Shape::GetMassProperties
+	virtual MassProperties			GetMassProperties() const override;
+
+	// See Shape::GetMaterial
+	virtual const PhysicsMaterial *	GetMaterial(const SubShapeID &inSubShapeID) const override;
+
+	/// Overload to get the material at a particular location
+	const PhysicsMaterial *			GetMaterial(uint inX, uint inY) const;
+
+	// See Shape::GetSurfaceNormal
+	virtual Vec3					GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override;
+
+	// See Shape::GetSupportingFace
+	virtual void					GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
+
+	// See Shape::GetSubmergedVolume
+	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override { JPH_ASSERT(false, "Not supported"); }
+
+#ifdef JPH_DEBUG_RENDERER
+	// See Shape::Draw
+	virtual void					Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// See Shape::CastRay
+	virtual bool					CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+	virtual void					CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollidePoint
+	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollideSoftBodyVertices
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
+
+	// See Shape::GetTrianglesStart
+	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
+
+	// See Shape::GetTrianglesNext
+	virtual int						GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override;
+
+	/// Get height field position at sampled location (inX, inY).
+	/// where inX and inY are integers in the range inX e [0, mSampleCount - 1] and inY e [0, mSampleCount - 1].
+	Vec3							GetPosition(uint inX, uint inY) const;
+
+	/// Check if height field at sampled location (inX, inY) has collision (has a hole or not)
+	bool							IsNoCollision(uint inX, uint inY) const;
+
+	/// Projects inLocalPosition (a point in the space of the shape) along the Y axis onto the surface and returns it in outSurfacePosition.
+	/// When there is no surface position (because of a hole or because the point is outside the heightfield) the function will return false.
+	bool							ProjectOntoSurface(Vec3Arg inLocalPosition, Vec3 &outSurfacePosition, SubShapeID &outSubShapeID) const;
+
+	/// Returns the coordinates of the triangle that a sub shape ID represents
+	/// @param inSubShapeID The sub shape ID to decode
+	/// @param outX X coordinate of the triangle (in the range [0, mSampleCount - 2])
+	/// @param outY Y coordinate of the triangle (in the range [0, mSampleCount - 2])
+	/// @param outTriangleIndex Triangle within the quad (0 = lower triangle or 1 = upper triangle)
+	void							GetSubShapeCoordinates(const SubShapeID &inSubShapeID, uint &outX, uint &outY, uint &outTriangleIndex) const;
+
+	/// Get the range of height values that this height field can encode. Can be used to determine the allowed range when setting the height values with SetHeights.
+	float							GetMinHeightValue() const					{ return mOffset.GetY(); }
+	float							GetMaxHeightValue() const					{ return mOffset.GetY() + mScale.GetY() * HeightFieldShapeConstants::cMaxHeightValue16; }
+
+	/// Get the height values of a block of data.
+	/// Note that the height values are decompressed so will be slightly different from what the shape was originally created with.
+	/// @param inX Start X position, must be a multiple of mBlockSize and in the range [0, mSampleCount - 1]
+	/// @param inY Start Y position, must be a multiple of mBlockSize and in the range [0, mSampleCount - 1]
+	/// @param inSizeX Number of samples in X direction, must be a multiple of mBlockSize and in the range [0, mSampleCount - inX]
+	/// @param inSizeY Number of samples in Y direction, must be a multiple of mBlockSize and in the range [0, mSampleCount - inY]
+	/// @param outHeights Returned height values, must be at least inSizeX * inSizeY floats. Values are returned in x-major order and can be cNoCollisionValue.
+	/// @param inHeightsStride Stride in floats between two consecutive rows of outHeights (can be negative if the data is upside down).
+	void							GetHeights(uint inX, uint inY, uint inSizeX, uint inSizeY, float *outHeights, intptr_t inHeightsStride) const;
+
+	/// Set the height values of a block of data.
+	/// Note that this requires decompressing and recompressing a border of size mBlockSize in the negative x/y direction so will cause some precision loss.
+	/// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information.
+	/// @param inX Start X position, must be a multiple of mBlockSize and in the range [0, mSampleCount - 1]
+	/// @param inY Start Y position, must be a multiple of mBlockSize and in the range [0, mSampleCount - 1]
+	/// @param inSizeX Number of samples in X direction, must be a multiple of mBlockSize and in the range [0, mSampleCount - inX]
+	/// @param inSizeY Number of samples in Y direction, must be a multiple of mBlockSize and in the range [0, mSampleCount - inY]
+	/// @param inHeights The new height values to set, must be an array of inSizeX * inSizeY floats, can be cNoCollisionValue. Values outside of the range [GetMinHeightValue(), GetMaxHeightValue()] will be clamped.
+	/// @param inHeightsStride Stride in floats between two consecutive rows of inHeights (can be negative if the data is upside down).
+	/// @param inAllocator Allocator to use for temporary memory
+	/// @param inActiveEdgeCosThresholdAngle Cosine of the threshold angle (if the angle between the two triangles is bigger than this, the edge is active, note that a concave edge is always inactive).
+	void							SetHeights(uint inX, uint inY, uint inSizeX, uint inSizeY, const float *inHeights, intptr_t inHeightsStride, TempAllocator &inAllocator, float inActiveEdgeCosThresholdAngle = 0.996195f);
+
+	/// Get the current list of materials, the indices returned by GetMaterials() will index into this list.
+	const PhysicsMaterialList &		GetMaterialList() const						{ return mMaterials; }
+
+	/// Get the material indices of a block of data.
+	/// @param inX Start X position, must in the range [0, mSampleCount - 1]
+	/// @param inY Start Y position, must in the range [0, mSampleCount - 1]
+	/// @param inSizeX Number of samples in X direction
+	/// @param inSizeY Number of samples in Y direction
+	/// @param outMaterials Returned material indices, must be at least inSizeX * inSizeY uint8s. Values are returned in x-major order.
+	/// @param inMaterialsStride Stride in uint8s between two consecutive rows of outMaterials (can be negative if the data is upside down).
+	void							GetMaterials(uint inX, uint inY, uint inSizeX, uint inSizeY, uint8 *outMaterials, intptr_t inMaterialsStride) const;
+
+	/// Set the material indices of a block of data.
+	/// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information.
+	/// @param inX Start X position, must in the range [0, mSampleCount - 1]
+	/// @param inY Start Y position, must in the range [0, mSampleCount - 1]
+	/// @param inSizeX Number of samples in X direction
+	/// @param inSizeY Number of samples in Y direction
+	/// @param inMaterials The new material indices, must be at least inSizeX * inSizeY uint8s. Values are returned in x-major order.
+	/// @param inMaterialsStride Stride in uint8s between two consecutive rows of inMaterials (can be negative if the data is upside down).
+	/// @param inMaterialList The material list to use for the new material indices or nullptr if the material list should not be updated
+	/// @param inAllocator Allocator to use for temporary memory
+	/// @return True if the material indices were set, false if the total number of materials exceeded 256
+	bool							SetMaterials(uint inX, uint inY, uint inSizeX, uint inSizeY, const uint8 *inMaterials, intptr_t inMaterialsStride, const PhysicsMaterialList *inMaterialList, TempAllocator &inAllocator);
+
+	// See Shape
+	virtual void					SaveBinaryState(StreamOut &inStream) const override;
+	virtual void					SaveMaterialState(PhysicsMaterialList &outMaterials) const override;
+	virtual void					RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) override;
+
+	// See Shape::GetStats
+	virtual Stats					GetStats() const override;
+
+	// See Shape::GetVolume
+	virtual float					GetVolume() const override					{ return 0; }
+
+#ifdef JPH_DEBUG_RENDERER
+	// Settings
+	static bool						sDrawTriangleOutlines;
+#endif // JPH_DEBUG_RENDERER
+
+	// Register shape functions with the registry
+	static void						sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void					RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	class							DecodingContext;							///< Context class for walking through all nodes of a heightfield
+	struct							HSGetTrianglesContext;						///< Context class for GetTrianglesStart/Next
+
+	/// Calculate commonly used values and store them in the shape
+	void							CacheValues();
+
+	/// Allocate the mRangeBlocks, mHeightSamples and mActiveEdges buffers as a single data block
+	void							AllocateBuffers();
+
+	/// Calculate bit mask for all active edges in the heightfield for a specific region
+	void							CalculateActiveEdges(uint inX, uint inY, uint inSizeX, uint inSizeY, const float *inHeights, uint inHeightsStartX, uint inHeightsStartY, intptr_t inHeightsStride, float inHeightsScale, float inActiveEdgeCosThresholdAngle, TempAllocator &inAllocator);
+
+	/// Calculate bit mask for all active edges in the heightfield
+	void							CalculateActiveEdges(const HeightFieldShapeSettings &inSettings);
+
+	/// Store material indices in the least amount of bits per index possible
+	void							StoreMaterialIndices(const HeightFieldShapeSettings &inSettings);
+
+	/// Get the amount of horizontal/vertical blocks
+	inline uint						GetNumBlocks() const						{ return mSampleCount / mBlockSize; }
+
+	/// Get the maximum level (amount of grids) of the tree
+	static inline uint				sGetMaxLevel(uint inNumBlocks)				{ return 32 - CountLeadingZeros(inNumBlocks - 1); }
+
+	/// Get the range block offset and stride for GetBlockOffsetAndScale
+	static inline void				sGetRangeBlockOffsetAndStride(uint inNumBlocks, uint inMaxLevel, uint &outRangeBlockOffset, uint &outRangeBlockStride);
+
+	/// For block (inBlockX, inBlockY) get the offset and scale needed to decode a uint8 height sample to a uint16
+	inline void						GetBlockOffsetAndScale(uint inBlockX, uint inBlockY, uint inRangeBlockOffset, uint inRangeBlockStride, float &outBlockOffset, float &outBlockScale) const;
+
+	/// Get the height sample at position (inX, inY)
+	inline uint8					GetHeightSample(uint inX, uint inY) const;
+
+	/// Faster version of GetPosition when block offset and scale are already known
+	inline Vec3						GetPosition(uint inX, uint inY, float inBlockOffset, float inBlockScale, bool &outNoCollision) const;
+
+	/// Determine amount of bits needed to encode sub shape id
+	uint							GetSubShapeIDBits() const;
+
+	/// En/decode a sub shape ID. inX and inY specify the coordinate of the triangle. inTriangle == 0 is the lower triangle, inTriangle == 1 is the upper triangle.
+	inline SubShapeID				EncodeSubShapeID(const SubShapeIDCreator &inCreator, uint inX, uint inY, uint inTriangle) const;
+	inline void						DecodeSubShapeID(const SubShapeID &inSubShapeID, uint &outX, uint &outY, uint &outTriangle) const;
+
+	/// Get the edge flags for a triangle
+	inline uint8					GetEdgeFlags(uint inX, uint inY, uint inTriangle) const;
+
+	// Helper functions called by CollisionDispatch
+	static void						sCollideConvexVsHeightField(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCollideSphereVsHeightField(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCastConvexVsHeightField(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+	static void						sCastSphereVsHeightField(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+
+	/// Visit the entire height field using a visitor pattern
+	/// Note: Used to be inlined but this triggers a bug in MSVC where it will not free the memory allocated by alloca which causes a stack overflow when WalkHeightField is called in a loop (clang does it correct)
+	template <class Visitor>
+	void							WalkHeightField(Visitor &ioVisitor) const;
+
+	/// A block of 2x2 ranges used to form a hierarchical grid, ordered left top, right top, left bottom, right bottom
+	struct alignas(16) RangeBlock
+	{
+		uint16						mMin[4];
+		uint16						mMax[4];
+	};
+
+	/// For block (inBlockX, inBlockY) get the range block and the entry in the range block
+	inline void						GetRangeBlock(uint inBlockX, uint inBlockY, uint inRangeBlockOffset, uint inRangeBlockStride, RangeBlock *&outBlock, uint &outIndexInBlock);
+
+	/// Offset of first RangedBlock in grid per level
+	static const uint				sGridOffsets[];
+
+	/// The height field is a surface defined by: mOffset + mScale * (x, mHeightSamples[y * mSampleCount + x], y).
+	/// where x and y are integers in the range x and y e [0, mSampleCount - 1].
+	Vec3							mOffset = Vec3::sZero();
+	Vec3							mScale = Vec3::sReplicate(1.0f);
+
+	/// Height data
+	uint32							mSampleCount = 0;							///< See HeightFieldShapeSettings::mSampleCount
+	uint32							mBlockSize = 2;								///< See HeightFieldShapeSettings::mBlockSize
+	uint32							mHeightSamplesSize = 0;						///< Size of mHeightSamples in bytes
+	uint32							mRangeBlocksSize = 0;						///< Size of mRangeBlocks in elements
+	uint32							mActiveEdgesSize = 0;						///< Size of mActiveEdges in bytes
+	uint8							mBitsPerSample = 8;							///< See HeightFieldShapeSettings::mBitsPerSample
+	uint8							mSampleMask = 0xff;							///< All bits set for a sample: (1 << mBitsPerSample) - 1, used to indicate that there's no collision
+	uint16							mMinSample = HeightFieldShapeConstants::cNoCollisionValue16; ///< Min and max value in mHeightSamples quantized to 16 bit, for calculating bounding box
+	uint16							mMaxSample = HeightFieldShapeConstants::cNoCollisionValue16;
+	RangeBlock *					mRangeBlocks = nullptr;						///< Hierarchical grid of range data describing the height variations within 1 block. The grid for level <level> starts at offset sGridOffsets[<level>]
+	uint8 *							mHeightSamples = nullptr;					///< mBitsPerSample-bit height samples. Value [0, mMaxHeightValue] maps to highest detail grid in mRangeBlocks [mMin, mMax]. mNoCollisionValue is reserved to indicate no collision.
+	uint8 *							mActiveEdges = nullptr;						///< (mSampleCount - 1)^2 * 3-bit active edge flags.
+
+	/// Materials
+	PhysicsMaterialList				mMaterials;									///< The materials of square at (x, y) is: mMaterials[mMaterialIndices[x + y * (mSampleCount - 1)]]
+	Array<uint8>					mMaterialIndices;							///< Compressed to the minimum amount of bits per material index (mSampleCount - 1) * (mSampleCount - 1) * mNumBitsPerMaterialIndex bits of data
+	uint32							mNumBitsPerMaterialIndex = 0;				///< Number of bits per material index
+
+#ifdef JPH_DEBUG_RENDERER
+	/// Temporary rendering data
+	mutable Array<DebugRenderer::GeometryRef> mGeometry;
+	mutable bool					mCachedUseMaterialColors = false;			///< This is used to regenerate the triangle batch if the drawing settings change
+#endif // JPH_DEBUG_RENDERER
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.cpp
new file mode 100644
index 0000000000..852021a040
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.cpp
@@ -0,0 +1,1266 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/Shape/MeshShape.h>
+#include <Jolt/Physics/Collision/Shape/ConvexShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/ShapeFilter.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/CollideConvexVsTriangles.h>
+#include <Jolt/Physics/Collision/CollideSphereVsTriangles.h>
+#include <Jolt/Physics/Collision/CastConvexVsTriangles.h>
+#include <Jolt/Physics/Collision/CastSphereVsTriangles.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/ActiveEdges.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Physics/Collision/SortReverseAndStore.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h>
+#include <Jolt/Core/StringTools.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/Core/Profiler.h>
+#include <Jolt/Core/UnorderedMap.h>
+#include <Jolt/Geometry/AABox4.h>
+#include <Jolt/Geometry/RayAABox.h>
+#include <Jolt/Geometry/Indexify.h>
+#include <Jolt/Geometry/Plane.h>
+#include <Jolt/Geometry/OrientedBox.h>
+#include <Jolt/TriangleSplitter/TriangleSplitterBinning.h>
+#include <Jolt/AABBTree/AABBTreeBuilder.h>
+#include <Jolt/AABBTree/AABBTreeToBuffer.h>
+#include <Jolt/AABBTree/TriangleCodec/TriangleCodecIndexed8BitPackSOA4Flags.h>
+#include <Jolt/AABBTree/NodeCodec/NodeCodecQuadTreeHalfFloat.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+JPH_NAMESPACE_BEGIN
+
+#ifdef JPH_DEBUG_RENDERER
+bool MeshShape::sDrawTriangleGroups = false;
+bool MeshShape::sDrawTriangleOutlines = false;
+#endif // JPH_DEBUG_RENDERER
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(MeshShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(MeshShapeSettings, ShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(MeshShapeSettings, mTriangleVertices)
+	JPH_ADD_ATTRIBUTE(MeshShapeSettings, mIndexedTriangles)
+	JPH_ADD_ATTRIBUTE(MeshShapeSettings, mMaterials)
+	JPH_ADD_ATTRIBUTE(MeshShapeSettings, mMaxTrianglesPerLeaf)
+	JPH_ADD_ATTRIBUTE(MeshShapeSettings, mActiveEdgeCosThresholdAngle)
+	JPH_ADD_ATTRIBUTE(MeshShapeSettings, mPerTriangleUserData)
+}
+
+// Codecs this mesh shape is using
+using TriangleCodec = TriangleCodecIndexed8BitPackSOA4Flags;
+using NodeCodec = NodeCodecQuadTreeHalfFloat;
+
+// Get header for tree
+static JPH_INLINE const NodeCodec::Header *sGetNodeHeader(const ByteBuffer &inTree)
+{
+	return inTree.Get<NodeCodec::Header>(0);
+}
+
+// Get header for triangles
+static JPH_INLINE const TriangleCodec::TriangleHeader *sGetTriangleHeader(const ByteBuffer &inTree)
+{
+	return inTree.Get<TriangleCodec::TriangleHeader>(NodeCodec::HeaderSize);
+}
+
+MeshShapeSettings::MeshShapeSettings(const TriangleList &inTriangles, PhysicsMaterialList inMaterials) :
+	mMaterials(std::move(inMaterials))
+{
+	Indexify(inTriangles, mTriangleVertices, mIndexedTriangles);
+
+	Sanitize();
+}
+
+MeshShapeSettings::MeshShapeSettings(VertexList inVertices, IndexedTriangleList inTriangles, PhysicsMaterialList inMaterials) :
+	mTriangleVertices(std::move(inVertices)),
+	mIndexedTriangles(std::move(inTriangles)),
+	mMaterials(std::move(inMaterials))
+{
+	Sanitize();
+}
+
+void MeshShapeSettings::Sanitize()
+{
+	// Remove degenerate and duplicate triangles
+	UnorderedSet<IndexedTriangle> triangles;
+	triangles.reserve(UnorderedSet<IndexedTriangle>::size_type(mIndexedTriangles.size()));
+	TriangleCodec::ValidationContext validation_ctx(mIndexedTriangles, mTriangleVertices);
+	for (int t = (int)mIndexedTriangles.size() - 1; t >= 0; --t)
+	{
+		const IndexedTriangle &tri = mIndexedTriangles[t];
+
+		if (tri.IsDegenerate(mTriangleVertices)						// Degenerate triangle
+			|| validation_ctx.IsDegenerate(tri)						// Triangle is degenerate in the quantized space
+			|| !triangles.insert(tri.GetLowestIndexFirst()).second) // Duplicate triangle
+		{
+			// The order of triangles doesn't matter (gets reordered while building the tree), so we can just swap the last triangle into this slot
+			mIndexedTriangles[t] = mIndexedTriangles.back();
+			mIndexedTriangles.pop_back();
+		}
+	}
+}
+
+ShapeSettings::ShapeResult MeshShapeSettings::Create() const
+{
+	if (mCachedResult.IsEmpty())
+		Ref<Shape> shape = new MeshShape(*this, mCachedResult);
+	return mCachedResult;
+}
+
+MeshShape::MeshShape(const MeshShapeSettings &inSettings, ShapeResult &outResult) :
+	Shape(EShapeType::Mesh, EShapeSubType::Mesh, inSettings, outResult)
+{
+	// Check if there are any triangles
+	if (inSettings.mIndexedTriangles.empty())
+	{
+		outResult.SetError("Need triangles to create a mesh shape!");
+		return;
+	}
+
+	// Check triangles
+	TriangleCodec::ValidationContext validation_ctx(inSettings.mIndexedTriangles, inSettings.mTriangleVertices);
+	for (int t = (int)inSettings.mIndexedTriangles.size() - 1; t >= 0; --t)
+	{
+		const IndexedTriangle &triangle = inSettings.mIndexedTriangles[t];
+		if (triangle.IsDegenerate(inSettings.mTriangleVertices)
+			|| validation_ctx.IsDegenerate(triangle))
+		{
+			outResult.SetError(StringFormat("Triangle %d is degenerate!", t));
+			return;
+		}
+		else
+		{
+			// Check vertex indices
+			for (uint32 idx : triangle.mIdx)
+				if (idx >= inSettings.mTriangleVertices.size())
+				{
+					outResult.SetError(StringFormat("Vertex index %u is beyond vertex list (size: %u)", idx, (uint)inSettings.mTriangleVertices.size()));
+					return;
+				}
+		}
+	}
+
+	// Copy materials
+	mMaterials = inSettings.mMaterials;
+	if (!mMaterials.empty())
+	{
+		// Validate materials
+		if (mMaterials.size() > (1 << FLAGS_MATERIAL_BITS))
+		{
+			outResult.SetError(StringFormat("Supporting max %d materials per mesh", 1 << FLAGS_MATERIAL_BITS));
+			return;
+		}
+		for (const IndexedTriangle &t : inSettings.mIndexedTriangles)
+			if (t.mMaterialIndex >= mMaterials.size())
+			{
+				outResult.SetError(StringFormat("Triangle material %u is beyond material list (size: %u)", t.mMaterialIndex, (uint)mMaterials.size()));
+				return;
+			}
+	}
+	else
+	{
+		// No materials assigned, validate that all triangles use material index 0
+		for (const IndexedTriangle &t : inSettings.mIndexedTriangles)
+			if (t.mMaterialIndex != 0)
+			{
+				outResult.SetError("No materials present, all triangles should have material index 0");
+				return;
+			}
+	}
+
+	// Check max triangles
+	if (inSettings.mMaxTrianglesPerLeaf < 1 || inSettings.mMaxTrianglesPerLeaf > MaxTrianglesPerLeaf)
+	{
+		outResult.SetError("Invalid max triangles per leaf");
+		return;
+	}
+
+	// Fill in active edge bits
+	IndexedTriangleList indexed_triangles = inSettings.mIndexedTriangles; // Copy indices since we're adding the 'active edge' flag
+	sFindActiveEdges(inSettings, indexed_triangles);
+
+	// Create triangle splitter
+	TriangleSplitterBinning splitter(inSettings.mTriangleVertices, indexed_triangles);
+
+	// Build tree
+	AABBTreeBuilder builder(splitter, inSettings.mMaxTrianglesPerLeaf);
+	AABBTreeBuilderStats builder_stats;
+	const AABBTreeBuilder::Node *root = builder.Build(builder_stats);
+
+	// Convert to buffer
+	AABBTreeToBuffer<TriangleCodec, NodeCodec> buffer;
+	const char *error = nullptr;
+	if (!buffer.Convert(builder.GetTriangles(), builder.GetNodes(), inSettings.mTriangleVertices, root, inSettings.mPerTriangleUserData, error))
+	{
+		outResult.SetError(error);
+		return;
+	}
+
+	// Move data to this class
+	mTree.swap(buffer.GetBuffer());
+
+	// Check if we're not exceeding the amount of sub shape id bits
+	if (GetSubShapeIDBitsRecursive() > SubShapeID::MaxBits)
+	{
+		outResult.SetError("Mesh is too big and exceeds the amount of available sub shape ID bits");
+		return;
+	}
+
+	outResult.Set(this);
+}
+
+void MeshShape::sFindActiveEdges(const MeshShapeSettings &inSettings, IndexedTriangleList &ioIndices)
+{
+	// A struct to hold the two vertex indices of an edge
+	struct Edge
+	{
+				Edge(int inIdx1, int inIdx2) : mIdx1(min(inIdx1, inIdx2)), mIdx2(max(inIdx1, inIdx2)) { }
+
+		uint	GetIndexInTriangle(const IndexedTriangle &inTriangle) const
+		{
+			for (uint edge_idx = 0; edge_idx < 3; ++edge_idx)
+			{
+				Edge edge(inTriangle.mIdx[edge_idx], inTriangle.mIdx[(edge_idx + 1) % 3]);
+				if (*this == edge)
+					return edge_idx;
+			}
+
+			JPH_ASSERT(false);
+			return ~uint(0);
+		}
+
+		bool	operator == (const Edge &inRHS) const
+		{
+			return mIdx1 == inRHS.mIdx1 && mIdx2 == inRHS.mIdx2;
+		}
+
+		uint64	GetHash() const
+		{
+			static_assert(sizeof(*this) == 2 * sizeof(int), "No padding expected");
+			return HashBytes(this, sizeof(*this));
+		}
+
+		int		mIdx1;
+		int		mIdx2;
+	};
+
+	// A struct to hold the triangles that are connected to an edge
+	struct TriangleIndices
+	{
+		uint	mNumTriangles = 0;
+		uint	mTriangleIndices[2];
+	};
+
+	// Build a list of edge to triangles
+	using EdgeToTriangle = UnorderedMap<Edge, TriangleIndices>;
+	EdgeToTriangle edge_to_triangle;
+	edge_to_triangle.reserve(EdgeToTriangle::size_type(ioIndices.size() * 3));
+	for (uint triangle_idx = 0; triangle_idx < ioIndices.size(); ++triangle_idx)
+	{
+		IndexedTriangle &triangle = ioIndices[triangle_idx];
+		for (uint edge_idx = 0; edge_idx < 3; ++edge_idx)
+		{
+			Edge edge(triangle.mIdx[edge_idx], triangle.mIdx[(edge_idx + 1) % 3]);
+			EdgeToTriangle::iterator edge_to_triangle_it = edge_to_triangle.try_emplace(edge, TriangleIndices()).first;
+			TriangleIndices &indices = edge_to_triangle_it->second;
+			if (indices.mNumTriangles < 2)
+			{
+				// Store index of triangle that connects to this edge
+				indices.mTriangleIndices[indices.mNumTriangles] = triangle_idx;
+				indices.mNumTriangles++;
+			}
+			else
+			{
+				// 3 or more triangles share an edge, mark this edge as active
+				uint32 mask = 1 << (edge_idx + FLAGS_ACTIVE_EGDE_SHIFT);
+				JPH_ASSERT((triangle.mMaterialIndex & mask) == 0);
+				triangle.mMaterialIndex |= mask;
+			}
+		}
+	}
+
+	// Walk over all edges and determine which ones are active
+	for (const EdgeToTriangle::value_type &edge : edge_to_triangle)
+	{
+		uint num_active = 0;
+		if (edge.second.mNumTriangles == 1)
+		{
+			// Edge is not shared, it is an active edge
+			num_active = 1;
+		}
+		else if (edge.second.mNumTriangles == 2)
+		{
+			// Simple shared edge, determine if edge is active based on the two adjacent triangles
+			const IndexedTriangle &triangle1 = ioIndices[edge.second.mTriangleIndices[0]];
+			const IndexedTriangle &triangle2 = ioIndices[edge.second.mTriangleIndices[1]];
+
+			// Find which edge this is for both triangles
+			uint edge_idx1 = edge.first.GetIndexInTriangle(triangle1);
+			uint edge_idx2 = edge.first.GetIndexInTriangle(triangle2);
+
+			// Construct a plane for triangle 1 (e1 = edge vertex 1, e2 = edge vertex 2, op = opposing vertex)
+			Vec3 triangle1_e1 = Vec3(inSettings.mTriangleVertices[triangle1.mIdx[edge_idx1]]);
+			Vec3 triangle1_e2 = Vec3(inSettings.mTriangleVertices[triangle1.mIdx[(edge_idx1 + 1) % 3]]);
+			Vec3 triangle1_op = Vec3(inSettings.mTriangleVertices[triangle1.mIdx[(edge_idx1 + 2) % 3]]);
+			Plane triangle1_plane = Plane::sFromPointsCCW(triangle1_e1, triangle1_e2, triangle1_op);
+
+			// Construct a plane for triangle 2
+			Vec3 triangle2_e1 = Vec3(inSettings.mTriangleVertices[triangle2.mIdx[edge_idx2]]);
+			Vec3 triangle2_e2 = Vec3(inSettings.mTriangleVertices[triangle2.mIdx[(edge_idx2 + 1) % 3]]);
+			Vec3 triangle2_op = Vec3(inSettings.mTriangleVertices[triangle2.mIdx[(edge_idx2 + 2) % 3]]);
+			Plane triangle2_plane = Plane::sFromPointsCCW(triangle2_e1, triangle2_e2, triangle2_op);
+
+			// Determine if the edge is active
+			num_active = ActiveEdges::IsEdgeActive(triangle1_plane.GetNormal(), triangle2_plane.GetNormal(), triangle1_e2 - triangle1_e1, inSettings.mActiveEdgeCosThresholdAngle)? 2 : 0;
+		}
+		else
+		{
+			// More edges incoming, we've already marked all edges beyond the 2nd as active
+			num_active = 2;
+		}
+
+		// Mark edges of all original triangles active
+		for (uint i = 0; i < num_active; ++i)
+		{
+			uint triangle_idx = edge.second.mTriangleIndices[i];
+			IndexedTriangle &triangle = ioIndices[triangle_idx];
+			uint edge_idx = edge.first.GetIndexInTriangle(triangle);
+			uint32 mask = 1 << (edge_idx + FLAGS_ACTIVE_EGDE_SHIFT);
+			JPH_ASSERT((triangle.mMaterialIndex & mask) == 0);
+			triangle.mMaterialIndex |= mask;
+		}
+	}
+}
+
+MassProperties MeshShape::GetMassProperties() const
+{
+	// We cannot calculate the volume for an arbitrary mesh, so we return invalid mass properties.
+	// If you want your mesh to be dynamic, then you should provide the mass properties yourself when
+	// creating a Body:
+	//
+	// BodyCreationSettings::mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided;
+	// BodyCreationSettings::mMassPropertiesOverride.SetMassAndInertiaOfSolidBox(Vec3::sReplicate(1.0f), 1000.0f);
+	//
+	// Note that for a mesh shape to simulate properly, it is best if the mesh is manifold
+	// (i.e. closed, all edges shared by only two triangles, consistent winding order).
+	return MassProperties();
+}
+
+void MeshShape::DecodeSubShapeID(const SubShapeID &inSubShapeID, const void *&outTriangleBlock, uint32 &outTriangleIndex) const
+{
+	// Get block
+	SubShapeID triangle_idx_subshape_id;
+	uint32 block_id = inSubShapeID.PopID(NodeCodec::DecodingContext::sTriangleBlockIDBits(sGetNodeHeader(mTree)), triangle_idx_subshape_id);
+	outTriangleBlock = NodeCodec::DecodingContext::sGetTriangleBlockStart(&mTree[0], block_id);
+
+	// Fetch the triangle index
+	SubShapeID remainder;
+	outTriangleIndex = triangle_idx_subshape_id.PopID(NumTriangleBits, remainder);
+	JPH_ASSERT(remainder.IsEmpty(), "Invalid subshape ID");
+}
+
+uint MeshShape::GetMaterialIndex(const SubShapeID &inSubShapeID) const
+{
+	// Decode ID
+	const void *block_start;
+	uint32 triangle_idx;
+	DecodeSubShapeID(inSubShapeID, block_start, triangle_idx);
+
+	// Fetch the flags
+	uint8 flags = TriangleCodec::DecodingContext::sGetFlags(block_start, triangle_idx);
+	return flags & FLAGS_MATERIAL_MASK;
+}
+
+const PhysicsMaterial *MeshShape::GetMaterial(const SubShapeID &inSubShapeID) const
+{
+	// Return the default material if there are no materials on this shape
+	if (mMaterials.empty())
+		return PhysicsMaterial::sDefault;
+
+	return mMaterials[GetMaterialIndex(inSubShapeID)];
+}
+
+Vec3 MeshShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	// Decode ID
+	const void *block_start;
+	uint32 triangle_idx;
+	DecodeSubShapeID(inSubShapeID, block_start, triangle_idx);
+
+	// Decode triangle
+	Vec3 v1, v2, v3;
+	const TriangleCodec::DecodingContext triangle_ctx(sGetTriangleHeader(mTree));
+	triangle_ctx.GetTriangle(block_start, triangle_idx, v1, v2, v3);
+
+	// Calculate normal
+	return (v3 - v2).Cross(v1 - v2).Normalized();
+}
+
+void MeshShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
+	// Decode ID
+	const void *block_start;
+	uint32 triangle_idx;
+	DecodeSubShapeID(inSubShapeID, block_start, triangle_idx);
+
+	// Decode triangle
+	const TriangleCodec::DecodingContext triangle_ctx(sGetTriangleHeader(mTree));
+	outVertices.resize(3);
+	triangle_ctx.GetTriangle(block_start, triangle_idx, outVertices[0], outVertices[1], outVertices[2]);
+
+	// Flip triangle if scaled inside out
+	if (ScaleHelpers::IsInsideOut(inScale))
+		std::swap(outVertices[1], outVertices[2]);
+
+	// Calculate transform with scale
+	Mat44 transform = inCenterOfMassTransform.PreScaled(inScale);
+
+	// Transform to world space
+	for (Vec3 &v : outVertices)
+		v = transform * v;
+}
+
+AABox MeshShape::GetLocalBounds() const
+{
+	const NodeCodec::Header *header = sGetNodeHeader(mTree);
+	return AABox(Vec3::sLoadFloat3Unsafe(header->mRootBoundsMin), Vec3::sLoadFloat3Unsafe(header->mRootBoundsMax));
+}
+
+uint MeshShape::GetSubShapeIDBitsRecursive() const
+{
+	return NodeCodec::DecodingContext::sTriangleBlockIDBits(sGetNodeHeader(mTree)) + NumTriangleBits;
+}
+
+template <class Visitor>
+JPH_INLINE void MeshShape::WalkTree(Visitor &ioVisitor) const
+{
+	const NodeCodec::Header *header = sGetNodeHeader(mTree);
+	NodeCodec::DecodingContext node_ctx(header);
+
+	const TriangleCodec::DecodingContext triangle_ctx(sGetTriangleHeader(mTree));
+	const uint8 *buffer_start = &mTree[0];
+	node_ctx.WalkTree(buffer_start, triangle_ctx, ioVisitor);
+}
+
+template <class Visitor>
+JPH_INLINE void MeshShape::WalkTreePerTriangle(const SubShapeIDCreator &inSubShapeIDCreator2, Visitor &ioVisitor) const
+{
+	struct ChainedVisitor
+	{
+		JPH_INLINE			ChainedVisitor(Visitor &ioVisitor, const SubShapeIDCreator &inSubShapeIDCreator2, uint inTriangleBlockIDBits) :
+			mVisitor(ioVisitor),
+			mSubShapeIDCreator2(inSubShapeIDCreator2),
+			mTriangleBlockIDBits(inTriangleBlockIDBits)
+		{
+		}
+
+		JPH_INLINE bool		ShouldAbort() const
+		{
+			return mVisitor.ShouldAbort();
+		}
+
+		JPH_INLINE bool		ShouldVisitNode(int inStackTop) const
+		{
+			return mVisitor.ShouldVisitNode(inStackTop);
+		}
+
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
+		{
+			return mVisitor.VisitNodes(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, ioProperties, inStackTop);
+		}
+
+		JPH_INLINE void		VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, uint32 inTriangleBlockID)
+		{
+			// Create ID for triangle block
+			SubShapeIDCreator block_sub_shape_id = mSubShapeIDCreator2.PushID(inTriangleBlockID, mTriangleBlockIDBits);
+
+			// Decode vertices and flags
+			JPH_ASSERT(inNumTriangles <= MaxTrianglesPerLeaf);
+			Vec3 vertices[MaxTrianglesPerLeaf * 3];
+			uint8 flags[MaxTrianglesPerLeaf];
+			ioContext.Unpack(inTriangles, inNumTriangles, vertices, flags);
+
+			int triangle_idx = 0;
+			for (const Vec3 *v = vertices, *v_end = vertices + inNumTriangles * 3; v < v_end; v += 3, triangle_idx++)
+			{
+				// Determine active edges
+				uint8 active_edges = (flags[triangle_idx] >> FLAGS_ACTIVE_EGDE_SHIFT) & FLAGS_ACTIVE_EDGE_MASK;
+
+				// Create ID for triangle
+				SubShapeIDCreator triangle_sub_shape_id = block_sub_shape_id.PushID(triangle_idx, NumTriangleBits);
+
+				mVisitor.VisitTriangle(v[0], v[1], v[2], active_edges, triangle_sub_shape_id.GetID());
+
+				// Check if we should early out now
+				if (mVisitor.ShouldAbort())
+					break;
+			}
+		}
+
+		Visitor &			mVisitor;
+		SubShapeIDCreator	mSubShapeIDCreator2;
+		uint				mTriangleBlockIDBits;
+	};
+
+	ChainedVisitor visitor(ioVisitor, inSubShapeIDCreator2, NodeCodec::DecodingContext::sTriangleBlockIDBits(sGetNodeHeader(mTree)));
+	WalkTree(visitor);
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void MeshShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	// Reset the batch if we switch coloring mode
+	if (mCachedTrianglesColoredPerGroup != sDrawTriangleGroups || mCachedUseMaterialColors != inUseMaterialColors)
+	{
+		mGeometry = nullptr;
+		mCachedTrianglesColoredPerGroup = sDrawTriangleGroups;
+		mCachedUseMaterialColors = inUseMaterialColors;
+	}
+
+	if (mGeometry == nullptr)
+	{
+		struct Visitor
+		{
+			JPH_INLINE bool		ShouldAbort() const
+			{
+				return false;
+			}
+
+			JPH_INLINE bool		ShouldVisitNode(int inStackTop) const
+			{
+				return true;
+			}
+
+			JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
+			{
+				UVec4 valid = UVec4::sOr(UVec4::sOr(Vec4::sLess(inBoundsMinX, inBoundsMaxX), Vec4::sLess(inBoundsMinY, inBoundsMaxY)), Vec4::sLess(inBoundsMinZ, inBoundsMaxZ));
+				return CountAndSortTrues(valid, ioProperties);
+			}
+
+			JPH_INLINE void		VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, [[maybe_unused]] uint32 inTriangleBlockID)
+			{
+				JPH_ASSERT(inNumTriangles <= MaxTrianglesPerLeaf);
+				Vec3 vertices[MaxTrianglesPerLeaf * 3];
+				ioContext.Unpack(inTriangles, inNumTriangles, vertices);
+
+				if (mDrawTriangleGroups || !mUseMaterialColors || mMaterials.empty())
+				{
+					// Single color for mesh
+					Color color = mDrawTriangleGroups? Color::sGetDistinctColor(mColorIdx++) : (mUseMaterialColors? PhysicsMaterial::sDefault->GetDebugColor() : Color::sWhite);
+					for (const Vec3 *v = vertices, *v_end = vertices + inNumTriangles * 3; v < v_end; v += 3)
+						mTriangles.push_back({ v[0], v[1], v[2], color });
+				}
+				else
+				{
+					// Per triangle color
+					uint8 flags[MaxTrianglesPerLeaf];
+					TriangleCodec::DecodingContext::sGetFlags(inTriangles, inNumTriangles, flags);
+
+					const uint8 *f = flags;
+					for (const Vec3 *v = vertices, *v_end = vertices + inNumTriangles * 3; v < v_end; v += 3, f++)
+						mTriangles.push_back({ v[0], v[1], v[2], mMaterials[*f & FLAGS_MATERIAL_MASK]->GetDebugColor() });
+				}
+			}
+
+			Array<DebugRenderer::Triangle> &		mTriangles;
+			const PhysicsMaterialList &				mMaterials;
+			bool									mUseMaterialColors;
+			bool									mDrawTriangleGroups;
+			int										mColorIdx = 0;
+		};
+
+		Array<DebugRenderer::Triangle> triangles;
+		Visitor visitor { triangles, mMaterials, mCachedUseMaterialColors, mCachedTrianglesColoredPerGroup };
+		WalkTree(visitor);
+		mGeometry = new DebugRenderer::Geometry(inRenderer->CreateTriangleBatch(triangles), GetLocalBounds());
+	}
+
+	// Test if the shape is scaled inside out
+	DebugRenderer::ECullMode cull_mode = ScaleHelpers::IsInsideOut(inScale)? DebugRenderer::ECullMode::CullFrontFace : DebugRenderer::ECullMode::CullBackFace;
+
+	// Determine the draw mode
+	DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid;
+
+	// Draw the geometry
+	inRenderer->DrawGeometry(inCenterOfMassTransform * Mat44::sScale(inScale), inColor, mGeometry, cull_mode, DebugRenderer::ECastShadow::On, draw_mode);
+
+	if (sDrawTriangleOutlines)
+	{
+		struct Visitor
+		{
+			JPH_INLINE			Visitor(DebugRenderer *inRenderer, RMat44Arg inTransform) :
+				mRenderer(inRenderer),
+				mTransform(inTransform)
+			{
+			}
+
+			JPH_INLINE bool		ShouldAbort() const
+			{
+				return false;
+			}
+
+			JPH_INLINE bool		ShouldVisitNode(int inStackTop) const
+			{
+				return true;
+			}
+
+			JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
+			{
+				UVec4 valid = UVec4::sOr(UVec4::sOr(Vec4::sLess(inBoundsMinX, inBoundsMaxX), Vec4::sLess(inBoundsMinY, inBoundsMaxY)), Vec4::sLess(inBoundsMinZ, inBoundsMaxZ));
+				return CountAndSortTrues(valid, ioProperties);
+			}
+
+			JPH_INLINE void		VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, uint32 inTriangleBlockID)
+			{
+				// Decode vertices and flags
+				JPH_ASSERT(inNumTriangles <= MaxTrianglesPerLeaf);
+				Vec3 vertices[MaxTrianglesPerLeaf * 3];
+				uint8 flags[MaxTrianglesPerLeaf];
+				ioContext.Unpack(inTriangles, inNumTriangles, vertices, flags);
+
+				// Loop through triangles
+				const uint8 *f = flags;
+				for (Vec3 *v = vertices, *v_end = vertices + inNumTriangles * 3; v < v_end; v += 3, ++f)
+				{
+					// Loop through edges
+					for (uint edge_idx = 0; edge_idx < 3; ++edge_idx)
+					{
+						RVec3 v1 = mTransform * v[edge_idx];
+						RVec3 v2 = mTransform * v[(edge_idx + 1) % 3];
+
+						// Draw active edge as a green arrow, other edges as grey
+						if (*f & (1 << (edge_idx + FLAGS_ACTIVE_EGDE_SHIFT)))
+							mRenderer->DrawArrow(v1, v2, Color::sGreen, 0.01f);
+						else
+							mRenderer->DrawLine(v1, v2, Color::sGrey);
+					}
+				}
+			}
+
+			DebugRenderer *	mRenderer;
+			RMat44			mTransform;
+		};
+
+		Visitor visitor { inRenderer, inCenterOfMassTransform.PreScaled(inScale) };
+		WalkTree(visitor);
+	}
+}
+#endif // JPH_DEBUG_RENDERER
+
+bool MeshShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	struct Visitor
+	{
+		JPH_INLINE explicit	Visitor(RayCastResult &ioHit) :
+			mHit(ioHit)
+		{
+		}
+
+		JPH_INLINE bool		ShouldAbort() const
+		{
+			return mHit.mFraction <= 0.0f;
+		}
+
+		JPH_INLINE bool		ShouldVisitNode(int inStackTop) const
+		{
+			return mDistanceStack[inStackTop] < mHit.mFraction;
+		}
+
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
+		{
+			// Test bounds of 4 children
+			Vec4 distance = RayAABox4(mRayOrigin, mRayInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+
+			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
+			return SortReverseAndStore(distance, mHit.mFraction, ioProperties, &mDistanceStack[inStackTop]);
+		}
+
+		JPH_INLINE void		VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, uint32 inTriangleBlockID)
+		{
+			// Test against triangles
+			uint32 triangle_idx;
+			float fraction = ioContext.TestRay(mRayOrigin, mRayDirection, inTriangles, inNumTriangles, mHit.mFraction, triangle_idx);
+			if (fraction < mHit.mFraction)
+			{
+				mHit.mFraction = fraction;
+				mHit.mSubShapeID2 = mSubShapeIDCreator.PushID(inTriangleBlockID, mTriangleBlockIDBits).PushID(triangle_idx, NumTriangleBits).GetID();
+				mReturnValue = true;
+			}
+		}
+
+		RayCastResult &		mHit;
+		Vec3				mRayOrigin;
+		Vec3				mRayDirection;
+		RayInvDirection		mRayInvDirection;
+		uint				mTriangleBlockIDBits;
+		SubShapeIDCreator	mSubShapeIDCreator;
+		bool				mReturnValue = false;
+		float				mDistanceStack[NodeCodec::StackSize];
+	};
+
+	Visitor visitor(ioHit);
+	visitor.mRayOrigin = inRay.mOrigin;
+	visitor.mRayDirection = inRay.mDirection;
+	visitor.mRayInvDirection.Set(inRay.mDirection);
+	visitor.mTriangleBlockIDBits = NodeCodec::DecodingContext::sTriangleBlockIDBits(sGetNodeHeader(mTree));
+	visitor.mSubShapeIDCreator = inSubShapeIDCreator;
+	WalkTree(visitor);
+
+	return visitor.mReturnValue;
+}
+
+void MeshShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	struct Visitor
+	{
+		JPH_INLINE explicit	Visitor(CastRayCollector &ioCollector) :
+			mCollector(ioCollector)
+		{
+		}
+
+		JPH_INLINE bool		ShouldAbort() const
+		{
+			return mCollector.ShouldEarlyOut();
+		}
+
+		JPH_INLINE bool		ShouldVisitNode(int inStackTop) const
+		{
+			return mDistanceStack[inStackTop] < mCollector.GetEarlyOutFraction();
+		}
+
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
+		{
+			// Test bounds of 4 children
+			Vec4 distance = RayAABox4(mRayOrigin, mRayInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+
+			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
+			return SortReverseAndStore(distance, mCollector.GetEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
+		}
+
+		JPH_INLINE void		VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, [[maybe_unused]] uint8 inActiveEdges, SubShapeID inSubShapeID2)
+		{
+			// Back facing check
+			if (mBackFaceMode == EBackFaceMode::IgnoreBackFaces && (inV2 - inV0).Cross(inV1 - inV0).Dot(mRayDirection) < 0)
+				return;
+
+			// Check the triangle
+			float fraction = RayTriangle(mRayOrigin, mRayDirection, inV0, inV1, inV2);
+			if (fraction < mCollector.GetEarlyOutFraction())
+			{
+				RayCastResult hit;
+				hit.mBodyID = TransformedShape::sGetBodyID(mCollector.GetContext());
+				hit.mFraction = fraction;
+				hit.mSubShapeID2 = inSubShapeID2;
+				mCollector.AddHit(hit);
+			}
+		}
+
+		CastRayCollector &	mCollector;
+		Vec3				mRayOrigin;
+		Vec3				mRayDirection;
+		RayInvDirection		mRayInvDirection;
+		EBackFaceMode		mBackFaceMode;
+		float				mDistanceStack[NodeCodec::StackSize];
+	};
+
+	Visitor visitor(ioCollector);
+	visitor.mBackFaceMode = inRayCastSettings.mBackFaceModeTriangles;
+	visitor.mRayOrigin = inRay.mOrigin;
+	visitor.mRayDirection = inRay.mDirection;
+	visitor.mRayInvDirection.Set(inRay.mDirection);
+	WalkTreePerTriangle(inSubShapeIDCreator, visitor);
+}
+
+void MeshShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	sCollidePointUsingRayCast(*this, inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter);
+}
+
+void MeshShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	struct Visitor : public CollideSoftBodyVerticesVsTriangles
+	{
+		using CollideSoftBodyVerticesVsTriangles::CollideSoftBodyVerticesVsTriangles;
+
+		JPH_INLINE bool	ShouldAbort() const
+		{
+			return false;
+		}
+
+		JPH_INLINE bool	ShouldVisitNode([[maybe_unused]] int inStackTop) const
+		{
+			return mDistanceStack[inStackTop] < mClosestDistanceSq;
+		}
+
+		JPH_INLINE int	VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
+		{
+			// Get distance to vertex
+			Vec4 dist_sq = AABox4DistanceSqToPoint(mLocalPosition, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+
+			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
+			return SortReverseAndStore(dist_sq, mClosestDistanceSq, ioProperties, &mDistanceStack[inStackTop]);
+		}
+
+		JPH_INLINE void	VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, [[maybe_unused]] uint8 inActiveEdges, [[maybe_unused]] SubShapeID inSubShapeID2)
+		{
+			ProcessTriangle(inV0, inV1, inV2);
+		}
+
+		float			mDistanceStack[NodeCodec::StackSize];
+	};
+
+	Visitor visitor(inCenterOfMassTransform, inScale);
+
+	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
+		if (v.GetInvMass() > 0.0f)
+		{
+			visitor.StartVertex(v);
+			WalkTreePerTriangle(SubShapeIDCreator(), visitor);
+			visitor.FinishVertex(v, inCollidingShapeIndex);
+		}
+}
+
+void MeshShape::sCastConvexVsMesh(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_PROFILE_FUNCTION();
+
+	struct Visitor : public CastConvexVsTriangles
+	{
+		using CastConvexVsTriangles::CastConvexVsTriangles;
+
+		JPH_INLINE bool		ShouldAbort() const
+		{
+			return mCollector.ShouldEarlyOut();
+		}
+
+		JPH_INLINE bool		ShouldVisitNode(int inStackTop) const
+		{
+			return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction();
+		}
+
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
+		{
+			// Scale the bounding boxes of this node
+			Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
+			AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+			// Enlarge them by the casted shape's box extents
+			AABox4EnlargeWithExtent(mBoxExtent, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+			// Test bounds of 4 children
+			Vec4 distance = RayAABox4(mBoxCenter, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
+			return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
+		}
+
+		JPH_INLINE void		VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, SubShapeID inSubShapeID2)
+		{
+			Cast(inV0, inV1, inV2, inActiveEdges, inSubShapeID2);
+		}
+
+		RayInvDirection		mInvDirection;
+		Vec3				mBoxCenter;
+		Vec3				mBoxExtent;
+		float				mDistanceStack[NodeCodec::StackSize];
+	};
+
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::Mesh);
+	const MeshShape *shape = static_cast<const MeshShape *>(inShape);
+
+	Visitor visitor(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector);
+	visitor.mInvDirection.Set(inShapeCast.mDirection);
+	visitor.mBoxCenter = inShapeCast.mShapeWorldBounds.GetCenter();
+	visitor.mBoxExtent = inShapeCast.mShapeWorldBounds.GetExtent();
+	shape->WalkTreePerTriangle(inSubShapeIDCreator2, visitor);
+}
+
+void MeshShape::sCastSphereVsMesh(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_PROFILE_FUNCTION();
+
+	struct Visitor : public CastSphereVsTriangles
+	{
+		using CastSphereVsTriangles::CastSphereVsTriangles;
+
+		JPH_INLINE bool		ShouldAbort() const
+		{
+			return mCollector.ShouldEarlyOut();
+		}
+
+		JPH_INLINE bool		ShouldVisitNode(int inStackTop) const
+		{
+			return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction();
+		}
+
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
+		{
+			// Scale the bounding boxes of this node
+			Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
+			AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+			// Enlarge them by the radius of the sphere
+			AABox4EnlargeWithExtent(Vec3::sReplicate(mRadius), bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+			// Test bounds of 4 children
+			Vec4 distance = RayAABox4(mStart, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
+			return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
+		}
+
+		JPH_INLINE void		VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, SubShapeID inSubShapeID2)
+		{
+			Cast(inV0, inV1, inV2, inActiveEdges, inSubShapeID2);
+		}
+
+		RayInvDirection		mInvDirection;
+		float				mDistanceStack[NodeCodec::StackSize];
+	};
+
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::Mesh);
+	const MeshShape *shape = static_cast<const MeshShape *>(inShape);
+
+	Visitor visitor(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector);
+	visitor.mInvDirection.Set(inShapeCast.mDirection);
+	shape->WalkTreePerTriangle(inSubShapeIDCreator2, visitor);
+}
+
+struct MeshShape::MSGetTrianglesContext
+{
+	JPH_INLINE		MSGetTrianglesContext(const MeshShape *inShape, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) :
+		mDecodeCtx(sGetNodeHeader(inShape->mTree)),
+		mShape(inShape),
+		mLocalBox(Mat44::sInverseRotationTranslation(inRotation, inPositionCOM), inBox),
+		mMeshScale(inScale),
+		mLocalToWorld(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale)),
+		mIsInsideOut(ScaleHelpers::IsInsideOut(inScale))
+	{
+	}
+
+	JPH_INLINE bool	ShouldAbort() const
+	{
+		return mShouldAbort;
+	}
+
+	JPH_INLINE bool	ShouldVisitNode([[maybe_unused]] int inStackTop) const
+	{
+		return true;
+	}
+
+	JPH_INLINE int	VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const
+	{
+		// Scale the bounding boxes of this node
+		Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
+		AABox4Scale(mMeshScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+		// Test which nodes collide
+		UVec4 collides = AABox4VsBox(mLocalBox, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+		return CountAndSortTrues(collides, ioProperties);
+	}
+
+	JPH_INLINE void	VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, [[maybe_unused]] uint32 inTriangleBlockID)
+	{
+		// When the buffer is full and we cannot process the triangles, abort the tree walk. The next time GetTrianglesNext is called we will continue here.
+		if (mNumTrianglesFound + inNumTriangles > mMaxTrianglesRequested)
+		{
+			mShouldAbort = true;
+			return;
+		}
+
+		// Decode vertices
+		JPH_ASSERT(inNumTriangles <= MaxTrianglesPerLeaf);
+		Vec3 vertices[MaxTrianglesPerLeaf * 3];
+		ioContext.Unpack(inTriangles, inNumTriangles, vertices);
+
+		// Store vertices as Float3
+		if (mIsInsideOut)
+		{
+			// Scaled inside out, flip the triangles
+			for (const Vec3 *v = vertices, *v_end = v + 3 * inNumTriangles; v < v_end; v += 3)
+			{
+				(mLocalToWorld * v[0]).StoreFloat3(mTriangleVertices++);
+				(mLocalToWorld * v[2]).StoreFloat3(mTriangleVertices++);
+				(mLocalToWorld * v[1]).StoreFloat3(mTriangleVertices++);
+			}
+		}
+		else
+		{
+			// Normal scale
+			for (const Vec3 *v = vertices, *v_end = v + 3 * inNumTriangles; v < v_end; ++v)
+				(mLocalToWorld * *v).StoreFloat3(mTriangleVertices++);
+		}
+
+		if (mMaterials != nullptr)
+		{
+			if (mShape->mMaterials.empty())
+			{
+				// No materials, output default
+				const PhysicsMaterial *default_material = PhysicsMaterial::sDefault;
+				for (int m = 0; m < inNumTriangles; ++m)
+					*mMaterials++ = default_material;
+			}
+			else
+			{
+				// Decode triangle flags
+				uint8 flags[MaxTrianglesPerLeaf];
+				TriangleCodec::DecodingContext::sGetFlags(inTriangles, inNumTriangles, flags);
+
+				// Store materials
+				for (const uint8 *f = flags, *f_end = f + inNumTriangles; f < f_end; ++f)
+					*mMaterials++ = mShape->mMaterials[*f & FLAGS_MATERIAL_MASK].GetPtr();
+			}
+		}
+
+		// Accumulate triangles found
+		mNumTrianglesFound += inNumTriangles;
+	}
+
+	NodeCodec::DecodingContext	mDecodeCtx;
+	const MeshShape *			mShape;
+	OrientedBox					mLocalBox;
+	Vec3						mMeshScale;
+	Mat44						mLocalToWorld;
+	int							mMaxTrianglesRequested;
+	Float3 *					mTriangleVertices;
+	int							mNumTrianglesFound;
+	const PhysicsMaterial **	mMaterials;
+	bool						mShouldAbort;
+	bool						mIsInsideOut;
+};
+
+void MeshShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const
+{
+	static_assert(sizeof(MSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small");
+	JPH_ASSERT(IsAligned(&ioContext, alignof(MSGetTrianglesContext)));
+
+	new (&ioContext) MSGetTrianglesContext(this, inBox, inPositionCOM, inRotation, inScale);
+}
+
+int MeshShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const
+{
+	static_assert(cGetTrianglesMinTrianglesRequested >= MaxTrianglesPerLeaf, "cGetTrianglesMinTrianglesRequested is too small");
+	JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested);
+
+	// Check if we're done
+	MSGetTrianglesContext &context = (MSGetTrianglesContext &)ioContext;
+	if (context.mDecodeCtx.IsDoneWalking())
+		return 0;
+
+	// Store parameters on context
+	context.mMaxTrianglesRequested = inMaxTrianglesRequested;
+	context.mTriangleVertices = outTriangleVertices;
+	context.mMaterials = outMaterials;
+	context.mShouldAbort = false; // Reset the abort flag
+	context.mNumTrianglesFound = 0;
+
+	// Continue (or start) walking the tree
+	const TriangleCodec::DecodingContext triangle_ctx(sGetTriangleHeader(mTree));
+	const uint8 *buffer_start = &mTree[0];
+	context.mDecodeCtx.WalkTree(buffer_start, triangle_ctx, context);
+	return context.mNumTrianglesFound;
+}
+
+void MeshShape::sCollideConvexVsMesh(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Get the shapes
+	JPH_ASSERT(inShape1->GetType() == EShapeType::Convex);
+	JPH_ASSERT(inShape2->GetType() == EShapeType::Mesh);
+	const ConvexShape *shape1 = static_cast<const ConvexShape *>(inShape1);
+	const MeshShape *shape2 = static_cast<const MeshShape *>(inShape2);
+
+	struct Visitor : public CollideConvexVsTriangles
+	{
+		using CollideConvexVsTriangles::CollideConvexVsTriangles;
+
+		JPH_INLINE bool	ShouldAbort() const
+		{
+			return mCollector.ShouldEarlyOut();
+		}
+
+		JPH_INLINE bool	ShouldVisitNode([[maybe_unused]] int inStackTop) const
+		{
+			return true;
+		}
+
+		JPH_INLINE int	VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const
+		{
+			// Scale the bounding boxes of this node
+			Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
+			AABox4Scale(mScale2, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+			// Test which nodes collide
+			UVec4 collides = AABox4VsBox(mBoundsOf1InSpaceOf2, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+			return CountAndSortTrues(collides, ioProperties);
+		}
+
+		JPH_INLINE void	VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, SubShapeID inSubShapeID2)
+		{
+			Collide(inV0, inV1, inV2, inActiveEdges, inSubShapeID2);
+		}
+	};
+
+	Visitor visitor(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector);
+	shape2->WalkTreePerTriangle(inSubShapeIDCreator2, visitor);
+}
+
+void MeshShape::sCollideSphereVsMesh(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Get the shapes
+	JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::Sphere);
+	JPH_ASSERT(inShape2->GetType() == EShapeType::Mesh);
+	const SphereShape *shape1 = static_cast<const SphereShape *>(inShape1);
+	const MeshShape *shape2 = static_cast<const MeshShape *>(inShape2);
+
+	struct Visitor : public CollideSphereVsTriangles
+	{
+		using CollideSphereVsTriangles::CollideSphereVsTriangles;
+
+		JPH_INLINE bool	ShouldAbort() const
+		{
+			return mCollector.ShouldEarlyOut();
+		}
+
+		JPH_INLINE bool	ShouldVisitNode([[maybe_unused]] int inStackTop) const
+		{
+			return true;
+		}
+
+		JPH_INLINE int	VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const
+		{
+			// Scale the bounding boxes of this node
+			Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
+			AABox4Scale(mScale2, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+
+			// Test which nodes collide
+			UVec4 collides = AABox4VsSphere(mSphereCenterIn2, mRadiusPlusMaxSeparationSq, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
+			return CountAndSortTrues(collides, ioProperties);
+		}
+
+		JPH_INLINE void	VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, SubShapeID inSubShapeID2)
+		{
+			Collide(inV0, inV1, inV2, inActiveEdges, inSubShapeID2);
+		}
+	};
+
+	Visitor visitor(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector);
+	shape2->WalkTreePerTriangle(inSubShapeIDCreator2, visitor);
+}
+
+void MeshShape::SaveBinaryState(StreamOut &inStream) const
+{
+	Shape::SaveBinaryState(inStream);
+
+	inStream.Write(static_cast<const ByteBufferVector &>(mTree)); // Make sure we use the Array<> overload
+}
+
+void MeshShape::RestoreBinaryState(StreamIn &inStream)
+{
+	Shape::RestoreBinaryState(inStream);
+
+	inStream.Read(static_cast<ByteBufferVector &>(mTree)); // Make sure we use the Array<> overload
+}
+
+void MeshShape::SaveMaterialState(PhysicsMaterialList &outMaterials) const
+{
+	outMaterials = mMaterials;
+}
+
+void MeshShape::RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials)
+{
+	mMaterials.assign(inMaterials, inMaterials + inNumMaterials);
+}
+
+Shape::Stats MeshShape::GetStats() const
+{
+	// Walk the tree to count the triangles
+	struct Visitor
+	{
+		JPH_INLINE bool		ShouldAbort() const
+		{
+			return false;
+		}
+
+		JPH_INLINE bool		ShouldVisitNode([[maybe_unused]] int inStackTop) const
+		{
+			return true;
+		}
+
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const
+		{
+			// Visit all valid children
+			UVec4 valid = UVec4::sOr(UVec4::sOr(Vec4::sLess(inBoundsMinX, inBoundsMaxX), Vec4::sLess(inBoundsMinY, inBoundsMaxY)), Vec4::sLess(inBoundsMinZ, inBoundsMaxZ));
+			return CountAndSortTrues(valid, ioProperties);
+		}
+
+		JPH_INLINE void		VisitTriangles([[maybe_unused]] const TriangleCodec::DecodingContext &ioContext, [[maybe_unused]] const void *inTriangles, int inNumTriangles, [[maybe_unused]] uint32 inTriangleBlockID)
+		{
+			mNumTriangles += inNumTriangles;
+		}
+
+		uint				mNumTriangles = 0;
+	};
+
+	Visitor visitor;
+	WalkTree(visitor);
+
+	return Stats(sizeof(*this) + mMaterials.size() * sizeof(Ref<PhysicsMaterial>) + mTree.size() * sizeof(uint8), visitor.mNumTriangles);
+}
+
+uint32 MeshShape::GetTriangleUserData(const SubShapeID &inSubShapeID) const
+{
+	// Decode ID
+	const void *block_start;
+	uint32 triangle_idx;
+	DecodeSubShapeID(inSubShapeID, block_start, triangle_idx);
+
+	// Decode triangle
+	const TriangleCodec::DecodingContext triangle_ctx(sGetTriangleHeader(mTree));
+	return triangle_ctx.GetUserData(block_start, triangle_idx);
+}
+
+void MeshShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Mesh);
+	f.mConstruct = []() -> Shape * { return new MeshShape; };
+	f.mColor = Color::sRed;
+
+	for (EShapeSubType s : sConvexSubShapeTypes)
+	{
+		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::Mesh, sCollideConvexVsMesh);
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Mesh, sCastConvexVsMesh);
+
+		CollisionDispatch::sRegisterCastShape(EShapeSubType::Mesh, s, CollisionDispatch::sReversedCastShape);
+		CollisionDispatch::sRegisterCollideShape(EShapeSubType::Mesh, s, CollisionDispatch::sReversedCollideShape);
+	}
+
+	// Specialized collision functions
+	CollisionDispatch::sRegisterCollideShape(EShapeSubType::Sphere, EShapeSubType::Mesh, sCollideSphereVsMesh);
+	CollisionDispatch::sRegisterCastShape(EShapeSubType::Sphere, EShapeSubType::Mesh, sCastSphereVsMesh);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.h
new file mode 100644
index 0000000000..a211b7a0ad
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MeshShape.h
@@ -0,0 +1,217 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+#include <Jolt/Physics/Collision/PhysicsMaterial.h>
+#include <Jolt/Core/ByteBuffer.h>
+#include <Jolt/Geometry/Triangle.h>
+#include <Jolt/Geometry/IndexedTriangle.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+class ConvexShape;
+class CollideShapeSettings;
+
+/// Class that constructs a MeshShape
+class JPH_EXPORT MeshShapeSettings final : public ShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, MeshShapeSettings)
+
+public:
+	/// Default constructor for deserialization
+									MeshShapeSettings() = default;
+
+	/// Create a mesh shape.
+									MeshShapeSettings(const TriangleList &inTriangles, PhysicsMaterialList inMaterials = PhysicsMaterialList());
+									MeshShapeSettings(VertexList inVertices, IndexedTriangleList inTriangles, PhysicsMaterialList inMaterials = PhysicsMaterialList());
+
+	/// Sanitize the mesh data. Remove duplicate and degenerate triangles. This is called automatically when constructing the MeshShapeSettings with a list of (indexed-) triangles.
+	void							Sanitize();
+
+	// See: ShapeSettings
+	virtual ShapeResult				Create() const override;
+
+	/// Vertices belonging to mIndexedTriangles
+	VertexList						mTriangleVertices;
+
+	/// Original list of indexed triangles (triangles will be reordered internally in the mesh shape).
+	/// Triangles must be provided in counter clockwise order.
+	/// Degenerate triangles will automatically be removed during mesh creation but no other mesh simplifications are performed, use an external library if this is desired.
+	/// For simulation, the triangles are considered to be single sided.
+	/// For ray casts you can choose to make triangles double sided by setting RayCastSettings::mBackFaceMode to EBackFaceMode::CollideWithBackFaces.
+	/// For collide shape tests you can use CollideShapeSettings::mBackFaceMode and for shape casts you can use ShapeCastSettings::mBackFaceModeTriangles.
+	IndexedTriangleList				mIndexedTriangles;
+
+	/// Materials assigned to the triangles. Each triangle specifies which material it uses through its mMaterialIndex
+	PhysicsMaterialList				mMaterials;
+
+	/// Maximum number of triangles in each leaf of the axis aligned box tree. This is a balance between memory and performance. Can be in the range [1, MeshShape::MaxTrianglesPerLeaf].
+	/// Sensible values are between 4 (for better performance) and 8 (for less memory usage).
+	uint							mMaxTrianglesPerLeaf = 8;
+
+	/// Cosine of the threshold angle (if the angle between the two triangles is bigger than this, the edge is active, note that a concave edge is always inactive).
+	/// Setting this value too small can cause ghost collisions with edges, setting it too big can cause depenetration artifacts (objects not depenetrating quickly).
+	/// Valid ranges are between cos(0 degrees) and cos(90 degrees). The default value is cos(5 degrees).
+	float							mActiveEdgeCosThresholdAngle = 0.996195f;					// cos(5 degrees)
+
+	/// When true, we store the user data coming from Triangle::mUserData or IndexedTriangle::mUserData in the mesh shape.
+	/// This can be used to store additional data like the original index of the triangle in the mesh.
+	/// Can be retrieved using MeshShape::GetTriangleUserData.
+	/// Turning this on increases the memory used by the MeshShape by roughly 25%.
+	bool							mPerTriangleUserData = false;
+};
+
+/// A mesh shape, consisting of triangles. Mesh shapes are mostly used for static geometry.
+/// They can be used by dynamic or kinematic objects but only if they don't collide with other mesh or heightfield shapes as those collisions are currently not supported.
+/// Note that if you make a mesh shape a dynamic or kinematic object, you need to provide a mass yourself as mesh shapes don't need to form a closed hull so don't have a well defined volume from which the mass can be calculated.
+class JPH_EXPORT MeshShape final : public Shape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+									MeshShape() : Shape(EShapeType::Mesh, EShapeSubType::Mesh) { }
+									MeshShape(const MeshShapeSettings &inSettings, ShapeResult &outResult);
+
+	// See Shape::MustBeStatic
+	virtual bool					MustBeStatic() const override								{ return true; }
+
+	// See Shape::GetLocalBounds
+	virtual AABox					GetLocalBounds() const override;
+
+	// See Shape::GetSubShapeIDBitsRecursive
+	virtual uint					GetSubShapeIDBitsRecursive() const override;
+
+	// See Shape::GetInnerRadius
+	virtual float					GetInnerRadius() const override								{ return 0.0f; }
+
+	// See Shape::GetMassProperties
+	virtual MassProperties			GetMassProperties() const override;
+
+	// See Shape::GetMaterial
+	virtual const PhysicsMaterial *	GetMaterial(const SubShapeID &inSubShapeID) const override;
+
+	/// Get the list of all materials
+	const PhysicsMaterialList &		GetMaterialList() const										{ return mMaterials; }
+
+	/// Determine which material index a particular sub shape uses (note that if there are no materials this function will return 0 so check the array size)
+	/// Note: This could for example be used to create a decorator shape around a mesh shape that overrides the GetMaterial call to replace a material with another material.
+	uint							GetMaterialIndex(const SubShapeID &inSubShapeID) const;
+
+	// See Shape::GetSurfaceNormal
+	virtual Vec3					GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override;
+
+	// See Shape::GetSupportingFace
+	virtual void					GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
+
+#ifdef JPH_DEBUG_RENDERER
+	// See Shape::Draw
+	virtual void					Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// See Shape::CastRay
+	virtual bool					CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+	virtual void					CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	/// See: Shape::CollidePoint
+	/// Note that for CollidePoint to work for a mesh shape, the mesh needs to be closed (a manifold) or multiple non-intersecting manifolds. Triangles may be facing the interior of the manifold.
+	/// Insideness is tested by counting the amount of triangles encountered when casting an infinite ray from inPoint. If the number of hits is odd we're inside, if it's even we're outside.
+	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollideSoftBodyVertices
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
+
+	// See Shape::GetTrianglesStart
+	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
+
+	// See Shape::GetTrianglesNext
+	virtual int						GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override;
+
+	// See Shape::GetSubmergedVolume
+	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override { JPH_ASSERT(false, "Not supported"); }
+
+	// See Shape
+	virtual void					SaveBinaryState(StreamOut &inStream) const override;
+	virtual void					SaveMaterialState(PhysicsMaterialList &outMaterials) const override;
+	virtual void					RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) override;
+
+	// See Shape::GetStats
+	virtual Stats					GetStats() const override;
+
+	// See Shape::GetVolume
+	virtual float					GetVolume() const override									{ return 0; }
+
+	// When MeshShape::mPerTriangleUserData is true, this function can be used to retrieve the user data that was stored in the mesh shape.
+	uint32							GetTriangleUserData(const SubShapeID &inSubShapeID) const;
+
+#ifdef JPH_DEBUG_RENDERER
+	// Settings
+	static bool						sDrawTriangleGroups;
+	static bool						sDrawTriangleOutlines;
+#endif // JPH_DEBUG_RENDERER
+
+	// Register shape functions with the registry
+	static void						sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void					RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	struct							MSGetTrianglesContext;										///< Context class for GetTrianglesStart/Next
+
+	static constexpr int			NumTriangleBits = 3;										///< How many bits to reserve to encode the triangle index
+	static constexpr int			MaxTrianglesPerLeaf = 1 << NumTriangleBits;					///< Number of triangles that are stored max per leaf aabb node
+
+	/// Find and flag active edges
+	static void						sFindActiveEdges(const MeshShapeSettings &inSettings, IndexedTriangleList &ioIndices);
+
+	/// Visit the entire tree using a visitor pattern
+	template <class Visitor>
+	void							WalkTree(Visitor &ioVisitor) const;
+
+	/// Same as above but with a callback per triangle instead of per block of triangles
+	template <class Visitor>
+	void							WalkTreePerTriangle(const SubShapeIDCreator &inSubShapeIDCreator2, Visitor &ioVisitor) const;
+
+	/// Decode a sub shape ID
+	inline void						DecodeSubShapeID(const SubShapeID &inSubShapeID, const void *&outTriangleBlock, uint32 &outTriangleIndex) const;
+
+	// Helper functions called by CollisionDispatch
+	static void						sCollideConvexVsMesh(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCollideSphereVsMesh(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCastConvexVsMesh(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+	static void						sCastSphereVsMesh(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+
+	/// Materials assigned to the triangles. Each triangle specifies which material it uses through its mMaterialIndex
+	PhysicsMaterialList				mMaterials;
+
+	ByteBuffer						mTree;														///< Resulting packed data structure
+
+	/// 8 bit flags stored per triangle
+	enum ETriangleFlags
+	{
+		/// Material index
+		FLAGS_MATERIAL_BITS			= 5,
+		FLAGS_MATERIAL_MASK			= (1 << FLAGS_MATERIAL_BITS) - 1,
+
+		/// Active edge bits
+		FLAGS_ACTIVE_EGDE_SHIFT		= FLAGS_MATERIAL_BITS,
+		FLAGS_ACTIVE_EDGE_BITS		= 3,
+		FLAGS_ACTIVE_EDGE_MASK		= (1 << FLAGS_ACTIVE_EDGE_BITS) - 1
+	};
+
+#ifdef JPH_DEBUG_RENDERER
+	mutable DebugRenderer::GeometryRef	mGeometry;												///< Debug rendering data
+	mutable bool					mCachedTrianglesColoredPerGroup = false;					///< This is used to regenerate the triangle batch if the drawing settings change
+	mutable bool					mCachedUseMaterialColors = false;							///< This is used to regenerate the triangle batch if the drawing settings change
+#endif // JPH_DEBUG_RENDERER
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MutableCompoundShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MutableCompoundShape.cpp
new file mode 100644
index 0000000000..fdedc9a317
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MutableCompoundShape.cpp
@@ -0,0 +1,589 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/Shape/MutableCompoundShape.h>
+#include <Jolt/Physics/Collision/Shape/CompoundShapeVisitors.h>
+#include <Jolt/Core/Profiler.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(MutableCompoundShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(MutableCompoundShapeSettings, CompoundShapeSettings)
+}
+
+ShapeSettings::ShapeResult MutableCompoundShapeSettings::Create() const
+{
+	// Build a mutable compound shape
+	if (mCachedResult.IsEmpty())
+		Ref<Shape> shape = new MutableCompoundShape(*this, mCachedResult);
+
+	return mCachedResult;
+}
+
+MutableCompoundShape::MutableCompoundShape(const MutableCompoundShapeSettings &inSettings, ShapeResult &outResult) :
+	CompoundShape(EShapeSubType::MutableCompound, inSettings, outResult)
+{
+	mSubShapes.reserve(inSettings.mSubShapes.size());
+	for (const CompoundShapeSettings::SubShapeSettings &shape : inSettings.mSubShapes)
+	{
+		// Start constructing the runtime sub shape
+		SubShape out_shape;
+		if (!out_shape.FromSettings(shape, outResult))
+			return;
+
+		mSubShapes.push_back(out_shape);
+	}
+
+	AdjustCenterOfMass();
+
+	CalculateSubShapeBounds(0, (uint)mSubShapes.size());
+
+	// Check if we're not exceeding the amount of sub shape id bits
+	if (GetSubShapeIDBitsRecursive() > SubShapeID::MaxBits)
+	{
+		outResult.SetError("Compound hierarchy is too deep and exceeds the amount of available sub shape ID bits");
+		return;
+	}
+
+	outResult.Set(this);
+}
+
+Ref<MutableCompoundShape> MutableCompoundShape::Clone() const
+{
+	Ref<MutableCompoundShape> clone = new MutableCompoundShape();
+	clone->SetUserData(GetUserData());
+
+	clone->mCenterOfMass = mCenterOfMass;
+	clone->mLocalBounds = mLocalBounds;
+	clone->mSubShapes = mSubShapes;
+	clone->mInnerRadius = mInnerRadius;
+	clone->mSubShapeBounds = mSubShapeBounds;
+
+	return clone;
+}
+
+void MutableCompoundShape::AdjustCenterOfMass()
+{
+	// First calculate the delta of the center of mass
+	float mass = 0.0f;
+	Vec3 center_of_mass = Vec3::sZero();
+	for (const CompoundShape::SubShape &sub_shape : mSubShapes)
+	{
+		MassProperties child = sub_shape.mShape->GetMassProperties();
+		mass += child.mMass;
+		center_of_mass += sub_shape.GetPositionCOM() * child.mMass;
+	}
+	if (mass > 0.0f)
+		center_of_mass /= mass;
+
+	// Now adjust all shapes to recenter around center of mass
+	for (CompoundShape::SubShape &sub_shape : mSubShapes)
+		sub_shape.SetPositionCOM(sub_shape.GetPositionCOM() - center_of_mass);
+
+	// Update bounding boxes
+	for (Bounds &bounds : mSubShapeBounds)
+	{
+		Vec4 xxxx = center_of_mass.SplatX();
+		Vec4 yyyy = center_of_mass.SplatY();
+		Vec4 zzzz = center_of_mass.SplatZ();
+		bounds.mMinX -= xxxx;
+		bounds.mMinY -= yyyy;
+		bounds.mMinZ -= zzzz;
+		bounds.mMaxX -= xxxx;
+		bounds.mMaxY -= yyyy;
+		bounds.mMaxZ -= zzzz;
+	}
+	mLocalBounds.Translate(-center_of_mass);
+
+	// And adjust the center of mass for this shape in the opposite direction
+	mCenterOfMass += center_of_mass;
+}
+
+void MutableCompoundShape::CalculateLocalBounds()
+{
+	uint num_blocks = GetNumBlocks();
+	if (num_blocks > 0)
+	{
+		// Initialize min/max for first block
+		const Bounds *bounds = mSubShapeBounds.data();
+		Vec4 min_x = bounds->mMinX;
+		Vec4 min_y = bounds->mMinY;
+		Vec4 min_z = bounds->mMinZ;
+		Vec4 max_x = bounds->mMaxX;
+		Vec4 max_y = bounds->mMaxY;
+		Vec4 max_z = bounds->mMaxZ;
+
+		// Accumulate other blocks
+		const Bounds *bounds_end = bounds + num_blocks;
+		for (++bounds; bounds < bounds_end; ++bounds)
+		{
+			min_x = Vec4::sMin(min_x, bounds->mMinX);
+			min_y = Vec4::sMin(min_y, bounds->mMinY);
+			min_z = Vec4::sMin(min_z, bounds->mMinZ);
+			max_x = Vec4::sMax(max_x, bounds->mMaxX);
+			max_y = Vec4::sMax(max_y, bounds->mMaxY);
+			max_z = Vec4::sMax(max_z, bounds->mMaxZ);
+		}
+
+		// Calculate resulting bounding box
+		mLocalBounds.mMin.SetX(min_x.ReduceMin());
+		mLocalBounds.mMin.SetY(min_y.ReduceMin());
+		mLocalBounds.mMin.SetZ(min_z.ReduceMin());
+		mLocalBounds.mMax.SetX(max_x.ReduceMax());
+		mLocalBounds.mMax.SetY(max_y.ReduceMax());
+		mLocalBounds.mMax.SetZ(max_z.ReduceMax());
+	}
+	else
+	{
+		// There are no subshapes, set the bounding box to invalid
+		mLocalBounds.SetEmpty();
+	}
+
+	// Cache the inner radius as it can take a while to recursively iterate over all sub shapes
+	CalculateInnerRadius();
+}
+
+void MutableCompoundShape::EnsureSubShapeBoundsCapacity()
+{
+	// Check if we have enough space
+	uint new_capacity = ((uint)mSubShapes.size() + 3) >> 2;
+	if (mSubShapeBounds.size() < new_capacity)
+		mSubShapeBounds.resize(new_capacity);
+}
+
+void MutableCompoundShape::CalculateSubShapeBounds(uint inStartIdx, uint inNumber)
+{
+	// Ensure that we have allocated the required space for mSubShapeBounds
+	EnsureSubShapeBoundsCapacity();
+
+	// Loop over blocks of 4 sub shapes
+	for (uint sub_shape_idx_start = inStartIdx & ~uint(3), sub_shape_idx_end = inStartIdx + inNumber; sub_shape_idx_start < sub_shape_idx_end; sub_shape_idx_start += 4)
+	{
+		Mat44 bounds_min;
+		Mat44 bounds_max;
+
+		AABox sub_shape_bounds;
+		for (uint col = 0; col < 4; ++col)
+		{
+			uint sub_shape_idx = sub_shape_idx_start + col;
+			if (sub_shape_idx < mSubShapes.size()) // else reuse sub_shape_bounds from previous iteration
+			{
+				const SubShape &sub_shape = mSubShapes[sub_shape_idx];
+
+				// Transform the shape's bounds into our local space
+				Mat44 transform = Mat44::sRotationTranslation(sub_shape.GetRotation(), sub_shape.GetPositionCOM());
+
+				// Get the bounding box
+				sub_shape_bounds = sub_shape.mShape->GetWorldSpaceBounds(transform, Vec3::sReplicate(1.0f));
+			}
+
+			// Put the bounds as columns in a matrix
+			bounds_min.SetColumn3(col, sub_shape_bounds.mMin);
+			bounds_max.SetColumn3(col, sub_shape_bounds.mMax);
+		}
+
+		// Transpose to go to structure of arrays format
+		Mat44 bounds_min_t = bounds_min.Transposed();
+		Mat44 bounds_max_t = bounds_max.Transposed();
+
+		// Store in our bounds array
+		Bounds &bounds = mSubShapeBounds[sub_shape_idx_start >> 2];
+		bounds.mMinX = bounds_min_t.GetColumn4(0);
+		bounds.mMinY = bounds_min_t.GetColumn4(1);
+		bounds.mMinZ = bounds_min_t.GetColumn4(2);
+		bounds.mMaxX = bounds_max_t.GetColumn4(0);
+		bounds.mMaxY = bounds_max_t.GetColumn4(1);
+		bounds.mMaxZ = bounds_max_t.GetColumn4(2);
+	}
+
+	CalculateLocalBounds();
+}
+
+uint MutableCompoundShape::AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData)
+{
+	SubShape sub_shape;
+	sub_shape.mShape = inShape;
+	sub_shape.mUserData = inUserData;
+	sub_shape.SetTransform(inPosition, inRotation, mCenterOfMass);
+	mSubShapes.push_back(sub_shape);
+	uint shape_idx = (uint)mSubShapes.size() - 1;
+
+	CalculateSubShapeBounds(shape_idx, 1);
+
+	return shape_idx;
+}
+
+void MutableCompoundShape::RemoveShape(uint inIndex)
+{
+	mSubShapes.erase(mSubShapes.begin() + inIndex);
+
+	uint num_bounds = (uint)mSubShapes.size() - inIndex;
+	if (num_bounds > 0)
+		CalculateSubShapeBounds(inIndex, num_bounds);
+	else
+		CalculateLocalBounds();
+}
+
+void MutableCompoundShape::ModifyShape(uint inIndex, Vec3Arg inPosition, QuatArg inRotation)
+{
+	SubShape &sub_shape = mSubShapes[inIndex];
+	sub_shape.SetTransform(inPosition, inRotation, mCenterOfMass);
+
+	CalculateSubShapeBounds(inIndex, 1);
+}
+
+void MutableCompoundShape::ModifyShape(uint inIndex, Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape)
+{
+	SubShape &sub_shape = mSubShapes[inIndex];
+	sub_shape.mShape = inShape;
+	sub_shape.SetTransform(inPosition, inRotation, mCenterOfMass);
+
+	CalculateSubShapeBounds(inIndex, 1);
+}
+
+void MutableCompoundShape::ModifyShapes(uint inStartIndex, uint inNumber, const Vec3 *inPositions, const Quat *inRotations, uint inPositionStride, uint inRotationStride)
+{
+	JPH_ASSERT(inStartIndex + inNumber <= mSubShapes.size());
+
+	const Vec3 *pos = inPositions;
+	const Quat *rot = inRotations;
+	for (SubShape *dest = &mSubShapes[inStartIndex], *dest_end = dest + inNumber; dest < dest_end; ++dest)
+	{
+		// Update transform
+		dest->SetTransform(*pos, *rot, mCenterOfMass);
+
+		// Advance pointer in position / rotation buffer
+		pos = reinterpret_cast<const Vec3 *>(reinterpret_cast<const uint8 *>(pos) + inPositionStride);
+		rot = reinterpret_cast<const Quat *>(reinterpret_cast<const uint8 *>(rot) + inRotationStride);
+	}
+
+	CalculateSubShapeBounds(inStartIndex, inNumber);
+}
+
+template <class Visitor>
+inline void MutableCompoundShape::WalkSubShapes(Visitor &ioVisitor) const
+{
+	// Loop over all blocks of 4 bounding boxes
+	for (uint block = 0, num_blocks = GetNumBlocks(); block < num_blocks; ++block)
+	{
+		// Test the bounding boxes
+		const Bounds &bounds = mSubShapeBounds[block];
+		typename Visitor::Result result = ioVisitor.TestBlock(bounds.mMinX, bounds.mMinY, bounds.mMinZ, bounds.mMaxX, bounds.mMaxY, bounds.mMaxZ);
+
+		// Check if any of the bounding boxes collided
+		if (ioVisitor.ShouldVisitBlock(result))
+		{
+			// Go through the individual boxes
+			uint sub_shape_start_idx = block << 2;
+			for (uint col = 0, max_col = min<uint>(4, (uint)mSubShapes.size() - sub_shape_start_idx); col < max_col; ++col) // Don't read beyond the end of the subshapes array
+				if (ioVisitor.ShouldVisitSubShape(result, col)) // Because the early out fraction can change, we need to retest every shape
+				{
+					// Test sub shape
+					uint sub_shape_idx = sub_shape_start_idx + col;
+					const SubShape &sub_shape = mSubShapes[sub_shape_idx];
+					ioVisitor.VisitShape(sub_shape, sub_shape_idx);
+
+					// If no better collision is available abort
+					if (ioVisitor.ShouldAbort())
+						break;
+				}
+		}
+	}
+}
+
+bool MutableCompoundShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	struct Visitor : public CastRayVisitor
+	{
+		using CastRayVisitor::CastRayVisitor;
+
+		using Result = Vec4;
+
+		JPH_INLINE Result	TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+		{
+			return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+		}
+
+		JPH_INLINE bool		ShouldVisitBlock(Vec4Arg inResult) const
+		{
+			UVec4 closer = Vec4::sLess(inResult, Vec4::sReplicate(mHit.mFraction));
+			return closer.TestAnyTrue();
+		}
+
+		JPH_INLINE bool		ShouldVisitSubShape(Vec4Arg inResult, uint inIndexInBlock) const
+		{
+			return inResult[inIndexInBlock] < mHit.mFraction;
+		}
+	};
+
+	Visitor visitor(inRay, this, inSubShapeIDCreator, ioHit);
+	WalkSubShapes(visitor);
+	return visitor.mReturnValue;
+}
+
+void MutableCompoundShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	struct Visitor : public CastRayVisitorCollector
+	{
+		using CastRayVisitorCollector::CastRayVisitorCollector;
+
+		using Result = Vec4;
+
+		JPH_INLINE Result	TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+		{
+			return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+		}
+
+		JPH_INLINE bool		ShouldVisitBlock(Vec4Arg inResult) const
+		{
+			UVec4 closer = Vec4::sLess(inResult, Vec4::sReplicate(mCollector.GetEarlyOutFraction()));
+			return closer.TestAnyTrue();
+		}
+
+		JPH_INLINE bool		ShouldVisitSubShape(Vec4Arg inResult, uint inIndexInBlock) const
+		{
+			return inResult[inIndexInBlock] < mCollector.GetEarlyOutFraction();
+		}
+	};
+
+	Visitor visitor(inRay, inRayCastSettings, this, inSubShapeIDCreator, ioCollector, inShapeFilter);
+	WalkSubShapes(visitor);
+}
+
+void MutableCompoundShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	struct Visitor : public CollidePointVisitor
+	{
+		using CollidePointVisitor::CollidePointVisitor;
+
+		using Result = UVec4;
+
+		JPH_INLINE Result	TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+		{
+			return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+		}
+
+		JPH_INLINE bool		ShouldVisitBlock(UVec4Arg inResult) const
+		{
+			return inResult.TestAnyTrue();
+		}
+
+		JPH_INLINE bool		ShouldVisitSubShape(UVec4Arg inResult, uint inIndexInBlock) const
+		{
+			return inResult[inIndexInBlock] != 0;
+		}
+	};
+
+	Visitor visitor(inPoint, this, inSubShapeIDCreator, ioCollector, inShapeFilter);
+	WalkSubShapes(visitor);
+}
+
+void MutableCompoundShape::sCastShapeVsCompound(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_PROFILE_FUNCTION();
+
+	struct Visitor : public CastShapeVisitor
+	{
+		using CastShapeVisitor::CastShapeVisitor;
+
+		using Result = Vec4;
+
+		JPH_INLINE Result	TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+		{
+			return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+		}
+
+		JPH_INLINE bool		ShouldVisitBlock(Vec4Arg inResult) const
+		{
+			UVec4 closer = Vec4::sLess(inResult, Vec4::sReplicate(mCollector.GetPositiveEarlyOutFraction()));
+			return closer.TestAnyTrue();
+		}
+
+		JPH_INLINE bool		ShouldVisitSubShape(Vec4Arg inResult, uint inIndexInBlock) const
+		{
+			return inResult[inIndexInBlock] < mCollector.GetPositiveEarlyOutFraction();
+		}
+	};
+
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::MutableCompound);
+	const MutableCompoundShape *shape = static_cast<const MutableCompoundShape *>(inShape);
+
+	Visitor visitor(inShapeCast, inShapeCastSettings, shape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+	shape->WalkSubShapes(visitor);
+}
+
+void MutableCompoundShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	struct Visitor : public CollectTransformedShapesVisitor
+	{
+		using CollectTransformedShapesVisitor::CollectTransformedShapesVisitor;
+
+		using Result = UVec4;
+
+		JPH_INLINE Result	TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+		{
+			return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+		}
+
+		JPH_INLINE bool		ShouldVisitBlock(UVec4Arg inResult) const
+		{
+			return inResult.TestAnyTrue();
+		}
+
+		JPH_INLINE bool		ShouldVisitSubShape(UVec4Arg inResult, uint inIndexInBlock) const
+		{
+			return inResult[inIndexInBlock] != 0;
+		}
+	};
+
+	Visitor visitor(inBox, this, inPositionCOM, inRotation, inScale, inSubShapeIDCreator, ioCollector, inShapeFilter);
+	WalkSubShapes(visitor);
+}
+
+int MutableCompoundShape::GetIntersectingSubShapes(const AABox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	GetIntersectingSubShapesVisitorMC<AABox> visitor(inBox, outSubShapeIndices, inMaxSubShapeIndices);
+	WalkSubShapes(visitor);
+	return visitor.GetNumResults();
+}
+
+int MutableCompoundShape::GetIntersectingSubShapes(const OrientedBox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	GetIntersectingSubShapesVisitorMC<OrientedBox> visitor(inBox, outSubShapeIndices, inMaxSubShapeIndices);
+	WalkSubShapes(visitor);
+	return visitor.GetNumResults();
+}
+
+void MutableCompoundShape::sCollideCompoundVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::MutableCompound);
+	const MutableCompoundShape *shape1 = static_cast<const MutableCompoundShape *>(inShape1);
+
+	struct Visitor : public CollideCompoundVsShapeVisitor
+	{
+		using CollideCompoundVsShapeVisitor::CollideCompoundVsShapeVisitor;
+
+		using Result = UVec4;
+
+		JPH_INLINE Result	TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+		{
+			return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+		}
+
+		JPH_INLINE bool		ShouldVisitBlock(UVec4Arg inResult) const
+		{
+			return inResult.TestAnyTrue();
+		}
+
+		JPH_INLINE bool		ShouldVisitSubShape(UVec4Arg inResult, uint inIndexInBlock) const
+		{
+			return inResult[inIndexInBlock] != 0;
+		}
+	};
+
+	Visitor visitor(shape1, inShape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
+	shape1->WalkSubShapes(visitor);
+}
+
+void MutableCompoundShape::sCollideShapeVsCompound(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::MutableCompound);
+	const MutableCompoundShape *shape2 = static_cast<const MutableCompoundShape *>(inShape2);
+
+	struct Visitor : public CollideShapeVsCompoundVisitor
+	{
+		using CollideShapeVsCompoundVisitor::CollideShapeVsCompoundVisitor;
+
+		using Result = UVec4;
+
+		JPH_INLINE Result	TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+		{
+			return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+		}
+
+		JPH_INLINE bool		ShouldVisitBlock(UVec4Arg inResult) const
+		{
+			return inResult.TestAnyTrue();
+		}
+
+		JPH_INLINE bool		ShouldVisitSubShape(UVec4Arg inResult, uint inIndexInBlock) const
+		{
+			return inResult[inIndexInBlock] != 0;
+		}
+	};
+
+	Visitor visitor(inShape1, shape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
+	shape2->WalkSubShapes(visitor);
+}
+
+void MutableCompoundShape::SaveBinaryState(StreamOut &inStream) const
+{
+	CompoundShape::SaveBinaryState(inStream);
+
+	// Write bounds
+	uint bounds_size = (((uint)mSubShapes.size() + 3) >> 2) * sizeof(Bounds);
+	inStream.WriteBytes(mSubShapeBounds.data(), bounds_size);
+}
+
+void MutableCompoundShape::RestoreBinaryState(StreamIn &inStream)
+{
+	CompoundShape::RestoreBinaryState(inStream);
+
+	// Ensure that we have allocated the required space for mSubShapeBounds
+	EnsureSubShapeBoundsCapacity();
+
+	// Read bounds
+	uint bounds_size = (((uint)mSubShapes.size() + 3) >> 2) * sizeof(Bounds);
+	inStream.ReadBytes(mSubShapeBounds.data(), bounds_size);
+}
+
+void MutableCompoundShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::MutableCompound);
+	f.mConstruct = []() -> Shape * { return new MutableCompoundShape; };
+	f.mColor = Color::sDarkOrange;
+
+	for (EShapeSubType s : sAllSubShapeTypes)
+	{
+		CollisionDispatch::sRegisterCollideShape(EShapeSubType::MutableCompound, s, sCollideCompoundVsShape);
+		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::MutableCompound, sCollideShapeVsCompound);
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::MutableCompound, sCastShapeVsCompound);
+	}
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MutableCompoundShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MutableCompoundShape.h
new file mode 100644
index 0000000000..c3f6ff2964
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/MutableCompoundShape.h
@@ -0,0 +1,169 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/CompoundShape.h>
+
+JPH_NAMESPACE_BEGIN
+
+class CollideShapeSettings;
+
+/// Class that constructs a MutableCompoundShape.
+class JPH_EXPORT MutableCompoundShapeSettings final : public CompoundShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, MutableCompoundShapeSettings)
+
+public:
+	// See: ShapeSettings
+	virtual ShapeResult				Create() const override;
+};
+
+/// A compound shape, sub shapes can be rotated and translated.
+/// This shape is optimized for adding / removing and changing the rotation / translation of sub shapes but is less efficient in querying.
+/// Shifts all child objects so that they're centered around the center of mass (which needs to be kept up to date by calling AdjustCenterOfMass).
+///
+/// Note: If you're using MutableCompoundShape and are querying data while modifying the shape you'll have a race condition.
+/// In this case it is best to create a new MutableCompoundShape using the Clone function. You replace the shape on a body using BodyInterface::SetShape.
+/// If a query is still working on the old shape, it will have taken a reference and keep the old shape alive until the query finishes.
+class JPH_EXPORT MutableCompoundShape final : public CompoundShape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+									MutableCompoundShape() : CompoundShape(EShapeSubType::MutableCompound) { }
+									MutableCompoundShape(const MutableCompoundShapeSettings &inSettings, ShapeResult &outResult);
+
+	/// Clone this shape. Can be used to avoid race conditions. See the documentation of this class for more information.
+	Ref<MutableCompoundShape>		Clone() const;
+
+	// See Shape::CastRay
+	virtual bool					CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+	virtual void					CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollidePoint
+	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See Shape::CollectTransformedShapes
+	virtual void					CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override;
+
+	// See: CompoundShape::GetIntersectingSubShapes
+	virtual int						GetIntersectingSubShapes(const AABox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const override;
+
+	// See: CompoundShape::GetIntersectingSubShapes
+	virtual int						GetIntersectingSubShapes(const OrientedBox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const override;
+
+	// See Shape
+	virtual void					SaveBinaryState(StreamOut &inStream) const override;
+
+	// See Shape::GetStats
+	virtual Stats					GetStats() const override								{ return Stats(sizeof(*this) + mSubShapes.size() * sizeof(SubShape) + mSubShapeBounds.size() * sizeof(Bounds), 0); }
+
+	///@{
+	/// @name Mutating shapes. Note that this is not thread safe, so you need to ensure that any bodies that use this shape are locked at the time of modification using BodyLockWrite. After modification you need to call BodyInterface::NotifyShapeChanged to update the broadphase and collision caches.
+
+	/// Adding a new shape.
+	/// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information.
+	/// @return The index of the newly added shape
+	uint							AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData = 0);
+
+	/// Remove a shape by index.
+	/// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information.
+	void							RemoveShape(uint inIndex);
+
+	/// Modify the position / orientation of a shape.
+	/// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information.
+	void							ModifyShape(uint inIndex, Vec3Arg inPosition, QuatArg inRotation);
+
+	/// Modify the position / orientation and shape at the same time.
+	/// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information.
+	void							ModifyShape(uint inIndex, Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape);
+
+	/// @brief Batch set positions / orientations, this avoids duplicate work due to bounding box calculation.
+	/// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information.
+	/// @param inStartIndex Index of first shape to update
+	/// @param inNumber Number of shapes to update
+	/// @param inPositions A list of positions with arbitrary stride
+	/// @param inRotations A list of orientations with arbitrary stride
+	/// @param inPositionStride The position stride (the number of bytes between the first and second element)
+	/// @param inRotationStride The orientation stride (the number of bytes between the first and second element)
+	void							ModifyShapes(uint inStartIndex, uint inNumber, const Vec3 *inPositions, const Quat *inRotations, uint inPositionStride = sizeof(Vec3), uint inRotationStride = sizeof(Quat));
+
+	/// Recalculate the center of mass and shift all objects so they're centered around it
+	/// (this needs to be done of dynamic bodies and if the center of mass changes significantly due to adding / removing / repositioning sub shapes or else the simulation will look unnatural)
+	/// Note that after adjusting the center of mass of an object you need to call BodyInterface::NotifyShapeChanged and Constraint::NotifyShapeChanged on the relevant bodies / constraints.
+	/// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information.
+	void							AdjustCenterOfMass();
+
+	///@}
+
+	// Register shape functions with the registry
+	static void						sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void					RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	// Visitor for GetIntersectingSubShapes
+	template <class BoxType>
+	struct GetIntersectingSubShapesVisitorMC : public GetIntersectingSubShapesVisitor<BoxType>
+	{
+		using GetIntersectingSubShapesVisitor<BoxType>::GetIntersectingSubShapesVisitor;
+
+		using Result = UVec4;
+
+		JPH_INLINE Result			TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const
+		{
+			return GetIntersectingSubShapesVisitor<BoxType>::TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+		}
+
+		JPH_INLINE bool				ShouldVisitBlock(UVec4Arg inResult) const
+		{
+			return inResult.TestAnyTrue();
+		}
+
+		JPH_INLINE bool				ShouldVisitSubShape(UVec4Arg inResult, uint inIndexInBlock) const
+		{
+			return inResult[inIndexInBlock] != 0;
+		}
+	};
+
+	/// Get the number of blocks of 4 bounding boxes
+	inline uint						GetNumBlocks() const										{ return ((uint)mSubShapes.size() + 3) >> 2; }
+
+	/// Ensure that the mSubShapeBounds has enough space to store bounding boxes equivalent to the number of shapes in mSubShapes
+	void							EnsureSubShapeBoundsCapacity();
+
+	/// Update mSubShapeBounds
+	/// @param inStartIdx First sub shape to update
+	/// @param inNumber Number of shapes to update
+	void							CalculateSubShapeBounds(uint inStartIdx, uint inNumber);
+
+	/// Calculate mLocalBounds from mSubShapeBounds
+	void							CalculateLocalBounds();
+
+	template <class Visitor>
+	JPH_INLINE void					WalkSubShapes(Visitor &ioVisitor) const;					///< Walk the sub shapes and call Visitor::VisitShape for each sub shape encountered
+
+	// Helper functions called by CollisionDispatch
+	static void						sCollideCompoundVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCollideShapeVsCompound(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCastShapeVsCompound(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+
+	struct Bounds
+	{
+		Vec4						mMinX;
+		Vec4						mMinY;
+		Vec4						mMinZ;
+		Vec4						mMaxX;
+		Vec4						mMaxY;
+		Vec4						mMaxZ;
+	};
+
+	Array<Bounds>					mSubShapeBounds;											///< Bounding boxes of all sub shapes in SOA format (in blocks of 4 boxes), MinX 0..3, MinY 0..3, MinZ 0..3, MaxX 0..3, MaxY 0..3, MaxZ 0..3, MinX 4..7, MinY 4..7, ...
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.cpp
new file mode 100644
index 0000000000..8996b46c44
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.cpp
@@ -0,0 +1,217 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(OffsetCenterOfMassShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(OffsetCenterOfMassShapeSettings, DecoratedShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(OffsetCenterOfMassShapeSettings, mOffset)
+}
+
+ShapeSettings::ShapeResult OffsetCenterOfMassShapeSettings::Create() const
+{
+	if (mCachedResult.IsEmpty())
+		Ref<Shape> shape = new OffsetCenterOfMassShape(*this, mCachedResult);
+	return mCachedResult;
+}
+
+OffsetCenterOfMassShape::OffsetCenterOfMassShape(const OffsetCenterOfMassShapeSettings &inSettings, ShapeResult &outResult) :
+	DecoratedShape(EShapeSubType::OffsetCenterOfMass, inSettings, outResult),
+	mOffset(inSettings.mOffset)
+{
+	if (outResult.HasError())
+		return;
+
+	outResult.Set(this);
+}
+
+AABox OffsetCenterOfMassShape::GetLocalBounds() const
+{
+	AABox bounds = mInnerShape->GetLocalBounds();
+	bounds.mMin -= mOffset;
+	bounds.mMax -= mOffset;
+	return bounds;
+}
+
+AABox OffsetCenterOfMassShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
+	return mInnerShape->GetWorldSpaceBounds(inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale);
+}
+
+TransformedShape OffsetCenterOfMassShape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const
+{
+	// We don't use any bits in the sub shape ID
+	outRemainder = inSubShapeID;
+
+	TransformedShape ts(RVec3(inPositionCOM - inRotation * (inScale * mOffset)), inRotation, mInnerShape, BodyID());
+	ts.SetShapeScale(inScale);
+	return ts;
+}
+
+Vec3 OffsetCenterOfMassShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	// Transform surface position to local space and pass call on
+	return mInnerShape->GetSurfaceNormal(inSubShapeID, inLocalSurfacePosition + mOffset);
+}
+
+void OffsetCenterOfMassShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
+	mInnerShape->GetSupportingFace(inSubShapeID, inDirection, inScale, inCenterOfMassTransform.PreTranslated(-inScale * mOffset), outVertices);
+}
+
+void OffsetCenterOfMassShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const
+{
+	mInnerShape->GetSubmergedVolume(inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale, inSurface, outTotalVolume, outSubmergedVolume, outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, inBaseOffset));
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void OffsetCenterOfMassShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	mInnerShape->Draw(inRenderer, inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale, inColor, inUseMaterialColors, inDrawWireframe);
+}
+
+void OffsetCenterOfMassShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const
+{
+	mInnerShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale, inColor, inDrawSupportDirection);
+}
+
+void OffsetCenterOfMassShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
+	mInnerShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale);
+}
+#endif // JPH_DEBUG_RENDERER
+
+bool OffsetCenterOfMassShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	// Transform the ray to local space
+	RayCast ray = inRay;
+	ray.mOrigin += mOffset;
+
+	return mInnerShape->CastRay(ray, inSubShapeIDCreator, ioHit);
+}
+
+void OffsetCenterOfMassShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	// Transform the ray to local space
+	RayCast ray = inRay;
+	ray.mOrigin += mOffset;
+
+	return mInnerShape->CastRay(ray, inRayCastSettings, inSubShapeIDCreator, ioCollector, inShapeFilter);
+}
+
+void OffsetCenterOfMassShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	// Pass the point on to the inner shape in local space
+	mInnerShape->CollidePoint(inPoint + mOffset, inSubShapeIDCreator, ioCollector, inShapeFilter);
+}
+
+void OffsetCenterOfMassShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
+{
+	mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale, inVertices, inNumVertices, inCollidingShapeIndex);
+}
+
+void OffsetCenterOfMassShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	mInnerShape->CollectTransformedShapes(inBox, inPositionCOM - inRotation * (inScale * mOffset), inRotation, inScale, inSubShapeIDCreator, ioCollector, inShapeFilter);
+}
+
+void OffsetCenterOfMassShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const
+{
+	mInnerShape->TransformShape(inCenterOfMassTransform.PreTranslated(-mOffset), ioCollector);
+}
+
+void OffsetCenterOfMassShape::sCollideOffsetCenterOfMassVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+{
+	JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::OffsetCenterOfMass);
+	const OffsetCenterOfMassShape *shape1 = static_cast<const OffsetCenterOfMassShape *>(inShape1);
+
+	CollisionDispatch::sCollideShapeVsShape(shape1->mInnerShape, inShape2, inScale1, inScale2, inCenterOfMassTransform1.PreTranslated(-inScale1 * shape1->mOffset), inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
+}
+
+void OffsetCenterOfMassShape::sCollideShapeVsOffsetCenterOfMass(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+{
+	JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::OffsetCenterOfMass);
+	const OffsetCenterOfMassShape *shape2 = static_cast<const OffsetCenterOfMassShape *>(inShape2);
+
+	CollisionDispatch::sCollideShapeVsShape(inShape1, shape2->mInnerShape, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2.PreTranslated(-inScale2 * shape2->mOffset), inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
+}
+
+void OffsetCenterOfMassShape::sCastOffsetCenterOfMassVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	// Fetch offset center of mass shape from cast shape
+	JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::OffsetCenterOfMass);
+	const OffsetCenterOfMassShape *shape1 = static_cast<const OffsetCenterOfMassShape *>(inShapeCast.mShape);
+
+	// Transform the shape cast and update the shape
+	ShapeCast shape_cast(shape1->mInnerShape, inShapeCast.mScale, inShapeCast.mCenterOfMassStart.PreTranslated(-inShapeCast.mScale * shape1->mOffset), inShapeCast.mDirection);
+
+	CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+}
+
+void OffsetCenterOfMassShape::sCastShapeVsOffsetCenterOfMass(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::OffsetCenterOfMass);
+	const OffsetCenterOfMassShape *shape = static_cast<const OffsetCenterOfMassShape *>(inShape);
+
+	// Transform the shape cast
+	ShapeCast shape_cast = inShapeCast.PostTransformed(Mat44::sTranslation(inScale * shape->mOffset));
+
+	CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, shape->mInnerShape, inScale, inShapeFilter, inCenterOfMassTransform2.PreTranslated(-inScale * shape->mOffset), inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+}
+
+void OffsetCenterOfMassShape::SaveBinaryState(StreamOut &inStream) const
+{
+	DecoratedShape::SaveBinaryState(inStream);
+
+	inStream.Write(mOffset);
+}
+
+void OffsetCenterOfMassShape::RestoreBinaryState(StreamIn &inStream)
+{
+	DecoratedShape::RestoreBinaryState(inStream);
+
+	inStream.Read(mOffset);
+}
+
+void OffsetCenterOfMassShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::OffsetCenterOfMass);
+	f.mConstruct = []() -> Shape * { return new OffsetCenterOfMassShape; };
+	f.mColor = Color::sCyan;
+
+	for (EShapeSubType s : sAllSubShapeTypes)
+	{
+		CollisionDispatch::sRegisterCollideShape(EShapeSubType::OffsetCenterOfMass, s, sCollideOffsetCenterOfMassVsShape);
+		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::OffsetCenterOfMass, sCollideShapeVsOffsetCenterOfMass);
+		CollisionDispatch::sRegisterCastShape(EShapeSubType::OffsetCenterOfMass, s, sCastOffsetCenterOfMassVsShape);
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::OffsetCenterOfMass, sCastShapeVsOffsetCenterOfMass);
+	}
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h
new file mode 100644
index 0000000000..7ba6a5a7a0
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h
@@ -0,0 +1,140 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/DecoratedShape.h>
+
+JPH_NAMESPACE_BEGIN
+
+class CollideShapeSettings;
+
+/// Class that constructs an OffsetCenterOfMassShape
+class JPH_EXPORT OffsetCenterOfMassShapeSettings final : public DecoratedShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, OffsetCenterOfMassShapeSettings)
+
+public:
+	/// Constructor
+									OffsetCenterOfMassShapeSettings() = default;
+
+	/// Construct with shape settings, can be serialized.
+									OffsetCenterOfMassShapeSettings(Vec3Arg inOffset, const ShapeSettings *inShape) : DecoratedShapeSettings(inShape), mOffset(inOffset) { }
+
+	/// Variant that uses a concrete shape, which means this object cannot be serialized.
+									OffsetCenterOfMassShapeSettings(Vec3Arg inOffset, const Shape *inShape): DecoratedShapeSettings(inShape), mOffset(inOffset) { }
+
+	// See: ShapeSettings
+	virtual ShapeResult				Create() const override;
+
+	Vec3							mOffset;												///< Offset to be applied to the center of mass of the child shape
+};
+
+/// This shape will shift the center of mass of a child shape, it can e.g. be used to lower the center of mass of an unstable object like a boat to make it stable
+class JPH_EXPORT OffsetCenterOfMassShape final : public DecoratedShape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+									OffsetCenterOfMassShape() : DecoratedShape(EShapeSubType::OffsetCenterOfMass) { }
+									OffsetCenterOfMassShape(const OffsetCenterOfMassShapeSettings &inSettings, ShapeResult &outResult);
+									OffsetCenterOfMassShape(const Shape *inShape, Vec3Arg inOffset) : DecoratedShape(EShapeSubType::OffsetCenterOfMass, inShape), mOffset(inOffset) { }
+
+	/// Access the offset that is applied to the center of mass
+	Vec3							GetOffset() const										{ return mOffset; }
+
+	// See Shape::GetCenterOfMass
+	virtual Vec3					GetCenterOfMass() const override						{ return mInnerShape->GetCenterOfMass() + mOffset; }
+
+	// See Shape::GetLocalBounds
+	virtual AABox					GetLocalBounds() const override;
+
+	// See Shape::GetWorldSpaceBounds
+	virtual AABox					GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+	using Shape::GetWorldSpaceBounds;
+
+	// See Shape::GetInnerRadius
+	virtual float					GetInnerRadius() const override							{ return mInnerShape->GetInnerRadius(); }
+
+	// See Shape::GetMassProperties
+	virtual MassProperties			GetMassProperties() const override
+	{
+		MassProperties mp = mInnerShape->GetMassProperties();
+		mp.Translate(mOffset);
+		return mp;
+	}
+
+	// See Shape::GetSubShapeTransformedShape
+	virtual TransformedShape		GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const override;
+
+	// See Shape::GetSurfaceNormal
+	virtual Vec3					GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override;
+
+	// See Shape::GetSupportingFace
+	virtual void					GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
+
+	// See Shape::GetSubmergedVolume
+	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override;
+
+#ifdef JPH_DEBUG_RENDERER
+	// See Shape::Draw
+	virtual void					Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+
+	// See Shape::DrawGetSupportFunction
+	virtual void					DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override;
+
+	// See Shape::DrawGetSupportingFace
+	virtual void					DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// See Shape::CastRay
+	virtual bool					CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+	virtual void					CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollidePoint
+	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollideSoftBodyVertices
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
+
+	// See Shape::CollectTransformedShapes
+	virtual void					CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override;
+
+	// See Shape::TransformShape
+	virtual void					TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override;
+
+	// See Shape::GetTrianglesStart
+	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); }
+
+	// See Shape::GetTrianglesNext
+	virtual int						GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); return 0; }
+
+	// See Shape
+	virtual void					SaveBinaryState(StreamOut &inStream) const override;
+
+	// See Shape::GetStats
+	virtual Stats					GetStats() const override								{ return Stats(sizeof(*this), 0); }
+
+	// See Shape::GetVolume
+	virtual float					GetVolume() const override								{ return mInnerShape->GetVolume(); }
+
+	// Register shape functions with the registry
+	static void						sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void					RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	// Helper functions called by CollisionDispatch
+	static void						sCollideOffsetCenterOfMassVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCollideShapeVsOffsetCenterOfMass(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCastOffsetCenterOfMassVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+	static void						sCastShapeVsOffsetCenterOfMass(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+
+	Vec3							mOffset;												///< Offset of the center of mass
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PlaneShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PlaneShape.cpp
new file mode 100644
index 0000000000..c1401fd7fc
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PlaneShape.cpp
@@ -0,0 +1,541 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/Shape/PlaneShape.h>
+#include <Jolt/Physics/Collision/Shape/ConvexShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/ShapeFilter.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
+#include <Jolt/Core/Profiler.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/Geometry/Plane.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PlaneShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(PlaneShapeSettings, ShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(PlaneShapeSettings, mPlane)
+	JPH_ADD_ATTRIBUTE(PlaneShapeSettings, mMaterial)
+	JPH_ADD_ATTRIBUTE(PlaneShapeSettings, mHalfExtent)
+}
+
+ShapeSettings::ShapeResult PlaneShapeSettings::Create() const
+{
+	if (mCachedResult.IsEmpty())
+		Ref<Shape> shape = new PlaneShape(*this, mCachedResult);
+	return mCachedResult;
+}
+
+inline static void sPlaneGetOrthogonalBasis(Vec3Arg inNormal, Vec3 &outPerp1, Vec3 &outPerp2)
+{
+	outPerp1 = inNormal.Cross(Vec3::sAxisY()).NormalizedOr(Vec3::sAxisX());
+	outPerp2 = outPerp1.Cross(inNormal).Normalized();
+	outPerp1 = inNormal.Cross(outPerp2);
+}
+
+void PlaneShape::GetVertices(Vec3 *outVertices) const
+{
+	// Create orthogonal basis
+	Vec3 normal = mPlane.GetNormal();
+	Vec3 perp1, perp2;
+	sPlaneGetOrthogonalBasis(normal, perp1, perp2);
+
+	// Scale basis
+	perp1 *= mHalfExtent;
+	perp2 *= mHalfExtent;
+
+	// Calculate corners
+	Vec3 point = -normal * mPlane.GetConstant();
+	outVertices[0] = point + perp1 + perp2;
+	outVertices[1] = point + perp1 - perp2;
+	outVertices[2] = point - perp1 - perp2;
+	outVertices[3] = point - perp1 + perp2;
+}
+
+void PlaneShape::CalculateLocalBounds()
+{
+	// Get the vertices of the plane
+	Vec3 vertices[4];
+	GetVertices(vertices);
+
+	// Encapsulate the vertices and a point mHalfExtent behind the plane
+	mLocalBounds = AABox();
+	Vec3 normal = mPlane.GetNormal();
+	for (const Vec3 &v : vertices)
+	{
+		mLocalBounds.Encapsulate(v);
+		mLocalBounds.Encapsulate(v - mHalfExtent * normal);
+	}
+}
+
+PlaneShape::PlaneShape(const PlaneShapeSettings &inSettings, ShapeResult &outResult) :
+	Shape(EShapeType::Plane, EShapeSubType::Plane, inSettings, outResult),
+	mPlane(inSettings.mPlane),
+	mMaterial(inSettings.mMaterial),
+	mHalfExtent(inSettings.mHalfExtent)
+{
+	if (!mPlane.GetNormal().IsNormalized())
+	{
+		outResult.SetError("Plane normal needs to be normalized!");
+		return;
+	}
+
+	CalculateLocalBounds();
+
+	outResult.Set(this);
+}
+
+MassProperties PlaneShape::GetMassProperties() const
+{
+	// Object should always be static, return default mass properties
+	return MassProperties();
+}
+
+void PlaneShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
+	// Get the vertices of the plane
+	Vec3 vertices[4];
+	GetVertices(vertices);
+
+	// Reverse if scale is inside out
+	if (ScaleHelpers::IsInsideOut(inScale))
+	{
+		std::swap(vertices[0], vertices[3]);
+		std::swap(vertices[1], vertices[2]);
+	}
+
+	// Transform them to world space
+	outVertices.clear();
+	Mat44 com = inCenterOfMassTransform.PreScaled(inScale);
+	for (const Vec3 &v : vertices)
+		outVertices.push_back(com * v);
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void PlaneShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	// Get the vertices of the plane
+	Vec3 local_vertices[4];
+	GetVertices(local_vertices);
+
+	// Reverse if scale is inside out
+	if (ScaleHelpers::IsInsideOut(inScale))
+	{
+		std::swap(local_vertices[0], local_vertices[3]);
+		std::swap(local_vertices[1], local_vertices[2]);
+	}
+
+	// Transform them to world space
+	RMat44 com = inCenterOfMassTransform.PreScaled(inScale);
+	RVec3 vertices[4];
+	for (uint i = 0; i < 4; ++i)
+		vertices[i] = com * local_vertices[i];
+
+	// Determine the color
+	Color color = inUseMaterialColors? GetMaterial(SubShapeID())->GetDebugColor() : inColor;
+
+	// Draw the plane
+	if (inDrawWireframe)
+	{
+		inRenderer->DrawWireTriangle(vertices[0], vertices[1], vertices[2], color);
+		inRenderer->DrawWireTriangle(vertices[0], vertices[2], vertices[3], color);
+	}
+	else
+	{
+		inRenderer->DrawTriangle(vertices[0], vertices[1], vertices[2], color, DebugRenderer::ECastShadow::On);
+		inRenderer->DrawTriangle(vertices[0], vertices[2], vertices[3], color, DebugRenderer::ECastShadow::On);
+	}
+}
+#endif // JPH_DEBUG_RENDERER
+
+bool PlaneShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Test starting inside of negative half space
+	float distance = mPlane.SignedDistance(inRay.mOrigin);
+	if (distance <= 0.0f)
+	{
+		ioHit.mFraction = 0.0f;
+		ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID();
+		return true;
+	}
+
+	// Test ray parallel to plane
+	float dot = inRay.mDirection.Dot(mPlane.GetNormal());
+	if (dot == 0.0f)
+		return false;
+
+	// Calculate hit fraction
+	float fraction = -distance / dot;
+	if (fraction >= 0.0f && fraction < ioHit.mFraction)
+	{
+		ioHit.mFraction = fraction;
+		ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID();
+		return true;
+	}
+
+	return false;
+}
+
+void PlaneShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	// Inside solid half space?
+	float distance = mPlane.SignedDistance(inRay.mOrigin);
+	if (inRayCastSettings.mTreatConvexAsSolid
+		&& distance <= 0.0f // Inside plane
+		&& ioCollector.GetEarlyOutFraction() > 0.0f) // Willing to accept hits at fraction 0
+	{
+		// Hit at fraction 0
+		RayCastResult hit;
+		hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext());
+		hit.mFraction = 0.0f;
+		hit.mSubShapeID2 = inSubShapeIDCreator.GetID();
+		ioCollector.AddHit(hit);
+	}
+
+	float dot = inRay.mDirection.Dot(mPlane.GetNormal());
+	if (dot != 0.0f // Parallel ray will not hit plane
+		&& (inRayCastSettings.mBackFaceModeConvex == EBackFaceMode::CollideWithBackFaces || dot < 0.0f)) // Back face culling
+	{
+		// Calculate hit with plane
+		float fraction = -distance / dot;
+		if (fraction >= 0.0f && fraction < ioCollector.GetEarlyOutFraction())
+		{
+			RayCastResult hit;
+			hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext());
+			hit.mFraction = fraction;
+			hit.mSubShapeID2 = inSubShapeIDCreator.GetID();
+			ioCollector.AddHit(hit);
+		}
+	}
+}
+
+void PlaneShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	// Check if the point is inside the plane
+	if (mPlane.SignedDistance(inPoint) < 0.0f)
+		ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
+}
+
+void PlaneShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Convert plane to world space
+	Plane plane = mPlane.Scaled(inScale).GetTransformed(inCenterOfMassTransform);
+
+	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
+		if (v.GetInvMass() > 0.0f)
+		{
+			// Calculate penetration
+			float penetration = -plane.SignedDistance(v.GetPosition());
+			if (v.UpdatePenetration(penetration))
+				v.SetCollision(plane, inCollidingShapeIndex);
+		}
+}
+
+// This is a version of GetSupportingFace that returns a face that is large enough to cover the shape we're colliding with but not as large as the regular GetSupportedFace to avoid numerical precision issues
+inline static void sGetSupportingFace(const ConvexShape *inShape, Vec3Arg inShapeCOM, const Plane &inPlane, Mat44Arg inPlaneToWorld, ConvexShape::SupportingFace &outPlaneFace)
+{
+	// Project COM of shape onto plane
+	Plane world_plane = inPlane.GetTransformed(inPlaneToWorld);
+	Vec3 center = world_plane.ProjectPointOnPlane(inShapeCOM);
+
+	// Create orthogonal basis for the plane
+	Vec3 normal = world_plane.GetNormal();
+	Vec3 perp1, perp2;
+	sPlaneGetOrthogonalBasis(normal, perp1, perp2);
+
+	// Base the size of the face on the bounding box of the shape, ensuring that it is large enough to cover the entire shape
+	float size = inShape->GetLocalBounds().GetSize().Length();
+	perp1 *= size;
+	perp2 *= size;
+
+	// Emit the vertices
+	outPlaneFace.resize(4);
+	outPlaneFace[0] = center + perp1 + perp2;
+	outPlaneFace[1] = center + perp1 - perp2;
+	outPlaneFace[2] = center - perp1 - perp2;
+	outPlaneFace[3] = center - perp1 + perp2;
+}
+
+void PlaneShape::sCastConvexVsPlane(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Get the shapes
+	JPH_ASSERT(inShapeCast.mShape->GetType() == EShapeType::Convex);
+	JPH_ASSERT(inShape->GetType() == EShapeType::Plane);
+	const ConvexShape *convex_shape = static_cast<const ConvexShape *>(inShapeCast.mShape);
+	const PlaneShape *plane_shape = static_cast<const PlaneShape *>(inShape);
+
+	// Shape cast is provided relative to COM of inShape, so all we need to do is transform our plane with inScale
+	Plane plane = plane_shape->mPlane.Scaled(inScale);
+	Vec3 normal = plane.GetNormal();
+
+	// Get support function
+	ConvexShape::SupportBuffer shape1_support_buffer;
+	const ConvexShape::Support *shape1_support = convex_shape->GetSupportFunction(ConvexShape::ESupportMode::Default, shape1_support_buffer, inShapeCast.mScale);
+
+	// Get the support point of the convex shape in the opposite direction of the plane normal in our local space
+	Vec3 normal_in_convex_shape_space = inShapeCast.mCenterOfMassStart.Multiply3x3Transposed(normal);
+	Vec3 support_point = inShapeCast.mCenterOfMassStart * shape1_support->GetSupport(-normal_in_convex_shape_space);
+	float signed_distance = plane.SignedDistance(support_point);
+	float convex_radius = shape1_support->GetConvexRadius();
+	float penetration_depth = -signed_distance + convex_radius;
+	float dot = inShapeCast.mDirection.Dot(normal);
+
+	// Collision output
+	Mat44 com_hit;
+	Vec3 point1, point2;
+	float fraction;
+
+	// Do we start in collision?
+	if (penetration_depth > 0.0f)
+	{
+		// Back face culling?
+		if (inShapeCastSettings.mBackFaceModeConvex == EBackFaceMode::IgnoreBackFaces && dot > 0.0f)
+			return;
+
+		// Shallower hit?
+		if (penetration_depth <= -ioCollector.GetEarlyOutFraction())
+			return;
+
+		// We're hitting at fraction 0
+		fraction = 0.0f;
+
+		// Get contact point
+		com_hit = inCenterOfMassTransform2;
+		point1 = inCenterOfMassTransform2 * (support_point - normal * convex_radius);
+		point2 = inCenterOfMassTransform2 * (support_point - normal * signed_distance);
+	}
+	else if (dot < 0.0f) // Moving towards the plane?
+	{
+		// Calculate hit fraction
+		fraction = penetration_depth / dot;
+		JPH_ASSERT(fraction >= 0.0f);
+
+		// Further than early out fraction?
+		if (fraction >= ioCollector.GetEarlyOutFraction())
+			return;
+
+		// Get contact point
+		com_hit = inCenterOfMassTransform2.PostTranslated(fraction * inShapeCast.mDirection);
+		point1 = point2 = com_hit * (support_point - normal * convex_radius);
+	}
+	else
+	{
+		// Moving away from the plane
+		return;
+	}
+
+	// Create cast result
+	Vec3 penetration_axis_world = com_hit.Multiply3x3(-normal);
+	bool back_facing = dot > 0.0f;
+	ShapeCastResult result(fraction, point1, point2, penetration_axis_world, back_facing, inSubShapeIDCreator1.GetID(), inSubShapeIDCreator2.GetID(), TransformedShape::sGetBodyID(ioCollector.GetContext()));
+
+	// Gather faces
+	if (inShapeCastSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces)
+	{
+		// Get supporting face of convex shape
+		Mat44 shape_to_world = com_hit * inShapeCast.mCenterOfMassStart;
+		convex_shape->GetSupportingFace(SubShapeID(), normal_in_convex_shape_space, inShapeCast.mScale, shape_to_world, result.mShape1Face);
+
+		// Get supporting face of plane
+		if (!result.mShape1Face.empty())
+			sGetSupportingFace(convex_shape, shape_to_world.GetTranslation(), plane, inCenterOfMassTransform2, result.mShape2Face);
+	}
+
+	// Notify the collector
+	JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;)
+	ioCollector.AddHit(result);
+}
+
+struct PlaneShape::PSGetTrianglesContext
+{
+	Float3	mVertices[4];
+	bool	mDone = false;
+};
+
+void PlaneShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const
+{
+	static_assert(sizeof(PSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small");
+	JPH_ASSERT(IsAligned(&ioContext, alignof(PSGetTrianglesContext)));
+
+	PSGetTrianglesContext *context = new (&ioContext) PSGetTrianglesContext();
+
+	// Get the vertices of the plane
+	Vec3 vertices[4];
+	GetVertices(vertices);
+
+	// Reverse if scale is inside out
+	if (ScaleHelpers::IsInsideOut(inScale))
+	{
+		std::swap(vertices[0], vertices[3]);
+		std::swap(vertices[1], vertices[2]);
+	}
+
+	// Transform them to world space
+	Mat44 com = Mat44::sRotationTranslation(inRotation, inPositionCOM).PreScaled(inScale);
+	for (uint i = 0; i < 4; ++i)
+		(com * vertices[i]).StoreFloat3(&context->mVertices[i]);
+}
+
+int PlaneShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const
+{
+	static_assert(cGetTrianglesMinTrianglesRequested >= 2, "cGetTrianglesMinTrianglesRequested is too small");
+	JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested);
+
+	// Check if we're done
+	PSGetTrianglesContext &context = (PSGetTrianglesContext &)ioContext;
+	if (context.mDone)
+		return 0;
+	context.mDone = true;
+
+	// 1st triangle
+	outTriangleVertices[0] = context.mVertices[0];
+	outTriangleVertices[1] = context.mVertices[1];
+	outTriangleVertices[2] = context.mVertices[2];
+
+	// 2nd triangle
+	outTriangleVertices[3] = context.mVertices[0];
+	outTriangleVertices[4] = context.mVertices[2];
+	outTriangleVertices[5] = context.mVertices[3];
+
+	if (outMaterials != nullptr)
+	{
+		// Get material
+		const PhysicsMaterial *material = GetMaterial(SubShapeID());
+		outMaterials[0] = material;
+		outMaterials[1] = material;
+	}
+
+	return 2;
+}
+
+void PlaneShape::sCollideConvexVsPlane(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Get the shapes
+	JPH_ASSERT(inShape1->GetType() == EShapeType::Convex);
+	JPH_ASSERT(inShape2->GetType() == EShapeType::Plane);
+	const ConvexShape *shape1 = static_cast<const ConvexShape *>(inShape1);
+	const PlaneShape *shape2 = static_cast<const PlaneShape *>(inShape2);
+
+	// Transform the plane to the space of the convex shape
+	Plane scaled_plane = shape2->mPlane.Scaled(inScale2);
+	Plane plane = scaled_plane.GetTransformed(inCenterOfMassTransform1.InversedRotationTranslation() * inCenterOfMassTransform2);
+	Vec3 normal = plane.GetNormal();
+
+	// Get support function
+	ConvexShape::SupportBuffer shape1_support_buffer;
+	const ConvexShape::Support *shape1_support = shape1->GetSupportFunction(ConvexShape::ESupportMode::Default, shape1_support_buffer, inScale1);
+
+	// Get the support point of the convex shape in the opposite direction of the plane normal
+	Vec3 support_point = shape1_support->GetSupport(-normal);
+	float signed_distance = plane.SignedDistance(support_point);
+	float convex_radius = shape1_support->GetConvexRadius();
+	float penetration_depth = -signed_distance + convex_radius;
+	if (penetration_depth > -inCollideShapeSettings.mMaxSeparationDistance)
+	{
+		// Get contact point
+		Vec3 point1 = inCenterOfMassTransform1 * (support_point - normal * convex_radius);
+		Vec3 point2 = inCenterOfMassTransform1 * (support_point - normal * signed_distance);
+		Vec3 penetration_axis_world = inCenterOfMassTransform1.Multiply3x3(-normal);
+
+		// Create collision result
+		CollideShapeResult result(point1, point2, penetration_axis_world, penetration_depth, inSubShapeIDCreator1.GetID(), inSubShapeIDCreator2.GetID(), TransformedShape::sGetBodyID(ioCollector.GetContext()));
+
+		// Gather faces
+		if (inCollideShapeSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces)
+		{
+			// Get supporting face of shape 1
+			shape1->GetSupportingFace(SubShapeID(), normal, inScale1, inCenterOfMassTransform1, result.mShape1Face);
+
+			// Get supporting face of shape 2
+			if (!result.mShape1Face.empty())
+				sGetSupportingFace(shape1, inCenterOfMassTransform1.GetTranslation(), scaled_plane, inCenterOfMassTransform2, result.mShape2Face);
+		}
+
+		// Notify the collector
+		JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;)
+		ioCollector.AddHit(result);
+	}
+}
+
+void PlaneShape::SaveBinaryState(StreamOut &inStream) const
+{
+	Shape::SaveBinaryState(inStream);
+
+	inStream.Write(mPlane);
+	inStream.Write(mHalfExtent);
+}
+
+void PlaneShape::RestoreBinaryState(StreamIn &inStream)
+{
+	Shape::RestoreBinaryState(inStream);
+
+	inStream.Read(mPlane);
+	inStream.Read(mHalfExtent);
+
+	CalculateLocalBounds();
+}
+
+void PlaneShape::SaveMaterialState(PhysicsMaterialList &outMaterials) const
+{
+	outMaterials = { mMaterial };
+}
+
+void PlaneShape::RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials)
+{
+	JPH_ASSERT(inNumMaterials == 1);
+	mMaterial = inMaterials[0];
+}
+
+void PlaneShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Plane);
+	f.mConstruct = []() -> Shape * { return new PlaneShape; };
+	f.mColor = Color::sDarkRed;
+
+	for (EShapeSubType s : sConvexSubShapeTypes)
+	{
+		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::Plane, sCollideConvexVsPlane);
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Plane, sCastConvexVsPlane);
+
+		CollisionDispatch::sRegisterCastShape(EShapeSubType::Plane, s, CollisionDispatch::sReversedCastShape);
+		CollisionDispatch::sRegisterCollideShape(EShapeSubType::Plane, s, CollisionDispatch::sReversedCollideShape);
+	}
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PlaneShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PlaneShape.h
new file mode 100644
index 0000000000..c9a7810b74
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PlaneShape.h
@@ -0,0 +1,143 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+#include <Jolt/Physics/Collision/PhysicsMaterial.h>
+
+JPH_NAMESPACE_BEGIN
+
+class CollideShapeSettings;
+
+/// Class that constructs a PlaneShape
+class JPH_EXPORT PlaneShapeSettings final : public ShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PlaneShapeSettings)
+
+public:
+	/// Default constructor for deserialization
+									PlaneShapeSettings() = default;
+
+	/// Create a plane shape.
+									PlaneShapeSettings(const Plane &inPlane, const PhysicsMaterial *inMaterial = nullptr, float inHalfExtent = cDefaultHalfExtent) : mPlane(inPlane), mMaterial(inMaterial), mHalfExtent(inHalfExtent) { }
+
+	// See: ShapeSettings
+	virtual ShapeResult				Create() const override;
+
+	Plane							mPlane;														///< Plane that describes the shape. The negative half space is considered solid.
+
+	RefConst<PhysicsMaterial>		mMaterial;													///< Surface material of the plane
+
+	static constexpr float			cDefaultHalfExtent = 1000.0f;								///< Default half-extent of the plane (total size along 1 axis will be 2 * half-extent)
+
+	float							mHalfExtent = cDefaultHalfExtent;							///< The bounding box of this plane will run from [-half_extent, half_extent]. Keep this as low as possible for better broad phase performance.
+};
+
+/// A plane shape. The negative half space is considered solid. Planes cannot be dynamic objects, only static or kinematic.
+/// The plane is considered an infinite shape, but testing collision outside of its bounding box (defined by the half-extent parameter) will not return a collision result.
+/// At the edge of the bounding box collision with the plane will be inconsistent. If you need something of a well defined size, a box shape may be better.
+class JPH_EXPORT PlaneShape final : public Shape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+									PlaneShape() : Shape(EShapeType::Plane, EShapeSubType::Plane) { }
+									PlaneShape(const Plane &inPlane, const PhysicsMaterial *inMaterial = nullptr, float inHalfExtent = PlaneShapeSettings::cDefaultHalfExtent) : Shape(EShapeType::Plane, EShapeSubType::Plane), mPlane(inPlane), mMaterial(inMaterial), mHalfExtent(inHalfExtent) { CalculateLocalBounds(); }
+									PlaneShape(const PlaneShapeSettings &inSettings, ShapeResult &outResult);
+
+	/// Get the plane
+	const Plane &					GetPlane() const											{ return mPlane; }
+
+	/// Get the half-extent of the bounding box of the plane
+	float							GetHalfExtent() const										{ return mHalfExtent; }
+
+	// See Shape::MustBeStatic
+	virtual bool					MustBeStatic() const override								{ return true; }
+
+	// See Shape::GetLocalBounds
+	virtual AABox					GetLocalBounds() const override								{ return mLocalBounds; }
+
+	// See Shape::GetSubShapeIDBitsRecursive
+	virtual uint					GetSubShapeIDBitsRecursive() const override					{ return 0; }
+
+	// See Shape::GetInnerRadius
+	virtual float					GetInnerRadius() const override								{ return 0.0f; }
+
+	// See Shape::GetMassProperties
+	virtual MassProperties			GetMassProperties() const override;
+
+	// See Shape::GetMaterial
+	virtual const PhysicsMaterial *	GetMaterial(const SubShapeID &inSubShapeID) const override	{ JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); return mMaterial != nullptr? mMaterial : PhysicsMaterial::sDefault; }
+
+	// See Shape::GetSurfaceNormal
+	virtual Vec3					GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override { JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); return mPlane.GetNormal(); }
+
+	// See Shape::GetSupportingFace
+	virtual void					GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
+
+#ifdef JPH_DEBUG_RENDERER
+	// See Shape::Draw
+	virtual void					Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// See Shape::CastRay
+	virtual bool					CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+	virtual void					CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollidePoint
+	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollideSoftBodyVertices
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
+
+	// See Shape::GetTrianglesStart
+	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
+
+	// See Shape::GetTrianglesNext
+	virtual int						GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override;
+
+	// See Shape::GetSubmergedVolume
+	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override { JPH_ASSERT(false, "Not supported"); }
+
+	// See Shape
+	virtual void					SaveBinaryState(StreamOut &inStream) const override;
+	virtual void					SaveMaterialState(PhysicsMaterialList &outMaterials) const override;
+	virtual void					RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) override;
+
+	// See Shape::GetStats
+	virtual Stats					GetStats() const override									{ return Stats(sizeof(*this), 0); }
+
+	// See Shape::GetVolume
+	virtual float					GetVolume() const override									{ return 0; }
+
+	// Register shape functions with the registry
+	static void						sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void					RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	struct							PSGetTrianglesContext;										///< Context class for GetTrianglesStart/Next
+
+	// Get 4 vertices that form the plane
+	void							GetVertices(Vec3 *outVertices) const;
+
+	// Cache the local bounds
+	void							CalculateLocalBounds();
+
+	// Helper functions called by CollisionDispatch
+	static void						sCollideConvexVsPlane(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCastConvexVsPlane(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+
+	Plane							mPlane;
+	RefConst<PhysicsMaterial>		mMaterial;
+	float							mHalfExtent;
+	AABox							mLocalBounds;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PolyhedronSubmergedVolumeCalculator.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PolyhedronSubmergedVolumeCalculator.h
new file mode 100644
index 0000000000..96e69f0b19
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/PolyhedronSubmergedVolumeCalculator.h
@@ -0,0 +1,319 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/Plane.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+/// This class calculates the intersection between a fluid surface and a polyhedron and returns the submerged volume and its center of buoyancy
+/// Construct this class and then one by one add all faces of the polyhedron using the AddFace function. After all faces have been added the result
+/// can be gotten through GetResult.
+class PolyhedronSubmergedVolumeCalculator
+{
+private:
+	// Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 4 vertices submerged
+	// inV1 .. inV4 are submerged
+	inline static void	sTetrahedronVolume4(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, Vec3Arg inV4, float &outVolumeTimes6, Vec3 &outCenterTimes4)
+	{
+		// Calculate center of mass and mass of this tetrahedron,
+		// see: https://en.wikipedia.org/wiki/Tetrahedron#Volume
+		outVolumeTimes6 = max((inV1 - inV4).Dot((inV2 - inV4).Cross(inV3 - inV4)), 0.0f); // All contributions should be positive because we use a reference point that is on the surface of the hull
+		outCenterTimes4 = inV1 + inV2 + inV3 + inV4;
+	}
+
+	// Get the intersection point with a plane.
+	// inV1 is inD1 distance away from the plane, inV2 is inD2 distance away from the plane
+	inline static Vec3	sGetPlaneIntersection(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2)
+	{
+		JPH_ASSERT(Sign(inD1) != Sign(inD2), "Assuming both points are on opposite ends of the plane");
+		float delta = inD1 - inD2;
+		if (abs(delta) < 1.0e-6f)
+			return inV1; // Parallel to plane, just pick a point
+		else
+			return inV1 + inD1 * (inV2 - inV1) / delta;
+	}
+
+	// Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 1 vertex submerged
+	// inV1 is submerged, inV2 .. inV4 are not
+	// inD1 .. inD4 are the distances from the points to the plane
+	inline JPH_IF_NOT_DEBUG_RENDERER(static) void sTetrahedronVolume1(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2, Vec3Arg inV3, float inD3, Vec3Arg inV4, float inD4, float &outVolumeTimes6, Vec3 &outCenterTimes4)
+	{
+		// A tetrahedron with 1 point submerged is cut along 3 edges forming a new tetrahedron
+		Vec3 v2 = sGetPlaneIntersection(inV1, inD1, inV2, inD2);
+		Vec3 v3 = sGetPlaneIntersection(inV1, inD1, inV3, inD3);
+		Vec3 v4 = sGetPlaneIntersection(inV1, inD1, inV4, inD4);
+
+	#ifdef JPH_DEBUG_RENDERER
+		// Draw intersection between tetrahedron and surface
+		if (Shape::sDrawSubmergedVolumes)
+		{
+			RVec3 v2w = mBaseOffset + v2;
+			RVec3 v3w = mBaseOffset + v3;
+			RVec3 v4w = mBaseOffset + v4;
+
+			DebugRenderer::sInstance->DrawTriangle(v4w, v3w, v2w, Color::sGreen);
+			DebugRenderer::sInstance->DrawWireTriangle(v4w, v3w, v2w, Color::sWhite);
+		}
+	#endif // JPH_DEBUG_RENDERER
+
+		sTetrahedronVolume4(inV1, v2, v3, v4, outVolumeTimes6, outCenterTimes4);
+	}
+
+	// Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 2 vertices submerged
+	// inV1, inV2 are submerged, inV3, inV4 are not
+	// inD1 .. inD4 are the distances from the points to the plane
+	inline JPH_IF_NOT_DEBUG_RENDERER(static) void sTetrahedronVolume2(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2, Vec3Arg inV3, float inD3, Vec3Arg inV4, float inD4, float &outVolumeTimes6, Vec3 &outCenterTimes4)
+	{
+		// A tetrahedron with 2 points submerged is cut along 4 edges forming a quad
+		Vec3 c = sGetPlaneIntersection(inV1, inD1, inV3, inD3);
+		Vec3 d = sGetPlaneIntersection(inV1, inD1, inV4, inD4);
+		Vec3 e = sGetPlaneIntersection(inV2, inD2, inV4, inD4);
+		Vec3 f = sGetPlaneIntersection(inV2, inD2, inV3, inD3);
+
+	#ifdef JPH_DEBUG_RENDERER
+		// Draw intersection between tetrahedron and surface
+		if (Shape::sDrawSubmergedVolumes)
+		{
+			RVec3 cw = mBaseOffset + c;
+			RVec3 dw = mBaseOffset + d;
+			RVec3 ew = mBaseOffset + e;
+			RVec3 fw = mBaseOffset + f;
+
+			DebugRenderer::sInstance->DrawTriangle(cw, ew, dw, Color::sGreen);
+			DebugRenderer::sInstance->DrawTriangle(cw, fw, ew, Color::sGreen);
+			DebugRenderer::sInstance->DrawWireTriangle(cw, ew, dw, Color::sWhite);
+			DebugRenderer::sInstance->DrawWireTriangle(cw, fw, ew, Color::sWhite);
+		}
+	#endif // JPH_DEBUG_RENDERER
+
+		// We pick point c as reference (which is on the cut off surface)
+		// This leaves us with three tetrahedrons to sum up (any faces that are in the same plane as c will have zero volume)
+		Vec3 center1, center2, center3;
+		float volume1, volume2, volume3;
+		sTetrahedronVolume4(e, f, inV2, c, volume1, center1);
+		sTetrahedronVolume4(e, inV1, d, c, volume2, center2);
+		sTetrahedronVolume4(e, inV2, inV1, c, volume3, center3);
+
+		// Tally up the totals
+		outVolumeTimes6 = volume1 + volume2 + volume3;
+		outCenterTimes4 = outVolumeTimes6 > 0.0f? (volume1 * center1 + volume2 * center2 + volume3 * center3) / outVolumeTimes6 : Vec3::sZero();
+	}
+
+	// Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 3 vertices submerged
+	// inV1, inV2, inV3 are submerged, inV4 is not
+	// inD1 .. inD4 are the distances from the points to the plane
+	inline JPH_IF_NOT_DEBUG_RENDERER(static) void sTetrahedronVolume3(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2, Vec3Arg inV3, float inD3, Vec3Arg inV4, float inD4, float &outVolumeTimes6, Vec3 &outCenterTimes4)
+	{
+		// A tetrahedron with 1 point above the surface is cut along 3 edges forming a new tetrahedron
+		Vec3 v1 = sGetPlaneIntersection(inV1, inD1, inV4, inD4);
+		Vec3 v2 = sGetPlaneIntersection(inV2, inD2, inV4, inD4);
+		Vec3 v3 = sGetPlaneIntersection(inV3, inD3, inV4, inD4);
+
+	#ifdef JPH_DEBUG_RENDERER
+		// Draw intersection between tetrahedron and surface
+		if (Shape::sDrawSubmergedVolumes)
+		{
+			RVec3 v1w = mBaseOffset + v1;
+			RVec3 v2w = mBaseOffset + v2;
+			RVec3 v3w = mBaseOffset + v3;
+
+			DebugRenderer::sInstance->DrawTriangle(v3w, v2w, v1w, Color::sGreen);
+			DebugRenderer::sInstance->DrawWireTriangle(v3w, v2w, v1w, Color::sWhite);
+		}
+	#endif // JPH_DEBUG_RENDERER
+
+		Vec3 dry_center, total_center;
+		float dry_volume, total_volume;
+
+		// We first calculate the part that is above the surface
+		sTetrahedronVolume4(v1, v2, v3, inV4, dry_volume, dry_center);
+
+		// Calculate the total volume
+		sTetrahedronVolume4(inV1, inV2, inV3, inV4, total_volume, total_center);
+
+		// From this we can calculate the center and volume of the submerged part
+		outVolumeTimes6 = max(total_volume - dry_volume, 0.0f);
+		outCenterTimes4 = outVolumeTimes6 > 0.0f? (total_center * total_volume - dry_center * dry_volume) / outVolumeTimes6 : Vec3::sZero();
+	}
+
+public:
+	/// A helper class that contains cached information about a polyhedron vertex
+	class Point
+	{
+	public:
+		Vec3			mPosition;						///< World space position of vertex
+		float			mDistanceToSurface;				///< Signed distance to the surface (> 0 is above, < 0 is below)
+		bool			mAboveSurface;					///< If the point is above the surface (mDistanceToSurface > 0)
+	};
+
+	/// Constructor
+	/// @param inTransform Transform to transform all incoming points with
+	/// @param inPoints Array of points that are part of the polyhedron
+	/// @param inPointStride Amount of bytes between each point (should usually be sizeof(Vec3))
+	/// @param inNumPoints The amount of points
+	/// @param inSurface The plane that forms the fluid surface (normal should point up)
+	/// @param ioBuffer A temporary buffer of Point's that should have inNumPoints entries and should stay alive while this class is alive
+#ifdef JPH_DEBUG_RENDERER
+	/// @param inBaseOffset The offset to transform inTransform to world space (in double precision mode this can be used to shift the whole operation closer to the origin). Only used for debug drawing.
+#endif // JPH_DEBUG_RENDERER
+						PolyhedronSubmergedVolumeCalculator(const Mat44 &inTransform, const Vec3 *inPoints, int inPointStride, int inNumPoints, const Plane &inSurface, Point *ioBuffer
+#ifdef JPH_DEBUG_RENDERER // Not using JPH_IF_DEBUG_RENDERER for Doxygen
+		, RVec3 inBaseOffset
+#endif // JPH_DEBUG_RENDERER
+		) :
+		mPoints(ioBuffer)
+#ifdef JPH_DEBUG_RENDERER
+		, mBaseOffset(inBaseOffset)
+#endif // JPH_DEBUG_RENDERER
+	{
+		// Convert the points to world space and determine the distance to the surface
+		float reference_dist = FLT_MAX;
+		for (int p = 0; p < inNumPoints; ++p)
+		{
+			// Calculate values
+			Vec3 transformed_point = inTransform * *reinterpret_cast<const Vec3 *>(reinterpret_cast<const uint8 *>(inPoints) + p * inPointStride);
+			float dist = inSurface.SignedDistance(transformed_point);
+			bool above = dist >= 0.0f;
+
+			// Keep track if all are above or below
+			mAllAbove &= above;
+			mAllBelow &= !above;
+
+			// Calculate lowest point, we use this to create tetrahedrons out of all faces
+			if (reference_dist > dist)
+			{
+				mReferencePointIdx = p;
+				reference_dist = dist;
+			}
+
+			// Store values
+			ioBuffer->mPosition = transformed_point;
+			ioBuffer->mDistanceToSurface = dist;
+			ioBuffer->mAboveSurface = above;
+			++ioBuffer;
+		}
+	}
+
+	/// Check if all points are above the surface. Should be used as early out.
+	inline bool			AreAllAbove() const
+	{
+		return mAllAbove;
+	}
+
+	/// Check if all points are below the surface. Should be used as early out.
+	inline bool			AreAllBelow() const
+	{
+		return mAllBelow;
+	}
+
+	/// Get the lowest point of the polyhedron. Used to form the 4th vertex to make a tetrahedron out of a polyhedron face.
+	inline int			GetReferencePointIdx() const
+	{
+		return mReferencePointIdx;
+	}
+
+	/// Add a polyhedron face. Supply the indices of the points that form the face (in counter clockwise order).
+	void				AddFace(int inIdx1, int inIdx2, int inIdx3)
+	{
+		JPH_ASSERT(inIdx1 != mReferencePointIdx && inIdx2 != mReferencePointIdx && inIdx3 != mReferencePointIdx, "A face using the reference point will not contribute to the volume");
+
+		// Find the points
+		const Point &ref = mPoints[mReferencePointIdx];
+		const Point &p1 = mPoints[inIdx1];
+		const Point &p2 = mPoints[inIdx2];
+		const Point &p3 = mPoints[inIdx3];
+
+		// Determine which vertices are submerged
+		uint code = (p1.mAboveSurface? 0 : 0b001) | (p2.mAboveSurface? 0 : 0b010) | (p3.mAboveSurface? 0 : 0b100);
+
+		float volume;
+		Vec3 center;
+		switch (code)
+		{
+		case 0b000:
+			// One point submerged
+			sTetrahedronVolume1(ref.mPosition, ref.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, volume, center);
+			break;
+
+		case 0b001:
+			// Two points submerged
+			sTetrahedronVolume2(ref.mPosition, ref.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, volume, center);
+			break;
+
+		case 0b010:
+			// Two points submerged
+			sTetrahedronVolume2(ref.mPosition, ref.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, volume, center);
+			break;
+
+		case 0b100:
+			// Two points submerged
+			sTetrahedronVolume2(ref.mPosition, ref.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, volume, center);
+			break;
+
+		case 0b011:
+			// Three points submerged
+			sTetrahedronVolume3(ref.mPosition, ref.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, volume, center);
+			break;
+
+		case 0b101:
+			// Three points submerged
+			sTetrahedronVolume3(ref.mPosition, ref.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, volume, center);
+			break;
+
+		case 0b110:
+			// Three points submerged
+			sTetrahedronVolume3(ref.mPosition, ref.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, volume, center);
+			break;
+
+		case 0b111:
+			// Four points submerged
+			sTetrahedronVolume4(ref.mPosition, p3.mPosition, p2.mPosition, p1.mPosition, volume, center);
+			break;
+
+		default:
+			// Should not be possible
+			JPH_ASSERT(false);
+			volume = 0.0f;
+			center = Vec3::sZero();
+			break;
+		}
+
+		mSubmergedVolume += volume;
+		mCenterOfBuoyancy += volume * center;
+	}
+
+	/// Call after all faces have been added. Returns the submerged volume and the center of buoyancy for the submerged volume.
+	void				GetResult(float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy) const
+	{
+		outCenterOfBuoyancy = mSubmergedVolume > 0.0f? mCenterOfBuoyancy / (4.0f * mSubmergedVolume) : Vec3::sZero(); // Do this before dividing submerged volume by 6 to get correct weight factor
+		outSubmergedVolume = mSubmergedVolume / 6.0f;
+	}
+
+private:
+	// The precalculated points for this polyhedron
+	const Point *		mPoints;
+
+	// If all points are above/below the surface
+	bool				mAllBelow = true;
+	bool				mAllAbove = true;
+
+	// The lowest point
+	int					mReferencePointIdx = 0;
+
+	// Aggregator for submerged volume and center of buoyancy
+	float				mSubmergedVolume = 0.0f;
+	Vec3				mCenterOfBuoyancy = Vec3::sZero();
+
+#ifdef JPH_DEBUG_RENDERER
+	// Base offset used for drawing
+	RVec3				mBaseOffset;
+#endif
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.cpp
new file mode 100644
index 0000000000..66b8bea1eb
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.cpp
@@ -0,0 +1,333 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(RotatedTranslatedShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(RotatedTranslatedShapeSettings, DecoratedShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(RotatedTranslatedShapeSettings, mPosition)
+	JPH_ADD_ATTRIBUTE(RotatedTranslatedShapeSettings, mRotation)
+}
+
+ShapeSettings::ShapeResult RotatedTranslatedShapeSettings::Create() const
+{
+	if (mCachedResult.IsEmpty())
+		Ref<Shape> shape = new RotatedTranslatedShape(*this, mCachedResult);
+	return mCachedResult;
+}
+
+RotatedTranslatedShape::RotatedTranslatedShape(const RotatedTranslatedShapeSettings &inSettings, ShapeResult &outResult) :
+	DecoratedShape(EShapeSubType::RotatedTranslated, inSettings, outResult)
+{
+	if (outResult.HasError())
+		return;
+
+	// Calculate center of mass position
+	mCenterOfMass = inSettings.mPosition + inSettings.mRotation * mInnerShape->GetCenterOfMass();
+
+	// Store rotation (position is always zero because we center around the center of mass)
+	mRotation = inSettings.mRotation;
+	mIsRotationIdentity = mRotation.IsClose(Quat::sIdentity());
+
+	outResult.Set(this);
+}
+
+RotatedTranslatedShape::RotatedTranslatedShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape) :
+	DecoratedShape(EShapeSubType::RotatedTranslated, inShape)
+{
+	// Calculate center of mass position
+	mCenterOfMass = inPosition + inRotation * mInnerShape->GetCenterOfMass();
+
+	// Store rotation (position is always zero because we center around the center of mass)
+	mRotation = inRotation;
+	mIsRotationIdentity = mRotation.IsClose(Quat::sIdentity());
+}
+
+MassProperties RotatedTranslatedShape::GetMassProperties() const
+{
+	// Rotate inertia of child into place
+	MassProperties p = mInnerShape->GetMassProperties();
+	p.Rotate(Mat44::sRotation(mRotation));
+	return p;
+}
+
+AABox RotatedTranslatedShape::GetLocalBounds() const
+{
+	return mInnerShape->GetLocalBounds().Transformed(Mat44::sRotation(mRotation));
+}
+
+AABox RotatedTranslatedShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
+	Mat44 transform = inCenterOfMassTransform * Mat44::sRotation(mRotation);
+	return mInnerShape->GetWorldSpaceBounds(transform, TransformScale(inScale));
+}
+
+TransformedShape RotatedTranslatedShape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const
+{
+	// We don't use any bits in the sub shape ID
+	outRemainder = inSubShapeID;
+
+	TransformedShape ts(RVec3(inPositionCOM), inRotation * mRotation, mInnerShape, BodyID());
+	ts.SetShapeScale(TransformScale(inScale));
+	return ts;
+}
+
+Vec3 RotatedTranslatedShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	// Transform surface position to local space and pass call on
+	Mat44 transform = Mat44::sRotation(mRotation.Conjugated());
+	Vec3 normal = mInnerShape->GetSurfaceNormal(inSubShapeID, transform * inLocalSurfacePosition);
+
+	// Transform normal to this shape's space
+	return transform.Multiply3x3Transposed(normal);
+}
+
+void RotatedTranslatedShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
+	Mat44 transform = Mat44::sRotation(mRotation);
+	mInnerShape->GetSupportingFace(inSubShapeID, transform.Multiply3x3Transposed(inDirection), TransformScale(inScale), inCenterOfMassTransform * transform, outVertices);
+}
+
+void RotatedTranslatedShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const
+{
+	// Get center of mass transform of child
+	Mat44 transform = inCenterOfMassTransform * Mat44::sRotation(mRotation);
+
+	// Recurse to child
+	mInnerShape->GetSubmergedVolume(transform, TransformScale(inScale), inSurface, outTotalVolume, outSubmergedVolume, outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, inBaseOffset));
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void RotatedTranslatedShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	mInnerShape->Draw(inRenderer, inCenterOfMassTransform * Mat44::sRotation(mRotation), TransformScale(inScale), inColor, inUseMaterialColors, inDrawWireframe);
+}
+
+void RotatedTranslatedShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const
+{
+	mInnerShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform * Mat44::sRotation(mRotation), TransformScale(inScale), inColor, inDrawSupportDirection);
+}
+
+void RotatedTranslatedShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
+	mInnerShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform * Mat44::sRotation(mRotation), TransformScale(inScale));
+}
+#endif // JPH_DEBUG_RENDERER
+
+bool RotatedTranslatedShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	// Transform the ray
+	Mat44 transform = Mat44::sRotation(mRotation.Conjugated());
+	RayCast ray = inRay.Transformed(transform);
+
+	return mInnerShape->CastRay(ray, inSubShapeIDCreator, ioHit);
+}
+
+void RotatedTranslatedShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	// Transform the ray
+	Mat44 transform = Mat44::sRotation(mRotation.Conjugated());
+	RayCast ray = inRay.Transformed(transform);
+
+	return mInnerShape->CastRay(ray, inRayCastSettings, inSubShapeIDCreator, ioCollector, inShapeFilter);
+}
+
+void RotatedTranslatedShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	// Transform the point
+	Mat44 transform = Mat44::sRotation(mRotation.Conjugated());
+	mInnerShape->CollidePoint(transform * inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter);
+}
+
+void RotatedTranslatedShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
+{
+	mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform * Mat44::sRotation(mRotation), inScale, inVertices, inNumVertices, inCollidingShapeIndex);
+}
+
+void RotatedTranslatedShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	mInnerShape->CollectTransformedShapes(inBox, inPositionCOM, inRotation * mRotation, TransformScale(inScale), inSubShapeIDCreator, ioCollector, inShapeFilter);
+}
+
+void RotatedTranslatedShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const
+{
+	mInnerShape->TransformShape(inCenterOfMassTransform * Mat44::sRotation(mRotation), ioCollector);
+}
+
+void RotatedTranslatedShape::sCollideRotatedTranslatedVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+{
+	JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::RotatedTranslated);
+	const RotatedTranslatedShape *shape1 = static_cast<const RotatedTranslatedShape *>(inShape1);
+
+	// Get world transform of 1
+	Mat44 transform1 = inCenterOfMassTransform1 * Mat44::sRotation(shape1->mRotation);
+
+	CollisionDispatch::sCollideShapeVsShape(shape1->mInnerShape, inShape2, shape1->TransformScale(inScale1), inScale2, transform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
+}
+
+void RotatedTranslatedShape::sCollideShapeVsRotatedTranslated(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+{
+	JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::RotatedTranslated);
+	const RotatedTranslatedShape *shape2 = static_cast<const RotatedTranslatedShape *>(inShape2);
+
+	// Get world transform of 2
+	Mat44 transform2 = inCenterOfMassTransform2 * Mat44::sRotation(shape2->mRotation);
+
+	CollisionDispatch::sCollideShapeVsShape(inShape1, shape2->mInnerShape, inScale1, shape2->TransformScale(inScale2), inCenterOfMassTransform1, transform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
+}
+
+void RotatedTranslatedShape::sCollideRotatedTranslatedVsRotatedTranslated(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+{
+	JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::RotatedTranslated);
+	const RotatedTranslatedShape *shape1 = static_cast<const RotatedTranslatedShape *>(inShape1);
+	JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::RotatedTranslated);
+	const RotatedTranslatedShape *shape2 = static_cast<const RotatedTranslatedShape *>(inShape2);
+
+	// Get world transform of 1 and 2
+	Mat44 transform1 = inCenterOfMassTransform1 * Mat44::sRotation(shape1->mRotation);
+	Mat44 transform2 = inCenterOfMassTransform2 * Mat44::sRotation(shape2->mRotation);
+
+	CollisionDispatch::sCollideShapeVsShape(shape1->mInnerShape, shape2->mInnerShape, shape1->TransformScale(inScale1), shape2->TransformScale(inScale2), transform1, transform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
+}
+
+void RotatedTranslatedShape::sCastRotatedTranslatedVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	// Fetch rotated translated shape from cast shape
+	JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::RotatedTranslated);
+	const RotatedTranslatedShape *shape1 = static_cast<const RotatedTranslatedShape *>(inShapeCast.mShape);
+
+	// Transform the shape cast and update the shape
+	Mat44 transform = inShapeCast.mCenterOfMassStart * Mat44::sRotation(shape1->mRotation);
+	Vec3 scale = shape1->TransformScale(inShapeCast.mScale);
+	ShapeCast shape_cast(shape1->mInnerShape, scale, transform, inShapeCast.mDirection);
+
+	CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+}
+
+void RotatedTranslatedShape::sCastShapeVsRotatedTranslated(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::RotatedTranslated);
+	const RotatedTranslatedShape *shape = static_cast<const RotatedTranslatedShape *>(inShape);
+
+	// Determine the local transform
+	Mat44 local_transform = Mat44::sRotation(shape->mRotation);
+
+	// Transform the shape cast
+	ShapeCast shape_cast = inShapeCast.PostTransformed(local_transform.Transposed3x3());
+
+	CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, shape->mInnerShape, shape->TransformScale(inScale), inShapeFilter, inCenterOfMassTransform2 * local_transform, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+}
+
+void RotatedTranslatedShape::sCastRotatedTranslatedVsRotatedTranslated(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::RotatedTranslated);
+	const RotatedTranslatedShape *shape1 = static_cast<const RotatedTranslatedShape *>(inShapeCast.mShape);
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::RotatedTranslated);
+	const RotatedTranslatedShape *shape2 = static_cast<const RotatedTranslatedShape *>(inShape);
+
+	// Determine the local transform of shape 2
+	Mat44 local_transform2 = Mat44::sRotation(shape2->mRotation);
+	Mat44 local_transform2_transposed = local_transform2.Transposed3x3();
+
+	// Transform the shape cast and update the shape
+	Mat44 transform = (local_transform2_transposed * inShapeCast.mCenterOfMassStart) * Mat44::sRotation(shape1->mRotation);
+	Vec3 scale = shape1->TransformScale(inShapeCast.mScale);
+	ShapeCast shape_cast(shape1->mInnerShape, scale, transform, local_transform2_transposed.Multiply3x3(inShapeCast.mDirection));
+
+	CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, shape2->mInnerShape, shape2->TransformScale(inScale), inShapeFilter, inCenterOfMassTransform2 * local_transform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+}
+
+void RotatedTranslatedShape::SaveBinaryState(StreamOut &inStream) const
+{
+	DecoratedShape::SaveBinaryState(inStream);
+
+	inStream.Write(mCenterOfMass);
+	inStream.Write(mRotation);
+}
+
+void RotatedTranslatedShape::RestoreBinaryState(StreamIn &inStream)
+{
+	DecoratedShape::RestoreBinaryState(inStream);
+
+	inStream.Read(mCenterOfMass);
+	inStream.Read(mRotation);
+	mIsRotationIdentity = mRotation.IsClose(Quat::sIdentity());
+}
+
+bool RotatedTranslatedShape::IsValidScale(Vec3Arg inScale) const
+{
+	if (!Shape::IsValidScale(inScale))
+		return false;
+
+	if (mIsRotationIdentity || ScaleHelpers::IsUniformScale(inScale))
+		return mInnerShape->IsValidScale(inScale);
+
+	if (!ScaleHelpers::CanScaleBeRotated(mRotation, inScale))
+		return false;
+
+	return mInnerShape->IsValidScale(ScaleHelpers::RotateScale(mRotation, inScale));
+}
+
+Vec3 RotatedTranslatedShape::MakeScaleValid(Vec3Arg inScale) const
+{
+	Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale);
+
+	if (mIsRotationIdentity || ScaleHelpers::IsUniformScale(scale))
+		return mInnerShape->MakeScaleValid(scale);
+
+	if (ScaleHelpers::CanScaleBeRotated(mRotation, scale))
+		return ScaleHelpers::RotateScale(mRotation.Conjugated(), mInnerShape->MakeScaleValid(ScaleHelpers::RotateScale(mRotation, scale)));
+
+	Vec3 abs_uniform_scale = ScaleHelpers::MakeUniformScale(scale.Abs());
+	Vec3 uniform_scale = scale.GetSign() * abs_uniform_scale;
+	if (ScaleHelpers::CanScaleBeRotated(mRotation, uniform_scale))
+		return uniform_scale;
+
+	return Sign(scale.GetX()) * abs_uniform_scale;
+}
+
+void RotatedTranslatedShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::RotatedTranslated);
+	f.mConstruct = []() -> Shape * { return new RotatedTranslatedShape; };
+	f.mColor = Color::sBlue;
+
+	for (EShapeSubType s : sAllSubShapeTypes)
+	{
+		CollisionDispatch::sRegisterCollideShape(EShapeSubType::RotatedTranslated, s, sCollideRotatedTranslatedVsShape);
+		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::RotatedTranslated, sCollideShapeVsRotatedTranslated);
+		CollisionDispatch::sRegisterCastShape(EShapeSubType::RotatedTranslated, s, sCastRotatedTranslatedVsShape);
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::RotatedTranslated, sCastShapeVsRotatedTranslated);
+	}
+
+	CollisionDispatch::sRegisterCollideShape(EShapeSubType::RotatedTranslated, EShapeSubType::RotatedTranslated, sCollideRotatedTranslatedVsRotatedTranslated);
+	CollisionDispatch::sRegisterCastShape(EShapeSubType::RotatedTranslated, EShapeSubType::RotatedTranslated, sCastRotatedTranslatedVsRotatedTranslated);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h
new file mode 100644
index 0000000000..2978a9559c
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h
@@ -0,0 +1,161 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/DecoratedShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+
+JPH_NAMESPACE_BEGIN
+
+class CollideShapeSettings;
+
+/// Class that constructs a RotatedTranslatedShape
+class JPH_EXPORT RotatedTranslatedShapeSettings final : public DecoratedShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, RotatedTranslatedShapeSettings)
+
+public:
+	/// Constructor
+									RotatedTranslatedShapeSettings() = default;
+
+	/// Construct with shape settings, can be serialized.
+									RotatedTranslatedShapeSettings(Vec3Arg inPosition, QuatArg inRotation, const ShapeSettings *inShape) : DecoratedShapeSettings(inShape), mPosition(inPosition), mRotation(inRotation) { }
+
+	/// Variant that uses a concrete shape, which means this object cannot be serialized.
+									RotatedTranslatedShapeSettings(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape): DecoratedShapeSettings(inShape), mPosition(inPosition), mRotation(inRotation) { }
+
+	// See: ShapeSettings
+	virtual ShapeResult				Create() const override;
+
+	Vec3							mPosition;												///< Position of the sub shape
+	Quat							mRotation;												///< Rotation of the sub shape
+};
+
+/// A rotated translated shape will rotate and translate a child shape.
+/// Shifts the child object so that it is centered around the center of mass.
+class JPH_EXPORT RotatedTranslatedShape final : public DecoratedShape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+									RotatedTranslatedShape() : DecoratedShape(EShapeSubType::RotatedTranslated) { }
+									RotatedTranslatedShape(const RotatedTranslatedShapeSettings &inSettings, ShapeResult &outResult);
+									RotatedTranslatedShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape);
+
+	/// Access the rotation that is applied to the inner shape
+	Quat							GetRotation() const										{ return mRotation; }
+
+	/// Access the translation that has been applied to the inner shape
+	Vec3							GetPosition() const										{ return mCenterOfMass - mRotation * mInnerShape->GetCenterOfMass(); }
+
+	// See Shape::GetCenterOfMass
+	virtual Vec3					GetCenterOfMass() const override						{ return mCenterOfMass; }
+
+	// See Shape::GetLocalBounds
+	virtual AABox					GetLocalBounds() const override;
+
+	// See Shape::GetWorldSpaceBounds
+	virtual AABox					GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+	using Shape::GetWorldSpaceBounds;
+
+	// See Shape::GetInnerRadius
+	virtual float					GetInnerRadius() const override							{ return mInnerShape->GetInnerRadius(); }
+
+	// See Shape::GetMassProperties
+	virtual MassProperties			GetMassProperties() const override;
+
+	// See Shape::GetSubShapeTransformedShape
+	virtual TransformedShape		GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const override;
+
+	// See Shape::GetSurfaceNormal
+	virtual Vec3					GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override;
+
+	// See Shape::GetSupportingFace
+	virtual void					GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
+
+	// See Shape::GetSubmergedVolume
+	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override;
+
+#ifdef JPH_DEBUG_RENDERER
+	// See Shape::Draw
+	virtual void					Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+
+	// See Shape::DrawGetSupportFunction
+	virtual void					DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override;
+
+	// See Shape::DrawGetSupportingFace
+	virtual void					DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// See Shape::CastRay
+	virtual bool					CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+	virtual void					CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollidePoint
+	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollideSoftBodyVertices
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
+
+	// See Shape::CollectTransformedShapes
+	virtual void					CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override;
+
+	// See Shape::TransformShape
+	virtual void					TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override;
+
+	// See Shape::GetTrianglesStart
+	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); }
+
+	// See Shape::GetTrianglesNext
+	virtual int						GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); return 0; }
+
+	// See Shape
+	virtual void					SaveBinaryState(StreamOut &inStream) const override;
+
+	// See Shape::GetStats
+	virtual Stats					GetStats() const override								{ return Stats(sizeof(*this), 0); }
+
+	// See Shape::GetVolume
+	virtual float					GetVolume() const override								{ return mInnerShape->GetVolume(); }
+
+	// See Shape::IsValidScale
+	virtual bool					IsValidScale(Vec3Arg inScale) const override;
+
+	// See Shape::MakeScaleValid
+	virtual Vec3					MakeScaleValid(Vec3Arg inScale) const override;
+
+	/// Transform the scale to the local space of the child shape
+	inline Vec3						TransformScale(Vec3Arg inScale) const
+	{
+		// We don't need to transform uniform scale or if the rotation is identity
+		if (mIsRotationIdentity || ScaleHelpers::IsUniformScale(inScale))
+			return inScale;
+
+		return ScaleHelpers::RotateScale(mRotation, inScale);
+	}
+
+	// Register shape functions with the registry
+	static void						sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void					RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	// Helper functions called by CollisionDispatch
+	static void						sCollideRotatedTranslatedVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCollideShapeVsRotatedTranslated(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCollideRotatedTranslatedVsRotatedTranslated(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCastRotatedTranslatedVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+	static void						sCastShapeVsRotatedTranslated(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+	static void						sCastRotatedTranslatedVsRotatedTranslated(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+
+	bool							mIsRotationIdentity;									///< If mRotation is close to identity (put here because it falls in padding bytes)
+	Vec3							mCenterOfMass;											///< Position of the center of mass
+	Quat							mRotation;												///< Rotation of the child shape
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaleHelpers.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaleHelpers.h
new file mode 100644
index 0000000000..4035dd77ae
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaleHelpers.h
@@ -0,0 +1,83 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/PhysicsSettings.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Helper functions to get properties of a scaling vector
+namespace ScaleHelpers
+{
+	/// Minimum valid scale value. This is used to prevent division by zero when scaling a shape with a zero scale.
+	static constexpr float	cMinScale = 1.0e-6f;
+
+	/// The tolerance used to check if components of the scale vector are the same
+	static constexpr float	cScaleToleranceSq = 1.0e-8f;
+
+	/// Test if a scale is identity
+	inline bool				IsNotScaled(Vec3Arg inScale)									{ return inScale.IsClose(Vec3::sReplicate(1.0f), cScaleToleranceSq); }
+
+	/// Test if a scale is uniform
+	inline bool				IsUniformScale(Vec3Arg inScale)									{ return inScale.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>().IsClose(inScale, cScaleToleranceSq); }
+
+	/// Test if a scale is uniform in XZ
+	inline bool				IsUniformScaleXZ(Vec3Arg inScale)								{ return inScale.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X>().IsClose(inScale, ScaleHelpers::cScaleToleranceSq); }
+
+	/// Scale the convex radius of an object
+	inline float			ScaleConvexRadius(float inConvexRadius, Vec3Arg inScale)		{ return min(inConvexRadius * inScale.Abs().ReduceMin(), cDefaultConvexRadius); }
+
+	/// Test if a scale flips an object inside out (which requires flipping all normals and polygon windings)
+	inline bool				IsInsideOut(Vec3Arg inScale)									{ return (CountBits(Vec3::sLess(inScale, Vec3::sZero()).GetTrues() & 0x7) & 1) != 0; }
+
+	/// Test if any of the components of the scale have a value below cMinScale
+	inline bool				IsZeroScale(Vec3Arg inScale)									{ return Vec3::sLess(inScale.Abs(), Vec3::sReplicate(cMinScale)).TestAnyXYZTrue(); }
+
+	/// Ensure that the scale for each component is at least cMinScale
+	inline Vec3				MakeNonZeroScale(Vec3Arg inScale)								{ return inScale.GetSign() * Vec3::sMax(inScale.Abs(), Vec3::sReplicate(cMinScale)); }
+
+	/// Get the average scale if inScale, used to make the scale uniform when a shape doesn't support non-uniform scale
+	inline Vec3				MakeUniformScale(Vec3Arg inScale)								{ return Vec3::sReplicate((inScale.GetX() + inScale.GetY() + inScale.GetZ()) / 3.0f); }
+
+	/// Average the scale in XZ, used to make the scale uniform when a shape doesn't support non-uniform scale in the XZ plane
+	inline Vec3				MakeUniformScaleXZ(Vec3Arg inScale)								{ return 0.5f * (inScale + inScale.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X>()); }
+
+	/// Checks in scale can be rotated to child shape
+	/// @param inRotation Rotation of child shape
+	/// @param inScale Scale in local space of parent shape
+	/// @return True if the scale is valid (no shearing introduced)
+	inline bool				CanScaleBeRotated(QuatArg inRotation, Vec3Arg inScale)
+	{
+		// inScale is a scale in local space of the shape, so the transform for the shape (ignoring translation) is: T = Mat44::sScale(inScale) * mRotation.
+		// when we pass the scale to the child it needs to be local to the child, so we want T = mRotation * Mat44::sScale(ChildScale).
+		// Solving for ChildScale: ChildScale = mRotation^-1 * Mat44::sScale(inScale) * mRotation = mRotation^T * Mat44::sScale(inScale) * mRotation
+		// If any of the off diagonal elements are non-zero, it means the scale / rotation is not compatible.
+		Mat44 r = Mat44::sRotation(inRotation);
+		Mat44 child_scale = r.Multiply3x3LeftTransposed(r.PostScaled(inScale));
+
+		// Get the columns, but zero the diagonal
+		Vec4 zero = Vec4::sZero();
+		Vec4 c0 = Vec4::sSelect(child_scale.GetColumn4(0), zero, UVec4(0xffffffff, 0, 0, 0)).Abs();
+		Vec4 c1 = Vec4::sSelect(child_scale.GetColumn4(1), zero, UVec4(0, 0xffffffff, 0, 0)).Abs();
+		Vec4 c2 = Vec4::sSelect(child_scale.GetColumn4(2), zero, UVec4(0, 0, 0xffffffff, 0)).Abs();
+
+		// Check if all elements are less than epsilon
+		Vec4 epsilon = Vec4::sReplicate(1.0e-6f);
+		return UVec4::sAnd(UVec4::sAnd(Vec4::sLess(c0, epsilon), Vec4::sLess(c1, epsilon)), Vec4::sLess(c2, epsilon)).TestAllTrue();
+	}
+
+	/// Adjust scale for rotated child shape
+	/// @param inRotation Rotation of child shape
+	/// @param inScale Scale in local space of parent shape
+	/// @return Rotated scale
+	inline Vec3				RotateScale(QuatArg inRotation, Vec3Arg inScale)
+	{
+		// Get the diagonal of mRotation^T * Mat44::sScale(inScale) * mRotation (see comment at CanScaleBeRotated)
+		Mat44 r = Mat44::sRotation(inRotation);
+		return r.Multiply3x3LeftTransposed(r.PostScaled(inScale)).GetDiagonal3();
+	}
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaledShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaledShape.cpp
new file mode 100644
index 0000000000..1511813892
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaledShape.cpp
@@ -0,0 +1,238 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/Shape/ScaledShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(ScaledShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(ScaledShapeSettings, DecoratedShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(ScaledShapeSettings, mScale)
+}
+
+ShapeSettings::ShapeResult ScaledShapeSettings::Create() const
+{
+	if (mCachedResult.IsEmpty())
+		Ref<Shape> shape = new ScaledShape(*this, mCachedResult);
+	return mCachedResult;
+}
+
+ScaledShape::ScaledShape(const ScaledShapeSettings &inSettings, ShapeResult &outResult) :
+	DecoratedShape(EShapeSubType::Scaled, inSettings, outResult),
+	mScale(inSettings.mScale)
+{
+	if (outResult.HasError())
+		return;
+
+	if (ScaleHelpers::IsZeroScale(inSettings.mScale))
+	{
+		outResult.SetError("Can't use zero scale!");
+		return;
+	}
+
+	outResult.Set(this);
+}
+
+MassProperties ScaledShape::GetMassProperties() const
+{
+	MassProperties p = mInnerShape->GetMassProperties();
+	p.Scale(mScale);
+	return p;
+}
+
+AABox ScaledShape::GetLocalBounds() const
+{
+	return mInnerShape->GetLocalBounds().Scaled(mScale);
+}
+
+AABox ScaledShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
+	return mInnerShape->GetWorldSpaceBounds(inCenterOfMassTransform, inScale * mScale);
+}
+
+TransformedShape ScaledShape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const
+{
+	// We don't use any bits in the sub shape ID
+	outRemainder = inSubShapeID;
+
+	TransformedShape ts(RVec3(inPositionCOM), inRotation, mInnerShape, BodyID());
+	ts.SetShapeScale(inScale * mScale);
+	return ts;
+}
+
+Vec3 ScaledShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	// Transform the surface point to local space and pass the query on
+	Vec3 normal = mInnerShape->GetSurfaceNormal(inSubShapeID, inLocalSurfacePosition / mScale);
+
+	// Need to transform the plane normals using inScale
+	// Transforming a direction with matrix M is done through multiplying by (M^-1)^T
+	// In this case M is a diagonal matrix with the scale vector, so we need to multiply our normal by 1 / scale and renormalize afterwards
+	return (normal / mScale).Normalized();
+}
+
+void ScaledShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
+	mInnerShape->GetSupportingFace(inSubShapeID, inDirection, inScale * mScale, inCenterOfMassTransform, outVertices);
+}
+
+void ScaledShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const
+{
+	mInnerShape->GetSubmergedVolume(inCenterOfMassTransform, inScale * mScale, inSurface, outTotalVolume, outSubmergedVolume, outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, inBaseOffset));
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void ScaledShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	mInnerShape->Draw(inRenderer, inCenterOfMassTransform, inScale * mScale, inColor, inUseMaterialColors, inDrawWireframe);
+}
+
+void ScaledShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const
+{
+	mInnerShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform, inScale * mScale, inColor, inDrawSupportDirection);
+}
+
+void ScaledShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
+	mInnerShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform, inScale * mScale);
+}
+#endif // JPH_DEBUG_RENDERER
+
+bool ScaledShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	Vec3 inv_scale = mScale.Reciprocal();
+	RayCast scaled_ray { inv_scale * inRay.mOrigin, inv_scale * inRay.mDirection };
+	return mInnerShape->CastRay(scaled_ray, inSubShapeIDCreator, ioHit);
+}
+
+void ScaledShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	Vec3 inv_scale = mScale.Reciprocal();
+	RayCast scaled_ray { inv_scale * inRay.mOrigin, inv_scale * inRay.mDirection };
+	return mInnerShape->CastRay(scaled_ray, inRayCastSettings, inSubShapeIDCreator, ioCollector, inShapeFilter);
+}
+
+void ScaledShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	Vec3 inv_scale = mScale.Reciprocal();
+	mInnerShape->CollidePoint(inv_scale * inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter);
+}
+
+void ScaledShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
+{
+	mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform, inScale * mScale, inVertices, inNumVertices, inCollidingShapeIndex);
+}
+
+void ScaledShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	mInnerShape->CollectTransformedShapes(inBox, inPositionCOM, inRotation, inScale * mScale, inSubShapeIDCreator, ioCollector, inShapeFilter);
+}
+
+void ScaledShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const
+{
+	mInnerShape->TransformShape(inCenterOfMassTransform * Mat44::sScale(mScale), ioCollector);
+}
+
+void ScaledShape::SaveBinaryState(StreamOut &inStream) const
+{
+	DecoratedShape::SaveBinaryState(inStream);
+
+	inStream.Write(mScale);
+}
+
+void ScaledShape::RestoreBinaryState(StreamIn &inStream)
+{
+	DecoratedShape::RestoreBinaryState(inStream);
+
+	inStream.Read(mScale);
+}
+
+float ScaledShape::GetVolume() const
+{
+	return abs(mScale.GetX() * mScale.GetY() * mScale.GetZ()) * mInnerShape->GetVolume();
+}
+
+bool ScaledShape::IsValidScale(Vec3Arg inScale) const
+{
+	return mInnerShape->IsValidScale(inScale * mScale);
+}
+
+Vec3 ScaledShape::MakeScaleValid(Vec3Arg inScale) const
+{
+	return mInnerShape->MakeScaleValid(mScale * inScale) / mScale;
+}
+
+void ScaledShape::sCollideScaledVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+{
+	JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::Scaled);
+	const ScaledShape *shape1 = static_cast<const ScaledShape *>(inShape1);
+
+	CollisionDispatch::sCollideShapeVsShape(shape1->GetInnerShape(), inShape2, inScale1 * shape1->GetScale(), inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
+}
+
+void ScaledShape::sCollideShapeVsScaled(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+{
+	JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::Scaled);
+	const ScaledShape *shape2 = static_cast<const ScaledShape *>(inShape2);
+
+	CollisionDispatch::sCollideShapeVsShape(inShape1, shape2->GetInnerShape(), inScale1, inScale2 * shape2->GetScale(), inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
+}
+
+void ScaledShape::sCastScaledVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::Scaled);
+	const ScaledShape *shape = static_cast<const ScaledShape *>(inShapeCast.mShape);
+
+	ShapeCast scaled_cast(shape->GetInnerShape(), inShapeCast.mScale * shape->GetScale(), inShapeCast.mCenterOfMassStart, inShapeCast.mDirection);
+	CollisionDispatch::sCastShapeVsShapeLocalSpace(scaled_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+}
+
+void ScaledShape::sCastShapeVsScaled(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::Scaled);
+	const ScaledShape *shape = static_cast<const ScaledShape *>(inShape);
+
+	CollisionDispatch::sCastShapeVsShapeLocalSpace(inShapeCast, inShapeCastSettings, shape->mInnerShape, inScale * shape->mScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+}
+
+void ScaledShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Scaled);
+	f.mConstruct = []() -> Shape * { return new ScaledShape; };
+	f.mColor = Color::sYellow;
+
+	for (EShapeSubType s : sAllSubShapeTypes)
+	{
+		CollisionDispatch::sRegisterCollideShape(EShapeSubType::Scaled, s, sCollideScaledVsShape);
+		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::Scaled, sCollideShapeVsScaled);
+		CollisionDispatch::sRegisterCastShape(EShapeSubType::Scaled, s, sCastScaledVsShape);
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Scaled, sCastShapeVsScaled);
+	}
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaledShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaledShape.h
new file mode 100644
index 0000000000..6da92b6ed3
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaledShape.h
@@ -0,0 +1,145 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/DecoratedShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+
+JPH_NAMESPACE_BEGIN
+
+class SubShapeIDCreator;
+class CollideShapeSettings;
+
+/// Class that constructs a ScaledShape
+class JPH_EXPORT ScaledShapeSettings final : public DecoratedShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, ScaledShapeSettings)
+
+public:
+	/// Default constructor for deserialization
+									ScaledShapeSettings() = default;
+
+	/// Constructor that decorates another shape with a scale
+									ScaledShapeSettings(const ShapeSettings *inShape, Vec3Arg inScale) : DecoratedShapeSettings(inShape), mScale(inScale) { }
+
+	/// Variant that uses a concrete shape, which means this object cannot be serialized.
+									ScaledShapeSettings(const Shape *inShape, Vec3Arg inScale) : DecoratedShapeSettings(inShape), mScale(inScale) { }
+
+	// See: ShapeSettings
+	virtual ShapeResult				Create() const override;
+
+	Vec3							mScale = Vec3(1, 1, 1);
+};
+
+/// A shape that scales a child shape in local space of that shape. The scale can be non-uniform and can even turn it inside out when one or three components of the scale are negative.
+class JPH_EXPORT ScaledShape final : public DecoratedShape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+									ScaledShape() : DecoratedShape(EShapeSubType::Scaled) { }
+									ScaledShape(const ScaledShapeSettings &inSettings, ShapeResult &outResult);
+
+	/// Constructor that decorates another shape with a scale
+									ScaledShape(const Shape *inShape, Vec3Arg inScale)		: DecoratedShape(EShapeSubType::Scaled, inShape), mScale(inScale) { JPH_ASSERT(!ScaleHelpers::IsZeroScale(mScale)); }
+
+	/// Get the scale
+	Vec3							GetScale() const										{ return mScale; }
+
+	// See Shape::GetCenterOfMass
+	virtual Vec3					GetCenterOfMass() const override						{ return mScale * mInnerShape->GetCenterOfMass(); }
+
+	// See Shape::GetLocalBounds
+	virtual AABox					GetLocalBounds() const override;
+
+	// See Shape::GetWorldSpaceBounds
+	virtual AABox					GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+	using Shape::GetWorldSpaceBounds;
+
+	// See Shape::GetInnerRadius
+	virtual float					GetInnerRadius() const override							{ return mScale.ReduceMin() * mInnerShape->GetInnerRadius(); }
+
+	// See Shape::GetMassProperties
+	virtual MassProperties			GetMassProperties() const override;
+
+	// See Shape::GetSubShapeTransformedShape
+	virtual TransformedShape		GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const override;
+
+	// See Shape::GetSurfaceNormal
+	virtual Vec3					GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override;
+
+	// See Shape::GetSupportingFace
+	virtual void					GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
+
+	// See Shape::GetSubmergedVolume
+	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override;
+
+#ifdef JPH_DEBUG_RENDERER
+	// See Shape::Draw
+	virtual void					Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+
+	// See Shape::DrawGetSupportFunction
+	virtual void					DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override;
+
+	// See Shape::DrawGetSupportingFace
+	virtual void					DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// See Shape::CastRay
+	virtual bool					CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+	virtual void					CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollidePoint
+	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollideSoftBodyVertices
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
+
+	// See Shape::CollectTransformedShapes
+	virtual void					CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override;
+
+	// See Shape::TransformShape
+	virtual void					TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override;
+
+	// See Shape::GetTrianglesStart
+	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); }
+
+	// See Shape::GetTrianglesNext
+	virtual int						GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); return 0; }
+
+	// See Shape
+	virtual void					SaveBinaryState(StreamOut &inStream) const override;
+
+	// See Shape::GetStats
+	virtual Stats					GetStats() const override								{ return Stats(sizeof(*this), 0); }
+
+	// See Shape::GetVolume
+	virtual float					GetVolume() const override;
+
+	// See Shape::IsValidScale
+	virtual bool					IsValidScale(Vec3Arg inScale) const override;
+
+	// See Shape::MakeScaleValid
+	virtual Vec3					MakeScaleValid(Vec3Arg inScale) const override;
+
+	// Register shape functions with the registry
+	static void						sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void					RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	// Helper functions called by CollisionDispatch
+	static void						sCollideScaledVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCollideShapeVsScaled(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCastScaledVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+	static void						sCastShapeVsScaled(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+
+	Vec3							mScale = Vec3(1, 1, 1);
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/Shape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/Shape.cpp
new file mode 100644
index 0000000000..a3c37c0dae
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/Shape.cpp
@@ -0,0 +1,325 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+#include <Jolt/Physics/Collision/Shape/ScaledShape.h>
+#include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/PhysicsMaterial.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/Core/Factory.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT_BASE(ShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(ShapeSettings, SerializableObject)
+
+	JPH_ADD_ATTRIBUTE(ShapeSettings, mUserData)
+}
+
+#ifdef JPH_DEBUG_RENDERER
+bool Shape::sDrawSubmergedVolumes = false;
+#endif // JPH_DEBUG_RENDERER
+
+ShapeFunctions ShapeFunctions::sRegistry[NumSubShapeTypes];
+
+const Shape *Shape::GetLeafShape([[maybe_unused]] const SubShapeID &inSubShapeID, SubShapeID &outRemainder) const
+{
+	outRemainder = inSubShapeID;
+	return this;
+}
+
+TransformedShape Shape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const
+{
+	// We have reached the leaf shape so there is no remainder
+	outRemainder = SubShapeID();
+
+	// Just return the transformed shape for this shape
+	TransformedShape ts(RVec3(inPositionCOM), inRotation, this, BodyID());
+	ts.SetShapeScale(inScale);
+	return ts;
+}
+
+void Shape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	TransformedShape ts(RVec3(inPositionCOM), inRotation, this, TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator);
+	ts.SetShapeScale(inScale);
+	ioCollector.AddHit(ts);
+}
+
+void Shape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const
+{
+	Vec3 scale;
+	Mat44 transform = inCenterOfMassTransform.Decompose(scale);
+	TransformedShape ts(RVec3(transform.GetTranslation()), transform.GetQuaternion(), this, BodyID(), SubShapeIDCreator());
+	ts.SetShapeScale(MakeScaleValid(scale));
+	ioCollector.AddHit(ts);
+}
+
+void Shape::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(mShapeSubType);
+	inStream.Write(mUserData);
+}
+
+void Shape::RestoreBinaryState(StreamIn &inStream)
+{
+	// Type hash read by sRestoreFromBinaryState
+	inStream.Read(mUserData);
+}
+
+Shape::ShapeResult Shape::sRestoreFromBinaryState(StreamIn &inStream)
+{
+	ShapeResult result;
+
+	// Read the type of the shape
+	EShapeSubType shape_sub_type;
+	inStream.Read(shape_sub_type);
+	if (inStream.IsEOF() || inStream.IsFailed())
+	{
+		result.SetError("Failed to read type id");
+		return result;
+	}
+
+	// Construct and read the data of the shape
+	Ref<Shape> shape = ShapeFunctions::sGet(shape_sub_type).mConstruct();
+	shape->RestoreBinaryState(inStream);
+	if (inStream.IsEOF() || inStream.IsFailed())
+	{
+		result.SetError("Failed to restore shape");
+		return result;
+	}
+
+	result.Set(shape);
+	return result;
+}
+
+void Shape::SaveWithChildren(StreamOut &inStream, ShapeToIDMap &ioShapeMap, MaterialToIDMap &ioMaterialMap) const
+{
+	ShapeToIDMap::const_iterator shape_id_iter = ioShapeMap.find(this);
+	if (shape_id_iter == ioShapeMap.end())
+	{
+		// Write shape ID of this shape
+		uint32 shape_id = ioShapeMap.size();
+		ioShapeMap[this] = shape_id;
+		inStream.Write(shape_id);
+
+		// Write the shape itself
+		SaveBinaryState(inStream);
+
+		// Write the ID's of all sub shapes
+		ShapeList sub_shapes;
+		SaveSubShapeState(sub_shapes);
+		inStream.Write(uint32(sub_shapes.size()));
+		for (const Shape *shape : sub_shapes)
+		{
+			if (shape == nullptr)
+				inStream.Write(~uint32(0));
+			else
+				shape->SaveWithChildren(inStream, ioShapeMap, ioMaterialMap);
+		}
+
+		// Write the materials
+		PhysicsMaterialList materials;
+		SaveMaterialState(materials);
+		StreamUtils::SaveObjectArray(inStream, materials, &ioMaterialMap);
+	}
+	else
+	{
+		// Known shape, just write the ID
+		inStream.Write(shape_id_iter->second);
+	}
+}
+
+Shape::ShapeResult Shape::sRestoreWithChildren(StreamIn &inStream, IDToShapeMap &ioShapeMap, IDToMaterialMap &ioMaterialMap)
+{
+	ShapeResult result;
+
+	// Read ID of this shape
+	uint32 shape_id;
+	inStream.Read(shape_id);
+	if (inStream.IsEOF() || inStream.IsFailed())
+	{
+		result.SetError("Failed to read shape id");
+		return result;
+	}
+
+	// Check nullptr shape
+	if (shape_id == ~uint32(0))
+	{
+		result.Set(nullptr);
+		return result;
+	}
+
+	// Check if we already read this shape
+	if (shape_id < ioShapeMap.size())
+	{
+		result.Set(ioShapeMap[shape_id]);
+		return result;
+	}
+
+	// Read the shape
+	result = sRestoreFromBinaryState(inStream);
+	if (result.HasError())
+		return result;
+	JPH_ASSERT(ioShapeMap.size() == shape_id); // Assert that this is the next ID in the map
+	ioShapeMap.push_back(result.Get());
+
+	// Read the sub shapes
+	uint32 len;
+	inStream.Read(len);
+	if (inStream.IsEOF() || inStream.IsFailed())
+	{
+		result.SetError("Failed to read stream");
+		return result;
+	}
+	ShapeList sub_shapes;
+	sub_shapes.reserve(len);
+	for (size_t i = 0; i < len; ++i)
+	{
+		ShapeResult sub_shape_result = sRestoreWithChildren(inStream, ioShapeMap, ioMaterialMap);
+		if (sub_shape_result.HasError())
+			return sub_shape_result;
+		sub_shapes.push_back(sub_shape_result.Get());
+	}
+	result.Get()->RestoreSubShapeState(sub_shapes.data(), (uint)sub_shapes.size());
+
+	// Read the materials
+	Result mlresult = StreamUtils::RestoreObjectArray<PhysicsMaterialList>(inStream, ioMaterialMap);
+	if (mlresult.HasError())
+	{
+		result.SetError(mlresult.GetError());
+		return result;
+	}
+	const PhysicsMaterialList &materials = mlresult.Get();
+	result.Get()->RestoreMaterialState(materials.data(), (uint)materials.size());
+
+	return result;
+}
+
+Shape::Stats Shape::GetStatsRecursive(VisitedShapes &ioVisitedShapes) const
+{
+	Stats stats = GetStats();
+
+	// If shape is already visited, don't count its size again
+	if (!ioVisitedShapes.insert(this).second)
+		stats.mSizeBytes = 0;
+
+	return stats;
+}
+
+bool Shape::IsValidScale(Vec3Arg inScale) const
+{
+	return !ScaleHelpers::IsZeroScale(inScale);
+}
+
+Vec3 Shape::MakeScaleValid(Vec3Arg inScale) const
+{
+	return ScaleHelpers::MakeNonZeroScale(inScale);
+}
+
+Shape::ShapeResult Shape::ScaleShape(Vec3Arg inScale) const
+{
+	const Vec3 unit_scale = Vec3::sReplicate(1.0f);
+
+	if (inScale.IsNearZero())
+	{
+		ShapeResult result;
+		result.SetError("Can't use zero scale!");
+		return result;
+	}
+
+	// First test if we can just wrap this shape in a scaled shape
+	if (IsValidScale(inScale))
+	{
+		// Test if the scale is near unit
+		ShapeResult result;
+		if (inScale.IsClose(unit_scale))
+			result.Set(const_cast<Shape *>(this));
+		else
+			result.Set(new ScaledShape(this, inScale));
+		return result;
+	}
+
+	// Collect the leaf shapes and their transforms
+	struct Collector : TransformedShapeCollector
+	{
+		virtual void				AddHit(const ResultType &inResult) override
+		{
+			mShapes.push_back(inResult);
+		}
+
+		Array<TransformedShape>		mShapes;
+	};
+	Collector collector;
+	TransformShape(Mat44::sScale(inScale) * Mat44::sTranslation(GetCenterOfMass()), collector);
+
+	// Construct a compound shape
+	StaticCompoundShapeSettings compound;
+	compound.mSubShapes.reserve(collector.mShapes.size());
+	for (const TransformedShape &ts : collector.mShapes)
+	{
+		const Shape *shape = ts.mShape;
+
+		// Construct a scaled shape if scale is not unit
+		Vec3 scale = ts.GetShapeScale();
+		if (!scale.IsClose(unit_scale))
+			shape = new ScaledShape(shape, scale);
+
+		// Add the shape
+		compound.AddShape(Vec3(ts.mShapePositionCOM) - ts.mShapeRotation * shape->GetCenterOfMass(), ts.mShapeRotation, shape);
+	}
+
+	return compound.Create();
+}
+
+void Shape::sCollidePointUsingRayCast(const Shape &inShape, Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter)
+{
+	// First test if we're inside our bounding box
+	AABox bounds = inShape.GetLocalBounds();
+	if (bounds.Contains(inPoint))
+	{
+		// A collector that just counts the number of hits
+		class HitCountCollector : public CastRayCollector
+		{
+		public:
+			virtual void	AddHit(const RayCastResult &inResult) override
+			{
+				// Store the last sub shape ID so that we can provide something to our outer hit collector
+				mSubShapeID = inResult.mSubShapeID2;
+
+				++mHitCount;
+			}
+
+			int				mHitCount = 0;
+			SubShapeID		mSubShapeID;
+		};
+		HitCountCollector collector;
+
+		// Configure the raycast
+		RayCastSettings settings;
+		settings.SetBackFaceMode(EBackFaceMode::CollideWithBackFaces);
+
+		// Cast a ray that's 10% longer than the height of our bounding box
+		inShape.CastRay(RayCast { inPoint, 1.1f * bounds.GetSize().GetY() * Vec3::sAxisY() }, settings, inSubShapeIDCreator, collector, inShapeFilter);
+
+		// Odd amount of hits means inside
+		if ((collector.mHitCount & 1) == 1)
+			ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), collector.mSubShapeID });
+	}
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/Shape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/Shape.h
new file mode 100644
index 0000000000..f43b79c3f0
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/Shape.h
@@ -0,0 +1,466 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/MassProperties.h>
+#include <Jolt/Physics/Collision/BackFaceMode.h>
+#include <Jolt/Physics/Collision/CollisionCollector.h>
+#include <Jolt/Physics/Collision/ShapeFilter.h>
+#include <Jolt/Geometry/AABox.h>
+#include <Jolt/Core/Reference.h>
+#include <Jolt/Core/Color.h>
+#include <Jolt/Core/Result.h>
+#include <Jolt/Core/NonCopyable.h>
+#include <Jolt/Core/UnorderedMap.h>
+#include <Jolt/Core/UnorderedSet.h>
+#include <Jolt/Core/StreamUtils.h>
+#include <Jolt/ObjectStream/SerializableObject.h>
+
+JPH_NAMESPACE_BEGIN
+
+struct RayCast;
+class RayCastSettings;
+struct ShapeCast;
+class ShapeCastSettings;
+class RayCastResult;
+class ShapeCastResult;
+class CollidePointResult;
+class CollideShapeResult;
+class SubShapeIDCreator;
+class SubShapeID;
+class PhysicsMaterial;
+class TransformedShape;
+class Plane;
+class CollideSoftBodyVertexIterator;
+class Shape;
+class StreamOut;
+class StreamIn;
+#ifdef JPH_DEBUG_RENDERER
+class DebugRenderer;
+#endif // JPH_DEBUG_RENDERER
+
+using CastRayCollector = CollisionCollector<RayCastResult, CollisionCollectorTraitsCastRay>;
+using CastShapeCollector = CollisionCollector<ShapeCastResult, CollisionCollectorTraitsCastShape>;
+using CollidePointCollector = CollisionCollector<CollidePointResult, CollisionCollectorTraitsCollidePoint>;
+using CollideShapeCollector = CollisionCollector<CollideShapeResult, CollisionCollectorTraitsCollideShape>;
+using TransformedShapeCollector = CollisionCollector<TransformedShape, CollisionCollectorTraitsCollideShape>;
+
+using ShapeRefC = RefConst<Shape>;
+using ShapeList = Array<ShapeRefC>;
+using PhysicsMaterialRefC = RefConst<PhysicsMaterial>;
+using PhysicsMaterialList = Array<PhysicsMaterialRefC>;
+
+/// Shapes are categorized in groups, each shape can return which group it belongs to through its Shape::GetType function.
+enum class EShapeType : uint8
+{
+	Convex,							///< Used by ConvexShape, all shapes that use the generic convex vs convex collision detection system (box, sphere, capsule, tapered capsule, cylinder, triangle)
+	Compound,						///< Used by CompoundShape
+	Decorated,						///< Used by DecoratedShape
+	Mesh,							///< Used by MeshShape
+	HeightField,					///< Used by HeightFieldShape
+	SoftBody,						///< Used by SoftBodyShape
+
+	// User defined shapes
+	User1,
+	User2,
+	User3,
+	User4,
+
+	Plane,							///< Used by PlaneShape
+	Empty,							///< Used by EmptyShape
+};
+
+/// This enumerates all shape types, each shape can return its type through Shape::GetSubType
+enum class EShapeSubType : uint8
+{
+	// Convex shapes
+	Sphere,
+	Box,
+	Triangle,
+	Capsule,
+	TaperedCapsule,
+	Cylinder,
+	ConvexHull,
+
+	// Compound shapes
+	StaticCompound,
+	MutableCompound,
+
+	// Decorated shapes
+	RotatedTranslated,
+	Scaled,
+	OffsetCenterOfMass,
+
+	// Other shapes
+	Mesh,
+	HeightField,
+	SoftBody,
+
+	// User defined shapes
+	User1,
+	User2,
+	User3,
+	User4,
+	User5,
+	User6,
+	User7,
+	User8,
+
+	// User defined convex shapes
+	UserConvex1,
+	UserConvex2,
+	UserConvex3,
+	UserConvex4,
+	UserConvex5,
+	UserConvex6,
+	UserConvex7,
+	UserConvex8,
+
+	// Other shapes
+	Plane,
+	TaperedCylinder,
+	Empty,
+};
+
+// Sets of shape sub types
+static constexpr EShapeSubType sAllSubShapeTypes[] = { EShapeSubType::Sphere, EShapeSubType::Box, EShapeSubType::Triangle, EShapeSubType::Capsule, EShapeSubType::TaperedCapsule, EShapeSubType::Cylinder, EShapeSubType::ConvexHull, EShapeSubType::StaticCompound, EShapeSubType::MutableCompound, EShapeSubType::RotatedTranslated, EShapeSubType::Scaled, EShapeSubType::OffsetCenterOfMass, EShapeSubType::Mesh, EShapeSubType::HeightField, EShapeSubType::SoftBody, EShapeSubType::User1, EShapeSubType::User2, EShapeSubType::User3, EShapeSubType::User4, EShapeSubType::User5, EShapeSubType::User6, EShapeSubType::User7, EShapeSubType::User8, EShapeSubType::UserConvex1, EShapeSubType::UserConvex2, EShapeSubType::UserConvex3, EShapeSubType::UserConvex4, EShapeSubType::UserConvex5, EShapeSubType::UserConvex6, EShapeSubType::UserConvex7, EShapeSubType::UserConvex8, EShapeSubType::Plane, EShapeSubType::TaperedCylinder, EShapeSubType::Empty };
+static constexpr EShapeSubType sConvexSubShapeTypes[] = { EShapeSubType::Sphere, EShapeSubType::Box, EShapeSubType::Triangle, EShapeSubType::Capsule, EShapeSubType::TaperedCapsule, EShapeSubType::Cylinder, EShapeSubType::ConvexHull, EShapeSubType::TaperedCylinder, EShapeSubType::UserConvex1, EShapeSubType::UserConvex2, EShapeSubType::UserConvex3, EShapeSubType::UserConvex4, EShapeSubType::UserConvex5, EShapeSubType::UserConvex6, EShapeSubType::UserConvex7, EShapeSubType::UserConvex8 };
+static constexpr EShapeSubType sCompoundSubShapeTypes[] = { EShapeSubType::StaticCompound, EShapeSubType::MutableCompound };
+static constexpr EShapeSubType sDecoratorSubShapeTypes[] = { EShapeSubType::RotatedTranslated, EShapeSubType::Scaled, EShapeSubType::OffsetCenterOfMass };
+
+/// How many shape types we support
+static constexpr uint NumSubShapeTypes = uint(std::size(sAllSubShapeTypes));
+
+/// Names of sub shape types
+static constexpr const char *sSubShapeTypeNames[] = { "Sphere", "Box", "Triangle", "Capsule", "TaperedCapsule", "Cylinder", "ConvexHull", "StaticCompound", "MutableCompound", "RotatedTranslated", "Scaled", "OffsetCenterOfMass", "Mesh", "HeightField", "SoftBody", "User1", "User2", "User3", "User4", "User5", "User6", "User7", "User8", "UserConvex1", "UserConvex2", "UserConvex3", "UserConvex4", "UserConvex5", "UserConvex6", "UserConvex7", "UserConvex8", "Plane", "TaperedCylinder", "Empty" };
+static_assert(std::size(sSubShapeTypeNames) == NumSubShapeTypes);
+
+/// Class that can construct shapes and that is serializable using the ObjectStream system.
+/// Can be used to store shape data in 'uncooked' form (i.e. in a form that is still human readable and authorable).
+/// Once the shape has been created using the Create() function, the data will be moved into the Shape class
+/// in a form that is optimized for collision detection. After this, the ShapeSettings object is no longer needed
+/// and can be destroyed. Each shape class has a derived class of the ShapeSettings object to store shape specific
+/// data.
+class JPH_EXPORT ShapeSettings : public SerializableObject, public RefTarget<ShapeSettings>
+{
+	JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, ShapeSettings)
+
+public:
+	using ShapeResult = Result<Ref<Shape>>;
+
+	/// Create a shape according to the settings specified by this object.
+	virtual ShapeResult				Create() const = 0;
+
+	/// When creating a shape, the result is cached so that calling Create() again will return the same shape.
+	/// If you make changes to the ShapeSettings you need to call this function to clear the cached result to allow Create() to build a new shape.
+	void							ClearCachedResult()													{ mCachedResult.Clear(); }
+
+	/// User data (to be used freely by the application)
+	uint64							mUserData = 0;
+
+protected:
+	mutable ShapeResult				mCachedResult;
+};
+
+/// Function table for functions on shapes
+class JPH_EXPORT ShapeFunctions
+{
+public:
+	/// Construct a shape
+	Shape *							(*mConstruct)() = nullptr;
+
+	/// Color of the shape when drawing
+	Color							mColor = Color::sBlack;
+
+	/// Get an entry in the registry for a particular sub type
+	static inline ShapeFunctions &	sGet(EShapeSubType inSubType)										{ return sRegistry[int(inSubType)]; }
+
+private:
+	static ShapeFunctions			sRegistry[NumSubShapeTypes];
+};
+
+/// Base class for all shapes (collision volume of a body). Defines a virtual interface for collision detection.
+class JPH_EXPORT Shape : public RefTarget<Shape>, public NonCopyable
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	using ShapeResult = ShapeSettings::ShapeResult;
+
+	/// Constructor
+									Shape(EShapeType inType, EShapeSubType inSubType) : mShapeType(inType), mShapeSubType(inSubType) { }
+									Shape(EShapeType inType, EShapeSubType inSubType, const ShapeSettings &inSettings, [[maybe_unused]] ShapeResult &outResult) : mUserData(inSettings.mUserData), mShapeType(inType), mShapeSubType(inSubType) { }
+
+	/// Destructor
+	virtual							~Shape() = default;
+
+	/// Get type
+	inline EShapeType				GetType() const														{ return mShapeType; }
+	inline EShapeSubType			GetSubType() const													{ return mShapeSubType; }
+
+	/// User data (to be used freely by the application)
+	uint64							GetUserData() const													{ return mUserData; }
+	void							SetUserData(uint64 inUserData)										{ mUserData = inUserData; }
+
+	/// Check if this shape can only be used to create a static body or if it can also be dynamic/kinematic
+	virtual bool					MustBeStatic() const												{ return false; }
+
+	/// All shapes are centered around their center of mass. This function returns the center of mass position that needs to be applied to transform the shape to where it was created.
+	virtual Vec3					GetCenterOfMass() const												{ return Vec3::sZero(); }
+
+	/// Get local bounding box including convex radius, this box is centered around the center of mass rather than the world transform
+	virtual AABox					GetLocalBounds() const = 0;
+
+	/// Get the max number of sub shape ID bits that are needed to be able to address any leaf shape in this shape. Used mainly for checking that it is smaller or equal than SubShapeID::MaxBits.
+	virtual uint					GetSubShapeIDBitsRecursive() const = 0;
+
+	/// Get world space bounds including convex radius.
+	/// This shape is scaled by inScale in local space first.
+	/// This function can be overridden to return a closer fitting world space bounding box, by default it will just transform what GetLocalBounds() returns.
+	virtual AABox					GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const { return GetLocalBounds().Scaled(inScale).Transformed(inCenterOfMassTransform); }
+
+	/// Get world space bounds including convex radius.
+	AABox							GetWorldSpaceBounds(DMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+	{
+		// Use single precision version using the rotation only
+		AABox bounds = GetWorldSpaceBounds(inCenterOfMassTransform.GetRotation(), inScale);
+
+		// Apply translation
+		bounds.Translate(inCenterOfMassTransform.GetTranslation());
+
+		return bounds;
+	}
+
+	/// Returns the radius of the biggest sphere that fits entirely in the shape. In case this shape consists of multiple sub shapes, it returns the smallest sphere of the parts.
+	/// This can be used as a measure of how far the shape can be moved without risking going through geometry.
+	virtual float					GetInnerRadius() const = 0;
+
+	/// Calculate the mass and inertia of this shape
+	virtual MassProperties			GetMassProperties() const = 0;
+
+	/// Get the leaf shape for a particular sub shape ID.
+	/// @param inSubShapeID The full sub shape ID that indicates the path to the leaf shape
+	/// @param outRemainder What remains of the sub shape ID after removing the path to the leaf shape (could e.g. refer to a triangle within a MeshShape)
+	/// @return The shape or null if the sub shape ID is invalid
+	virtual const Shape *			GetLeafShape([[maybe_unused]] const SubShapeID &inSubShapeID, SubShapeID &outRemainder) const;
+
+	/// Get the material assigned to a particular sub shape ID
+	virtual const PhysicsMaterial *	GetMaterial(const SubShapeID &inSubShapeID) const = 0;
+
+	/// Get the surface normal of a particular sub shape ID and point on surface (all vectors are relative to center of mass for this shape).
+	/// Note: When you have a CollideShapeResult or ShapeCastResult you should use -mPenetrationAxis.Normalized() as contact normal as GetSurfaceNormal will only return face normals (and not vertex or edge normals).
+	virtual Vec3					GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const = 0;
+
+	/// Type definition for a supporting face
+	using SupportingFace = StaticArray<Vec3, 32>;
+
+	/// Get the vertices of the face that faces inDirection the most (includes any convex radius). Note that this function can only return faces of
+	/// convex shapes or triangles, which is why a sub shape ID to get to that leaf must be provided.
+	/// @param inSubShapeID Sub shape ID of target shape
+	/// @param inDirection Direction that the face should be facing (in local space to this shape)
+	/// @param inCenterOfMassTransform Transform to transform outVertices with
+	/// @param inScale Scale in local space of the shape (scales relative to its center of mass)
+	/// @param outVertices Resulting face. The returned face can be empty if the shape doesn't have polygons to return (e.g. because it's a sphere). The face will be returned in world space.
+	virtual void					GetSupportingFace([[maybe_unused]] const SubShapeID &inSubShapeID, [[maybe_unused]] Vec3Arg inDirection, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] Mat44Arg inCenterOfMassTransform, [[maybe_unused]] SupportingFace &outVertices) const { /* Nothing */ }
+
+	/// Get the user data of a particular sub shape ID. Corresponds with the value stored in Shape::GetUserData of the leaf shape pointed to by inSubShapeID.
+	virtual uint64					GetSubShapeUserData([[maybe_unused]] const SubShapeID &inSubShapeID) const			{ return mUserData; }
+
+	/// Get the direct child sub shape and its transform for a sub shape ID.
+	/// @param inSubShapeID Sub shape ID that indicates the path to the leaf shape
+	/// @param inPositionCOM The position of the center of mass of this shape
+	/// @param inRotation The orientation of this shape
+	/// @param inScale Scale in local space of the shape (scales relative to its center of mass)
+	/// @param outRemainder The remainder of the sub shape ID after removing the sub shape
+	/// @return Direct child sub shape and its transform, note that the body ID and sub shape ID will be invalid
+	virtual TransformedShape		GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const;
+
+	/// Gets the properties needed to do buoyancy calculations for a body using this shape
+	/// @param inCenterOfMassTransform Transform that takes this shape (centered around center of mass) to world space (or a desired other space)
+	/// @param inScale Scale in local space of the shape (scales relative to its center of mass)
+	/// @param inSurface The surface plane of the liquid relative to inCenterOfMassTransform
+	/// @param outTotalVolume On return this contains the total volume of the shape
+	/// @param outSubmergedVolume On return this contains the submerged volume of the shape
+	/// @param outCenterOfBuoyancy On return this contains the world space center of mass of the submerged volume
+#ifdef JPH_DEBUG_RENDERER
+	/// @param inBaseOffset The offset to transform inCenterOfMassTransform to world space (in double precision mode this can be used to shift the whole operation closer to the origin). Only used for debug drawing.
+#endif
+	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy
+#ifdef JPH_DEBUG_RENDERER // Not using JPH_IF_DEBUG_RENDERER for Doxygen
+		, RVec3Arg inBaseOffset
+#endif
+		) const = 0;
+
+#ifdef JPH_DEBUG_RENDERER
+	/// Draw the shape at a particular location with a particular color (debugging purposes)
+	virtual void					Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const = 0;
+
+	/// Draw the results of the GetSupportFunction with the convex radius added back on to show any errors introduced by this process (only relevant for convex shapes)
+	virtual void					DrawGetSupportFunction([[maybe_unused]] DebugRenderer *inRenderer, [[maybe_unused]] RMat44Arg inCenterOfMassTransform, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] ColorArg inColor, [[maybe_unused]] bool inDrawSupportDirection) const { /* Only implemented for convex shapes */ }
+
+	/// Draw the results of the GetSupportingFace function to show any errors introduced by this process (only relevant for convex shapes)
+	virtual void					DrawGetSupportingFace([[maybe_unused]] DebugRenderer *inRenderer, [[maybe_unused]] RMat44Arg inCenterOfMassTransform, [[maybe_unused]] Vec3Arg inScale) const { /* Only implemented for convex shapes */ }
+#endif // JPH_DEBUG_RENDERER
+
+	/// Cast a ray against this shape, returns true if it finds a hit closer than ioHit.mFraction and updates that fraction. Otherwise ioHit is left untouched and the function returns false.
+	/// Note that the ray should be relative to the center of mass of this shape (i.e. subtract Shape::GetCenterOfMass() from RayCast::mOrigin if you want to cast against the shape in the space it was created).
+	/// Convex objects will be treated as solid (meaning if the ray starts inside, you'll get a hit fraction of 0) and back face hits against triangles are returned.
+	/// If you want the surface normal of the hit use GetSurfaceNormal(ioHit.mSubShapeID2, inRay.GetPointOnRay(ioHit.mFraction)).
+	virtual bool					CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const = 0;
+
+	/// Cast a ray against this shape. Allows returning multiple hits through ioCollector. Note that this version is more flexible but also slightly slower than the CastRay function that returns only a single hit.
+	/// If you want the surface normal of the hit use GetSurfaceNormal(collected sub shape ID, inRay.GetPointOnRay(collected faction)).
+	virtual void					CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const = 0;
+
+	/// Check if inPoint is inside this shape. For this tests all shapes are treated as if they were solid.
+	/// Note that inPoint should be relative to the center of mass of this shape (i.e. subtract Shape::GetCenterOfMass() from inPoint if you want to test against the shape in the space it was created).
+	/// For a mesh shape, this test will only provide sensible information if the mesh is a closed manifold.
+	/// For each shape that collides, ioCollector will receive a hit.
+	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const = 0;
+
+	/// Collides all vertices of a soft body with this shape and updates SoftBodyVertex::mCollisionPlane, SoftBodyVertex::mCollidingShapeIndex and SoftBodyVertex::mLargestPenetration if a collision with more penetration was found.
+	/// @param inCenterOfMassTransform Center of mass transform for this shape relative to the vertices.
+	/// @param inScale Scale in local space of the shape (scales relative to its center of mass)
+	/// @param inVertices The vertices of the soft body
+	/// @param inNumVertices The number of vertices in inVertices
+	/// @param inCollidingShapeIndex Value to store in CollideSoftBodyVertexIterator::mCollidingShapeIndex when a collision was found
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const = 0;
+
+	/// Collect the leaf transformed shapes of all leaf shapes of this shape.
+	/// inBox is the world space axis aligned box which leaf shapes should collide with.
+	/// inPositionCOM/inRotation/inScale describes the transform of this shape.
+	/// inSubShapeIDCeator represents the current sub shape ID of this shape.
+	virtual void					CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const;
+
+	/// Transforms this shape and all of its children with inTransform, resulting shape(s) are passed to ioCollector.
+	/// Note that not all shapes support all transforms (especially true for scaling), the resulting shape will try to match the transform as accurately as possible.
+	/// @param inCenterOfMassTransform The transform (rotation, translation, scale) that the center of mass of the shape should get
+	/// @param ioCollector The transformed shapes will be passed to this collector
+	virtual void					TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const;
+
+	/// Scale this shape. Note that not all shapes support all scales, this will return a shape that matches the scale as accurately as possible. See Shape::IsValidScale for more information.
+	/// @param inScale The scale to use for this shape (note: this scale is applied to the entire shape in the space it was created, most other functions apply the scale in the space of the leaf shapes and from the center of mass!)
+	ShapeResult						ScaleShape(Vec3Arg inScale) const;
+
+	/// An opaque buffer that holds shape specific information during GetTrianglesStart/Next.
+	struct alignas(16)				GetTrianglesContext { uint8 mData[4288]; };
+
+	/// This is the minimum amount of triangles that should be requested through GetTrianglesNext.
+	static constexpr int			cGetTrianglesMinTrianglesRequested = 32;
+
+	/// To start iterating over triangles, call this function first.
+	/// ioContext is a temporary buffer and should remain untouched until the last call to GetTrianglesNext.
+	/// inBox is the world space bounding in which you want to get the triangles.
+	/// inPositionCOM/inRotation/inScale describes the transform of this shape.
+	/// To get the actual triangles call GetTrianglesNext.
+	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const = 0;
+
+	/// Call this repeatedly to get all triangles in the box.
+	/// outTriangleVertices should be large enough to hold 3 * inMaxTriangleRequested entries.
+	/// outMaterials (if it is not null) should contain inMaxTrianglesRequested entries.
+	/// The function returns the amount of triangles that it found (which will be <= inMaxTrianglesRequested), or 0 if there are no more triangles.
+	/// Note that the function can return a value < inMaxTrianglesRequested and still have more triangles to process (triangles can be returned in blocks).
+	/// Note that the function may return triangles outside of the requested box, only coarse culling is performed on the returned triangles.
+	virtual int						GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const = 0;
+
+	///@name Binary serialization of the shape. Note that this saves the 'cooked' shape in a format which will not be backwards compatible for newer library versions.
+	/// In this case you need to recreate the shape from the ShapeSettings object and save it again. The user is expected to call SaveBinaryState followed by SaveMaterialState and SaveSubShapeState.
+	/// The stream should be stored as is and the material and shape list should be saved using the applications own serialization system (e.g. by assigning an ID to each pointer).
+	/// When restoring data, call sRestoreFromBinaryState to get the shape and then call RestoreMaterialState and RestoreSubShapeState to restore the pointers to the external objects.
+	/// Alternatively you can use SaveWithChildren and sRestoreWithChildren to save and restore the shape and all its child shapes and materials in a single stream.
+	///@{
+
+	/// Saves the contents of the shape in binary form to inStream.
+	virtual void					SaveBinaryState(StreamOut &inStream) const;
+
+	/// Creates a Shape of the correct type and restores its contents from the binary stream inStream.
+	static ShapeResult				sRestoreFromBinaryState(StreamIn &inStream);
+
+	/// Outputs the material references that this shape has to outMaterials.
+	virtual void					SaveMaterialState([[maybe_unused]] PhysicsMaterialList &outMaterials) const			{ /* By default do nothing */ }
+
+	/// Restore the material references after calling sRestoreFromBinaryState. Note that the exact same materials need to be provided in the same order as returned by SaveMaterialState.
+	virtual void					RestoreMaterialState([[maybe_unused]] const PhysicsMaterialRefC *inMaterials, [[maybe_unused]] uint inNumMaterials) { JPH_ASSERT(inNumMaterials == 0); }
+
+	/// Outputs the shape references that this shape has to outSubShapes.
+	virtual void					SaveSubShapeState([[maybe_unused]] ShapeList &outSubShapes) const					{ /* By default do nothing */ }
+
+	/// Restore the shape references after calling sRestoreFromBinaryState. Note that the exact same shapes need to be provided in the same order as returned by SaveSubShapeState.
+	virtual void					RestoreSubShapeState([[maybe_unused]] const ShapeRefC *inSubShapes, [[maybe_unused]] uint inNumShapes) { JPH_ASSERT(inNumShapes == 0); }
+
+	using ShapeToIDMap = StreamUtils::ObjectToIDMap<Shape>;
+	using IDToShapeMap = StreamUtils::IDToObjectMap<Shape>;
+	using MaterialToIDMap = StreamUtils::ObjectToIDMap<PhysicsMaterial>;
+	using IDToMaterialMap = StreamUtils::IDToObjectMap<PhysicsMaterial>;
+
+	/// Save this shape, all its children and its materials. Pass in an empty map in ioShapeMap / ioMaterialMap or reuse the same map while saving multiple shapes to the same stream in order to avoid writing duplicates.
+	void							SaveWithChildren(StreamOut &inStream, ShapeToIDMap &ioShapeMap, MaterialToIDMap &ioMaterialMap) const;
+
+	/// Restore a shape, all its children and materials. Pass in an empty map in ioShapeMap / ioMaterialMap or reuse the same map while reading multiple shapes from the same stream in order to restore duplicates.
+	static ShapeResult				sRestoreWithChildren(StreamIn &inStream, IDToShapeMap &ioShapeMap, IDToMaterialMap &ioMaterialMap);
+
+	///@}
+
+	/// Class that holds information about the shape that can be used for logging / data collection purposes
+	struct Stats
+	{
+									Stats(size_t inSizeBytes, uint inNumTriangles) : mSizeBytes(inSizeBytes), mNumTriangles(inNumTriangles) { }
+
+		size_t						mSizeBytes;				///< Amount of memory used by this shape (size in bytes)
+		uint						mNumTriangles;			///< Number of triangles in this shape (when applicable)
+	};
+
+	/// Get stats of this shape. Use for logging / data collection purposes only. Does not add values from child shapes, use GetStatsRecursive for this.
+	virtual Stats					GetStats() const = 0;
+
+	using VisitedShapes = UnorderedSet<const Shape *>;
+
+	/// Get the combined stats of this shape and its children.
+	/// @param ioVisitedShapes is used to track which shapes have already been visited, to avoid calculating the wrong memory size.
+	virtual Stats					GetStatsRecursive(VisitedShapes &ioVisitedShapes) const;
+
+	///< Volume of this shape (m^3). Note that for compound shapes the volume may be incorrect since child shapes can overlap which is not accounted for.
+	virtual float					GetVolume() const = 0;
+
+	/// Test if inScale is a valid scale for this shape. Some shapes can only be scaled uniformly, compound shapes cannot handle shapes
+	/// being rotated and scaled (this would cause shearing), scale can never be zero. When the scale is invalid, the function will return false.
+	///
+	/// Here's a list of supported scales:
+	/// * SphereShape: Scale must be uniform (signs of scale are ignored).
+	/// * BoxShape: Any scale supported (signs of scale are ignored).
+	/// * TriangleShape: Any scale supported when convex radius is zero, otherwise only uniform scale supported.
+	/// * CapsuleShape: Scale must be uniform (signs of scale are ignored).
+	/// * TaperedCapsuleShape: Scale must be uniform (sign of Y scale can be used to flip the capsule).
+	/// * CylinderShape: Scale must be uniform in XZ plane, Y can scale independently (signs of scale are ignored).
+	/// * RotatedTranslatedShape: Scale must not cause shear in the child shape.
+	/// * CompoundShape: Scale must not cause shear in any of the child shapes.
+	virtual bool					IsValidScale(Vec3Arg inScale) const;
+
+	/// This function will make sure that if you wrap this shape in a ScaledShape that the scale is valid.
+	/// Note that this involves discarding components of the scale that are invalid, so the resulting scaled shape may be different than the requested scale.
+	/// Compare the return value of this function with the scale you passed in to detect major inconsistencies and possibly warn the user.
+	/// @param inScale Local space scale for this shape.
+	/// @return Scale that can be used to wrap this shape in a ScaledShape. IsValidScale will return true for this scale.
+	virtual Vec3					MakeScaleValid(Vec3Arg inScale) const;
+
+#ifdef JPH_DEBUG_RENDERER
+	/// Debug helper which draws the intersection between water and the shapes, the center of buoyancy and the submerged volume
+	static bool						sDrawSubmergedVolumes;
+#endif // JPH_DEBUG_RENDERER
+
+protected:
+	/// This function should not be called directly, it is used by sRestoreFromBinaryState.
+	virtual void					RestoreBinaryState(StreamIn &inStream);
+
+	/// A fallback version of CollidePoint that uses a ray cast and counts the number of hits to determine if the point is inside the shape. Odd number of hits means inside, even number of hits means outside.
+	static void						sCollidePointUsingRayCast(const Shape &inShape, Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter);
+
+private:
+	uint64							mUserData = 0;
+	EShapeType						mShapeType;
+	EShapeSubType					mShapeSubType;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SphereShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SphereShape.cpp
new file mode 100644
index 0000000000..b44dd77a1e
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SphereShape.cpp
@@ -0,0 +1,347 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/Shape/GetTrianglesContext.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
+#include <Jolt/Geometry/RaySphere.h>
+#include <Jolt/Geometry/Plane.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SphereShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(SphereShapeSettings, ConvexShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(SphereShapeSettings, mRadius)
+}
+
+ShapeSettings::ShapeResult SphereShapeSettings::Create() const
+{
+	if (mCachedResult.IsEmpty())
+		Ref<Shape> shape = new SphereShape(*this, mCachedResult);
+	return mCachedResult;
+}
+
+SphereShape::SphereShape(const SphereShapeSettings &inSettings, ShapeResult &outResult) :
+	ConvexShape(EShapeSubType::Sphere, inSettings, outResult),
+	mRadius(inSettings.mRadius)
+{
+	if (inSettings.mRadius <= 0.0f)
+	{
+		outResult.SetError("Invalid radius");
+		return;
+	}
+
+	outResult.Set(this);
+}
+
+float SphereShape::GetScaledRadius(Vec3Arg inScale) const
+{
+	JPH_ASSERT(IsValidScale(inScale));
+
+	Vec3 abs_scale = inScale.Abs();
+	return abs_scale.GetX() * mRadius;
+}
+
+AABox SphereShape::GetLocalBounds() const
+{
+	Vec3 half_extent = Vec3::sReplicate(mRadius);
+	return AABox(-half_extent, half_extent);
+}
+
+AABox SphereShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
+	float scaled_radius = GetScaledRadius(inScale);
+	Vec3 half_extent = Vec3::sReplicate(scaled_radius);
+	AABox bounds(-half_extent, half_extent);
+	bounds.Translate(inCenterOfMassTransform.GetTranslation());
+	return bounds;
+}
+
+class SphereShape::SphereNoConvex final : public Support
+{
+public:
+	explicit		SphereNoConvex(float inRadius) :
+		mRadius(inRadius)
+	{
+		static_assert(sizeof(SphereNoConvex) <= sizeof(SupportBuffer), "Buffer size too small");
+		JPH_ASSERT(IsAligned(this, alignof(SphereNoConvex)));
+	}
+
+	virtual Vec3	GetSupport(Vec3Arg inDirection) const override
+	{
+		return Vec3::sZero();
+	}
+
+	virtual float	GetConvexRadius() const override
+	{
+		return mRadius;
+	}
+
+private:
+	float			mRadius;
+};
+
+class SphereShape::SphereWithConvex final : public Support
+{
+public:
+	explicit		SphereWithConvex(float inRadius) :
+		mRadius(inRadius)
+	{
+		static_assert(sizeof(SphereWithConvex) <= sizeof(SupportBuffer), "Buffer size too small");
+		JPH_ASSERT(IsAligned(this, alignof(SphereWithConvex)));
+	}
+
+	virtual Vec3	GetSupport(Vec3Arg inDirection) const override
+	{
+		float len = inDirection.Length();
+		return len > 0.0f? (mRadius / len) * inDirection : Vec3::sZero();
+	}
+
+	virtual float	GetConvexRadius() const override
+	{
+		return 0.0f;
+	}
+
+private:
+	float			mRadius;
+};
+
+const ConvexShape::Support *SphereShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const
+{
+	float scaled_radius = GetScaledRadius(inScale);
+
+	switch (inMode)
+	{
+	case ESupportMode::IncludeConvexRadius:
+		return new (&inBuffer) SphereWithConvex(scaled_radius);
+
+	case ESupportMode::ExcludeConvexRadius:
+	case ESupportMode::Default:
+		return new (&inBuffer) SphereNoConvex(scaled_radius);
+	}
+
+	JPH_ASSERT(false);
+	return nullptr;
+}
+
+MassProperties SphereShape::GetMassProperties() const
+{
+	MassProperties p;
+
+	// Calculate mass
+	float r2 = mRadius * mRadius;
+	p.mMass = (4.0f / 3.0f * JPH_PI) * mRadius * r2 * GetDensity();
+
+	// Calculate inertia
+	float inertia = (2.0f / 5.0f) * p.mMass * r2;
+	p.mInertia = Mat44::sScale(inertia);
+
+	return p;
+}
+
+Vec3 SphereShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
+
+	float len = inLocalSurfacePosition.Length();
+	return len != 0.0f? inLocalSurfacePosition / len : Vec3::sAxisY();
+}
+
+void SphereShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const
+{
+	float scaled_radius = GetScaledRadius(inScale);
+	outTotalVolume = (4.0f / 3.0f * JPH_PI) * Cubed(scaled_radius);
+
+	float distance_to_surface = inSurface.SignedDistance(inCenterOfMassTransform.GetTranslation());
+	if (distance_to_surface >= scaled_radius)
+	{
+		// Above surface
+		outSubmergedVolume = 0.0f;
+		outCenterOfBuoyancy = Vec3::sZero();
+	}
+	else if (distance_to_surface <= -scaled_radius)
+	{
+		// Under surface
+		outSubmergedVolume = outTotalVolume;
+		outCenterOfBuoyancy = inCenterOfMassTransform.GetTranslation();
+	}
+	else
+	{
+		// Intersecting surface
+
+		// Calculate submerged volume, see: https://en.wikipedia.org/wiki/Spherical_cap
+		float h = scaled_radius - distance_to_surface;
+		outSubmergedVolume = (JPH_PI / 3.0f) * Square(h) * (3.0f * scaled_radius - h);
+
+		// Calculate center of buoyancy, see: http://mathworld.wolfram.com/SphericalCap.html (eq 10)
+		float z = (3.0f / 4.0f) * Square(2.0f * scaled_radius - h) / (3.0f * scaled_radius - h);
+		outCenterOfBuoyancy = inCenterOfMassTransform.GetTranslation() - z * inSurface.GetNormal(); // Negative normal since we want the portion under the water
+
+	#ifdef JPH_DEBUG_RENDERER
+		// Draw intersection between sphere and water plane
+		if (sDrawSubmergedVolumes)
+		{
+			Vec3 circle_center = inCenterOfMassTransform.GetTranslation() - distance_to_surface * inSurface.GetNormal();
+			float circle_radius = sqrt(Square(scaled_radius) - Square(distance_to_surface));
+			DebugRenderer::sInstance->DrawPie(inBaseOffset + circle_center, circle_radius, inSurface.GetNormal(), inSurface.GetNormal().GetNormalizedPerpendicular(), -JPH_PI, JPH_PI, Color::sGreen, DebugRenderer::ECastShadow::Off);
+		}
+	#endif // JPH_DEBUG_RENDERER
+	}
+
+#ifdef JPH_DEBUG_RENDERER
+	// Draw center of buoyancy
+	if (sDrawSubmergedVolumes)
+		DebugRenderer::sInstance->DrawWireSphere(inBaseOffset + outCenterOfBuoyancy, 0.05f, Color::sRed, 1);
+#endif // JPH_DEBUG_RENDERER
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void SphereShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid;
+	inRenderer->DrawUnitSphere(inCenterOfMassTransform * Mat44::sScale(mRadius * inScale.Abs().GetX()), inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor, DebugRenderer::ECastShadow::On, draw_mode);
+}
+#endif // JPH_DEBUG_RENDERER
+
+bool SphereShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	float fraction = RaySphere(inRay.mOrigin, inRay.mDirection, Vec3::sZero(), mRadius);
+	if (fraction < ioHit.mFraction)
+	{
+		ioHit.mFraction = fraction;
+		ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID();
+		return true;
+	}
+	return false;
+}
+
+void SphereShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	float min_fraction, max_fraction;
+	int num_results = RaySphere(inRay.mOrigin, inRay.mDirection, Vec3::sZero(), mRadius, min_fraction, max_fraction);
+	if (num_results > 0 // Ray should intersect
+		&& max_fraction >= 0.0f // End of ray should be inside sphere
+		&& min_fraction < ioCollector.GetEarlyOutFraction()) // Start of ray should be before early out fraction
+	{
+		// Better hit than the current hit
+		RayCastResult hit;
+		hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext());
+		hit.mSubShapeID2 = inSubShapeIDCreator.GetID();
+
+		// Check front side hit
+		if (inRayCastSettings.mTreatConvexAsSolid || min_fraction > 0.0f)
+		{
+			hit.mFraction = max(0.0f, min_fraction);
+			ioCollector.AddHit(hit);
+		}
+
+		// Check back side hit
+		if (inRayCastSettings.mBackFaceModeConvex == EBackFaceMode::CollideWithBackFaces
+			&& num_results > 1 // Ray should have 2 intersections
+			&& max_fraction < ioCollector.GetEarlyOutFraction()) // End of ray should be before early out fraction
+		{
+			hit.mFraction = max_fraction;
+			ioCollector.AddHit(hit);
+		}
+	}
+}
+
+void SphereShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	if (inPoint.LengthSq() <= Square(mRadius))
+		ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
+}
+
+void SphereShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
+{
+	Vec3 center = inCenterOfMassTransform.GetTranslation();
+	float radius = GetScaledRadius(inScale);
+
+	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
+		if (v.GetInvMass() > 0.0f)
+		{
+			// Calculate penetration
+			Vec3 delta = v.GetPosition() - center;
+			float distance = delta.Length();
+			float penetration = radius - distance;
+			if (v.UpdatePenetration(penetration))
+			{
+				// Calculate contact point and normal
+				Vec3 normal = distance > 0.0f? delta / distance : Vec3::sAxisY();
+				Vec3 point = center + radius * normal;
+
+				// Store collision
+				v.SetCollision(Plane::sFromPointAndNormal(point, normal), inCollidingShapeIndex);
+			}
+		}
+}
+
+void SphereShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const
+{
+	float scaled_radius = GetScaledRadius(inScale);
+	new (&ioContext) GetTrianglesContextVertexList(inPositionCOM, inRotation, Vec3::sReplicate(1.0f), Mat44::sScale(scaled_radius), sUnitSphereTriangles.data(), sUnitSphereTriangles.size(), GetMaterial());
+}
+
+int SphereShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const
+{
+	return ((GetTrianglesContextVertexList &)ioContext).GetTrianglesNext(inMaxTrianglesRequested, outTriangleVertices, outMaterials);
+}
+
+void SphereShape::SaveBinaryState(StreamOut &inStream) const
+{
+	ConvexShape::SaveBinaryState(inStream);
+
+	inStream.Write(mRadius);
+}
+
+void SphereShape::RestoreBinaryState(StreamIn &inStream)
+{
+	ConvexShape::RestoreBinaryState(inStream);
+
+	inStream.Read(mRadius);
+}
+
+bool SphereShape::IsValidScale(Vec3Arg inScale) const
+{
+	return ConvexShape::IsValidScale(inScale) && ScaleHelpers::IsUniformScale(inScale.Abs());
+}
+
+Vec3 SphereShape::MakeScaleValid(Vec3Arg inScale) const
+{
+	Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale);
+
+	return scale.GetSign() * ScaleHelpers::MakeUniformScale(scale.Abs());
+}
+
+void SphereShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Sphere);
+	f.mConstruct = []() -> Shape * { return new SphereShape; };
+	f.mColor = Color::sGreen;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SphereShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SphereShape.h
new file mode 100644
index 0000000000..c305523396
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SphereShape.h
@@ -0,0 +1,125 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/ConvexShape.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that constructs a SphereShape
+class JPH_EXPORT SphereShapeSettings final : public ConvexShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, SphereShapeSettings)
+
+public:
+	/// Default constructor for deserialization
+							SphereShapeSettings() = default;
+
+	/// Create a sphere with radius inRadius
+							SphereShapeSettings(float inRadius, const PhysicsMaterial *inMaterial = nullptr)	: ConvexShapeSettings(inMaterial), mRadius(inRadius) { }
+
+	// See: ShapeSettings
+	virtual ShapeResult		Create() const override;
+
+	float					mRadius = 0.0f;
+};
+
+/// A sphere, centered around the origin.
+/// Note that it is implemented as a point with convex radius.
+class JPH_EXPORT SphereShape final : public ConvexShape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+							SphereShape() : ConvexShape(EShapeSubType::Sphere) { }
+							SphereShape(const SphereShapeSettings &inSettings, ShapeResult &outResult);
+
+	/// Create a sphere with radius inRadius
+							SphereShape(float inRadius, const PhysicsMaterial *inMaterial = nullptr)			: ConvexShape(EShapeSubType::Sphere, inMaterial), mRadius(inRadius) { JPH_ASSERT(inRadius > 0.0f); }
+
+	/// Radius of the sphere
+	float					GetRadius() const																	{ return mRadius; }
+
+	// See Shape::GetLocalBounds
+	virtual AABox			GetLocalBounds() const override;
+
+	// See Shape::GetWorldSpaceBounds
+	virtual AABox			GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+	using Shape::GetWorldSpaceBounds;
+
+	// See Shape::GetInnerRadius
+	virtual float			GetInnerRadius() const override														{ return mRadius; }
+
+	// See Shape::GetMassProperties
+	virtual MassProperties	GetMassProperties() const override;
+
+	// See Shape::GetSurfaceNormal
+	virtual Vec3			GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override;
+
+	// See Shape::GetSupportingFace
+	virtual void			GetSupportingFace([[maybe_unused]] const SubShapeID &inSubShapeID, [[maybe_unused]] Vec3Arg inDirection, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] Mat44Arg inCenterOfMassTransform, [[maybe_unused]] SupportingFace &outVertices) const override { /* Hit is always a single point, no point in returning anything */ }
+
+	// See ConvexShape::GetSupportFunction
+	virtual const Support *	GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override;
+
+	// See Shape::GetSubmergedVolume
+	virtual void			GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override;
+
+#ifdef JPH_DEBUG_RENDERER
+	// See Shape::Draw
+	virtual void			Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// See Shape::CastRay
+	virtual bool			CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+	virtual void			CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollidePoint
+	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollideSoftBodyVertices
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
+
+	// See Shape::GetTrianglesStart
+	virtual void			GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
+
+	// See Shape::GetTrianglesNext
+	virtual int				GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override;
+
+	// See Shape
+	virtual void			SaveBinaryState(StreamOut &inStream) const override;
+
+	// See Shape::GetStats
+	virtual Stats			GetStats() const override															{ return Stats(sizeof(*this), 0); }
+
+	// See Shape::GetVolume
+	virtual float			GetVolume() const override															{ return 4.0f / 3.0f * JPH_PI * Cubed(mRadius); }
+
+	// See Shape::IsValidScale
+	virtual bool			IsValidScale(Vec3Arg inScale) const override;
+
+	// See Shape::MakeScaleValid
+	virtual Vec3			MakeScaleValid(Vec3Arg inScale) const override;
+
+	// Register shape functions with the registry
+	static void				sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void			RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	// Get the radius of this sphere scaled by inScale
+	inline float			GetScaledRadius(Vec3Arg inScale) const;
+
+	// Classes for GetSupportFunction
+	class					SphereNoConvex;
+	class					SphereWithConvex;
+
+	float					mRadius = 0.0f;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/StaticCompoundShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/StaticCompoundShape.cpp
new file mode 100644
index 0000000000..eb9ec06b9e
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/StaticCompoundShape.cpp
@@ -0,0 +1,674 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
+#include <Jolt/Physics/Collision/Shape/CompoundShapeVisitors.h>
+#include <Jolt/Core/Profiler.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/Core/TempAllocator.h>
+#include <Jolt/Core/ScopeExit.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(StaticCompoundShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(StaticCompoundShapeSettings, CompoundShapeSettings)
+}
+
+ShapeSettings::ShapeResult StaticCompoundShapeSettings::Create(TempAllocator &inTempAllocator) const
+{
+	if (mCachedResult.IsEmpty())
+	{
+		if (mSubShapes.size() == 0)
+		{
+			// It's an error to create a compound with no subshapes (the compound cannot encode this)
+			mCachedResult.SetError("Compound needs a sub shape!");
+		}
+		else if (mSubShapes.size() == 1)
+		{
+			// If there's only 1 part we don't need a StaticCompoundShape
+			const SubShapeSettings &s = mSubShapes[0];
+			if (s.mPosition == Vec3::sZero()
+				&& s.mRotation == Quat::sIdentity())
+			{
+				// No rotation or translation, we can use the shape directly
+				if (s.mShapePtr != nullptr)
+					mCachedResult.Set(const_cast<Shape *>(s.mShapePtr.GetPtr()));
+				else if (s.mShape != nullptr)
+					mCachedResult = s.mShape->Create();
+				else
+					mCachedResult.SetError("Sub shape is null!");
+			}
+			else
+			{
+				// We can use a RotatedTranslatedShape instead
+				RotatedTranslatedShapeSettings settings;
+				settings.mPosition = s.mPosition;
+				settings.mRotation = s.mRotation;
+				settings.mInnerShape = s.mShape;
+				settings.mInnerShapePtr = s.mShapePtr;
+				Ref<Shape> shape = new RotatedTranslatedShape(settings, mCachedResult);
+			}
+		}
+		else
+		{
+			// Build a regular compound shape
+			Ref<Shape> shape = new StaticCompoundShape(*this, inTempAllocator, mCachedResult);
+		}
+	}
+	return mCachedResult;
+}
+
+ShapeSettings::ShapeResult StaticCompoundShapeSettings::Create() const
+{
+	TempAllocatorMalloc allocator;
+	return Create(allocator);
+}
+
+void StaticCompoundShape::Node::SetChildInvalid(uint inIndex)
+{
+	// Make this an invalid node
+	mNodeProperties[inIndex] = INVALID_NODE;
+
+	// Make bounding box invalid
+	mBoundsMinX[inIndex] = HALF_FLT_MAX;
+	mBoundsMinY[inIndex] = HALF_FLT_MAX;
+	mBoundsMinZ[inIndex] = HALF_FLT_MAX;
+	mBoundsMaxX[inIndex] = HALF_FLT_MAX;
+	mBoundsMaxY[inIndex] = HALF_FLT_MAX;
+	mBoundsMaxZ[inIndex] = HALF_FLT_MAX;
+}
+
+void StaticCompoundShape::Node::SetChildBounds(uint inIndex, const AABox &inBounds)
+{
+	mBoundsMinX[inIndex] = HalfFloatConversion::FromFloat<HalfFloatConversion::ROUND_TO_NEG_INF>(inBounds.mMin.GetX());
+	mBoundsMinY[inIndex] = HalfFloatConversion::FromFloat<HalfFloatConversion::ROUND_TO_NEG_INF>(inBounds.mMin.GetY());
+	mBoundsMinZ[inIndex] = HalfFloatConversion::FromFloat<HalfFloatConversion::ROUND_TO_NEG_INF>(inBounds.mMin.GetZ());
+	mBoundsMaxX[inIndex] = HalfFloatConversion::FromFloat<HalfFloatConversion::ROUND_TO_POS_INF>(inBounds.mMax.GetX());
+	mBoundsMaxY[inIndex] = HalfFloatConversion::FromFloat<HalfFloatConversion::ROUND_TO_POS_INF>(inBounds.mMax.GetY());
+	mBoundsMaxZ[inIndex] = HalfFloatConversion::FromFloat<HalfFloatConversion::ROUND_TO_POS_INF>(inBounds.mMax.GetZ());
+}
+
+void StaticCompoundShape::sPartition(uint *ioBodyIdx, AABox *ioBounds, int inNumber, int &outMidPoint)
+{
+	// Handle trivial case
+	if (inNumber <= 4)
+	{
+		outMidPoint = inNumber / 2;
+		return;
+	}
+
+	// Calculate bounding box of box centers
+	Vec3 center_min = Vec3::sReplicate(FLT_MAX);
+	Vec3 center_max = Vec3::sReplicate(-FLT_MAX);
+	for (const AABox *b = ioBounds, *b_end = ioBounds + inNumber; b < b_end; ++b)
+	{
+		Vec3 center = b->GetCenter();
+		center_min = Vec3::sMin(center_min, center);
+		center_max = Vec3::sMax(center_max, center);
+	}
+
+	// Calculate split plane
+	int dimension = (center_max - center_min).GetHighestComponentIndex();
+	float split = 0.5f * (center_min + center_max)[dimension];
+
+	// Divide bodies
+	int start = 0, end = inNumber;
+	while (start < end)
+	{
+		// Search for first element that is on the right hand side of the split plane
+		while (start < end && ioBounds[start].GetCenter()[dimension] < split)
+			++start;
+
+		// Search for the first element that is on the left hand side of the split plane
+		while (start < end && ioBounds[end - 1].GetCenter()[dimension] >= split)
+			--end;
+
+		if (start < end)
+		{
+			// Swap the two elements
+			std::swap(ioBodyIdx[start], ioBodyIdx[end - 1]);
+			std::swap(ioBounds[start], ioBounds[end - 1]);
+			++start;
+			--end;
+		}
+	}
+	JPH_ASSERT(start == end);
+
+	if (start > 0 && start < inNumber)
+	{
+		// Success!
+		outMidPoint = start;
+	}
+	else
+	{
+		// Failed to divide bodies
+		outMidPoint = inNumber / 2;
+	}
+}
+
+void StaticCompoundShape::sPartition4(uint *ioBodyIdx, AABox *ioBounds, int inBegin, int inEnd, int *outSplit)
+{
+	uint *body_idx = ioBodyIdx + inBegin;
+	AABox *node_bounds = ioBounds + inBegin;
+	int number = inEnd - inBegin;
+
+	// Partition entire range
+	sPartition(body_idx, node_bounds, number, outSplit[2]);
+
+	// Partition lower half
+	sPartition(body_idx, node_bounds, outSplit[2], outSplit[1]);
+
+	// Partition upper half
+	sPartition(body_idx + outSplit[2], node_bounds + outSplit[2], number - outSplit[2], outSplit[3]);
+
+	// Convert to proper range
+	outSplit[0] = inBegin;
+	outSplit[1] += inBegin;
+	outSplit[2] += inBegin;
+	outSplit[3] += outSplit[2];
+	outSplit[4] = inEnd;
+}
+
+StaticCompoundShape::StaticCompoundShape(const StaticCompoundShapeSettings &inSettings, TempAllocator &inTempAllocator, ShapeResult &outResult) :
+	CompoundShape(EShapeSubType::StaticCompound, inSettings, outResult)
+{
+	// Check that there's at least 1 shape
+	uint num_subshapes = (uint)inSettings.mSubShapes.size();
+	if (num_subshapes < 2)
+	{
+		outResult.SetError("Compound needs at least 2 sub shapes, otherwise you should use a RotatedTranslatedShape!");
+		return;
+	}
+
+	// Keep track of total mass to calculate center of mass
+	float mass = 0.0f;
+
+	mSubShapes.resize(num_subshapes);
+	for (uint i = 0; i < num_subshapes; ++i)
+	{
+		const CompoundShapeSettings::SubShapeSettings &shape = inSettings.mSubShapes[i];
+
+		// Start constructing the runtime sub shape
+		SubShape &out_shape = mSubShapes[i];
+		if (!out_shape.FromSettings(shape, outResult))
+			return;
+
+		// Calculate mass properties of child
+		MassProperties child = out_shape.mShape->GetMassProperties();
+
+		// Accumulate center of mass
+		mass += child.mMass;
+		mCenterOfMass += out_shape.GetPositionCOM() * child.mMass;
+	}
+
+	if (mass > 0.0f)
+		mCenterOfMass /= mass;
+
+	// Cache the inner radius as it can take a while to recursively iterate over all sub shapes
+	CalculateInnerRadius();
+
+	// Temporary storage for the bounding boxes of all shapes
+	uint bounds_size = num_subshapes * sizeof(AABox);
+	AABox *bounds = (AABox *)inTempAllocator.Allocate(bounds_size);
+	JPH_SCOPE_EXIT([&inTempAllocator, bounds, bounds_size]{ inTempAllocator.Free(bounds, bounds_size); });
+
+	// Temporary storage for body indexes (we're shuffling them)
+	uint body_idx_size = num_subshapes * sizeof(uint);
+	uint *body_idx = (uint *)inTempAllocator.Allocate(body_idx_size);
+	JPH_SCOPE_EXIT([&inTempAllocator, body_idx, body_idx_size]{ inTempAllocator.Free(body_idx, body_idx_size); });
+
+	// Shift all shapes so that the center of mass is now at the origin and calculate bounds
+	for (uint i = 0; i < num_subshapes; ++i)
+	{
+		SubShape &shape = mSubShapes[i];
+
+		// Shift the shape so it's centered around our center of mass
+		shape.SetPositionCOM(shape.GetPositionCOM() - mCenterOfMass);
+
+		// Transform the shape's bounds into our local space
+		Mat44 transform = Mat44::sRotationTranslation(shape.GetRotation(), shape.GetPositionCOM());
+		AABox shape_bounds = shape.mShape->GetWorldSpaceBounds(transform, Vec3::sReplicate(1.0f));
+
+		// Store bounds and body index for tree construction
+		bounds[i] = shape_bounds;
+		body_idx[i] = i;
+
+		// Update our local bounds
+		mLocalBounds.Encapsulate(shape_bounds);
+	}
+
+	// The algorithm is a recursive tree build, but to avoid the call overhead we keep track of a stack here
+	struct StackEntry
+	{
+		uint32			mNodeIdx;					// Node index of node that is generated
+		int				mChildIdx;					// Index of child that we're currently processing
+		int				mSplit[5];					// Indices where the node ID's have been split to form 4 partitions
+		AABox			mBounds;					// Bounding box of this node
+	};
+	uint stack_size = num_subshapes * sizeof(StackEntry);
+	StackEntry *stack = (StackEntry *)inTempAllocator.Allocate(stack_size);
+	JPH_SCOPE_EXIT([&inTempAllocator, stack, stack_size]{ inTempAllocator.Free(stack, stack_size); });
+	int top = 0;
+
+	// Reserve enough space so that every sub shape gets its own leaf node
+	uint next_node_idx = 0;
+	mNodes.resize(num_subshapes + (num_subshapes + 2) / 3); // = Sum(num_subshapes * 4^-i) with i = [0, Inf].
+
+	// Create root node
+	stack[0].mNodeIdx = next_node_idx++;
+	stack[0].mChildIdx = -1;
+	stack[0].mBounds = AABox();
+	sPartition4(body_idx, bounds, 0, num_subshapes, stack[0].mSplit);
+
+	for (;;)
+	{
+		StackEntry &cur_stack = stack[top];
+
+		// Next child
+		cur_stack.mChildIdx++;
+
+		// Check if all children processed
+		if (cur_stack.mChildIdx >= 4)
+		{
+			// Terminate if there's nothing left to pop
+			if (top <= 0)
+				break;
+
+			// Add our bounds to our parents bounds
+			StackEntry &prev_stack = stack[top - 1];
+			prev_stack.mBounds.Encapsulate(cur_stack.mBounds);
+
+			// Store this node's properties in the parent node
+			Node &parent_node = mNodes[prev_stack.mNodeIdx];
+			parent_node.mNodeProperties[prev_stack.mChildIdx] = cur_stack.mNodeIdx;
+			parent_node.SetChildBounds(prev_stack.mChildIdx, cur_stack.mBounds);
+
+			// Pop entry from stack
+			--top;
+		}
+		else
+		{
+			// Get low and high index to bodies to process
+			int low = cur_stack.mSplit[cur_stack.mChildIdx];
+			int high = cur_stack.mSplit[cur_stack.mChildIdx + 1];
+			int num_bodies = high - low;
+
+			if (num_bodies == 0)
+			{
+				// Mark invalid
+				Node &node = mNodes[cur_stack.mNodeIdx];
+				node.SetChildInvalid(cur_stack.mChildIdx);
+			}
+			else if (num_bodies == 1)
+			{
+				// Get body info
+				uint child_node_idx = body_idx[low];
+				const AABox &child_bounds = bounds[low];
+
+				// Update node
+				Node &node = mNodes[cur_stack.mNodeIdx];
+				node.mNodeProperties[cur_stack.mChildIdx] = child_node_idx | IS_SUBSHAPE;
+				node.SetChildBounds(cur_stack.mChildIdx, child_bounds);
+
+				// Encapsulate bounding box in parent
+				cur_stack.mBounds.Encapsulate(child_bounds);
+			}
+			else
+			{
+				// Allocate new node
+				StackEntry &new_stack = stack[++top];
+				JPH_ASSERT(top < (int)num_subshapes);
+				new_stack.mNodeIdx = next_node_idx++;
+				new_stack.mChildIdx = -1;
+				new_stack.mBounds = AABox();
+				sPartition4(body_idx, bounds, low, high, new_stack.mSplit);
+			}
+		}
+	}
+
+	// Resize nodes to actual size
+	JPH_ASSERT(next_node_idx <= mNodes.size());
+	mNodes.resize(next_node_idx);
+	mNodes.shrink_to_fit();
+
+	// Check if we ran out of bits for addressing a node
+	if (next_node_idx > IS_SUBSHAPE)
+	{
+		outResult.SetError("Compound hierarchy has too many nodes");
+		return;
+	}
+
+	// Check if we're not exceeding the amount of sub shape id bits
+	if (GetSubShapeIDBitsRecursive() > SubShapeID::MaxBits)
+	{
+		outResult.SetError("Compound hierarchy is too deep and exceeds the amount of available sub shape ID bits");
+		return;
+	}
+
+	outResult.Set(this);
+}
+
+template <class Visitor>
+inline void StaticCompoundShape::WalkTree(Visitor &ioVisitor) const
+{
+	uint32 node_stack[cStackSize];
+	node_stack[0] = 0;
+	int top = 0;
+	do
+	{
+		// Test if the node is valid, the node should rarely be invalid but it is possible when testing
+		// a really large box against the tree that the invalid nodes will intersect with the box
+		uint32 node_properties = node_stack[top];
+		if (node_properties != INVALID_NODE)
+		{
+			// Test if node contains triangles
+			bool is_node = (node_properties & IS_SUBSHAPE) == 0;
+			if (is_node)
+			{
+				const Node &node = mNodes[node_properties];
+
+				// Unpack bounds
+				UVec4 bounds_minxy = UVec4::sLoadInt4(reinterpret_cast<const uint32 *>(&node.mBoundsMinX[0]));
+				Vec4 bounds_minx = HalfFloatConversion::ToFloat(bounds_minxy);
+				Vec4 bounds_miny = HalfFloatConversion::ToFloat(bounds_minxy.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_UNUSED, SWIZZLE_UNUSED>());
+
+				UVec4 bounds_minzmaxx = UVec4::sLoadInt4(reinterpret_cast<const uint32 *>(&node.mBoundsMinZ[0]));
+				Vec4 bounds_minz = HalfFloatConversion::ToFloat(bounds_minzmaxx);
+				Vec4 bounds_maxx = HalfFloatConversion::ToFloat(bounds_minzmaxx.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_UNUSED, SWIZZLE_UNUSED>());
+
+				UVec4 bounds_maxyz = UVec4::sLoadInt4(reinterpret_cast<const uint32 *>(&node.mBoundsMaxY[0]));
+				Vec4 bounds_maxy = HalfFloatConversion::ToFloat(bounds_maxyz);
+				Vec4 bounds_maxz = HalfFloatConversion::ToFloat(bounds_maxyz.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_UNUSED, SWIZZLE_UNUSED>());
+
+				// Load properties for 4 children
+				UVec4 properties = UVec4::sLoadInt4(&node.mNodeProperties[0]);
+
+				// Check which sub nodes to visit
+				int num_results = ioVisitor.VisitNodes(bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz, properties, top);
+
+				// Push them onto the stack
+				JPH_ASSERT(top + 4 < cStackSize);
+				properties.StoreInt4(&node_stack[top]);
+				top += num_results;
+			}
+			else
+			{
+				// Points to a sub shape
+				uint32 sub_shape_idx = node_properties ^ IS_SUBSHAPE;
+				const SubShape &sub_shape = mSubShapes[sub_shape_idx];
+
+				ioVisitor.VisitShape(sub_shape, sub_shape_idx);
+			}
+
+			// Check if we're done
+			if (ioVisitor.ShouldAbort())
+				break;
+		}
+
+		// Fetch next node until we find one that the visitor wants to see
+		do
+			--top;
+		while (top >= 0 && !ioVisitor.ShouldVisitNode(top));
+	}
+	while (top >= 0);
+}
+
+bool StaticCompoundShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	struct Visitor : public CastRayVisitor
+	{
+		using CastRayVisitor::CastRayVisitor;
+
+		JPH_INLINE bool		ShouldVisitNode(int inStackTop) const
+		{
+			return mDistanceStack[inStackTop] < mHit.mFraction;
+		}
+
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
+		{
+			// Test bounds of 4 children
+			Vec4 distance = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+
+			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
+			return SortReverseAndStore(distance, mHit.mFraction, ioProperties, &mDistanceStack[inStackTop]);
+		}
+
+		float				mDistanceStack[cStackSize];
+	};
+
+	Visitor visitor(inRay, this, inSubShapeIDCreator, ioHit);
+	WalkTree(visitor);
+	return visitor.mReturnValue;
+}
+
+void StaticCompoundShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	JPH_PROFILE_FUNCTION();
+
+	struct Visitor : public CastRayVisitorCollector
+	{
+		using CastRayVisitorCollector::CastRayVisitorCollector;
+
+		JPH_INLINE bool		ShouldVisitNode(int inStackTop) const
+		{
+			return mDistanceStack[inStackTop] < mCollector.GetEarlyOutFraction();
+		}
+
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
+		{
+			// Test bounds of 4 children
+			Vec4 distance = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+
+			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
+			return SortReverseAndStore(distance, mCollector.GetEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
+		}
+
+		float				mDistanceStack[cStackSize];
+	};
+
+	Visitor visitor(inRay, inRayCastSettings, this, inSubShapeIDCreator, ioCollector, inShapeFilter);
+	WalkTree(visitor);
+}
+
+void StaticCompoundShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	struct Visitor : public CollidePointVisitor
+	{
+		using CollidePointVisitor::CollidePointVisitor;
+
+		JPH_INLINE bool		ShouldVisitNode([[maybe_unused]] int inStackTop) const
+		{
+			return true;
+		}
+
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const
+		{
+			// Test if point overlaps with box
+			UVec4 collides = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+			return CountAndSortTrues(collides, ioProperties);
+		}
+	};
+
+	Visitor visitor(inPoint, this, inSubShapeIDCreator, ioCollector, inShapeFilter);
+	WalkTree(visitor);
+}
+
+void StaticCompoundShape::sCastShapeVsCompound(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_PROFILE_FUNCTION();
+
+	struct Visitor : public CastShapeVisitor
+	{
+		using CastShapeVisitor::CastShapeVisitor;
+
+		JPH_INLINE bool		ShouldVisitNode(int inStackTop) const
+		{
+			return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction();
+		}
+
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
+		{
+			// Test bounds of 4 children
+			Vec4 distance = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+
+			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
+			return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
+		}
+
+		float				mDistanceStack[cStackSize];
+	};
+
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::StaticCompound);
+	const StaticCompoundShape *shape = static_cast<const StaticCompoundShape *>(inShape);
+
+	Visitor visitor(inShapeCast, inShapeCastSettings, shape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
+	shape->WalkTree(visitor);
+}
+
+void StaticCompoundShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	struct Visitor : public CollectTransformedShapesVisitor
+	{
+		using CollectTransformedShapesVisitor::CollectTransformedShapesVisitor;
+
+		JPH_INLINE bool		ShouldVisitNode([[maybe_unused]] int inStackTop) const
+		{
+			return true;
+		}
+
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const
+		{
+			// Test which nodes collide
+			UVec4 collides = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+			return CountAndSortTrues(collides, ioProperties);
+		}
+	};
+
+	Visitor visitor(inBox, this, inPositionCOM, inRotation, inScale, inSubShapeIDCreator, ioCollector, inShapeFilter);
+	WalkTree(visitor);
+}
+
+int StaticCompoundShape::GetIntersectingSubShapes(const AABox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	GetIntersectingSubShapesVisitorSC<AABox> visitor(inBox, outSubShapeIndices, inMaxSubShapeIndices);
+	WalkTree(visitor);
+	return visitor.GetNumResults();
+}
+
+int StaticCompoundShape::GetIntersectingSubShapes(const OrientedBox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	GetIntersectingSubShapesVisitorSC<OrientedBox> visitor(inBox, outSubShapeIndices, inMaxSubShapeIndices);
+	WalkTree(visitor);
+	return visitor.GetNumResults();
+}
+
+void StaticCompoundShape::sCollideCompoundVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::StaticCompound);
+	const StaticCompoundShape *shape1 = static_cast<const StaticCompoundShape *>(inShape1);
+
+	struct Visitor : public CollideCompoundVsShapeVisitor
+	{
+		using CollideCompoundVsShapeVisitor::CollideCompoundVsShapeVisitor;
+
+		JPH_INLINE bool		ShouldVisitNode([[maybe_unused]] int inStackTop) const
+		{
+			return true;
+		}
+
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const
+		{
+			// Test which nodes collide
+			UVec4 collides = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+			return CountAndSortTrues(collides, ioProperties);
+		}
+	};
+
+	Visitor visitor(shape1, inShape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
+	shape1->WalkTree(visitor);
+}
+
+void StaticCompoundShape::sCollideShapeVsCompound(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
+{
+	JPH_PROFILE_FUNCTION();
+
+	struct Visitor : public CollideShapeVsCompoundVisitor
+	{
+		using CollideShapeVsCompoundVisitor::CollideShapeVsCompoundVisitor;
+
+		JPH_INLINE bool		ShouldVisitNode([[maybe_unused]] int inStackTop) const
+		{
+			return true;
+		}
+
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const
+		{
+			// Test which nodes collide
+			UVec4 collides = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+			return CountAndSortTrues(collides, ioProperties);
+		}
+	};
+
+	JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::StaticCompound);
+	const StaticCompoundShape *shape2 = static_cast<const StaticCompoundShape *>(inShape2);
+
+	Visitor visitor(inShape1, shape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
+	shape2->WalkTree(visitor);
+}
+
+void StaticCompoundShape::SaveBinaryState(StreamOut &inStream) const
+{
+	CompoundShape::SaveBinaryState(inStream);
+
+	inStream.Write(mNodes);
+}
+
+void StaticCompoundShape::RestoreBinaryState(StreamIn &inStream)
+{
+	CompoundShape::RestoreBinaryState(inStream);
+
+	inStream.Read(mNodes);
+}
+
+void StaticCompoundShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::StaticCompound);
+	f.mConstruct = []() -> Shape * { return new StaticCompoundShape; };
+	f.mColor = Color::sOrange;
+
+	for (EShapeSubType s : sAllSubShapeTypes)
+	{
+		CollisionDispatch::sRegisterCollideShape(EShapeSubType::StaticCompound, s, sCollideCompoundVsShape);
+		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::StaticCompound, sCollideShapeVsCompound);
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::StaticCompound, sCastShapeVsCompound);
+	}
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/StaticCompoundShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/StaticCompoundShape.h
new file mode 100644
index 0000000000..ea0a86fd3b
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/StaticCompoundShape.h
@@ -0,0 +1,139 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/CompoundShape.h>
+#include <Jolt/Physics/Collision/SortReverseAndStore.h>
+#include <Jolt/Math/HalfFloat.h>
+
+JPH_NAMESPACE_BEGIN
+
+class CollideShapeSettings;
+class TempAllocator;
+
+/// Class that constructs a StaticCompoundShape. Note that if you only want a compound of 1 shape, use a RotatedTranslatedShape instead.
+class JPH_EXPORT StaticCompoundShapeSettings final : public CompoundShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, StaticCompoundShapeSettings)
+
+public:
+	// See: ShapeSettings
+	virtual ShapeResult				Create() const override;
+
+	/// Specialization of Create() function that allows specifying a temp allocator to avoid temporary memory allocations on the heap
+	ShapeResult						Create(TempAllocator &inTempAllocator) const;
+};
+
+/// A compound shape, sub shapes can be rotated and translated.
+/// Sub shapes cannot be modified once the shape is constructed.
+/// Shifts all child objects so that they're centered around the center of mass.
+class JPH_EXPORT StaticCompoundShape final : public CompoundShape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+									StaticCompoundShape() : CompoundShape(EShapeSubType::StaticCompound) { }
+									StaticCompoundShape(const StaticCompoundShapeSettings &inSettings, TempAllocator &inTempAllocator, ShapeResult &outResult);
+
+	// See Shape::CastRay
+	virtual bool					CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+	virtual void					CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollidePoint
+	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See Shape::CollectTransformedShapes
+	virtual void					CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override;
+
+	// See: CompoundShape::GetIntersectingSubShapes
+	virtual int						GetIntersectingSubShapes(const AABox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const override;
+
+	// See: CompoundShape::GetIntersectingSubShapes
+	virtual int						GetIntersectingSubShapes(const OrientedBox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const override;
+
+	// See Shape
+	virtual void					SaveBinaryState(StreamOut &inStream) const override;
+
+	// See Shape::GetStats
+	virtual Stats					GetStats() const override								{ return Stats(sizeof(*this) + mSubShapes.size() * sizeof(SubShape) + mNodes.size() * sizeof(Node), 0); }
+
+	// Register shape functions with the registry
+	static void						sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void					RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	// Visitor for GetIntersectingSubShapes
+	template <class BoxType>
+	struct GetIntersectingSubShapesVisitorSC : public GetIntersectingSubShapesVisitor<BoxType>
+	{
+		using GetIntersectingSubShapesVisitor<BoxType>::GetIntersectingSubShapesVisitor;
+
+		JPH_INLINE bool				ShouldVisitNode(int inStackTop) const
+		{
+			return true;
+		}
+
+		JPH_INLINE int				VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
+		{
+			// Test if point overlaps with box
+			UVec4 collides = GetIntersectingSubShapesVisitor<BoxType>::TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
+			return CountAndSortTrues(collides, ioProperties);
+		}
+	};
+
+	/// Sorts ioBodyIdx spatially into 2 groups. Second groups starts at ioBodyIdx + outMidPoint.
+	/// After the function returns ioBodyIdx and ioBounds will be shuffled
+	static void						sPartition(uint *ioBodyIdx, AABox *ioBounds, int inNumber, int &outMidPoint);
+
+	/// Sorts ioBodyIdx from inBegin to (but excluding) inEnd spatially into 4 groups.
+	/// outSplit needs to be 5 ints long, when the function returns each group runs from outSplit[i] to (but excluding) outSplit[i + 1]
+	/// After the function returns ioBodyIdx and ioBounds will be shuffled
+	static void						sPartition4(uint *ioBodyIdx, AABox *ioBounds, int inBegin, int inEnd, int *outSplit);
+
+	// Helper functions called by CollisionDispatch
+	static void						sCollideCompoundVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCollideShapeVsCompound(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCastShapeVsCompound(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+
+	// Maximum size of the stack during tree walk
+	static constexpr int			cStackSize = 128;
+
+	template <class Visitor>
+	JPH_INLINE void					WalkTree(Visitor &ioVisitor) const;						///< Walk the node tree calling the Visitor::VisitNodes for each node encountered and Visitor::VisitShape for each sub shape encountered
+
+	/// Bits used in Node::mNodeProperties
+	enum : uint32
+	{
+		IS_SUBSHAPE					= 0x80000000,											///< If this bit is set, the other bits index in mSubShape, otherwise in mNodes
+		INVALID_NODE				= 0x7fffffff,											///< Signifies an invalid node
+	};
+
+	/// Node structure
+	struct Node
+	{
+		void						SetChildBounds(uint inIndex, const AABox &inBounds);	///< Set bounding box for child inIndex to inBounds
+		void						SetChildInvalid(uint inIndex);							///< Mark the child inIndex as invalid and set its bounding box to invalid
+
+		HalfFloat					mBoundsMinX[4];											///< 4 child bounding boxes
+		HalfFloat					mBoundsMinY[4];
+		HalfFloat					mBoundsMinZ[4];
+		HalfFloat					mBoundsMaxX[4];
+		HalfFloat					mBoundsMaxY[4];
+		HalfFloat					mBoundsMaxZ[4];
+		uint32						mNodeProperties[4];										///< 4 child node properties
+	};
+
+	static_assert(sizeof(Node) == 64, "Node should be 64 bytes");
+
+	using Nodes = Array<Node>;
+
+	Nodes							mNodes;													///< Quad tree node structure
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SubShapeID.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SubShapeID.h
new file mode 100644
index 0000000000..f1e8d3713b
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SubShapeID.h
@@ -0,0 +1,138 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// @brief A sub shape id contains a path to an element (usually a triangle or other primitive type) of a compound shape
+///
+/// Each sub shape knows how many bits it needs to encode its ID, so knows how many bits to take from the sub shape ID.
+///
+/// For example:
+/// * We have a CompoundShape A with 5 child shapes (identify sub shape using 3 bits AAA)
+/// * One of its child shapes is CompoundShape B which has 3 child shapes (identify sub shape using 2 bits BB)
+/// * One of its child shapes is MeshShape C which contains enough triangles to need 7 bits to identify a triangle (identify sub shape using 7 bits CCCCCCC, note that MeshShape is block based and sorts triangles spatially, you can't assume that the first triangle will have bit pattern 0000000).
+///
+/// The bit pattern of the sub shape ID to identify a triangle in MeshShape C will then be CCCCCCCBBAAA.
+///
+/// A sub shape ID will become invalid when the structure of the shape changes. For example, if a child shape is removed from a compound shape, the sub shape ID will no longer be valid.
+/// This can be a problem when caching sub shape IDs from one frame to the next. See comments at ContactListener::OnContactPersisted / OnContactRemoved.
+class SubShapeID
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Underlying storage type
+	using Type = uint32;
+
+	/// Type that is bigger than the underlying storage type for operations that would otherwise overflow
+	using BiggerType = uint64;
+
+	static_assert(sizeof(BiggerType) > sizeof(Type), "The calculation below assumes BiggerType is a bigger type than Type");
+
+	/// How many bits we can store in this ID
+	static constexpr uint MaxBits = 8 * sizeof(Type);
+
+	/// Constructor
+						SubShapeID() = default;
+
+	/// Get the next id in the chain of ids (pops parents before children)
+	Type				PopID(uint inBits, SubShapeID &outRemainder) const
+	{
+		Type mask_bits = Type((BiggerType(1) << inBits) - 1);
+		Type fill_bits = Type(BiggerType(cEmpty) << (MaxBits - inBits)); // Fill left side bits with 1 so that if there's no remainder all bits will be set, note that we do this using a BiggerType since on intel 0xffffffff << 32 == 0xffffffff
+		Type v = mValue & mask_bits;
+		outRemainder = SubShapeID(Type(BiggerType(mValue) >> inBits) | fill_bits);
+		return v;
+	}
+
+	/// Get the value of the path to the sub shape ID
+	inline Type			GetValue() const
+	{
+		return mValue;
+	}
+
+	/// Set the value of the sub shape ID (use with care!)
+	inline void			SetValue(Type inValue)
+	{
+		mValue = inValue;
+	}
+
+	/// Check if there is any bits of subshape ID left.
+	/// Note that this is not a 100% guarantee as the subshape ID could consist of all 1 bits. Use for asserts only.
+	inline bool			IsEmpty() const
+	{
+		return mValue == cEmpty;
+	}
+
+	/// Check equal
+	inline bool			operator == (const SubShapeID &inRHS) const
+	{
+		return mValue == inRHS.mValue;
+	}
+
+	/// Check not-equal
+	inline bool			operator != (const SubShapeID &inRHS) const
+	{
+		return mValue != inRHS.mValue;
+	}
+
+private:
+	friend class SubShapeIDCreator;
+
+	/// An empty SubShapeID has all bits set
+	static constexpr Type cEmpty = ~Type(0);
+
+	/// Constructor
+	explicit			SubShapeID(const Type &inValue) : mValue(inValue) { }
+
+	/// Adds an id at a particular position in the chain
+	/// (this should really only be called by the SubShapeIDCreator)
+	void				PushID(Type inValue, uint inFirstBit, uint inBits)
+	{
+		// First clear the bits
+		mValue &= ~(Type((BiggerType(1) << inBits) - 1) << inFirstBit);
+
+		// Then set them to the new value
+		mValue |= inValue << inFirstBit;
+	}
+
+	Type				mValue = cEmpty;
+};
+
+/// A sub shape id creator can be used to create a new sub shape id by recursing through the shape
+/// hierarchy and pushing new ID's onto the chain
+class SubShapeIDCreator
+{
+public:
+	/// Add a new id to the chain of id's and return it
+	SubShapeIDCreator	PushID(uint inValue, uint inBits) const
+	{
+		JPH_ASSERT(inValue < (SubShapeID::BiggerType(1) << inBits));
+		SubShapeIDCreator copy = *this;
+		copy.mID.PushID(inValue, mCurrentBit, inBits);
+		copy.mCurrentBit += inBits;
+		JPH_ASSERT(copy.mCurrentBit <= SubShapeID::MaxBits);
+		return copy;
+	}
+
+	// Get the resulting sub shape ID
+	const SubShapeID &	GetID() const
+	{
+		return mID;
+	}
+
+	/// Get the number of bits that have been written to the sub shape ID so far
+	inline uint			GetNumBitsWritten() const
+	{
+		return mCurrentBit;
+	}
+
+private:
+	SubShapeID			mID;
+	uint				mCurrentBit = 0;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SubShapeIDPair.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SubShapeIDPair.h
new file mode 100644
index 0000000000..f33ca310e7
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/SubShapeIDPair.h
@@ -0,0 +1,80 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/BodyID.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+#include <Jolt/Core/HashCombine.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// A pair of bodies and their sub shape ID's. Can be used as a key in a map to find a contact point.
+class SubShapeIDPair
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+							SubShapeIDPair() = default;
+							SubShapeIDPair(const BodyID &inBody1ID, const SubShapeID &inSubShapeID1, const BodyID &inBody2ID, const SubShapeID &inSubShapeID2) : mBody1ID(inBody1ID), mSubShapeID1(inSubShapeID1), mBody2ID(inBody2ID), mSubShapeID2(inSubShapeID2) { }
+	SubShapeIDPair &		operator = (const SubShapeIDPair &) = default;
+							SubShapeIDPair(const SubShapeIDPair &) = default;
+
+	/// Equality operator
+	inline bool				operator == (const SubShapeIDPair &inRHS) const
+	{
+		return UVec4::sLoadInt4(reinterpret_cast<const uint32 *>(this)) == UVec4::sLoadInt4(reinterpret_cast<const uint32 *>(&inRHS));
+	}
+
+	/// Less than operator, used to consistently order contact points for a deterministic simulation
+	inline bool				operator < (const SubShapeIDPair &inRHS) const
+	{
+		if (mBody1ID != inRHS.mBody1ID)
+			return mBody1ID < inRHS.mBody1ID;
+
+		if (mSubShapeID1.GetValue() != inRHS.mSubShapeID1.GetValue())
+			return mSubShapeID1.GetValue() < inRHS.mSubShapeID1.GetValue();
+
+		if (mBody2ID != inRHS.mBody2ID)
+			return mBody2ID < inRHS.mBody2ID;
+
+		return mSubShapeID2.GetValue() < inRHS.mSubShapeID2.GetValue();
+	}
+
+	const BodyID &			GetBody1ID() const				{ return mBody1ID; }
+	const SubShapeID &		GetSubShapeID1() const			{ return mSubShapeID1; }
+	const BodyID &			GetBody2ID() const				{ return mBody2ID; }
+	const SubShapeID &		GetSubShapeID2() const			{ return mSubShapeID2; }
+
+	uint64					GetHash() const					{ return HashBytes(this, sizeof(SubShapeIDPair)); }
+
+private:
+	BodyID					mBody1ID;
+	SubShapeID				mSubShapeID1;
+	BodyID					mBody2ID;
+	SubShapeID				mSubShapeID2;
+};
+
+static_assert(sizeof(SubShapeIDPair) == 16, "Unexpected size");
+static_assert(alignof(SubShapeIDPair) == 4, "Assuming 4 byte aligned");
+
+JPH_NAMESPACE_END
+
+JPH_SUPPRESS_WARNINGS_STD_BEGIN
+
+namespace std
+{
+	/// Declare std::hash for SubShapeIDPair
+	template <>
+	struct hash<JPH::SubShapeIDPair>
+	{
+		inline size_t operator () (const JPH::SubShapeIDPair &inRHS) const
+		{
+			return static_cast<size_t>(inRHS.GetHash());
+		}
+	};
+}
+
+JPH_SUPPRESS_WARNINGS_STD_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp
new file mode 100644
index 0000000000..a3a9e020d5
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp
@@ -0,0 +1,453 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
+#include <Jolt/Geometry/RayCapsule.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(TaperedCapsuleShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(TaperedCapsuleShapeSettings, ConvexShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(TaperedCapsuleShapeSettings, mHalfHeightOfTaperedCylinder)
+	JPH_ADD_ATTRIBUTE(TaperedCapsuleShapeSettings, mTopRadius)
+	JPH_ADD_ATTRIBUTE(TaperedCapsuleShapeSettings, mBottomRadius)
+}
+
+bool TaperedCapsuleShapeSettings::IsSphere() const
+{
+	return max(mTopRadius, mBottomRadius) >= 2.0f * mHalfHeightOfTaperedCylinder + min(mTopRadius, mBottomRadius);
+}
+
+ShapeSettings::ShapeResult TaperedCapsuleShapeSettings::Create() const
+{
+	if (mCachedResult.IsEmpty())
+	{
+		Ref<Shape> shape;
+		if (IsValid() && IsSphere())
+		{
+			// Determine sphere center and radius
+			float radius, center;
+			if (mTopRadius > mBottomRadius)
+			{
+				radius = mTopRadius;
+				center = mHalfHeightOfTaperedCylinder;
+			}
+			else
+			{
+				radius = mBottomRadius;
+				center = -mHalfHeightOfTaperedCylinder;
+			}
+
+			// Create sphere
+			shape = new SphereShape(radius, mMaterial);
+
+			// Offset sphere if needed
+			if (abs(center) > 1.0e-6f)
+			{
+				RotatedTranslatedShapeSettings rot_trans(Vec3(0, center, 0), Quat::sIdentity(), shape);
+				mCachedResult = rot_trans.Create();
+			}
+			else
+				mCachedResult.Set(shape);
+		}
+		else
+		{
+			// Normal tapered capsule shape
+			shape = new TaperedCapsuleShape(*this, mCachedResult);
+		}
+	}
+	return mCachedResult;
+}
+
+TaperedCapsuleShapeSettings::TaperedCapsuleShapeSettings(float inHalfHeightOfTaperedCylinder, float inTopRadius, float inBottomRadius, const PhysicsMaterial *inMaterial) :
+	ConvexShapeSettings(inMaterial),
+	mHalfHeightOfTaperedCylinder(inHalfHeightOfTaperedCylinder),
+	mTopRadius(inTopRadius),
+	mBottomRadius(inBottomRadius)
+{
+}
+
+TaperedCapsuleShape::TaperedCapsuleShape(const TaperedCapsuleShapeSettings &inSettings, ShapeResult &outResult) :
+	ConvexShape(EShapeSubType::TaperedCapsule, inSettings, outResult),
+	mTopRadius(inSettings.mTopRadius),
+	mBottomRadius(inSettings.mBottomRadius)
+{
+	if (mTopRadius <= 0.0f)
+	{
+		outResult.SetError("Invalid top radius");
+		return;
+	}
+
+	if (mBottomRadius <= 0.0f)
+	{
+		outResult.SetError("Invalid bottom radius");
+		return;
+	}
+
+	if (inSettings.mHalfHeightOfTaperedCylinder <= 0.0f)
+	{
+		outResult.SetError("Invalid height");
+		return;
+	}
+
+	// If this goes off one of the sphere ends falls totally inside the other and you should use a sphere instead
+	if (inSettings.IsSphere())
+	{
+		outResult.SetError("One sphere embedded in other sphere, please use sphere shape instead");
+		return;
+	}
+
+	// Approximation: The center of mass is exactly half way between the top and bottom cap of the tapered capsule
+	mTopCenter = inSettings.mHalfHeightOfTaperedCylinder + 0.5f * (mBottomRadius - mTopRadius);
+	mBottomCenter = -inSettings.mHalfHeightOfTaperedCylinder + 0.5f * (mBottomRadius - mTopRadius);
+
+	// Calculate center of mass
+	mCenterOfMass = Vec3(0, inSettings.mHalfHeightOfTaperedCylinder - mTopCenter, 0);
+
+	// Calculate convex radius
+	mConvexRadius = min(mTopRadius, mBottomRadius);
+	JPH_ASSERT(mConvexRadius > 0.0f);
+
+	// Calculate the sin and tan of the angle that the cone surface makes with the Y axis
+	// See: TaperedCapsuleShape.gliffy
+	mSinAlpha = (mBottomRadius - mTopRadius) / (mTopCenter - mBottomCenter);
+	JPH_ASSERT(mSinAlpha >= -1.0f && mSinAlpha <= 1.0f);
+	mTanAlpha = Tan(ASin(mSinAlpha));
+
+	outResult.Set(this);
+}
+
+class TaperedCapsuleShape::TaperedCapsule final : public Support
+{
+public:
+					TaperedCapsule(Vec3Arg inTopCenter, Vec3Arg inBottomCenter, float inTopRadius, float inBottomRadius, float inConvexRadius) :
+		mTopCenter(inTopCenter),
+		mBottomCenter(inBottomCenter),
+		mTopRadius(inTopRadius),
+		mBottomRadius(inBottomRadius),
+		mConvexRadius(inConvexRadius)
+	{
+		static_assert(sizeof(TaperedCapsule) <= sizeof(SupportBuffer), "Buffer size too small");
+		JPH_ASSERT(IsAligned(this, alignof(TaperedCapsule)));
+	}
+
+	virtual Vec3	GetSupport(Vec3Arg inDirection) const override
+	{
+		// Check zero vector
+		float len = inDirection.Length();
+		if (len == 0.0f)
+			return mTopCenter + Vec3(0, mTopRadius, 0); // Return top
+
+		// Check if the support of the top sphere or bottom sphere is bigger
+		Vec3 support_top = mTopCenter + (mTopRadius / len) * inDirection;
+		Vec3 support_bottom = mBottomCenter + (mBottomRadius / len) * inDirection;
+		if (support_top.Dot(inDirection) > support_bottom.Dot(inDirection))
+			return support_top;
+		else
+			return support_bottom;
+	}
+
+	virtual float	GetConvexRadius() const override
+	{
+		return mConvexRadius;
+	}
+
+private:
+	Vec3			mTopCenter;
+	Vec3			mBottomCenter;
+	float			mTopRadius;
+	float			mBottomRadius;
+	float			mConvexRadius;
+};
+
+const ConvexShape::Support *TaperedCapsuleShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const
+{
+	JPH_ASSERT(IsValidScale(inScale));
+
+	// Get scaled tapered capsule
+	Vec3 abs_scale = inScale.Abs();
+	float scale_xz = abs_scale.GetX();
+	float scale_y = inScale.GetY(); // The sign of y is important as it flips the tapered capsule
+	Vec3 scaled_top_center = Vec3(0, scale_y * mTopCenter, 0);
+	Vec3 scaled_bottom_center = Vec3(0, scale_y * mBottomCenter, 0);
+	float scaled_top_radius = scale_xz * mTopRadius;
+	float scaled_bottom_radius = scale_xz * mBottomRadius;
+	float scaled_convex_radius = scale_xz * mConvexRadius;
+
+	switch (inMode)
+	{
+	case ESupportMode::IncludeConvexRadius:
+		return new (&inBuffer) TaperedCapsule(scaled_top_center, scaled_bottom_center, scaled_top_radius, scaled_bottom_radius, 0.0f);
+
+	case ESupportMode::ExcludeConvexRadius:
+	case ESupportMode::Default:
+		{
+			// Get radii reduced by convex radius
+			float tr = scaled_top_radius - scaled_convex_radius;
+			float br = scaled_bottom_radius - scaled_convex_radius;
+			JPH_ASSERT(tr >= 0.0f && br >= 0.0f);
+			JPH_ASSERT(tr == 0.0f || br == 0.0f, "Convex radius should be that of the smallest sphere");
+			return new (&inBuffer) TaperedCapsule(scaled_top_center, scaled_bottom_center, tr, br, scaled_convex_radius);
+		}
+	}
+
+	JPH_ASSERT(false);
+	return nullptr;
+}
+
+void TaperedCapsuleShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
+	JPH_ASSERT(IsValidScale(inScale));
+
+	// Check zero vector
+	float len = inDirection.Length();
+	if (len == 0.0f)
+		return;
+
+	// Get scaled tapered capsule
+	Vec3 abs_scale = inScale.Abs();
+	float scale_xz = abs_scale.GetX();
+	float scale_y = inScale.GetY(); // The sign of y is important as it flips the tapered capsule
+	Vec3 scaled_top_center = Vec3(0, scale_y * mTopCenter, 0);
+	Vec3 scaled_bottom_center = Vec3(0, scale_y * mBottomCenter, 0);
+	float scaled_top_radius = scale_xz * mTopRadius;
+	float scaled_bottom_radius = scale_xz * mBottomRadius;
+
+	// Get support point for top and bottom sphere in the opposite of inDirection (including convex radius)
+	Vec3 support_top = scaled_top_center - (scaled_top_radius / len) * inDirection;
+	Vec3 support_bottom = scaled_bottom_center - (scaled_bottom_radius / len) * inDirection;
+
+	// Get projection on inDirection
+	float proj_top = support_top.Dot(inDirection);
+	float proj_bottom = support_bottom.Dot(inDirection);
+
+	// If projection is roughly equal then return line, otherwise we return nothing as there's only 1 point
+	if (abs(proj_top - proj_bottom) < cCapsuleProjectionSlop * len)
+	{
+		outVertices.push_back(inCenterOfMassTransform * support_top);
+		outVertices.push_back(inCenterOfMassTransform * support_bottom);
+	}
+}
+
+MassProperties TaperedCapsuleShape::GetMassProperties() const
+{
+	AABox box = GetInertiaApproximation();
+
+	MassProperties p;
+	p.SetMassAndInertiaOfSolidBox(box.GetSize(), GetDensity());
+	return p;
+}
+
+Vec3 TaperedCapsuleShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
+
+	// See: TaperedCapsuleShape.gliffy
+	// We need to calculate ty and by in order to see if the position is on the top or bottom sphere
+	// sin(alpha) = by / br = ty / tr
+	// => by = sin(alpha) * br, ty = sin(alpha) * tr
+
+	if (inLocalSurfacePosition.GetY() > mTopCenter + mSinAlpha * mTopRadius)
+		return (inLocalSurfacePosition - Vec3(0, mTopCenter, 0)).Normalized();
+	else if (inLocalSurfacePosition.GetY() < mBottomCenter + mSinAlpha * mBottomRadius)
+		return (inLocalSurfacePosition - Vec3(0, mBottomCenter, 0)).Normalized();
+	else
+	{
+		// Get perpendicular vector to the surface in the xz plane
+		Vec3 perpendicular = Vec3(inLocalSurfacePosition.GetX(), 0, inLocalSurfacePosition.GetZ()).NormalizedOr(Vec3::sAxisX());
+
+		// We know that the perpendicular has length 1 and that it needs a y component where tan(alpha) = y / 1 in order to align it to the surface
+		perpendicular.SetY(mTanAlpha);
+		return perpendicular.Normalized();
+	}
+}
+
+AABox TaperedCapsuleShape::GetLocalBounds() const
+{
+	float max_radius = max(mTopRadius, mBottomRadius);
+	return AABox(Vec3(-max_radius, mBottomCenter - mBottomRadius, -max_radius), Vec3(max_radius, mTopCenter + mTopRadius, max_radius));
+}
+
+AABox TaperedCapsuleShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
+	JPH_ASSERT(IsValidScale(inScale));
+
+	Vec3 abs_scale = inScale.Abs();
+	float scale_xz = abs_scale.GetX();
+	float scale_y = inScale.GetY(); // The sign of y is important as it flips the tapered capsule
+	Vec3 bottom_extent = Vec3::sReplicate(scale_xz * mBottomRadius);
+	Vec3 bottom_center = inCenterOfMassTransform * Vec3(0, scale_y * mBottomCenter, 0);
+	Vec3 top_extent = Vec3::sReplicate(scale_xz * mTopRadius);
+	Vec3 top_center = inCenterOfMassTransform * Vec3(0, scale_y * mTopCenter, 0);
+	Vec3 p1 = Vec3::sMin(top_center - top_extent, bottom_center - bottom_extent);
+	Vec3 p2 = Vec3::sMax(top_center + top_extent, bottom_center + bottom_extent);
+	return AABox(p1, p2);
+}
+
+void TaperedCapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
+{
+	JPH_ASSERT(IsValidScale(inScale));
+
+	Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation();
+
+	// Get scaled tapered capsule
+	Vec3 abs_scale = inScale.Abs();
+	float scale_y = abs_scale.GetY();
+	float scale_xz = abs_scale.GetX();
+	Vec3 scale_y_flip(1, Sign(inScale.GetY()), 1);
+	Vec3 scaled_top_center(0, scale_y * mTopCenter, 0);
+	Vec3 scaled_bottom_center(0, scale_y * mBottomCenter, 0);
+	float scaled_top_radius = scale_xz * mTopRadius;
+	float scaled_bottom_radius = scale_xz * mBottomRadius;
+
+	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
+		if (v.GetInvMass() > 0.0f)
+		{
+			Vec3 local_pos = scale_y_flip * (inverse_transform * v.GetPosition());
+
+			Vec3 position, normal;
+
+			// If the vertex is inside the cone starting at the top center pointing along the y-axis with angle PI/2 - alpha then the closest point is on the top sphere
+			// This corresponds to: Dot(y-axis, (local_pos - top_center) / |local_pos - top_center|) >= cos(PI/2 - alpha)
+			// <=> (local_pos - top_center).y >= sin(alpha) * |local_pos - top_center|
+			Vec3 top_center_to_local_pos = local_pos - scaled_top_center;
+			float top_center_to_local_pos_len = top_center_to_local_pos.Length();
+			if (top_center_to_local_pos.GetY() >= mSinAlpha * top_center_to_local_pos_len)
+			{
+				// Top sphere
+				normal = top_center_to_local_pos_len != 0.0f? top_center_to_local_pos / top_center_to_local_pos_len : Vec3::sAxisY();
+				position = scaled_top_center + scaled_top_radius * normal;
+			}
+			else
+			{
+				// If the vertex is outside the cone starting at the bottom center pointing along the y-axis with angle PI/2 - alpha then the closest point is on the bottom sphere
+				// This corresponds to: Dot(y-axis, (local_pos - bottom_center) / |local_pos - bottom_center|) <= cos(PI/2 - alpha)
+				// <=> (local_pos - bottom_center).y <= sin(alpha) * |local_pos - bottom_center|
+				Vec3 bottom_center_to_local_pos = local_pos - scaled_bottom_center;
+				float bottom_center_to_local_pos_len = bottom_center_to_local_pos.Length();
+				if (bottom_center_to_local_pos.GetY() <= mSinAlpha * bottom_center_to_local_pos_len)
+				{
+					// Bottom sphere
+					normal = bottom_center_to_local_pos_len != 0.0f? bottom_center_to_local_pos / bottom_center_to_local_pos_len : -Vec3::sAxisY();
+				}
+				else
+				{
+					// Tapered cylinder
+					normal = Vec3(local_pos.GetX(), 0, local_pos.GetZ()).NormalizedOr(Vec3::sAxisX());
+					normal.SetY(mTanAlpha);
+					normal = normal.NormalizedOr(Vec3::sAxisX());
+				}
+				position = scaled_bottom_center + scaled_bottom_radius * normal;
+			}
+
+			Plane plane = Plane::sFromPointAndNormal(position, normal);
+			float penetration = -plane.SignedDistance(local_pos);
+			if (v.UpdatePenetration(penetration))
+			{
+				// Need to flip the normal's y if capsule is flipped (this corresponds to flipping both the point and the normal around y)
+				plane.SetNormal(scale_y_flip * plane.GetNormal());
+
+				// Store collision
+				v.SetCollision(plane.GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex);
+			}
+		}
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void TaperedCapsuleShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	if (mGeometry == nullptr)
+	{
+		SupportBuffer buffer;
+		const Support *support = GetSupportFunction(ESupportMode::IncludeConvexRadius, buffer, Vec3::sReplicate(1.0f));
+		mGeometry = inRenderer->CreateTriangleGeometryForConvex([support](Vec3Arg inDirection) { return support->GetSupport(inDirection); });
+	}
+
+	// Preserve flip along y axis but make sure we're not inside out
+	Vec3 scale = ScaleHelpers::IsInsideOut(inScale)? Vec3(-1, 1, 1) * inScale : inScale;
+	RMat44 world_transform = inCenterOfMassTransform * Mat44::sScale(scale);
+
+	AABox bounds = Shape::GetWorldSpaceBounds(inCenterOfMassTransform, inScale);
+
+	float lod_scale_sq = Square(max(mTopRadius, mBottomRadius));
+
+	Color color = inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor;
+
+	DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid;
+
+	inRenderer->DrawGeometry(world_transform, bounds, lod_scale_sq, color, mGeometry, DebugRenderer::ECullMode::CullBackFace, DebugRenderer::ECastShadow::On, draw_mode);
+}
+#endif // JPH_DEBUG_RENDERER
+
+AABox TaperedCapsuleShape::GetInertiaApproximation() const
+{
+	// TODO: For now the mass and inertia is that of a box
+	float avg_radius = 0.5f * (mTopRadius + mBottomRadius);
+	return AABox(Vec3(-avg_radius, mBottomCenter - mBottomRadius, -avg_radius), Vec3(avg_radius, mTopCenter + mTopRadius, avg_radius));
+}
+
+void TaperedCapsuleShape::SaveBinaryState(StreamOut &inStream) const
+{
+	ConvexShape::SaveBinaryState(inStream);
+
+	inStream.Write(mCenterOfMass);
+	inStream.Write(mTopRadius);
+	inStream.Write(mBottomRadius);
+	inStream.Write(mTopCenter);
+	inStream.Write(mBottomCenter);
+	inStream.Write(mConvexRadius);
+	inStream.Write(mSinAlpha);
+	inStream.Write(mTanAlpha);
+}
+
+void TaperedCapsuleShape::RestoreBinaryState(StreamIn &inStream)
+{
+	ConvexShape::RestoreBinaryState(inStream);
+
+	inStream.Read(mCenterOfMass);
+	inStream.Read(mTopRadius);
+	inStream.Read(mBottomRadius);
+	inStream.Read(mTopCenter);
+	inStream.Read(mBottomCenter);
+	inStream.Read(mConvexRadius);
+	inStream.Read(mSinAlpha);
+	inStream.Read(mTanAlpha);
+}
+
+bool TaperedCapsuleShape::IsValidScale(Vec3Arg inScale) const
+{
+	return ConvexShape::IsValidScale(inScale) && ScaleHelpers::IsUniformScale(inScale.Abs());
+}
+
+Vec3 TaperedCapsuleShape::MakeScaleValid(Vec3Arg inScale) const
+{
+	Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale);
+
+	return scale.GetSign() * ScaleHelpers::MakeUniformScale(scale.Abs());
+}
+
+void TaperedCapsuleShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::TaperedCapsule);
+	f.mConstruct = []() -> Shape * { return new TaperedCapsuleShape; };
+	f.mColor = Color::sGreen;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.gliffy b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.gliffy
new file mode 100644
index 0000000000..3e5221b865
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.gliffy
@@ -0,0 +1 @@
+{"contentType":"application/gliffy+json","version":"1.1","metadata":{"title":"untitled","revision":0,"exportBorder":false},"embeddedResources":{"index":0,"resources":[]},"stage":{"objects":[{"x":870,"y":406,"rotation":0,"id":62,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":62,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":"1.0,1.0","startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[1.5,5.5],[1.5,-46.196228102251325]],"lockSegments":{}}},"children":null,"constraints":{"constraints":[],"startConstraint":{"type":"StartPositionConstraint","StartPositionConstraint":{"nodeId":4,"px":0.5,"py":0.5}}},"linkMap":[]},{"x":410,"y":406,"rotation":0,"id":60,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":60,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":"1.0,1.0","startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,5.5],[0,-49.03668490108288]],"lockSegments":{}}},"children":null,"constraints":{"constraints":[],"startConstraint":{"type":"StartPositionConstraint","StartPositionConstraint":{"nodeId":0,"px":0.5,"py":0.5}}},"linkMap":[]},{"x":614,"y":385,"rotation":0,"id":58,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":58,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":2,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[-204.06126531020038,0]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":626,"y":385,"rotation":0,"id":57,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":57,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":2,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[248.0020161208372,0]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":818,"y":520,"rotation":0,"id":55,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":55,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":2,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[-48,76]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":830,"y":502,"rotation":0,"id":54,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":54,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":2,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[48,-82]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":373,"y":410,"rotation":0,"id":50,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":28,"height":23,"lockAspectRatio":false,"lockShape":false,"order":19,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"<p style=\"text-align:center;\"><span style=\"font-size: 12px; font-family: Arial; white-space: pre-wrap; line-height: 14px; color: rgb(0, 0, 0);\">tx</span></p>","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":387,"y":392,"rotation":0,"id":49,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":26,"height":23,"lockAspectRatio":false,"lockShape":false,"order":18,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"<p style=\"text-align:center;\"><span style=\"font-size: 12px; font-family: Arial; white-space: pre-wrap; line-height: 14px; color: rgb(0, 0, 0);\">ty</span></p>","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":722,"y":488,"rotation":0,"id":45,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":20,"height":20,"lockAspectRatio":false,"lockShape":false,"order":16,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"<p style=\"text-align:center;\"><span style=\"font-size: 12px; font-family: Arial; white-space: pre-wrap; line-height: 14px; color: rgb(0, 0, 0);\">bx</span></p>","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":440,"y":411.5,"rotation":0,"id":40,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":29,"height":28.500000000000004,"lockAspectRatio":false,"lockShape":false,"order":13,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"<p style=\"text-align:center;\"><span style=\"font-size: 12px; font-family: Arial; white-space: pre-wrap; line-height: 14px; color: rgb(0, 0, 0);\">α</span></p>","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":411,"y":414,"rotation":0,"id":34,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":12,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":"1.0,1.0","startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[-1,-2.5],[349,180]],"lockSegments":{}}},"children":null,"constraints":{"constraints":[],"startConstraint":{"type":"StartPositionConstraint","StartPositionConstraint":{"nodeId":0,"px":0.5,"py":0.5}}},"linkMap":[]},{"x":744,"y":613,"rotation":0,"id":32,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":11,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":"1.0,1.0","startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[-1.1368683772161603e-13,-201.00995000248122]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":394,"y":436,"rotation":0,"id":30,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":10,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":"1.0,1.0","startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[0,-26.076809620810593]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":607,"y":368.5,"rotation":0,"id":26,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":20,"height":30,"lockAspectRatio":false,"lockShape":false,"order":9,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"<p style=\"text-align:center;\"><span style=\"font-size: 12px; font-family: Arial; white-space: pre-wrap; line-height: 14px; color: rgb(0, 0, 0);\">h</span></p>","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":401,"y":412.5,"rotation":0,"id":25,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":20,"height":30,"lockAspectRatio":false,"lockShape":false,"order":8,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"<p style=\"text-align:center;\"><span style=\"font-size: 12px; font-family: Arial; white-space: pre-wrap; line-height: 14px; color: rgb(0, 0, 0);\">tr</span></p>","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":808.5,"y":500,"rotation":0,"id":23,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":49,"height":27,"lockAspectRatio":false,"lockShape":false,"order":7,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"<p style=\"text-align:center;\"><span style=\"font-size: 12px; font-family: Arial; white-space: pre-wrap; line-height: 14px; color: rgb(0, 0, 0);\">br - tr</span></p>","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":402,"y":416,"rotation":0,"id":21,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":6,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[8,-4.5],[-7,21]],"lockSegments":{}}},"children":null,"constraints":{"constraints":[],"startConstraint":{"type":"StartPositionConstraint","StartPositionConstraint":{"nodeId":0,"px":0.5,"py":0.5}}},"linkMap":[]},{"x":347,"y":412,"rotation":0,"id":16,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":5,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[523.5,2.5]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":854,"y":433,"rotation":0,"id":14,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":4,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[17.5,-21.5],[-106,184]],"lockSegments":{}}},"children":null,"constraints":{"constraints":[],"startConstraint":{"type":"StartPositionConstraint","StartPositionConstraint":{"nodeId":4,"px":0.5,"py":0.5}}},"linkMap":[]},{"x":395,"y":437,"rotation":0,"id":9,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":3,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[-52,-25],[695,360]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":395,"y":384,"rotation":0,"id":7,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":2,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[-49,26],[703,-359]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":630,"y":169.99999999999997,"rotation":0,"id":4,"uid":"com.gliffy.shape.basic.basic_v1.default.circle","width":483.00000000000006,"height":483.00000000000006,"lockAspectRatio":true,"lockShape":false,"order":1,"graphic":{"type":"Shape","Shape":{"tid":"com.gliffy.stencil.ellipse.basic_v1","strokeWidth":2,"strokeColor":"#333333","fillColor":"#FFFFFF","gradient":false,"dropShadow":false,"state":0,"shadowX":0,"shadowY":0,"opacity":1}},"children":[],"linkMap":[]},{"x":380,"y":381.5,"rotation":0,"id":0,"uid":"com.gliffy.shape.basic.basic_v1.default.circle","width":60,"height":60,"lockAspectRatio":true,"lockShape":false,"order":0,"graphic":{"type":"Shape","Shape":{"tid":"com.gliffy.stencil.ellipse.basic_v1","strokeWidth":2,"strokeColor":"#333333","fillColor":"#FFFFFF","gradient":false,"dropShadow":false,"state":0,"shadowX":0,"shadowY":0,"opacity":1}},"children":[],"linkMap":[]},{"x":728,"y":612,"rotation":0,"id":41,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":29,"height":28.500000000000004,"lockAspectRatio":false,"lockShape":false,"order":14,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"<p style=\"text-align:center;\"><span style=\"font-size: 12px; font-family: Arial; white-space: pre-wrap; line-height: 14px; color: rgb(0, 0, 0);\">α</span></p>","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":375,"y":432.5,"rotation":0,"id":42,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":29,"height":28.500000000000004,"lockAspectRatio":false,"lockShape":false,"order":15,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"<p style=\"text-align:center;\"><span style=\"font-size: 12px; font-family: Arial; white-space: pre-wrap; line-height: 14px; color: rgb(0, 0, 0);\">α</span></p>","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":753,"y":595.5,"rotation":0,"id":53,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":20,"height":30,"lockAspectRatio":false,"lockShape":false,"order":20,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"<p style=\"text-align:center;\"><span style=\"font-size: 12px; font-family: Arial; white-space: pre-wrap; line-height: 14px; color: rgb(0, 0, 0);\">tr</span></p>","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":794,"y":400,"rotation":0,"id":65,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":65,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":2,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[-48.01041553663117,0]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":790,"y":393.25,"rotation":0,"id":46,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":29,"height":14,"lockAspectRatio":false,"lockShape":false,"order":17,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"<p style=\"text-align:center;\"><span style=\"font-size: 12px; font-family: Arial; white-space: pre-wrap; line-height: 14px; color: rgb(0, 0, 0);\">by</span></p>","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":818,"y":400,"rotation":0,"id":66,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":66,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":2,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[54.230987451824944,0]],"lockSegments":{}}},"children":null,"linkMap":[]}],"background":"#FFFFFF","width":1113,"height":802,"maxWidth":5000,"maxHeight":5000,"nodeIndex":69,"autoFit":true,"exportBorder":false,"gridOn":true,"snapToGrid":false,"drawingGuidesOn":false,"pageBreaksOn":false,"printGridOn":false,"printPaper":"LETTER","printShrinkToFit":false,"printPortrait":true,"shapeStyles":{"com.gliffy.shape.basic.basic_v1.default":{"fill":"#FFFFFF","stroke":"#333333","strokeWidth":2}},"lineStyles":{"global":{"dashStyle":null,"endArrow":2,"startArrow":0}},"textStyles":{},"themeData":null}}
\ No newline at end of file
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h
new file mode 100644
index 0000000000..5e53ee1360
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h
@@ -0,0 +1,135 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/ConvexShape.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that constructs a TaperedCapsuleShape
+class JPH_EXPORT TaperedCapsuleShapeSettings final : public ConvexShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, TaperedCapsuleShapeSettings)
+
+public:
+	/// Default constructor for deserialization
+							TaperedCapsuleShapeSettings() = default;
+
+	/// Create a tapered capsule centered around the origin with one sphere cap at (0, -inHalfHeightOfTaperedCylinder, 0) with radius inBottomRadius and the other at (0, inHalfHeightOfTaperedCylinder, 0) with radius inTopRadius
+							TaperedCapsuleShapeSettings(float inHalfHeightOfTaperedCylinder, float inTopRadius, float inBottomRadius, const PhysicsMaterial *inMaterial = nullptr);
+
+	/// Check if the settings are valid
+	bool					IsValid() const															{ return mTopRadius > 0.0f && mBottomRadius > 0.0f && mHalfHeightOfTaperedCylinder >= 0.0f; }
+
+	/// Checks if the settings of this tapered capsule make this shape a sphere
+	bool					IsSphere() const;
+
+	// See: ShapeSettings
+	virtual ShapeResult		Create() const override;
+
+	float					mHalfHeightOfTaperedCylinder = 0.0f;
+	float					mTopRadius = 0.0f;
+	float					mBottomRadius = 0.0f;
+};
+
+/// A capsule with different top and bottom radii
+class JPH_EXPORT TaperedCapsuleShape final : public ConvexShape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+							TaperedCapsuleShape() : ConvexShape(EShapeSubType::TaperedCapsule) { }
+							TaperedCapsuleShape(const TaperedCapsuleShapeSettings &inSettings, ShapeResult &outResult);
+
+	/// Get top radius of the tapered capsule
+	inline float			GetTopRadius() const													{ return mTopRadius; }
+
+	/// Get bottom radius of the tapered capsule
+	inline float			GetBottomRadius() const													{ return mBottomRadius; }
+
+	/// Get half height between the top and bottom sphere center
+	inline float			GetHalfHeight() const													{ return 0.5f * (mTopCenter - mBottomCenter); }
+
+	// See Shape::GetCenterOfMass
+	virtual Vec3			GetCenterOfMass() const override										{ return mCenterOfMass; }
+
+	// See Shape::GetLocalBounds
+	virtual AABox			GetLocalBounds() const override;
+
+	// See Shape::GetWorldSpaceBounds
+	virtual AABox			GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+	using Shape::GetWorldSpaceBounds;
+
+	// See Shape::GetInnerRadius
+	virtual float			GetInnerRadius() const override											{ return min(mTopRadius, mBottomRadius); }
+
+	// See Shape::GetMassProperties
+	virtual MassProperties	GetMassProperties() const override;
+
+	// See Shape::GetSurfaceNormal
+	virtual Vec3			GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override;
+
+	// See Shape::GetSupportingFace
+	virtual void			GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
+
+	// See ConvexShape::GetSupportFunction
+	virtual const Support *	GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override;
+
+	// See: Shape::CollideSoftBodyVertices
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
+
+#ifdef JPH_DEBUG_RENDERER
+	// See Shape::Draw
+	virtual void			Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// See Shape
+	virtual void			SaveBinaryState(StreamOut &inStream) const override;
+
+	// See Shape::GetStats
+	virtual Stats			GetStats() const override												{ return Stats(sizeof(*this), 0); }
+
+	// See Shape::GetVolume
+	virtual float			GetVolume() const override												{ return GetLocalBounds().GetVolume(); } // Volume is approximate!
+
+	// See Shape::IsValidScale
+	virtual bool			IsValidScale(Vec3Arg inScale) const override;
+
+	// See Shape::MakeScaleValid
+	virtual Vec3			MakeScaleValid(Vec3Arg inScale) const override;
+
+	// Register shape functions with the registry
+	static void				sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void			RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	// Class for GetSupportFunction
+	class					TaperedCapsule;
+
+	/// Returns box that approximates the inertia
+	AABox					GetInertiaApproximation() const;
+
+	Vec3					mCenterOfMass = Vec3::sZero();
+	float					mTopRadius = 0.0f;
+	float					mBottomRadius = 0.0f;
+	float					mTopCenter = 0.0f;
+	float					mBottomCenter = 0.0f;
+	float					mConvexRadius = 0.0f;
+	float					mSinAlpha = 0.0f;
+	float					mTanAlpha = 0.0f;
+
+#ifdef JPH_DEBUG_RENDERER
+	mutable DebugRenderer::GeometryRef mGeometry;
+#endif // JPH_DEBUG_RENDERER
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCylinderShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCylinderShape.cpp
new file mode 100644
index 0000000000..d817644fb4
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCylinderShape.cpp
@@ -0,0 +1,687 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/Shape/TaperedCylinderShape.h>
+#include <Jolt/Physics/Collision/Shape/CylinderShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+// Approximation of a face of the tapered cylinder
+static const Vec3 cTaperedCylinderFace[] =
+{
+	Vec3(0.0f,			0.0f,	1.0f),
+	Vec3(0.707106769f,	0.0f,	0.707106769f),
+	Vec3(1.0f,			0.0f,	0.0f),
+	Vec3(0.707106769f,	0.0f,	-0.707106769f),
+	Vec3(-0.0f,			0.0f,	-1.0f),
+	Vec3(-0.707106769f,	0.0f,	-0.707106769f),
+	Vec3(-1.0f,			0.0f,	0.0f),
+	Vec3(-0.707106769f,	0.0f,	0.707106769f)
+};
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(TaperedCylinderShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(TaperedCylinderShapeSettings, ConvexShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(TaperedCylinderShapeSettings, mHalfHeight)
+	JPH_ADD_ATTRIBUTE(TaperedCylinderShapeSettings, mTopRadius)
+	JPH_ADD_ATTRIBUTE(TaperedCylinderShapeSettings, mBottomRadius)
+	JPH_ADD_ATTRIBUTE(TaperedCylinderShapeSettings, mConvexRadius)
+}
+
+ShapeSettings::ShapeResult TaperedCylinderShapeSettings::Create() const
+{
+	if (mCachedResult.IsEmpty())
+	{
+		Ref<Shape> shape;
+		if (mTopRadius == mBottomRadius)
+		{
+			// Convert to regular cylinder
+			CylinderShapeSettings settings;
+			settings.mHalfHeight = mHalfHeight;
+			settings.mRadius = mTopRadius;
+			settings.mMaterial = mMaterial;
+			settings.mConvexRadius = mConvexRadius;
+			new CylinderShape(settings, mCachedResult);
+		}
+		else
+		{
+			// Normal tapered cylinder shape
+			new TaperedCylinderShape(*this, mCachedResult);
+		}
+	}
+	return mCachedResult;
+}
+
+TaperedCylinderShapeSettings::TaperedCylinderShapeSettings(float inHalfHeightOfTaperedCylinder, float inTopRadius, float inBottomRadius, float inConvexRadius, const PhysicsMaterial *inMaterial) :
+	ConvexShapeSettings(inMaterial),
+	mHalfHeight(inHalfHeightOfTaperedCylinder),
+	mTopRadius(inTopRadius),
+	mBottomRadius(inBottomRadius),
+	mConvexRadius(inConvexRadius)
+{
+}
+
+TaperedCylinderShape::TaperedCylinderShape(const TaperedCylinderShapeSettings &inSettings, ShapeResult &outResult) :
+	ConvexShape(EShapeSubType::TaperedCylinder, inSettings, outResult),
+	mTopRadius(inSettings.mTopRadius),
+	mBottomRadius(inSettings.mBottomRadius),
+	mConvexRadius(inSettings.mConvexRadius)
+{
+	if (mTopRadius < 0.0f)
+	{
+		outResult.SetError("Invalid top radius");
+		return;
+	}
+
+	if (mBottomRadius < 0.0f)
+	{
+		outResult.SetError("Invalid bottom radius");
+		return;
+	}
+
+	if (inSettings.mHalfHeight <= 0.0f)
+	{
+		outResult.SetError("Invalid height");
+		return;
+	}
+
+	if (inSettings.mConvexRadius < 0.0f)
+	{
+		outResult.SetError("Invalid convex radius");
+		return;
+	}
+
+	if (inSettings.mTopRadius < inSettings.mConvexRadius)
+	{
+		outResult.SetError("Convex radius must be smaller than convex radius");
+		return;
+	}
+
+	if (inSettings.mBottomRadius < inSettings.mConvexRadius)
+	{
+		outResult.SetError("Convex radius must be smaller than bottom radius");
+		return;
+	}
+
+	// Calculate the center of mass (using wxMaxima).
+	// Radius of cross section for tapered cylinder from 0 to h:
+	// r(x):=br+x*(tr-br)/h;
+	// Area:
+	// area(x):=%pi*r(x)^2;
+	// Total volume of cylinder:
+	// volume(h):=integrate(area(x),x,0,h);
+	// Center of mass:
+	// com(br,tr,h):=integrate(x*area(x),x,0,h)/volume(h);
+	// Results:
+	// ratsimp(com(br,tr,h),br,bt);
+	// Non-tapered cylinder should have com = 0.5:
+	// ratsimp(com(r,r,h));
+	// Cone with tip at origin and height h should have com = 3/4 h
+	// ratsimp(com(0,r,h));
+	float h = 2.0f * inSettings.mHalfHeight;
+	float tr = mTopRadius;
+	float tr2 = Square(tr);
+	float br = mBottomRadius;
+	float br2 = Square(br);
+	float com = h * (3 * tr2 + 2 * br * tr + br2) / (4.0f * (tr2 + br * tr + br2));
+	mTop = h - com;
+	mBottom = -com;
+
+	outResult.Set(this);
+}
+
+class TaperedCylinderShape::TaperedCylinder final : public Support
+{
+public:
+					TaperedCylinder(float inTop, float inBottom, float inTopRadius, float inBottomRadius, float inConvexRadius) :
+		mTop(inTop),
+		mBottom(inBottom),
+		mTopRadius(inTopRadius),
+		mBottomRadius(inBottomRadius),
+		mConvexRadius(inConvexRadius)
+	{
+		static_assert(sizeof(TaperedCylinder) <= sizeof(SupportBuffer), "Buffer size too small");
+		JPH_ASSERT(IsAligned(this, alignof(TaperedCylinder)));
+	}
+
+	virtual Vec3	GetSupport(Vec3Arg inDirection) const override
+	{
+		float x = inDirection.GetX(), y = inDirection.GetY(), z = inDirection.GetZ();
+		float o = sqrt(Square(x) + Square(z));
+		if (o > 0.0f)
+		{
+			Vec3 top_support((mTopRadius * x) / o, mTop, (mTopRadius * z) / o);
+			Vec3 bottom_support((mBottomRadius * x) / o, mBottom, (mBottomRadius * z) / o);
+			return inDirection.Dot(top_support) > inDirection.Dot(bottom_support)? top_support : bottom_support;
+		}
+		else
+		{
+			if (y > 0.0f)
+				return Vec3(0, mTop, 0);
+			else
+				return Vec3(0, mBottom, 0);
+		}
+	}
+
+	virtual float	GetConvexRadius() const override
+	{
+		return mConvexRadius;
+	}
+
+private:
+	float			mTop;
+	float			mBottom;
+	float			mTopRadius;
+	float			mBottomRadius;
+	float			mConvexRadius;
+};
+
+JPH_INLINE void TaperedCylinderShape::GetScaled(Vec3Arg inScale, float &outTop, float &outBottom, float &outTopRadius, float &outBottomRadius, float &outConvexRadius) const
+{
+	Vec3 abs_scale = inScale.Abs();
+	float scale_xz = abs_scale.GetX();
+	float scale_y = inScale.GetY();
+
+	outTop = scale_y * mTop;
+	outBottom = scale_y * mBottom;
+	outTopRadius = scale_xz * mTopRadius;
+	outBottomRadius = scale_xz * mBottomRadius;
+	outConvexRadius = min(abs_scale.GetY(), scale_xz) * mConvexRadius;
+
+	// Negative Y-scale flips the top and bottom
+	if (outBottom > outTop)
+	{
+		std::swap(outTop, outBottom);
+		std::swap(outTopRadius, outBottomRadius);
+	}
+}
+
+const ConvexShape::Support *TaperedCylinderShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const
+{
+	JPH_ASSERT(IsValidScale(inScale));
+
+	// Get scaled tapered cylinder
+	float top, bottom, top_radius, bottom_radius, convex_radius;
+	GetScaled(inScale, top, bottom, top_radius, bottom_radius, convex_radius);
+
+	switch (inMode)
+	{
+	case ESupportMode::IncludeConvexRadius:
+	case ESupportMode::Default:
+		return new (&inBuffer) TaperedCylinder(top, bottom, top_radius, bottom_radius, 0.0f);
+
+	case ESupportMode::ExcludeConvexRadius:
+		return new (&inBuffer) TaperedCylinder(top - convex_radius, bottom + convex_radius, top_radius - convex_radius, bottom_radius - convex_radius, convex_radius);
+	}
+
+	JPH_ASSERT(false);
+	return nullptr;
+}
+
+JPH_INLINE static Vec3 sCalculateSideNormalXZ(Vec3Arg inSurfacePosition)
+{
+	return (Vec3(1, 0, 1) * inSurfacePosition).NormalizedOr(Vec3::sAxisX());
+}
+
+JPH_INLINE static Vec3 sCalculateSideNormal(Vec3Arg inNormalXZ, float inTop, float inBottom, float inTopRadius, float inBottomRadius)
+{
+	float tan_alpha = (inBottomRadius - inTopRadius) / (inTop - inBottom);
+	return Vec3(inNormalXZ.GetX(), tan_alpha, inNormalXZ.GetZ()).Normalized();
+}
+
+void TaperedCylinderShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
+	JPH_ASSERT(IsValidScale(inScale));
+
+	// Get scaled tapered cylinder
+	float top, bottom, top_radius, bottom_radius, convex_radius;
+	GetScaled(inScale, top, bottom, top_radius, bottom_radius, convex_radius);
+
+	// Get the normal of the side of the cylinder
+	Vec3 normal_xz = sCalculateSideNormalXZ(-inDirection);
+	Vec3 normal = sCalculateSideNormal(normal_xz, top, bottom, top_radius, bottom_radius);
+
+	constexpr float cMinRadius = 1.0e-3f;
+
+	// Check if the normal is closer to the side than to the top or bottom
+	if (abs(normal.Dot(inDirection)) > abs(inDirection.GetY()))
+	{
+		// Return the side of the cylinder
+		outVertices.push_back(inCenterOfMassTransform * (normal_xz * top_radius + Vec3(0, top, 0)));
+		outVertices.push_back(inCenterOfMassTransform * (normal_xz * bottom_radius + Vec3(0, bottom, 0)));
+	}
+	else if (inDirection.GetY() < 0.0f)
+	{
+		// Top of the cylinder
+		if (top_radius > cMinRadius)
+		{
+			Vec3 top_3d(0, top, 0);
+			for (Vec3 v : cTaperedCylinderFace)
+				outVertices.push_back(inCenterOfMassTransform * (top_radius * v + top_3d));
+		}
+	}
+	else
+	{
+		// Bottom of the cylinder
+		if (bottom_radius > cMinRadius)
+		{
+			Vec3 bottom_3d(0, bottom, 0);
+			for (const Vec3 *v = cTaperedCylinderFace + std::size(cTaperedCylinderFace) - 1; v >= cTaperedCylinderFace; --v)
+				outVertices.push_back(inCenterOfMassTransform * (bottom_radius * *v + bottom_3d));
+		}
+	}
+}
+
+MassProperties TaperedCylinderShape::GetMassProperties() const
+{
+	MassProperties p;
+
+	// Calculate mass
+	float density = GetDensity();
+	p.mMass = GetVolume() * density;
+
+	// Calculate inertia of a tapered cylinder (using wxMaxima)
+	// Radius:
+	// r(x):=br+(x-b)*(tr-br)/(t-b);
+	// Where t=top, b=bottom, tr=top radius, br=bottom radius
+	// Area of the cross section of the cylinder at x:
+	// area(x):=%pi*r(x)^2;
+	// Inertia x slice at x (using inertia of a solid disc, see https://en.wikipedia.org/wiki/List_of_moments_of_inertia, note needs to be multiplied by density):
+	// dix(x):=area(x)*r(x)^2/4;
+	// Inertia y slice at y (note needs to be multiplied by density)
+	// diy(x):=area(x)*r(x)^2/2;
+	// Volume:
+	// volume(b,t):=integrate(area(x),x,b,t);
+	// The constant density (note that we have this through GetDensity() so we'll use that instead):
+	// density(b,t):=m/volume(b,t);
+	// Inertia tensor element xx, note that we use the parallel axis theorem to move the inertia: Ixx' = Ixx + m translation^2, also note we multiply by density here:
+	// Ixx(br,tr,b,t):=integrate(dix(x)+area(x)*x^2,x,b,t)*density(b,t);
+	// Inertia tensor element yy:
+	// Iyy(br,tr,b,t):=integrate(diy(x),x,b,t)*density(b,t);
+	// Note that we can simplfy Ixx by using:
+	// Ixx_delta(br,tr,b,t):=Ixx(br,tr,b,t)-Iyy(br,tr,b,t)/2;
+	// For a cylinder this formula matches what is listed on the wiki:
+	// factor(Ixx(r,r,-h/2,h/2));
+	// factor(Iyy(r,r,-h/2,h/2));
+	// For a cone with tip at origin too:
+	// factor(Ixx(0,r,0,h));
+	// factor(Iyy(0,r,0,h));
+	// Now for the tapered cylinder:
+	// rat(Ixx(br,tr,b,t),br,bt);
+	// rat(Iyy(br,tr,b,t),br,bt);
+	// rat(Ixx_delta(br,tr,b,t),br,bt);
+	float t = mTop;
+	float t2 = Square(t);
+	float t3 = t * t2;
+
+	float b = mBottom;
+	float b2 = Square(b);
+	float b3 = b * b2;
+
+	float br = mBottomRadius;
+	float br2 = Square(br);
+	float br3 = br * br2;
+	float br4 = Square(br2);
+
+	float tr = mTopRadius;
+	float tr2 = Square(tr);
+	float tr3 = tr * tr2;
+	float tr4 = Square(tr2);
+
+	float inertia_y = (JPH_PI / 10.0f) * density * (t - b) * (br4 + tr * br3 + tr2 * br2 + tr3 * br + tr4);
+	float inertia_x_delta = (JPH_PI / 30.0f) * density * ((t3 + 2 * b * t2 + 3 * b2 * t - 6 * b3) * br2 + (3 * t3 + b * t2 - b2 * t - 3 * b3) * tr * br + (6 * t3 - 3 * b * t2 - 2 * b2 * t - b3) * tr2);
+	float inertia_x = inertia_x_delta + inertia_y / 2;
+	float inertia_z = inertia_x;
+	p.mInertia = Mat44::sScale(Vec3(inertia_x, inertia_y, inertia_z));
+	return p;
+}
+
+Vec3 TaperedCylinderShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
+
+	constexpr float cEpsilon = 1.0e-5f;
+
+	if (inLocalSurfacePosition.GetY() > mTop - cEpsilon)
+		return Vec3(0, 1, 0);
+	else if (inLocalSurfacePosition.GetY() < mBottom + cEpsilon)
+		return Vec3(0, -1, 0);
+	else
+		return sCalculateSideNormal(sCalculateSideNormalXZ(inLocalSurfacePosition), mTop, mBottom, mTopRadius, mBottomRadius);
+}
+
+AABox TaperedCylinderShape::GetLocalBounds() const
+{
+	float max_radius = max(mTopRadius, mBottomRadius);
+	return AABox(Vec3(-max_radius, mBottom, -max_radius), Vec3(max_radius, mTop, max_radius));
+}
+
+void TaperedCylinderShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	// Check if the point is in the tapered cylinder
+	if (inPoint.GetY() >= mBottom && inPoint.GetY() <= mTop // Within height
+		&& Square(inPoint.GetX()) + Square(inPoint.GetZ()) <= Square(mBottomRadius + (inPoint.GetY() - mBottom) * (mTopRadius - mBottomRadius) / (mTop - mBottom))) // Within the radius
+		ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
+}
+
+void TaperedCylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
+{
+	JPH_ASSERT(IsValidScale(inScale));
+
+	Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation();
+
+	// Get scaled tapered cylinder
+	float top, bottom, top_radius, bottom_radius, convex_radius;
+	GetScaled(inScale, top, bottom, top_radius, bottom_radius, convex_radius);
+	Vec3 top_3d(0, top, 0);
+	Vec3 bottom_3d(0, bottom, 0);
+
+	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
+		if (v.GetInvMass() > 0.0f)
+		{
+			Vec3 local_pos = inverse_transform * v.GetPosition();
+
+			// Calculate penetration into side surface
+			Vec3 normal_xz = sCalculateSideNormalXZ(local_pos);
+			Vec3 side_normal = sCalculateSideNormal(normal_xz, top, bottom, top_radius, bottom_radius);
+			Vec3 side_support_top = normal_xz * top_radius + top_3d;
+			float side_penetration = (side_support_top - local_pos).Dot(side_normal);
+
+			// Calculate penetration into top and bottom plane
+			float top_penetration = top - local_pos.GetY();
+			float bottom_penetration = local_pos.GetY() - bottom;
+			float min_top_bottom_penetration = min(top_penetration, bottom_penetration);
+
+			Vec3 point, normal;
+			if (side_penetration < 0.0f || min_top_bottom_penetration < 0.0f)
+			{
+				// We're outside the cylinder
+				// Calculate the closest point on the line segment from bottom to top support point:
+				// closest_point = bottom + fraction * (top - bottom) / |top - bottom|^2
+				Vec3 side_support_bottom = normal_xz * bottom_radius + bottom_3d;
+				Vec3 bottom_to_top = side_support_top - side_support_bottom;
+				float fraction = (local_pos - side_support_bottom).Dot(bottom_to_top);
+
+				// Calculate the distance to the axis of the cylinder
+				float distance_to_axis = normal_xz.Dot(local_pos);
+				bool inside_top_radius = distance_to_axis <= top_radius;
+				bool inside_bottom_radius = distance_to_axis <= bottom_radius;
+
+				/*
+					Regions of tapered cylinder (side view):
+
+						_  B |       |
+						 --_ |   A   |
+							 t-------+
+					   C    /         \
+						   /  tapered  \
+					_     /  cylinder   \
+					 --_ /               \
+						b-----------------+
+					 D  |        E        |
+						|                 |
+
+					t = side_support_top, b = side_support_bottom
+					Lines between B and C and C and D are at a 90 degree angle to the line between t and b
+				*/
+				if (fraction >= bottom_to_top.LengthSq() // Region B: Above the line segment
+					&& !inside_top_radius) // Outside the top radius
+				{
+					// Top support point is closest
+					point = side_support_top;
+					normal = (local_pos - point).NormalizedOr(Vec3::sAxisY());
+				}
+				else if (fraction < 0.0f // Region D: Below the line segment
+					&& !inside_bottom_radius) // Outside the bottom radius
+				{
+					// Bottom support point is closest
+					point = side_support_bottom;
+					normal = (local_pos - point).NormalizedOr(Vec3::sAxisY());
+				}
+				else if (top_penetration < 0.0f // Region A: Above the top plane
+					&& inside_top_radius) // Inside the top radius
+				{
+					// Top plane is closest
+					point = top_3d;
+					normal = Vec3(0, 1, 0);
+				}
+				else if (bottom_penetration < 0.0f // Region E: Below the bottom plane
+					&& inside_bottom_radius) // Inside the bottom radius
+				{
+					// Bottom plane is closest
+					point = bottom_3d;
+					normal = Vec3(0, -1, 0);
+				}
+				else // Region C
+				{
+					// Side surface is closest
+					point = side_support_top;
+					normal = side_normal;
+				}
+			}
+			else if (side_penetration < min_top_bottom_penetration)
+			{
+				// Side surface is closest
+				point = side_support_top;
+				normal = side_normal;
+			}
+			else if (top_penetration < bottom_penetration)
+			{
+				// Top plane is closest
+				point = top_3d;
+				normal = Vec3(0, 1, 0);
+			}
+			else
+			{
+				// Bottom plane is closest
+				point = bottom_3d;
+				normal = Vec3(0, -1, 0);
+			}
+
+			// Calculate penetration
+			Plane plane = Plane::sFromPointAndNormal(point, normal);
+			float penetration = -plane.SignedDistance(local_pos);
+			if (v.UpdatePenetration(penetration))
+				v.SetCollision(plane.GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex);
+		}
+}
+
+class TaperedCylinderShape::TCSGetTrianglesContext
+{
+public:
+	explicit	TCSGetTrianglesContext(Mat44Arg inTransform) : mTransform(inTransform) { }
+
+	Mat44		mTransform;
+	uint		mProcessed = 0; // Which elements we processed, bit 0 = top, bit 1 = bottom, bit 2 = side
+};
+
+void TaperedCylinderShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const
+{
+	static_assert(sizeof(TCSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small");
+	JPH_ASSERT(IsAligned(&ioContext, alignof(TCSGetTrianglesContext)));
+
+	// Make sure the scale is not inside out
+	Vec3 scale = ScaleHelpers::IsInsideOut(inScale)? Vec3(-1, 1, 1) * inScale : inScale;
+
+	// Mark top and bottom processed if their radius is too small
+	TCSGetTrianglesContext *context = new (&ioContext) TCSGetTrianglesContext(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(scale));
+	constexpr float cMinRadius = 1.0e-3f;
+	if (mTopRadius < cMinRadius)
+		context->mProcessed |= 0b001;
+	if (mBottomRadius < cMinRadius)
+		context->mProcessed |= 0b010;
+}
+
+int TaperedCylinderShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const
+{
+	constexpr int cNumVertices = int(std::size(cTaperedCylinderFace));
+
+	static_assert(cGetTrianglesMinTrianglesRequested >= 2 * cNumVertices);
+	JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested);
+
+	TCSGetTrianglesContext &context = (TCSGetTrianglesContext &)ioContext;
+
+	int total_num_triangles = 0;
+
+	// Top cap
+	Vec3 top_3d(0, mTop, 0);
+	if ((context.mProcessed & 0b001) == 0)
+	{
+		Vec3 v0 = context.mTransform * (top_3d + mTopRadius * cTaperedCylinderFace[0]);
+		Vec3 v1 = context.mTransform * (top_3d + mTopRadius * cTaperedCylinderFace[1]);
+
+		for (const Vec3 *v = cTaperedCylinderFace + 2, *v_end = cTaperedCylinderFace + cNumVertices; v < v_end; ++v)
+		{
+			Vec3 v2 = context.mTransform * (top_3d + mTopRadius * *v);
+
+			v0.StoreFloat3(outTriangleVertices++);
+			v1.StoreFloat3(outTriangleVertices++);
+			v2.StoreFloat3(outTriangleVertices++);
+
+			v1 = v2;
+		}
+
+		total_num_triangles = cNumVertices - 2;
+		context.mProcessed |= 0b001;
+	}
+
+	// Bottom cap
+	Vec3 bottom_3d(0, mBottom, 0);
+	if ((context.mProcessed & 0b010) == 0
+		&& total_num_triangles + cNumVertices - 2 < inMaxTrianglesRequested)
+	{
+		Vec3 v0 = context.mTransform * (bottom_3d + mBottomRadius * cTaperedCylinderFace[0]);
+		Vec3 v1 = context.mTransform * (bottom_3d + mBottomRadius * cTaperedCylinderFace[1]);
+
+		for (const Vec3 *v = cTaperedCylinderFace + 2, *v_end = cTaperedCylinderFace + cNumVertices; v < v_end; ++v)
+		{
+			Vec3 v2 = context.mTransform * (bottom_3d + mBottomRadius * *v);
+
+			v0.StoreFloat3(outTriangleVertices++);
+			v2.StoreFloat3(outTriangleVertices++);
+			v1.StoreFloat3(outTriangleVertices++);
+
+			v1 = v2;
+		}
+
+		total_num_triangles += cNumVertices - 2;
+		context.mProcessed |= 0b010;
+	}
+
+	// Side
+	if ((context.mProcessed & 0b100) == 0
+		&& total_num_triangles + 2 * cNumVertices < inMaxTrianglesRequested)
+	{
+		Vec3 v0t = context.mTransform * (top_3d + mTopRadius * cTaperedCylinderFace[cNumVertices - 1]);
+		Vec3 v0b = context.mTransform * (bottom_3d + mBottomRadius * cTaperedCylinderFace[cNumVertices - 1]);
+
+		for (const Vec3 *v = cTaperedCylinderFace, *v_end = cTaperedCylinderFace + cNumVertices; v < v_end; ++v)
+		{
+			Vec3 v1t = context.mTransform * (top_3d + mTopRadius * *v);
+			v0t.StoreFloat3(outTriangleVertices++);
+			v0b.StoreFloat3(outTriangleVertices++);
+			v1t.StoreFloat3(outTriangleVertices++);
+
+			Vec3 v1b = context.mTransform * (bottom_3d + mBottomRadius * *v);
+			v1t.StoreFloat3(outTriangleVertices++);
+			v0b.StoreFloat3(outTriangleVertices++);
+			v1b.StoreFloat3(outTriangleVertices++);
+
+			v0t = v1t;
+			v0b = v1b;
+		}
+
+		total_num_triangles += 2 * cNumVertices;
+		context.mProcessed |= 0b100;
+	}
+
+	// Store materials
+	if (outMaterials != nullptr)
+	{
+		const PhysicsMaterial *material = GetMaterial();
+		for (const PhysicsMaterial **m = outMaterials, **m_end = outMaterials + total_num_triangles; m < m_end; ++m)
+			*m = material;
+	}
+
+	return total_num_triangles;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void TaperedCylinderShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	// Preserve flip along y axis but make sure we're not inside out
+	Vec3 scale = ScaleHelpers::IsInsideOut(inScale)? Vec3(-1, 1, 1) * inScale : inScale;
+	RMat44 world_transform = inCenterOfMassTransform * Mat44::sScale(scale);
+
+	DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid;
+	inRenderer->DrawTaperedCylinder(world_transform, mTop, mBottom, mTopRadius, mBottomRadius, inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor, DebugRenderer::ECastShadow::On, draw_mode);
+}
+#endif // JPH_DEBUG_RENDERER
+
+void TaperedCylinderShape::SaveBinaryState(StreamOut &inStream) const
+{
+	ConvexShape::SaveBinaryState(inStream);
+
+	inStream.Write(mTop);
+	inStream.Write(mBottom);
+	inStream.Write(mTopRadius);
+	inStream.Write(mBottomRadius);
+	inStream.Write(mConvexRadius);
+}
+
+void TaperedCylinderShape::RestoreBinaryState(StreamIn &inStream)
+{
+	ConvexShape::RestoreBinaryState(inStream);
+
+	inStream.Read(mTop);
+	inStream.Read(mBottom);
+	inStream.Read(mTopRadius);
+	inStream.Read(mBottomRadius);
+	inStream.Read(mConvexRadius);
+}
+
+float TaperedCylinderShape::GetVolume() const
+{
+	// Volume of a tapered cylinder is: integrate(%pi*(b+x*(t-b)/h)^2,x,0,h) where t is the top radius, b is the bottom radius and h is the height
+	return (JPH_PI / 3.0f) * (mTop - mBottom) * (Square(mTopRadius) + mTopRadius * mBottomRadius + Square(mBottomRadius));
+}
+
+bool TaperedCylinderShape::IsValidScale(Vec3Arg inScale) const
+{
+	return ConvexShape::IsValidScale(inScale) && ScaleHelpers::IsUniformScaleXZ(inScale.Abs());
+}
+
+Vec3 TaperedCylinderShape::MakeScaleValid(Vec3Arg inScale) const
+{
+	Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale);
+
+	return scale.GetSign() * ScaleHelpers::MakeUniformScaleXZ(scale.Abs());
+}
+
+void TaperedCylinderShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::TaperedCylinder);
+	f.mConstruct = []() -> Shape * { return new TaperedCylinderShape; };
+	f.mColor = Color::sGreen;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCylinderShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCylinderShape.h
new file mode 100644
index 0000000000..e48ca93d0f
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TaperedCylinderShape.h
@@ -0,0 +1,132 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/ConvexShape.h>
+#include <Jolt/Physics/PhysicsSettings.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that constructs a TaperedCylinderShape
+class JPH_EXPORT TaperedCylinderShapeSettings final : public ConvexShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, TaperedCylinderShapeSettings)
+
+public:
+	/// Default constructor for deserialization
+							TaperedCylinderShapeSettings() = default;
+
+	/// Create a tapered cylinder centered around the origin with bottom at (0, -inHalfHeightOfTaperedCylinder, 0) with radius inBottomRadius and top at (0, inHalfHeightOfTaperedCylinder, 0) with radius inTopRadius
+							TaperedCylinderShapeSettings(float inHalfHeightOfTaperedCylinder, float inTopRadius, float inBottomRadius, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr);
+
+	// See: ShapeSettings
+	virtual ShapeResult		Create() const override;
+
+	float					mHalfHeight = 0.0f;
+	float					mTopRadius = 0.0f;
+	float					mBottomRadius = 0.0f;
+	float					mConvexRadius = 0.0f;
+};
+
+/// A cylinder with different top and bottom radii
+class JPH_EXPORT TaperedCylinderShape final : public ConvexShape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+							TaperedCylinderShape() : ConvexShape(EShapeSubType::TaperedCylinder) { }
+							TaperedCylinderShape(const TaperedCylinderShapeSettings &inSettings, ShapeResult &outResult);
+
+	/// Get top radius of the tapered cylinder
+	inline float			GetTopRadius() const													{ return mTopRadius; }
+
+	/// Get bottom radius of the tapered cylinder
+	inline float			GetBottomRadius() const													{ return mBottomRadius; }
+
+	/// Get convex radius of the tapered cylinder
+	inline float			GetConvexRadius() const													{ return mConvexRadius; }
+
+	/// Get half height of the tapered cylinder
+	inline float			GetHalfHeight() const													{ return 0.5f * (mTop - mBottom); }
+
+	// See Shape::GetCenterOfMass
+	virtual Vec3			GetCenterOfMass() const override										{ return Vec3(0, -0.5f * (mTop + mBottom), 0); }
+
+	// See Shape::GetLocalBounds
+	virtual AABox			GetLocalBounds() const override;
+
+	// See Shape::GetInnerRadius
+	virtual float			GetInnerRadius() const override											{ return min(mTopRadius, mBottomRadius); }
+
+	// See Shape::GetMassProperties
+	virtual MassProperties	GetMassProperties() const override;
+
+	// See Shape::GetSurfaceNormal
+	virtual Vec3			GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override;
+
+	// See Shape::GetSupportingFace
+	virtual void			GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
+
+	// See ConvexShape::GetSupportFunction
+	virtual const Support *	GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override;
+
+	// See: Shape::CollidePoint
+	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollideSoftBodyVertices
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
+
+	// See Shape::GetTrianglesStart
+	virtual void			GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
+
+	// See Shape::GetTrianglesNext
+	virtual int				GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override;
+
+#ifdef JPH_DEBUG_RENDERER
+	// See Shape::Draw
+	virtual void			Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// See Shape
+	virtual void			SaveBinaryState(StreamOut &inStream) const override;
+
+	// See Shape::GetStats
+	virtual Stats			GetStats() const override												{ return Stats(sizeof(*this), 0); }
+
+	// See Shape::GetVolume
+	virtual float			GetVolume() const override;
+
+	// See Shape::IsValidScale
+	virtual bool			IsValidScale(Vec3Arg inScale) const override;
+
+	// See Shape::MakeScaleValid
+	virtual Vec3			MakeScaleValid(Vec3Arg inScale) const override;
+
+	// Register shape functions with the registry
+	static void				sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void			RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	// Class for GetSupportFunction
+	class					TaperedCylinder;
+
+	// Class for GetTrianglesTart
+	class					TCSGetTrianglesContext;
+
+	// Scale the cylinder
+	JPH_INLINE void			GetScaled(Vec3Arg inScale, float &outTop, float &outBottom, float &outTopRadius, float &outBottomRadius, float &outConvexRadius) const;
+
+	float					mTop = 0.0f;
+	float					mBottom = 0.0f;
+	float					mTopRadius = 0.0f;
+	float					mBottomRadius = 0.0f;
+	float					mConvexRadius = 0.0f;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TriangleShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TriangleShape.cpp
new file mode 100644
index 0000000000..5f2454bbb5
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TriangleShape.cpp
@@ -0,0 +1,423 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/Shape/TriangleShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
+#include <Jolt/Physics/Collision/Shape/GetTrianglesContext.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/CastConvexVsTriangles.h>
+#include <Jolt/Physics/Collision/CastSphereVsTriangles.h>
+#include <Jolt/Physics/Collision/CollideConvexVsTriangles.h>
+#include <Jolt/Physics/Collision/CollideSphereVsTriangles.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h>
+#include <Jolt/Geometry/ConvexSupport.h>
+#include <Jolt/Geometry/RayTriangle.h>
+#include <Jolt/Geometry/ClosestPoint.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(TriangleShapeSettings)
+{
+	JPH_ADD_BASE_CLASS(TriangleShapeSettings, ConvexShapeSettings)
+
+	JPH_ADD_ATTRIBUTE(TriangleShapeSettings, mV1)
+	JPH_ADD_ATTRIBUTE(TriangleShapeSettings, mV2)
+	JPH_ADD_ATTRIBUTE(TriangleShapeSettings, mV3)
+	JPH_ADD_ATTRIBUTE(TriangleShapeSettings, mConvexRadius)
+}
+
+ShapeSettings::ShapeResult TriangleShapeSettings::Create() const
+{
+	if (mCachedResult.IsEmpty())
+		Ref<Shape> shape = new TriangleShape(*this, mCachedResult);
+	return mCachedResult;
+}
+
+TriangleShape::TriangleShape(const TriangleShapeSettings &inSettings, ShapeResult &outResult) :
+	ConvexShape(EShapeSubType::Triangle, inSettings, outResult),
+	mV1(inSettings.mV1),
+	mV2(inSettings.mV2),
+	mV3(inSettings.mV3),
+	mConvexRadius(inSettings.mConvexRadius)
+{
+	if (inSettings.mConvexRadius < 0.0f)
+	{
+		outResult.SetError("Invalid convex radius");
+		return;
+	}
+
+	outResult.Set(this);
+}
+
+AABox TriangleShape::GetLocalBounds() const
+{
+	AABox bounds(mV1, mV1);
+	bounds.Encapsulate(mV2);
+	bounds.Encapsulate(mV3);
+	bounds.ExpandBy(Vec3::sReplicate(mConvexRadius));
+	return bounds;
+}
+
+AABox TriangleShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
+	JPH_ASSERT(IsValidScale(inScale));
+
+	Vec3 v1 = inCenterOfMassTransform * (inScale * mV1);
+	Vec3 v2 = inCenterOfMassTransform * (inScale * mV2);
+	Vec3 v3 = inCenterOfMassTransform * (inScale * mV3);
+
+	AABox bounds(v1, v1);
+	bounds.Encapsulate(v2);
+	bounds.Encapsulate(v3);
+	bounds.ExpandBy(inScale * mConvexRadius);
+	return bounds;
+}
+
+class TriangleShape::TriangleNoConvex final : public Support
+{
+public:
+							TriangleNoConvex(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3) :
+		mTriangleSuport(inV1, inV2, inV3)
+	{
+		static_assert(sizeof(TriangleNoConvex) <= sizeof(SupportBuffer), "Buffer size too small");
+		JPH_ASSERT(IsAligned(this, alignof(TriangleNoConvex)));
+	}
+
+	virtual Vec3			GetSupport(Vec3Arg inDirection) const override
+	{
+		return mTriangleSuport.GetSupport(inDirection);
+	}
+
+	virtual float			GetConvexRadius() const override
+	{
+		return 0.0f;
+	}
+
+private:
+	TriangleConvexSupport	mTriangleSuport;
+};
+
+class TriangleShape::TriangleWithConvex final : public Support
+{
+public:
+							TriangleWithConvex(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, float inConvexRadius) :
+		mConvexRadius(inConvexRadius),
+		mTriangleSuport(inV1, inV2, inV3)
+	{
+		static_assert(sizeof(TriangleWithConvex) <= sizeof(SupportBuffer), "Buffer size too small");
+		JPH_ASSERT(IsAligned(this, alignof(TriangleWithConvex)));
+	}
+
+	virtual Vec3			GetSupport(Vec3Arg inDirection) const override
+	{
+		Vec3 support = mTriangleSuport.GetSupport(inDirection);
+		float len = inDirection.Length();
+		if (len > 0.0f)
+			support += (mConvexRadius / len) * inDirection;
+		return support;
+	}
+
+	virtual float			GetConvexRadius() const override
+	{
+		return mConvexRadius;
+	}
+
+private:
+	float					mConvexRadius;
+	TriangleConvexSupport	mTriangleSuport;
+};
+
+const ConvexShape::Support *TriangleShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const
+{
+	switch (inMode)
+	{
+	case ESupportMode::IncludeConvexRadius:
+	case ESupportMode::Default:
+		if (mConvexRadius > 0.0f)
+			return new (&inBuffer) TriangleWithConvex(inScale * mV1, inScale * mV2, inScale * mV3, mConvexRadius);
+		[[fallthrough]];
+
+	case ESupportMode::ExcludeConvexRadius:
+		return new (&inBuffer) TriangleNoConvex(inScale * mV1, inScale * mV2, inScale * mV3);
+	}
+
+	JPH_ASSERT(false);
+	return nullptr;
+}
+
+void TriangleShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
+
+	// Calculate transform with scale
+	Mat44 transform = inCenterOfMassTransform.PreScaled(inScale);
+
+	// Flip triangle if scaled inside out
+	if (ScaleHelpers::IsInsideOut(inScale))
+	{
+		outVertices.push_back(transform * mV1);
+		outVertices.push_back(transform * mV3);
+		outVertices.push_back(transform * mV2);
+	}
+	else
+	{
+		outVertices.push_back(transform * mV1);
+		outVertices.push_back(transform * mV2);
+		outVertices.push_back(transform * mV3);
+	}
+}
+
+MassProperties TriangleShape::GetMassProperties() const
+{
+	// We cannot calculate the volume for a triangle, so we return invalid mass properties.
+	// If you want your triangle to be dynamic, then you should provide the mass properties yourself when
+	// creating a Body:
+	//
+	// BodyCreationSettings::mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided;
+	// BodyCreationSettings::mMassPropertiesOverride.SetMassAndInertiaOfSolidBox(Vec3::sReplicate(1.0f), 1000.0f);
+	//
+	// Note that this makes the triangle shape behave the same as a mesh shape with a single triangle.
+	// In practice there is very little use for a dynamic triangle shape as back side collisions will be ignored
+	// so if the triangle falls the wrong way it will sink through the floor.
+	return MassProperties();
+}
+
+Vec3 TriangleShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
+
+	Vec3 cross = (mV2 - mV1).Cross(mV3 - mV1);
+	float len = cross.Length();
+	return len != 0.0f? cross / len : Vec3::sAxisY();
+}
+
+void TriangleShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const
+{
+	// A triangle has no volume
+	outTotalVolume = outSubmergedVolume = 0.0f;
+	outCenterOfBuoyancy = Vec3::sZero();
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void TriangleShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	RVec3 v1 = inCenterOfMassTransform * (inScale * mV1);
+	RVec3 v2 = inCenterOfMassTransform * (inScale * mV2);
+	RVec3 v3 = inCenterOfMassTransform * (inScale * mV3);
+
+	if (ScaleHelpers::IsInsideOut(inScale))
+		std::swap(v1, v2);
+
+	if (inDrawWireframe)
+		inRenderer->DrawWireTriangle(v1, v2, v3, inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor);
+	else
+		inRenderer->DrawTriangle(v1, v2, v3, inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor);
+}
+#endif // JPH_DEBUG_RENDERER
+
+bool TriangleShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	float fraction = RayTriangle(inRay.mOrigin, inRay.mDirection, mV1, mV2, mV3);
+	if (fraction < ioHit.mFraction)
+	{
+		ioHit.mFraction = fraction;
+		ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID();
+		return true;
+	}
+	return false;
+}
+
+void TriangleShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	// Back facing check
+	if (inRayCastSettings.mBackFaceModeTriangles == EBackFaceMode::IgnoreBackFaces && (mV2 - mV1).Cross(mV3 - mV1).Dot(inRay.mDirection) > 0.0f)
+		return;
+
+	// Test ray against triangle
+	float fraction = RayTriangle(inRay.mOrigin, inRay.mDirection, mV1, mV2, mV3);
+	if (fraction < ioCollector.GetEarlyOutFraction())
+	{
+		// Better hit than the current hit
+		RayCastResult hit;
+		hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext());
+		hit.mFraction = fraction;
+		hit.mSubShapeID2 = inSubShapeIDCreator.GetID();
+		ioCollector.AddHit(hit);
+	}
+}
+
+void TriangleShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	// Can't be inside a triangle
+}
+
+void TriangleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
+{
+	CollideSoftBodyVerticesVsTriangles collider(inCenterOfMassTransform, inScale);
+
+	for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
+		if (v.GetInvMass() > 0.0f)
+		{
+			collider.StartVertex(v);
+			collider.ProcessTriangle(mV1, mV2, mV3);
+			collider.FinishVertex(v, inCollidingShapeIndex);
+		}
+}
+
+void TriangleShape::sCollideConvexVsTriangle(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter)
+{
+	JPH_ASSERT(inShape1->GetType() == EShapeType::Convex);
+	const ConvexShape *shape1 = static_cast<const ConvexShape *>(inShape1);
+	JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::Triangle);
+	const TriangleShape *shape2 = static_cast<const TriangleShape *>(inShape2);
+
+	CollideConvexVsTriangles collider(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector);
+	collider.Collide(shape2->mV1, shape2->mV2, shape2->mV3, 0b111, inSubShapeIDCreator2.GetID());
+}
+
+void TriangleShape::sCollideSphereVsTriangle(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter)
+{
+	JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::Sphere);
+	const SphereShape *shape1 = static_cast<const SphereShape *>(inShape1);
+	JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::Triangle);
+	const TriangleShape *shape2 = static_cast<const TriangleShape *>(inShape2);
+
+	CollideSphereVsTriangles collider(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector);
+	collider.Collide(shape2->mV1, shape2->mV2, shape2->mV3, 0b111, inSubShapeIDCreator2.GetID());
+}
+
+void TriangleShape::sCastConvexVsTriangle(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::Triangle);
+	const TriangleShape *shape = static_cast<const TriangleShape *>(inShape);
+
+	CastConvexVsTriangles caster(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector);
+	caster.Cast(shape->mV1, shape->mV2, shape->mV3, 0b111, inSubShapeIDCreator2.GetID());
+}
+
+void TriangleShape::sCastSphereVsTriangle(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::Triangle);
+	const TriangleShape *shape = static_cast<const TriangleShape *>(inShape);
+
+	CastSphereVsTriangles caster(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector);
+	caster.Cast(shape->mV1, shape->mV2, shape->mV3, 0b111, inSubShapeIDCreator2.GetID());
+}
+
+class TriangleShape::TSGetTrianglesContext
+{
+public:
+					TSGetTrianglesContext(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3) : mV1(inV1), mV2(inV2), mV3(inV3) { }
+
+	Vec3			mV1;
+	Vec3			mV2;
+	Vec3			mV3;
+
+	bool			mIsDone = false;
+};
+
+void TriangleShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const
+{
+	static_assert(sizeof(TSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small");
+	JPH_ASSERT(IsAligned(&ioContext, alignof(TSGetTrianglesContext)));
+
+	Mat44 m = Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale);
+
+	new (&ioContext) TSGetTrianglesContext(m * mV1, m * mV2, m * mV3);
+}
+
+int TriangleShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const
+{
+	static_assert(cGetTrianglesMinTrianglesRequested >= 3, "cGetTrianglesMinTrianglesRequested is too small");
+	JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested);
+
+	TSGetTrianglesContext &context = (TSGetTrianglesContext &)ioContext;
+
+	// Only return the triangle the 1st time
+	if (context.mIsDone)
+		return 0;
+	context.mIsDone = true;
+
+	// Store triangle
+	context.mV1.StoreFloat3(outTriangleVertices);
+	context.mV2.StoreFloat3(outTriangleVertices + 1);
+	context.mV3.StoreFloat3(outTriangleVertices + 2);
+
+	// Store material
+	if (outMaterials != nullptr)
+		*outMaterials = GetMaterial();
+
+	return 1;
+}
+
+void TriangleShape::SaveBinaryState(StreamOut &inStream) const
+{
+	ConvexShape::SaveBinaryState(inStream);
+
+	inStream.Write(mV1);
+	inStream.Write(mV2);
+	inStream.Write(mV3);
+	inStream.Write(mConvexRadius);
+}
+
+void TriangleShape::RestoreBinaryState(StreamIn &inStream)
+{
+	ConvexShape::RestoreBinaryState(inStream);
+
+	inStream.Read(mV1);
+	inStream.Read(mV2);
+	inStream.Read(mV3);
+	inStream.Read(mConvexRadius);
+}
+
+bool TriangleShape::IsValidScale(Vec3Arg inScale) const
+{
+	return ConvexShape::IsValidScale(inScale) && (mConvexRadius == 0.0f || ScaleHelpers::IsUniformScale(inScale.Abs()));
+}
+
+Vec3 TriangleShape::MakeScaleValid(Vec3Arg inScale) const
+{
+	Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale);
+
+	if (mConvexRadius == 0.0f)
+		return scale;
+
+	return scale.GetSign() * ScaleHelpers::MakeUniformScale(scale.Abs());
+}
+
+void TriangleShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Triangle);
+	f.mConstruct = []() -> Shape * { return new TriangleShape; };
+	f.mColor = Color::sGreen;
+
+	for (EShapeSubType s : sConvexSubShapeTypes)
+	{
+		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::Triangle, sCollideConvexVsTriangle);
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Triangle, sCastConvexVsTriangle);
+	}
+
+	// Specialized collision functions
+	CollisionDispatch::sRegisterCollideShape(EShapeSubType::Sphere, EShapeSubType::Triangle, sCollideSphereVsTriangle);
+	CollisionDispatch::sRegisterCastShape(EShapeSubType::Sphere, EShapeSubType::Triangle, sCastSphereVsTriangle);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TriangleShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TriangleShape.h
new file mode 100644
index 0000000000..b56ccc1f69
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/TriangleShape.h
@@ -0,0 +1,143 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/ConvexShape.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that constructs a TriangleShape
+class JPH_EXPORT TriangleShapeSettings final : public ConvexShapeSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, TriangleShapeSettings)
+
+public:
+	/// Default constructor for deserialization
+							TriangleShapeSettings() = default;
+
+	/// Create a triangle with points (inV1, inV2, inV3) (counter clockwise) and convex radius inConvexRadius.
+	/// Note that the convex radius is currently only used for shape vs shape collision, for all other purposes the triangle is infinitely thin.
+							TriangleShapeSettings(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, float inConvexRadius = 0.0f, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mV1(inV1), mV2(inV2), mV3(inV3), mConvexRadius(inConvexRadius) { }
+
+	// See: ShapeSettings
+	virtual ShapeResult		Create() const override;
+
+	Vec3					mV1;
+	Vec3					mV2;
+	Vec3					mV3;
+	float					mConvexRadius = 0.0f;
+};
+
+/// A single triangle, not the most efficient way of creating a world filled with triangles but can be used as a query shape for example.
+class JPH_EXPORT TriangleShape final : public ConvexShape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+							TriangleShape() : ConvexShape(EShapeSubType::Triangle) { }
+							TriangleShape(const TriangleShapeSettings &inSettings, ShapeResult &outResult);
+
+	/// Create a triangle with points (inV1, inV2, inV3) (counter clockwise) and convex radius inConvexRadius.
+	/// Note that the convex radius is currently only used for shape vs shape collision, for all other purposes the triangle is infinitely thin.
+							TriangleShape(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, float inConvexRadius = 0.0f, const PhysicsMaterial *inMaterial = nullptr) : ConvexShape(EShapeSubType::Triangle, inMaterial), mV1(inV1), mV2(inV2), mV3(inV3), mConvexRadius(inConvexRadius) { JPH_ASSERT(inConvexRadius >= 0.0f); }
+
+	/// Get the vertices of the triangle
+	inline Vec3				GetVertex1() const																	{ return mV1; }
+	inline Vec3				GetVertex2() const																	{ return mV2; }
+	inline Vec3				GetVertex3() const																	{ return mV3; }
+
+	/// Convex radius
+	float					GetConvexRadius() const																{ return mConvexRadius; }
+
+	// See Shape::GetLocalBounds
+	virtual AABox			GetLocalBounds() const override;
+
+	// See Shape::GetWorldSpaceBounds
+	virtual AABox			GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+	using Shape::GetWorldSpaceBounds;
+
+	// See Shape::GetInnerRadius
+	virtual float			GetInnerRadius() const override														{ return mConvexRadius; }
+
+	// See Shape::GetMassProperties
+	virtual MassProperties	GetMassProperties() const override;
+
+	// See Shape::GetSurfaceNormal
+	virtual Vec3			GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override;
+
+	// See Shape::GetSupportingFace
+	virtual void			GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
+
+	// See ConvexShape::GetSupportFunction
+	virtual const Support *	GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override;
+
+	// See Shape::GetSubmergedVolume
+	virtual void			GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override;
+
+#ifdef JPH_DEBUG_RENDERER
+	// See Shape::Draw
+	virtual void			Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// See Shape::CastRay
+	virtual bool			CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+	virtual void			CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollidePoint
+	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+
+	// See: Shape::CollideSoftBodyVertices
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
+
+	// See Shape::GetTrianglesStart
+	virtual void			GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
+
+	// See Shape::GetTrianglesNext
+	virtual int				GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override;
+
+	// See Shape
+	virtual void			SaveBinaryState(StreamOut &inStream) const override;
+
+	// See Shape::GetStats
+	virtual Stats			GetStats() const override															{ return Stats(sizeof(*this), 1); }
+
+	// See Shape::GetVolume
+	virtual float			GetVolume() const override															{ return 0; }
+
+	// See Shape::IsValidScale
+	virtual bool			IsValidScale(Vec3Arg inScale) const override;
+
+	// See Shape::MakeScaleValid
+	virtual Vec3			MakeScaleValid(Vec3Arg inScale) const override;
+
+	// Register shape functions with the registry
+	static void				sRegister();
+
+protected:
+	// See: Shape::RestoreBinaryState
+	virtual void			RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	// Helper functions called by CollisionDispatch
+	static void				sCollideConvexVsTriangle(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void				sCollideSphereVsTriangle(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void				sCastConvexVsTriangle(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+	static void				sCastSphereVsTriangle(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+
+	// Context for GetTrianglesStart/Next
+	class					TSGetTrianglesContext;
+
+	// Classes for GetSupportFunction
+	class					TriangleNoConvex;
+	class					TriangleWithConvex;
+
+	Vec3					mV1;
+	Vec3					mV2;
+	Vec3					mV3;
+	float					mConvexRadius = 0.0f;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ShapeCast.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/ShapeCast.h
new file mode 100644
index 0000000000..4271ac06f7
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ShapeCast.h
@@ -0,0 +1,173 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/AABox.h>
+#include <Jolt/Physics/Collision/CollideShape.h>
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Structure that holds a single shape cast (a shape moving along a linear path in 3d space with no rotation)
+template <class Vec, class Mat, class ShapeCastType>
+struct ShapeCastT
+{
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+								ShapeCastT(const Shape *inShape, Vec3Arg inScale, typename Mat::ArgType inCenterOfMassStart, Vec3Arg inDirection, const AABox &inWorldSpaceBounds) :
+		mShape(inShape),
+		mScale(inScale),
+		mCenterOfMassStart(inCenterOfMassStart),
+		mDirection(inDirection),
+		mShapeWorldBounds(inWorldSpaceBounds)
+	{
+	}
+
+	/// Constructor
+								ShapeCastT(const Shape *inShape, Vec3Arg inScale, typename Mat::ArgType inCenterOfMassStart, Vec3Arg inDirection) :
+		ShapeCastT<Vec, Mat, ShapeCastType>(inShape, inScale, inCenterOfMassStart, inDirection, inShape->GetWorldSpaceBounds(inCenterOfMassStart, inScale))
+	{
+	}
+
+	/// Construct a shape cast using a world transform for a shape instead of a center of mass transform
+	static inline ShapeCastType	sFromWorldTransform(const Shape *inShape, Vec3Arg inScale, typename Mat::ArgType inWorldTransform, Vec3Arg inDirection)
+	{
+		return ShapeCastType(inShape, inScale, inWorldTransform.PreTranslated(inShape->GetCenterOfMass()), inDirection);
+	}
+
+	/// Transform this shape cast using inTransform. Multiply transform on the left left hand side.
+	ShapeCastType				PostTransformed(typename Mat::ArgType inTransform) const
+	{
+		Mat44 start = inTransform * mCenterOfMassStart;
+		Vec3 direction = inTransform.Multiply3x3(mDirection);
+		return { mShape, mScale, start, direction };
+	}
+
+	/// Translate this shape cast by inTranslation.
+	ShapeCastType				PostTranslated(typename Vec::ArgType inTranslation) const
+	{
+		return { mShape, mScale, mCenterOfMassStart.PostTranslated(inTranslation), mDirection };
+	}
+
+	/// Get point with fraction inFraction on ray from mCenterOfMassStart to mCenterOfMassStart + mDirection (0 = start of ray, 1 = end of ray)
+	inline Vec					GetPointOnRay(float inFraction) const
+	{
+		return mCenterOfMassStart.GetTranslation() + inFraction * mDirection;
+	}
+
+	const Shape *				mShape;								///< Shape that's being cast (cannot be mesh shape). Note that this structure does not assume ownership over the shape for performance reasons.
+	const Vec3					mScale;								///< Scale in local space of the shape being cast (scales relative to its center of mass)
+	const Mat					mCenterOfMassStart;					///< Start position and orientation of the center of mass of the shape (construct using sFromWorldTransform if you have a world transform for your shape)
+	const Vec3					mDirection;							///< Direction and length of the cast (anything beyond this length will not be reported as a hit)
+	const AABox					mShapeWorldBounds;					///< Cached shape's world bounds, calculated in constructor
+};
+
+struct ShapeCast : public ShapeCastT<Vec3, Mat44, ShapeCast>
+{
+	using ShapeCastT<Vec3, Mat44, ShapeCast>::ShapeCastT;
+};
+
+struct RShapeCast : public ShapeCastT<RVec3, RMat44, RShapeCast>
+{
+	using ShapeCastT<RVec3, RMat44, RShapeCast>::ShapeCastT;
+
+	/// Convert from ShapeCast, converts single to double precision
+	explicit					RShapeCast(const ShapeCast &inCast) :
+		RShapeCast(inCast.mShape, inCast.mScale, RMat44(inCast.mCenterOfMassStart), inCast.mDirection, inCast.mShapeWorldBounds)
+	{
+	}
+
+	/// Convert to ShapeCast, which implies casting from double precision to single precision
+	explicit					operator ShapeCast() const
+	{
+		return ShapeCast(mShape, mScale, mCenterOfMassStart.ToMat44(), mDirection, mShapeWorldBounds);
+	}
+};
+
+/// Settings to be passed with a shape cast
+class ShapeCastSettings : public CollideSettingsBase
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Set the backfacing mode for all shapes
+	void						SetBackFaceMode(EBackFaceMode inMode) { mBackFaceModeTriangles = mBackFaceModeConvex = inMode; }
+
+	/// How backfacing triangles should be treated (should we report moving from back to front for triangle based shapes, e.g. for MeshShape/HeightFieldShape?)
+	EBackFaceMode				mBackFaceModeTriangles = EBackFaceMode::IgnoreBackFaces;
+
+	/// How backfacing convex objects should be treated (should we report starting inside an object and moving out?)
+	EBackFaceMode				mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces;
+
+	/// Indicates if we want to shrink the shape by the convex radius and then expand it again. This speeds up collision detection and gives a more accurate normal at the cost of a more 'rounded' shape.
+	bool						mUseShrunkenShapeAndConvexRadius = false;
+
+	/// When true, and the shape is intersecting at the beginning of the cast (fraction = 0) then this will calculate the deepest penetration point (costing additional CPU time)
+	bool						mReturnDeepestPoint = false;
+};
+
+/// Result of a shape cast test
+class ShapeCastResult : public CollideShapeResult
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Default constructor
+								ShapeCastResult() = default;
+
+	/// Constructor
+	/// @param inFraction Fraction at which the cast hit
+	/// @param inContactPoint1 Contact point on shape 1
+	/// @param inContactPoint2 Contact point on shape 2
+	/// @param inContactNormalOrPenetrationDepth Contact normal pointing from shape 1 to 2 or penetration depth vector when the objects are penetrating (also from 1 to 2)
+	/// @param inBackFaceHit If this hit was a back face hit
+	/// @param inSubShapeID1 Sub shape id for shape 1
+	/// @param inSubShapeID2 Sub shape id for shape 2
+	/// @param inBodyID2 BodyID that was hit
+								ShapeCastResult(float inFraction, Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, Vec3Arg inContactNormalOrPenetrationDepth, bool inBackFaceHit, const SubShapeID &inSubShapeID1, const SubShapeID &inSubShapeID2, const BodyID &inBodyID2) :
+		CollideShapeResult(inContactPoint1, inContactPoint2, inContactNormalOrPenetrationDepth, (inContactPoint2 - inContactPoint1).Length(), inSubShapeID1, inSubShapeID2, inBodyID2),
+		mFraction(inFraction),
+		mIsBackFaceHit(inBackFaceHit)
+	{
+	}
+
+	/// Function required by the CollisionCollector. A smaller fraction is considered to be a 'better hit'. For rays/cast shapes we can just use the collision fraction. The fraction and penetration depth are combined in such a way that deeper hits at fraction 0 go first.
+	inline float				GetEarlyOutFraction() const			{ return mFraction > 0.0f? mFraction : -mPenetrationDepth; }
+
+	/// Reverses the hit result, swapping contact point 1 with contact point 2 etc.
+	/// @param inWorldSpaceCastDirection Direction of the shape cast in world space
+	ShapeCastResult				Reversed(Vec3Arg inWorldSpaceCastDirection) const
+	{
+		// Calculate by how much to shift the contact points
+		Vec3 delta = mFraction * inWorldSpaceCastDirection;
+
+		ShapeCastResult result;
+		result.mContactPointOn2 = mContactPointOn1 - delta;
+		result.mContactPointOn1 = mContactPointOn2 - delta;
+		result.mPenetrationAxis = -mPenetrationAxis;
+		result.mPenetrationDepth = mPenetrationDepth;
+		result.mSubShapeID2 = mSubShapeID1;
+		result.mSubShapeID1 = mSubShapeID2;
+		result.mBodyID2 = mBodyID2;
+		result.mFraction = mFraction;
+		result.mIsBackFaceHit = mIsBackFaceHit;
+
+		result.mShape2Face.resize(mShape1Face.size());
+		for (Face::size_type i = 0; i < mShape1Face.size(); ++i)
+			result.mShape2Face[i] = mShape1Face[i] - delta;
+
+		result.mShape1Face.resize(mShape2Face.size());
+		for (Face::size_type i = 0; i < mShape2Face.size(); ++i)
+			result.mShape1Face[i] = mShape2Face[i] - delta;
+
+		return result;
+	}
+
+	float						mFraction;							///< This is the fraction where the shape hit the other shape: CenterOfMassOnHit = Start + value * (End - Start)
+	bool						mIsBackFaceHit;						///< True if the shape was hit from the back side
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/ShapeFilter.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/ShapeFilter.h
new file mode 100644
index 0000000000..3e9f4ff71a
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/ShapeFilter.h
@@ -0,0 +1,73 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/BodyID.h>
+#include <Jolt/Core/NonCopyable.h>
+
+JPH_NAMESPACE_BEGIN
+
+class Shape;
+class SubShapeID;
+
+/// Filter class
+class ShapeFilter : public NonCopyable
+{
+public:
+	/// Destructor
+	virtual					~ShapeFilter() = default;
+
+	/// Filter function to determine if we should collide with a shape. Returns true if the filter passes.
+	/// This overload is called when the query doesn't have a source shape (e.g. ray cast / collide point)
+	/// @param inShape2 Shape we're colliding against
+	/// @param inSubShapeIDOfShape2 The sub shape ID that will lead from the root shape to inShape2 (i.e. the shape of mBodyID2)
+	virtual bool			ShouldCollide([[maybe_unused]] const Shape *inShape2, [[maybe_unused]] const SubShapeID &inSubShapeIDOfShape2) const
+	{
+		return true;
+	}
+
+	/// Filter function to determine if two shapes should collide. Returns true if the filter passes.
+	/// This overload is called when querying a shape vs a shape (e.g. collide object / cast object).
+	/// It is called at each level of the shape hierarchy, so if you have a compound shape with a box, this function will be called twice.
+	/// It will not be called on triangles that are part of another shape, i.e a mesh shape will not trigger a callback per triangle. You can filter out individual triangles in the CollisionCollector::AddHit function by their sub shape ID.
+	/// @param inShape1 1st shape that is colliding
+	/// @param inSubShapeIDOfShape1 The sub shape ID that will lead from the root shape to inShape1 (i.e. the shape that is used to collide or cast against shape 2)
+	/// @param inShape2 2nd shape that is colliding
+	/// @param inSubShapeIDOfShape2 The sub shape ID that will lead from the root shape to inShape2 (i.e. the shape of mBodyID2)
+	virtual bool			ShouldCollide([[maybe_unused]] const Shape *inShape1, [[maybe_unused]] const SubShapeID &inSubShapeIDOfShape1, [[maybe_unused]] const Shape *inShape2, [[maybe_unused]] const SubShapeID &inSubShapeIDOfShape2) const
+	{
+		return true;
+	}
+
+	/// Used during NarrowPhase queries and TransformedShape queries. Set to the body ID of inShape2 before calling ShouldCollide.
+	/// Provides context to the filter to indicate which body is colliding.
+	mutable BodyID			mBodyID2;
+};
+
+/// Helper class to reverse the order of the shapes in the ShouldCollide function
+class ReversedShapeFilter : public ShapeFilter
+{
+public:
+	/// Constructor
+	explicit				ReversedShapeFilter(const ShapeFilter &inFilter) : mFilter(inFilter)
+	{
+		mBodyID2 = inFilter.mBodyID2;
+	}
+
+	virtual bool			ShouldCollide(const Shape *inShape2, const SubShapeID &inSubShapeIDOfShape2) const override
+	{
+		return mFilter.ShouldCollide(inShape2, inSubShapeIDOfShape2);
+	}
+
+	virtual bool			ShouldCollide(const Shape *inShape1, const SubShapeID &inSubShapeIDOfShape1, const Shape *inShape2, const SubShapeID &inSubShapeIDOfShape2) const override
+	{
+		return mFilter.ShouldCollide(inShape2, inSubShapeIDOfShape2, inShape1, inSubShapeIDOfShape1);
+	}
+
+private:
+	const ShapeFilter &		mFilter;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/SimShapeFilter.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/SimShapeFilter.h
new file mode 100644
index 0000000000..8c0320a83d
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/SimShapeFilter.h
@@ -0,0 +1,40 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/NonCopyable.h>
+
+JPH_NAMESPACE_BEGIN
+
+class Body;
+class Shape;
+class SubShapeID;
+
+/// Filter class used during the simulation (PhysicsSystem::Update) to filter out collisions at shape level
+class SimShapeFilter : public NonCopyable
+{
+public:
+	/// Destructor
+	virtual					~SimShapeFilter() = default;
+
+	/// Filter function to determine if two shapes should collide. Returns true if the filter passes.
+	/// This overload is called during the simulation (PhysicsSystem::Update) and must be registered with PhysicsSystem::SetSimShapeFilter.
+	/// It is called at each level of the shape hierarchy, so if you have a compound shape with a box, this function will be called twice.
+	/// It will not be called on triangles that are part of another shape, i.e a mesh shape will not trigger a callback per triangle.
+	/// Note that this function is called from multiple threads and must be thread safe. All properties are read only.
+	/// @param inBody1 1st body that is colliding
+	/// @param inShape1 1st shape that is colliding
+	/// @param inSubShapeIDOfShape1 The sub shape ID that will lead from inBody1.GetShape() to inShape1
+	/// @param inBody2 2nd body that is colliding
+	/// @param inShape2 2nd shape that is colliding
+	/// @param inSubShapeIDOfShape2 The sub shape ID that will lead from inBody2.GetShape() to inShape2
+	virtual bool			ShouldCollide([[maybe_unused]] const Body &inBody1, [[maybe_unused]] const Shape *inShape1, [[maybe_unused]] const SubShapeID &inSubShapeIDOfShape1,
+										  [[maybe_unused]] const Body &inBody2, [[maybe_unused]] const Shape *inShape2, [[maybe_unused]] const SubShapeID &inSubShapeIDOfShape2) const
+	{
+		return true;
+	}
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/SimShapeFilterWrapper.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/SimShapeFilterWrapper.h
new file mode 100644
index 0000000000..02e4f0dcfe
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/SimShapeFilterWrapper.h
@@ -0,0 +1,81 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/SimShapeFilter.h>
+#include <Jolt/Physics/Body/Body.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Helper class to forward ShapeFilter calls to a SimShapeFilter
+/// INTERNAL CLASS DO NOT USE!
+class SimShapeFilterWrapper : public ShapeFilter
+{
+public:
+	/// Constructor
+							SimShapeFilterWrapper(const SimShapeFilter *inFilter, const Body *inBody1) :
+		mFilter(inFilter),
+		mBody1(inBody1)
+	{
+	}
+
+	/// Forward to the simulation shape filter
+	virtual bool			ShouldCollide(const Shape *inShape1, const SubShapeID &inSubShapeIDOfShape1, const Shape *inShape2, const SubShapeID &inSubShapeIDOfShape2) const override
+	{
+		return mFilter->ShouldCollide(*mBody1, inShape1, inSubShapeIDOfShape1, *mBody2, inShape2, inSubShapeIDOfShape2);
+	}
+
+	/// Forward to the simulation shape filter
+	virtual bool			ShouldCollide(const Shape *inShape2, const SubShapeID &inSubShapeIDOfShape2) const override
+	{
+		return mFilter->ShouldCollide(*mBody1, mBody1->GetShape(), SubShapeID(), *mBody2, inShape2, inSubShapeIDOfShape2);
+	}
+
+	/// Set the body we're colliding against
+	void					SetBody2(const Body *inBody2)
+	{
+		mBody2 = inBody2;
+	}
+
+private:
+	const SimShapeFilter *	mFilter;
+	const Body *			mBody1;
+	const Body *			mBody2;
+};
+
+/// In case we don't have a simulation shape filter, we fall back to using a default shape filter that always returns true
+/// INTERNAL CLASS DO NOT USE!
+union SimShapeFilterWrapperUnion
+{
+public:
+	/// Constructor
+							SimShapeFilterWrapperUnion(const SimShapeFilter *inFilter, const Body *inBody1)
+	{
+		// Dirty trick: if we don't have a filter, placement new a standard ShapeFilter so that we
+		// don't have to check for nullptr in the ShouldCollide function
+		if (inFilter != nullptr)
+			new (&mSimShapeFilterWrapper) SimShapeFilterWrapper(inFilter, inBody1);
+		else
+			new (&mSimShapeFilterWrapper) ShapeFilter();
+	}
+
+	/// Destructor
+							~SimShapeFilterWrapperUnion()
+	{
+		// Doesn't need to be destructed
+	}
+
+	/// Accessor
+	SimShapeFilterWrapper &	GetSimShapeFilterWrapper()
+	{
+		return mSimShapeFilterWrapper;
+	}
+
+private:
+	SimShapeFilterWrapper	mSimShapeFilterWrapper;
+	ShapeFilter				mShapeFilter;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/SortReverseAndStore.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/SortReverseAndStore.h
new file mode 100644
index 0000000000..4a073096c2
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/SortReverseAndStore.h
@@ -0,0 +1,48 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// This function will sort values from high to low and only keep the ones that are less than inMaxValue
+/// @param inValues Values to be sorted
+/// @param inMaxValue Values need to be less than this to keep them
+/// @param ioIdentifiers 4 identifiers that will be sorted in the same way as the values
+/// @param outValues The values are stored here from high to low
+/// @return The number of values that were kept
+JPH_INLINE int SortReverseAndStore(Vec4Arg inValues, float inMaxValue, UVec4 &ioIdentifiers, float *outValues)
+{
+	// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
+	Vec4 values = inValues;
+	Vec4::sSort4Reverse(values, ioIdentifiers);
+
+	// Count how many results are less than the max value
+	UVec4 closer = Vec4::sLess(values, Vec4::sReplicate(inMaxValue));
+	int num_results = closer.CountTrues();
+
+	// Shift the values so that only the ones that are less than max are kept
+	values = values.ReinterpretAsInt().ShiftComponents4Minus(num_results).ReinterpretAsFloat();
+	ioIdentifiers = ioIdentifiers.ShiftComponents4Minus(num_results);
+
+	// Store the values
+	values.StoreFloat4(reinterpret_cast<Float4 *>(outValues));
+
+	return num_results;
+}
+
+/// Shift the elements so that the identifiers that correspond with the trues in inValue come first
+/// @param inValue Values to test for true or false
+/// @param ioIdentifiers the identifiers that are shifted, on return they are shifted
+/// @return The number of trues
+JPH_INLINE int CountAndSortTrues(UVec4Arg inValue, UVec4 &ioIdentifiers)
+{
+	// Sort the hits
+	ioIdentifiers = UVec4::sSort4True(inValue, ioIdentifiers);
+
+	// Return the amount of hits
+	return inValue.CountTrues();
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/TransformedShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/TransformedShape.cpp
new file mode 100644
index 0000000000..470387b52a
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/TransformedShape.cpp
@@ -0,0 +1,180 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Geometry/OrientedBox.h>
+
+JPH_NAMESPACE_BEGIN
+
+bool TransformedShape::CastRay(const RRayCast &inRay, RayCastResult &ioHit) const
+{
+	if (mShape != nullptr)
+	{
+		// Transform the ray to local space, note that this drops precision which is possible because we're in local space now
+		RayCast ray(inRay.Transformed(GetInverseCenterOfMassTransform()));
+
+		// Scale the ray
+		Vec3 inv_scale = GetShapeScale().Reciprocal();
+		ray.mOrigin *= inv_scale;
+		ray.mDirection *= inv_scale;
+
+		// Cast the ray on the shape
+		SubShapeIDCreator sub_shape_id(mSubShapeIDCreator);
+		if (mShape->CastRay(ray, sub_shape_id, ioHit))
+		{
+			// Set body ID on the hit result
+			ioHit.mBodyID = mBodyID;
+
+			return true;
+		}
+	}
+
+	return false;
+}
+
+void TransformedShape::CastRay(const RRayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	if (mShape != nullptr)
+	{
+		// Set the context on the collector and filter
+		ioCollector.SetContext(this);
+		inShapeFilter.mBodyID2 = mBodyID;
+
+		// Transform the ray to local space, note that this drops precision which is possible because we're in local space now
+		RayCast ray(inRay.Transformed(GetInverseCenterOfMassTransform()));
+
+		// Scale the ray
+		Vec3 inv_scale = GetShapeScale().Reciprocal();
+		ray.mOrigin *= inv_scale;
+		ray.mDirection *= inv_scale;
+
+		// Cast the ray on the shape
+		SubShapeIDCreator sub_shape_id(mSubShapeIDCreator);
+		mShape->CastRay(ray, inRayCastSettings, sub_shape_id, ioCollector, inShapeFilter);
+	}
+}
+
+void TransformedShape::CollidePoint(RVec3Arg inPoint, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	if (mShape != nullptr)
+	{
+		// Set the context on the collector and filter
+		ioCollector.SetContext(this);
+		inShapeFilter.mBodyID2 = mBodyID;
+
+		// Transform and scale the point to local space
+		Vec3 point = Vec3(GetInverseCenterOfMassTransform() * inPoint) / GetShapeScale();
+
+		// Do point collide on the shape
+		SubShapeIDCreator sub_shape_id(mSubShapeIDCreator);
+		mShape->CollidePoint(point, sub_shape_id, ioCollector, inShapeFilter);
+	}
+}
+
+void TransformedShape::CollideShape(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	if (mShape != nullptr)
+	{
+		// Set the context on the collector and filter
+		ioCollector.SetContext(this);
+		inShapeFilter.mBodyID2 = mBodyID;
+
+		SubShapeIDCreator sub_shape_id1, sub_shape_id2(mSubShapeIDCreator);
+		Mat44 transform1 = inCenterOfMassTransform.PostTranslated(-inBaseOffset).ToMat44();
+		Mat44 transform2 = GetCenterOfMassTransform().PostTranslated(-inBaseOffset).ToMat44();
+		CollisionDispatch::sCollideShapeVsShape(inShape, mShape, inShapeScale, GetShapeScale(), transform1, transform2, sub_shape_id1, sub_shape_id2, inCollideShapeSettings, ioCollector, inShapeFilter);
+	}
+}
+
+void TransformedShape::CastShape(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	if (mShape != nullptr)
+	{
+		// Set the context on the collector and filter
+		ioCollector.SetContext(this);
+		inShapeFilter.mBodyID2 = mBodyID;
+
+		// Get the shape cast relative to the base offset and convert it to floats
+		ShapeCast shape_cast(inShapeCast.PostTranslated(-inBaseOffset));
+
+		// Get center of mass of object we're casting against relative to the base offset and convert it to floats
+		Mat44 center_of_mass_transform2 = GetCenterOfMassTransform().PostTranslated(-inBaseOffset).ToMat44();
+
+		SubShapeIDCreator sub_shape_id1, sub_shape_id2(mSubShapeIDCreator);
+		CollisionDispatch::sCastShapeVsShapeWorldSpace(shape_cast, inShapeCastSettings, mShape, GetShapeScale(), inShapeFilter, center_of_mass_transform2, sub_shape_id1, sub_shape_id2, ioCollector);
+	}
+}
+
+void TransformedShape::CollectTransformedShapes(const AABox &inBox, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	if (mShape != nullptr)
+	{
+		struct MyCollector : public TransformedShapeCollector
+		{
+										MyCollector(TransformedShapeCollector &ioCollector, RVec3 inShapePositionCOM) :
+				TransformedShapeCollector(ioCollector),
+				mCollector(ioCollector),
+				mShapePositionCOM(inShapePositionCOM)
+			{
+			}
+
+			virtual void				AddHit(const TransformedShape &inResult) override
+			{
+				// Apply the center of mass offset
+				TransformedShape ts = inResult;
+				ts.mShapePositionCOM += mShapePositionCOM;
+
+				// Pass hit on to child collector
+				mCollector.AddHit(ts);
+
+				// Update early out fraction based on child collector
+				UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
+			}
+
+			TransformedShapeCollector &	mCollector;
+			RVec3						mShapePositionCOM;
+		};
+
+		// Set the context on the collector
+		ioCollector.SetContext(this);
+
+		// Wrap the collector so we can add the center of mass precision, we do this to avoid losing precision because CollectTransformedShapes uses single precision floats
+		MyCollector collector(ioCollector, mShapePositionCOM);
+
+		// Take box to local space for the shape
+		AABox box = inBox;
+		box.Translate(-mShapePositionCOM);
+
+		mShape->CollectTransformedShapes(box, Vec3::sZero(), mShapeRotation, GetShapeScale(), mSubShapeIDCreator, collector, inShapeFilter);
+	}
+}
+
+void TransformedShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, RVec3Arg inBaseOffset) const
+{
+	if (mShape != nullptr)
+	{
+		// Take box to local space for the shape
+		AABox box = inBox;
+		box.Translate(-inBaseOffset);
+
+		mShape->GetTrianglesStart(ioContext, box, Vec3(mShapePositionCOM - inBaseOffset), mShapeRotation, GetShapeScale());
+	}
+}
+
+int TransformedShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const
+{
+	if (mShape != nullptr)
+		return mShape->GetTrianglesNext(ioContext, inMaxTrianglesRequested, outTriangleVertices, outMaterials);
+	else
+		return 0;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/TransformedShape.h b/thirdparty/jolt_physics/Jolt/Physics/Collision/TransformedShape.h
new file mode 100644
index 0000000000..887a8ee441
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/TransformedShape.h
@@ -0,0 +1,194 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/ObjectLayer.h>
+#include <Jolt/Physics/Collision/ShapeFilter.h>
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+#include <Jolt/Physics/Collision/BackFaceMode.h>
+#include <Jolt/Physics/Body/BodyID.h>
+
+JPH_NAMESPACE_BEGIN
+
+struct RRayCast;
+struct RShapeCast;
+class CollideShapeSettings;
+class RayCastResult;
+
+/// Temporary data structure that contains a shape and a transform.
+/// This structure can be obtained from a body (e.g. after a broad phase query) under lock protection.
+/// The lock can then be released and collision detection operations can be safely performed since
+/// the class takes a reference on the shape and does not use anything from the body anymore.
+class JPH_EXPORT TransformedShape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+								TransformedShape() = default;
+								TransformedShape(RVec3Arg inPositionCOM, QuatArg inRotation, const Shape *inShape, const BodyID &inBodyID, const SubShapeIDCreator &inSubShapeIDCreator = SubShapeIDCreator()) : mShapePositionCOM(inPositionCOM), mShapeRotation(inRotation), mShape(inShape), mBodyID(inBodyID), mSubShapeIDCreator(inSubShapeIDCreator) { }
+
+	/// Cast a ray and find the closest hit. Returns true if it finds a hit. Hits further than ioHit.mFraction will not be considered and in this case ioHit will remain unmodified (and the function will return false).
+	/// Convex objects will be treated as solid (meaning if the ray starts inside, you'll get a hit fraction of 0) and back face hits are returned.
+	/// If you want the surface normal of the hit use GetWorldSpaceSurfaceNormal(ioHit.mSubShapeID2, inRay.GetPointOnRay(ioHit.mFraction)) on this object.
+	bool						CastRay(const RRayCast &inRay, RayCastResult &ioHit) const;
+
+	/// Cast a ray, allows collecting multiple hits. Note that this version is more flexible but also slightly slower than the CastRay function that returns only a single hit.
+	/// If you want the surface normal of the hit use GetWorldSpaceSurfaceNormal(collected sub shape ID, inRay.GetPointOnRay(collected fraction)) on this object.
+	void						CastRay(const RRayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const;
+
+	/// Check if inPoint is inside any shapes. For this tests all shapes are treated as if they were solid.
+	/// For a mesh shape, this test will only provide sensible information if the mesh is a closed manifold.
+	/// For each shape that collides, ioCollector will receive a hit
+	void						CollidePoint(RVec3Arg inPoint, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const;
+
+	/// Collide a shape and report any hits to ioCollector
+	/// @param inShape Shape to test
+	/// @param inShapeScale Scale in local space of shape
+	/// @param inCenterOfMassTransform Center of mass transform for the shape
+	/// @param inCollideShapeSettings Settings
+	/// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. mShapePositionCOM since floats are most accurate near the origin
+	/// @param ioCollector Collector that receives the hits
+	/// @param inShapeFilter Filter that allows you to reject collisions
+	void						CollideShape(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const;
+
+	/// Cast a shape and report any hits to ioCollector
+	/// @param inShapeCast The shape cast and its position and direction
+	/// @param inShapeCastSettings Settings for the shape cast
+	/// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. mShapePositionCOM or inShapeCast.mCenterOfMassStart.GetTranslation() since floats are most accurate near the origin
+	/// @param ioCollector Collector that receives the hits
+	/// @param inShapeFilter Filter that allows you to reject collisions
+	void						CastShape(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const;
+
+	/// Collect the leaf transformed shapes of all leaf shapes of this shape
+	/// inBox is the world space axis aligned box which leaf shapes should collide with
+	void						CollectTransformedShapes(const AABox &inBox, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const;
+
+	/// Use the context from Shape
+	using GetTrianglesContext = Shape::GetTrianglesContext;
+
+	/// To start iterating over triangles, call this function first.
+	/// To get the actual triangles call GetTrianglesNext.
+	/// @param ioContext A temporary buffer and should remain untouched until the last call to GetTrianglesNext.
+	/// @param inBox The world space bounding in which you want to get the triangles.
+	/// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. inBox.GetCenter() since floats are most accurate near the origin
+	void						GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, RVec3Arg inBaseOffset) const;
+
+	/// Call this repeatedly to get all triangles in the box.
+	/// outTriangleVertices should be large enough to hold 3 * inMaxTriangleRequested entries
+	/// outMaterials (if it is not null) should contain inMaxTrianglesRequested entries
+	/// The function returns the amount of triangles that it found (which will be <= inMaxTrianglesRequested), or 0 if there are no more triangles.
+	/// Note that the function can return a value < inMaxTrianglesRequested and still have more triangles to process (triangles can be returned in blocks)
+	/// Note that the function may return triangles outside of the requested box, only coarse culling is performed on the returned triangles
+	int							GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const;
+
+	/// Get/set the scale of the shape as a Vec3
+	inline Vec3					GetShapeScale() const						{ return Vec3::sLoadFloat3Unsafe(mShapeScale); }
+	inline void					SetShapeScale(Vec3Arg inScale)				{ inScale.StoreFloat3(&mShapeScale); }
+
+	/// Calculates the transform for this shapes's center of mass (excluding scale)
+	inline RMat44				GetCenterOfMassTransform() const			{ return RMat44::sRotationTranslation(mShapeRotation, mShapePositionCOM); }
+
+	/// Calculates the inverse of the transform for this shape's center of mass (excluding scale)
+	inline RMat44				GetInverseCenterOfMassTransform() const		{ return RMat44::sInverseRotationTranslation(mShapeRotation, mShapePositionCOM); }
+
+	/// Sets the world transform (including scale) of this transformed shape (not from the center of mass but in the space the shape was created)
+	inline void					SetWorldTransform(RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inScale)
+	{
+		mShapePositionCOM = inPosition + inRotation * (inScale * mShape->GetCenterOfMass());
+		mShapeRotation = inRotation;
+		SetShapeScale(inScale);
+	}
+
+	/// Sets the world transform (including scale) of this transformed shape (not from the center of mass but in the space the shape was created)
+	inline void					SetWorldTransform(RMat44Arg inTransform)
+	{
+		Vec3 scale;
+		RMat44 rot_trans = inTransform.Decompose(scale);
+		SetWorldTransform(rot_trans.GetTranslation(), rot_trans.GetQuaternion(), scale);
+	}
+
+	/// Calculates the world transform including scale of this shape (not from the center of mass but in the space the shape was created)
+	inline RMat44				GetWorldTransform() const
+	{
+		RMat44 transform = RMat44::sRotation(mShapeRotation).PreScaled(GetShapeScale());
+		transform.SetTranslation(mShapePositionCOM - transform.Multiply3x3(mShape->GetCenterOfMass()));
+		return transform;
+	}
+
+	/// Get the world space bounding box for this transformed shape
+	AABox						GetWorldSpaceBounds() const					{ return mShape != nullptr? mShape->GetWorldSpaceBounds(GetCenterOfMassTransform(), GetShapeScale()) : AABox(); }
+
+	/// Make inSubShapeID relative to mShape. When mSubShapeIDCreator is not empty, this is needed in order to get the correct path to the sub shape.
+	inline SubShapeID			MakeSubShapeIDRelativeToShape(const SubShapeID &inSubShapeID) const
+	{
+		// Take off the sub shape ID part that comes from mSubShapeIDCreator and validate that it is the same
+		SubShapeID sub_shape_id;
+		uint num_bits_written = mSubShapeIDCreator.GetNumBitsWritten();
+		JPH_IF_ENABLE_ASSERTS(uint32 root_id =) inSubShapeID.PopID(num_bits_written, sub_shape_id);
+		JPH_ASSERT(root_id == (mSubShapeIDCreator.GetID().GetValue() & ((1 << num_bits_written) - 1)));
+		return sub_shape_id;
+	}
+
+	/// Get surface normal of a particular sub shape and its world space surface position on this body.
+	/// Note: When you have a CollideShapeResult or ShapeCastResult you should use -mPenetrationAxis.Normalized() as contact normal as GetWorldSpaceSurfaceNormal will only return face normals (and not vertex or edge normals).
+	inline Vec3					GetWorldSpaceSurfaceNormal(const SubShapeID &inSubShapeID, RVec3Arg inPosition) const
+	{
+		RMat44 inv_com = GetInverseCenterOfMassTransform();
+		Vec3 scale = GetShapeScale(); // See comment at ScaledShape::GetSurfaceNormal for the math behind the scaling of the normal
+		return inv_com.Multiply3x3Transposed(mShape->GetSurfaceNormal(MakeSubShapeIDRelativeToShape(inSubShapeID), Vec3(inv_com * inPosition) / scale) / scale).Normalized();
+	}
+
+	/// Get the vertices of the face that faces inDirection the most (includes any convex radius). Note that this function can only return faces of
+	/// convex shapes or triangles, which is why a sub shape ID to get to that leaf must be provided.
+	/// @param inSubShapeID Sub shape ID of target shape
+	/// @param inDirection Direction that the face should be facing (in world space)
+	/// @param inBaseOffset The vertices will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. mShapePositionCOM since floats are most accurate near the origin
+	/// @param outVertices Resulting face. Note the returned face can have a single point if the shape doesn't have polygons to return (e.g. because it's a sphere). The face will be returned in world space.
+	void						GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, RVec3Arg inBaseOffset, Shape::SupportingFace &outVertices) const
+	{
+		Mat44 com = GetCenterOfMassTransform().PostTranslated(-inBaseOffset).ToMat44();
+		mShape->GetSupportingFace(MakeSubShapeIDRelativeToShape(inSubShapeID), com.Multiply3x3Transposed(inDirection), GetShapeScale(), com, outVertices);
+	}
+
+	/// Get material of a particular sub shape
+	inline const PhysicsMaterial *GetMaterial(const SubShapeID &inSubShapeID) const
+	{
+		return mShape->GetMaterial(MakeSubShapeIDRelativeToShape(inSubShapeID));
+	}
+
+	/// Get the user data of a particular sub shape
+	inline uint64				GetSubShapeUserData(const SubShapeID &inSubShapeID) const
+	{
+		return mShape->GetSubShapeUserData(MakeSubShapeIDRelativeToShape(inSubShapeID));
+	}
+
+	/// Get the direct child sub shape and its transform for a sub shape ID.
+	/// @param inSubShapeID Sub shape ID that indicates the path to the leaf shape
+	/// @param outRemainder The remainder of the sub shape ID after removing the sub shape
+	/// @return Direct child sub shape and its transform, note that the body ID and sub shape ID will be invalid
+	TransformedShape			GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, SubShapeID &outRemainder) const
+	{
+		TransformedShape ts = mShape->GetSubShapeTransformedShape(inSubShapeID, Vec3::sZero(), mShapeRotation, GetShapeScale(), outRemainder);
+		ts.mShapePositionCOM += mShapePositionCOM;
+		return ts;
+	}
+
+	/// Helper function to return the body id from a transformed shape. If the transformed shape is null an invalid body ID will be returned.
+	inline static BodyID		sGetBodyID(const TransformedShape *inTS)	{ return inTS != nullptr? inTS->mBodyID : BodyID(); }
+
+	RVec3						mShapePositionCOM;							///< Center of mass world position of the shape
+	Quat						mShapeRotation;								///< Rotation of the shape
+	RefConst<Shape>				mShape;										///< The shape itself
+	Float3						mShapeScale { 1, 1, 1 };					///< Not stored as Vec3 to get a nicely packed structure
+	BodyID						mBodyID;									///< Optional body ID from which this shape comes
+	SubShapeIDCreator			mSubShapeIDCreator;							///< Optional sub shape ID creator for the shape (can be used when expanding compound shapes into multiple transformed shapes)
+};
+
+static_assert(JPH_CPU_ADDRESS_BITS != 64 || sizeof(TransformedShape) == JPH_IF_SINGLE_PRECISION_ELSE(64, 96), "Not properly packed");
+static_assert(alignof(TransformedShape) == JPH_RVECTOR_ALIGNMENT, "Not properly aligned");
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/CalculateSolverSteps.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/CalculateSolverSteps.h
new file mode 100644
index 0000000000..857eddfb8e
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/CalculateSolverSteps.h
@@ -0,0 +1,66 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/PhysicsSettings.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class used to calculate the total number of velocity and position steps
+class CalculateSolverSteps
+{
+public:
+	/// Constructor
+	JPH_INLINE explicit			CalculateSolverSteps(const PhysicsSettings &inSettings) : mSettings(inSettings) { }
+
+	/// Combine the number of velocity and position steps for this body/constraint with the current values
+	template <class Type>
+	JPH_INLINE void				operator () (const Type *inObject)
+	{
+		uint num_velocity_steps = inObject->GetNumVelocityStepsOverride();
+		mNumVelocitySteps = max(mNumVelocitySteps, num_velocity_steps);
+		mApplyDefaultVelocity |= num_velocity_steps == 0;
+
+		uint num_position_steps = inObject->GetNumPositionStepsOverride();
+		mNumPositionSteps = max(mNumPositionSteps, num_position_steps);
+		mApplyDefaultPosition |= num_position_steps == 0;
+	}
+
+	/// Must be called after all bodies/constraints have been processed
+	JPH_INLINE void				Finalize()
+	{
+		// If we have a default velocity/position step count, take the max of the default and the overrides
+		if (mApplyDefaultVelocity)
+			mNumVelocitySteps = max(mNumVelocitySteps, mSettings.mNumVelocitySteps);
+		if (mApplyDefaultPosition)
+			mNumPositionSteps = max(mNumPositionSteps, mSettings.mNumPositionSteps);
+	}
+
+	/// Get the results of the calculation
+	JPH_INLINE uint				GetNumPositionSteps() const					{ return mNumPositionSteps; }
+	JPH_INLINE uint				GetNumVelocitySteps() const					{ return mNumVelocitySteps; }
+
+private:
+	const PhysicsSettings &		mSettings;
+
+	uint						mNumVelocitySteps = 0;
+	uint						mNumPositionSteps = 0;
+
+	bool						mApplyDefaultVelocity = false;
+	bool						mApplyDefaultPosition = false;
+};
+
+/// Dummy class to replace the steps calculator when we don't need the result
+class DummyCalculateSolverSteps
+{
+public:
+	template <class Type>
+	JPH_INLINE void				operator () (const Type *) const
+	{
+		/* Nothing to do */
+	}
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConeConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConeConstraint.cpp
new file mode 100644
index 0000000000..9889fa643d
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConeConstraint.cpp
@@ -0,0 +1,246 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/ConeConstraint.h>
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(ConeConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(ConeConstraintSettings, TwoBodyConstraintSettings)
+
+	JPH_ADD_ENUM_ATTRIBUTE(ConeConstraintSettings, mSpace)
+	JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mPoint1)
+	JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mTwistAxis1)
+	JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mPoint2)
+	JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mTwistAxis2)
+	JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mHalfConeAngle)
+}
+
+void ConeConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	ConstraintSettings::SaveBinaryState(inStream);
+
+	inStream.Write(mSpace);
+	inStream.Write(mPoint1);
+	inStream.Write(mTwistAxis1);
+	inStream.Write(mPoint2);
+	inStream.Write(mTwistAxis2);
+	inStream.Write(mHalfConeAngle);
+}
+
+void ConeConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	ConstraintSettings::RestoreBinaryState(inStream);
+
+	inStream.Read(mSpace);
+	inStream.Read(mPoint1);
+	inStream.Read(mTwistAxis1);
+	inStream.Read(mPoint2);
+	inStream.Read(mTwistAxis2);
+	inStream.Read(mHalfConeAngle);
+}
+
+TwoBodyConstraint *ConeConstraintSettings::Create(Body &inBody1, Body &inBody2) const
+{
+	return new ConeConstraint(inBody1, inBody2, *this);
+}
+
+ConeConstraint::ConeConstraint(Body &inBody1, Body &inBody2, const ConeConstraintSettings &inSettings) :
+	TwoBodyConstraint(inBody1, inBody2, inSettings)
+{
+	// Store limits
+	SetHalfConeAngle(inSettings.mHalfConeAngle);
+
+	// Initialize rotation axis to perpendicular of twist axis in case the angle between the twist axis is 0 in the first frame
+	mWorldSpaceRotationAxis = inSettings.mTwistAxis1.GetNormalizedPerpendicular();
+
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		// If all properties were specified in world space, take them to local space now
+		RMat44 inv_transform1 = inBody1.GetInverseCenterOfMassTransform();
+		mLocalSpacePosition1 = Vec3(inv_transform1 * inSettings.mPoint1);
+		mLocalSpaceTwistAxis1 = inv_transform1.Multiply3x3(inSettings.mTwistAxis1);
+
+		RMat44 inv_transform2 = inBody2.GetInverseCenterOfMassTransform();
+		mLocalSpacePosition2 = Vec3(inv_transform2 * inSettings.mPoint2);
+		mLocalSpaceTwistAxis2 = inv_transform2.Multiply3x3(inSettings.mTwistAxis2);
+	}
+	else
+	{
+		// Properties already in local space
+		mLocalSpacePosition1 = Vec3(inSettings.mPoint1);
+		mLocalSpacePosition2 = Vec3(inSettings.mPoint2);
+		mLocalSpaceTwistAxis1 = inSettings.mTwistAxis1;
+		mLocalSpaceTwistAxis2 = inSettings.mTwistAxis2;
+
+		// If they were in local space, we need to take the initial rotation axis to world space
+		mWorldSpaceRotationAxis = inBody1.GetRotation() * mWorldSpaceRotationAxis;
+	}
+}
+
+void ConeConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
+{
+	if (mBody1->GetID() == inBodyID)
+		mLocalSpacePosition1 -= inDeltaCOM;
+	else if (mBody2->GetID() == inBodyID)
+		mLocalSpacePosition2 -= inDeltaCOM;
+}
+
+void ConeConstraint::CalculateRotationConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2)
+{
+	// Rotation is along the cross product of both twist axis
+	Vec3 twist1 = inRotation1.Multiply3x3(mLocalSpaceTwistAxis1);
+	Vec3 twist2 = inRotation2.Multiply3x3(mLocalSpaceTwistAxis2);
+
+	// Calculate dot product between twist axis, if it's smaller than the cone angle we need to correct
+	mCosTheta = twist1.Dot(twist2);
+	if (mCosTheta < mCosHalfConeAngle)
+	{
+		// Rotation axis is defined by the two twist axis
+		Vec3 rot_axis = twist2.Cross(twist1);
+
+		// If we can't find a rotation axis because the twist is too small, we'll use last frame's rotation axis
+		float len = rot_axis.Length();
+		if (len > 0.0f)
+			mWorldSpaceRotationAxis = rot_axis / len;
+
+		mAngleConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceRotationAxis);
+	}
+	else
+		mAngleConstraintPart.Deactivate();
+}
+
+void ConeConstraint::SetupVelocityConstraint(float inDeltaTime)
+{
+	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
+	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
+	mPointConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, mLocalSpacePosition1, *mBody2, rotation2, mLocalSpacePosition2);
+	CalculateRotationConstraintProperties(rotation1, rotation2);
+}
+
+void ConeConstraint::ResetWarmStart()
+{
+	mPointConstraintPart.Deactivate();
+	mAngleConstraintPart.Deactivate();
+}
+
+void ConeConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
+{
+	// Warm starting: Apply previous frame impulse
+	mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+	mAngleConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+}
+
+bool ConeConstraint::SolveVelocityConstraint(float inDeltaTime)
+{
+	bool pos = mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+
+	bool rot = false;
+	if (mAngleConstraintPart.IsActive())
+		rot = mAngleConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceRotationAxis, 0, FLT_MAX);
+
+	return pos || rot;
+}
+
+bool ConeConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
+{
+	mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2);
+	bool pos = mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte);
+
+	bool rot = false;
+	CalculateRotationConstraintProperties(Mat44::sRotation(mBody1->GetRotation()), Mat44::sRotation(mBody2->GetRotation()));
+	if (mAngleConstraintPart.IsActive())
+		rot = mAngleConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mCosTheta - mCosHalfConeAngle, inBaumgarte);
+
+	return pos || rot;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void ConeConstraint::DrawConstraint(DebugRenderer *inRenderer) const
+{
+	RMat44 transform1 = mBody1->GetCenterOfMassTransform();
+	RMat44 transform2 = mBody2->GetCenterOfMassTransform();
+
+	RVec3 p1 = transform1 * mLocalSpacePosition1;
+	RVec3 p2 = transform2 * mLocalSpacePosition2;
+
+	// Draw constraint
+	inRenderer->DrawMarker(p1, Color::sRed, 0.1f);
+	inRenderer->DrawMarker(p2, Color::sGreen, 0.1f);
+
+	// Draw twist axis
+	inRenderer->DrawLine(p1, p1 + mDrawConstraintSize * transform1.Multiply3x3(mLocalSpaceTwistAxis1), Color::sRed);
+	inRenderer->DrawLine(p2, p2 + mDrawConstraintSize * transform2.Multiply3x3(mLocalSpaceTwistAxis2), Color::sGreen);
+}
+
+void ConeConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const
+{
+	// Get constraint properties in world space
+	RMat44 transform1 = mBody1->GetCenterOfMassTransform();
+	RVec3 position1 = transform1 * mLocalSpacePosition1;
+	Vec3 twist_axis1 = transform1.Multiply3x3(mLocalSpaceTwistAxis1);
+	Vec3 normal_axis1 = transform1.Multiply3x3(mLocalSpaceTwistAxis1.GetNormalizedPerpendicular());
+
+	inRenderer->DrawOpenCone(position1, twist_axis1, normal_axis1, ACos(mCosHalfConeAngle), mDrawConstraintSize * mCosHalfConeAngle, Color::sPurple, DebugRenderer::ECastShadow::Off);
+}
+#endif // JPH_DEBUG_RENDERER
+
+void ConeConstraint::SaveState(StateRecorder &inStream) const
+{
+	TwoBodyConstraint::SaveState(inStream);
+
+	mPointConstraintPart.SaveState(inStream);
+	mAngleConstraintPart.SaveState(inStream);
+	inStream.Write(mWorldSpaceRotationAxis); // When twist is too small, the rotation is used from last frame so we need to store it
+}
+
+void ConeConstraint::RestoreState(StateRecorder &inStream)
+{
+	TwoBodyConstraint::RestoreState(inStream);
+
+	mPointConstraintPart.RestoreState(inStream);
+	mAngleConstraintPart.RestoreState(inStream);
+	inStream.Read(mWorldSpaceRotationAxis);
+}
+
+Ref<ConstraintSettings> ConeConstraint::GetConstraintSettings() const
+{
+	ConeConstraintSettings *settings = new ConeConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mPoint1 = RVec3(mLocalSpacePosition1);
+	settings->mTwistAxis1 = mLocalSpaceTwistAxis1;
+	settings->mPoint2 = RVec3(mLocalSpacePosition2);
+	settings->mTwistAxis2 = mLocalSpaceTwistAxis2;
+	settings->mHalfConeAngle = ACos(mCosHalfConeAngle);
+	return settings;
+}
+
+Mat44 ConeConstraint::GetConstraintToBody1Matrix() const
+{
+	Vec3 perp = mLocalSpaceTwistAxis1.GetNormalizedPerpendicular();
+	Vec3 perp2 = mLocalSpaceTwistAxis1.Cross(perp);
+	return Mat44(Vec4(mLocalSpaceTwistAxis1, 0), Vec4(perp, 0), Vec4(perp2, 0), Vec4(mLocalSpacePosition1, 1));
+}
+
+Mat44 ConeConstraint::GetConstraintToBody2Matrix() const
+{
+	// Note: Incorrect in rotation around the twist axis (the perpendicular does not match that of body 1),
+	// this should not matter as we're not limiting rotation around the twist axis.
+	Vec3 perp = mLocalSpaceTwistAxis2.GetNormalizedPerpendicular();
+	Vec3 perp2 = mLocalSpaceTwistAxis2.Cross(perp);
+	return Mat44(Vec4(mLocalSpaceTwistAxis2, 0), Vec4(perp, 0), Vec4(perp2, 0), Vec4(mLocalSpacePosition2, 1));
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConeConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConeConstraint.h
new file mode 100644
index 0000000000..1e25ab2086
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConeConstraint.h
@@ -0,0 +1,133 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Cone constraint settings, used to create a cone constraint
+class JPH_EXPORT ConeConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, ConeConstraintSettings)
+
+public:
+	// See: ConstraintSettings::SaveBinaryState
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+
+	/// Create an instance of this constraint
+	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
+
+	/// This determines in which space the constraint is setup, all properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	/// Body 1 constraint reference frame (space determined by mSpace)
+	RVec3						mPoint1 = RVec3::sZero();
+	Vec3						mTwistAxis1 = Vec3::sAxisX();
+
+	/// Body 2 constraint reference frame (space determined by mSpace)
+	RVec3						mPoint2 = RVec3::sZero();
+	Vec3						mTwistAxis2 = Vec3::sAxisX();
+
+	/// Half of maximum angle between twist axis of body 1 and 2
+	float						mHalfConeAngle = 0.0f;
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// A cone constraint constraints 2 bodies to a single point and limits the swing between the twist axis within a cone:
+///
+/// t1 . t2 <= cos(theta)
+///
+/// Where:
+///
+/// t1 = twist axis of body 1.
+/// t2 = twist axis of body 2.
+/// theta = half cone angle (angle from the principal axis of the cone to the edge).
+///
+/// Calculating the Jacobian:
+///
+/// Constraint equation:
+///
+/// C = t1 . t2 - cos(theta)
+///
+/// Derivative:
+///
+/// d/dt C = d/dt (t1 . t2) = (d/dt t1) . t2 + t1 . (d/dt t2) = (w1 x t1) . t2 + t1 . (w2 x t2) = (t1 x t2) . w1 + (t2 x t1) . w2
+///
+/// d/dt C = J v = [0, -t2 x t1, 0, t2 x t1] [v1, w1, v2, w2]
+///
+/// Where J is the Jacobian.
+///
+/// Note that this is the exact same equation as used in AngleConstraintPart if we use t2 x t1 as the world space axis
+class JPH_EXPORT ConeConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct cone constraint
+								ConeConstraint(Body &inBody1, Body &inBody2, const ConeConstraintSettings &inSettings);
+
+	// Generic interface of a constraint
+	virtual EConstraintSubType	GetSubType() const override					{ return EConstraintSubType::Cone; }
+	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
+	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
+	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
+	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
+	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void				DrawConstraint(DebugRenderer *inRenderer) const override;
+	virtual void				DrawConstraintLimits(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual void				SaveState(StateRecorder &inStream) const override;
+	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
+
+	// See: TwoBodyConstraint
+	virtual Mat44				GetConstraintToBody1Matrix() const override;
+	virtual Mat44				GetConstraintToBody2Matrix() const override;
+
+	/// Update maximum angle between body 1 and 2 (see ConeConstraintSettings)
+	void						SetHalfConeAngle(float inHalfConeAngle)		{ JPH_ASSERT(inHalfConeAngle >= 0.0f && inHalfConeAngle <= JPH_PI); mCosHalfConeAngle = Cos(inHalfConeAngle); }
+	float						GetCosHalfConeAngle() const					{ return mCosHalfConeAngle; }
+
+	///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint)
+	inline Vec3					GetTotalLambdaPosition() const				{ return mPointConstraintPart.GetTotalLambda(); }
+	inline float				GetTotalLambdaRotation() const				{ return mAngleConstraintPart.GetTotalLambda(); }
+
+private:
+	// Internal helper function to calculate the values below
+	void						CalculateRotationConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2);
+
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	// Local space constraint positions
+	Vec3						mLocalSpacePosition1;
+	Vec3						mLocalSpacePosition2;
+
+	// Local space constraint axis
+	Vec3						mLocalSpaceTwistAxis1;
+	Vec3						mLocalSpaceTwistAxis2;
+
+	// Angular limits
+	float						mCosHalfConeAngle;
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	// Axis and angle of rotation between the two bodies
+	Vec3						mWorldSpaceRotationAxis;
+	float						mCosTheta;
+
+	// The constraint parts
+	PointConstraintPart			mPointConstraintPart;
+	AngleConstraintPart			mAngleConstraintPart;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.cpp
new file mode 100644
index 0000000000..b81a8d81a7
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.cpp
@@ -0,0 +1,73 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/Constraint.h>
+#include <Jolt/Physics/StateRecorder.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamUtils.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(ConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(ConstraintSettings, SerializableObject)
+
+	JPH_ADD_ATTRIBUTE(ConstraintSettings, mEnabled)
+	JPH_ADD_ATTRIBUTE(ConstraintSettings, mDrawConstraintSize)
+	JPH_ADD_ATTRIBUTE(ConstraintSettings, mConstraintPriority)
+	JPH_ADD_ATTRIBUTE(ConstraintSettings, mNumVelocityStepsOverride)
+	JPH_ADD_ATTRIBUTE(ConstraintSettings, mNumPositionStepsOverride)
+	JPH_ADD_ATTRIBUTE(ConstraintSettings, mUserData)
+}
+
+void ConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(GetRTTI()->GetHash());
+	inStream.Write(mEnabled);
+	inStream.Write(mDrawConstraintSize);
+	inStream.Write(mConstraintPriority);
+	inStream.Write(mNumVelocityStepsOverride);
+	inStream.Write(mNumPositionStepsOverride);
+}
+
+void ConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	// Type hash read by sRestoreFromBinaryState
+	inStream.Read(mEnabled);
+	inStream.Read(mDrawConstraintSize);
+	inStream.Read(mConstraintPriority);
+	inStream.Read(mNumVelocityStepsOverride);
+	inStream.Read(mNumPositionStepsOverride);
+}
+
+ConstraintSettings::ConstraintResult ConstraintSettings::sRestoreFromBinaryState(StreamIn &inStream)
+{
+	return StreamUtils::RestoreObject<ConstraintSettings>(inStream, &ConstraintSettings::RestoreBinaryState);
+}
+
+void Constraint::SaveState(StateRecorder &inStream) const
+{
+	inStream.Write(mEnabled);
+}
+
+void Constraint::RestoreState(StateRecorder &inStream)
+{
+	inStream.Read(mEnabled);
+}
+
+void Constraint::ToConstraintSettings(ConstraintSettings &outSettings) const
+{
+	outSettings.mEnabled = mEnabled;
+	outSettings.mConstraintPriority = mConstraintPriority;
+	outSettings.mNumVelocityStepsOverride = mNumVelocityStepsOverride;
+	outSettings.mNumPositionStepsOverride = mNumPositionStepsOverride;
+	outSettings.mUserData = mUserData;
+#ifdef JPH_DEBUG_RENDERER
+	outSettings.mDrawConstraintSize = mDrawConstraintSize;
+#endif // JPH_DEBUG_RENDERER
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.h
new file mode 100644
index 0000000000..5127f39895
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/Constraint.h
@@ -0,0 +1,238 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/Reference.h>
+#include <Jolt/Core/NonCopyable.h>
+#include <Jolt/Core/Result.h>
+#include <Jolt/ObjectStream/SerializableObject.h>
+
+JPH_NAMESPACE_BEGIN
+
+class BodyID;
+class IslandBuilder;
+class LargeIslandSplitter;
+class BodyManager;
+class StateRecorder;
+class StreamIn;
+class StreamOut;
+#ifdef JPH_DEBUG_RENDERER
+class DebugRenderer;
+#endif // JPH_DEBUG_RENDERER
+
+/// Enum to identify constraint type
+enum class EConstraintType
+{
+	Constraint,
+	TwoBodyConstraint,
+};
+
+/// Enum to identify constraint sub type
+enum class EConstraintSubType
+{
+	Fixed,
+	Point,
+	Hinge,
+	Slider,
+	Distance,
+	Cone,
+	SwingTwist,
+	SixDOF,
+	Path,
+	Vehicle,
+	RackAndPinion,
+	Gear,
+	Pulley,
+
+	/// User defined constraint types start here
+	User1,
+	User2,
+	User3,
+	User4
+};
+
+/// Certain constraints support setting them up in local or world space. This governs what is used.
+enum class EConstraintSpace
+{
+	LocalToBodyCOM,				///< All constraint properties are specified in local space to center of mass of the bodies that are being constrained (so e.g. 'constraint position 1' will be local to body 1 COM, 'constraint position 2' will be local to body 2 COM). Note that this means you need to subtract Shape::GetCenterOfMass() from positions!
+	WorldSpace,					///< All constraint properties are specified in world space
+};
+
+/// Class used to store the configuration of a constraint. Allows run-time creation of constraints.
+class JPH_EXPORT ConstraintSettings : public SerializableObject, public RefTarget<ConstraintSettings>
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, ConstraintSettings)
+
+public:
+	using ConstraintResult = Result<Ref<ConstraintSettings>>;
+
+	/// Saves the contents of the constraint settings in binary form to inStream.
+	virtual void				SaveBinaryState(StreamOut &inStream) const;
+
+	/// Creates a constraint of the correct type and restores its contents from the binary stream inStream.
+	static ConstraintResult		sRestoreFromBinaryState(StreamIn &inStream);
+
+	/// If this constraint is enabled initially. Use Constraint::SetEnabled to toggle after creation.
+	bool						mEnabled = true;
+
+	/// Priority of the constraint when solving. Higher numbers have are more likely to be solved correctly.
+	/// Note that if you want a deterministic simulation and you cannot guarantee the order in which constraints are added/removed, you can make the priority for all constraints unique to get a deterministic ordering.
+	uint32						mConstraintPriority = 0;
+
+	/// Used only when the constraint is active. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island.
+	uint						mNumVelocityStepsOverride = 0;
+
+	/// Used only when the constraint is active. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island.
+	uint						mNumPositionStepsOverride = 0;
+
+	/// Size of constraint when drawing it through the debug renderer
+	float						mDrawConstraintSize = 1.0f;
+
+	/// User data value (can be used by application)
+	uint64						mUserData = 0;
+
+protected:
+	/// This function should not be called directly, it is used by sRestoreFromBinaryState.
+	virtual void				RestoreBinaryState(StreamIn &inStream);
+};
+
+/// Base class for all physics constraints. A constraint removes one or more degrees of freedom for a rigid body.
+class JPH_EXPORT Constraint : public RefTarget<Constraint>, public NonCopyable
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+	explicit					Constraint(const ConstraintSettings &inSettings) :
+#ifdef JPH_DEBUG_RENDERER
+		mDrawConstraintSize(inSettings.mDrawConstraintSize),
+#endif // JPH_DEBUG_RENDERER
+		mConstraintPriority(inSettings.mConstraintPriority),
+		mNumVelocityStepsOverride(uint8(inSettings.mNumVelocityStepsOverride)),
+		mNumPositionStepsOverride(uint8(inSettings.mNumPositionStepsOverride)),
+		mEnabled(inSettings.mEnabled),
+		mUserData(inSettings.mUserData)
+	{
+		JPH_ASSERT(inSettings.mNumVelocityStepsOverride < 256);
+		JPH_ASSERT(inSettings.mNumPositionStepsOverride < 256);
+	}
+
+	/// Virtual destructor
+	virtual						~Constraint() = default;
+
+	/// Get the type of a constraint
+	virtual EConstraintType		GetType() const								{ return EConstraintType::Constraint; }
+
+	/// Get the sub type of a constraint
+	virtual EConstraintSubType	GetSubType() const = 0;
+
+	/// Priority of the constraint when solving. Higher numbers have are more likely to be solved correctly.
+	/// Note that if you want a deterministic simulation and you cannot guarantee the order in which constraints are added/removed, you can make the priority for all constraints unique to get a deterministic ordering.
+	uint32						GetConstraintPriority() const				{ return mConstraintPriority; }
+	void						SetConstraintPriority(uint32 inPriority)	{ mConstraintPriority = inPriority; }
+
+	/// Used only when the constraint is active. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island.
+	void						SetNumVelocityStepsOverride(uint inN)		{ JPH_ASSERT(inN < 256); mNumVelocityStepsOverride = uint8(inN); }
+	uint						GetNumVelocityStepsOverride() const			{ return mNumVelocityStepsOverride; }
+
+	/// Used only when the constraint is active. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island.
+	void						SetNumPositionStepsOverride(uint inN)		{ JPH_ASSERT(inN < 256); mNumPositionStepsOverride = uint8(inN); }
+	uint						GetNumPositionStepsOverride() const			{ return mNumPositionStepsOverride; }
+
+	/// Enable / disable this constraint. This can e.g. be used to implement a breakable constraint by detecting that the constraint impulse
+	/// (see e.g. PointConstraint::GetTotalLambdaPosition) went over a certain limit and then disabling the constraint.
+	/// Note that although a disabled constraint will not affect the simulation in any way anymore, it does incur some processing overhead.
+	/// Alternatively you can remove a constraint from the constraint manager (which may be more costly if you want to disable the constraint for a short while).
+	void						SetEnabled(bool inEnabled)					{ mEnabled = inEnabled; }
+
+	/// Test if a constraint is enabled.
+	bool						GetEnabled() const							{ return mEnabled; }
+
+	/// Access to the user data, can be used for anything by the application
+	uint64						GetUserData() const							{ return mUserData; }
+	void						SetUserData(uint64 inUserData)				{ mUserData = inUserData; }
+
+	/// Notify the constraint that the shape of a body has changed and that its center of mass has moved by inDeltaCOM.
+	/// Bodies don't know which constraints are connected to them so the user is responsible for notifying the relevant constraints when a body changes.
+	/// @param inBodyID ID of the body that has changed
+	/// @param inDeltaCOM The delta of the center of mass of the body (shape->GetCenterOfMass() - shape_before_change->GetCenterOfMass())
+	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) = 0;
+
+	/// Notify the system that the configuration of the bodies and/or constraint has changed enough so that the warm start impulses should not be applied the next frame.
+	/// You can use this function for example when repositioning a ragdoll through Ragdoll::SetPose in such a way that the orientation of the bodies completely changes so that
+	/// the previous frame impulses are no longer a good approximation of what the impulses will be in the next frame. Calling this function when there are no big changes
+	/// will result in the constraints being much 'softer' than usual so they are more easily violated (e.g. a long chain of bodies might sag a bit if you call this every frame).
+	virtual void				ResetWarmStart() = 0;
+
+	///@name Solver interface
+	///@{
+	virtual bool				IsActive() const							{ return mEnabled; }
+	virtual void				SetupVelocityConstraint(float inDeltaTime) = 0;
+	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) = 0;
+	virtual bool				SolveVelocityConstraint(float inDeltaTime) = 0;
+	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) = 0;
+	///@}
+
+	/// Link bodies that are connected by this constraint in the island builder
+	virtual void				BuildIslands(uint32 inConstraintIndex, IslandBuilder &ioBuilder, BodyManager &inBodyManager) = 0;
+
+	/// Link bodies that are connected by this constraint in the same split. Returns the split index.
+	virtual uint				BuildIslandSplits(LargeIslandSplitter &ioSplitter) const = 0;
+
+#ifdef JPH_DEBUG_RENDERER
+	// Drawing interface
+	virtual void				DrawConstraint(DebugRenderer *inRenderer) const = 0;
+	virtual void				DrawConstraintLimits([[maybe_unused]] DebugRenderer *inRenderer) const { }
+	virtual void				DrawConstraintReferenceFrame([[maybe_unused]] DebugRenderer *inRenderer) const { }
+
+	/// Size of constraint when drawing it through the debug renderer
+	float						GetDrawConstraintSize() const				{ return mDrawConstraintSize; }
+	void						SetDrawConstraintSize(float inSize)			{ mDrawConstraintSize = inSize; }
+#endif // JPH_DEBUG_RENDERER
+
+	/// Saving state for replay
+	virtual void				SaveState(StateRecorder &inStream) const;
+
+	/// Restoring state for replay
+	virtual void				RestoreState(StateRecorder &inStream);
+
+	/// Debug function to convert a constraint to its settings, note that this will not save to which bodies the constraint is connected to
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const = 0;
+
+protected:
+	/// Helper function to copy settings back to constraint settings for this base class
+	void						ToConstraintSettings(ConstraintSettings &outSettings) const;
+
+#ifdef JPH_DEBUG_RENDERER
+	/// Size of constraint when drawing it through the debug renderer
+	float						mDrawConstraintSize;
+#endif // JPH_DEBUG_RENDERER
+
+private:
+	friend class ConstraintManager;
+
+	/// Index that indicates this constraint is not in the constraint manager
+	static constexpr uint32		cInvalidConstraintIndex = 0xffffffff;
+
+	/// Index in the mConstraints list of the ConstraintManager for easy finding
+	uint32						mConstraintIndex = cInvalidConstraintIndex;
+
+	/// Priority of the constraint when solving. Higher numbers have are more likely to be solved correctly.
+	uint32						mConstraintPriority = 0;
+
+	/// Used only when the constraint is active. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island.
+	uint8						mNumVelocityStepsOverride = 0;
+
+	/// Used only when the constraint is active. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island.
+	uint8						mNumPositionStepsOverride = 0;
+
+	/// If this constraint is currently enabled
+	bool						mEnabled = true;
+
+	/// User data value (can be used by application)
+	uint64						mUserData;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintManager.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintManager.cpp
new file mode 100644
index 0000000000..509bddbd97
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintManager.cpp
@@ -0,0 +1,289 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/ConstraintManager.h>
+#include <Jolt/Physics/Constraints/CalculateSolverSteps.h>
+#include <Jolt/Physics/IslandBuilder.h>
+#include <Jolt/Physics/StateRecorder.h>
+#include <Jolt/Physics/PhysicsLock.h>
+#include <Jolt/Core/Profiler.h>
+#include <Jolt/Core/QuickSort.h>
+
+JPH_NAMESPACE_BEGIN
+
+void ConstraintManager::Add(Constraint **inConstraints, int inNumber)
+{
+	UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList));
+
+	mConstraints.reserve(mConstraints.size() + inNumber);
+
+	for (Constraint **c = inConstraints, **c_end = inConstraints + inNumber; c < c_end; ++c)
+	{
+		Constraint *constraint = *c;
+
+		// Assume this constraint has not been added yet
+		JPH_ASSERT(constraint->mConstraintIndex == Constraint::cInvalidConstraintIndex);
+
+		// Add to the list
+		constraint->mConstraintIndex = uint32(mConstraints.size());
+		mConstraints.push_back(constraint);
+	}
+}
+
+void ConstraintManager::Remove(Constraint **inConstraints, int inNumber)
+{
+	UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList));
+
+	for (Constraint **c = inConstraints, **c_end = inConstraints + inNumber; c < c_end; ++c)
+	{
+		Constraint *constraint = *c;
+
+		// Reset constraint index for this constraint
+		uint32 this_constraint_idx = constraint->mConstraintIndex;
+		constraint->mConstraintIndex = Constraint::cInvalidConstraintIndex;
+		JPH_ASSERT(this_constraint_idx != Constraint::cInvalidConstraintIndex);
+
+		// Check if this constraint is somewhere in the middle of the constraints, in this case we need to move the last constraint to this position
+		uint32 last_constraint_idx = uint32(mConstraints.size() - 1);
+		if (this_constraint_idx < last_constraint_idx)
+		{
+			Constraint *last_constraint = mConstraints[last_constraint_idx];
+			last_constraint->mConstraintIndex = this_constraint_idx;
+			mConstraints[this_constraint_idx] = last_constraint;
+		}
+
+		// Pop last constraint
+		mConstraints.pop_back();
+	}
+}
+
+Constraints ConstraintManager::GetConstraints() const
+{
+	UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList));
+
+	Constraints copy = mConstraints;
+	return copy;
+}
+
+void ConstraintManager::GetActiveConstraints(uint32 inStartConstraintIdx, uint32 inEndConstraintIdx, Constraint **outActiveConstraints, uint32 &outNumActiveConstraints) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(inEndConstraintIdx <= mConstraints.size());
+
+	uint32 num_active_constraints = 0;
+	for (uint32 constraint_idx = inStartConstraintIdx; constraint_idx < inEndConstraintIdx; ++constraint_idx)
+	{
+		Constraint *c = mConstraints[constraint_idx];
+		JPH_ASSERT(c->mConstraintIndex == constraint_idx);
+		if (c->IsActive())
+		{
+			*(outActiveConstraints++) = c;
+			num_active_constraints++;
+		}
+	}
+
+	outNumActiveConstraints = num_active_constraints;
+}
+
+void ConstraintManager::sBuildIslands(Constraint **inActiveConstraints, uint32 inNumActiveConstraints, IslandBuilder &ioBuilder, BodyManager &inBodyManager)
+{
+	JPH_PROFILE_FUNCTION();
+
+	for (uint32 constraint_idx = 0; constraint_idx < inNumActiveConstraints; ++constraint_idx)
+	{
+		Constraint *c = inActiveConstraints[constraint_idx];
+		c->BuildIslands(constraint_idx, ioBuilder, inBodyManager);
+	}
+}
+
+void ConstraintManager::sSortConstraints(Constraint **inActiveConstraints, uint32 *inConstraintIdxBegin, uint32 *inConstraintIdxEnd)
+{
+	JPH_PROFILE_FUNCTION();
+
+	QuickSort(inConstraintIdxBegin, inConstraintIdxEnd, [inActiveConstraints](uint32 inLHS, uint32 inRHS) {
+		const Constraint *lhs = inActiveConstraints[inLHS];
+		const Constraint *rhs = inActiveConstraints[inRHS];
+
+		if (lhs->GetConstraintPriority() != rhs->GetConstraintPriority())
+			return lhs->GetConstraintPriority() < rhs->GetConstraintPriority();
+
+		return lhs->mConstraintIndex < rhs->mConstraintIndex;
+	});
+}
+
+void ConstraintManager::sSetupVelocityConstraints(Constraint **inActiveConstraints, uint32 inNumActiveConstraints, float inDeltaTime)
+{
+	JPH_PROFILE_FUNCTION();
+
+	for (Constraint **c = inActiveConstraints, **c_end = inActiveConstraints + inNumActiveConstraints; c < c_end; ++c)
+		(*c)->SetupVelocityConstraint(inDeltaTime);
+}
+
+template <class ConstraintCallback>
+void ConstraintManager::sWarmStartVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, ConstraintCallback &ioCallback)
+{
+	JPH_PROFILE_FUNCTION();
+
+	for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx)
+	{
+		Constraint *c = inActiveConstraints[*constraint_idx];
+		ioCallback(c);
+		c->WarmStartVelocityConstraint(inWarmStartImpulseRatio);
+	}
+}
+
+// Specialize for the two constraint callback types
+template void ConstraintManager::sWarmStartVelocityConstraints<CalculateSolverSteps>(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, CalculateSolverSteps &ioCallback);
+template void ConstraintManager::sWarmStartVelocityConstraints<DummyCalculateSolverSteps>(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, DummyCalculateSolverSteps &ioCallback);
+
+bool ConstraintManager::sSolveVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inDeltaTime)
+{
+	JPH_PROFILE_FUNCTION();
+
+	bool any_impulse_applied = false;
+
+	for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx)
+	{
+		Constraint *c = inActiveConstraints[*constraint_idx];
+		any_impulse_applied |= c->SolveVelocityConstraint(inDeltaTime);
+	}
+
+	return any_impulse_applied;
+}
+
+bool ConstraintManager::sSolvePositionConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inDeltaTime, float inBaumgarte)
+{
+	JPH_PROFILE_FUNCTION();
+
+	bool any_impulse_applied = false;
+
+	for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx)
+	{
+		Constraint *c = inActiveConstraints[*constraint_idx];
+		any_impulse_applied |= c->SolvePositionConstraint(inDeltaTime, inBaumgarte);
+	}
+
+	return any_impulse_applied;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void ConstraintManager::DrawConstraints(DebugRenderer *inRenderer) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList));
+
+	for (const Ref<Constraint> &c : mConstraints)
+		c->DrawConstraint(inRenderer);
+}
+
+void ConstraintManager::DrawConstraintLimits(DebugRenderer *inRenderer) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList));
+
+	for (const Ref<Constraint> &c : mConstraints)
+		c->DrawConstraintLimits(inRenderer);
+}
+
+void ConstraintManager::DrawConstraintReferenceFrame(DebugRenderer *inRenderer) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList));
+
+	for (const Ref<Constraint> &c : mConstraints)
+		c->DrawConstraintReferenceFrame(inRenderer);
+}
+#endif // JPH_DEBUG_RENDERER
+
+void ConstraintManager::SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const
+{
+	UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList));
+
+	// Write state of constraints
+	if (inFilter != nullptr)
+	{
+		// Determine which constraints to save
+		Array<Constraint *> constraints;
+		constraints.reserve(mConstraints.size());
+		for (const Ref<Constraint> &c : mConstraints)
+			if (inFilter->ShouldSaveConstraint(*c))
+				constraints.push_back(c);
+
+		// Save them
+		uint32 num_constraints = (uint32)constraints.size();
+		inStream.Write(num_constraints);
+		for (const Constraint *c : constraints)
+		{
+			inStream.Write(c->mConstraintIndex);
+			c->SaveState(inStream);
+		}
+	}
+	else
+	{
+		// Save all constraints
+		uint32 num_constraints = (uint32)mConstraints.size();
+		inStream.Write(num_constraints);
+		for (const Ref<Constraint> &c : mConstraints)
+		{
+			inStream.Write(c->mConstraintIndex);
+			c->SaveState(inStream);
+		}
+	}
+}
+
+bool ConstraintManager::RestoreState(StateRecorder &inStream)
+{
+	UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList));
+
+	if (inStream.IsValidating())
+	{
+		// Read state of constraints
+		uint32 num_constraints = (uint32)mConstraints.size(); // Initialize to current value for validation
+		inStream.Read(num_constraints);
+		if (num_constraints != mConstraints.size())
+		{
+			JPH_ASSERT(false, "Cannot handle adding/removing constraints");
+			return false;
+		}
+		for (const Ref<Constraint> &c : mConstraints)
+		{
+			uint32 constraint_index = c->mConstraintIndex;
+			inStream.Read(constraint_index);
+			if (constraint_index != c->mConstraintIndex)
+			{
+				JPH_ASSERT(false, "Unexpected constraint index");
+				return false;
+			}
+			c->RestoreState(inStream);
+		}
+	}
+	else
+	{
+		// Not validating, use more flexible reading, read number of constraints
+		uint32 num_constraints = 0;
+		inStream.Read(num_constraints);
+
+		for (uint32 idx = 0; idx < num_constraints; ++idx)
+		{
+			uint32 constraint_index;
+			inStream.Read(constraint_index);
+			if (mConstraints.size() <= constraint_index)
+			{
+				JPH_ASSERT(false, "Restoring state for non-existing constraint");
+				return false;
+			}
+			mConstraints[constraint_index]->RestoreState(inStream);
+		}
+	}
+
+	return true;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintManager.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintManager.h
new file mode 100644
index 0000000000..e1bfb0f214
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintManager.h
@@ -0,0 +1,97 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/Constraint.h>
+#include <Jolt/Physics/PhysicsLock.h>
+#include <Jolt/Core/Mutex.h>
+
+JPH_NAMESPACE_BEGIN
+
+class IslandBuilder;
+class BodyManager;
+class StateRecorderFilter;
+#ifdef JPH_DEBUG_RENDERER
+class DebugRenderer;
+#endif // JPH_DEBUG_RENDERER
+
+/// A list of constraints
+using Constraints = Array<Ref<Constraint>>;
+
+/// A constraint manager manages all constraints of the same type
+class JPH_EXPORT ConstraintManager : public NonCopyable
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+#ifdef JPH_ENABLE_ASSERTS
+	/// Constructor
+							ConstraintManager(PhysicsLockContext inContext) : mLockContext(inContext) { }
+#endif // JPH_ENABLE_ASSERTS
+
+	/// Add a new constraint. This is thread safe.
+	void					Add(Constraint **inConstraints, int inNumber);
+
+	/// Remove a constraint. This is thread safe.
+	void					Remove(Constraint **inConstraint, int inNumber);
+
+	/// Get a list of all constraints
+	Constraints				GetConstraints() const;
+
+	/// Get total number of constraints
+	inline uint32			GetNumConstraints() const					{ return uint32(mConstraints.size()); }
+
+	/// Determine the active constraints of a subset of the constraints
+	void					GetActiveConstraints(uint32 inStartConstraintIdx, uint32 inEndConstraintIdx, Constraint **outActiveConstraints, uint32 &outNumActiveConstraints) const;
+
+	/// Link bodies to form islands
+	static void				sBuildIslands(Constraint **inActiveConstraints, uint32 inNumActiveConstraints, IslandBuilder &ioBuilder, BodyManager &inBodyManager);
+
+	/// In order to have a deterministic simulation, we need to sort the constraints of an island before solving them
+	static void				sSortConstraints(Constraint **inActiveConstraints, uint32 *inConstraintIdxBegin, uint32 *inConstraintIdxEnd);
+
+	/// Prior to solving the velocity constraints, you must call SetupVelocityConstraints once to precalculate values that are independent of velocity
+	static void				sSetupVelocityConstraints(Constraint **inActiveConstraints, uint32 inNumActiveConstraints, float inDeltaTime);
+
+	/// Apply last frame's impulses, must be called prior to SolveVelocityConstraints
+	template <class ConstraintCallback>
+	static void				sWarmStartVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, ConstraintCallback &ioCallback);
+
+	/// This function is called multiple times to iteratively come to a solution that meets all velocity constraints
+	static bool				sSolveVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inDeltaTime);
+
+	/// This function is called multiple times to iteratively come to a solution that meets all position constraints
+	static bool				sSolvePositionConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inDeltaTime, float inBaumgarte);
+
+#ifdef JPH_DEBUG_RENDERER
+	/// Draw all constraints
+	void					DrawConstraints(DebugRenderer *inRenderer) const;
+
+	/// Draw all constraint limits
+	void					DrawConstraintLimits(DebugRenderer *inRenderer) const;
+
+	/// Draw all constraint reference frames
+	void					DrawConstraintReferenceFrame(DebugRenderer *inRenderer) const;
+#endif // JPH_DEBUG_RENDERER
+
+	/// Save state of constraints
+	void					SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const;
+
+	/// Restore the state of constraints. Returns false if failed.
+	bool					RestoreState(StateRecorder &inStream);
+
+	/// Lock all constraints. This should only be done during PhysicsSystem::Update().
+	void					LockAllConstraints()						{ PhysicsLock::sLock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); }
+	void					UnlockAllConstraints()						{ PhysicsLock::sUnlock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); }
+
+private:
+#ifdef JPH_ENABLE_ASSERTS
+	PhysicsLockContext		mLockContext;
+#endif // JPH_ENABLE_ASSERTS
+	Constraints				mConstraints;
+	mutable Mutex			mConstraintsMutex;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h
new file mode 100644
index 0000000000..f7493102e2
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h
@@ -0,0 +1,257 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/SpringPart.h>
+#include <Jolt/Physics/Constraints/SpringSettings.h>
+#include <Jolt/Physics/StateRecorder.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Constraint that constrains rotation along 1 axis
+///
+/// Based on: "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, see section 2.4.5
+///
+/// Constraint equation (eq 108):
+///
+/// \f[C = \theta(t) - \theta_{min}\f]
+///
+/// Jacobian (eq 109):
+///
+/// \f[J = \begin{bmatrix}0 & -a^T & 0 & a^T\end{bmatrix}\f]
+///
+/// Used terms (here and below, everything in world space):\n
+/// a = axis around which rotation is constrained (normalized).\n
+/// x1, x2 = center of mass for the bodies.\n
+/// v = [v1, w1, v2, w2].\n
+/// v1, v2 = linear velocity of body 1 and 2.\n
+/// w1, w2 = angular velocity of body 1 and 2.\n
+/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n
+/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n
+/// b = velocity bias.\n
+/// \f$\beta\f$ = baumgarte constant.
+class AngleConstraintPart
+{
+	/// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated
+	JPH_INLINE bool				ApplyVelocityStep(Body &ioBody1, Body &ioBody2, float inLambda) const
+	{
+		// Apply impulse if delta is not zero
+		if (inLambda != 0.0f)
+		{
+			// Calculate velocity change due to constraint
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler velocity integration:
+			// v' = v + M^-1 P
+			if (ioBody1.IsDynamic())
+				ioBody1.GetMotionProperties()->SubAngularVelocityStep(inLambda * mInvI1_Axis);
+			if (ioBody2.IsDynamic())
+				ioBody2.GetMotionProperties()->AddAngularVelocityStep(inLambda * mInvI2_Axis);
+			return true;
+		}
+
+		return false;
+	}
+
+	/// Internal helper function to calculate the inverse effective mass
+	JPH_INLINE float				CalculateInverseEffectiveMass(const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis)
+	{
+		JPH_ASSERT(inWorldSpaceAxis.IsNormalized(1.0e-4f));
+
+		// Calculate properties used below
+		mInvI1_Axis = inBody1.IsDynamic()? inBody1.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody1.GetRotation(), inWorldSpaceAxis) : Vec3::sZero();
+		mInvI2_Axis = inBody2.IsDynamic()? inBody2.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody2.GetRotation(), inWorldSpaceAxis) : Vec3::sZero();
+
+		// Calculate inverse effective mass: K = J M^-1 J^T
+		return inWorldSpaceAxis.Dot(mInvI1_Axis + mInvI2_Axis);
+	}
+
+public:
+	/// Calculate properties used during the functions below
+	/// @param inBody1 The first body that this constraint is attached to
+	/// @param inBody2 The second body that this constraint is attached to
+	/// @param inWorldSpaceAxis The axis of rotation along which the constraint acts (normalized)
+	/// Set the following terms to zero if you don't want to drive the constraint to zero with a spring:
+	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
+	inline void					CalculateConstraintProperties(const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f)
+	{
+		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis);
+
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+		{
+			mEffectiveMass = 1.0f / inv_effective_mass;
+			mSpringPart.CalculateSpringPropertiesWithBias(inBias);
+		}
+	}
+
+	/// Calculate properties used during the functions below
+	/// @param inDeltaTime Time step
+	/// @param inBody1 The first body that this constraint is attached to
+	/// @param inBody2 The second body that this constraint is attached to
+	/// @param inWorldSpaceAxis The axis of rotation along which the constraint acts (normalized)
+	/// Set the following terms to zero if you don't want to drive the constraint to zero with a spring:
+	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
+	///	@param inC Value of the constraint equation (C)
+	///	@param inFrequency Oscillation frequency (Hz)
+	///	@param inDamping Damping factor (0 = no damping, 1 = critical damping)
+	inline void					CalculateConstraintPropertiesWithFrequencyAndDamping(float inDeltaTime, const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inFrequency, float inDamping)
+	{
+		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis);
+
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+			mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inFrequency, inDamping, mEffectiveMass);
+	}
+
+	/// Calculate properties used during the functions below
+	/// @param inDeltaTime Time step
+	/// @param inBody1 The first body that this constraint is attached to
+	/// @param inBody2 The second body that this constraint is attached to
+	/// @param inWorldSpaceAxis The axis of rotation along which the constraint acts (normalized)
+	/// Set the following terms to zero if you don't want to drive the constraint to zero with a spring:
+	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
+	///	@param inC Value of the constraint equation (C)
+	///	@param inStiffness Spring stiffness k.
+	///	@param inDamping Spring damping coefficient c.
+	inline void					CalculateConstraintPropertiesWithStiffnessAndDamping(float inDeltaTime, const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inStiffness, float inDamping)
+	{
+		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis);
+
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+			mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inStiffness, inDamping, mEffectiveMass);
+	}
+
+	/// Selects one of the above functions based on the spring settings
+	inline void					CalculateConstraintPropertiesWithSettings(float inDeltaTime, const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, const SpringSettings &inSpringSettings)
+	{
+		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis);
+
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else if (inSpringSettings.mMode == ESpringMode::FrequencyAndDamping)
+			mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mFrequency, inSpringSettings.mDamping, mEffectiveMass);
+		else
+			mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mStiffness, inSpringSettings.mDamping, mEffectiveMass);
+	}
+
+	/// Deactivate this constraint
+	inline void					Deactivate()
+	{
+		mEffectiveMass = 0.0f;
+		mTotalLambda = 0.0f;
+	}
+
+	/// Check if constraint is active
+	inline bool					IsActive() const
+	{
+		return mEffectiveMass != 0.0f;
+	}
+
+	/// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame
+	inline void					WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio)
+	{
+		mTotalLambda *= inWarmStartImpulseRatio;
+		ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda);
+	}
+
+	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inWorldSpaceAxis The axis of rotation along which the constraint acts (normalized)
+	/// @param inMinLambda Minimum angular impulse to apply (N m s)
+	/// @param inMaxLambda Maximum angular impulse to apply (N m s)
+	inline bool					SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda)
+	{
+		// Lagrange multiplier is:
+		//
+		// lambda = -K^-1 (J v + b)
+		float lambda = mEffectiveMass * (inWorldSpaceAxis.Dot(ioBody1.GetAngularVelocity() - ioBody2.GetAngularVelocity()) - mSpringPart.GetBias(mTotalLambda));
+		float new_lambda = Clamp(mTotalLambda + lambda, inMinLambda, inMaxLambda); // Clamp impulse
+		lambda = new_lambda - mTotalLambda; // Lambda potentially got clamped, calculate the new impulse to apply
+		mTotalLambda = new_lambda; // Store accumulated impulse
+
+		return ApplyVelocityStep(ioBody1, ioBody2, lambda);
+	}
+
+	/// Return lagrange multiplier
+	float						GetTotalLambda() const
+	{
+		return mTotalLambda;
+	}
+
+	/// Iteratively update the position constraint. Makes sure C(...) == 0.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inC Value of the constraint equation (C)
+	/// @param inBaumgarte Baumgarte constant (fraction of the error to correct)
+	inline bool					SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inC, float inBaumgarte) const
+	{
+		// Only apply position constraint when the constraint is hard, otherwise the velocity bias will fix the constraint
+		if (inC != 0.0f && !mSpringPart.IsActive())
+		{
+			// Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
+			//
+			// lambda = -K^-1 * beta / dt * C
+			//
+			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
+			float lambda = -mEffectiveMass * inBaumgarte * inC;
+
+			// Directly integrate velocity change for one time step
+			//
+			// Euler velocity integration:
+			// dv = M^-1 P
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler position integration:
+			// x' = x + dv * dt
+			//
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
+			// integrate + a position integrate and then discard the velocity change.
+			if (ioBody1.IsDynamic())
+				ioBody1.SubRotationStep(lambda * mInvI1_Axis);
+			if (ioBody2.IsDynamic())
+				ioBody2.AddRotationStep(lambda * mInvI2_Axis);
+			return true;
+		}
+
+		return false;
+	}
+
+	/// Save state of this constraint part
+	void						SaveState(StateRecorder &inStream) const
+	{
+		inStream.Write(mTotalLambda);
+	}
+
+	/// Restore state of this constraint part
+	void						RestoreState(StateRecorder &inStream)
+	{
+		inStream.Read(mTotalLambda);
+	}
+
+private:
+	Vec3						mInvI1_Axis;
+	Vec3						mInvI2_Axis;
+	float						mEffectiveMass = 0.0f;
+	SpringPart					mSpringPart;
+	float						mTotalLambda = 0.0f;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h
new file mode 100644
index 0000000000..a41ee3af3c
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h
@@ -0,0 +1,682 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/SpringPart.h>
+#include <Jolt/Physics/Constraints/SpringSettings.h>
+#include <Jolt/Physics/StateRecorder.h>
+#include <Jolt/Physics/DeterminismLog.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Constraint that constrains motion along 1 axis
+///
+/// @see "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, section 2.1.1
+/// (we're not using the approximation of eq 27 but instead add the U term as in eq 55)
+///
+/// Constraint equation (eq 25):
+///
+/// \f[C = (p_2 - p_1) \cdot n\f]
+///
+/// Jacobian (eq 28):
+///
+/// \f[J = \begin{bmatrix} -n^T & (-(r_1 + u) \times n)^T & n^T & (r_2 \times n)^T \end{bmatrix}\f]
+///
+/// Used terms (here and below, everything in world space):\n
+/// n = constraint axis (normalized).\n
+/// p1, p2 = constraint points.\n
+/// r1 = p1 - x1.\n
+/// r2 = p2 - x2.\n
+/// u = x2 + r2 - x1 - r1 = p2 - p1.\n
+/// x1, x2 = center of mass for the bodies.\n
+/// v = [v1, w1, v2, w2].\n
+/// v1, v2 = linear velocity of body 1 and 2.\n
+/// w1, w2 = angular velocity of body 1 and 2.\n
+/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n
+/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n
+/// b = velocity bias.\n
+/// \f$\beta\f$ = baumgarte constant.
+class AxisConstraintPart
+{
+	/// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated
+	template <EMotionType Type1, EMotionType Type2>
+	JPH_INLINE bool				ApplyVelocityStep(MotionProperties *ioMotionProperties1, float inInvMass1, MotionProperties *ioMotionProperties2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inLambda) const
+	{
+		// Apply impulse if delta is not zero
+		if (inLambda != 0.0f)
+		{
+			// Calculate velocity change due to constraint
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler velocity integration:
+			// v' = v + M^-1 P
+			if constexpr (Type1 == EMotionType::Dynamic)
+			{
+				ioMotionProperties1->SubLinearVelocityStep((inLambda * inInvMass1) * inWorldSpaceAxis);
+				ioMotionProperties1->SubAngularVelocityStep(inLambda * Vec3::sLoadFloat3Unsafe(mInvI1_R1PlusUxAxis));
+			}
+			if constexpr (Type2 == EMotionType::Dynamic)
+			{
+				ioMotionProperties2->AddLinearVelocityStep((inLambda * inInvMass2) * inWorldSpaceAxis);
+				ioMotionProperties2->AddAngularVelocityStep(inLambda * Vec3::sLoadFloat3Unsafe(mInvI2_R2xAxis));
+			}
+			return true;
+		}
+
+		return false;
+	}
+
+	/// Internal helper function to calculate the inverse effective mass
+	template <EMotionType Type1, EMotionType Type2>
+	JPH_INLINE float			TemplatedCalculateInverseEffectiveMass(float inInvMass1, Mat44Arg inInvI1, Vec3Arg inR1PlusU, float inInvMass2, Mat44Arg inInvI2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis)
+	{
+		JPH_ASSERT(inWorldSpaceAxis.IsNormalized(1.0e-5f));
+
+		// Calculate properties used below
+		Vec3 r1_plus_u_x_axis;
+		if constexpr (Type1 != EMotionType::Static)
+		{
+			r1_plus_u_x_axis = inR1PlusU.Cross(inWorldSpaceAxis);
+			r1_plus_u_x_axis.StoreFloat3(&mR1PlusUxAxis);
+		}
+		else
+		{
+		#ifdef JPH_DEBUG
+			Vec3::sNaN().StoreFloat3(&mR1PlusUxAxis);
+		#endif
+		}
+
+		Vec3 r2_x_axis;
+		if constexpr (Type2 != EMotionType::Static)
+		{
+			r2_x_axis = inR2.Cross(inWorldSpaceAxis);
+			r2_x_axis.StoreFloat3(&mR2xAxis);
+		}
+		else
+		{
+		#ifdef JPH_DEBUG
+			Vec3::sNaN().StoreFloat3(&mR2xAxis);
+		#endif
+		}
+
+		// Calculate inverse effective mass: K = J M^-1 J^T
+		float inv_effective_mass;
+
+		if constexpr (Type1 == EMotionType::Dynamic)
+		{
+			Vec3 invi1_r1_plus_u_x_axis = inInvI1.Multiply3x3(r1_plus_u_x_axis);
+			invi1_r1_plus_u_x_axis.StoreFloat3(&mInvI1_R1PlusUxAxis);
+			inv_effective_mass = inInvMass1 + invi1_r1_plus_u_x_axis.Dot(r1_plus_u_x_axis);
+		}
+		else
+		{
+			(void)r1_plus_u_x_axis; // Fix compiler warning: Not using this (it's not calculated either)
+			JPH_IF_DEBUG(Vec3::sNaN().StoreFloat3(&mInvI1_R1PlusUxAxis);)
+			inv_effective_mass = 0.0f;
+		}
+
+		if constexpr (Type2 == EMotionType::Dynamic)
+		{
+			Vec3 invi2_r2_x_axis = inInvI2.Multiply3x3(r2_x_axis);
+			invi2_r2_x_axis.StoreFloat3(&mInvI2_R2xAxis);
+			inv_effective_mass += inInvMass2 + invi2_r2_x_axis.Dot(r2_x_axis);
+		}
+		else
+		{
+			(void)r2_x_axis; // Fix compiler warning: Not using this (it's not calculated either)
+			JPH_IF_DEBUG(Vec3::sNaN().StoreFloat3(&mInvI2_R2xAxis);)
+		}
+
+		return inv_effective_mass;
+	}
+
+	/// Internal helper function to calculate the inverse effective mass
+	JPH_INLINE float			CalculateInverseEffectiveMass(const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis)
+	{
+		// Dispatch to the correct templated form
+		switch (inBody1.GetMotionType())
+		{
+		case EMotionType::Dynamic:
+			{
+				const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked();
+				float inv_m1 = mp1->GetInverseMass();
+				Mat44 inv_i1 = inBody1.GetInverseInertia();
+				switch (inBody2.GetMotionType())
+				{
+				case EMotionType::Dynamic:
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Dynamic>(inv_m1, inv_i1, inR1PlusU, inBody2.GetMotionPropertiesUnchecked()->GetInverseMass(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
+
+				case EMotionType::Kinematic:
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Kinematic>(inv_m1, inv_i1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis);
+
+				case EMotionType::Static:
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Static>(inv_m1, inv_i1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis);
+
+				default:
+					break;
+				}
+				break;
+			}
+
+		case EMotionType::Kinematic:
+			JPH_ASSERT(inBody2.IsDynamic());
+			return TemplatedCalculateInverseEffectiveMass<EMotionType::Kinematic, EMotionType::Dynamic>(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inBody2.GetMotionPropertiesUnchecked()->GetInverseMass(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
+
+		case EMotionType::Static:
+			JPH_ASSERT(inBody2.IsDynamic());
+			return TemplatedCalculateInverseEffectiveMass<EMotionType::Static, EMotionType::Dynamic>(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inBody2.GetMotionPropertiesUnchecked()->GetInverseMass(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
+
+		default:
+			break;
+		}
+
+		JPH_ASSERT(false);
+		return 0.0f;
+	}
+
+	/// Internal helper function to calculate the inverse effective mass, version that supports mass scaling
+	JPH_INLINE float			CalculateInverseEffectiveMassWithMassOverride(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, Vec3Arg inR1PlusU, const Body &inBody2, float inInvMass2, float inInvInertiaScale2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis)
+	{
+		// Dispatch to the correct templated form
+		switch (inBody1.GetMotionType())
+		{
+		case EMotionType::Dynamic:
+			{
+				Mat44 inv_i1 = inInvInertiaScale1 * inBody1.GetInverseInertia();
+				switch (inBody2.GetMotionType())
+				{
+				case EMotionType::Dynamic:
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Dynamic>(inInvMass1, inv_i1, inR1PlusU, inInvMass2, inInvInertiaScale2 * inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
+
+				case EMotionType::Kinematic:
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Kinematic>(inInvMass1, inv_i1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis);
+
+				case EMotionType::Static:
+					return TemplatedCalculateInverseEffectiveMass<EMotionType::Dynamic, EMotionType::Static>(inInvMass1, inv_i1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis);
+
+				default:
+					break;
+				}
+				break;
+			}
+
+		case EMotionType::Kinematic:
+			JPH_ASSERT(inBody2.IsDynamic());
+			return TemplatedCalculateInverseEffectiveMass<EMotionType::Kinematic, EMotionType::Dynamic>(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inInvMass2, inInvInertiaScale2 * inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
+
+		case EMotionType::Static:
+			JPH_ASSERT(inBody2.IsDynamic());
+			return TemplatedCalculateInverseEffectiveMass<EMotionType::Static, EMotionType::Dynamic>(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inInvMass2, inInvInertiaScale2 * inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis);
+
+		default:
+			break;
+		}
+
+		JPH_ASSERT(false);
+		return 0.0f;
+	}
+
+public:
+	/// Templated form of CalculateConstraintProperties with the motion types baked in
+	template <EMotionType Type1, EMotionType Type2>
+	JPH_INLINE void				TemplatedCalculateConstraintProperties(float inInvMass1, Mat44Arg inInvI1, Vec3Arg inR1PlusU, float inInvMass2, Mat44Arg inInvI2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f)
+	{
+		float inv_effective_mass = TemplatedCalculateInverseEffectiveMass<Type1, Type2>(inInvMass1, inInvI1, inR1PlusU, inInvMass2, inInvI2, inR2, inWorldSpaceAxis);
+
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+		{
+			mEffectiveMass = 1.0f / inv_effective_mass;
+			mSpringPart.CalculateSpringPropertiesWithBias(inBias);
+		}
+
+		JPH_DET_LOG("TemplatedCalculateConstraintProperties: invM1: " << inInvMass1 << " invI1: " << inInvI1 << " r1PlusU: " << inR1PlusU << " invM2: " << inInvMass2 << " invI2: " << inInvI2 << " r2: " << inR2 << " bias: " << inBias << " r1PlusUxAxis: " << mR1PlusUxAxis << " r2xAxis: " << mR2xAxis << " invI1_R1PlusUxAxis: " << mInvI1_R1PlusUxAxis << " invI2_R2xAxis: " << mInvI2_R2xAxis << " effectiveMass: " << mEffectiveMass << " totalLambda: " << mTotalLambda);
+	}
+
+	/// Calculate properties used during the functions below
+	/// @param inBody1 The first body that this constraint is attached to
+	/// @param inBody2 The second body that this constraint is attached to
+	/// @param inR1PlusU See equations above (r1 + u)
+	/// @param inR2 See equations above (r2)
+	/// @param inWorldSpaceAxis Axis along which the constraint acts (normalized, pointing from body 1 to 2)
+	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
+	inline void					CalculateConstraintProperties(const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f)
+	{
+		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis);
+
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+		{
+			mEffectiveMass = 1.0f / inv_effective_mass;
+			mSpringPart.CalculateSpringPropertiesWithBias(inBias);
+		}
+	}
+
+	/// Calculate properties used during the functions below, version that supports mass scaling
+	/// @param inBody1 The first body that this constraint is attached to
+	/// @param inBody2 The second body that this constraint is attached to
+	/// @param inInvMass1 The inverse mass of body 1 (only used when body 1 is dynamic)
+	/// @param inInvMass2 The inverse mass of body 2 (only used when body 2 is dynamic)
+	/// @param inInvInertiaScale1 Scale factor for the inverse inertia of body 1
+	/// @param inInvInertiaScale2 Scale factor for the inverse inertia of body 2
+	/// @param inR1PlusU See equations above (r1 + u)
+	/// @param inR2 See equations above (r2)
+	/// @param inWorldSpaceAxis Axis along which the constraint acts (normalized, pointing from body 1 to 2)
+	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
+	inline void					CalculateConstraintPropertiesWithMassOverride(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, Vec3Arg inR1PlusU, const Body &inBody2, float inInvMass2, float inInvInertiaScale2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f)
+	{
+		float inv_effective_mass = CalculateInverseEffectiveMassWithMassOverride(inBody1, inInvMass1, inInvInertiaScale1, inR1PlusU, inBody2, inInvMass2, inInvInertiaScale2, inR2, inWorldSpaceAxis);
+
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+		{
+			mEffectiveMass = 1.0f / inv_effective_mass;
+			mSpringPart.CalculateSpringPropertiesWithBias(inBias);
+		}
+	}
+
+	/// Calculate properties used during the functions below
+	/// @param inDeltaTime Time step
+	/// @param inBody1 The first body that this constraint is attached to
+	/// @param inBody2 The second body that this constraint is attached to
+	/// @param inR1PlusU See equations above (r1 + u)
+	/// @param inR2 See equations above (r2)
+	/// @param inWorldSpaceAxis Axis along which the constraint acts (normalized, pointing from body 1 to 2)
+	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
+	///	@param inC Value of the constraint equation (C).
+	///	@param inFrequency Oscillation frequency (Hz).
+	///	@param inDamping Damping factor (0 = no damping, 1 = critical damping).
+	inline void					CalculateConstraintPropertiesWithFrequencyAndDamping(float inDeltaTime, const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inFrequency, float inDamping)
+	{
+		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis);
+
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+			mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inFrequency, inDamping, mEffectiveMass);
+	}
+
+	/// Calculate properties used during the functions below
+	/// @param inDeltaTime Time step
+	/// @param inBody1 The first body that this constraint is attached to
+	/// @param inBody2 The second body that this constraint is attached to
+	/// @param inR1PlusU See equations above (r1 + u)
+	/// @param inR2 See equations above (r2)
+	/// @param inWorldSpaceAxis Axis along which the constraint acts (normalized, pointing from body 1 to 2)
+	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
+	///	@param inC Value of the constraint equation (C).
+	///	@param inStiffness Spring stiffness k.
+	///	@param inDamping Spring damping coefficient c.
+	inline void					CalculateConstraintPropertiesWithStiffnessAndDamping(float inDeltaTime, const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inStiffness, float inDamping)
+	{
+		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis);
+
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+			mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inStiffness, inDamping, mEffectiveMass);
+	}
+
+	/// Selects one of the above functions based on the spring settings
+	inline void					CalculateConstraintPropertiesWithSettings(float inDeltaTime, const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, const SpringSettings &inSpringSettings)
+	{
+		float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis);
+
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else if (inSpringSettings.mMode == ESpringMode::FrequencyAndDamping)
+			mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mFrequency, inSpringSettings.mDamping, mEffectiveMass);
+		else
+			mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mStiffness, inSpringSettings.mDamping, mEffectiveMass);
+	}
+
+	/// Deactivate this constraint
+	inline void					Deactivate()
+	{
+		mEffectiveMass = 0.0f;
+		mTotalLambda = 0.0f;
+	}
+
+	/// Check if constraint is active
+	inline bool					IsActive() const
+	{
+		return mEffectiveMass != 0.0f;
+	}
+
+	/// Templated form of WarmStart with the motion types baked in
+	template <EMotionType Type1, EMotionType Type2>
+	inline void					TemplatedWarmStart(MotionProperties *ioMotionProperties1, float inInvMass1, MotionProperties *ioMotionProperties2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inWarmStartImpulseRatio)
+	{
+		mTotalLambda *= inWarmStartImpulseRatio;
+
+		ApplyVelocityStep<Type1, Type2>(ioMotionProperties1, inInvMass1, ioMotionProperties2, inInvMass2, inWorldSpaceAxis, mTotalLambda);
+	}
+
+	/// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inWorldSpaceAxis Axis along which the constraint acts (normalized)
+	/// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame
+	inline void					WarmStart(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inWarmStartImpulseRatio)
+	{
+		EMotionType motion_type1 = ioBody1.GetMotionType();
+		MotionProperties *motion_properties1 = ioBody1.GetMotionPropertiesUnchecked();
+
+		EMotionType motion_type2 = ioBody2.GetMotionType();
+		MotionProperties *motion_properties2 = ioBody2.GetMotionPropertiesUnchecked();
+
+		// Dispatch to the correct templated form
+		// Note: Warm starting doesn't differentiate between kinematic/static bodies so we handle both as static bodies
+		if (motion_type1 == EMotionType::Dynamic)
+		{
+			if (motion_type2 == EMotionType::Dynamic)
+				TemplatedWarmStart<EMotionType::Dynamic, EMotionType::Dynamic>(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inWarmStartImpulseRatio);
+			else
+				TemplatedWarmStart<EMotionType::Dynamic, EMotionType::Static>(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inWarmStartImpulseRatio);
+		}
+		else
+		{
+			JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
+			TemplatedWarmStart<EMotionType::Static, EMotionType::Dynamic>(motion_properties1, 0.0f /* Unused */, motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inWarmStartImpulseRatio);
+		}
+	}
+
+	/// Templated form of SolveVelocityConstraint with the motion types baked in, part 1: get the total lambda
+	template <EMotionType Type1, EMotionType Type2>
+	JPH_INLINE float			TemplatedSolveVelocityConstraintGetTotalLambda(const MotionProperties *ioMotionProperties1, const MotionProperties *ioMotionProperties2, Vec3Arg inWorldSpaceAxis) const
+	{
+		// Calculate jacobian multiplied by linear velocity
+		float jv;
+		if constexpr (Type1 != EMotionType::Static && Type2 != EMotionType::Static)
+			jv = inWorldSpaceAxis.Dot(ioMotionProperties1->GetLinearVelocity() - ioMotionProperties2->GetLinearVelocity());
+		else if constexpr (Type1 != EMotionType::Static)
+			jv = inWorldSpaceAxis.Dot(ioMotionProperties1->GetLinearVelocity());
+		else if constexpr (Type2 != EMotionType::Static)
+			jv = inWorldSpaceAxis.Dot(-ioMotionProperties2->GetLinearVelocity());
+		else
+			JPH_ASSERT(false); // Static vs static is nonsensical!
+
+		// Calculate jacobian multiplied by angular velocity
+		if constexpr (Type1 != EMotionType::Static)
+			jv += Vec3::sLoadFloat3Unsafe(mR1PlusUxAxis).Dot(ioMotionProperties1->GetAngularVelocity());
+		if constexpr (Type2 != EMotionType::Static)
+			jv -= Vec3::sLoadFloat3Unsafe(mR2xAxis).Dot(ioMotionProperties2->GetAngularVelocity());
+
+		// Lagrange multiplier is:
+		//
+		// lambda = -K^-1 (J v + b)
+		float lambda = mEffectiveMass * (jv - mSpringPart.GetBias(mTotalLambda));
+
+		// Return the total accumulated lambda
+		return mTotalLambda + lambda;
+	}
+
+	/// Templated form of SolveVelocityConstraint with the motion types baked in, part 2: apply new lambda
+	template <EMotionType Type1, EMotionType Type2>
+	JPH_INLINE bool				TemplatedSolveVelocityConstraintApplyLambda(MotionProperties *ioMotionProperties1, float inInvMass1, MotionProperties *ioMotionProperties2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inTotalLambda)
+	{
+		float delta_lambda = inTotalLambda - mTotalLambda; // Calculate change in lambda
+		mTotalLambda = inTotalLambda; // Store accumulated impulse
+
+		return ApplyVelocityStep<Type1, Type2>(ioMotionProperties1, inInvMass1, ioMotionProperties2, inInvMass2, inWorldSpaceAxis, delta_lambda);
+	}
+
+	/// Templated form of SolveVelocityConstraint with the motion types baked in
+	template <EMotionType Type1, EMotionType Type2>
+	inline bool					TemplatedSolveVelocityConstraint(MotionProperties *ioMotionProperties1, float inInvMass1, MotionProperties *ioMotionProperties2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda)
+	{
+		float total_lambda = TemplatedSolveVelocityConstraintGetTotalLambda<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, inWorldSpaceAxis);
+
+		// Clamp impulse to specified range
+		total_lambda = Clamp(total_lambda, inMinLambda, inMaxLambda);
+
+		return TemplatedSolveVelocityConstraintApplyLambda<Type1, Type2>(ioMotionProperties1, inInvMass1, ioMotionProperties2, inInvMass2, inWorldSpaceAxis, total_lambda);
+	}
+
+	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inWorldSpaceAxis Axis along which the constraint acts (normalized)
+	/// @param inMinLambda Minimum value of constraint impulse to apply (N s)
+	/// @param inMaxLambda Maximum value of constraint impulse to apply (N s)
+	inline bool					SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda)
+	{
+		EMotionType motion_type1 = ioBody1.GetMotionType();
+		MotionProperties *motion_properties1 = ioBody1.GetMotionPropertiesUnchecked();
+
+		EMotionType motion_type2 = ioBody2.GetMotionType();
+		MotionProperties *motion_properties2 = ioBody2.GetMotionPropertiesUnchecked();
+
+		// Dispatch to the correct templated form
+		switch (motion_type1)
+		{
+		case EMotionType::Dynamic:
+			switch (motion_type2)
+			{
+			case EMotionType::Dynamic:
+				return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Dynamic>(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+			case EMotionType::Kinematic:
+				return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Kinematic>(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+			case EMotionType::Static:
+				return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Static>(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+			default:
+				JPH_ASSERT(false);
+				break;
+			}
+			break;
+
+		case EMotionType::Kinematic:
+			JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
+			return TemplatedSolveVelocityConstraint<EMotionType::Kinematic, EMotionType::Dynamic>(motion_properties1, 0.0f /* Unused */, motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+		case EMotionType::Static:
+			JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
+			return TemplatedSolveVelocityConstraint<EMotionType::Static, EMotionType::Dynamic>(motion_properties1, 0.0f /* Unused */, motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+		default:
+			JPH_ASSERT(false);
+			break;
+		}
+
+		return false;
+	}
+
+	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inInvMass1 The inverse mass of body 1 (only used when body 1 is dynamic)
+	/// @param inInvMass2 The inverse mass of body 2 (only used when body 2 is dynamic)
+	/// @param inWorldSpaceAxis Axis along which the constraint acts (normalized)
+	/// @param inMinLambda Minimum value of constraint impulse to apply (N s)
+	/// @param inMaxLambda Maximum value of constraint impulse to apply (N s)
+	inline bool					SolveVelocityConstraintWithMassOverride(Body &ioBody1, float inInvMass1, Body &ioBody2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda)
+	{
+		EMotionType motion_type1 = ioBody1.GetMotionType();
+		MotionProperties *motion_properties1 = ioBody1.GetMotionPropertiesUnchecked();
+
+		EMotionType motion_type2 = ioBody2.GetMotionType();
+		MotionProperties *motion_properties2 = ioBody2.GetMotionPropertiesUnchecked();
+
+		// Dispatch to the correct templated form
+		switch (motion_type1)
+		{
+		case EMotionType::Dynamic:
+			switch (motion_type2)
+			{
+			case EMotionType::Dynamic:
+				return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Dynamic>(motion_properties1, inInvMass1, motion_properties2, inInvMass2, inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+			case EMotionType::Kinematic:
+				return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Kinematic>(motion_properties1, inInvMass1, motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+			case EMotionType::Static:
+				return TemplatedSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Static>(motion_properties1, inInvMass1, motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+			default:
+				JPH_ASSERT(false);
+				break;
+			}
+			break;
+
+		case EMotionType::Kinematic:
+			JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
+			return TemplatedSolveVelocityConstraint<EMotionType::Kinematic, EMotionType::Dynamic>(motion_properties1, 0.0f /* Unused */, motion_properties2, inInvMass2, inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+		case EMotionType::Static:
+			JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
+			return TemplatedSolveVelocityConstraint<EMotionType::Static, EMotionType::Dynamic>(motion_properties1, 0.0f /* Unused */, motion_properties2, inInvMass2, inWorldSpaceAxis, inMinLambda, inMaxLambda);
+
+		default:
+			JPH_ASSERT(false);
+			break;
+		}
+
+		return false;
+	}
+
+	/// Iteratively update the position constraint. Makes sure C(...) = 0.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inWorldSpaceAxis Axis along which the constraint acts (normalized)
+	/// @param inC Value of the constraint equation (C)
+	/// @param inBaumgarte Baumgarte constant (fraction of the error to correct)
+	inline bool					SolvePositionConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inC, float inBaumgarte) const
+	{
+		// Only apply position constraint when the constraint is hard, otherwise the velocity bias will fix the constraint
+		if (inC != 0.0f && !mSpringPart.IsActive())
+		{
+			// Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
+			//
+			// lambda = -K^-1 * beta / dt * C
+			//
+			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
+			float lambda = -mEffectiveMass * inBaumgarte * inC;
+
+			// Directly integrate velocity change for one time step
+			//
+			// Euler velocity integration:
+			// dv = M^-1 P
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler position integration:
+			// x' = x + dv * dt
+			//
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
+			// integrate + a position integrate and then discard the velocity change.
+			if (ioBody1.IsDynamic())
+			{
+				ioBody1.SubPositionStep((lambda * ioBody1.GetMotionProperties()->GetInverseMass()) * inWorldSpaceAxis);
+				ioBody1.SubRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI1_R1PlusUxAxis));
+			}
+			if (ioBody2.IsDynamic())
+			{
+				ioBody2.AddPositionStep((lambda * ioBody2.GetMotionProperties()->GetInverseMass()) * inWorldSpaceAxis);
+				ioBody2.AddRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI2_R2xAxis));
+			}
+			return true;
+		}
+
+		return false;
+	}
+
+	/// Iteratively update the position constraint. Makes sure C(...) = 0.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inInvMass1 The inverse mass of body 1 (only used when body 1 is dynamic)
+	/// @param inInvMass2 The inverse mass of body 2 (only used when body 2 is dynamic)
+	/// @param inWorldSpaceAxis Axis along which the constraint acts (normalized)
+	/// @param inC Value of the constraint equation (C)
+	/// @param inBaumgarte Baumgarte constant (fraction of the error to correct)
+	inline bool					SolvePositionConstraintWithMassOverride(Body &ioBody1, float inInvMass1, Body &ioBody2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inC, float inBaumgarte) const
+	{
+		// Only apply position constraint when the constraint is hard, otherwise the velocity bias will fix the constraint
+		if (inC != 0.0f && !mSpringPart.IsActive())
+		{
+			// Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
+			//
+			// lambda = -K^-1 * beta / dt * C
+			//
+			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
+			float lambda = -mEffectiveMass * inBaumgarte * inC;
+
+			// Directly integrate velocity change for one time step
+			//
+			// Euler velocity integration:
+			// dv = M^-1 P
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler position integration:
+			// x' = x + dv * dt
+			//
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
+			// integrate + a position integrate and then discard the velocity change.
+			if (ioBody1.IsDynamic())
+			{
+				ioBody1.SubPositionStep((lambda * inInvMass1) * inWorldSpaceAxis);
+				ioBody1.SubRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI1_R1PlusUxAxis));
+			}
+			if (ioBody2.IsDynamic())
+			{
+				ioBody2.AddPositionStep((lambda * inInvMass2) * inWorldSpaceAxis);
+				ioBody2.AddRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI2_R2xAxis));
+			}
+			return true;
+		}
+
+		return false;
+	}
+
+	/// Override total lagrange multiplier, can be used to set the initial value for warm starting
+	inline void					SetTotalLambda(float inLambda)
+	{
+		mTotalLambda = inLambda;
+	}
+
+	/// Return lagrange multiplier
+	inline float				GetTotalLambda() const
+	{
+		return mTotalLambda;
+	}
+
+	/// Save state of this constraint part
+	void						SaveState(StateRecorder &inStream) const
+	{
+		inStream.Write(mTotalLambda);
+	}
+
+	/// Restore state of this constraint part
+	void						RestoreState(StateRecorder &inStream)
+	{
+		inStream.Read(mTotalLambda);
+	}
+
+private:
+	Float3						mR1PlusUxAxis;
+	Float3						mR2xAxis;
+	Float3						mInvI1_R1PlusUxAxis;
+	Float3						mInvI2_R2xAxis;
+	float						mEffectiveMass = 0.0f;
+	SpringPart					mSpringPart;
+	float						mTotalLambda = 0.0f;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/DualAxisConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/DualAxisConstraintPart.h
new file mode 100644
index 0000000000..c0ebe5ac42
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/DualAxisConstraintPart.h
@@ -0,0 +1,276 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Physics/StateRecorder.h>
+#include <Jolt/Math/Vector.h>
+#include <Jolt/Math/Matrix.h>
+
+JPH_NAMESPACE_BEGIN
+
+/**
+	Constrains movement on 2 axis
+
+	@see "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, section 2.3.1
+
+	Constraint equation (eq 51):
+
+	\f[C = \begin{bmatrix} (p_2 - p_1) \cdot n_1 \\ (p_2 - p_1) \cdot n_2\end{bmatrix}\f]
+
+	Jacobian (transposed) (eq 55):
+
+	\f[J^T = \begin{bmatrix}
+	-n_1					& -n_2					\\
+	-(r_1 + u) \times n_1	& -(r_1 + u) \times n_2	\\
+	n_1						& n_2					\\
+	r_2 \times n_1			& r_2 \times n_2
+	\end{bmatrix}\f]
+
+	Used terms (here and below, everything in world space):\n
+	n1, n2 = constraint axis (normalized).\n
+	p1, p2 = constraint points.\n
+	r1 = p1 - x1.\n
+	r2 = p2 - x2.\n
+	u = x2 + r2 - x1 - r1 = p2 - p1.\n
+	x1, x2 = center of mass for the bodies.\n
+	v = [v1, w1, v2, w2].\n
+	v1, v2 = linear velocity of body 1 and 2.\n
+	w1, w2 = angular velocity of body 1 and 2.\n
+	M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n
+	\f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n
+	b = velocity bias.\n
+	\f$\beta\f$ = baumgarte constant.
+**/
+class DualAxisConstraintPart
+{
+public:
+	using Vec2 = Vector<2>;
+	using Mat22 = Matrix<2, 2>;
+
+private:
+	/// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated
+	JPH_INLINE bool				ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, const Vec2 &inLambda) const
+	{
+		// Apply impulse if delta is not zero
+		if (!inLambda.IsZero())
+		{
+			// Calculate velocity change due to constraint
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler velocity integration:
+			// v' = v + M^-1 P
+			Vec3 impulse = inN1 * inLambda[0] + inN2 * inLambda[1];
+			if (ioBody1.IsDynamic())
+			{
+				MotionProperties *mp1 = ioBody1.GetMotionProperties();
+				mp1->SubLinearVelocityStep(mp1->GetInverseMass() * impulse);
+				mp1->SubAngularVelocityStep(mInvI1_R1PlusUxN1 * inLambda[0] + mInvI1_R1PlusUxN2 * inLambda[1]);
+			}
+			if (ioBody2.IsDynamic())
+			{
+				MotionProperties *mp2 = ioBody2.GetMotionProperties();
+				mp2->AddLinearVelocityStep(mp2->GetInverseMass() * impulse);
+				mp2->AddAngularVelocityStep(mInvI2_R2xN1 * inLambda[0] + mInvI2_R2xN2 * inLambda[1]);
+			}
+			return true;
+		}
+
+		return false;
+	}
+
+	/// Internal helper function to calculate the lagrange multiplier
+	inline void					CalculateLagrangeMultiplier(const Body &inBody1, const Body &inBody2, Vec3Arg inN1, Vec3Arg inN2, Vec2 &outLambda) const
+	{
+		// Calculate lagrange multiplier:
+		//
+		// lambda = -K^-1 (J v + b)
+		Vec3 delta_lin = inBody1.GetLinearVelocity() - inBody2.GetLinearVelocity();
+		Vec2 jv;
+		jv[0] = inN1.Dot(delta_lin) + mR1PlusUxN1.Dot(inBody1.GetAngularVelocity()) - mR2xN1.Dot(inBody2.GetAngularVelocity());
+		jv[1] = inN2.Dot(delta_lin) + mR1PlusUxN2.Dot(inBody1.GetAngularVelocity()) - mR2xN2.Dot(inBody2.GetAngularVelocity());
+		outLambda = mEffectiveMass * jv;
+	}
+
+public:
+	/// Calculate properties used during the functions below
+	/// All input vectors are in world space
+	inline void					CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, Vec3Arg inR1PlusU, const Body &inBody2, Mat44Arg inRotation2, Vec3Arg inR2, Vec3Arg inN1, Vec3Arg inN2)
+	{
+		JPH_ASSERT(inN1.IsNormalized(1.0e-5f));
+		JPH_ASSERT(inN2.IsNormalized(1.0e-5f));
+
+		// Calculate properties used during constraint solving
+		mR1PlusUxN1 = inR1PlusU.Cross(inN1);
+		mR1PlusUxN2 = inR1PlusU.Cross(inN2);
+		mR2xN1 = inR2.Cross(inN1);
+		mR2xN2 = inR2.Cross(inN2);
+
+		// Calculate effective mass: K^-1 = (J M^-1 J^T)^-1, eq 59
+		Mat22 inv_effective_mass;
+		if (inBody1.IsDynamic())
+		{
+			const MotionProperties *mp1 = inBody1.GetMotionProperties();
+			Mat44 inv_i1 = mp1->GetInverseInertiaForRotation(inRotation1);
+			mInvI1_R1PlusUxN1 = inv_i1.Multiply3x3(mR1PlusUxN1);
+			mInvI1_R1PlusUxN2 = inv_i1.Multiply3x3(mR1PlusUxN2);
+
+			inv_effective_mass(0, 0) = mp1->GetInverseMass() + mR1PlusUxN1.Dot(mInvI1_R1PlusUxN1);
+			inv_effective_mass(0, 1) = mR1PlusUxN1.Dot(mInvI1_R1PlusUxN2);
+			inv_effective_mass(1, 0) = mR1PlusUxN2.Dot(mInvI1_R1PlusUxN1);
+			inv_effective_mass(1, 1) = mp1->GetInverseMass() + mR1PlusUxN2.Dot(mInvI1_R1PlusUxN2);
+		}
+		else
+		{
+			JPH_IF_DEBUG(mInvI1_R1PlusUxN1 = Vec3::sNaN();)
+			JPH_IF_DEBUG(mInvI1_R1PlusUxN2 = Vec3::sNaN();)
+
+			inv_effective_mass = Mat22::sZero();
+		}
+
+		if (inBody2.IsDynamic())
+		{
+			const MotionProperties *mp2 = inBody2.GetMotionProperties();
+			Mat44 inv_i2 = mp2->GetInverseInertiaForRotation(inRotation2);
+			mInvI2_R2xN1 = inv_i2.Multiply3x3(mR2xN1);
+			mInvI2_R2xN2 = inv_i2.Multiply3x3(mR2xN2);
+
+			inv_effective_mass(0, 0) += mp2->GetInverseMass() + mR2xN1.Dot(mInvI2_R2xN1);
+			inv_effective_mass(0, 1) += mR2xN1.Dot(mInvI2_R2xN2);
+			inv_effective_mass(1, 0) += mR2xN2.Dot(mInvI2_R2xN1);
+			inv_effective_mass(1, 1) += mp2->GetInverseMass() + mR2xN2.Dot(mInvI2_R2xN2);
+		}
+		else
+		{
+			JPH_IF_DEBUG(mInvI2_R2xN1 = Vec3::sNaN();)
+			JPH_IF_DEBUG(mInvI2_R2xN2 = Vec3::sNaN();)
+		}
+
+		if (!mEffectiveMass.SetInversed(inv_effective_mass))
+			Deactivate();
+	}
+
+	/// Deactivate this constraint
+	inline void					Deactivate()
+	{
+		mEffectiveMass.SetZero();
+		mTotalLambda.SetZero();
+	}
+
+	/// Check if constraint is active
+	inline bool					IsActive() const
+	{
+		return !mEffectiveMass.IsZero();
+	}
+
+	/// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses
+	/// All input vectors are in world space
+	inline void					WarmStart(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inWarmStartImpulseRatio)
+	{
+		mTotalLambda *= inWarmStartImpulseRatio;
+		ApplyVelocityStep(ioBody1, ioBody2, inN1, inN2, mTotalLambda);
+	}
+
+	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.
+	/// All input vectors are in world space
+	inline bool					SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2)
+	{
+		Vec2 lambda;
+		CalculateLagrangeMultiplier(ioBody1, ioBody2, inN1, inN2, lambda);
+
+		// Store accumulated lambda
+		mTotalLambda += lambda;
+
+		return ApplyVelocityStep(ioBody1, ioBody2, inN1, inN2, lambda);
+	}
+
+	/// Iteratively update the position constraint. Makes sure C(...) = 0.
+	/// All input vectors are in world space
+	inline bool					SolvePositionConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inU, Vec3Arg inN1, Vec3Arg inN2, float inBaumgarte) const
+	{
+		Vec2 c;
+		c[0] = inU.Dot(inN1);
+		c[1] = inU.Dot(inN2);
+		if (!c.IsZero())
+		{
+			// Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
+			//
+			// lambda = -K^-1 * beta / dt * C
+			//
+			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
+			Vec2 lambda = -inBaumgarte * (mEffectiveMass * c);
+
+			// Directly integrate velocity change for one time step
+			//
+			// Euler velocity integration:
+			// dv = M^-1 P
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler position integration:
+			// x' = x + dv * dt
+			//
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
+			// integrate + a position integrate and then discard the velocity change.
+			Vec3 impulse = inN1 * lambda[0] + inN2 * lambda[1];
+			if (ioBody1.IsDynamic())
+			{
+				ioBody1.SubPositionStep(ioBody1.GetMotionProperties()->GetInverseMass() * impulse);
+				ioBody1.SubRotationStep(mInvI1_R1PlusUxN1 * lambda[0] + mInvI1_R1PlusUxN2 * lambda[1]);
+			}
+			if (ioBody2.IsDynamic())
+			{
+				ioBody2.AddPositionStep(ioBody2.GetMotionProperties()->GetInverseMass() * impulse);
+				ioBody2.AddRotationStep(mInvI2_R2xN1 * lambda[0] + mInvI2_R2xN2 * lambda[1]);
+			}
+			return true;
+		}
+
+		return false;
+	}
+
+	/// Override total lagrange multiplier, can be used to set the initial value for warm starting
+	inline void					SetTotalLambda(const Vec2 &inLambda)
+	{
+		mTotalLambda = inLambda;
+	}
+
+	/// Return lagrange multiplier
+	inline const Vec2 &			GetTotalLambda() const
+	{
+		return mTotalLambda;
+	}
+
+	/// Save state of this constraint part
+	void						SaveState(StateRecorder &inStream) const
+	{
+		inStream.Write(mTotalLambda);
+	}
+
+	/// Restore state of this constraint part
+	void						RestoreState(StateRecorder &inStream)
+	{
+		inStream.Read(mTotalLambda);
+	}
+
+private:
+	Vec3						mR1PlusUxN1;
+	Vec3						mR1PlusUxN2;
+	Vec3						mR2xN1;
+	Vec3						mR2xN2;
+	Vec3						mInvI1_R1PlusUxN1;
+	Vec3						mInvI1_R1PlusUxN2;
+	Vec3						mInvI2_R2xN1;
+	Vec3						mInvI2_R2xN2;
+	Mat22						mEffectiveMass;
+	Vec2						mTotalLambda { Vec2::sZero() };
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/GearConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/GearConstraintPart.h
new file mode 100644
index 0000000000..6e277a64b5
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/GearConstraintPart.h
@@ -0,0 +1,195 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Physics/StateRecorder.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Constraint that constrains two rotations using a gear (rotating in opposite direction)
+///
+/// Constraint equation:
+///
+/// C = Rotation1(t) + r Rotation2(t)
+///
+/// Derivative:
+///
+/// d/dt C = 0
+/// <=> w1 . a + r w2 . b = 0
+///
+/// Jacobian:
+///
+/// \f[J = \begin{bmatrix}0 & a^T & 0 & r b^T\end{bmatrix}\f]
+///
+/// Used terms (here and below, everything in world space):\n
+/// a = axis around which body 1 rotates (normalized).\n
+/// b = axis along which body 2 slides (normalized).\n
+/// Rotation1(t) = rotation around a of body 1.\n
+/// Rotation2(t) = rotation around b of body 2.\n
+/// r = ratio between rotation for body 1 and 2.\n
+/// v = [v1, w1, v2, w2].\n
+/// v1, v2 = linear velocity of body 1 and 2.\n
+/// w1, w2 = angular velocity of body 1 and 2.\n
+/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n
+/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n
+/// \f$\beta\f$ = baumgarte constant.
+class GearConstraintPart
+{
+	/// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated
+	JPH_INLINE bool				ApplyVelocityStep(Body &ioBody1, Body &ioBody2, float inLambda) const
+	{
+		// Apply impulse if delta is not zero
+		if (inLambda != 0.0f)
+		{
+			// Calculate velocity change due to constraint
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler velocity integration:
+			// v' = v + M^-1 P
+			ioBody1.GetMotionProperties()->AddAngularVelocityStep(inLambda * mInvI1_A);
+			ioBody2.GetMotionProperties()->AddAngularVelocityStep(inLambda * mInvI2_B);
+			return true;
+		}
+
+		return false;
+	}
+
+public:
+	/// Calculate properties used during the functions below
+	/// @param inBody1 The first body that this constraint is attached to
+	/// @param inBody2 The second body that this constraint is attached to
+	/// @param inWorldSpaceHingeAxis1 The axis around which body 1 rotates
+	/// @param inWorldSpaceHingeAxis2 The axis around which body 2 rotates
+	/// @param inRatio The ratio between rotation and translation
+	inline void					CalculateConstraintProperties(const Body &inBody1, Vec3Arg inWorldSpaceHingeAxis1, const Body &inBody2, Vec3Arg inWorldSpaceHingeAxis2, float inRatio)
+	{
+		JPH_ASSERT(inWorldSpaceHingeAxis1.IsNormalized(1.0e-4f));
+		JPH_ASSERT(inWorldSpaceHingeAxis2.IsNormalized(1.0e-4f));
+
+		// Calculate: I1^-1 a
+		mInvI1_A = inBody1.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody1.GetRotation(), inWorldSpaceHingeAxis1);
+
+		// Calculate: I2^-1 b
+		mInvI2_B = inBody2.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody2.GetRotation(), inWorldSpaceHingeAxis2);
+
+		// K^-1 = 1 / (J M^-1 J^T) = 1 / (a^T I1^-1 a + r^2 * b^T I2^-1 b)
+		float inv_effective_mass = (inWorldSpaceHingeAxis1.Dot(mInvI1_A) + inWorldSpaceHingeAxis2.Dot(mInvI2_B) * Square(inRatio));
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+			mEffectiveMass = 1.0f / inv_effective_mass;
+	}
+
+	/// Deactivate this constraint
+	inline void					Deactivate()
+	{
+		mEffectiveMass = 0.0f;
+		mTotalLambda = 0.0f;
+	}
+
+	/// Check if constraint is active
+	inline bool					IsActive() const
+	{
+		return mEffectiveMass != 0.0f;
+	}
+
+	/// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame
+	inline void					WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio)
+	{
+		mTotalLambda *= inWarmStartImpulseRatio;
+		ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda);
+	}
+
+	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inWorldSpaceHingeAxis1 The axis around which body 1 rotates
+	/// @param inWorldSpaceHingeAxis2 The axis around which body 2 rotates
+	/// @param inRatio The ratio between rotation and translation
+	inline bool					SolveVelocityConstraint(Body &ioBody1, Vec3Arg inWorldSpaceHingeAxis1, Body &ioBody2, Vec3Arg inWorldSpaceHingeAxis2, float inRatio)
+	{
+		// Lagrange multiplier is:
+		//
+		// lambda = -K^-1 (J v + b)
+		float lambda = -mEffectiveMass * (inWorldSpaceHingeAxis1.Dot(ioBody1.GetAngularVelocity()) + inRatio * inWorldSpaceHingeAxis2.Dot(ioBody2.GetAngularVelocity()));
+		mTotalLambda += lambda; // Store accumulated impulse
+
+		return ApplyVelocityStep(ioBody1, ioBody2, lambda);
+	}
+
+	/// Return lagrange multiplier
+	float						GetTotalLambda() const
+	{
+		return mTotalLambda;
+	}
+
+	/// Iteratively update the position constraint. Makes sure C(...) == 0.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inC Value of the constraint equation (C)
+	/// @param inBaumgarte Baumgarte constant (fraction of the error to correct)
+	inline bool					SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inC, float inBaumgarte) const
+	{
+		// Only apply position constraint when the constraint is hard, otherwise the velocity bias will fix the constraint
+		if (inC != 0.0f)
+		{
+			// Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
+			//
+			// lambda = -K^-1 * beta / dt * C
+			//
+			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
+			float lambda = -mEffectiveMass * inBaumgarte * inC;
+
+			// Directly integrate velocity change for one time step
+			//
+			// Euler velocity integration:
+			// dv = M^-1 P
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler position integration:
+			// x' = x + dv * dt
+			//
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
+			// integrate + a position integrate and then discard the velocity change.
+			if (ioBody1.IsDynamic())
+				ioBody1.AddRotationStep(lambda * mInvI1_A);
+			if (ioBody2.IsDynamic())
+				ioBody2.AddRotationStep(lambda * mInvI2_B);
+			return true;
+		}
+
+		return false;
+	}
+
+	/// Save state of this constraint part
+	void						SaveState(StateRecorder &inStream) const
+	{
+		inStream.Write(mTotalLambda);
+	}
+
+	/// Restore state of this constraint part
+	void						RestoreState(StateRecorder &inStream)
+	{
+		inStream.Read(mTotalLambda);
+	}
+
+private:
+	Vec3						mInvI1_A;
+	Vec3						mInvI2_B;
+	float						mEffectiveMass = 0.0f;
+	float						mTotalLambda = 0.0f;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/HingeRotationConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/HingeRotationConstraintPart.h
new file mode 100644
index 0000000000..5f566f7017
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/HingeRotationConstraintPart.h
@@ -0,0 +1,222 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Physics/StateRecorder.h>
+#include <Jolt/Math/Vector.h>
+#include <Jolt/Math/Matrix.h>
+
+JPH_NAMESPACE_BEGIN
+
+/**
+	Constrains rotation around 2 axis so that it only allows rotation around 1 axis
+
+	Based on: "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, section 2.4.1
+
+	Constraint equation (eq 87):
+
+	\f[C = \begin{bmatrix}a_1 \cdot b_2 \\ a_1 \cdot c_2\end{bmatrix}\f]
+
+	Jacobian (eq 90):
+
+	\f[J = \begin{bmatrix}
+	0	& -b_2 \times a_1	& 0		& b_2 \times a_1	\\
+	0	& -c_2 \times a_1	& 0		& c2 \times a_1
+	\end{bmatrix}\f]
+
+	Used terms (here and below, everything in world space):\n
+	a1 = hinge axis on body 1.\n
+	b2, c2 = axis perpendicular to hinge axis on body 2.\n
+	x1, x2 = center of mass for the bodies.\n
+	v = [v1, w1, v2, w2].\n
+	v1, v2 = linear velocity of body 1 and 2.\n
+	w1, w2 = angular velocity of body 1 and 2.\n
+	M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n
+	\f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n
+	b = velocity bias.\n
+	\f$\beta\f$ = baumgarte constant.\n
+	E = identity matrix.
+**/
+class HingeRotationConstraintPart
+{
+public:
+	using Vec2 = Vector<2>;
+	using Mat22 = Matrix<2, 2>;
+
+private:
+	/// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated
+	JPH_INLINE bool				ApplyVelocityStep(Body &ioBody1, Body &ioBody2, const Vec2 &inLambda) const
+	{
+		// Apply impulse if delta is not zero
+		if (!inLambda.IsZero())
+		{
+			// Calculate velocity change due to constraint
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler velocity integration:
+			// v' = v + M^-1 P
+			Vec3 impulse = mB2xA1 * inLambda[0] + mC2xA1 * inLambda[1];
+			if (ioBody1.IsDynamic())
+				ioBody1.GetMotionProperties()->SubAngularVelocityStep(mInvI1.Multiply3x3(impulse));
+			if (ioBody2.IsDynamic())
+				ioBody2.GetMotionProperties()->AddAngularVelocityStep(mInvI2.Multiply3x3(impulse));
+			return true;
+		}
+
+		return false;
+	}
+
+public:
+	/// Calculate properties used during the functions below
+	inline void					CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, Vec3Arg inWorldSpaceHingeAxis1, const Body &inBody2, Mat44Arg inRotation2, Vec3Arg inWorldSpaceHingeAxis2)
+	{
+		JPH_ASSERT(inWorldSpaceHingeAxis1.IsNormalized(1.0e-5f));
+		JPH_ASSERT(inWorldSpaceHingeAxis2.IsNormalized(1.0e-5f));
+
+		// Calculate hinge axis in world space
+		mA1 = inWorldSpaceHingeAxis1;
+		Vec3 a2 = inWorldSpaceHingeAxis2;
+		float dot = mA1.Dot(a2);
+		if (dot <= 1.0e-3f)
+		{
+			// World space axes are more than 90 degrees apart, get a perpendicular vector in the plane formed by mA1 and a2 as hinge axis until the rotation is less than 90 degrees
+			Vec3 perp = a2 - dot * mA1;
+			if (perp.LengthSq() < 1.0e-6f)
+			{
+				// mA1 ~ -a2, take random perpendicular
+				perp = mA1.GetNormalizedPerpendicular();
+			}
+
+			// Blend in a little bit from mA1 so we're less than 90 degrees apart
+			a2 = (0.99f * perp.Normalized() + 0.01f * mA1).Normalized();
+		}
+		mB2 = a2.GetNormalizedPerpendicular();
+		mC2 = a2.Cross(mB2);
+
+		// Calculate properties used during constraint solving
+		mInvI1 = inBody1.IsDynamic()? inBody1.GetMotionProperties()->GetInverseInertiaForRotation(inRotation1) : Mat44::sZero();
+		mInvI2 = inBody2.IsDynamic()? inBody2.GetMotionProperties()->GetInverseInertiaForRotation(inRotation2) : Mat44::sZero();
+		mB2xA1 = mB2.Cross(mA1);
+		mC2xA1 = mC2.Cross(mA1);
+
+		// Calculate effective mass: K^-1 = (J M^-1 J^T)^-1
+		Mat44 summed_inv_inertia = mInvI1 + mInvI2;
+		Mat22 inv_effective_mass;
+		inv_effective_mass(0, 0) = mB2xA1.Dot(summed_inv_inertia.Multiply3x3(mB2xA1));
+		inv_effective_mass(0, 1) = mB2xA1.Dot(summed_inv_inertia.Multiply3x3(mC2xA1));
+		inv_effective_mass(1, 0) = mC2xA1.Dot(summed_inv_inertia.Multiply3x3(mB2xA1));
+		inv_effective_mass(1, 1) = mC2xA1.Dot(summed_inv_inertia.Multiply3x3(mC2xA1));
+		if (!mEffectiveMass.SetInversed(inv_effective_mass))
+			Deactivate();
+	}
+
+	/// Deactivate this constraint
+	inline void					Deactivate()
+	{
+		mEffectiveMass.SetZero();
+		mTotalLambda.SetZero();
+	}
+
+	/// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses
+	inline void					WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio)
+	{
+		mTotalLambda *= inWarmStartImpulseRatio;
+		ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda);
+	}
+
+	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.
+	inline bool					SolveVelocityConstraint(Body &ioBody1, Body &ioBody2)
+	{
+		// Calculate lagrange multiplier:
+		//
+		// lambda = -K^-1 (J v + b)
+		Vec3 delta_ang = ioBody1.GetAngularVelocity() - ioBody2.GetAngularVelocity();
+		Vec2 jv;
+		jv[0] = mB2xA1.Dot(delta_ang);
+		jv[1] = mC2xA1.Dot(delta_ang);
+		Vec2 lambda = mEffectiveMass * jv;
+
+		// Store accumulated lambda
+		mTotalLambda += lambda;
+
+		return ApplyVelocityStep(ioBody1, ioBody2, lambda);
+	}
+
+	/// Iteratively update the position constraint. Makes sure C(...) = 0.
+	inline bool					SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inBaumgarte) const
+	{
+		// Constraint needs Axis of body 1 perpendicular to both B and C from body 2 (which are both perpendicular to the Axis of body 2)
+		Vec2 c;
+		c[0] = mA1.Dot(mB2);
+		c[1] = mA1.Dot(mC2);
+		if (!c.IsZero())
+		{
+			// Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
+			//
+			// lambda = -K^-1 * beta / dt * C
+			//
+			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
+			Vec2 lambda = -inBaumgarte * (mEffectiveMass * c);
+
+			// Directly integrate velocity change for one time step
+			//
+			// Euler velocity integration:
+			// dv = M^-1 P
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler position integration:
+			// x' = x + dv * dt
+			//
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
+			// integrate + a position integrate and then discard the velocity change.
+			Vec3 impulse = mB2xA1 * lambda[0] + mC2xA1 * lambda[1];
+			if (ioBody1.IsDynamic())
+				ioBody1.SubRotationStep(mInvI1.Multiply3x3(impulse));
+			if (ioBody2.IsDynamic())
+				ioBody2.AddRotationStep(mInvI2.Multiply3x3(impulse));
+			return true;
+		}
+
+		return false;
+	}
+
+	/// Return lagrange multiplier
+	const Vec2 &				GetTotalLambda() const
+	{
+		return mTotalLambda;
+	}
+
+	/// Save state of this constraint part
+	void						SaveState(StateRecorder &inStream) const
+	{
+		inStream.Write(mTotalLambda);
+	}
+
+	/// Restore state of this constraint part
+	void						RestoreState(StateRecorder &inStream)
+	{
+		inStream.Read(mTotalLambda);
+	}
+
+private:
+	Vec3						mA1;						///< World space hinge axis for body 1
+	Vec3						mB2;						///< World space perpendiculars of hinge axis for body 2
+	Vec3						mC2;
+	Mat44						mInvI1;
+	Mat44						mInvI2;
+	Vec3						mB2xA1;
+	Vec3						mC2xA1;
+	Mat22						mEffectiveMass;
+	Vec2						mTotalLambda { Vec2::sZero() };
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/IndependentAxisConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/IndependentAxisConstraintPart.h
new file mode 100644
index 0000000000..5b752c6c69
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/IndependentAxisConstraintPart.h
@@ -0,0 +1,246 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Physics/StateRecorder.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Constraint part to an AxisConstraintPart but both bodies have an independent axis on which the force is applied.
+///
+/// Constraint equation:
+///
+/// \f[C = (x_1 + r_1 - f_1) . n_1 + r (x_2 + r_2 - f_2) \cdot n_2\f]
+///
+/// Calculating the Jacobian:
+///
+/// \f[dC/dt = (v_1 + w_1 \times r_1) \cdot n_1 + (x_1 + r_1 - f_1) \cdot d n_1/dt + r (v_2 + w_2 \times r_2) \cdot n_2 + r (x_2 + r_2 - f_2) \cdot d n_2/dt\f]
+///
+/// Assuming that d n1/dt and d n2/dt are small this becomes:
+///
+/// \f[(v_1 + w_1 \times r_1) \cdot n_1 + r (v_2 + w_2 \times r_2) \cdot n_2\f]
+/// \f[= v_1 \cdot n_1 + r_1 \times n_1 \cdot w_1 + r v_2 \cdot n_2 + r r_2 \times n_2 \cdot w_2\f]
+///
+/// Jacobian:
+///
+/// \f[J = \begin{bmatrix}n_1 & r_1 \times n_1 & r n_2 & r r_2 \times n_2\end{bmatrix}\f]
+///
+/// Effective mass:
+///
+/// \f[K = m_1^{-1} + r_1 \times n_1 I_1^{-1} r_1 \times n_1 + r^2 m_2^{-1} + r^2 r_2 \times n_2 I_2^{-1} r_2 \times n_2\f]
+///
+/// Used terms (here and below, everything in world space):\n
+/// n1 = (x1 + r1 - f1) / |x1 + r1 - f1|, axis along which the force is applied for body 1\n
+/// n2 = (x2 + r2 - f2) / |x2 + r2 - f2|, axis along which the force is applied for body 2\n
+/// r = ratio how forces are applied between bodies.\n
+/// x1, x2 = center of mass for the bodies.\n
+/// v = [v1, w1, v2, w2].\n
+/// v1, v2 = linear velocity of body 1 and 2.\n
+/// w1, w2 = angular velocity of body 1 and 2.\n
+/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n
+/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n
+/// b = velocity bias.\n
+/// \f$\beta\f$ = baumgarte constant.
+class IndependentAxisConstraintPart
+{
+	/// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated
+	JPH_INLINE bool				ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, float inLambda) const
+	{
+		// Apply impulse if delta is not zero
+		if (inLambda != 0.0f)
+		{
+			// Calculate velocity change due to constraint
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler velocity integration:
+			// v' = v + M^-1 P
+			if (ioBody1.IsDynamic())
+			{
+				MotionProperties *mp1 = ioBody1.GetMotionProperties();
+				mp1->AddLinearVelocityStep((mp1->GetInverseMass() * inLambda) * inN1);
+				mp1->AddAngularVelocityStep(mInvI1_R1xN1 * inLambda);
+			}
+			if (ioBody2.IsDynamic())
+			{
+				MotionProperties *mp2 = ioBody2.GetMotionProperties();
+				mp2->AddLinearVelocityStep((inRatio * mp2->GetInverseMass() * inLambda) * inN2);
+				mp2->AddAngularVelocityStep(mInvI2_RatioR2xN2 * inLambda);
+			}
+			return true;
+		}
+
+		return false;
+	}
+
+public:
+	/// Calculate properties used during the functions below
+	/// @param inBody1 The first body that this constraint is attached to
+	/// @param inBody2 The second body that this constraint is attached to
+	/// @param inR1 The position on which the constraint operates on body 1 relative to COM
+	/// @param inN1 The world space normal in which the constraint operates for body 1
+	/// @param inR2 The position on which the constraint operates on body 1 relative to COM
+	/// @param inN2 The world space normal in which the constraint operates for body 2
+	/// @param inRatio The ratio how forces are applied between bodies
+	inline void					CalculateConstraintProperties(const Body &inBody1, const Body &inBody2, Vec3Arg inR1, Vec3Arg inN1, Vec3Arg inR2, Vec3Arg inN2, float inRatio)
+	{
+		JPH_ASSERT(inN1.IsNormalized(1.0e-4f) && inN2.IsNormalized(1.0e-4f));
+
+		float inv_effective_mass = 0.0f;
+
+		if (!inBody1.IsStatic())
+		{
+			const MotionProperties *mp1 = inBody1.GetMotionProperties();
+
+			mR1xN1 = inR1.Cross(inN1);
+			mInvI1_R1xN1 = mp1->MultiplyWorldSpaceInverseInertiaByVector(inBody1.GetRotation(), mR1xN1);
+
+			inv_effective_mass += mp1->GetInverseMass() + mInvI1_R1xN1.Dot(mR1xN1);
+		}
+
+		if (!inBody2.IsStatic())
+		{
+			const MotionProperties *mp2 = inBody2.GetMotionProperties();
+
+			mRatioR2xN2 = inRatio * inR2.Cross(inN2);
+			mInvI2_RatioR2xN2 = mp2->MultiplyWorldSpaceInverseInertiaByVector(inBody2.GetRotation(), mRatioR2xN2);
+
+			inv_effective_mass += Square(inRatio) * mp2->GetInverseMass() + mInvI2_RatioR2xN2.Dot(mRatioR2xN2);
+		}
+
+		// Calculate inverse effective mass: K = J M^-1 J^T
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+			mEffectiveMass = 1.0f / inv_effective_mass;
+	}
+
+	/// Deactivate this constraint
+	inline void					Deactivate()
+	{
+		mEffectiveMass = 0.0f;
+		mTotalLambda = 0.0f;
+	}
+
+	/// Check if constraint is active
+	inline bool					IsActive() const
+	{
+		return mEffectiveMass != 0.0f;
+	}
+
+	/// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inN1 The world space normal in which the constraint operates for body 1
+	/// @param inN2 The world space normal in which the constraint operates for body 2
+	/// @param inRatio The ratio how forces are applied between bodies
+	/// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame
+	inline void					WarmStart(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, float inWarmStartImpulseRatio)
+	{
+		mTotalLambda *= inWarmStartImpulseRatio;
+		ApplyVelocityStep(ioBody1, ioBody2, inN1, inN2, inRatio, mTotalLambda);
+	}
+
+	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inN1 The world space normal in which the constraint operates for body 1
+	/// @param inN2 The world space normal in which the constraint operates for body 2
+	/// @param inRatio The ratio how forces are applied between bodies
+	/// @param inMinLambda Minimum angular impulse to apply (N m s)
+	/// @param inMaxLambda Maximum angular impulse to apply (N m s)
+	inline bool					SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, float inMinLambda, float inMaxLambda)
+	{
+		// Lagrange multiplier is:
+		//
+		// lambda = -K^-1 (J v + b)
+		float lambda = -mEffectiveMass * (inN1.Dot(ioBody1.GetLinearVelocity()) + mR1xN1.Dot(ioBody1.GetAngularVelocity()) + inRatio * inN2.Dot(ioBody2.GetLinearVelocity()) + mRatioR2xN2.Dot(ioBody2.GetAngularVelocity()));
+		float new_lambda = Clamp(mTotalLambda + lambda, inMinLambda, inMaxLambda); // Clamp impulse
+		lambda = new_lambda - mTotalLambda; // Lambda potentially got clamped, calculate the new impulse to apply
+		mTotalLambda = new_lambda; // Store accumulated impulse
+
+		return ApplyVelocityStep(ioBody1, ioBody2, inN1, inN2, inRatio, lambda);
+	}
+
+	/// Return lagrange multiplier
+	float						GetTotalLambda() const
+	{
+		return mTotalLambda;
+	}
+
+	/// Iteratively update the position constraint. Makes sure C(...) == 0.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inN1 The world space normal in which the constraint operates for body 1
+	/// @param inN2 The world space normal in which the constraint operates for body 2
+	/// @param inRatio The ratio how forces are applied between bodies
+	/// @param inC Value of the constraint equation (C)
+	/// @param inBaumgarte Baumgarte constant (fraction of the error to correct)
+	inline bool					SolvePositionConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, float inC, float inBaumgarte) const
+	{
+		if (inC != 0.0f)
+		{
+			// Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
+			//
+			// lambda = -K^-1 * beta / dt * C
+			//
+			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
+			float lambda = -mEffectiveMass * inBaumgarte * inC;
+
+			// Directly integrate velocity change for one time step
+			//
+			// Euler velocity integration:
+			// dv = M^-1 P
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler position integration:
+			// x' = x + dv * dt
+			//
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
+			// integrate + a position integrate and then discard the velocity change.
+			if (ioBody1.IsDynamic())
+			{
+				ioBody1.AddPositionStep((lambda * ioBody1.GetMotionPropertiesUnchecked()->GetInverseMass()) * inN1);
+				ioBody1.AddRotationStep(lambda * mInvI1_R1xN1);
+			}
+			if (ioBody2.IsDynamic())
+			{
+				ioBody2.AddPositionStep((lambda * inRatio * ioBody2.GetMotionPropertiesUnchecked()->GetInverseMass()) * inN2);
+				ioBody2.AddRotationStep(lambda * mInvI2_RatioR2xN2);
+			}
+			return true;
+		}
+
+		return false;
+	}
+
+	/// Save state of this constraint part
+	void						SaveState(StateRecorder &inStream) const
+	{
+		inStream.Write(mTotalLambda);
+	}
+
+	/// Restore state of this constraint part
+	void						RestoreState(StateRecorder &inStream)
+	{
+		inStream.Read(mTotalLambda);
+	}
+
+private:
+	Vec3						mR1xN1;
+	Vec3						mInvI1_R1xN1;
+	Vec3						mRatioR2xN2;
+	Vec3						mInvI2_RatioR2xN2;
+	float						mEffectiveMass = 0.0f;
+	float						mTotalLambda = 0.0f;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h
new file mode 100644
index 0000000000..f9e86b86b2
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h
@@ -0,0 +1,239 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Physics/StateRecorder.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Constrains movement along 3 axis
+///
+/// @see "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, section 2.2.1
+///
+/// Constraint equation (eq 45):
+///
+/// \f[C = p_2 - p_1\f]
+///
+/// Jacobian (transposed) (eq 47):
+///
+/// \f[J^T = \begin{bmatrix}-E & r1x & E & -r2x^T\end{bmatrix}
+/// = \begin{bmatrix}-E^T \\ r1x^T \\ E^T \\ -r2x^T\end{bmatrix}
+/// = \begin{bmatrix}-E \\ -r1x \\ E \\ r2x\end{bmatrix}\f]
+///
+/// Used terms (here and below, everything in world space):\n
+/// p1, p2 = constraint points.\n
+/// r1 = p1 - x1.\n
+/// r2 = p2 - x2.\n
+/// r1x = 3x3 matrix for which r1x v = r1 x v (cross product).\n
+/// x1, x2 = center of mass for the bodies.\n
+/// v = [v1, w1, v2, w2].\n
+/// v1, v2 = linear velocity of body 1 and 2.\n
+/// w1, w2 = angular velocity of body 1 and 2.\n
+/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n
+/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n
+/// b = velocity bias.\n
+/// \f$\beta\f$ = baumgarte constant.\n
+/// E = identity matrix.
+class PointConstraintPart
+{
+	JPH_INLINE bool				ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inLambda) const
+	{
+		// Apply impulse if delta is not zero
+		if (inLambda != Vec3::sZero())
+		{
+			// Calculate velocity change due to constraint
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler velocity integration:
+			// v' = v + M^-1 P
+			if (ioBody1.IsDynamic())
+			{
+				MotionProperties *mp1 = ioBody1.GetMotionProperties();
+				mp1->SubLinearVelocityStep(mp1->GetInverseMass() * inLambda);
+				mp1->SubAngularVelocityStep(mInvI1_R1X * inLambda);
+			}
+			if (ioBody2.IsDynamic())
+			{
+				MotionProperties *mp2 = ioBody2.GetMotionProperties();
+				mp2->AddLinearVelocityStep(mp2->GetInverseMass() * inLambda);
+				mp2->AddAngularVelocityStep(mInvI2_R2X * inLambda);
+			}
+			return true;
+		}
+
+		return false;
+	}
+
+public:
+	/// Calculate properties used during the functions below
+	/// @param inBody1 The first body that this constraint is attached to
+	/// @param inBody2 The second body that this constraint is attached to
+	/// @param inRotation1 The 3x3 rotation matrix for body 1 (translation part is ignored)
+	/// @param inRotation2 The 3x3 rotation matrix for body 2 (translation part is ignored)
+	/// @param inR1 Local space vector from center of mass to constraint point for body 1
+	/// @param inR2 Local space vector from center of mass to constraint point for body 2
+	inline void					CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, Vec3Arg inR1, const Body &inBody2, Mat44Arg inRotation2, Vec3Arg inR2)
+	{
+		// Positions where the point constraint acts on (middle point between center of masses) in world space
+		mR1 = inRotation1.Multiply3x3(inR1);
+		mR2 = inRotation2.Multiply3x3(inR2);
+
+		// Calculate effective mass: K^-1 = (J M^-1 J^T)^-1
+		// Using: I^-1 = R * Ibody^-1 * R^T
+		float summed_inv_mass;
+		Mat44 inv_effective_mass;
+		if (inBody1.IsDynamic())
+		{
+			const MotionProperties *mp1 = inBody1.GetMotionProperties();
+			Mat44 inv_i1 = mp1->GetInverseInertiaForRotation(inRotation1);
+			summed_inv_mass = mp1->GetInverseMass();
+
+			Mat44 r1x = Mat44::sCrossProduct(mR1);
+			mInvI1_R1X = inv_i1.Multiply3x3(r1x);
+			inv_effective_mass = r1x.Multiply3x3(inv_i1).Multiply3x3RightTransposed(r1x);
+		}
+		else
+		{
+			JPH_IF_DEBUG(mInvI1_R1X = Mat44::sNaN();)
+
+			summed_inv_mass = 0.0f;
+			inv_effective_mass = Mat44::sZero();
+		}
+
+		if (inBody2.IsDynamic())
+		{
+			const MotionProperties *mp2 = inBody2.GetMotionProperties();
+			Mat44 inv_i2 = mp2->GetInverseInertiaForRotation(inRotation2);
+			summed_inv_mass += mp2->GetInverseMass();
+
+			Mat44 r2x = Mat44::sCrossProduct(mR2);
+			mInvI2_R2X = inv_i2.Multiply3x3(r2x);
+			inv_effective_mass += r2x.Multiply3x3(inv_i2).Multiply3x3RightTransposed(r2x);
+		}
+		else
+		{
+			JPH_IF_DEBUG(mInvI2_R2X = Mat44::sNaN();)
+		}
+
+		inv_effective_mass += Mat44::sScale(summed_inv_mass);
+		if (!mEffectiveMass.SetInversed3x3(inv_effective_mass))
+			Deactivate();
+	}
+
+	/// Deactivate this constraint
+	inline void					Deactivate()
+	{
+		mEffectiveMass = Mat44::sZero();
+		mTotalLambda = Vec3::sZero();
+	}
+
+	/// Check if constraint is active
+	inline bool					IsActive() const
+	{
+		return mEffectiveMass(3, 3) != 0.0f;
+	}
+
+	/// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame
+	inline void					WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio)
+	{
+		mTotalLambda *= inWarmStartImpulseRatio;
+		ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda);
+	}
+
+	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	inline bool					SolveVelocityConstraint(Body &ioBody1, Body &ioBody2)
+	{
+		// Calculate lagrange multiplier:
+		//
+		// lambda = -K^-1 (J v + b)
+		Vec3 lambda = mEffectiveMass * (ioBody1.GetLinearVelocity() - mR1.Cross(ioBody1.GetAngularVelocity()) - ioBody2.GetLinearVelocity() + mR2.Cross(ioBody2.GetAngularVelocity()));
+		mTotalLambda += lambda; // Store accumulated lambda
+		return ApplyVelocityStep(ioBody1, ioBody2, lambda);
+	}
+
+	/// Iteratively update the position constraint. Makes sure C(...) = 0.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inBaumgarte Baumgarte constant (fraction of the error to correct)
+	inline bool					SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inBaumgarte) const
+	{
+		Vec3 separation = (Vec3(ioBody2.GetCenterOfMassPosition() - ioBody1.GetCenterOfMassPosition()) + mR2 - mR1);
+		if (separation != Vec3::sZero())
+		{
+			// Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
+			//
+			// lambda = -K^-1 * beta / dt * C
+			//
+			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
+			Vec3 lambda = mEffectiveMass * -inBaumgarte * separation;
+
+			// Directly integrate velocity change for one time step
+			//
+			// Euler velocity integration:
+			// dv = M^-1 P
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler position integration:
+			// x' = x + dv * dt
+			//
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
+			// integrate + a position integrate and then discard the velocity change.
+			if (ioBody1.IsDynamic())
+			{
+				ioBody1.SubPositionStep(ioBody1.GetMotionProperties()->GetInverseMass() * lambda);
+				ioBody1.SubRotationStep(mInvI1_R1X * lambda);
+			}
+			if (ioBody2.IsDynamic())
+			{
+				ioBody2.AddPositionStep(ioBody2.GetMotionProperties()->GetInverseMass() * lambda);
+				ioBody2.AddRotationStep(mInvI2_R2X * lambda);
+			}
+
+			return true;
+		}
+
+		return false;
+	}
+
+	/// Return lagrange multiplier
+	Vec3						GetTotalLambda() const
+	{
+		return mTotalLambda;
+	}
+
+	/// Save state of this constraint part
+	void						SaveState(StateRecorder &inStream) const
+	{
+		inStream.Write(mTotalLambda);
+	}
+
+	/// Restore state of this constraint part
+	void						RestoreState(StateRecorder &inStream)
+	{
+		inStream.Read(mTotalLambda);
+	}
+
+private:
+	Vec3						mR1;
+	Vec3						mR2;
+	Mat44						mInvI1_R1X;
+	Mat44						mInvI2_R2X;
+	Mat44						mEffectiveMass;
+	Vec3						mTotalLambda { Vec3::sZero() };
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RackAndPinionConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RackAndPinionConstraintPart.h
new file mode 100644
index 0000000000..641f4867a1
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RackAndPinionConstraintPart.h
@@ -0,0 +1,196 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Physics/StateRecorder.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Constraint that constrains a rotation to a translation
+///
+/// Constraint equation:
+///
+/// C = Theta(t) - r d(t)
+///
+/// Derivative:
+///
+/// d/dt C = 0
+/// <=> w1 . a - r v2 . b = 0
+///
+/// Jacobian:
+///
+/// \f[J = \begin{bmatrix}0 & a^T & -r b^T & 0\end{bmatrix}\f]
+///
+/// Used terms (here and below, everything in world space):\n
+/// a = axis around which body 1 rotates (normalized).\n
+/// b = axis along which body 2 slides (normalized).\n
+/// Theta(t) = rotation around a of body 1.\n
+/// d(t) = distance body 2 slides.\n
+/// r = ratio between rotation and translation.\n
+/// v = [v1, w1, v2, w2].\n
+/// v1, v2 = linear velocity of body 1 and 2.\n
+/// w1, w2 = angular velocity of body 1 and 2.\n
+/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n
+/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n
+/// \f$\beta\f$ = baumgarte constant.
+class RackAndPinionConstraintPart
+{
+	/// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated
+	JPH_INLINE bool				ApplyVelocityStep(Body &ioBody1, Body &ioBody2, float inLambda) const
+	{
+		// Apply impulse if delta is not zero
+		if (inLambda != 0.0f)
+		{
+			// Calculate velocity change due to constraint
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler velocity integration:
+			// v' = v + M^-1 P
+			ioBody1.GetMotionProperties()->AddAngularVelocityStep(inLambda * mInvI1_A);
+			ioBody2.GetMotionProperties()->SubLinearVelocityStep(inLambda * mRatio_InvM2_B);
+			return true;
+		}
+
+		return false;
+	}
+
+public:
+	/// Calculate properties used during the functions below
+	/// @param inBody1 The first body that this constraint is attached to
+	/// @param inBody2 The second body that this constraint is attached to
+	/// @param inWorldSpaceHingeAxis The axis around which body 1 rotates
+	/// @param inWorldSpaceSliderAxis The axis along which body 2 slides
+	/// @param inRatio The ratio between rotation and translation
+	inline void					CalculateConstraintProperties(const Body &inBody1, Vec3Arg inWorldSpaceHingeAxis, const Body &inBody2, Vec3Arg inWorldSpaceSliderAxis, float inRatio)
+	{
+		JPH_ASSERT(inWorldSpaceHingeAxis.IsNormalized(1.0e-4f));
+		JPH_ASSERT(inWorldSpaceSliderAxis.IsNormalized(1.0e-4f));
+
+		// Calculate: I1^-1 a
+		mInvI1_A = inBody1.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody1.GetRotation(), inWorldSpaceHingeAxis);
+
+		// Calculate: r/m2 b
+		float inv_m2 = inBody2.GetMotionProperties()->GetInverseMass();
+		mRatio_InvM2_B = inRatio * inv_m2 * inWorldSpaceSliderAxis;
+
+		// K^-1 = 1 / (J M^-1 J^T) = 1 / (a^T I1^-1 a + 1/m2 * r^2 * b . b)
+		float inv_effective_mass = (inWorldSpaceHingeAxis.Dot(mInvI1_A) + inv_m2 * Square(inRatio));
+		if (inv_effective_mass == 0.0f)
+			Deactivate();
+		else
+			mEffectiveMass = 1.0f / inv_effective_mass;
+	}
+
+	/// Deactivate this constraint
+	inline void					Deactivate()
+	{
+		mEffectiveMass = 0.0f;
+		mTotalLambda = 0.0f;
+	}
+
+	/// Check if constraint is active
+	inline bool					IsActive() const
+	{
+		return mEffectiveMass != 0.0f;
+	}
+
+	/// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame
+	inline void					WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio)
+	{
+		mTotalLambda *= inWarmStartImpulseRatio;
+		ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda);
+	}
+
+	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inWorldSpaceHingeAxis The axis around which body 1 rotates
+	/// @param inWorldSpaceSliderAxis The axis along which body 2 slides
+	/// @param inRatio The ratio between rotation and translation
+	inline bool					SolveVelocityConstraint(Body &ioBody1, Vec3Arg inWorldSpaceHingeAxis, Body &ioBody2, Vec3Arg inWorldSpaceSliderAxis, float inRatio)
+	{
+		// Lagrange multiplier is:
+		//
+		// lambda = -K^-1 (J v + b)
+		float lambda = mEffectiveMass * (inRatio * inWorldSpaceSliderAxis.Dot(ioBody2.GetLinearVelocity()) - inWorldSpaceHingeAxis.Dot(ioBody1.GetAngularVelocity()));
+		mTotalLambda += lambda; // Store accumulated impulse
+
+		return ApplyVelocityStep(ioBody1, ioBody2, lambda);
+	}
+
+	/// Return lagrange multiplier
+	float						GetTotalLambda() const
+	{
+		return mTotalLambda;
+	}
+
+	/// Iteratively update the position constraint. Makes sure C(...) == 0.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inC Value of the constraint equation (C)
+	/// @param inBaumgarte Baumgarte constant (fraction of the error to correct)
+	inline bool					SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inC, float inBaumgarte) const
+	{
+		// Only apply position constraint when the constraint is hard, otherwise the velocity bias will fix the constraint
+		if (inC != 0.0f)
+		{
+			// Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
+			//
+			// lambda = -K^-1 * beta / dt * C
+			//
+			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
+			float lambda = -mEffectiveMass * inBaumgarte * inC;
+
+			// Directly integrate velocity change for one time step
+			//
+			// Euler velocity integration:
+			// dv = M^-1 P
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler position integration:
+			// x' = x + dv * dt
+			//
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
+			// integrate + a position integrate and then discard the velocity change.
+			if (ioBody1.IsDynamic())
+				ioBody1.AddRotationStep(lambda * mInvI1_A);
+			if (ioBody2.IsDynamic())
+				ioBody2.SubPositionStep(lambda * mRatio_InvM2_B);
+			return true;
+		}
+
+		return false;
+	}
+
+	/// Save state of this constraint part
+	void						SaveState(StateRecorder &inStream) const
+	{
+		inStream.Write(mTotalLambda);
+	}
+
+	/// Restore state of this constraint part
+	void						RestoreState(StateRecorder &inStream)
+	{
+		inStream.Read(mTotalLambda);
+	}
+
+private:
+	Vec3						mInvI1_A;
+	Vec3						mRatio_InvM2_B;
+	float						mEffectiveMass = 0.0f;
+	float						mTotalLambda = 0.0f;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h
new file mode 100644
index 0000000000..ec84776a46
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h
@@ -0,0 +1,270 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Physics/StateRecorder.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Constrains rotation around all axis so that only translation is allowed
+///
+/// Based on: "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, section 2.5.1
+///
+/// Constraint equation (eq 129):
+///
+/// \f[C = \begin{bmatrix}\Delta\theta_x, \Delta\theta_y, \Delta\theta_z\end{bmatrix}\f]
+///
+/// Jacobian (eq 131):
+///
+/// \f[J = \begin{bmatrix}0 & -E & 0 & E\end{bmatrix}\f]
+///
+/// Used terms (here and below, everything in world space):\n
+/// delta_theta_* = difference in rotation between initial rotation of bodies 1 and 2.\n
+/// x1, x2 = center of mass for the bodies.\n
+/// v = [v1, w1, v2, w2].\n
+/// v1, v2 = linear velocity of body 1 and 2.\n
+/// w1, w2 = angular velocity of body 1 and 2.\n
+/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n
+/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n
+/// b = velocity bias.\n
+/// \f$\beta\f$ = baumgarte constant.\n
+/// E = identity matrix.\n
+class RotationEulerConstraintPart
+{
+private:
+	/// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated
+	JPH_INLINE bool				ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inLambda) const
+	{
+		// Apply impulse if delta is not zero
+		if (inLambda != Vec3::sZero())
+		{
+			// Calculate velocity change due to constraint
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler velocity integration:
+			// v' = v + M^-1 P
+			if (ioBody1.IsDynamic())
+				ioBody1.GetMotionProperties()->SubAngularVelocityStep(mInvI1.Multiply3x3(inLambda));
+			if (ioBody2.IsDynamic())
+				ioBody2.GetMotionProperties()->AddAngularVelocityStep(mInvI2.Multiply3x3(inLambda));
+			return true;
+		}
+
+		return false;
+	}
+
+public:
+	/// Return inverse of initial rotation from body 1 to body 2 in body 1 space
+	static Quat					sGetInvInitialOrientation(const Body &inBody1, const Body &inBody2)
+	{
+		// q20 = q10 r0
+		// <=> r0 = q10^-1 q20
+		// <=> r0^-1 = q20^-1 q10
+		//
+		// where:
+		//
+		// q20 = initial orientation of body 2
+		// q10 = initial orientation of body 1
+		// r0 = initial rotation from body 1 to body 2
+		return inBody2.GetRotation().Conjugated() * inBody1.GetRotation();
+	}
+
+	/// @brief Return inverse of initial rotation from body 1 to body 2 in body 1 space
+	/// @param inAxisX1 Reference axis X for body 1
+	/// @param inAxisY1 Reference axis Y for body 1
+	/// @param inAxisX2 Reference axis X for body 2
+	/// @param inAxisY2 Reference axis Y for body 2
+	static Quat					sGetInvInitialOrientationXY(Vec3Arg inAxisX1, Vec3Arg inAxisY1, Vec3Arg inAxisX2, Vec3Arg inAxisY2)
+	{
+		// Store inverse of initial rotation from body 1 to body 2 in body 1 space:
+		//
+		// q20 = q10 r0
+		// <=> r0 = q10^-1 q20
+		// <=> r0^-1 = q20^-1 q10
+		//
+		// where:
+		//
+		// q10, q20 = world space initial orientation of body 1 and 2
+		// r0 = initial rotation from body 1 to body 2 in local space of body 1
+		//
+		// We can also write this in terms of the constraint matrices:
+		//
+		// q20 c2 = q10 c1
+		// <=> q20 = q10 c1 c2^-1
+		// => r0 = c1 c2^-1
+		// <=> r0^-1 = c2 c1^-1
+		//
+		// where:
+		//
+		// c1, c2 = matrix that takes us from body 1 and 2 COM to constraint space 1 and 2
+		if (inAxisX1 == inAxisX2 && inAxisY1 == inAxisY2)
+		{
+			// Axis are the same -> identity transform
+			return Quat::sIdentity();
+		}
+		else
+		{
+			Mat44 constraint1(Vec4(inAxisX1, 0), Vec4(inAxisY1, 0), Vec4(inAxisX1.Cross(inAxisY1), 0), Vec4(0, 0, 0, 1));
+			Mat44 constraint2(Vec4(inAxisX2, 0), Vec4(inAxisY2, 0), Vec4(inAxisX2.Cross(inAxisY2), 0), Vec4(0, 0, 0, 1));
+			return constraint2.GetQuaternion() * constraint1.GetQuaternion().Conjugated();
+		}
+	}
+
+	/// @brief Return inverse of initial rotation from body 1 to body 2 in body 1 space
+	/// @param inAxisX1 Reference axis X for body 1
+	/// @param inAxisZ1 Reference axis Z for body 1
+	/// @param inAxisX2 Reference axis X for body 2
+	/// @param inAxisZ2 Reference axis Z for body 2
+	static Quat					sGetInvInitialOrientationXZ(Vec3Arg inAxisX1, Vec3Arg inAxisZ1, Vec3Arg inAxisX2, Vec3Arg inAxisZ2)
+	{
+		// See comment at sGetInvInitialOrientationXY
+		if (inAxisX1 == inAxisX2 && inAxisZ1 == inAxisZ2)
+		{
+			return Quat::sIdentity();
+		}
+		else
+		{
+			Mat44 constraint1(Vec4(inAxisX1, 0), Vec4(inAxisZ1.Cross(inAxisX1), 0), Vec4(inAxisZ1, 0), Vec4(0, 0, 0, 1));
+			Mat44 constraint2(Vec4(inAxisX2, 0), Vec4(inAxisZ2.Cross(inAxisX2), 0), Vec4(inAxisZ2, 0), Vec4(0, 0, 0, 1));
+			return constraint2.GetQuaternion() * constraint1.GetQuaternion().Conjugated();
+		}
+	}
+
+	/// Calculate properties used during the functions below
+	inline void					CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, const Body &inBody2, Mat44Arg inRotation2)
+	{
+		// Calculate properties used during constraint solving
+		mInvI1 = inBody1.IsDynamic()? inBody1.GetMotionProperties()->GetInverseInertiaForRotation(inRotation1) : Mat44::sZero();
+		mInvI2 = inBody2.IsDynamic()? inBody2.GetMotionProperties()->GetInverseInertiaForRotation(inRotation2) : Mat44::sZero();
+
+		// Calculate effective mass: K^-1 = (J M^-1 J^T)^-1
+		if (!mEffectiveMass.SetInversed3x3(mInvI1 + mInvI2))
+			Deactivate();
+	}
+
+	/// Deactivate this constraint
+	inline void					Deactivate()
+	{
+		mEffectiveMass = Mat44::sZero();
+		mTotalLambda = Vec3::sZero();
+	}
+
+	/// Check if constraint is active
+	inline bool					IsActive() const
+	{
+		return mEffectiveMass(3, 3) != 0.0f;
+	}
+
+	/// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses
+	inline void					WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio)
+	{
+		mTotalLambda *= inWarmStartImpulseRatio;
+		ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda);
+	}
+
+	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.
+	inline bool					SolveVelocityConstraint(Body &ioBody1, Body &ioBody2)
+	{
+		// Calculate lagrange multiplier:
+		//
+		// lambda = -K^-1 (J v + b)
+		Vec3 lambda = mEffectiveMass.Multiply3x3(ioBody1.GetAngularVelocity() - ioBody2.GetAngularVelocity());
+		mTotalLambda += lambda;
+		return ApplyVelocityStep(ioBody1, ioBody2, lambda);
+	}
+
+	/// Iteratively update the position constraint. Makes sure C(...) = 0.
+	inline bool					SolvePositionConstraint(Body &ioBody1, Body &ioBody2, QuatArg inInvInitialOrientation, float inBaumgarte) const
+	{
+		// Calculate difference in rotation
+		//
+		// The rotation should be:
+		//
+		// q2 = q1 r0
+		//
+		// But because of drift the actual rotation is
+		//
+		// q2 = diff q1 r0
+		// <=> diff = q2 r0^-1 q1^-1
+		//
+		// Where:
+		// q1 = current rotation of body 1
+		// q2 = current rotation of body 2
+		// diff = error that needs to be reduced to zero
+		Quat diff = ioBody2.GetRotation() * inInvInitialOrientation * ioBody1.GetRotation().Conjugated();
+
+		// A quaternion can be seen as:
+		//
+		// q = [sin(theta / 2) * v, cos(theta/2)]
+		//
+		// Where:
+		// v = rotation vector
+		// theta = rotation angle
+		//
+		// If we assume theta is small (error is small) then sin(x) = x so an approximation of the error angles is:
+		Vec3 error = 2.0f * diff.EnsureWPositive().GetXYZ();
+		if (error != Vec3::sZero())
+		{
+			// Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
+			//
+			// lambda = -K^-1 * beta / dt * C
+			//
+			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
+			Vec3 lambda = -inBaumgarte * mEffectiveMass * error;
+
+			// Directly integrate velocity change for one time step
+			//
+			// Euler velocity integration:
+			// dv = M^-1 P
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler position integration:
+			// x' = x + dv * dt
+			//
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
+			// integrate + a position integrate and then discard the velocity change.
+			if (ioBody1.IsDynamic())
+				ioBody1.SubRotationStep(mInvI1.Multiply3x3(lambda));
+			if (ioBody2.IsDynamic())
+				ioBody2.AddRotationStep(mInvI2.Multiply3x3(lambda));
+			return true;
+		}
+
+		return false;
+	}
+
+	/// Return lagrange multiplier
+	Vec3						GetTotalLambda() const
+	{
+		return mTotalLambda;
+	}
+
+	/// Save state of this constraint part
+	void						SaveState(StateRecorder &inStream) const
+	{
+		inStream.Write(mTotalLambda);
+	}
+
+	/// Restore state of this constraint part
+	void						RestoreState(StateRecorder &inStream)
+	{
+		inStream.Read(mTotalLambda);
+	}
+
+private:
+	Mat44						mInvI1;
+	Mat44						mInvI2;
+	Mat44						mEffectiveMass;
+	Vec3						mTotalLambda { Vec3::sZero() };
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RotationQuatConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RotationQuatConstraintPart.h
new file mode 100644
index 0000000000..a4675a51b9
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RotationQuatConstraintPart.h
@@ -0,0 +1,246 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Physics/StateRecorder.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Quaternion based constraint that constrains rotation around all axis so that only translation is allowed.
+///
+/// NOTE: This constraint part is more expensive than the RotationEulerConstraintPart and slightly more correct since
+/// RotationEulerConstraintPart::SolvePositionConstraint contains an approximation. In practice the difference
+/// is small, so the RotationEulerConstraintPart is probably the better choice.
+///
+/// Rotation is fixed between bodies like this:
+///
+/// q2 = q1 r0
+///
+/// Where:
+/// q1, q2 = world space quaternions representing rotation of body 1 and 2.
+/// r0 = initial rotation between bodies in local space of body 1, this can be calculated by:
+///
+/// q20 = q10 r0
+/// <=> r0 = q10^* q20
+///
+/// Where:
+/// q10, q20 = initial world space rotations of body 1 and 2.
+/// q10^* = conjugate of quaternion q10 (which is the same as the inverse for a unit quaternion)
+///
+/// We exclusively use the conjugate below:
+///
+/// r0^* = q20^* q10
+///
+/// The error in the rotation is (in local space of body 1):
+///
+/// q2 = q1 error r0
+/// <=> error = q1^* q2 r0^*
+///
+/// The imaginary part of the quaternion represents the rotation axis * sin(angle / 2). The real part of the quaternion
+/// does not add any additional information (we know the quaternion in normalized) and we're removing 3 degrees of freedom
+/// so we want 3 parameters. Therefore we define the constraint equation like:
+///
+/// C = A q1^* q2 r0^* = 0
+///
+/// Where (if you write a quaternion as [real-part, i-part, j-part, k-part]):
+///
+///		    [0, 1, 0, 0]
+///		A = [0, 0, 1, 0]
+///		    [0, 0, 0, 1]
+///
+/// or in our case since we store a quaternion like [i-part, j-part, k-part, real-part]:
+///
+///		    [1, 0, 0, 0]
+///		A = [0, 1, 0, 0]
+///		    [0, 0, 1, 0]
+///
+/// Time derivative:
+///
+/// d/dt C = A (q1^* d/dt(q2) + d/dt(q1^*) q2) r0^*
+/// = A (q1^* (1/2 W2 q2) + (1/2 W1 q1)^* q2) r0^*
+/// = 1/2 A (q1^* W2 q2 + q1^* W1^* q2) r0^*
+/// = 1/2 A (q1^* W2 q2 - q1^* W1 * q2) r0^*
+/// = 1/2 A ML(q1^*) MR(q2 r0^*) (W2 - W1)
+/// = 1/2 A ML(q1^*) MR(q2 r0^*) A^T (w2 - w1)
+///
+/// Where:
+/// W1 = [0, w1], W2 = [0, w2] (converting angular velocity to imaginary part of quaternion).
+/// w1, w2 = angular velocity of body 1 and 2.
+/// d/dt(q) = 1/2 W q (time derivative of a quaternion).
+/// W^* = -W (conjugate negates angular velocity as quaternion).
+/// ML(q): 4x4 matrix so that q * p = ML(q) * p, where q and p are quaternions.
+/// MR(p): 4x4 matrix so that q * p = MR(p) * q, where q and p are quaternions.
+/// A^T: Transpose of A.
+///
+/// Jacobian:
+///
+/// J = [0, -1/2 A ML(q1^*) MR(q2 r0^*) A^T, 0, 1/2 A ML(q1^*) MR(q2 r0^*) A^T]
+/// = [0, -JP, 0, JP]
+///
+/// Suggested reading:
+/// - 3D Constraint Derivations for Impulse Solvers - Marijn Tamis
+/// - Game Physics Pearls - Section 9 - Quaternion Based Constraints - Claude Lacoursiere
+class RotationQuatConstraintPart
+{
+private:
+	/// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated
+	JPH_INLINE bool				ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inLambda) const
+	{
+		// Apply impulse if delta is not zero
+		if (inLambda != Vec3::sZero())
+		{
+			// Calculate velocity change due to constraint
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler velocity integration:
+			// v' = v + M^-1 P
+			if (ioBody1.IsDynamic())
+				ioBody1.GetMotionProperties()->SubAngularVelocityStep(mInvI1_JPT.Multiply3x3(inLambda));
+			if (ioBody2.IsDynamic())
+				ioBody2.GetMotionProperties()->AddAngularVelocityStep(mInvI2_JPT.Multiply3x3(inLambda));
+			return true;
+		}
+
+		return false;
+	}
+
+public:
+	/// Return inverse of initial rotation from body 1 to body 2 in body 1 space
+	static Quat					sGetInvInitialOrientation(const Body &inBody1, const Body &inBody2)
+	{
+		// q20 = q10 r0
+		// <=> r0 = q10^-1 q20
+		// <=> r0^-1 = q20^-1 q10
+		//
+		// where:
+		//
+		// q20 = initial orientation of body 2
+		// q10 = initial orientation of body 1
+		// r0 = initial rotation from body 1 to body 2
+		return inBody2.GetRotation().Conjugated() * inBody1.GetRotation();
+	}
+
+	/// Calculate properties used during the functions below
+	inline void					CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, const Body &inBody2, Mat44Arg inRotation2, QuatArg inInvInitialOrientation)
+	{
+		// Calculate: JP = 1/2 A ML(q1^*) MR(q2 r0^*) A^T
+		Mat44 jp = (Mat44::sQuatLeftMultiply(0.5f * inBody1.GetRotation().Conjugated()) * Mat44::sQuatRightMultiply(inBody2.GetRotation() * inInvInitialOrientation)).GetRotationSafe();
+
+		// Calculate properties used during constraint solving
+		Mat44 inv_i1 = inBody1.IsDynamic()? inBody1.GetMotionProperties()->GetInverseInertiaForRotation(inRotation1) : Mat44::sZero();
+		Mat44 inv_i2 = inBody2.IsDynamic()? inBody2.GetMotionProperties()->GetInverseInertiaForRotation(inRotation2) : Mat44::sZero();
+		mInvI1_JPT = inv_i1.Multiply3x3RightTransposed(jp);
+		mInvI2_JPT = inv_i2.Multiply3x3RightTransposed(jp);
+
+		// Calculate effective mass: K^-1 = (J M^-1 J^T)^-1
+		// = (JP * I1^-1 * JP^T + JP * I2^-1 * JP^T)^-1
+		// = (JP * (I1^-1 + I2^-1) * JP^T)^-1
+		if (!mEffectiveMass.SetInversed3x3(jp.Multiply3x3(inv_i1 + inv_i2).Multiply3x3RightTransposed(jp)))
+			Deactivate();
+		else
+			mEffectiveMass_JP = mEffectiveMass.Multiply3x3(jp);
+	}
+
+	/// Deactivate this constraint
+	inline void					Deactivate()
+	{
+		mEffectiveMass = Mat44::sZero();
+		mEffectiveMass_JP = Mat44::sZero();
+		mTotalLambda = Vec3::sZero();
+	}
+
+	/// Check if constraint is active
+	inline bool					IsActive() const
+	{
+		return mEffectiveMass(3, 3) != 0.0f;
+	}
+
+	/// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses
+	inline void					WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio)
+	{
+		mTotalLambda *= inWarmStartImpulseRatio;
+		ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda);
+	}
+
+	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.
+	inline bool					SolveVelocityConstraint(Body &ioBody1, Body &ioBody2)
+	{
+		// Calculate lagrange multiplier:
+		//
+		// lambda = -K^-1 (J v + b)
+		Vec3 lambda = mEffectiveMass_JP.Multiply3x3(ioBody1.GetAngularVelocity() - ioBody2.GetAngularVelocity());
+		mTotalLambda += lambda;
+		return ApplyVelocityStep(ioBody1, ioBody2, lambda);
+	}
+
+	/// Iteratively update the position constraint. Makes sure C(...) = 0.
+	inline bool					SolvePositionConstraint(Body &ioBody1, Body &ioBody2, QuatArg inInvInitialOrientation, float inBaumgarte) const
+	{
+		// Calculate constraint equation
+		Vec3 c = (ioBody1.GetRotation().Conjugated() * ioBody2.GetRotation() * inInvInitialOrientation).GetXYZ();
+		if (c != Vec3::sZero())
+		{
+			// Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
+			//
+			// lambda = -K^-1 * beta / dt * C
+			//
+			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
+			Vec3 lambda = -inBaumgarte * mEffectiveMass * c;
+
+			// Directly integrate velocity change for one time step
+			//
+			// Euler velocity integration:
+			// dv = M^-1 P
+			//
+			// Impulse:
+			// P = J^T lambda
+			//
+			// Euler position integration:
+			// x' = x + dv * dt
+			//
+			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
+			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
+			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
+			// integrate + a position integrate and then discard the velocity change.
+			if (ioBody1.IsDynamic())
+				ioBody1.SubRotationStep(mInvI1_JPT.Multiply3x3(lambda));
+			if (ioBody2.IsDynamic())
+				ioBody2.AddRotationStep(mInvI2_JPT.Multiply3x3(lambda));
+			return true;
+		}
+
+		return false;
+	}
+
+	/// Return lagrange multiplier
+	Vec3						GetTotalLambda() const
+	{
+		return mTotalLambda;
+	}
+
+	/// Save state of this constraint part
+	void						SaveState(StateRecorder &inStream) const
+	{
+		inStream.Write(mTotalLambda);
+	}
+
+	/// Restore state of this constraint part
+	void						RestoreState(StateRecorder &inStream)
+	{
+		inStream.Read(mTotalLambda);
+	}
+
+private:
+	Mat44						mInvI1_JPT;
+	Mat44						mInvI2_JPT;
+	Mat44						mEffectiveMass;
+	Mat44						mEffectiveMass_JP;
+	Vec3						mTotalLambda { Vec3::sZero() };
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/SpringPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/SpringPart.h
new file mode 100644
index 0000000000..0a8a4a9730
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/SpringPart.h
@@ -0,0 +1,169 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+#ifndef JPH_PLATFORM_DOXYGEN // Somehow Doxygen gets confused and thinks the parameters to CalculateSpringProperties belong to this macro
+JPH_MSVC_SUPPRESS_WARNING(4723) // potential divide by 0 - caused by line: outEffectiveMass = 1.0f / inInvEffectiveMass, note that JPH_NAMESPACE_BEGIN already pushes the warning state
+#endif // !JPH_PLATFORM_DOXYGEN
+
+/// Class used in other constraint parts to calculate the required bias factor in the lagrange multiplier for creating springs
+class SpringPart
+{
+private:
+	JPH_INLINE void				CalculateSpringPropertiesHelper(float inDeltaTime, float inInvEffectiveMass, float inBias, float inC, float inStiffness, float inDamping, float &outEffectiveMass)
+	{
+		// Soft constraints as per: Soft Constraints: Reinventing The Spring - Erin Catto - GDC 2011
+
+		// Note that the calculation of beta and gamma below are based on the solution of an implicit Euler integration scheme
+		// This scheme is unconditionally stable but has built in damping, so even when you set the damping ratio to 0 there will still
+		// be damping. See page 16 and 32.
+
+		// Calculate softness (gamma in the slides)
+		// See page 34 and note that the gamma needs to be divided by delta time since we're working with impulses rather than forces:
+		// softness = 1 / (dt * (c + dt * k))
+		// Note that the spring stiffness is k and the spring damping is c
+		mSoftness = 1.0f / (inDeltaTime * (inDamping + inDeltaTime * inStiffness));
+
+		// Calculate bias factor (baumgarte stabilization):
+		// beta = dt * k / (c + dt * k) = dt * k^2 * softness
+		// b = beta / dt * C = dt * k * softness * C
+		mBias = inBias + inDeltaTime * inStiffness * mSoftness * inC;
+
+		// Update the effective mass, see post by Erin Catto: http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=1354
+		//
+		// Newton's Law:
+		// M * (v2 - v1) = J^T * lambda
+		//
+		// Velocity constraint with softness and Baumgarte:
+		// J * v2 + softness * lambda + b = 0
+		//
+		// where b = beta * C / dt
+		//
+		// We know everything except v2 and lambda.
+		//
+		// First solve Newton's law for v2 in terms of lambda:
+		//
+		// v2 = v1 + M^-1 * J^T * lambda
+		//
+		// Substitute this expression into the velocity constraint:
+		//
+		// J * (v1 + M^-1 * J^T * lambda) + softness * lambda + b = 0
+		//
+		// Now collect coefficients of lambda:
+		//
+		// (J * M^-1 * J^T + softness) * lambda = - J * v1 - b
+		//
+		// Now we define:
+		//
+		// K = J * M^-1 * J^T + softness
+		//
+		// So our new effective mass is K^-1
+		outEffectiveMass = 1.0f / (inInvEffectiveMass + mSoftness);
+	}
+
+public:
+	/// Turn off the spring and set a bias only
+	///
+	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
+	inline void					CalculateSpringPropertiesWithBias(float inBias)
+	{
+		mSoftness = 0.0f;
+		mBias = inBias;
+	}
+
+	/// Calculate spring properties based on frequency and damping ratio
+	///
+	/// @param inDeltaTime Time step
+	/// @param inInvEffectiveMass Inverse effective mass K
+	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
+	///	@param inC Value of the constraint equation (C). Set to zero if you don't want to drive the constraint to zero with a spring.
+	///	@param inFrequency Oscillation frequency (Hz). Set to zero if you don't want to drive the constraint to zero with a spring.
+	///	@param inDamping Damping factor (0 = no damping, 1 = critical damping). Set to zero if you don't want to drive the constraint to zero with a spring.
+	/// @param outEffectiveMass On return, this contains the new effective mass K^-1
+	inline void					CalculateSpringPropertiesWithFrequencyAndDamping(float inDeltaTime, float inInvEffectiveMass, float inBias, float inC, float inFrequency, float inDamping, float &outEffectiveMass)
+	{
+		outEffectiveMass = 1.0f / inInvEffectiveMass;
+
+		if (inFrequency > 0.0f)
+		{
+			// Calculate angular frequency
+			float omega = 2.0f * JPH_PI * inFrequency;
+
+			// Calculate spring stiffness k and damping constant c (page 45)
+			float k = outEffectiveMass * Square(omega);
+			float c = 2.0f * outEffectiveMass * inDamping * omega;
+
+			CalculateSpringPropertiesHelper(inDeltaTime, inInvEffectiveMass, inBias, inC, k, c, outEffectiveMass);
+		}
+		else
+		{
+			CalculateSpringPropertiesWithBias(inBias);
+		}
+	}
+
+	/// Calculate spring properties with spring Stiffness (k) and damping (c), this is based on the spring equation: F = -k * x - c * v
+	///
+	/// @param inDeltaTime Time step
+	/// @param inInvEffectiveMass Inverse effective mass K
+	/// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b
+	///	@param inC Value of the constraint equation (C). Set to zero if you don't want to drive the constraint to zero with a spring.
+	///	@param inStiffness Spring stiffness k. Set to zero if you don't want to drive the constraint to zero with a spring.
+	///	@param inDamping Spring damping coefficient c. Set to zero if you don't want to drive the constraint to zero with a spring.
+	/// @param outEffectiveMass On return, this contains the new effective mass K^-1
+	inline void					CalculateSpringPropertiesWithStiffnessAndDamping(float inDeltaTime, float inInvEffectiveMass, float inBias, float inC, float inStiffness, float inDamping, float &outEffectiveMass)
+	{
+		if (inStiffness > 0.0f)
+		{
+			CalculateSpringPropertiesHelper(inDeltaTime, inInvEffectiveMass, inBias, inC, inStiffness, inDamping, outEffectiveMass);
+		}
+		else
+		{
+			outEffectiveMass = 1.0f / inInvEffectiveMass;
+
+			CalculateSpringPropertiesWithBias(inBias);
+		}
+	}
+
+	/// Returns if this spring is active
+	inline bool					IsActive() const
+	{
+		return mSoftness != 0.0f;
+	}
+
+	/// Get total bias b, including supplied bias and bias for spring: lambda = J v + b
+	inline float				GetBias(float inTotalLambda) const
+	{
+		// Remainder of post by Erin Catto: http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=1354
+		//
+		// Each iteration we are not computing the whole impulse, we are computing an increment to the impulse and we are updating the velocity.
+		// Also, as we solve each constraint we get a perfect v2, but then some other constraint will come along and mess it up.
+		// So we want to patch up the constraint while acknowledging the accumulated impulse and the damaged velocity.
+		// To help with that we use P for the accumulated impulse and lambda as the update. Mathematically we have:
+		//
+		// M * (v2new - v2damaged) = J^T * lambda
+		// J * v2new + softness * (total_lambda + lambda) + b = 0
+		//
+		// If we solve this we get:
+		//
+		// v2new = v2damaged + M^-1 * J^T * lambda
+		// J * (v2damaged + M^-1 * J^T * lambda) + softness * total_lambda + softness * lambda + b = 0
+		//
+		// (J * M^-1 * J^T + softness) * lambda = -(J * v2damaged + softness * total_lambda + b)
+		//
+		// So our lagrange multiplier becomes:
+		//
+		// lambda = -K^-1 (J v + softness * total_lambda + b)
+		//
+		// So we return the bias: softness * total_lambda + b
+		return mSoftness * inTotalLambda + mBias;
+	}
+
+private:
+	float						mBias  = 0.0f;
+	float						mSoftness  = 0.0f;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/SwingTwistConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/SwingTwistConstraintPart.h
new file mode 100644
index 0000000000..c2a47547f5
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/SwingTwistConstraintPart.h
@@ -0,0 +1,597 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/Ellipse.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// How the swing limit behaves
+enum class ESwingType : uint8
+{
+	Cone,						///< Swing is limited by a cone shape, note that this cone starts to deform for larger swing angles. Cone limits only support limits that are symmetric around 0.
+	Pyramid,					///< Swing is limited by a pyramid shape, note that this pyramid starts to deform for larger swing angles.
+};
+
+/// Quaternion based constraint that decomposes the rotation in constraint space in swing and twist: q = q_swing * q_twist
+/// where q_swing.x = 0 and where q_twist.y = q_twist.z = 0
+///
+/// - Rotation around the twist (x-axis) is within [inTwistMinAngle, inTwistMaxAngle].
+/// - Rotation around the swing axis (y and z axis) are limited to an ellipsoid in quaternion space formed by the equation:
+///
+/// (q_swing.y / sin(inSwingYHalfAngle / 2))^2 + (q_swing.z / sin(inSwingZHalfAngle / 2))^2 <= 1
+///
+/// Which roughly corresponds to an elliptic cone shape with major axis (inSwingYHalfAngle, inSwingZHalfAngle).
+///
+/// In case inSwingYHalfAngle = 0, the rotation around Y will be constrained to 0 and the rotation around Z
+/// will be constrained between [-inSwingZHalfAngle, inSwingZHalfAngle]. Vice versa if inSwingZHalfAngle = 0.
+class SwingTwistConstraintPart
+{
+public:
+	/// Override the swing type
+	void						SetSwingType(ESwingType inSwingType)
+	{
+		mSwingType = inSwingType;
+	}
+
+	/// Get the swing type for this part
+	ESwingType					GetSwingType() const
+	{
+		return mSwingType;
+	}
+
+	/// Set limits for this constraint (see description above for parameters)
+	void						SetLimits(float inTwistMinAngle, float inTwistMaxAngle, float inSwingYMinAngle, float inSwingYMaxAngle, float inSwingZMinAngle, float inSwingZMaxAngle)
+	{
+		constexpr float cLockedAngle = DegreesToRadians(0.5f);
+		constexpr float cFreeAngle = DegreesToRadians(179.5f);
+
+		// Assume sane input
+		JPH_ASSERT(inTwistMinAngle <= inTwistMaxAngle);
+		JPH_ASSERT(inSwingYMinAngle <= inSwingYMaxAngle);
+		JPH_ASSERT(inSwingZMinAngle <= inSwingZMaxAngle);
+		JPH_ASSERT(inSwingYMinAngle >= -JPH_PI && inSwingYMaxAngle <= JPH_PI);
+		JPH_ASSERT(inSwingZMinAngle >= -JPH_PI && inSwingZMaxAngle <= JPH_PI);
+
+		// Calculate the sine and cosine of the half angles
+		Vec4 half_twist = 0.5f * Vec4(inTwistMinAngle, inTwistMaxAngle, 0, 0);
+		Vec4 twist_s, twist_c;
+		half_twist.SinCos(twist_s, twist_c);
+		Vec4 half_swing = 0.5f * Vec4(inSwingYMinAngle, inSwingYMaxAngle, inSwingZMinAngle, inSwingZMaxAngle);
+		Vec4 swing_s, swing_c;
+		half_swing.SinCos(swing_s, swing_c);
+
+		// Store half angles for pyramid limit
+		mSwingYHalfMinAngle = half_swing.GetX();
+		mSwingYHalfMaxAngle = half_swing.GetY();
+		mSwingZHalfMinAngle = half_swing.GetZ();
+		mSwingZHalfMaxAngle = half_swing.GetW();
+
+		// Store axis flags which are used at runtime to quickly decided which constraints to apply
+		mRotationFlags = 0;
+		if (inTwistMinAngle > -cLockedAngle && inTwistMaxAngle < cLockedAngle)
+		{
+			mRotationFlags |= TwistXLocked;
+			mSinTwistHalfMinAngle = 0.0f;
+			mSinTwistHalfMaxAngle = 0.0f;
+			mCosTwistHalfMinAngle = 1.0f;
+			mCosTwistHalfMaxAngle = 1.0f;
+		}
+		else if (inTwistMinAngle < -cFreeAngle && inTwistMaxAngle > cFreeAngle)
+		{
+			mRotationFlags |= TwistXFree;
+			mSinTwistHalfMinAngle = -1.0f;
+			mSinTwistHalfMaxAngle = 1.0f;
+			mCosTwistHalfMinAngle = 0.0f;
+			mCosTwistHalfMaxAngle = 0.0f;
+		}
+		else
+		{
+			mSinTwistHalfMinAngle = twist_s.GetX();
+			mSinTwistHalfMaxAngle = twist_s.GetY();
+			mCosTwistHalfMinAngle = twist_c.GetX();
+			mCosTwistHalfMaxAngle = twist_c.GetY();
+		}
+
+		if (inSwingYMinAngle > -cLockedAngle && inSwingYMaxAngle < cLockedAngle)
+		{
+			mRotationFlags |= SwingYLocked;
+			mSinSwingYHalfMinAngle = 0.0f;
+			mSinSwingYHalfMaxAngle = 0.0f;
+			mCosSwingYHalfMinAngle = 1.0f;
+			mCosSwingYHalfMaxAngle = 1.0f;
+		}
+		else if (inSwingYMinAngle < -cFreeAngle && inSwingYMaxAngle > cFreeAngle)
+		{
+			mRotationFlags |= SwingYFree;
+			mSinSwingYHalfMinAngle = -1.0f;
+			mSinSwingYHalfMaxAngle = 1.0f;
+			mCosSwingYHalfMinAngle = 0.0f;
+			mCosSwingYHalfMaxAngle = 0.0f;
+		}
+		else
+		{
+			mSinSwingYHalfMinAngle = swing_s.GetX();
+			mSinSwingYHalfMaxAngle = swing_s.GetY();
+			mCosSwingYHalfMinAngle = swing_c.GetX();
+			mCosSwingYHalfMaxAngle = swing_c.GetY();
+			JPH_ASSERT(mSinSwingYHalfMinAngle <= mSinSwingYHalfMaxAngle);
+		}
+
+		if (inSwingZMinAngle > -cLockedAngle && inSwingZMaxAngle < cLockedAngle)
+		{
+			mRotationFlags |= SwingZLocked;
+			mSinSwingZHalfMinAngle = 0.0f;
+			mSinSwingZHalfMaxAngle = 0.0f;
+			mCosSwingZHalfMinAngle = 1.0f;
+			mCosSwingZHalfMaxAngle = 1.0f;
+		}
+		else if (inSwingZMinAngle < -cFreeAngle && inSwingZMaxAngle > cFreeAngle)
+		{
+			mRotationFlags |= SwingZFree;
+			mSinSwingZHalfMinAngle = -1.0f;
+			mSinSwingZHalfMaxAngle = 1.0f;
+			mCosSwingZHalfMinAngle = 0.0f;
+			mCosSwingZHalfMaxAngle = 0.0f;
+		}
+		else
+		{
+			mSinSwingZHalfMinAngle = swing_s.GetZ();
+			mSinSwingZHalfMaxAngle = swing_s.GetW();
+			mCosSwingZHalfMinAngle = swing_c.GetZ();
+			mCosSwingZHalfMaxAngle = swing_c.GetW();
+			JPH_ASSERT(mSinSwingZHalfMinAngle <= mSinSwingZHalfMaxAngle);
+		}
+	}
+
+	/// Flags to indicate which axis got clamped by ClampSwingTwist
+	static constexpr uint		cClampedTwistMin = 1 << 0;
+	static constexpr uint		cClampedTwistMax = 1 << 1;
+	static constexpr uint		cClampedSwingYMin = 1 << 2;
+	static constexpr uint		cClampedSwingYMax = 1 << 3;
+	static constexpr uint		cClampedSwingZMin = 1 << 4;
+	static constexpr uint		cClampedSwingZMax = 1 << 5;
+
+	/// Helper function to determine if we're clamped against the min or max limit
+	static JPH_INLINE bool		sDistanceToMinShorter(float inDeltaMin, float inDeltaMax)
+	{
+		// We're outside of the limits, get actual delta to min/max range
+		// Note that a swing/twist of -1 and 1 represent the same angle, so if the difference is bigger than 1, the shortest angle is the other way around (2 - difference)
+		// We should actually be working with angles rather than sin(angle / 2). When the difference is small the approximation is accurate, but
+		// when working with extreme values the calculation is off and e.g. when the limit is between 0 and 180 a value of approx -60 will clamp
+		// to 180 rather than 0 (you'd expect anything > -90 to go to 0).
+		inDeltaMin = abs(inDeltaMin);
+		if (inDeltaMin > 1.0f) inDeltaMin = 2.0f - inDeltaMin;
+		inDeltaMax = abs(inDeltaMax);
+		if (inDeltaMax > 1.0f) inDeltaMax = 2.0f - inDeltaMax;
+		return inDeltaMin < inDeltaMax;
+	}
+
+	/// Clamp twist and swing against the constraint limits, returns which parts were clamped (everything assumed in constraint space)
+	inline void					ClampSwingTwist(Quat &ioSwing, Quat &ioTwist, uint &outClampedAxis) const
+	{
+		// Start with not clamped
+		outClampedAxis = 0;
+
+		// Check that swing and twist quaternions don't contain rotations around the wrong axis
+		JPH_ASSERT(ioSwing.GetX() == 0.0f);
+		JPH_ASSERT(ioTwist.GetY() == 0.0f);
+		JPH_ASSERT(ioTwist.GetZ() == 0.0f);
+
+		// Ensure quaternions have w > 0
+		bool negate_swing = ioSwing.GetW() < 0.0f;
+		if (negate_swing)
+			ioSwing = -ioSwing;
+		bool negate_twist = ioTwist.GetW() < 0.0f;
+		if (negate_twist)
+			ioTwist = -ioTwist;
+
+		if (mRotationFlags & TwistXLocked)
+		{
+			// Twist axis is locked, clamp whenever twist is not identity
+			outClampedAxis |= ioTwist.GetX() != 0.0f? (cClampedTwistMin | cClampedTwistMax) : 0;
+			ioTwist = Quat::sIdentity();
+		}
+		else if ((mRotationFlags & TwistXFree) == 0)
+		{
+			// Twist axis has limit, clamp whenever out of range
+			float delta_min = mSinTwistHalfMinAngle - ioTwist.GetX();
+			float delta_max = ioTwist.GetX() - mSinTwistHalfMaxAngle;
+			if (delta_min > 0.0f || delta_max > 0.0f)
+			{
+				// Pick the twist that corresponds to the smallest delta
+				if (sDistanceToMinShorter(delta_min, delta_max))
+				{
+					ioTwist = Quat(mSinTwistHalfMinAngle, 0, 0, mCosTwistHalfMinAngle);
+					outClampedAxis |= cClampedTwistMin;
+				}
+				else
+				{
+					ioTwist = Quat(mSinTwistHalfMaxAngle, 0, 0, mCosTwistHalfMaxAngle);
+					outClampedAxis |= cClampedTwistMax;
+				}
+			}
+		}
+
+		// Clamp swing
+		if (mRotationFlags & SwingYLocked)
+		{
+			if (mRotationFlags & SwingZLocked)
+			{
+				// Both swing Y and Z are disabled, no degrees of freedom in swing
+				outClampedAxis |= ioSwing.GetY() != 0.0f? (cClampedSwingYMin | cClampedSwingYMax) : 0;
+				outClampedAxis |= ioSwing.GetZ() != 0.0f? (cClampedSwingZMin | cClampedSwingZMax) : 0;
+				ioSwing = Quat::sIdentity();
+			}
+			else
+			{
+				// Swing Y angle disabled, only 1 degree of freedom in swing
+				outClampedAxis |= ioSwing.GetY() != 0.0f? (cClampedSwingYMin | cClampedSwingYMax) : 0;
+				float delta_min = mSinSwingZHalfMinAngle - ioSwing.GetZ();
+				float delta_max = ioSwing.GetZ() - mSinSwingZHalfMaxAngle;
+				if (delta_min > 0.0f || delta_max > 0.0f)
+				{
+					// Pick the swing that corresponds to the smallest delta
+					if (sDistanceToMinShorter(delta_min, delta_max))
+					{
+						ioSwing = Quat(0, 0, mSinSwingZHalfMinAngle, mCosSwingZHalfMinAngle);
+						outClampedAxis |= cClampedSwingZMin;
+					}
+					else
+					{
+						ioSwing = Quat(0, 0, mSinSwingZHalfMaxAngle, mCosSwingZHalfMaxAngle);
+						outClampedAxis |= cClampedSwingZMax;
+					}
+				}
+				else if ((outClampedAxis & cClampedSwingYMin) != 0)
+				{
+					float z = ioSwing.GetZ();
+					ioSwing = Quat(0, 0, z, sqrt(1.0f - Square(z)));
+				}
+			}
+		}
+		else if (mRotationFlags & SwingZLocked)
+		{
+			// Swing Z angle disabled, only 1 degree of freedom in swing
+			outClampedAxis |= ioSwing.GetZ() != 0.0f? (cClampedSwingZMin | cClampedSwingZMax) : 0;
+			float delta_min = mSinSwingYHalfMinAngle - ioSwing.GetY();
+			float delta_max = ioSwing.GetY() - mSinSwingYHalfMaxAngle;
+			if (delta_min > 0.0f || delta_max > 0.0f)
+			{
+				// Pick the swing that corresponds to the smallest delta
+				if (sDistanceToMinShorter(delta_min, delta_max))
+				{
+					ioSwing = Quat(0, mSinSwingYHalfMinAngle, 0, mCosSwingYHalfMinAngle);
+					outClampedAxis |= cClampedSwingYMin;
+				}
+				else
+				{
+					ioSwing = Quat(0, mSinSwingYHalfMaxAngle, 0, mCosSwingYHalfMaxAngle);
+					outClampedAxis |= cClampedSwingYMax;
+				}
+			}
+			else if ((outClampedAxis & cClampedSwingZMin) != 0)
+			{
+				float y = ioSwing.GetY();
+				ioSwing = Quat(0, y, 0, sqrt(1.0f - Square(y)));
+			}
+		}
+		else
+		{
+			// Two degrees of freedom
+			if (mSwingType == ESwingType::Cone)
+			{
+				// Use ellipse to solve limits
+				Ellipse ellipse(mSinSwingYHalfMaxAngle, mSinSwingZHalfMaxAngle);
+				Float2 point(ioSwing.GetY(), ioSwing.GetZ());
+				if (!ellipse.IsInside(point))
+				{
+					Float2 closest = ellipse.GetClosestPoint(point);
+					ioSwing = Quat(0, closest.x, closest.y, sqrt(max(0.0f, 1.0f - Square(closest.x) - Square(closest.y))));
+					outClampedAxis |= cClampedSwingYMin | cClampedSwingYMax | cClampedSwingZMin | cClampedSwingZMax; // We're not using the flags on which side we got clamped here
+				}
+			}
+			else
+			{
+				// Use pyramid to solve limits
+				// The quaternion rotating by angle y around the Y axis then rotating by angle z around the Z axis is:
+				// q = Quat::sRotation(Vec3::sAxisZ(), z) * Quat::sRotation(Vec3::sAxisY(), y)
+				// [q.x, q.y, q.z, q.w] = [-sin(y / 2) * sin(z / 2), sin(y / 2) * cos(z / 2), cos(y / 2) * sin(z / 2), cos(y / 2) * cos(z / 2)]
+				// So we can calculate y / 2 = atan2(q.y, q.w) and z / 2 = atan2(q.z, q.w)
+				Vec4 half_angle = Vec4::sATan2(ioSwing.GetXYZW().Swizzle<SWIZZLE_Y, SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_Z>(), ioSwing.GetXYZW().SplatW());
+				Vec4 min_half_angle(mSwingYHalfMinAngle, mSwingYHalfMinAngle, mSwingZHalfMinAngle, mSwingZHalfMinAngle);
+				Vec4 max_half_angle(mSwingYHalfMaxAngle, mSwingYHalfMaxAngle, mSwingZHalfMaxAngle, mSwingZHalfMaxAngle);
+				Vec4 clamped_half_angle = Vec4::sMin(Vec4::sMax(half_angle, min_half_angle), max_half_angle);
+				UVec4 unclamped = Vec4::sEquals(half_angle, clamped_half_angle);
+				if (!unclamped.TestAllTrue())
+				{
+					// We now calculate the quaternion again using the formula for q above,
+					// but we leave out the x component in order to not introduce twist
+					Vec4 s, c;
+					clamped_half_angle.SinCos(s, c);
+					ioSwing = Quat(0, s.GetY() * c.GetZ(), c.GetY() * s.GetZ(), c.GetY() * c.GetZ()).Normalized();
+					outClampedAxis |= cClampedSwingYMin | cClampedSwingYMax | cClampedSwingZMin | cClampedSwingZMax; // We're not using the flags on which side we got clamped here
+				}
+			}
+		}
+
+		// Flip sign back
+		if (negate_swing)
+			ioSwing = -ioSwing;
+		if (negate_twist)
+			ioTwist = -ioTwist;
+
+		JPH_ASSERT(ioSwing.IsNormalized());
+		JPH_ASSERT(ioTwist.IsNormalized());
+	}
+
+	/// Calculate properties used during the functions below
+	/// @param inBody1 The first body that this constraint is attached to
+	/// @param inBody2 The second body that this constraint is attached to
+	/// @param inConstraintRotation The current rotation of the constraint in constraint space
+	/// @param inConstraintToWorld Rotates from constraint space into world space
+	inline void					CalculateConstraintProperties(const Body &inBody1, const Body &inBody2, QuatArg inConstraintRotation, QuatArg inConstraintToWorld)
+	{
+		// Decompose into swing and twist
+		Quat q_swing, q_twist;
+		inConstraintRotation.GetSwingTwist(q_swing, q_twist);
+
+		// Clamp against joint limits
+		Quat q_clamped_swing = q_swing, q_clamped_twist = q_twist;
+		uint clamped_axis;
+		ClampSwingTwist(q_clamped_swing, q_clamped_twist, clamped_axis);
+
+		if (mRotationFlags & SwingYLocked)
+		{
+			Quat twist_to_world = inConstraintToWorld * q_swing;
+			mWorldSpaceSwingLimitYRotationAxis = twist_to_world.RotateAxisY();
+			mWorldSpaceSwingLimitZRotationAxis = twist_to_world.RotateAxisZ();
+
+			if (mRotationFlags & SwingZLocked)
+			{
+				// Swing fully locked
+				mSwingLimitYConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis);
+				mSwingLimitZConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitZRotationAxis);
+			}
+			else
+			{
+				// Swing only locked around Y
+				mSwingLimitYConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis);
+				if ((clamped_axis & (cClampedSwingZMin | cClampedSwingZMax)) != 0)
+				{
+					if ((clamped_axis & cClampedSwingZMin) != 0)
+						mWorldSpaceSwingLimitZRotationAxis = -mWorldSpaceSwingLimitZRotationAxis; // Flip axis if hitting min limit because the impulse limit is going to be between [-FLT_MAX, 0]
+					mSwingLimitZConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitZRotationAxis);
+				}
+				else
+					mSwingLimitZConstraintPart.Deactivate();
+			}
+		}
+		else if (mRotationFlags & SwingZLocked)
+		{
+			// Swing only locked around Z
+			Quat twist_to_world = inConstraintToWorld * q_swing;
+			mWorldSpaceSwingLimitYRotationAxis = twist_to_world.RotateAxisY();
+			mWorldSpaceSwingLimitZRotationAxis = twist_to_world.RotateAxisZ();
+
+			if ((clamped_axis & (cClampedSwingYMin | cClampedSwingYMax)) != 0)
+			{
+				if ((clamped_axis & cClampedSwingYMin) != 0)
+					mWorldSpaceSwingLimitYRotationAxis = -mWorldSpaceSwingLimitYRotationAxis; // Flip axis if hitting min limit because the impulse limit is going to be between [-FLT_MAX, 0]
+				mSwingLimitYConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis);
+			}
+			else
+				mSwingLimitYConstraintPart.Deactivate();
+			mSwingLimitZConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitZRotationAxis);
+		}
+		else if ((mRotationFlags & SwingYZFree) != SwingYZFree)
+		{
+			// Swing has limits around Y and Z
+			if ((clamped_axis & (cClampedSwingYMin | cClampedSwingYMax | cClampedSwingZMin | cClampedSwingZMax)) != 0)
+			{
+				// Calculate axis of rotation from clamped swing to swing
+				Vec3 current = (inConstraintToWorld * q_swing).RotateAxisX();
+				Vec3 desired = (inConstraintToWorld * q_clamped_swing).RotateAxisX();
+				mWorldSpaceSwingLimitYRotationAxis = desired.Cross(current);
+				float len = mWorldSpaceSwingLimitYRotationAxis.Length();
+				if (len != 0.0f)
+				{
+					mWorldSpaceSwingLimitYRotationAxis /= len;
+					mSwingLimitYConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis);
+				}
+				else
+					mSwingLimitYConstraintPart.Deactivate();
+			}
+			else
+				mSwingLimitYConstraintPart.Deactivate();
+			mSwingLimitZConstraintPart.Deactivate();
+		}
+		else
+		{
+			// No swing limits
+			mSwingLimitYConstraintPart.Deactivate();
+			mSwingLimitZConstraintPart.Deactivate();
+		}
+
+		if (mRotationFlags & TwistXLocked)
+		{
+			// Twist locked, always activate constraint
+			mWorldSpaceTwistLimitRotationAxis = (inConstraintToWorld * q_swing).RotateAxisX();
+			mTwistLimitConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceTwistLimitRotationAxis);
+		}
+		else if ((mRotationFlags & TwistXFree) == 0)
+		{
+			// Twist has limits
+			if ((clamped_axis & (cClampedTwistMin | cClampedTwistMax)) != 0)
+			{
+				mWorldSpaceTwistLimitRotationAxis = (inConstraintToWorld * q_swing).RotateAxisX();
+				if ((clamped_axis & cClampedTwistMin) != 0)
+					mWorldSpaceTwistLimitRotationAxis = -mWorldSpaceTwistLimitRotationAxis; // Flip axis if hitting min limit because the impulse limit is going to be between [-FLT_MAX, 0]
+				mTwistLimitConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceTwistLimitRotationAxis);
+			}
+			else
+				mTwistLimitConstraintPart.Deactivate();
+		}
+		else
+		{
+			// No twist limits
+			mTwistLimitConstraintPart.Deactivate();
+		}
+	}
+
+	/// Deactivate this constraint
+	void						Deactivate()
+	{
+		mSwingLimitYConstraintPart.Deactivate();
+		mSwingLimitZConstraintPart.Deactivate();
+		mTwistLimitConstraintPart.Deactivate();
+	}
+
+	/// Check if constraint is active
+	inline bool					IsActive() const
+	{
+		return mSwingLimitYConstraintPart.IsActive() || mSwingLimitZConstraintPart.IsActive() || mTwistLimitConstraintPart.IsActive();
+	}
+
+	/// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses
+	inline void					WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio)
+	{
+		mSwingLimitYConstraintPart.WarmStart(ioBody1, ioBody2, inWarmStartImpulseRatio);
+		mSwingLimitZConstraintPart.WarmStart(ioBody1, ioBody2, inWarmStartImpulseRatio);
+		mTwistLimitConstraintPart.WarmStart(ioBody1, ioBody2, inWarmStartImpulseRatio);
+	}
+
+	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.
+	inline bool					SolveVelocityConstraint(Body &ioBody1, Body &ioBody2)
+	{
+		bool impulse = false;
+
+		// Solve swing constraint
+		if (mSwingLimitYConstraintPart.IsActive())
+			impulse |= mSwingLimitYConstraintPart.SolveVelocityConstraint(ioBody1, ioBody2, mWorldSpaceSwingLimitYRotationAxis, -FLT_MAX, mSinSwingYHalfMinAngle == mSinSwingYHalfMaxAngle? FLT_MAX : 0.0f);
+
+		if (mSwingLimitZConstraintPart.IsActive())
+			impulse |= mSwingLimitZConstraintPart.SolveVelocityConstraint(ioBody1, ioBody2, mWorldSpaceSwingLimitZRotationAxis, -FLT_MAX, mSinSwingZHalfMinAngle == mSinSwingZHalfMaxAngle? FLT_MAX : 0.0f);
+
+		// Solve twist constraint
+		if (mTwistLimitConstraintPart.IsActive())
+			impulse |= mTwistLimitConstraintPart.SolveVelocityConstraint(ioBody1, ioBody2, mWorldSpaceTwistLimitRotationAxis, -FLT_MAX, mSinTwistHalfMinAngle == mSinTwistHalfMaxAngle? FLT_MAX : 0.0f);
+
+		return impulse;
+	}
+
+	/// Iteratively update the position constraint. Makes sure C(...) = 0.
+	/// @param ioBody1 The first body that this constraint is attached to
+	/// @param ioBody2 The second body that this constraint is attached to
+	/// @param inConstraintRotation The current rotation of the constraint in constraint space
+	/// @param inConstraintToBody1 , inConstraintToBody2 Rotates from constraint space to body 1/2 space
+	/// @param inBaumgarte Baumgarte constant (fraction of the error to correct)
+	inline bool					SolvePositionConstraint(Body &ioBody1, Body &ioBody2, QuatArg inConstraintRotation, QuatArg inConstraintToBody1, QuatArg inConstraintToBody2, float inBaumgarte) const
+	{
+		Quat q_swing, q_twist;
+		inConstraintRotation.GetSwingTwist(q_swing, q_twist);
+
+		uint clamped_axis;
+		ClampSwingTwist(q_swing, q_twist, clamped_axis);
+
+		// Solve rotation violations
+		if (clamped_axis != 0)
+		{
+			RotationEulerConstraintPart part;
+			Quat inv_initial_orientation = inConstraintToBody2 * (inConstraintToBody1 * q_swing * q_twist).Conjugated();
+			part.CalculateConstraintProperties(ioBody1, Mat44::sRotation(ioBody1.GetRotation()), ioBody2, Mat44::sRotation(ioBody2.GetRotation()));
+			return part.SolvePositionConstraint(ioBody1, ioBody2, inv_initial_orientation, inBaumgarte);
+		}
+
+		return false;
+	}
+
+	/// Return lagrange multiplier for swing
+	inline float				GetTotalSwingYLambda() const
+	{
+		return mSwingLimitYConstraintPart.GetTotalLambda();
+	}
+
+	inline float				GetTotalSwingZLambda() const
+	{
+		return mSwingLimitZConstraintPart.GetTotalLambda();
+	}
+
+	/// Return lagrange multiplier for twist
+	inline float				GetTotalTwistLambda() const
+	{
+		return mTwistLimitConstraintPart.GetTotalLambda();
+	}
+
+	/// Save state of this constraint part
+	void						SaveState(StateRecorder &inStream) const
+	{
+		mSwingLimitYConstraintPart.SaveState(inStream);
+		mSwingLimitZConstraintPart.SaveState(inStream);
+		mTwistLimitConstraintPart.SaveState(inStream);
+	}
+
+	/// Restore state of this constraint part
+	void						RestoreState(StateRecorder &inStream)
+	{
+		mSwingLimitYConstraintPart.RestoreState(inStream);
+		mSwingLimitZConstraintPart.RestoreState(inStream);
+		mTwistLimitConstraintPart.RestoreState(inStream);
+	}
+
+private:
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	enum ERotationFlags
+	{
+		/// Indicates that axis is completely locked (cannot rotate around this axis)
+		TwistXLocked			= 1 << 0,
+		SwingYLocked			= 1 << 1,
+		SwingZLocked			= 1 << 2,
+
+		/// Indicates that axis is completely free (can rotate around without limits)
+		TwistXFree				= 1 << 3,
+		SwingYFree				= 1 << 4,
+		SwingZFree				= 1 << 5,
+		SwingYZFree				= SwingYFree | SwingZFree
+	};
+
+	uint8						mRotationFlags;
+
+	// Constants
+	ESwingType					mSwingType = ESwingType::Cone;
+	float						mSinTwistHalfMinAngle;
+	float						mSinTwistHalfMaxAngle;
+	float						mCosTwistHalfMinAngle;
+	float						mCosTwistHalfMaxAngle;
+	float						mSwingYHalfMinAngle;
+	float						mSwingYHalfMaxAngle;
+	float						mSwingZHalfMinAngle;
+	float						mSwingZHalfMaxAngle;
+	float						mSinSwingYHalfMinAngle;
+	float						mSinSwingYHalfMaxAngle;
+	float						mSinSwingZHalfMinAngle;
+	float						mSinSwingZHalfMaxAngle;
+	float						mCosSwingYHalfMinAngle;
+	float						mCosSwingYHalfMaxAngle;
+	float						mCosSwingZHalfMinAngle;
+	float						mCosSwingZHalfMaxAngle;
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	/// Rotation axis for the angle constraint parts
+	Vec3						mWorldSpaceSwingLimitYRotationAxis;
+	Vec3						mWorldSpaceSwingLimitZRotationAxis;
+	Vec3						mWorldSpaceTwistLimitRotationAxis;
+
+	/// The constraint parts
+	AngleConstraintPart			mSwingLimitYConstraintPart;
+	AngleConstraintPart			mSwingLimitZConstraintPart;
+	AngleConstraintPart			mTwistLimitConstraintPart;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp
new file mode 100644
index 0000000000..928acdaa42
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp
@@ -0,0 +1,1783 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/ContactConstraintManager.h>
+#include <Jolt/Physics/Constraints/CalculateSolverSteps.h>
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Physics/PhysicsUpdateContext.h>
+#include <Jolt/Physics/PhysicsSettings.h>
+#include <Jolt/Physics/PhysicsSystem.h>
+#include <Jolt/Physics/IslandBuilder.h>
+#include <Jolt/Physics/DeterminismLog.h>
+#include <Jolt/Core/TempAllocator.h>
+#include <Jolt/Core/QuickSort.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+using namespace literals;
+
+#ifdef JPH_DEBUG_RENDERER
+bool ContactConstraintManager::sDrawContactPoint = false;
+bool ContactConstraintManager::sDrawSupportingFaces = false;
+bool ContactConstraintManager::sDrawContactPointReduction = false;
+bool ContactConstraintManager::sDrawContactManifolds = false;
+#endif // JPH_DEBUG_RENDERER
+
+//#define JPH_MANIFOLD_CACHE_DEBUG
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////
+// ContactConstraintManager::WorldContactPoint
+////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void ContactConstraintManager::WorldContactPoint::CalculateNonPenetrationConstraintProperties(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, const Body &inBody2, float inInvMass2, float inInvInertiaScale2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal)
+{
+	// Calculate collision points relative to body
+	RVec3 p = 0.5_r * (inWorldSpacePosition1 + inWorldSpacePosition2);
+	Vec3 r1 = Vec3(p - inBody1.GetCenterOfMassPosition());
+	Vec3 r2 = Vec3(p - inBody2.GetCenterOfMassPosition());
+
+	mNonPenetrationConstraint.CalculateConstraintPropertiesWithMassOverride(inBody1, inInvMass1, inInvInertiaScale1, r1, inBody2, inInvMass2, inInvInertiaScale2, r2, inWorldSpaceNormal);
+}
+
+template <EMotionType Type1, EMotionType Type2>
+JPH_INLINE void ContactConstraintManager::WorldContactPoint::TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(float inDeltaTime, float inGravityDeltaTimeDotNormal, const Body &inBody1, const Body &inBody2, float inInvM1, float inInvM2, Mat44Arg inInvI1, Mat44Arg inInvI2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal, Vec3Arg inWorldSpaceTangent1, Vec3Arg inWorldSpaceTangent2, const ContactSettings &inSettings, float inMinVelocityForRestitution)
+{
+	JPH_DET_LOG("TemplatedCalculateFrictionAndNonPenetrationConstraintProperties: p1: " << inWorldSpacePosition1 << " p2: " << inWorldSpacePosition2
+		<< " normal: " << inWorldSpaceNormal << " tangent1: " << inWorldSpaceTangent1 << " tangent2: " << inWorldSpaceTangent2
+		<< " restitution: " << inSettings.mCombinedRestitution << " friction: " << inSettings.mCombinedFriction << " minv: " << inMinVelocityForRestitution
+		<< " surface_vel: " << inSettings.mRelativeLinearSurfaceVelocity << " surface_ang: " << inSettings.mRelativeAngularSurfaceVelocity);
+
+	// Calculate collision points relative to body
+	RVec3 p = 0.5_r * (inWorldSpacePosition1 + inWorldSpacePosition2);
+	Vec3 r1 = Vec3(p - inBody1.GetCenterOfMassPosition());
+	Vec3 r2 = Vec3(p - inBody2.GetCenterOfMassPosition());
+
+	// The gravity is applied in the beginning of the time step. If we get here, there was a collision
+	// at the beginning of the time step, so we've applied too much gravity. This means that our
+	// calculated restitution can be too high, so when we apply restitution, we cancel the added
+	// velocity due to gravity.
+	float gravity_dt_dot_normal;
+
+	// Calculate velocity of collision points
+	Vec3 relative_velocity;
+	if constexpr (Type1 != EMotionType::Static && Type2 != EMotionType::Static)
+	{
+		const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked();
+		const MotionProperties *mp2 = inBody2.GetMotionPropertiesUnchecked();
+		relative_velocity = mp2->GetPointVelocityCOM(r2) - mp1->GetPointVelocityCOM(r1);
+		gravity_dt_dot_normal = inGravityDeltaTimeDotNormal * (mp2->GetGravityFactor() - mp1->GetGravityFactor());
+	}
+	else if constexpr (Type1 != EMotionType::Static)
+	{
+		const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked();
+		relative_velocity = -mp1->GetPointVelocityCOM(r1);
+		gravity_dt_dot_normal = inGravityDeltaTimeDotNormal * mp1->GetGravityFactor();
+	}
+	else if constexpr (Type2 != EMotionType::Static)
+	{
+		const MotionProperties *mp2 = inBody2.GetMotionPropertiesUnchecked();
+		relative_velocity = mp2->GetPointVelocityCOM(r2);
+		gravity_dt_dot_normal = inGravityDeltaTimeDotNormal * mp2->GetGravityFactor();
+	}
+	else
+	{
+		JPH_ASSERT(false); // Static vs static makes no sense
+		relative_velocity = Vec3::sZero();
+		gravity_dt_dot_normal = 0.0f;
+	}
+	float normal_velocity = relative_velocity.Dot(inWorldSpaceNormal);
+
+	// How much the shapes are penetrating (> 0 if penetrating, < 0 if separated)
+	float penetration = Vec3(inWorldSpacePosition1 - inWorldSpacePosition2).Dot(inWorldSpaceNormal);
+
+	// If there is no penetration, this is a speculative contact and we will apply a bias to the contact constraint
+	// so that the constraint becomes relative_velocity . contact normal > -penetration / delta_time
+	// instead of relative_velocity . contact normal > 0
+	// See: GDC 2013: "Physics for Game Programmers; Continuous Collision" - Erin Catto
+	float speculative_contact_velocity_bias = max(0.0f, -penetration / inDeltaTime);
+
+	// Determine if the velocity is big enough for restitution
+	float normal_velocity_bias;
+	if (inSettings.mCombinedRestitution > 0.0f && normal_velocity < -inMinVelocityForRestitution)
+	{
+		// We have a velocity that is big enough for restitution. This is where speculative contacts don't work
+		// great as we have to decide now if we're going to apply the restitution or not. If the relative
+		// velocity is big enough for a hit, we apply the restitution (in the end, due to other constraints,
+		// the objects may actually not collide and we will have applied restitution incorrectly). Another
+		// artifact that occurs because of this approximation is that the object will bounce from its current
+		// position rather than from a position where it is touching the other object. This causes the object
+		// to appear to move faster for 1 frame (the opposite of time stealing).
+		if (normal_velocity < -speculative_contact_velocity_bias)
+			normal_velocity_bias = inSettings.mCombinedRestitution * (normal_velocity - gravity_dt_dot_normal);
+		else
+			// In this case we have predicted that we don't hit the other object, but if we do (due to other constraints changing velocities)
+			// the speculative contact will prevent penetration but will not apply restitution leading to another artifact.
+			normal_velocity_bias = speculative_contact_velocity_bias;
+	}
+	else
+	{
+		// No restitution. We can safely apply our contact velocity bias.
+		normal_velocity_bias = speculative_contact_velocity_bias;
+	}
+
+	mNonPenetrationConstraint.TemplatedCalculateConstraintProperties<Type1, Type2>(inInvM1, inInvI1, r1, inInvM2, inInvI2, r2, inWorldSpaceNormal, normal_velocity_bias);
+
+	// Calculate friction part
+	if (inSettings.mCombinedFriction > 0.0f)
+	{
+		// Get surface velocity relative to tangents
+		Vec3 ws_surface_velocity = inSettings.mRelativeLinearSurfaceVelocity + inSettings.mRelativeAngularSurfaceVelocity.Cross(r1);
+		float surface_velocity1 = inWorldSpaceTangent1.Dot(ws_surface_velocity);
+		float surface_velocity2 = inWorldSpaceTangent2.Dot(ws_surface_velocity);
+
+		// Implement friction as 2 AxisContraintParts
+		mFrictionConstraint1.TemplatedCalculateConstraintProperties<Type1, Type2>(inInvM1, inInvI1, r1, inInvM2, inInvI2, r2, inWorldSpaceTangent1, surface_velocity1);
+		mFrictionConstraint2.TemplatedCalculateConstraintProperties<Type1, Type2>(inInvM1, inInvI1, r1, inInvM2, inInvI2, r2, inWorldSpaceTangent2, surface_velocity2);
+	}
+	else
+	{
+		// Turn off friction constraint
+		mFrictionConstraint1.Deactivate();
+		mFrictionConstraint2.Deactivate();
+	}
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////
+// ContactConstraintManager::ContactConstraint
+////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+#ifdef JPH_DEBUG_RENDERER
+void ContactConstraintManager::ContactConstraint::Draw(DebugRenderer *inRenderer, ColorArg inManifoldColor) const
+{
+	if (mContactPoints.empty())
+		return;
+
+	// Get body transforms
+	RMat44 transform_body1 = mBody1->GetCenterOfMassTransform();
+	RMat44 transform_body2 = mBody2->GetCenterOfMassTransform();
+
+	RVec3 prev_point = transform_body1 * Vec3::sLoadFloat3Unsafe(mContactPoints.back().mContactPoint->mPosition1);
+	for (const WorldContactPoint &wcp : mContactPoints)
+	{
+		// Test if any lambda from the previous frame was transferred
+		float radius = wcp.mNonPenetrationConstraint.GetTotalLambda() == 0.0f
+					&& wcp.mFrictionConstraint1.GetTotalLambda() == 0.0f
+					&& wcp.mFrictionConstraint2.GetTotalLambda() == 0.0f? 0.1f :  0.2f;
+
+		RVec3 next_point = transform_body1 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition1);
+		inRenderer->DrawMarker(next_point, Color::sCyan, radius);
+		inRenderer->DrawMarker(transform_body2 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition2), Color::sPurple, radius);
+
+		// Draw edge
+		inRenderer->DrawArrow(prev_point, next_point, inManifoldColor, 0.05f);
+		prev_point = next_point;
+	}
+
+	// Draw normal
+	RVec3 wp = transform_body1 * Vec3::sLoadFloat3Unsafe(mContactPoints[0].mContactPoint->mPosition1);
+	inRenderer->DrawArrow(wp, wp + GetWorldSpaceNormal(), Color::sRed, 0.05f);
+
+	// Get tangents
+	Vec3 t1, t2;
+	GetTangents(t1, t2);
+
+	// Draw tangents
+	inRenderer->DrawLine(wp, wp + t1, Color::sGreen);
+	inRenderer->DrawLine(wp, wp + t2, Color::sBlue);
+}
+#endif // JPH_DEBUG_RENDERER
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////
+// ContactConstraintManager::CachedContactPoint
+////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void ContactConstraintManager::CachedContactPoint::SaveState(StateRecorder &inStream) const
+{
+	inStream.Write(mPosition1);
+	inStream.Write(mPosition2);
+	inStream.Write(mNonPenetrationLambda);
+	inStream.Write(mFrictionLambda);
+}
+
+void ContactConstraintManager::CachedContactPoint::RestoreState(StateRecorder &inStream)
+{
+	inStream.Read(mPosition1);
+	inStream.Read(mPosition2);
+	inStream.Read(mNonPenetrationLambda);
+	inStream.Read(mFrictionLambda);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////
+// ContactConstraintManager::CachedManifold
+////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void ContactConstraintManager::CachedManifold::SaveState(StateRecorder &inStream) const
+{
+	inStream.Write(mContactNormal);
+}
+
+void ContactConstraintManager::CachedManifold::RestoreState(StateRecorder &inStream)
+{
+	inStream.Read(mContactNormal);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////
+// ContactConstraintManager::CachedBodyPair
+////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void ContactConstraintManager::CachedBodyPair::SaveState(StateRecorder &inStream) const
+{
+	inStream.Write(mDeltaPosition);
+	inStream.Write(mDeltaRotation);
+}
+
+void ContactConstraintManager::CachedBodyPair::RestoreState(StateRecorder &inStream)
+{
+	inStream.Read(mDeltaPosition);
+	inStream.Read(mDeltaRotation);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////
+// ContactConstraintManager::ManifoldCache
+////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void ContactConstraintManager::ManifoldCache::Init(uint inMaxBodyPairs, uint inMaxContactConstraints, uint inCachedManifoldsSize)
+{
+	mAllocator.Init(inMaxBodyPairs * sizeof(BodyPairMap::KeyValue) + inCachedManifoldsSize);
+	mCachedManifolds.Init(GetNextPowerOf2(inMaxContactConstraints));
+	mCachedBodyPairs.Init(GetNextPowerOf2(inMaxBodyPairs));
+}
+
+void ContactConstraintManager::ManifoldCache::Clear()
+{
+	JPH_PROFILE_FUNCTION();
+
+	mCachedManifolds.Clear();
+	mCachedBodyPairs.Clear();
+	mAllocator.Clear();
+
+#ifdef JPH_ENABLE_ASSERTS
+	// Mark as incomplete
+	mIsFinalized = false;
+#endif
+}
+
+void ContactConstraintManager::ManifoldCache::Prepare(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds)
+{
+	// Minimum amount of buckets to use in the hash map
+	constexpr uint32 cMinBuckets = 1024;
+
+	// Use the next higher power of 2 of amount of objects in the cache from last frame to determine the amount of buckets in this frame
+	mCachedManifolds.SetNumBuckets(min(max(cMinBuckets, GetNextPowerOf2(inExpectedNumManifolds)), mCachedManifolds.GetMaxBuckets()));
+	mCachedBodyPairs.SetNumBuckets(min(max(cMinBuckets, GetNextPowerOf2(inExpectedNumBodyPairs)), mCachedBodyPairs.GetMaxBuckets()));
+}
+
+const ContactConstraintManager::MKeyValue *ContactConstraintManager::ManifoldCache::Find(const SubShapeIDPair &inKey, uint64 inKeyHash) const
+{
+	JPH_ASSERT(mIsFinalized);
+	return mCachedManifolds.Find(inKey, inKeyHash);
+}
+
+ContactConstraintManager::MKeyValue *ContactConstraintManager::ManifoldCache::Create(ContactAllocator &ioContactAllocator, const SubShapeIDPair &inKey, uint64 inKeyHash, int inNumContactPoints)
+{
+	JPH_ASSERT(!mIsFinalized);
+	MKeyValue *kv = mCachedManifolds.Create(ioContactAllocator, inKey, inKeyHash, CachedManifold::sGetRequiredExtraSize(inNumContactPoints));
+	if (kv == nullptr)
+	{
+		ioContactAllocator.mErrors |= EPhysicsUpdateError::ManifoldCacheFull;
+		return nullptr;
+	}
+	kv->GetValue().mNumContactPoints = uint16(inNumContactPoints);
+	++ioContactAllocator.mNumManifolds;
+	return kv;
+}
+
+ContactConstraintManager::MKVAndCreated ContactConstraintManager::ManifoldCache::FindOrCreate(ContactAllocator &ioContactAllocator, const SubShapeIDPair &inKey, uint64 inKeyHash, int inNumContactPoints)
+{
+	MKeyValue *kv = const_cast<MKeyValue *>(mCachedManifolds.Find(inKey, inKeyHash));
+	if (kv != nullptr)
+		return { kv, false };
+
+	return { Create(ioContactAllocator, inKey, inKeyHash, inNumContactPoints), true };
+}
+
+uint32 ContactConstraintManager::ManifoldCache::ToHandle(const MKeyValue *inKeyValue) const
+{
+	JPH_ASSERT(!mIsFinalized);
+	return mCachedManifolds.ToHandle(inKeyValue);
+}
+
+const ContactConstraintManager::MKeyValue *ContactConstraintManager::ManifoldCache::FromHandle(uint32 inHandle) const
+{
+	JPH_ASSERT(mIsFinalized);
+	return mCachedManifolds.FromHandle(inHandle);
+}
+
+const ContactConstraintManager::BPKeyValue *ContactConstraintManager::ManifoldCache::Find(const BodyPair &inKey, uint64 inKeyHash) const
+{
+	JPH_ASSERT(mIsFinalized);
+	return mCachedBodyPairs.Find(inKey, inKeyHash);
+}
+
+ContactConstraintManager::BPKeyValue *ContactConstraintManager::ManifoldCache::Create(ContactAllocator &ioContactAllocator, const BodyPair &inKey, uint64 inKeyHash)
+{
+	JPH_ASSERT(!mIsFinalized);
+	BPKeyValue *kv = mCachedBodyPairs.Create(ioContactAllocator, inKey, inKeyHash, 0);
+	if (kv == nullptr)
+	{
+		ioContactAllocator.mErrors |= EPhysicsUpdateError::BodyPairCacheFull;
+		return nullptr;
+	}
+	++ioContactAllocator.mNumBodyPairs;
+	return kv;
+}
+
+void ContactConstraintManager::ManifoldCache::GetAllBodyPairsSorted(Array<const BPKeyValue *> &outAll) const
+{
+	JPH_ASSERT(mIsFinalized);
+	mCachedBodyPairs.GetAllKeyValues(outAll);
+
+	// Sort by key
+	QuickSort(outAll.begin(), outAll.end(), [](const BPKeyValue *inLHS, const BPKeyValue *inRHS) {
+		return inLHS->GetKey() < inRHS->GetKey();
+	});
+}
+
+void ContactConstraintManager::ManifoldCache::GetAllManifoldsSorted(const CachedBodyPair &inBodyPair, Array<const MKeyValue *> &outAll) const
+{
+	JPH_ASSERT(mIsFinalized);
+
+	// Iterate through the attached manifolds
+	for (uint32 handle = inBodyPair.mFirstCachedManifold; handle != ManifoldMap::cInvalidHandle; handle = FromHandle(handle)->GetValue().mNextWithSameBodyPair)
+	{
+		const MKeyValue *kv = mCachedManifolds.FromHandle(handle);
+		outAll.push_back(kv);
+	}
+
+	// Sort by key
+	QuickSort(outAll.begin(), outAll.end(), [](const MKeyValue *inLHS, const MKeyValue *inRHS) {
+		return inLHS->GetKey() < inRHS->GetKey();
+	});
+}
+
+void ContactConstraintManager::ManifoldCache::GetAllCCDManifoldsSorted(Array<const MKeyValue *> &outAll) const
+{
+	mCachedManifolds.GetAllKeyValues(outAll);
+
+	for (int i = (int)outAll.size() - 1; i >= 0; --i)
+		if ((outAll[i]->GetValue().mFlags & (uint16)CachedManifold::EFlags::CCDContact) == 0)
+		{
+			outAll[i] = outAll.back();
+			outAll.pop_back();
+		}
+
+	// Sort by key
+	QuickSort(outAll.begin(), outAll.end(), [](const MKeyValue *inLHS, const MKeyValue *inRHS) {
+		return inLHS->GetKey() < inRHS->GetKey();
+	});
+}
+
+void ContactConstraintManager::ManifoldCache::ContactPointRemovedCallbacks(ContactListener *inListener)
+{
+	JPH_PROFILE_FUNCTION();
+
+	for (MKeyValue &kv : mCachedManifolds)
+		if ((kv.GetValue().mFlags & uint16(CachedManifold::EFlags::ContactPersisted)) == 0)
+			inListener->OnContactRemoved(kv.GetKey());
+}
+
+#ifdef JPH_ENABLE_ASSERTS
+
+void ContactConstraintManager::ManifoldCache::Finalize()
+{
+	mIsFinalized = true;
+
+#ifdef JPH_MANIFOLD_CACHE_DEBUG
+	Trace("ManifoldMap:");
+	mCachedManifolds.TraceStats();
+	Trace("BodyPairMap:");
+	mCachedBodyPairs.TraceStats();
+#endif // JPH_MANIFOLD_CACHE_DEBUG
+}
+
+#endif
+
+void ContactConstraintManager::ManifoldCache::SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const
+{
+	JPH_ASSERT(mIsFinalized);
+
+	// Get contents of cache
+	Array<const BPKeyValue *> all_bp;
+	GetAllBodyPairsSorted(all_bp);
+
+	// Determine which ones to save
+	Array<const BPKeyValue *> selected_bp;
+	if (inFilter == nullptr)
+		selected_bp = std::move(all_bp);
+	else
+	{
+		selected_bp.reserve(all_bp.size());
+		for (const BPKeyValue *bp_kv : all_bp)
+			if (inFilter->ShouldSaveContact(bp_kv->GetKey().mBodyA, bp_kv->GetKey().mBodyB))
+				selected_bp.push_back(bp_kv);
+	}
+
+	// Write body pairs
+	uint32 num_body_pairs = uint32(selected_bp.size());
+	inStream.Write(num_body_pairs);
+	for (const BPKeyValue *bp_kv : selected_bp)
+	{
+		// Write body pair key
+		inStream.Write(bp_kv->GetKey());
+
+		// Write body pair
+		const CachedBodyPair &bp = bp_kv->GetValue();
+		bp.SaveState(inStream);
+
+		// Get attached manifolds
+		Array<const MKeyValue *> all_m;
+		GetAllManifoldsSorted(bp, all_m);
+
+		// Write num manifolds
+		uint32 num_manifolds = uint32(all_m.size());
+		inStream.Write(num_manifolds);
+
+		// Write all manifolds
+		for (const MKeyValue *m_kv : all_m)
+		{
+			// Write key
+			inStream.Write(m_kv->GetKey());
+			const CachedManifold &cm = m_kv->GetValue();
+			JPH_ASSERT((cm.mFlags & (uint16)CachedManifold::EFlags::CCDContact) == 0);
+
+			// Write amount of contacts
+			inStream.Write(cm.mNumContactPoints);
+
+			// Write manifold
+			cm.SaveState(inStream);
+
+			// Write contact points
+			for (uint32 i = 0; i < cm.mNumContactPoints; ++i)
+				cm.mContactPoints[i].SaveState(inStream);
+		}
+	}
+
+	// Get CCD manifolds
+	Array<const MKeyValue *> all_m;
+	GetAllCCDManifoldsSorted(all_m);
+
+	// Determine which ones to save
+	Array<const MKeyValue *> selected_m;
+	if (inFilter == nullptr)
+		selected_m = std::move(all_m);
+	else
+	{
+		selected_m.reserve(all_m.size());
+		for (const MKeyValue *m_kv : all_m)
+			if (inFilter->ShouldSaveContact(m_kv->GetKey().GetBody1ID(), m_kv->GetKey().GetBody2ID()))
+				selected_m.push_back(m_kv);
+	}
+
+	// Write all CCD manifold keys
+	uint32 num_manifolds = uint32(selected_m.size());
+	inStream.Write(num_manifolds);
+	for (const MKeyValue *m_kv : selected_m)
+		inStream.Write(m_kv->GetKey());
+}
+
+bool ContactConstraintManager::ManifoldCache::RestoreState(const ManifoldCache &inReadCache, StateRecorder &inStream, const StateRecorderFilter *inFilter)
+{
+	JPH_ASSERT(!mIsFinalized);
+
+	bool success = true;
+
+	// Create a contact allocator for restoring the contact cache
+	ContactAllocator contact_allocator(GetContactAllocator());
+
+	// When validating, get all existing body pairs
+	Array<const BPKeyValue *> all_bp;
+	if (inStream.IsValidating())
+		inReadCache.GetAllBodyPairsSorted(all_bp);
+
+	// Read amount of body pairs
+	uint32 num_body_pairs;
+	if (inStream.IsValidating())
+		num_body_pairs = uint32(all_bp.size());
+	inStream.Read(num_body_pairs);
+
+	// Read entire cache
+	for (uint32 i = 0; i < num_body_pairs; ++i)
+	{
+		// Read key
+		BodyPair body_pair_key;
+		if (inStream.IsValidating() && i < all_bp.size())
+			body_pair_key = all_bp[i]->GetKey();
+		inStream.Read(body_pair_key);
+
+		// Check if we want to restore this contact
+		if (inFilter == nullptr || inFilter->ShouldRestoreContact(body_pair_key.mBodyA, body_pair_key.mBodyB))
+		{
+			// Create new entry for this body pair
+			uint64 body_pair_hash = body_pair_key.GetHash();
+			BPKeyValue *bp_kv = Create(contact_allocator, body_pair_key, body_pair_hash);
+			if (bp_kv == nullptr)
+			{
+				// Out of cache space
+				success = false;
+				break;
+			}
+			CachedBodyPair &bp = bp_kv->GetValue();
+
+			// Read body pair
+			if (inStream.IsValidating() && i < all_bp.size())
+				memcpy(&bp, &all_bp[i]->GetValue(), sizeof(CachedBodyPair));
+			bp.RestoreState(inStream);
+
+			// When validating, get all existing manifolds
+			Array<const MKeyValue *> all_m;
+			if (inStream.IsValidating())
+				inReadCache.GetAllManifoldsSorted(all_bp[i]->GetValue(), all_m);
+
+			// Read amount of manifolds
+			uint32 num_manifolds = 0;
+			if (inStream.IsValidating())
+				num_manifolds = uint32(all_m.size());
+			inStream.Read(num_manifolds);
+
+			uint32 handle = ManifoldMap::cInvalidHandle;
+			for (uint32 j = 0; j < num_manifolds; ++j)
+			{
+				// Read key
+				SubShapeIDPair sub_shape_key;
+				if (inStream.IsValidating() && j < all_m.size())
+					sub_shape_key = all_m[j]->GetKey();
+				inStream.Read(sub_shape_key);
+				uint64 sub_shape_key_hash = sub_shape_key.GetHash();
+
+				// Read amount of contact points
+				uint16 num_contact_points = 0;
+				if (inStream.IsValidating() && j < all_m.size())
+					num_contact_points = all_m[j]->GetValue().mNumContactPoints;
+				inStream.Read(num_contact_points);
+
+				// Read manifold
+				MKeyValue *m_kv = Create(contact_allocator, sub_shape_key, sub_shape_key_hash, num_contact_points);
+				if (m_kv == nullptr)
+				{
+					// Out of cache space
+					success = false;
+					break;
+				}
+				CachedManifold &cm = m_kv->GetValue();
+				if (inStream.IsValidating() && j < all_m.size())
+				{
+					memcpy(&cm, &all_m[j]->GetValue(), CachedManifold::sGetRequiredTotalSize(num_contact_points));
+					cm.mNumContactPoints = uint16(num_contact_points); // Restore num contact points
+				}
+				cm.RestoreState(inStream);
+				cm.mNextWithSameBodyPair = handle;
+				handle = ToHandle(m_kv);
+
+				// Read contact points
+				for (uint32 k = 0; k < num_contact_points; ++k)
+					cm.mContactPoints[k].RestoreState(inStream);
+			}
+			bp.mFirstCachedManifold = handle;
+		}
+		else
+		{
+			// Skip the contact
+			CachedBodyPair bp;
+			bp.RestoreState(inStream);
+			uint32 num_manifolds = 0;
+			inStream.Read(num_manifolds);
+			for (uint32 j = 0; j < num_manifolds; ++j)
+			{
+				SubShapeIDPair sub_shape_key;
+				inStream.Read(sub_shape_key);
+				uint16 num_contact_points;
+				inStream.Read(num_contact_points);
+				CachedManifold cm;
+				cm.RestoreState(inStream);
+				for (uint32 k = 0; k < num_contact_points; ++k)
+					cm.mContactPoints[0].RestoreState(inStream);
+			}
+		}
+	}
+
+	// When validating, get all existing CCD manifolds
+	Array<const MKeyValue *> all_m;
+	if (inStream.IsValidating())
+		inReadCache.GetAllCCDManifoldsSorted(all_m);
+
+	// Read amount of CCD manifolds
+	uint32 num_manifolds;
+	if (inStream.IsValidating())
+		num_manifolds = uint32(all_m.size());
+	inStream.Read(num_manifolds);
+
+	for (uint32 j = 0; j < num_manifolds; ++j)
+	{
+		// Read key
+		SubShapeIDPair sub_shape_key;
+		if (inStream.IsValidating() && j < all_m.size())
+			sub_shape_key = all_m[j]->GetKey();
+		inStream.Read(sub_shape_key);
+
+		// Check if we want to restore this contact
+		if (inFilter == nullptr || inFilter->ShouldRestoreContact(sub_shape_key.GetBody1ID(), sub_shape_key.GetBody2ID()))
+		{
+			// Create CCD manifold
+			uint64 sub_shape_key_hash = sub_shape_key.GetHash();
+			MKeyValue *m_kv = Create(contact_allocator, sub_shape_key, sub_shape_key_hash, 0);
+			if (m_kv == nullptr)
+			{
+				// Out of cache space
+				success = false;
+				break;
+			}
+			CachedManifold &cm = m_kv->GetValue();
+			cm.mFlags |= (uint16)CachedManifold::EFlags::CCDContact;
+		}
+	}
+
+#ifdef JPH_ENABLE_ASSERTS
+	// We don't finalize until the last part is restored
+	if (inStream.IsLastPart())
+		mIsFinalized = true;
+#endif
+
+	return success;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////
+// ContactConstraintManager
+////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ContactConstraintManager::ContactConstraintManager(const PhysicsSettings &inPhysicsSettings) :
+	mPhysicsSettings(inPhysicsSettings)
+{
+#ifdef JPH_ENABLE_ASSERTS
+	// For the first frame mark this empty buffer as finalized
+	mCache[mCacheWriteIdx ^ 1].Finalize();
+#endif
+}
+
+ContactConstraintManager::~ContactConstraintManager()
+{
+	JPH_ASSERT(mConstraints == nullptr);
+}
+
+void ContactConstraintManager::Init(uint inMaxBodyPairs, uint inMaxContactConstraints)
+{
+	mMaxConstraints = inMaxContactConstraints;
+
+	// Calculate worst case cache usage
+	uint cached_manifolds_size = inMaxContactConstraints * (sizeof(CachedManifold) + (MaxContactPoints - 1) * sizeof(CachedContactPoint));
+
+	// Init the caches
+	mCache[0].Init(inMaxBodyPairs, inMaxContactConstraints, cached_manifolds_size);
+	mCache[1].Init(inMaxBodyPairs, inMaxContactConstraints, cached_manifolds_size);
+}
+
+void ContactConstraintManager::PrepareConstraintBuffer(PhysicsUpdateContext *inContext)
+{
+	// Store context
+	mUpdateContext = inContext;
+
+	// Allocate temporary constraint buffer
+	JPH_ASSERT(mConstraints == nullptr);
+	mConstraints = (ContactConstraint *)inContext->mTempAllocator->Allocate(mMaxConstraints * sizeof(ContactConstraint));
+}
+
+template <EMotionType Type1, EMotionType Type2>
+JPH_INLINE void ContactConstraintManager::TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2)
+{
+	// Calculate scaled mass and inertia
+	Mat44 inv_i1;
+	if constexpr (Type1 == EMotionType::Dynamic)
+	{
+		const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked();
+		inv_i1 = inSettings.mInvInertiaScale1 * mp1->GetInverseInertiaForRotation(inTransformBody1.GetRotation());
+	}
+	else
+	{
+		inv_i1 = Mat44::sZero();
+	}
+
+	Mat44 inv_i2;
+	if constexpr (Type2 == EMotionType::Dynamic)
+	{
+		const MotionProperties *mp2 = inBody2.GetMotionPropertiesUnchecked();
+		inv_i2 = inSettings.mInvInertiaScale2 * mp2->GetInverseInertiaForRotation(inTransformBody2.GetRotation());
+	}
+	else
+	{
+		inv_i2 = Mat44::sZero();
+	}
+
+	// Calculate tangents
+	Vec3 t1, t2;
+	ioConstraint.GetTangents(t1, t2);
+
+	Vec3 ws_normal = ioConstraint.GetWorldSpaceNormal();
+
+	// Calculate value for restitution correction
+	float gravity_dt_dot_normal = inGravityDeltaTime.Dot(ws_normal);
+
+	// Setup velocity constraint properties
+	float min_velocity_for_restitution = mPhysicsSettings.mMinVelocityForRestitution;
+	for (WorldContactPoint &wcp : ioConstraint.mContactPoints)
+	{
+		RVec3 p1 = inTransformBody1 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition1);
+		RVec3 p2 = inTransformBody2 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition2);
+		wcp.TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<Type1, Type2>(inDeltaTime, gravity_dt_dot_normal, inBody1, inBody2, ioConstraint.mInvMass1, ioConstraint.mInvMass2, inv_i1, inv_i2, p1, p2, ws_normal, t1, t2, inSettings, min_velocity_for_restitution);
+	}
+}
+
+inline void ContactConstraintManager::CalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2)
+{
+	// Dispatch to the correct templated form
+	switch (inBody1.GetMotionType())
+	{
+	case EMotionType::Dynamic:
+		switch (inBody2.GetMotionType())
+		{
+		case EMotionType::Dynamic:
+			TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Dynamic>(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
+			break;
+
+		case EMotionType::Kinematic:
+			TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Kinematic>(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
+			break;
+
+		case EMotionType::Static:
+			TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Static>(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
+			break;
+
+		default:
+			JPH_ASSERT(false);
+			break;
+		}
+		break;
+
+	case EMotionType::Kinematic:
+		JPH_ASSERT(inBody2.IsDynamic());
+		TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Kinematic, EMotionType::Dynamic>(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
+		break;
+
+	case EMotionType::Static:
+		JPH_ASSERT(inBody2.IsDynamic());
+		TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Static, EMotionType::Dynamic>(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
+		break;
+
+	default:
+		JPH_ASSERT(false);
+		break;
+	}
+}
+
+void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactAllocator, Body &inBody1, Body &inBody2, bool &outPairHandled, bool &outConstraintCreated)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Start with nothing found and not handled
+	outConstraintCreated = false;
+	outPairHandled = false;
+
+	// Swap bodies so that body 1 id < body 2 id
+	Body *body1, *body2;
+	if (inBody1.GetID() < inBody2.GetID())
+	{
+		body1 = &inBody1;
+		body2 = &inBody2;
+	}
+	else
+	{
+		body1 = &inBody2;
+		body2 = &inBody1;
+	}
+
+	// Find the cached body pair
+	BodyPair body_pair_key(body1->GetID(), body2->GetID());
+	uint64 body_pair_hash = body_pair_key.GetHash();
+	const ManifoldCache &read_cache = mCache[mCacheWriteIdx ^ 1];
+	const BPKeyValue *kv = read_cache.Find(body_pair_key, body_pair_hash);
+	if (kv == nullptr)
+		return;
+	const CachedBodyPair &input_cbp = kv->GetValue();
+
+	// Get relative translation
+	Quat inv_r1 = body1->GetRotation().Conjugated();
+	Vec3 delta_position = inv_r1 * Vec3(body2->GetCenterOfMassPosition() - body1->GetCenterOfMassPosition());
+
+	// Get old position delta
+	Vec3 old_delta_position = Vec3::sLoadFloat3Unsafe(input_cbp.mDeltaPosition);
+
+	// Check if bodies are still roughly in the same relative position
+	if ((delta_position - old_delta_position).LengthSq() > mPhysicsSettings.mBodyPairCacheMaxDeltaPositionSq)
+		return;
+
+	// Determine relative orientation
+	Quat delta_rotation = inv_r1 * body2->GetRotation();
+
+	// Reconstruct old quaternion delta
+	Quat old_delta_rotation = Quat::sLoadFloat3Unsafe(input_cbp.mDeltaRotation);
+
+	// Check if bodies are still roughly in the same relative orientation
+	// The delta between 2 quaternions p and q is: p q^* = [rotation_axis * sin(angle / 2), cos(angle / 2)]
+	// From the W component we can extract the angle: cos(angle / 2) = px * qx + py * qy + pz * qz + pw * qw = p . q
+	// Since we want to abort if the rotation is smaller than -angle or bigger than angle, we can write the comparison as |p . q| < cos(angle / 2)
+	if (abs(delta_rotation.Dot(old_delta_rotation)) < mPhysicsSettings.mBodyPairCacheCosMaxDeltaRotationDiv2)
+		return;
+
+	// The cache is valid, return that we've handled this body pair
+	outPairHandled = true;
+
+	// Copy the cached body pair to this frame
+	ManifoldCache &write_cache = mCache[mCacheWriteIdx];
+	BPKeyValue *output_bp_kv = write_cache.Create(ioContactAllocator, body_pair_key, body_pair_hash);
+	if (output_bp_kv == nullptr)
+		return; // Out of cache space
+	CachedBodyPair *output_cbp = &output_bp_kv->GetValue();
+	memcpy(output_cbp, &input_cbp, sizeof(CachedBodyPair));
+
+	// If there were no contacts, we have handled the contact
+	if (input_cbp.mFirstCachedManifold == ManifoldMap::cInvalidHandle)
+		return;
+
+	// Get body transforms
+	RMat44 transform_body1 = body1->GetCenterOfMassTransform();
+	RMat44 transform_body2 = body2->GetCenterOfMassTransform();
+
+	// Get time step
+	float delta_time = mUpdateContext->mStepDeltaTime;
+
+	// Calculate value for restitution correction
+	Vec3 gravity_dt = mUpdateContext->mPhysicsSystem->GetGravity() * delta_time;
+
+	// Copy manifolds
+	uint32 output_handle = ManifoldMap::cInvalidHandle;
+	uint32 input_handle = input_cbp.mFirstCachedManifold;
+	do
+	{
+		JPH_PROFILE("Add Constraint From Cached Manifold");
+
+		// Find the existing manifold
+		const MKeyValue *input_kv = read_cache.FromHandle(input_handle);
+		const SubShapeIDPair &input_key = input_kv->GetKey();
+		const CachedManifold &input_cm = input_kv->GetValue();
+		JPH_ASSERT(input_cm.mNumContactPoints > 0); // There should be contact points in this manifold!
+
+		// Create room for manifold in write buffer and copy data
+		uint64 input_hash = input_key.GetHash();
+		MKeyValue *output_kv = write_cache.Create(ioContactAllocator, input_key, input_hash, input_cm.mNumContactPoints);
+		if (output_kv == nullptr)
+			break; // Out of cache space
+		CachedManifold *output_cm = &output_kv->GetValue();
+		memcpy(output_cm, &input_cm, CachedManifold::sGetRequiredTotalSize(input_cm.mNumContactPoints));
+
+		// Link the object under the body pairs
+		output_cm->mNextWithSameBodyPair = output_handle;
+		output_handle = write_cache.ToHandle(output_kv);
+
+		// Calculate default contact settings
+		ContactSettings settings;
+		settings.mCombinedFriction = mCombineFriction(*body1, input_key.GetSubShapeID1(), *body2, input_key.GetSubShapeID2());
+		settings.mCombinedRestitution = mCombineRestitution(*body1, input_key.GetSubShapeID1(), *body2, input_key.GetSubShapeID2());
+		settings.mIsSensor = body1->IsSensor() || body2->IsSensor();
+
+		// Calculate world space contact normal
+		Vec3 world_space_normal = transform_body2.Multiply3x3(Vec3::sLoadFloat3Unsafe(output_cm->mContactNormal)).Normalized();
+
+		// Call contact listener to update settings
+		if (mContactListener != nullptr)
+		{
+			// Convert constraint to manifold structure for callback
+			ContactManifold manifold;
+			manifold.mWorldSpaceNormal = world_space_normal;
+			manifold.mSubShapeID1 = input_key.GetSubShapeID1();
+			manifold.mSubShapeID2 = input_key.GetSubShapeID2();
+			manifold.mBaseOffset = transform_body1.GetTranslation();
+			manifold.mRelativeContactPointsOn1.resize(output_cm->mNumContactPoints);
+			manifold.mRelativeContactPointsOn2.resize(output_cm->mNumContactPoints);
+			Mat44 local_transform_body2 = transform_body2.PostTranslated(-manifold.mBaseOffset).ToMat44();
+			float penetration_depth = -FLT_MAX;
+			for (uint32 i = 0; i < output_cm->mNumContactPoints; ++i)
+			{
+				const CachedContactPoint &ccp = output_cm->mContactPoints[i];
+				manifold.mRelativeContactPointsOn1[i] = transform_body1.Multiply3x3(Vec3::sLoadFloat3Unsafe(ccp.mPosition1));
+				manifold.mRelativeContactPointsOn2[i] = local_transform_body2 * Vec3::sLoadFloat3Unsafe(ccp.mPosition2);
+				penetration_depth = max(penetration_depth, (manifold.mRelativeContactPointsOn1[0] - manifold.mRelativeContactPointsOn2[0]).Dot(world_space_normal));
+			}
+			manifold.mPenetrationDepth = penetration_depth; // We don't have the penetration depth anymore, estimate it
+
+			// Notify callback
+			mContactListener->OnContactPersisted(*body1, *body2, manifold, settings);
+		}
+
+		JPH_ASSERT(settings.mIsSensor || !(body1->IsSensor() || body2->IsSensor()), "Sensors cannot be converted into regular bodies by a contact callback!");
+		if (!settings.mIsSensor // If one of the bodies is a sensor, don't actually create the constraint
+			&& ((body1->IsDynamic() && settings.mInvMassScale1 != 0.0f) // One of the bodies must have mass to be able to create a contact constraint
+				|| (body2->IsDynamic() && settings.mInvMassScale2 != 0.0f)))
+		{
+			// Add contact constraint in world space for the solver
+			uint32 constraint_idx = mNumConstraints++;
+			if (constraint_idx >= mMaxConstraints)
+			{
+				ioContactAllocator.mErrors |= EPhysicsUpdateError::ContactConstraintsFull;
+				break;
+			}
+
+			// A constraint will be created
+			outConstraintCreated = true;
+
+			ContactConstraint &constraint = mConstraints[constraint_idx];
+			new (&constraint) ContactConstraint();
+			constraint.mBody1 = body1;
+			constraint.mBody2 = body2;
+			constraint.mSortKey = input_hash;
+			world_space_normal.StoreFloat3(&constraint.mWorldSpaceNormal);
+			constraint.mCombinedFriction = settings.mCombinedFriction;
+			constraint.mInvMass1 = body1->GetMotionPropertiesUnchecked() != nullptr? settings.mInvMassScale1 * body1->GetMotionPropertiesUnchecked()->GetInverseMassUnchecked() : 0.0f;
+			constraint.mInvInertiaScale1 = settings.mInvInertiaScale1;
+			constraint.mInvMass2 = body2->GetMotionPropertiesUnchecked() != nullptr? settings.mInvMassScale2 * body2->GetMotionPropertiesUnchecked()->GetInverseMassUnchecked() : 0.0f;
+			constraint.mInvInertiaScale2 = settings.mInvInertiaScale2;
+			constraint.mContactPoints.resize(output_cm->mNumContactPoints);
+			for (uint32 i = 0; i < output_cm->mNumContactPoints; ++i)
+			{
+				CachedContactPoint &ccp = output_cm->mContactPoints[i];
+				WorldContactPoint &wcp = constraint.mContactPoints[i];
+				wcp.mNonPenetrationConstraint.SetTotalLambda(ccp.mNonPenetrationLambda);
+				wcp.mFrictionConstraint1.SetTotalLambda(ccp.mFrictionLambda[0]);
+				wcp.mFrictionConstraint2.SetTotalLambda(ccp.mFrictionLambda[1]);
+				wcp.mContactPoint = &ccp;
+			}
+
+			JPH_DET_LOG("GetContactsFromCache: id1: " << constraint.mBody1->GetID() << " id2: " << constraint.mBody2->GetID() << " key: " << constraint.mSortKey);
+
+			// Calculate friction and non-penetration constraint properties for all contact points
+			CalculateFrictionAndNonPenetrationConstraintProperties(constraint, settings, delta_time, gravity_dt, transform_body1, transform_body2, *body1, *body2);
+
+			// Notify island builder
+			mUpdateContext->mIslandBuilder->LinkContact(constraint_idx, body1->GetIndexInActiveBodiesInternal(), body2->GetIndexInActiveBodiesInternal());
+
+		#ifdef JPH_DEBUG_RENDERER
+			// Draw the manifold
+			if (sDrawContactManifolds)
+				constraint.Draw(DebugRenderer::sInstance, Color::sYellow);
+		#endif // JPH_DEBUG_RENDERER
+		}
+
+		// Mark contact as persisted so that we won't fire OnContactRemoved callbacks
+		input_cm.mFlags |= (uint16)CachedManifold::EFlags::ContactPersisted;
+
+		// Fetch the next manifold
+		input_handle = input_cm.mNextWithSameBodyPair;
+	}
+	while (input_handle != ManifoldMap::cInvalidHandle);
+	output_cbp->mFirstCachedManifold = output_handle;
+}
+
+ContactConstraintManager::BodyPairHandle ContactConstraintManager::AddBodyPair(ContactAllocator &ioContactAllocator, const Body &inBody1, const Body &inBody2)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Swap bodies so that body 1 id < body 2 id
+	const Body *body1, *body2;
+	if (inBody1.GetID() < inBody2.GetID())
+	{
+		body1 = &inBody1;
+		body2 = &inBody2;
+	}
+	else
+	{
+		body1 = &inBody2;
+		body2 = &inBody1;
+	}
+
+	// Add an entry
+	BodyPair body_pair_key(body1->GetID(), body2->GetID());
+	uint64 body_pair_hash = body_pair_key.GetHash();
+	BPKeyValue *body_pair_kv = mCache[mCacheWriteIdx].Create(ioContactAllocator, body_pair_key, body_pair_hash);
+	if (body_pair_kv == nullptr)
+		return nullptr; // Out of cache space
+	CachedBodyPair *cbp = &body_pair_kv->GetValue();
+	cbp->mFirstCachedManifold = ManifoldMap::cInvalidHandle;
+
+	// Get relative translation
+	Quat inv_r1 = body1->GetRotation().Conjugated();
+	Vec3 delta_position = inv_r1 * Vec3(body2->GetCenterOfMassPosition() - body1->GetCenterOfMassPosition());
+
+	// Store it
+	delta_position.StoreFloat3(&cbp->mDeltaPosition);
+
+	// Determine relative orientation
+	Quat delta_rotation = inv_r1 * body2->GetRotation();
+
+	// Store it
+	delta_rotation.StoreFloat3(&cbp->mDeltaRotation);
+
+	return cbp;
+}
+
+template <EMotionType Type1, EMotionType Type2>
+bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPairHandle, Body &inBody1, Body &inBody2, const ContactManifold &inManifold)
+{
+	// Calculate hash
+	SubShapeIDPair key { inBody1.GetID(), inManifold.mSubShapeID1, inBody2.GetID(), inManifold.mSubShapeID2 };
+	uint64 key_hash = key.GetHash();
+
+	// Determine number of contact points
+	int num_contact_points = (int)inManifold.mRelativeContactPointsOn1.size();
+	JPH_ASSERT(num_contact_points <= MaxContactPoints);
+	JPH_ASSERT(num_contact_points == (int)inManifold.mRelativeContactPointsOn2.size());
+
+	// Reserve space for new contact cache entry
+	// Note that for dynamic vs dynamic we always require the first body to have a lower body id to get a consistent key
+	// under which to look up the contact
+	ManifoldCache &write_cache = mCache[mCacheWriteIdx];
+	MKeyValue *new_manifold_kv = write_cache.Create(ioContactAllocator, key, key_hash, num_contact_points);
+	if (new_manifold_kv == nullptr)
+		return false; // Out of cache space
+	CachedManifold *new_manifold = &new_manifold_kv->GetValue();
+
+	// Transform the world space normal to the space of body 2 (this is usually the static body)
+	RMat44 inverse_transform_body2 = inBody2.GetInverseCenterOfMassTransform();
+	inverse_transform_body2.Multiply3x3(inManifold.mWorldSpaceNormal).Normalized().StoreFloat3(&new_manifold->mContactNormal);
+
+	// Settings object that gets passed to the callback
+	ContactSettings settings;
+	settings.mCombinedFriction = mCombineFriction(inBody1, inManifold.mSubShapeID1, inBody2, inManifold.mSubShapeID2);
+	settings.mCombinedRestitution = mCombineRestitution(inBody1, inManifold.mSubShapeID1, inBody2, inManifold.mSubShapeID2);
+	settings.mIsSensor = inBody1.IsSensor() || inBody2.IsSensor();
+
+	// Get the contact points for the old cache entry
+	const ManifoldCache &read_cache = mCache[mCacheWriteIdx ^ 1];
+	const MKeyValue *old_manifold_kv = read_cache.Find(key, key_hash);
+	const CachedContactPoint *ccp_start;
+	const CachedContactPoint *ccp_end;
+	if (old_manifold_kv != nullptr)
+	{
+		// Call point persisted listener
+		if (mContactListener != nullptr)
+			mContactListener->OnContactPersisted(inBody1, inBody2, inManifold, settings);
+
+		// Fetch the contact points from the old manifold
+		const CachedManifold *old_manifold = &old_manifold_kv->GetValue();
+		ccp_start = old_manifold->mContactPoints;
+		ccp_end = ccp_start + old_manifold->mNumContactPoints;
+
+		// Mark contact as persisted so that we won't fire OnContactRemoved callbacks
+		old_manifold->mFlags |= (uint16)CachedManifold::EFlags::ContactPersisted;
+	}
+	else
+	{
+		// Call point added listener
+		if (mContactListener != nullptr)
+			mContactListener->OnContactAdded(inBody1, inBody2, inManifold, settings);
+
+		// No contact points available from old manifold
+		ccp_start = nullptr;
+		ccp_end = nullptr;
+	}
+
+	// Get inverse transform for body 1
+	RMat44 inverse_transform_body1 = inBody1.GetInverseCenterOfMassTransform();
+
+	bool contact_constraint_created = false;
+
+	// If one of the bodies is a sensor, don't actually create the constraint
+	JPH_ASSERT(settings.mIsSensor || !(inBody1.IsSensor() || inBody2.IsSensor()), "Sensors cannot be converted into regular bodies by a contact callback!");
+	if (!settings.mIsSensor
+		&& ((inBody1.IsDynamic() && settings.mInvMassScale1 != 0.0f) // One of the bodies must have mass to be able to create a contact constraint
+			|| (inBody2.IsDynamic() && settings.mInvMassScale2 != 0.0f)))
+	{
+		// Add contact constraint
+		uint32 constraint_idx = mNumConstraints++;
+		if (constraint_idx >= mMaxConstraints)
+		{
+			ioContactAllocator.mErrors |= EPhysicsUpdateError::ContactConstraintsFull;
+
+			// Manifold has been created already, we're not filling it in, so we need to reset the contact number of points.
+			// Note that we don't hook it up to the body pair cache so that it won't be used as a cache during the next simulation.
+			new_manifold->mNumContactPoints = 0;
+			return false;
+		}
+
+		// We will create a contact constraint
+		contact_constraint_created = true;
+
+		ContactConstraint &constraint = mConstraints[constraint_idx];
+		new (&constraint) ContactConstraint();
+		constraint.mBody1 = &inBody1;
+		constraint.mBody2 = &inBody2;
+		constraint.mSortKey = key_hash;
+		inManifold.mWorldSpaceNormal.StoreFloat3(&constraint.mWorldSpaceNormal);
+		constraint.mCombinedFriction = settings.mCombinedFriction;
+		constraint.mInvMass1 = inBody1.GetMotionPropertiesUnchecked() != nullptr? settings.mInvMassScale1 * inBody1.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked() : 0.0f;
+		constraint.mInvInertiaScale1 = settings.mInvInertiaScale1;
+		constraint.mInvMass2 = inBody2.GetMotionPropertiesUnchecked() != nullptr? settings.mInvMassScale2 * inBody2.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked() : 0.0f;
+		constraint.mInvInertiaScale2 = settings.mInvInertiaScale2;
+
+		JPH_DET_LOG("TemplatedAddContactConstraint: id1: " << constraint.mBody1->GetID() << " id2: " << constraint.mBody2->GetID() << " key: " << constraint.mSortKey);
+
+		// Notify island builder
+		mUpdateContext->mIslandBuilder->LinkContact(constraint_idx, inBody1.GetIndexInActiveBodiesInternal(), inBody2.GetIndexInActiveBodiesInternal());
+
+		// Get time step
+		float delta_time = mUpdateContext->mStepDeltaTime;
+
+		// Calculate value for restitution correction
+		float gravity_dt_dot_normal = inManifold.mWorldSpaceNormal.Dot(mUpdateContext->mPhysicsSystem->GetGravity() * delta_time);
+
+		// Calculate scaled mass and inertia
+		float inv_m1;
+		Mat44 inv_i1;
+		if constexpr (Type1 == EMotionType::Dynamic)
+		{
+			const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked();
+			inv_m1 = settings.mInvMassScale1 * mp1->GetInverseMass();
+			inv_i1 = settings.mInvInertiaScale1 * mp1->GetInverseInertiaForRotation(inverse_transform_body1.Transposed3x3());
+		}
+		else
+		{
+			inv_m1 = 0.0f;
+			inv_i1 = Mat44::sZero();
+		}
+
+		float inv_m2;
+		Mat44 inv_i2;
+		if constexpr (Type2 == EMotionType::Dynamic)
+		{
+			const MotionProperties *mp2 = inBody2.GetMotionPropertiesUnchecked();
+			inv_m2 = settings.mInvMassScale2 * mp2->GetInverseMass();
+			inv_i2 = settings.mInvInertiaScale2 * mp2->GetInverseInertiaForRotation(inverse_transform_body2.Transposed3x3());
+		}
+		else
+		{
+			inv_m2 = 0.0f;
+			inv_i2 = Mat44::sZero();
+		}
+
+		// Calculate tangents
+		Vec3 t1, t2;
+		constraint.GetTangents(t1, t2);
+
+		constraint.mContactPoints.resize(num_contact_points);
+		for (int i = 0; i < num_contact_points; ++i)
+		{
+			// Convert to world space and set positions
+			WorldContactPoint &wcp = constraint.mContactPoints[i];
+			RVec3 p1_ws = inManifold.mBaseOffset + inManifold.mRelativeContactPointsOn1[i];
+			RVec3 p2_ws = inManifold.mBaseOffset + inManifold.mRelativeContactPointsOn2[i];
+
+			// Convert to local space to the body
+			Vec3 p1_ls = Vec3(inverse_transform_body1 * p1_ws);
+			Vec3 p2_ls = Vec3(inverse_transform_body2 * p2_ws);
+
+			// Check if we have a close contact point from last update
+			bool lambda_set = false;
+			for (const CachedContactPoint *ccp = ccp_start; ccp < ccp_end; ccp++)
+				if (Vec3::sLoadFloat3Unsafe(ccp->mPosition1).IsClose(p1_ls, mPhysicsSettings.mContactPointPreserveLambdaMaxDistSq)
+					&& Vec3::sLoadFloat3Unsafe(ccp->mPosition2).IsClose(p2_ls, mPhysicsSettings.mContactPointPreserveLambdaMaxDistSq))
+				{
+					// Get lambdas from previous frame
+					wcp.mNonPenetrationConstraint.SetTotalLambda(ccp->mNonPenetrationLambda);
+					wcp.mFrictionConstraint1.SetTotalLambda(ccp->mFrictionLambda[0]);
+					wcp.mFrictionConstraint2.SetTotalLambda(ccp->mFrictionLambda[1]);
+					lambda_set = true;
+					break;
+				}
+			if (!lambda_set)
+			{
+				wcp.mNonPenetrationConstraint.SetTotalLambda(0.0f);
+				wcp.mFrictionConstraint1.SetTotalLambda(0.0f);
+				wcp.mFrictionConstraint2.SetTotalLambda(0.0f);
+			}
+
+			// Create new contact point
+			CachedContactPoint &cp = new_manifold->mContactPoints[i];
+			p1_ls.StoreFloat3(&cp.mPosition1);
+			p2_ls.StoreFloat3(&cp.mPosition2);
+			wcp.mContactPoint = &cp;
+
+			// Setup velocity constraint
+			wcp.TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<Type1, Type2>(delta_time, gravity_dt_dot_normal, inBody1, inBody2, inv_m1, inv_m2, inv_i1, inv_i2, p1_ws, p2_ws, inManifold.mWorldSpaceNormal, t1, t2, settings, mPhysicsSettings.mMinVelocityForRestitution);
+		}
+
+	#ifdef JPH_DEBUG_RENDERER
+		// Draw the manifold
+		if (sDrawContactManifolds)
+			constraint.Draw(DebugRenderer::sInstance, Color::sOrange);
+	#endif // JPH_DEBUG_RENDERER
+	}
+	else
+	{
+		// Store the contact manifold in the cache
+		for (int i = 0; i < num_contact_points; ++i)
+		{
+			// Convert to local space to the body
+			Vec3 p1 = Vec3(inverse_transform_body1 * (inManifold.mBaseOffset + inManifold.mRelativeContactPointsOn1[i]));
+			Vec3 p2 = Vec3(inverse_transform_body2 * (inManifold.mBaseOffset + inManifold.mRelativeContactPointsOn2[i]));
+
+			// Create new contact point
+			CachedContactPoint &cp = new_manifold->mContactPoints[i];
+			p1.StoreFloat3(&cp.mPosition1);
+			p2.StoreFloat3(&cp.mPosition2);
+
+			// Reset contact impulses, we haven't applied any
+			cp.mNonPenetrationLambda = 0.0f;
+			cp.mFrictionLambda[0] = 0.0f;
+			cp.mFrictionLambda[1] = 0.0f;
+		}
+	}
+
+	// Store cached contact point in body pair cache
+	CachedBodyPair *cbp = reinterpret_cast<CachedBodyPair *>(inBodyPairHandle);
+	new_manifold->mNextWithSameBodyPair = cbp->mFirstCachedManifold;
+	cbp->mFirstCachedManifold = write_cache.ToHandle(new_manifold_kv);
+
+	// A contact constraint was added
+	return contact_constraint_created;
+}
+
+bool ContactConstraintManager::AddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPairHandle, Body &inBody1, Body &inBody2, const ContactManifold &inManifold)
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_DET_LOG("AddContactConstraint: id1: " << inBody1.GetID() << " id2: " << inBody2.GetID()
+		<< " subshape1: " << inManifold.mSubShapeID1 << " subshape2: " << inManifold.mSubShapeID2
+		<< " normal: " << inManifold.mWorldSpaceNormal << " pendepth: " << inManifold.mPenetrationDepth);
+
+	JPH_ASSERT(inManifold.mWorldSpaceNormal.IsNormalized());
+
+	// Swap bodies so that body 1 id < body 2 id
+	const ContactManifold *manifold;
+	Body *body1, *body2;
+	ContactManifold temp;
+	if (inBody2.GetID() < inBody1.GetID())
+	{
+		body1 = &inBody2;
+		body2 = &inBody1;
+		temp = inManifold.SwapShapes();
+		manifold = &temp;
+	}
+	else
+	{
+		body1 = &inBody1;
+		body2 = &inBody2;
+		manifold = &inManifold;
+	}
+
+	// Dispatch to the correct templated form
+	// Note: Non-dynamic vs non-dynamic can happen in this case due to one body being a sensor, so we need to have an extended switch case here
+	switch (body1->GetMotionType())
+	{
+	case EMotionType::Dynamic:
+		{
+			switch (body2->GetMotionType())
+			{
+			case EMotionType::Dynamic:
+				return TemplatedAddContactConstraint<EMotionType::Dynamic, EMotionType::Dynamic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold);
+
+			case EMotionType::Kinematic:
+				return TemplatedAddContactConstraint<EMotionType::Dynamic, EMotionType::Kinematic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold);
+
+			case EMotionType::Static:
+				return TemplatedAddContactConstraint<EMotionType::Dynamic, EMotionType::Static>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold);
+
+			default:
+				JPH_ASSERT(false);
+				break;
+			}
+			break;
+		}
+
+	case EMotionType::Kinematic:
+		switch (body2->GetMotionType())
+		{
+		case EMotionType::Dynamic:
+			return TemplatedAddContactConstraint<EMotionType::Kinematic, EMotionType::Dynamic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold);
+
+		case EMotionType::Kinematic:
+			return TemplatedAddContactConstraint<EMotionType::Kinematic, EMotionType::Kinematic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold);
+
+		case EMotionType::Static:
+			return TemplatedAddContactConstraint<EMotionType::Kinematic, EMotionType::Static>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold);
+
+		default:
+			JPH_ASSERT(false);
+			break;
+		}
+		break;
+
+	case EMotionType::Static:
+		switch (body2->GetMotionType())
+		{
+		case EMotionType::Dynamic:
+			return TemplatedAddContactConstraint<EMotionType::Static, EMotionType::Dynamic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold);
+
+		case EMotionType::Kinematic:
+			return TemplatedAddContactConstraint<EMotionType::Static, EMotionType::Kinematic>(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold);
+
+		case EMotionType::Static: // Static vs static not possible
+		default:
+			JPH_ASSERT(false);
+			break;
+		}
+		break;
+
+	default:
+		JPH_ASSERT(false);
+		break;
+	}
+
+	return false;
+}
+
+void ContactConstraintManager::OnCCDContactAdded(ContactAllocator &ioContactAllocator, const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &outSettings)
+{
+	JPH_ASSERT(inManifold.mWorldSpaceNormal.IsNormalized());
+
+	// Calculate contact settings
+	outSettings.mCombinedFriction = mCombineFriction(inBody1, inManifold.mSubShapeID1, inBody2, inManifold.mSubShapeID2);
+	outSettings.mCombinedRestitution = mCombineRestitution(inBody1, inManifold.mSubShapeID1, inBody2, inManifold.mSubShapeID2);
+	outSettings.mIsSensor = false; // For now, no sensors are supported during CCD
+
+	// The remainder of this function only deals with calling contact callbacks, if there's no contact callback we also don't need to do this work
+	if (mContactListener != nullptr)
+	{
+		// Swap bodies so that body 1 id < body 2 id
+		const ContactManifold *manifold;
+		const Body *body1, *body2;
+		ContactManifold temp;
+		if (inBody2.GetID() < inBody1.GetID())
+		{
+			body1 = &inBody2;
+			body2 = &inBody1;
+			temp = inManifold.SwapShapes();
+			manifold = &temp;
+		}
+		else
+		{
+			body1 = &inBody1;
+			body2 = &inBody2;
+			manifold = &inManifold;
+		}
+
+		// Calculate hash
+		SubShapeIDPair key { body1->GetID(), manifold->mSubShapeID1, body2->GetID(), manifold->mSubShapeID2 };
+		uint64 key_hash = key.GetHash();
+
+		// Check if we already created this contact this physics update
+		ManifoldCache &write_cache = mCache[mCacheWriteIdx];
+		MKVAndCreated new_manifold_kv = write_cache.FindOrCreate(ioContactAllocator, key, key_hash, 0);
+		if (new_manifold_kv.second)
+		{
+			// This contact is new for this physics update, check if previous update we already had this contact.
+			const ManifoldCache &read_cache = mCache[mCacheWriteIdx ^ 1];
+			const MKeyValue *old_manifold_kv = read_cache.Find(key, key_hash);
+			if (old_manifold_kv == nullptr)
+			{
+				// New contact
+				mContactListener->OnContactAdded(*body1, *body2, *manifold, outSettings);
+			}
+			else
+			{
+				// Existing contact
+				mContactListener->OnContactPersisted(*body1, *body2, *manifold, outSettings);
+
+				// Mark contact as persisted so that we won't fire OnContactRemoved callbacks
+				old_manifold_kv->GetValue().mFlags |= (uint16)CachedManifold::EFlags::ContactPersisted;
+			}
+
+			// Check if the cache is full
+			if (new_manifold_kv.first != nullptr)
+			{
+				// We don't store any contact points in this manifold as it is not for caching impulses, we only need to know that the contact was created
+				CachedManifold &new_manifold = new_manifold_kv.first->GetValue();
+				new_manifold.mContactNormal = { 0, 0, 0 };
+				new_manifold.mFlags |= (uint16)CachedManifold::EFlags::CCDContact;
+			}
+		}
+		else
+		{
+			// Already found this contact this physics update.
+			// Note that we can trigger OnContactPersisted multiple times per physics update, but otherwise we have no way of obtaining the settings
+			mContactListener->OnContactPersisted(*body1, *body2, *manifold, outSettings);
+		}
+
+		// If we swapped body1 and body2 we need to swap the mass scales back
+		if (manifold == &temp)
+		{
+			std::swap(outSettings.mInvMassScale1, outSettings.mInvMassScale2);
+			std::swap(outSettings.mInvInertiaScale1, outSettings.mInvInertiaScale2);
+			// Note we do not need to negate the relative surface velocity as it is not applied by the CCD collision constraint
+		}
+	}
+
+	JPH_ASSERT(outSettings.mIsSensor || !(inBody1.IsSensor() || inBody2.IsSensor()), "Sensors cannot be converted into regular bodies by a contact callback!");
+}
+
+void ContactConstraintManager::SortContacts(uint32 *inConstraintIdxBegin, uint32 *inConstraintIdxEnd) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	QuickSort(inConstraintIdxBegin, inConstraintIdxEnd, [this](uint32 inLHS, uint32 inRHS) {
+		const ContactConstraint &lhs = mConstraints[inLHS];
+		const ContactConstraint &rhs = mConstraints[inRHS];
+
+		// Most of the time the sort key will be different so we sort on that
+		if (lhs.mSortKey != rhs.mSortKey)
+			return lhs.mSortKey < rhs.mSortKey;
+
+		// If they're equal we use the IDs of body 1 to order
+		if (lhs.mBody1 != rhs.mBody1)
+			return lhs.mBody1->GetID() < rhs.mBody1->GetID();
+
+		// If they're still equal we use the IDs of body 2 to order
+		if (lhs.mBody2 != rhs.mBody2)
+			return lhs.mBody2->GetID() < rhs.mBody2->GetID();
+
+		JPH_ASSERT(inLHS == inRHS, "Hash collision, ordering will be inconsistent");
+		return false;
+	});
+}
+
+void ContactConstraintManager::FinalizeContactCacheAndCallContactPointRemovedCallbacks(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds)
+{
+	JPH_PROFILE_FUNCTION();
+
+#ifdef JPH_ENABLE_ASSERTS
+	// Mark cache as finalized
+	ManifoldCache &old_write_cache = mCache[mCacheWriteIdx];
+	old_write_cache.Finalize();
+
+	// Check that the count of body pairs and manifolds that we tracked outside of the cache (to avoid contention on an atomic) is correct
+	JPH_ASSERT(old_write_cache.GetNumBodyPairs() == inExpectedNumBodyPairs);
+	JPH_ASSERT(old_write_cache.GetNumManifolds() == inExpectedNumManifolds);
+#endif
+
+	// Buffers are now complete, make write buffer the read buffer
+	mCacheWriteIdx ^= 1;
+
+	// Get the old read cache / new write cache
+	ManifoldCache &old_read_cache = mCache[mCacheWriteIdx];
+
+	// Call the contact point removal callbacks
+	if (mContactListener != nullptr)
+		old_read_cache.ContactPointRemovedCallbacks(mContactListener);
+
+	// We're done with the old read cache now
+	old_read_cache.Clear();
+
+	// Use the amount of contacts from the last iteration to determine the amount of buckets to use in the hash map for the next iteration
+	old_read_cache.Prepare(inExpectedNumBodyPairs, inExpectedNumManifolds);
+}
+
+bool ContactConstraintManager::WereBodiesInContact(const BodyID &inBody1ID, const BodyID &inBody2ID) const
+{
+	// The body pair needs to be in the cache and it needs to have a manifold (otherwise it's just a record indicating that there are no collisions)
+	const ManifoldCache &read_cache = mCache[mCacheWriteIdx ^ 1];
+	BodyPair key;
+	if (inBody1ID < inBody2ID)
+		key = BodyPair(inBody1ID, inBody2ID);
+	else
+		key = BodyPair(inBody2ID, inBody1ID);
+	uint64 key_hash = key.GetHash();
+	const BPKeyValue *kv = read_cache.Find(key, key_hash);
+	return kv != nullptr && kv->GetValue().mFirstCachedManifold != ManifoldMap::cInvalidHandle;
+}
+
+template <EMotionType Type1, EMotionType Type2>
+JPH_INLINE void ContactConstraintManager::sWarmStartConstraint(ContactConstraint &ioConstraint, MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2, float inWarmStartImpulseRatio)
+{
+	// Calculate tangents
+	Vec3 t1, t2;
+	ioConstraint.GetTangents(t1, t2);
+
+	Vec3 ws_normal = ioConstraint.GetWorldSpaceNormal();
+
+	for (WorldContactPoint &wcp : ioConstraint.mContactPoints)
+	{
+		// Warm starting: Apply impulse from last frame
+		if (wcp.mFrictionConstraint1.IsActive() || wcp.mFrictionConstraint2.IsActive())
+		{
+			wcp.mFrictionConstraint1.TemplatedWarmStart<Type1, Type2>(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, t1, inWarmStartImpulseRatio);
+			wcp.mFrictionConstraint2.TemplatedWarmStart<Type1, Type2>(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, t2, inWarmStartImpulseRatio);
+		}
+		wcp.mNonPenetrationConstraint.TemplatedWarmStart<Type1, Type2>(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, ws_normal, inWarmStartImpulseRatio);
+	}
+}
+
+template <class MotionPropertiesCallback>
+void ContactConstraintManager::WarmStartVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, MotionPropertiesCallback &ioCallback)
+{
+	JPH_PROFILE_FUNCTION();
+
+	for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx)
+	{
+		ContactConstraint &constraint = mConstraints[*constraint_idx];
+
+		// Fetch bodies
+		Body &body1 = *constraint.mBody1;
+		EMotionType motion_type1 = body1.GetMotionType();
+		MotionProperties *motion_properties1 = body1.GetMotionPropertiesUnchecked();
+
+		Body &body2 = *constraint.mBody2;
+		EMotionType motion_type2 = body2.GetMotionType();
+		MotionProperties *motion_properties2 = body2.GetMotionPropertiesUnchecked();
+
+		// Dispatch to the correct templated form
+		// Note: Warm starting doesn't differentiate between kinematic/static bodies so we handle both as static bodies
+		if (motion_type1 == EMotionType::Dynamic)
+		{
+			if (motion_type2 == EMotionType::Dynamic)
+			{
+				sWarmStartConstraint<EMotionType::Dynamic, EMotionType::Dynamic>(constraint, motion_properties1, motion_properties2, inWarmStartImpulseRatio);
+
+				ioCallback(motion_properties2);
+			}
+			else
+				sWarmStartConstraint<EMotionType::Dynamic, EMotionType::Static>(constraint, motion_properties1, motion_properties2, inWarmStartImpulseRatio);
+
+			ioCallback(motion_properties1);
+		}
+		else
+		{
+			JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
+
+			sWarmStartConstraint<EMotionType::Static, EMotionType::Dynamic>(constraint, motion_properties1, motion_properties2, inWarmStartImpulseRatio);
+
+			ioCallback(motion_properties2);
+		}
+	}
+}
+
+// Specialize for the two body callback types
+template void ContactConstraintManager::WarmStartVelocityConstraints<CalculateSolverSteps>(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, CalculateSolverSteps &ioCallback);
+template void ContactConstraintManager::WarmStartVelocityConstraints<DummyCalculateSolverSteps>(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, DummyCalculateSolverSteps &ioCallback);
+
+template <EMotionType Type1, EMotionType Type2>
+JPH_INLINE bool ContactConstraintManager::sSolveVelocityConstraint(ContactConstraint &ioConstraint, MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2)
+{
+	bool any_impulse_applied = false;
+
+	// Calculate tangents
+	Vec3 t1, t2;
+	ioConstraint.GetTangents(t1, t2);
+
+	// First apply all friction constraints (non-penetration is more important than friction)
+	for (WorldContactPoint &wcp : ioConstraint.mContactPoints)
+	{
+		// Check if friction is enabled
+		if (wcp.mFrictionConstraint1.IsActive() || wcp.mFrictionConstraint2.IsActive())
+		{
+			// Calculate impulse to stop motion in tangential direction
+			float lambda1 = wcp.mFrictionConstraint1.TemplatedSolveVelocityConstraintGetTotalLambda<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, t1);
+			float lambda2 = wcp.mFrictionConstraint2.TemplatedSolveVelocityConstraintGetTotalLambda<Type1, Type2>(ioMotionProperties1, ioMotionProperties2, t2);
+			float total_lambda_sq = Square(lambda1) + Square(lambda2);
+
+			// Calculate max impulse that can be applied. Note that we're using the non-penetration impulse from the previous iteration here.
+			// We do this because non-penetration is more important so is solved last (the last things that are solved in an iterative solver
+			// contribute the most).
+			float max_lambda_f = ioConstraint.mCombinedFriction * wcp.mNonPenetrationConstraint.GetTotalLambda();
+
+			// If the total lambda that we will apply is too large, scale it back
+			if (total_lambda_sq > Square(max_lambda_f))
+			{
+				float scale = max_lambda_f / sqrt(total_lambda_sq);
+				lambda1 *= scale;
+				lambda2 *= scale;
+			}
+
+			// Apply the friction impulse
+			if (wcp.mFrictionConstraint1.TemplatedSolveVelocityConstraintApplyLambda<Type1, Type2>(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, t1, lambda1))
+				any_impulse_applied = true;
+			if (wcp.mFrictionConstraint2.TemplatedSolveVelocityConstraintApplyLambda<Type1, Type2>(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, t2, lambda2))
+				any_impulse_applied = true;
+		}
+	}
+
+	Vec3 ws_normal = ioConstraint.GetWorldSpaceNormal();
+
+	// Then apply all non-penetration constraints
+	for (WorldContactPoint &wcp : ioConstraint.mContactPoints)
+	{
+		// Solve non penetration velocities
+		if (wcp.mNonPenetrationConstraint.TemplatedSolveVelocityConstraint<Type1, Type2>(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, ws_normal, 0.0f, FLT_MAX))
+			any_impulse_applied = true;
+	}
+
+	return any_impulse_applied;
+}
+
+bool ContactConstraintManager::SolveVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd)
+{
+	JPH_PROFILE_FUNCTION();
+
+	bool any_impulse_applied = false;
+
+	for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx)
+	{
+		ContactConstraint &constraint = mConstraints[*constraint_idx];
+
+		// Fetch bodies
+		Body &body1 = *constraint.mBody1;
+		EMotionType motion_type1 = body1.GetMotionType();
+		MotionProperties *motion_properties1 = body1.GetMotionPropertiesUnchecked();
+
+		Body &body2 = *constraint.mBody2;
+		EMotionType motion_type2 = body2.GetMotionType();
+		MotionProperties *motion_properties2 = body2.GetMotionPropertiesUnchecked();
+
+		// Dispatch to the correct templated form
+		switch (motion_type1)
+		{
+		case EMotionType::Dynamic:
+			switch (motion_type2)
+			{
+			case EMotionType::Dynamic:
+				any_impulse_applied |= sSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Dynamic>(constraint, motion_properties1, motion_properties2);
+				break;
+
+			case EMotionType::Kinematic:
+				any_impulse_applied |= sSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Kinematic>(constraint, motion_properties1, motion_properties2);
+				break;
+
+			case EMotionType::Static:
+				any_impulse_applied |= sSolveVelocityConstraint<EMotionType::Dynamic, EMotionType::Static>(constraint, motion_properties1, motion_properties2);
+				break;
+
+			default:
+				JPH_ASSERT(false);
+				break;
+			}
+			break;
+
+		case EMotionType::Kinematic:
+			JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
+			any_impulse_applied |= sSolveVelocityConstraint<EMotionType::Kinematic, EMotionType::Dynamic>(constraint, motion_properties1, motion_properties2);
+			break;
+
+		case EMotionType::Static:
+			JPH_ASSERT(motion_type2 == EMotionType::Dynamic);
+			any_impulse_applied |= sSolveVelocityConstraint<EMotionType::Static, EMotionType::Dynamic>(constraint, motion_properties1, motion_properties2);
+			break;
+
+		default:
+			JPH_ASSERT(false);
+			break;
+		}
+	}
+
+	return any_impulse_applied;
+}
+
+void ContactConstraintManager::StoreAppliedImpulses(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd) const
+{
+	// Copy back total applied impulse to cache for the next frame
+	for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx)
+	{
+		const ContactConstraint &constraint = mConstraints[*constraint_idx];
+
+		for (const WorldContactPoint &wcp : constraint.mContactPoints)
+		{
+			wcp.mContactPoint->mNonPenetrationLambda = wcp.mNonPenetrationConstraint.GetTotalLambda();
+			wcp.mContactPoint->mFrictionLambda[0] = wcp.mFrictionConstraint1.GetTotalLambda();
+			wcp.mContactPoint->mFrictionLambda[1] = wcp.mFrictionConstraint2.GetTotalLambda();
+		}
+	}
+}
+
+bool ContactConstraintManager::SolvePositionConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd)
+{
+	JPH_PROFILE_FUNCTION();
+
+	bool any_impulse_applied = false;
+
+	for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx)
+	{
+		ContactConstraint &constraint = mConstraints[*constraint_idx];
+
+		// Fetch bodies
+		Body &body1 = *constraint.mBody1;
+		Body &body2 = *constraint.mBody2;
+
+		// Get transforms
+		RMat44 transform1 = body1.GetCenterOfMassTransform();
+		RMat44 transform2 = body2.GetCenterOfMassTransform();
+
+		Vec3 ws_normal = constraint.GetWorldSpaceNormal();
+
+		for (WorldContactPoint &wcp : constraint.mContactPoints)
+		{
+			// Calculate new contact point positions in world space (the bodies may have moved)
+			RVec3 p1 = transform1 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition1);
+			RVec3 p2 = transform2 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition2);
+
+			// Calculate separation along the normal (negative if interpenetrating)
+			// Allow a little penetration by default (PhysicsSettings::mPenetrationSlop) to avoid jittering between contact/no-contact which wipes out the contact cache and warm start impulses
+			// Clamp penetration to a max PhysicsSettings::mMaxPenetrationDistance so that we don't apply a huge impulse if we're penetrating a lot
+			float separation = max(Vec3(p2 - p1).Dot(ws_normal) + mPhysicsSettings.mPenetrationSlop, -mPhysicsSettings.mMaxPenetrationDistance);
+
+			// Only enforce constraint when separation < 0 (otherwise we're apart)
+			if (separation < 0.0f)
+			{
+				// Update constraint properties (bodies may have moved)
+				wcp.CalculateNonPenetrationConstraintProperties(body1, constraint.mInvMass1, constraint.mInvInertiaScale1, body2, constraint.mInvMass2, constraint.mInvInertiaScale2, p1, p2, ws_normal);
+
+				// Solve position errors
+				if (wcp.mNonPenetrationConstraint.SolvePositionConstraintWithMassOverride(body1, constraint.mInvMass1, body2, constraint.mInvMass2, ws_normal, separation, mPhysicsSettings.mBaumgarte))
+					any_impulse_applied = true;
+			}
+		}
+	}
+
+	return any_impulse_applied;
+}
+
+void ContactConstraintManager::RecycleConstraintBuffer()
+{
+	// Reset constraint array
+	mNumConstraints = 0;
+}
+
+void ContactConstraintManager::FinishConstraintBuffer()
+{
+	// Free constraints buffer
+	mUpdateContext->mTempAllocator->Free(mConstraints, mMaxConstraints * sizeof(ContactConstraint));
+	mConstraints = nullptr;
+	mNumConstraints = 0;
+
+	// Reset update context
+	mUpdateContext = nullptr;
+}
+
+void ContactConstraintManager::SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const
+{
+	mCache[mCacheWriteIdx ^ 1].SaveState(inStream, inFilter);
+}
+
+bool ContactConstraintManager::RestoreState(StateRecorder &inStream, const StateRecorderFilter *inFilter)
+{
+	bool success = mCache[mCacheWriteIdx].RestoreState(mCache[mCacheWriteIdx ^ 1], inStream, inFilter);
+
+	// If this is the last part, the cache is finalized
+	if (inStream.IsLastPart())
+	{
+		mCacheWriteIdx ^= 1;
+		mCache[mCacheWriteIdx].Clear();
+	}
+
+	return success;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.h
new file mode 100644
index 0000000000..3cb848cb58
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.h
@@ -0,0 +1,513 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/StaticArray.h>
+#include <Jolt/Core/LockFreeHashMap.h>
+#include <Jolt/Physics/EPhysicsUpdateError.h>
+#include <Jolt/Physics/Body/BodyPair.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeIDPair.h>
+#include <Jolt/Physics/Collision/ManifoldBetweenTwoFaces.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/DualAxisConstraintPart.h>
+#include <Jolt/Core/HashCombine.h>
+#include <Jolt/Core/NonCopyable.h>
+
+JPH_SUPPRESS_WARNINGS_STD_BEGIN
+#include <atomic>
+JPH_SUPPRESS_WARNINGS_STD_END
+
+JPH_NAMESPACE_BEGIN
+
+struct PhysicsSettings;
+class PhysicsUpdateContext;
+
+class JPH_EXPORT ContactConstraintManager : public NonCopyable
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+	explicit					ContactConstraintManager(const PhysicsSettings &inPhysicsSettings);
+								~ContactConstraintManager();
+
+	/// Initialize the system.
+	/// @param inMaxBodyPairs Maximum amount of body pairs to process (anything else will fall through the world), this number should generally be much higher than the max amount of contact points as there will be lots of bodies close that are not actually touching
+	/// @param inMaxContactConstraints Maximum amount of contact constraints to process (anything else will fall through the world)
+	void						Init(uint inMaxBodyPairs, uint inMaxContactConstraints);
+
+	/// Listener that is notified whenever a contact point between two bodies is added/updated/removed
+	void						SetContactListener(ContactListener *inListener)						{ mContactListener = inListener; }
+	ContactListener *			GetContactListener() const											{ return mContactListener; }
+
+	/// Callback function to combine the restitution or friction of two bodies
+	/// Note that when merging manifolds (when PhysicsSettings::mUseManifoldReduction is true) you will only get a callback for the merged manifold.
+	/// It is not possible in that case to get all sub shape ID pairs that were colliding, you'll get the first encountered pair.
+	using CombineFunction = float (*)(const Body &inBody1, const SubShapeID &inSubShapeID1, const Body &inBody2, const SubShapeID &inSubShapeID2);
+
+	/// Set the function that combines the friction of two bodies and returns it
+	/// Default method is the geometric mean: sqrt(friction1 * friction2).
+	void						SetCombineFriction(CombineFunction inCombineFriction)				{ mCombineFriction = inCombineFriction; }
+	CombineFunction				GetCombineFriction() const											{ return mCombineFriction; }
+
+	/// Set the function that combines the restitution of two bodies and returns it
+	/// Default method is max(restitution1, restitution1)
+	void						SetCombineRestitution(CombineFunction inCombineRestitution)			{ mCombineRestitution = inCombineRestitution; }
+	CombineFunction				GetCombineRestitution() const										{ return mCombineRestitution; }
+
+	/// Get the max number of contact constraints that are allowed
+	uint32						GetMaxConstraints() const											{ return mMaxConstraints; }
+
+	/// Check with the listener if inBody1 and inBody2 could collide, returns false if not
+	inline ValidateResult		ValidateContactPoint(const Body &inBody1, const Body &inBody2, RVec3Arg inBaseOffset, const CollideShapeResult &inCollisionResult) const
+	{
+		if (mContactListener == nullptr)
+			return ValidateResult::AcceptAllContactsForThisBodyPair;
+
+		return mContactListener->OnContactValidate(inBody1, inBody2, inBaseOffset, inCollisionResult);
+	}
+
+	/// Sets up the constraint buffer. Should be called before starting collision detection.
+	void						PrepareConstraintBuffer(PhysicsUpdateContext *inContext);
+
+	/// Max 4 contact points are needed for a stable manifold
+	static const int			MaxContactPoints = 4;
+
+	/// Contacts are allocated in a lock free hash map
+	class ContactAllocator : public LFHMAllocatorContext
+	{
+	public:
+		using LFHMAllocatorContext::LFHMAllocatorContext;
+
+		uint					mNumBodyPairs = 0;													///< Total number of body pairs added using this allocator
+		uint					mNumManifolds = 0;													///< Total number of manifolds added using this allocator
+		EPhysicsUpdateError		mErrors = EPhysicsUpdateError::None;								///< Errors reported on this allocator
+	};
+
+	/// Get a new allocator context for storing contacts. Note that you should call this once and then add multiple contacts using the context.
+	ContactAllocator			GetContactAllocator()												{ return mCache[mCacheWriteIdx].GetContactAllocator(); }
+
+	/// Check if the contact points from the previous frame are reusable and if so copy them.
+	/// When the cache was usable and the pair has been handled: outPairHandled = true.
+	/// When a contact constraint was produced: outConstraintCreated = true.
+	void						GetContactsFromCache(ContactAllocator &ioContactAllocator, Body &inBody1, Body &inBody2, bool &outPairHandled, bool &outConstraintCreated);
+
+	/// Handle used to keep track of the current body pair
+	using BodyPairHandle = void *;
+
+	/// Create a handle for a colliding body pair so that contact constraints can be added between them.
+	/// Needs to be called once per body pair per frame before calling AddContactConstraint.
+	BodyPairHandle				AddBodyPair(ContactAllocator &ioContactAllocator, const Body &inBody1, const Body &inBody2);
+
+	/// Add a contact constraint for this frame.
+	///
+	/// @param ioContactAllocator The allocator that reserves memory for the contacts
+	/// @param inBodyPair The handle for the contact cache for this body pair
+	/// @param inBody1 The first body that is colliding
+	/// @param inBody2 The second body that is colliding
+	/// @param inManifold The manifold that describes the collision
+	/// @return true if a contact constraint was created (can be false in the case of a sensor)
+	///
+	/// This is using the approach described in 'Modeling and Solving Constraints' by Erin Catto presented at GDC 2009 (and later years with slight modifications).
+	/// We're using the formulas from slide 50 - 53 combined.
+	///
+	/// Euler velocity integration:
+	///
+	/// v1' = v1 + M^-1 P
+	///
+	/// Impulse:
+	///
+	/// P = J^T lambda
+	///
+	/// Constraint force:
+	///
+	/// lambda = -K^-1 J v1
+	///
+	/// Inverse effective mass:
+	///
+	/// K = J M^-1 J^T
+	///
+	/// Constraint equation (limits movement in 1 axis):
+	///
+	/// C = (p2 - p1) . n
+	///
+	/// Jacobian (for position constraint)
+	///
+	/// J = [-n, -r1 x n, n, r2 x n]
+	///
+	/// n = contact normal (pointing away from body 1).
+	/// p1, p2 = positions of collision on body 1 and 2.
+	/// r1, r2 = contact point relative to center of mass of body 1 and body 2 (r1 = p1 - x1, r2 = p2 - x2).
+	/// v1, v2 = (linear velocity, angular velocity): 6 vectors containing linear and angular velocity for body 1 and 2.
+	/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].
+	bool						AddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPair, Body &inBody1, Body &inBody2, const ContactManifold &inManifold);
+
+	/// Finalizes the contact cache, the contact cache that was generated during the calls to AddContactConstraint in this update
+	/// will be used from now on to read from. After finalizing the contact cache, the contact removed callbacks will be called.
+	/// inExpectedNumBodyPairs / inExpectedNumManifolds are the amount of body pairs / manifolds found in the previous step and is
+	/// used to determine the amount of buckets the contact cache hash map will use in the next update.
+	void						FinalizeContactCacheAndCallContactPointRemovedCallbacks(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds);
+
+	/// Check if 2 bodies were in contact during the last simulation step. Since contacts are only detected between active bodies, at least one of the bodies must be active.
+	/// Uses the read collision cache to determine if 2 bodies are in contact.
+	bool						WereBodiesInContact(const BodyID &inBody1ID, const BodyID &inBody2ID) const;
+
+	/// Get the number of contact constraints that were found
+	uint32						GetNumConstraints() const											{ return min<uint32>(mNumConstraints, mMaxConstraints); }
+
+	/// Sort contact constraints deterministically
+	void						SortContacts(uint32 *inConstraintIdxBegin, uint32 *inConstraintIdxEnd) const;
+
+	/// Get the affected bodies for a given constraint
+	inline void					GetAffectedBodies(uint32 inConstraintIdx, const Body *&outBody1, const Body *&outBody2) const
+	{
+		const ContactConstraint &constraint = mConstraints[inConstraintIdx];
+		outBody1 = constraint.mBody1;
+		outBody2 = constraint.mBody2;
+	}
+
+	/// Apply last frame's impulses as an initial guess for this frame's impulses
+	template <class MotionPropertiesCallback>
+	void						WarmStartVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, MotionPropertiesCallback &ioCallback);
+
+	/// Solve velocity constraints, when almost nothing changes this should only apply very small impulses
+	/// since we're warm starting with the total impulse applied in the last frame above.
+	///
+	/// Friction wise we're using the Coulomb friction model which says that:
+	///
+	/// |F_T| <= mu |F_N|
+	///
+	/// Where F_T is the tangential force, F_N is the normal force and mu is the friction coefficient
+	///
+	/// In impulse terms this becomes:
+	///
+	/// |lambda_T| <= mu |lambda_N|
+	///
+	/// And the constraint that needs to be applied is exactly the same as a non penetration constraint
+	/// except that we use a tangent instead of a normal. The tangent should point in the direction of the
+	/// tangential velocity of the point:
+	///
+	/// J = [-T, -r1 x T, T, r2 x T]
+	///
+	/// Where T is the tangent.
+	///
+	/// See slide 42 and 43.
+	///
+	/// Restitution is implemented as a velocity bias (see slide 41):
+	///
+	/// b = e v_n^-
+	///
+	/// e = the restitution coefficient, v_n^- is the normal velocity prior to the collision
+	///
+	/// Restitution is only applied when v_n^- is large enough and the points are moving towards collision
+	bool						SolveVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd);
+
+	/// Save back the lambdas to the contact cache for the next warm start
+	void						StoreAppliedImpulses(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd) const;
+
+	/// Solve position constraints.
+	/// This is using the approach described in 'Modeling and Solving Constraints' by Erin Catto presented at GDC 2007.
+	/// On slide 78 it is suggested to split up the Baumgarte stabilization for positional drift so that it does not
+	/// actually add to the momentum. We combine an Euler velocity integrate + a position integrate and then discard the velocity
+	/// change.
+	///
+	/// Constraint force:
+	///
+	/// lambda = -K^-1 b
+	///
+	/// Baumgarte stabilization:
+	///
+	/// b = beta / dt C
+	///
+	/// beta = baumgarte stabilization factor.
+	/// dt = delta time.
+	bool						SolvePositionConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd);
+
+	/// Recycle the constraint buffer. Should be called between collision simulation steps.
+	void						RecycleConstraintBuffer();
+
+	/// Terminate the constraint buffer. Should be called after simulation ends.
+	void						FinishConstraintBuffer();
+
+	/// Called by continuous collision detection to notify the contact listener that a contact was added
+	/// @param ioContactAllocator The allocator that reserves memory for the contacts
+	/// @param inBody1 The first body that is colliding
+	/// @param inBody2 The second body that is colliding
+	/// @param inManifold The manifold that describes the collision
+	/// @param outSettings The calculated contact settings (may be overridden by the contact listener)
+	void						OnCCDContactAdded(ContactAllocator &ioContactAllocator, const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &outSettings);
+
+#ifdef JPH_DEBUG_RENDERER
+	// Drawing properties
+	static bool					sDrawContactPoint;
+	static bool					sDrawSupportingFaces;
+	static bool					sDrawContactPointReduction;
+	static bool					sDrawContactManifolds;
+#endif // JPH_DEBUG_RENDERER
+
+	/// Saving state for replay
+	void						SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const;
+
+	/// Restoring state for replay. Returns false when failed.
+	bool						RestoreState(StateRecorder &inStream, const StateRecorderFilter *inFilter);
+
+private:
+	/// Local space contact point, used for caching impulses
+	class CachedContactPoint
+	{
+	public:
+		/// Saving / restoring state for replay
+		void					SaveState(StateRecorder &inStream) const;
+		void					RestoreState(StateRecorder &inStream);
+
+		/// Local space positions on body 1 and 2.
+		/// Note: these values are read through sLoadFloat3Unsafe.
+		Float3					mPosition1;
+		Float3					mPosition2;
+
+		/// Total applied impulse during the last update that it was used
+		float					mNonPenetrationLambda;
+		Vector<2>				mFrictionLambda;
+	};
+
+	static_assert(sizeof(CachedContactPoint) == 36, "Unexpected size");
+	static_assert(alignof(CachedContactPoint) == 4, "Assuming 4 byte aligned");
+
+	/// A single cached manifold
+	class CachedManifold
+	{
+	public:
+		/// Calculate size in bytes needed beyond the size of the class to store inNumContactPoints
+		static int				sGetRequiredExtraSize(int inNumContactPoints)						{ return max(0, inNumContactPoints - 1) * sizeof(CachedContactPoint); }
+
+		/// Calculate total class size needed for storing inNumContactPoints
+		static int				sGetRequiredTotalSize(int inNumContactPoints)						{ return sizeof(CachedManifold) + sGetRequiredExtraSize(inNumContactPoints); }
+
+		/// Saving / restoring state for replay
+		void					SaveState(StateRecorder &inStream) const;
+		void					RestoreState(StateRecorder &inStream);
+
+		/// Handle to next cached contact points in ManifoldCache::mCachedManifolds for the same body pair
+		uint32					mNextWithSameBodyPair;
+
+		/// Contact normal in the space of 2.
+		/// Note: this value is read through sLoadFloat3Unsafe.
+		Float3					mContactNormal;
+
+		/// Flags for this cached manifold
+		enum class EFlags : uint16
+		{
+			ContactPersisted	= 1,																///< If this cache entry was reused in the next simulation update
+			CCDContact			= 2																	///< This is a cached manifold reported by continuous collision detection and was only used to create a contact callback
+		};
+
+		/// @see EFlags
+		mutable atomic<uint16>	mFlags { 0 };
+
+		/// Number of contact points in the array below
+		uint16					mNumContactPoints;
+
+		/// Contact points that this manifold consists of
+		CachedContactPoint		mContactPoints[1];
+	};
+
+	static_assert(sizeof(CachedManifold) == 56, "This structure is expect to not contain any waste due to alignment");
+	static_assert(alignof(CachedManifold) == 4, "Assuming 4 byte aligned");
+
+	/// Define a map that maps SubShapeIDPair -> manifold
+	using ManifoldMap = LockFreeHashMap<SubShapeIDPair, CachedManifold>;
+	using MKeyValue = ManifoldMap::KeyValue;
+	using MKVAndCreated = std::pair<MKeyValue *, bool>;
+
+	/// Start of list of contact points for a particular pair of bodies
+	class CachedBodyPair
+	{
+	public:
+		/// Saving / restoring state for replay
+		void					SaveState(StateRecorder &inStream) const;
+		void					RestoreState(StateRecorder &inStream);
+
+		/// Local space position difference from Body A to Body B.
+		/// Note: this value is read through sLoadFloat3Unsafe
+		Float3					mDeltaPosition;
+
+		/// Local space rotation difference from Body A to Body B, fourth component of quaternion is not stored but is guaranteed >= 0.
+		/// Note: this value is read through sLoadFloat3Unsafe
+		Float3					mDeltaRotation;
+
+		/// Handle to first manifold in ManifoldCache::mCachedManifolds
+		uint32					mFirstCachedManifold;
+	};
+
+	static_assert(sizeof(CachedBodyPair) == 28, "Unexpected size");
+	static_assert(alignof(CachedBodyPair) == 4, "Assuming 4 byte aligned");
+
+	/// Define a map that maps BodyPair -> CachedBodyPair
+	using BodyPairMap = LockFreeHashMap<BodyPair, CachedBodyPair>;
+	using BPKeyValue = BodyPairMap::KeyValue;
+
+	/// Holds all caches that are needed to quickly find cached body pairs / manifolds
+	class ManifoldCache
+	{
+	public:
+		/// Initialize the cache
+		void					Init(uint inMaxBodyPairs, uint inMaxContactConstraints, uint inCachedManifoldsSize);
+
+		/// Reset all entries from the cache
+		void					Clear();
+
+		/// Prepare cache before creating new contacts.
+		/// inExpectedNumBodyPairs / inExpectedNumManifolds are the amount of body pairs / manifolds found in the previous step and is used to determine the amount of buckets the contact cache hash map will use.
+		void					Prepare(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds);
+
+		/// Get a new allocator context for storing contacts. Note that you should call this once and then add multiple contacts using the context.
+		ContactAllocator		GetContactAllocator()						{ return ContactAllocator(mAllocator, cAllocatorBlockSize); }
+
+		/// Find / create cached entry for SubShapeIDPair -> CachedManifold
+		const MKeyValue *		Find(const SubShapeIDPair &inKey, uint64 inKeyHash) const;
+		MKeyValue *				Create(ContactAllocator &ioContactAllocator, const SubShapeIDPair &inKey, uint64 inKeyHash, int inNumContactPoints);
+		MKVAndCreated			FindOrCreate(ContactAllocator &ioContactAllocator, const SubShapeIDPair &inKey, uint64 inKeyHash, int inNumContactPoints);
+		uint32					ToHandle(const MKeyValue *inKeyValue) const;
+		const MKeyValue *		FromHandle(uint32 inHandle) const;
+
+		/// Find / create entry for BodyPair -> CachedBodyPair
+		const BPKeyValue *		Find(const BodyPair &inKey, uint64 inKeyHash) const;
+		BPKeyValue *			Create(ContactAllocator &ioContactAllocator, const BodyPair &inKey, uint64 inKeyHash);
+		void					GetAllBodyPairsSorted(Array<const BPKeyValue *> &outAll) const;
+		void					GetAllManifoldsSorted(const CachedBodyPair &inBodyPair, Array<const MKeyValue *> &outAll) const;
+		void					GetAllCCDManifoldsSorted(Array<const MKeyValue *> &outAll) const;
+		void					ContactPointRemovedCallbacks(ContactListener *inListener);
+
+#ifdef JPH_ENABLE_ASSERTS
+		/// Get the amount of manifolds in the cache
+		uint					GetNumManifolds() const						{ return mCachedManifolds.GetNumKeyValues(); }
+
+		/// Get the amount of body pairs in the cache
+		uint					GetNumBodyPairs() const						{ return mCachedBodyPairs.GetNumKeyValues(); }
+
+		/// Before a cache is finalized you can only do Create(), after only Find() or Clear()
+		void					Finalize();
+#endif
+
+		/// Saving / restoring state for replay
+		void					SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const;
+		bool					RestoreState(const ManifoldCache &inReadCache, StateRecorder &inStream, const StateRecorderFilter *inFilter);
+
+	private:
+		/// Block size used when allocating new blocks in the contact cache
+		static constexpr uint32	cAllocatorBlockSize = 4096;
+
+		/// Allocator used by both mCachedManifolds and mCachedBodyPairs, this makes it more likely that a body pair and its manifolds are close in memory
+		LFHMAllocator			mAllocator;
+
+		/// Simple hash map for SubShapeIDPair -> CachedManifold
+		ManifoldMap				mCachedManifolds { mAllocator };
+
+		/// Simple hash map for BodyPair -> CachedBodyPair
+		BodyPairMap				mCachedBodyPairs { mAllocator };
+
+#ifdef JPH_ENABLE_ASSERTS
+		bool					mIsFinalized = false;						///< Marks if this buffer is complete
+#endif
+	};
+
+	ManifoldCache				mCache[2];									///< We have one cache to read from and one to write to
+	int							mCacheWriteIdx = 0;							///< Which cache we're currently writing to
+
+	/// World space contact point, used for solving penetrations
+	class WorldContactPoint
+	{
+	public:
+		/// Calculate constraint properties below
+		void					CalculateNonPenetrationConstraintProperties(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, const Body &inBody2, float inInvMass2, float inInvInertiaScale2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal);
+
+		template <EMotionType Type1, EMotionType Type2>
+		JPH_INLINE void			TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(float inDeltaTime, float inGravityDeltaTimeDotNormal, const Body &inBody1, const Body &inBody2, float inInvM1, float inInvM2, Mat44Arg inInvI1, Mat44Arg inInvI2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal, Vec3Arg inWorldSpaceTangent1, Vec3Arg inWorldSpaceTangent2, const ContactSettings &inSettings, float inMinVelocityForRestitution);
+
+		/// The constraint parts
+		AxisConstraintPart		mNonPenetrationConstraint;
+		AxisConstraintPart		mFrictionConstraint1;
+		AxisConstraintPart		mFrictionConstraint2;
+
+		/// Contact cache
+		CachedContactPoint *	mContactPoint;
+	};
+
+	using WorldContactPoints = StaticArray<WorldContactPoint, MaxContactPoints>;
+
+	/// Contact constraint class, used for solving penetrations
+	class ContactConstraint
+	{
+	public:
+	#ifdef JPH_DEBUG_RENDERER
+		/// Draw the state of the contact constraint
+		void					Draw(DebugRenderer *inRenderer, ColorArg inManifoldColor) const;
+	#endif // JPH_DEBUG_RENDERER
+
+		/// Convert the world space normal to a Vec3
+		JPH_INLINE Vec3			GetWorldSpaceNormal() const
+		{
+			return Vec3::sLoadFloat3Unsafe(mWorldSpaceNormal);
+		}
+
+		/// Get the tangents for this contact constraint
+		JPH_INLINE void			GetTangents(Vec3 &outTangent1, Vec3 &outTangent2) const
+		{
+			Vec3 ws_normal = GetWorldSpaceNormal();
+			outTangent1 = ws_normal.GetNormalizedPerpendicular();
+			outTangent2 = ws_normal.Cross(outTangent1);
+		}
+
+		Body *					mBody1;
+		Body *					mBody2;
+		uint64					mSortKey;
+		Float3					mWorldSpaceNormal;
+		float					mCombinedFriction;
+		float					mInvMass1;
+		float					mInvInertiaScale1;
+		float					mInvMass2;
+		float					mInvInertiaScale2;
+		WorldContactPoints		mContactPoints;
+	};
+
+	/// Internal helper function to calculate the friction and non-penetration constraint properties. Templated to the motion type to reduce the amount of branches and calculations.
+	template <EMotionType Type1, EMotionType Type2>
+	JPH_INLINE void				TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2);
+
+	/// Internal helper function to calculate the friction and non-penetration constraint properties.
+	inline void					CalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2);
+
+	/// Internal helper function to add a contact constraint. Templated to the motion type to reduce the amount of branches and calculations.
+	template <EMotionType Type1, EMotionType Type2>
+	bool						TemplatedAddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPairHandle, Body &inBody1, Body &inBody2, const ContactManifold &inManifold);
+
+	/// Internal helper function to warm start contact constraint. Templated to the motion type to reduce the amount of branches and calculations.
+	template <EMotionType Type1, EMotionType Type2>
+	JPH_INLINE static void		sWarmStartConstraint(ContactConstraint &ioConstraint, MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2, float inWarmStartImpulseRatio);
+
+	/// Internal helper function to solve a single contact constraint. Templated to the motion type to reduce the amount of branches and calculations.
+	template <EMotionType Type1, EMotionType Type2>
+	JPH_INLINE static bool		sSolveVelocityConstraint(ContactConstraint &ioConstraint, MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2);
+
+	/// The main physics settings instance
+	const PhysicsSettings &		mPhysicsSettings;
+
+	/// Listener that is notified whenever a contact point between two bodies is added/updated/removed
+	ContactListener *			mContactListener = nullptr;
+
+	/// Functions that are used to combine friction and restitution of 2 bodies
+	CombineFunction				mCombineFriction = [](const Body &inBody1, const SubShapeID &, const Body &inBody2, const SubShapeID &) { return sqrt(inBody1.GetFriction() * inBody2.GetFriction()); };
+	CombineFunction				mCombineRestitution = [](const Body &inBody1, const SubShapeID &, const Body &inBody2, const SubShapeID &) { return max(inBody1.GetRestitution(), inBody2.GetRestitution()); };
+
+	/// The constraints that were added this frame
+	ContactConstraint *			mConstraints = nullptr;
+	uint32						mMaxConstraints = 0;
+	atomic<uint32>				mNumConstraints { 0 };
+
+	/// Context used for this physics update
+	PhysicsUpdateContext *		mUpdateContext;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/DistanceConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/DistanceConstraint.cpp
new file mode 100644
index 0000000000..e70bfb24ce
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/DistanceConstraint.cpp
@@ -0,0 +1,266 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/DistanceConstraint.h>
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+using namespace literals;
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(DistanceConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(DistanceConstraintSettings, TwoBodyConstraintSettings)
+
+	JPH_ADD_ENUM_ATTRIBUTE(DistanceConstraintSettings, mSpace)
+	JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mPoint1)
+	JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mPoint2)
+	JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mMinDistance)
+	JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mMaxDistance)
+	JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(DistanceConstraintSettings, mLimitsSpringSettings.mMode, "mSpringMode")
+	JPH_ADD_ATTRIBUTE_WITH_ALIAS(DistanceConstraintSettings, mLimitsSpringSettings.mFrequency, "mFrequency") // Renaming attributes to stay compatible with old versions of the library
+	JPH_ADD_ATTRIBUTE_WITH_ALIAS(DistanceConstraintSettings, mLimitsSpringSettings.mDamping, "mDamping")
+}
+
+void DistanceConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	ConstraintSettings::SaveBinaryState(inStream);
+
+	inStream.Write(mSpace);
+	inStream.Write(mPoint1);
+	inStream.Write(mPoint2);
+	inStream.Write(mMinDistance);
+	inStream.Write(mMaxDistance);
+	mLimitsSpringSettings.SaveBinaryState(inStream);
+}
+
+void DistanceConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	ConstraintSettings::RestoreBinaryState(inStream);
+
+	inStream.Read(mSpace);
+	inStream.Read(mPoint1);
+	inStream.Read(mPoint2);
+	inStream.Read(mMinDistance);
+	inStream.Read(mMaxDistance);
+	mLimitsSpringSettings.RestoreBinaryState(inStream);
+}
+
+TwoBodyConstraint *DistanceConstraintSettings::Create(Body &inBody1, Body &inBody2) const
+{
+	return new DistanceConstraint(inBody1, inBody2, *this);
+}
+
+DistanceConstraint::DistanceConstraint(Body &inBody1, Body &inBody2, const DistanceConstraintSettings &inSettings) :
+	TwoBodyConstraint(inBody1, inBody2, inSettings),
+	mMinDistance(inSettings.mMinDistance),
+	mMaxDistance(inSettings.mMaxDistance)
+{
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		// If all properties were specified in world space, take them to local space now
+		mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mPoint1);
+		mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mPoint2);
+		mWorldSpacePosition1 = inSettings.mPoint1;
+		mWorldSpacePosition2 = inSettings.mPoint2;
+	}
+	else
+	{
+		// If properties were specified in local space, we need to calculate world space positions
+		mLocalSpacePosition1 = Vec3(inSettings.mPoint1);
+		mLocalSpacePosition2 = Vec3(inSettings.mPoint2);
+		mWorldSpacePosition1 = inBody1.GetCenterOfMassTransform() * inSettings.mPoint1;
+		mWorldSpacePosition2 = inBody2.GetCenterOfMassTransform() * inSettings.mPoint2;
+	}
+
+	// Store distance we want to keep between the world space points
+	float distance = Vec3(mWorldSpacePosition2 - mWorldSpacePosition1).Length();
+	float min_distance, max_distance;
+	if (mMinDistance < 0.0f && mMaxDistance < 0.0f)
+	{
+		min_distance = max_distance = distance;
+	}
+	else
+	{
+		min_distance = mMinDistance < 0.0f? min(distance, mMaxDistance) : mMinDistance;
+		max_distance = mMaxDistance < 0.0f? max(distance, mMinDistance) : mMaxDistance;
+	}
+	SetDistance(min_distance, max_distance);
+
+	// Most likely gravity is going to tear us apart (this is only used when the distance between the points = 0)
+	mWorldSpaceNormal = Vec3::sAxisY();
+
+	// Store spring settings
+	SetLimitsSpringSettings(inSettings.mLimitsSpringSettings);
+}
+
+void DistanceConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
+{
+	if (mBody1->GetID() == inBodyID)
+		mLocalSpacePosition1 -= inDeltaCOM;
+	else if (mBody2->GetID() == inBodyID)
+		mLocalSpacePosition2 -= inDeltaCOM;
+}
+
+void DistanceConstraint::CalculateConstraintProperties(float inDeltaTime)
+{
+	// Update world space positions (the bodies may have moved)
+	mWorldSpacePosition1 = mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1;
+	mWorldSpacePosition2 = mBody2->GetCenterOfMassTransform() * mLocalSpacePosition2;
+
+	// Calculate world space normal
+	Vec3 delta = Vec3(mWorldSpacePosition2 - mWorldSpacePosition1);
+	float delta_len = delta.Length();
+	if (delta_len > 0.0f)
+		mWorldSpaceNormal = delta / delta_len;
+
+	// Calculate points relative to body
+	// r1 + u = (p1 - x1) + (p2 - p1) = p2 - x1
+	Vec3 r1_plus_u = Vec3(mWorldSpacePosition2 - mBody1->GetCenterOfMassPosition());
+	Vec3 r2 = Vec3(mWorldSpacePosition2 - mBody2->GetCenterOfMassPosition());
+
+	if (mMinDistance == mMaxDistance)
+	{
+		mAxisConstraint.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, mWorldSpaceNormal, 0.0f, delta_len - mMinDistance, mLimitsSpringSettings);
+
+		// Single distance, allow constraint forces in both directions
+		mMinLambda = -FLT_MAX;
+		mMaxLambda = FLT_MAX;
+	}
+	else if (delta_len <= mMinDistance)
+	{
+		mAxisConstraint.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, mWorldSpaceNormal, 0.0f, delta_len - mMinDistance, mLimitsSpringSettings);
+
+		// Allow constraint forces to make distance bigger only
+		mMinLambda = 0;
+		mMaxLambda = FLT_MAX;
+	}
+	else if (delta_len >= mMaxDistance)
+	{
+		mAxisConstraint.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, mWorldSpaceNormal, 0.0f, delta_len - mMaxDistance, mLimitsSpringSettings);
+
+		// Allow constraint forces to make distance smaller only
+		mMinLambda = -FLT_MAX;
+		mMaxLambda = 0;
+	}
+	else
+		mAxisConstraint.Deactivate();
+}
+
+void DistanceConstraint::SetupVelocityConstraint(float inDeltaTime)
+{
+	CalculateConstraintProperties(inDeltaTime);
+}
+
+void DistanceConstraint::ResetWarmStart()
+{
+	mAxisConstraint.Deactivate();
+}
+
+void DistanceConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
+{
+	mAxisConstraint.WarmStart(*mBody1, *mBody2, mWorldSpaceNormal, inWarmStartImpulseRatio);
+}
+
+bool DistanceConstraint::SolveVelocityConstraint(float inDeltaTime)
+{
+	if (mAxisConstraint.IsActive())
+		return mAxisConstraint.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceNormal, mMinLambda, mMaxLambda);
+	else
+		return false;
+}
+
+bool DistanceConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
+{
+	if (mLimitsSpringSettings.mFrequency <= 0.0f) // When the spring is active, we don't need to solve the position constraint
+	{
+		float distance = Vec3(mWorldSpacePosition2 - mWorldSpacePosition1).Dot(mWorldSpaceNormal);
+
+		// Calculate position error
+		float position_error = 0.0f;
+		if (distance < mMinDistance)
+			position_error = distance - mMinDistance;
+		else if (distance > mMaxDistance)
+			position_error = distance - mMaxDistance;
+
+		if (position_error != 0.0f)
+		{
+			// Update constraint properties (bodies may have moved)
+			CalculateConstraintProperties(inDeltaTime);
+
+			return mAxisConstraint.SolvePositionConstraint(*mBody1, *mBody2, mWorldSpaceNormal, position_error, inBaumgarte);
+		}
+	}
+
+	return false;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void DistanceConstraint::DrawConstraint(DebugRenderer *inRenderer) const
+{
+	// Draw constraint
+	Vec3 delta = Vec3(mWorldSpacePosition2 - mWorldSpacePosition1);
+	float len = delta.Length();
+	if (len < mMinDistance)
+	{
+		RVec3 real_end_pos = mWorldSpacePosition1 + (len > 0.0f? delta * mMinDistance / len : Vec3(0, len, 0));
+		inRenderer->DrawLine(mWorldSpacePosition1, mWorldSpacePosition2, Color::sGreen);
+		inRenderer->DrawLine(mWorldSpacePosition2, real_end_pos, Color::sYellow);
+	}
+	else if (len > mMaxDistance)
+	{
+		RVec3 real_end_pos = mWorldSpacePosition1 + (len > 0.0f? delta * mMaxDistance / len : Vec3(0, len, 0));
+		inRenderer->DrawLine(mWorldSpacePosition1, real_end_pos, Color::sGreen);
+		inRenderer->DrawLine(real_end_pos, mWorldSpacePosition2, Color::sRed);
+	}
+	else
+		inRenderer->DrawLine(mWorldSpacePosition1, mWorldSpacePosition2, Color::sGreen);
+
+	// Draw constraint end points
+	inRenderer->DrawMarker(mWorldSpacePosition1, Color::sWhite, 0.1f);
+	inRenderer->DrawMarker(mWorldSpacePosition2, Color::sWhite, 0.1f);
+
+	// Draw current length
+	inRenderer->DrawText3D(0.5_r * (mWorldSpacePosition1 + mWorldSpacePosition2), StringFormat("%.2f", (double)len));
+}
+#endif // JPH_DEBUG_RENDERER
+
+void DistanceConstraint::SaveState(StateRecorder &inStream) const
+{
+	TwoBodyConstraint::SaveState(inStream);
+
+	mAxisConstraint.SaveState(inStream);
+	inStream.Write(mWorldSpaceNormal); // When distance = 0, the normal is used from last frame so we need to store it
+}
+
+void DistanceConstraint::RestoreState(StateRecorder &inStream)
+{
+	TwoBodyConstraint::RestoreState(inStream);
+
+	mAxisConstraint.RestoreState(inStream);
+	inStream.Read(mWorldSpaceNormal);
+}
+
+Ref<ConstraintSettings> DistanceConstraint::GetConstraintSettings() const
+{
+	DistanceConstraintSettings *settings = new DistanceConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mPoint1 = RVec3(mLocalSpacePosition1);
+	settings->mPoint2 = RVec3(mLocalSpacePosition2);
+	settings->mMinDistance = mMinDistance;
+	settings->mMaxDistance = mMaxDistance;
+	settings->mLimitsSpringSettings = mLimitsSpringSettings;
+	return settings;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/DistanceConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/DistanceConstraint.h
new file mode 100644
index 0000000000..d237b8fd2b
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/DistanceConstraint.h
@@ -0,0 +1,120 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Distance constraint settings, used to create a distance constraint
+class JPH_EXPORT DistanceConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, DistanceConstraintSettings)
+
+public:
+	// See: ConstraintSettings::SaveBinaryState
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+
+	/// Create an instance of this constraint
+	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
+
+	/// This determines in which space the constraint is setup, all properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	/// Body 1 constraint reference frame (space determined by mSpace).
+	/// Constraint will keep mPoint1 (a point on body 1) and mPoint2 (a point on body 2) at the same distance.
+	/// Note that this constraint can be used as a cheap PointConstraint by setting mPoint1 = mPoint2 (but this removes only 1 degree of freedom instead of 3).
+	RVec3						mPoint1 = RVec3::sZero();
+
+	/// Body 2 constraint reference frame (space determined by mSpace)
+	RVec3						mPoint2 = RVec3::sZero();
+
+	/// Ability to override the distance range at which the two points are kept apart. If the value is negative, it will be replaced by the distance between mPoint1 and mPoint2 (works only if mSpace is world space).
+	float						mMinDistance = -1.0f;
+	float						mMaxDistance = -1.0f;
+
+	/// When enabled, this makes the limits soft. When the constraint exceeds the limits, a spring force will pull it back.
+	SpringSettings				mLimitsSpringSettings;
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// This constraint is a stiff spring that holds 2 points at a fixed distance from each other
+class JPH_EXPORT DistanceConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct distance constraint
+								DistanceConstraint(Body &inBody1, Body &inBody2, const DistanceConstraintSettings &inSettings);
+
+	// Generic interface of a constraint
+	virtual EConstraintSubType	GetSubType() const override									{ return EConstraintSubType::Distance; }
+	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
+	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
+	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
+	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
+	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void				DrawConstraint(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual void				SaveState(StateRecorder &inStream) const override;
+	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
+
+	// See: TwoBodyConstraint
+	virtual Mat44				GetConstraintToBody1Matrix() const override					{ return Mat44::sTranslation(mLocalSpacePosition1); }
+	virtual Mat44				GetConstraintToBody2Matrix() const override					{ return Mat44::sTranslation(mLocalSpacePosition2); } // Note: Incorrect rotation as we don't track the original rotation difference, should not matter though as the constraint is not limiting rotation.
+
+	/// Update the minimum and maximum distance for the constraint
+	void						SetDistance(float inMinDistance, float inMaxDistance)		{ JPH_ASSERT(inMinDistance <= inMaxDistance); mMinDistance = inMinDistance; mMaxDistance = inMaxDistance; }
+	float						GetMinDistance() const										{ return mMinDistance; }
+	float						GetMaxDistance() const										{ return mMaxDistance; }
+
+	/// Update the limits spring settings
+	const SpringSettings &		GetLimitsSpringSettings() const								{ return mLimitsSpringSettings; }
+	SpringSettings &			GetLimitsSpringSettings()									{ return mLimitsSpringSettings; }
+	void						SetLimitsSpringSettings(const SpringSettings &inLimitsSpringSettings) { mLimitsSpringSettings = inLimitsSpringSettings; }
+
+	///@name Get Lagrange multiplier from last physics update (the linear impulse applied to satisfy the constraint)
+	inline float				GetTotalLambdaPosition() const								{ return mAxisConstraint.GetTotalLambda(); }
+
+private:
+	// Internal helper function to calculate the values below
+	void						CalculateConstraintProperties(float inDeltaTime);
+
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	// Local space constraint positions
+	Vec3						mLocalSpacePosition1;
+	Vec3						mLocalSpacePosition2;
+
+	// Min/max distance that must be kept between the world space points
+	float						mMinDistance;
+	float						mMaxDistance;
+
+	// Soft constraint limits
+	SpringSettings				mLimitsSpringSettings;
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	// World space positions and normal
+	RVec3						mWorldSpacePosition1;
+	RVec3						mWorldSpacePosition2;
+	Vec3						mWorldSpaceNormal;
+
+	// Depending on if the distance < min or distance > max we can apply forces to prevent further violations
+	float						mMinLambda;
+	float						mMaxLambda;
+
+	// The constraint part
+	AxisConstraintPart			mAxisConstraint;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/FixedConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/FixedConstraint.cpp
new file mode 100644
index 0000000000..b063985167
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/FixedConstraint.cpp
@@ -0,0 +1,215 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/FixedConstraint.h>
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+using namespace literals;
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(FixedConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(FixedConstraintSettings, TwoBodyConstraintSettings)
+
+	JPH_ADD_ENUM_ATTRIBUTE(FixedConstraintSettings, mSpace)
+	JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAutoDetectPoint)
+	JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mPoint1)
+	JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAxisX1)
+	JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAxisY1)
+	JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mPoint2)
+	JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAxisX2)
+	JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAxisY2)
+}
+
+void FixedConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	ConstraintSettings::SaveBinaryState(inStream);
+
+	inStream.Write(mSpace);
+	inStream.Write(mAutoDetectPoint);
+	inStream.Write(mPoint1);
+	inStream.Write(mAxisX1);
+	inStream.Write(mAxisY1);
+	inStream.Write(mPoint2);
+	inStream.Write(mAxisX2);
+	inStream.Write(mAxisY2);
+}
+
+void FixedConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	ConstraintSettings::RestoreBinaryState(inStream);
+
+	inStream.Read(mSpace);
+	inStream.Read(mAutoDetectPoint);
+	inStream.Read(mPoint1);
+	inStream.Read(mAxisX1);
+	inStream.Read(mAxisY1);
+	inStream.Read(mPoint2);
+	inStream.Read(mAxisX2);
+	inStream.Read(mAxisY2);
+}
+
+TwoBodyConstraint *FixedConstraintSettings::Create(Body &inBody1, Body &inBody2) const
+{
+	return new FixedConstraint(inBody1, inBody2, *this);
+}
+
+FixedConstraint::FixedConstraint(Body &inBody1, Body &inBody2, const FixedConstraintSettings &inSettings) :
+	TwoBodyConstraint(inBody1, inBody2, inSettings)
+{
+	// Store inverse of initial rotation from body 1 to body 2 in body 1 space
+	mInvInitialOrientation = RotationEulerConstraintPart::sGetInvInitialOrientationXY(inSettings.mAxisX1, inSettings.mAxisY1, inSettings.mAxisX2, inSettings.mAxisY2);
+
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		if (inSettings.mAutoDetectPoint)
+		{
+			// Determine anchor point: If any of the bodies can never be dynamic use the other body as anchor point
+			RVec3 anchor;
+			if (!inBody1.CanBeKinematicOrDynamic())
+				anchor = inBody2.GetCenterOfMassPosition();
+			else if (!inBody2.CanBeKinematicOrDynamic())
+				anchor = inBody1.GetCenterOfMassPosition();
+			else
+			{
+				// Otherwise use weighted anchor point towards the lightest body
+				Real inv_m1 = Real(inBody1.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked());
+				Real inv_m2 = Real(inBody2.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked());
+				Real total_inv_mass = inv_m1 + inv_m2;
+				if (total_inv_mass != 0.0_r)
+					anchor = (inv_m1 * inBody1.GetCenterOfMassPosition() + inv_m2 * inBody2.GetCenterOfMassPosition()) / (inv_m1 + inv_m2);
+				else
+					anchor = inBody1.GetCenterOfMassPosition();
+			}
+
+			// Store local positions
+			mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * anchor);
+			mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * anchor);
+		}
+		else
+		{
+			// Store local positions
+			mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mPoint1);
+			mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mPoint2);
+		}
+
+		// Constraints were specified in world space, so we should have replaced c1 with q10^-1 c1 and c2 with q20^-1 c2
+		// => r0^-1 = (q20^-1 c2) (q10^-1 c1)^1 = q20^-1 (c2 c1^-1) q10
+		mInvInitialOrientation = inBody2.GetRotation().Conjugated() * mInvInitialOrientation * inBody1.GetRotation();
+	}
+	else
+	{
+		// Store local positions
+		mLocalSpacePosition1 = Vec3(inSettings.mPoint1);
+		mLocalSpacePosition2 = Vec3(inSettings.mPoint2);
+	}
+}
+
+void FixedConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
+{
+	if (mBody1->GetID() == inBodyID)
+		mLocalSpacePosition1 -= inDeltaCOM;
+	else if (mBody2->GetID() == inBodyID)
+		mLocalSpacePosition2 -= inDeltaCOM;
+}
+
+void FixedConstraint::SetupVelocityConstraint(float inDeltaTime)
+{
+	// Calculate constraint values that don't change when the bodies don't change position
+	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
+	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
+	mRotationConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, *mBody2, rotation2);
+	mPointConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, mLocalSpacePosition1, *mBody2, rotation2, mLocalSpacePosition2);
+}
+
+void FixedConstraint::ResetWarmStart()
+{
+	mRotationConstraintPart.Deactivate();
+	mPointConstraintPart.Deactivate();
+}
+
+void FixedConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
+{
+	// Warm starting: Apply previous frame impulse
+	mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+	mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+}
+
+bool FixedConstraint::SolveVelocityConstraint(float inDeltaTime)
+{
+	// Solve rotation constraint
+	bool rot = mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+
+	// Solve position constraint
+	bool pos = mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+
+	return rot || pos;
+}
+
+bool FixedConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
+{
+	// Solve rotation constraint
+	mRotationConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), *mBody2, Mat44::sRotation(mBody2->GetRotation()));
+	bool rot = mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mInvInitialOrientation, inBaumgarte);
+
+	// Solve position constraint
+	mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2);
+	bool pos = mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte);
+
+	return rot || pos;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void FixedConstraint::DrawConstraint(DebugRenderer *inRenderer) const
+{
+	RMat44 com1 = mBody1->GetCenterOfMassTransform();
+	RMat44 com2 = mBody2->GetCenterOfMassTransform();
+
+	RVec3 anchor1 = com1 * mLocalSpacePosition1;
+	RVec3 anchor2 = com2 * mLocalSpacePosition2;
+
+	// Draw constraint
+	inRenderer->DrawLine(com1.GetTranslation(), anchor1, Color::sGreen);
+	inRenderer->DrawLine(com2.GetTranslation(), anchor2, Color::sBlue);
+}
+#endif // JPH_DEBUG_RENDERER
+
+void FixedConstraint::SaveState(StateRecorder &inStream) const
+{
+	TwoBodyConstraint::SaveState(inStream);
+
+	mRotationConstraintPart.SaveState(inStream);
+	mPointConstraintPart.SaveState(inStream);
+}
+
+void FixedConstraint::RestoreState(StateRecorder &inStream)
+{
+	TwoBodyConstraint::RestoreState(inStream);
+
+	mRotationConstraintPart.RestoreState(inStream);
+	mPointConstraintPart.RestoreState(inStream);
+}
+
+Ref<ConstraintSettings> FixedConstraint::GetConstraintSettings() const
+{
+	FixedConstraintSettings *settings = new FixedConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mPoint1 = RVec3(mLocalSpacePosition1);
+	settings->mAxisX1 = Vec3::sAxisX();
+	settings->mAxisY1 = Vec3::sAxisY();
+	settings->mPoint2 = RVec3(mLocalSpacePosition2);
+	settings->mAxisX2 = mInvInitialOrientation.RotateAxisX();
+	settings->mAxisY2 = mInvInitialOrientation.RotateAxisY();
+	return settings;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/FixedConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/FixedConstraint.h
new file mode 100644
index 0000000000..114cf646e1
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/FixedConstraint.h
@@ -0,0 +1,96 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Fixed constraint settings, used to create a fixed constraint
+class JPH_EXPORT FixedConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, FixedConstraintSettings)
+
+public:
+	// See: ConstraintSettings::SaveBinaryState
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+
+	/// Create an instance of this constraint
+	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
+
+	/// This determines in which space the constraint is setup, all properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	/// When mSpace is WorldSpace mPoint1 and mPoint2 can be automatically calculated based on the positions of the bodies when the constraint is created (they will be fixated in their current relative position/orientation). Set this to false if you want to supply the attachment points yourself.
+	bool						mAutoDetectPoint = false;
+
+	/// Body 1 constraint reference frame (space determined by mSpace)
+	RVec3						mPoint1 = RVec3::sZero();
+	Vec3						mAxisX1 = Vec3::sAxisX();
+	Vec3						mAxisY1 = Vec3::sAxisY();
+
+	/// Body 2 constraint reference frame (space determined by mSpace)
+	RVec3						mPoint2 = RVec3::sZero();
+	Vec3						mAxisX2 = Vec3::sAxisX();
+	Vec3						mAxisY2 = Vec3::sAxisY();
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// A fixed constraint welds two bodies together removing all degrees of freedom between them.
+/// This variant uses Euler angles for the rotation constraint.
+class JPH_EXPORT FixedConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+								FixedConstraint(Body &inBody1, Body &inBody2, const FixedConstraintSettings &inSettings);
+
+	// Generic interface of a constraint
+	virtual EConstraintSubType	GetSubType() const override									{ return EConstraintSubType::Fixed; }
+	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
+	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
+	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
+	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
+	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void				DrawConstraint(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual void				SaveState(StateRecorder &inStream) const override;
+	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
+
+	// See: TwoBodyConstraint
+	virtual Mat44				GetConstraintToBody1Matrix() const override					{ return Mat44::sTranslation(mLocalSpacePosition1); }
+	virtual Mat44				GetConstraintToBody2Matrix() const override					{ return Mat44::sRotationTranslation(mInvInitialOrientation, mLocalSpacePosition2); }
+
+	///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint)
+	inline Vec3					GetTotalLambdaPosition() const								{ return mPointConstraintPart.GetTotalLambda(); }
+	inline Vec3					GetTotalLambdaRotation() const								{ return mRotationConstraintPart.GetTotalLambda(); }
+
+private:
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	// Local space constraint positions
+	Vec3						mLocalSpacePosition1;
+	Vec3						mLocalSpacePosition2;
+
+	// Inverse of initial rotation from body 1 to body 2 in body 1 space
+	Quat						mInvInitialOrientation;
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	// The constraint parts
+	RotationEulerConstraintPart	mRotationConstraintPart;
+	PointConstraintPart			mPointConstraintPart;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/GearConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/GearConstraint.cpp
new file mode 100644
index 0000000000..b2a7284c9e
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/GearConstraint.cpp
@@ -0,0 +1,188 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/GearConstraint.h>
+#include <Jolt/Physics/Constraints/HingeConstraint.h>
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(GearConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(GearConstraintSettings, TwoBodyConstraintSettings)
+
+	JPH_ADD_ENUM_ATTRIBUTE(GearConstraintSettings, mSpace)
+	JPH_ADD_ATTRIBUTE(GearConstraintSettings, mHingeAxis1)
+	JPH_ADD_ATTRIBUTE(GearConstraintSettings, mHingeAxis2)
+	JPH_ADD_ATTRIBUTE(GearConstraintSettings, mRatio)
+}
+
+void GearConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	ConstraintSettings::SaveBinaryState(inStream);
+
+	inStream.Write(mSpace);
+	inStream.Write(mHingeAxis1);
+	inStream.Write(mHingeAxis2);
+	inStream.Write(mRatio);
+}
+
+void GearConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	ConstraintSettings::RestoreBinaryState(inStream);
+
+	inStream.Read(mSpace);
+	inStream.Read(mHingeAxis1);
+	inStream.Read(mHingeAxis2);
+	inStream.Read(mRatio);
+}
+
+TwoBodyConstraint *GearConstraintSettings::Create(Body &inBody1, Body &inBody2) const
+{
+	return new GearConstraint(inBody1, inBody2, *this);
+}
+
+GearConstraint::GearConstraint(Body &inBody1, Body &inBody2, const GearConstraintSettings &inSettings) :
+	TwoBodyConstraint(inBody1, inBody2, inSettings),
+	mLocalSpaceHingeAxis1(inSettings.mHingeAxis1),
+	mLocalSpaceHingeAxis2(inSettings.mHingeAxis2),
+	mRatio(inSettings.mRatio)
+{
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		// If all properties were specified in world space, take them to local space now
+		mLocalSpaceHingeAxis1 = inBody1.GetInverseCenterOfMassTransform().Multiply3x3(mLocalSpaceHingeAxis1).Normalized();
+		mLocalSpaceHingeAxis2 = inBody2.GetInverseCenterOfMassTransform().Multiply3x3(mLocalSpaceHingeAxis2).Normalized();
+	}
+}
+
+void GearConstraint::CalculateConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2)
+{
+	// Calculate world space normals
+	mWorldSpaceHingeAxis1 = inRotation1 * mLocalSpaceHingeAxis1;
+	mWorldSpaceHingeAxis2 = inRotation2 * mLocalSpaceHingeAxis2;
+
+	mGearConstraintPart.CalculateConstraintProperties(*mBody1, mWorldSpaceHingeAxis1, *mBody2, mWorldSpaceHingeAxis2, mRatio);
+}
+
+void GearConstraint::SetupVelocityConstraint(float inDeltaTime)
+{
+	// Calculate constraint properties that are constant while bodies don't move
+	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
+	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
+	CalculateConstraintProperties(rotation1, rotation2);
+}
+
+void GearConstraint::ResetWarmStart()
+{
+	mGearConstraintPart.Deactivate();
+}
+
+void GearConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
+{
+	// Warm starting: Apply previous frame impulse
+	mGearConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+}
+
+bool GearConstraint::SolveVelocityConstraint(float inDeltaTime)
+{
+	return mGearConstraintPart.SolveVelocityConstraint(*mBody1, mWorldSpaceHingeAxis1, *mBody2, mWorldSpaceHingeAxis2, mRatio);
+}
+
+bool GearConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
+{
+	if (mGear1Constraint == nullptr || mGear2Constraint == nullptr)
+		return false;
+
+	float gear1rot;
+	if (mGear1Constraint->GetSubType() == EConstraintSubType::Hinge)
+	{
+		gear1rot = StaticCast<HingeConstraint>(mGear1Constraint)->GetCurrentAngle();
+	}
+	else
+	{
+		JPH_ASSERT(false, "Unsupported");
+		return false;
+	}
+
+	float gear2rot;
+	if (mGear2Constraint->GetSubType() == EConstraintSubType::Hinge)
+	{
+		gear2rot = StaticCast<HingeConstraint>(mGear2Constraint)->GetCurrentAngle();
+	}
+	else
+	{
+		JPH_ASSERT(false, "Unsupported");
+		return false;
+	}
+
+	float error = CenterAngleAroundZero(fmod(gear1rot + mRatio * gear2rot, 2.0f * JPH_PI));
+	if (error == 0.0f)
+		return false;
+
+	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
+	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
+	CalculateConstraintProperties(rotation1, rotation2);
+	return mGearConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, error, inBaumgarte);
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void GearConstraint::DrawConstraint(DebugRenderer *inRenderer) const
+{
+	RMat44 transform1 = mBody1->GetCenterOfMassTransform();
+	RMat44 transform2 = mBody2->GetCenterOfMassTransform();
+
+	// Draw constraint axis
+	inRenderer->DrawArrow(transform1.GetTranslation(), transform1 * mLocalSpaceHingeAxis1, Color::sGreen, 0.01f);
+	inRenderer->DrawArrow(transform2.GetTranslation(), transform2 * mLocalSpaceHingeAxis2, Color::sBlue, 0.01f);
+}
+
+#endif // JPH_DEBUG_RENDERER
+
+void GearConstraint::SaveState(StateRecorder &inStream) const
+{
+	TwoBodyConstraint::SaveState(inStream);
+
+	mGearConstraintPart.SaveState(inStream);
+}
+
+void GearConstraint::RestoreState(StateRecorder &inStream)
+{
+	TwoBodyConstraint::RestoreState(inStream);
+
+	mGearConstraintPart.RestoreState(inStream);
+}
+
+Ref<ConstraintSettings> GearConstraint::GetConstraintSettings() const
+{
+	GearConstraintSettings *settings = new GearConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mHingeAxis1 = mLocalSpaceHingeAxis1;
+	settings->mHingeAxis2 = mLocalSpaceHingeAxis2;
+	settings->mRatio = mRatio;
+	return settings;
+}
+
+Mat44 GearConstraint::GetConstraintToBody1Matrix() const
+{
+	Vec3 perp = mLocalSpaceHingeAxis1.GetNormalizedPerpendicular();
+	return Mat44(Vec4(mLocalSpaceHingeAxis1, 0), Vec4(perp, 0), Vec4(mLocalSpaceHingeAxis1.Cross(perp), 0), Vec4(0, 0, 0, 1));
+}
+
+Mat44 GearConstraint::GetConstraintToBody2Matrix() const
+{
+	Vec3 perp = mLocalSpaceHingeAxis2.GetNormalizedPerpendicular();
+	return Mat44(Vec4(mLocalSpaceHingeAxis2, 0), Vec4(perp, 0), Vec4(mLocalSpaceHingeAxis2.Cross(perp), 0), Vec4(0, 0, 0, 1));
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/GearConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/GearConstraint.h
new file mode 100644
index 0000000000..134e74c7cc
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/GearConstraint.h
@@ -0,0 +1,116 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/GearConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Gear constraint settings
+class JPH_EXPORT GearConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, GearConstraintSettings)
+
+public:
+	// See: ConstraintSettings::SaveBinaryState
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+
+	/// Create an instance of this constraint.
+	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
+
+	/// Defines the ratio between the rotation of both gears
+	/// The ratio is defined as: Gear1Rotation(t) = -ratio * Gear2Rotation(t)
+	/// @param inNumTeethGear1 Number of teeth that body 1 has
+	/// @param inNumTeethGear2 Number of teeth that body 2 has
+	void						SetRatio(int inNumTeethGear1, int inNumTeethGear2)
+	{
+		mRatio = float(inNumTeethGear2) / float(inNumTeethGear1);
+	}
+
+	/// This determines in which space the constraint is setup, all properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	/// Body 1 constraint reference frame (space determined by mSpace).
+	Vec3						mHingeAxis1 = Vec3::sAxisX();
+
+	/// Body 2 constraint reference frame (space determined by mSpace)
+	Vec3						mHingeAxis2 = Vec3::sAxisX();
+
+	/// Ratio between both gears, see SetRatio.
+	float						mRatio = 1.0f;
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// A gear constraint constrains the rotation of body1 to the rotation of body 2 using a gear.
+/// Note that this constraint needs to be used in conjunction with a two hinge constraints.
+class JPH_EXPORT GearConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct gear constraint
+								GearConstraint(Body &inBody1, Body &inBody2, const GearConstraintSettings &inSettings);
+
+	// Generic interface of a constraint
+	virtual EConstraintSubType	GetSubType() const override								{ return EConstraintSubType::Gear; }
+	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override { /* Do nothing */ }
+	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
+	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
+	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
+	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void				DrawConstraint(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual void				SaveState(StateRecorder &inStream) const override;
+	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
+
+	// See: TwoBodyConstraint
+	virtual Mat44				GetConstraintToBody1Matrix() const override;
+	virtual Mat44				GetConstraintToBody2Matrix() const override;
+
+	/// The constraints that constrain both gears (2 hinges), optional and used to calculate the rotation error and fix numerical drift.
+	void						SetConstraints(const Constraint *inGear1, const Constraint *inGear2)	{ mGear1Constraint = inGear1; mGear2Constraint = inGear2; }
+
+	///@name Get Lagrange multiplier from last physics update (the angular impulse applied to satisfy the constraint)
+	inline float				GetTotalLambda() const									{ return mGearConstraintPart.GetTotalLambda(); }
+
+private:
+	// Internal helper function to calculate the values below
+	void						CalculateConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2);
+
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	// Local space hinge axis for body 1
+	Vec3						mLocalSpaceHingeAxis1;
+
+	// Local space hinge axis for body 2
+	Vec3						mLocalSpaceHingeAxis2;
+
+	// Ratio between gear 1 and 2
+	float						mRatio;
+
+	// The constraints that constrain both gears (2 hinges), optional and used to calculate the rotation error and fix numerical drift.
+	RefConst<Constraint>		mGear1Constraint;
+	RefConst<Constraint>		mGear2Constraint;
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	// World space hinge axis for body 1
+	Vec3						mWorldSpaceHingeAxis1;
+
+	// World space hinge axis for body 2
+	Vec3						mWorldSpaceHingeAxis2;
+
+	// The constraint parts
+	GearConstraintPart			mGearConstraintPart;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.cpp
new file mode 100644
index 0000000000..8246573629
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.cpp
@@ -0,0 +1,424 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/HingeConstraint.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h>
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(HingeConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(HingeConstraintSettings, TwoBodyConstraintSettings)
+
+	JPH_ADD_ENUM_ATTRIBUTE(HingeConstraintSettings, mSpace)
+	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mPoint1)
+	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mHingeAxis1)
+	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mNormalAxis1)
+	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mPoint2)
+	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mHingeAxis2)
+	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mNormalAxis2)
+	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mLimitsMin)
+	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mLimitsMax)
+	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mLimitsSpringSettings)
+	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mMaxFrictionTorque)
+	JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mMotorSettings)
+}
+
+void HingeConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	ConstraintSettings::SaveBinaryState(inStream);
+
+	inStream.Write(mSpace);
+	inStream.Write(mPoint1);
+	inStream.Write(mHingeAxis1);
+	inStream.Write(mNormalAxis1);
+	inStream.Write(mPoint2);
+	inStream.Write(mHingeAxis2);
+	inStream.Write(mNormalAxis2);
+	inStream.Write(mLimitsMin);
+	inStream.Write(mLimitsMax);
+	inStream.Write(mMaxFrictionTorque);
+	mLimitsSpringSettings.SaveBinaryState(inStream);
+	mMotorSettings.SaveBinaryState(inStream);
+}
+
+void HingeConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	ConstraintSettings::RestoreBinaryState(inStream);
+
+	inStream.Read(mSpace);
+	inStream.Read(mPoint1);
+	inStream.Read(mHingeAxis1);
+	inStream.Read(mNormalAxis1);
+	inStream.Read(mPoint2);
+	inStream.Read(mHingeAxis2);
+	inStream.Read(mNormalAxis2);
+	inStream.Read(mLimitsMin);
+	inStream.Read(mLimitsMax);
+	inStream.Read(mMaxFrictionTorque);
+	mLimitsSpringSettings.RestoreBinaryState(inStream);
+	mMotorSettings.RestoreBinaryState(inStream);}
+
+TwoBodyConstraint *HingeConstraintSettings::Create(Body &inBody1, Body &inBody2) const
+{
+	return new HingeConstraint(inBody1, inBody2, *this);
+}
+
+HingeConstraint::HingeConstraint(Body &inBody1, Body &inBody2, const HingeConstraintSettings &inSettings) :
+	TwoBodyConstraint(inBody1, inBody2, inSettings),
+	mMaxFrictionTorque(inSettings.mMaxFrictionTorque),
+	mMotorSettings(inSettings.mMotorSettings)
+{
+	// Store limits
+	JPH_ASSERT(inSettings.mLimitsMin != inSettings.mLimitsMax || inSettings.mLimitsSpringSettings.mFrequency > 0.0f, "Better use a fixed constraint in this case");
+	SetLimits(inSettings.mLimitsMin, inSettings.mLimitsMax);
+
+	// Store inverse of initial rotation from body 1 to body 2 in body 1 space
+	mInvInitialOrientation = RotationEulerConstraintPart::sGetInvInitialOrientationXZ(inSettings.mNormalAxis1, inSettings.mHingeAxis1, inSettings.mNormalAxis2, inSettings.mHingeAxis2);
+
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		// If all properties were specified in world space, take them to local space now
+		RMat44 inv_transform1 = inBody1.GetInverseCenterOfMassTransform();
+		mLocalSpacePosition1 = Vec3(inv_transform1 * inSettings.mPoint1);
+		mLocalSpaceHingeAxis1 = inv_transform1.Multiply3x3(inSettings.mHingeAxis1).Normalized();
+		mLocalSpaceNormalAxis1 = inv_transform1.Multiply3x3(inSettings.mNormalAxis1).Normalized();
+
+		RMat44 inv_transform2 = inBody2.GetInverseCenterOfMassTransform();
+		mLocalSpacePosition2 = Vec3(inv_transform2 * inSettings.mPoint2);
+		mLocalSpaceHingeAxis2 = inv_transform2.Multiply3x3(inSettings.mHingeAxis2).Normalized();
+		mLocalSpaceNormalAxis2 = inv_transform2.Multiply3x3(inSettings.mNormalAxis2).Normalized();
+
+		// Constraints were specified in world space, so we should have replaced c1 with q10^-1 c1 and c2 with q20^-1 c2
+		// => r0^-1 = (q20^-1 c2) (q10^-1 c1)^1 = q20^-1 (c2 c1^-1) q10
+		mInvInitialOrientation = inBody2.GetRotation().Conjugated() * mInvInitialOrientation * inBody1.GetRotation();
+	}
+	else
+	{
+		mLocalSpacePosition1 = Vec3(inSettings.mPoint1);
+		mLocalSpaceHingeAxis1 = inSettings.mHingeAxis1;
+		mLocalSpaceNormalAxis1 = inSettings.mNormalAxis1;
+
+		mLocalSpacePosition2 = Vec3(inSettings.mPoint2);
+		mLocalSpaceHingeAxis2 = inSettings.mHingeAxis2;
+		mLocalSpaceNormalAxis2 = inSettings.mNormalAxis2;
+	}
+
+	// Store spring settings
+	SetLimitsSpringSettings(inSettings.mLimitsSpringSettings);
+}
+
+void HingeConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
+{
+	if (mBody1->GetID() == inBodyID)
+		mLocalSpacePosition1 -= inDeltaCOM;
+	else if (mBody2->GetID() == inBodyID)
+		mLocalSpacePosition2 -= inDeltaCOM;
+}
+
+float HingeConstraint::GetCurrentAngle() const
+{
+	// See: CalculateA1AndTheta
+	Quat rotation1 = mBody1->GetRotation();
+	Quat diff = mBody2->GetRotation() * mInvInitialOrientation * rotation1.Conjugated();
+	return diff.GetRotationAngle(rotation1 * mLocalSpaceHingeAxis1);
+}
+
+void HingeConstraint::SetLimits(float inLimitsMin, float inLimitsMax)
+{
+	JPH_ASSERT(inLimitsMin <= 0.0f && inLimitsMin >= -JPH_PI);
+	JPH_ASSERT(inLimitsMax >= 0.0f && inLimitsMax <= JPH_PI);
+	mLimitsMin = inLimitsMin;
+	mLimitsMax = inLimitsMax;
+	mHasLimits = mLimitsMin > -JPH_PI && mLimitsMax < JPH_PI;
+}
+
+void HingeConstraint::CalculateA1AndTheta()
+{
+	if (mHasLimits || mMotorState != EMotorState::Off || mMaxFrictionTorque > 0.0f)
+	{
+		Quat rotation1 = mBody1->GetRotation();
+
+		// Calculate relative rotation in world space
+		//
+		// The rest rotation is:
+		//
+		// q2 = q1 r0
+		//
+		// But the actual rotation is
+		//
+		// q2 = diff q1 r0
+		// <=> diff = q2 r0^-1 q1^-1
+		//
+		// Where:
+		// q1 = current rotation of body 1
+		// q2 = current rotation of body 2
+		// diff = relative rotation in world space
+		Quat diff = mBody2->GetRotation() * mInvInitialOrientation * rotation1.Conjugated();
+
+		// Calculate hinge axis in world space
+		mA1 = rotation1 * mLocalSpaceHingeAxis1;
+
+		// Get rotation angle around the hinge axis
+		mTheta = diff.GetRotationAngle(mA1);
+	}
+}
+
+void HingeConstraint::CalculateRotationLimitsConstraintProperties(float inDeltaTime)
+{
+	// Apply constraint if outside of limits
+	if (mHasLimits && (mTheta <= mLimitsMin || mTheta >= mLimitsMax))
+		mRotationLimitsConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mA1, 0.0f, GetSmallestAngleToLimit(), mLimitsSpringSettings);
+	else
+		mRotationLimitsConstraintPart.Deactivate();
+}
+
+void HingeConstraint::CalculateMotorConstraintProperties(float inDeltaTime)
+{
+	switch (mMotorState)
+	{
+	case EMotorState::Off:
+		if (mMaxFrictionTorque > 0.0f)
+			mMotorConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, mA1);
+		else
+			mMotorConstraintPart.Deactivate();
+		break;
+
+	case EMotorState::Velocity:
+		mMotorConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, mA1, -mTargetAngularVelocity);
+		break;
+
+	case EMotorState::Position:
+		if (mMotorSettings.mSpringSettings.HasStiffness())
+			mMotorConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mA1, 0.0f, CenterAngleAroundZero(mTheta - mTargetAngle), mMotorSettings.mSpringSettings);
+		else
+			mMotorConstraintPart.Deactivate();
+		break;
+	}
+}
+
+void HingeConstraint::SetupVelocityConstraint(float inDeltaTime)
+{
+	// Cache constraint values that are valid until the bodies move
+	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
+	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
+	mPointConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, mLocalSpacePosition1, *mBody2, rotation2, mLocalSpacePosition2);
+	mRotationConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, rotation1.Multiply3x3(mLocalSpaceHingeAxis1), *mBody2, rotation2, rotation2.Multiply3x3(mLocalSpaceHingeAxis2));
+	CalculateA1AndTheta();
+	CalculateRotationLimitsConstraintProperties(inDeltaTime);
+	CalculateMotorConstraintProperties(inDeltaTime);
+}
+
+void HingeConstraint::ResetWarmStart()
+{
+	mMotorConstraintPart.Deactivate();
+	mPointConstraintPart.Deactivate();
+	mRotationConstraintPart.Deactivate();
+	mRotationLimitsConstraintPart.Deactivate();
+}
+
+void HingeConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
+{
+	// Warm starting: Apply previous frame impulse
+	mMotorConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+	mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+	mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+	mRotationLimitsConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+}
+
+float HingeConstraint::GetSmallestAngleToLimit() const
+{
+	float dist_to_min = CenterAngleAroundZero(mTheta - mLimitsMin);
+	float dist_to_max = CenterAngleAroundZero(mTheta - mLimitsMax);
+	return abs(dist_to_min) < abs(dist_to_max)? dist_to_min : dist_to_max;
+}
+
+bool HingeConstraint::IsMinLimitClosest() const
+{
+	float dist_to_min = CenterAngleAroundZero(mTheta - mLimitsMin);
+	float dist_to_max = CenterAngleAroundZero(mTheta - mLimitsMax);
+	return abs(dist_to_min) < abs(dist_to_max);
+}
+
+bool HingeConstraint::SolveVelocityConstraint(float inDeltaTime)
+{
+	// Solve motor
+	bool motor = false;
+	if (mMotorConstraintPart.IsActive())
+	{
+		switch (mMotorState)
+		{
+		case EMotorState::Off:
+			{
+				float max_lambda = mMaxFrictionTorque * inDeltaTime;
+				motor = mMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mA1, -max_lambda, max_lambda);
+				break;
+			}
+
+		case EMotorState::Velocity:
+		case EMotorState::Position:
+			motor = mMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mA1, inDeltaTime * mMotorSettings.mMinTorqueLimit, inDeltaTime * mMotorSettings.mMaxTorqueLimit);
+			break;
+		}
+	}
+
+	// Solve point constraint
+	bool pos = mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+
+	// Solve rotation constraint
+	bool rot = mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+
+	// Solve rotation limits
+	bool limit = false;
+	if (mRotationLimitsConstraintPart.IsActive())
+	{
+		float min_lambda, max_lambda;
+		if (mLimitsMin == mLimitsMax)
+		{
+			min_lambda = -FLT_MAX;
+			max_lambda = FLT_MAX;
+		}
+		else if (IsMinLimitClosest())
+		{
+			min_lambda = 0.0f;
+			max_lambda = FLT_MAX;
+		}
+		else
+		{
+			min_lambda = -FLT_MAX;
+			max_lambda = 0.0f;
+		}
+		limit = mRotationLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mA1, min_lambda, max_lambda);
+	}
+
+	return motor || pos || rot || limit;
+}
+
+bool HingeConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
+{
+	// Motor operates on velocities only, don't call SolvePositionConstraint
+
+	// Solve point constraint
+	mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2);
+	bool pos = mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte);
+
+	// Solve rotation constraint
+	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); // Note that previous call to GetRotation() is out of date since the rotation has changed
+	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
+	mRotationConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, rotation1.Multiply3x3(mLocalSpaceHingeAxis1), *mBody2, rotation2, rotation2.Multiply3x3(mLocalSpaceHingeAxis2));
+	bool rot = mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte);
+
+	// Solve rotation limits
+	bool limit = false;
+	if (mHasLimits && mLimitsSpringSettings.mFrequency <= 0.0f)
+	{
+		CalculateA1AndTheta();
+		CalculateRotationLimitsConstraintProperties(inDeltaTime);
+		if (mRotationLimitsConstraintPart.IsActive())
+			limit = mRotationLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, GetSmallestAngleToLimit(), inBaumgarte);
+	}
+
+	return pos || rot || limit;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void HingeConstraint::DrawConstraint(DebugRenderer *inRenderer) const
+{
+	RMat44 transform1 = mBody1->GetCenterOfMassTransform();
+	RMat44 transform2 = mBody2->GetCenterOfMassTransform();
+
+	// Draw constraint
+	RVec3 constraint_pos1 = transform1 * mLocalSpacePosition1;
+	inRenderer->DrawMarker(constraint_pos1, Color::sRed, 0.1f);
+	inRenderer->DrawLine(constraint_pos1, transform1 * (mLocalSpacePosition1 + mDrawConstraintSize * mLocalSpaceHingeAxis1), Color::sRed);
+
+	RVec3 constraint_pos2 = transform2 * mLocalSpacePosition2;
+	inRenderer->DrawMarker(constraint_pos2, Color::sGreen, 0.1f);
+	inRenderer->DrawLine(constraint_pos2, transform2 * (mLocalSpacePosition2 + mDrawConstraintSize * mLocalSpaceHingeAxis2), Color::sGreen);
+	inRenderer->DrawLine(constraint_pos2, transform2 * (mLocalSpacePosition2 + mDrawConstraintSize * mLocalSpaceNormalAxis2), Color::sWhite);
+}
+
+void HingeConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const
+{
+	if (mHasLimits && mLimitsMax > mLimitsMin)
+	{
+		// Get constraint properties in world space
+		RMat44 transform1 = mBody1->GetCenterOfMassTransform();
+		RVec3 position1 = transform1 * mLocalSpacePosition1;
+		Vec3 hinge_axis1 = transform1.Multiply3x3(mLocalSpaceHingeAxis1);
+		Vec3 normal_axis1 = transform1.Multiply3x3(mLocalSpaceNormalAxis1);
+
+		inRenderer->DrawPie(position1, mDrawConstraintSize, hinge_axis1, normal_axis1, mLimitsMin, mLimitsMax, Color::sPurple, DebugRenderer::ECastShadow::Off);
+	}
+}
+#endif // JPH_DEBUG_RENDERER
+
+void HingeConstraint::SaveState(StateRecorder &inStream) const
+{
+	TwoBodyConstraint::SaveState(inStream);
+
+	mMotorConstraintPart.SaveState(inStream);
+	mRotationConstraintPart.SaveState(inStream);
+	mPointConstraintPart.SaveState(inStream);
+	mRotationLimitsConstraintPart.SaveState(inStream);
+
+	inStream.Write(mMotorState);
+	inStream.Write(mTargetAngularVelocity);
+	inStream.Write(mTargetAngle);
+}
+
+void HingeConstraint::RestoreState(StateRecorder &inStream)
+{
+	TwoBodyConstraint::RestoreState(inStream);
+
+	mMotorConstraintPart.RestoreState(inStream);
+	mRotationConstraintPart.RestoreState(inStream);
+	mPointConstraintPart.RestoreState(inStream);
+	mRotationLimitsConstraintPart.RestoreState(inStream);
+
+	inStream.Read(mMotorState);
+	inStream.Read(mTargetAngularVelocity);
+	inStream.Read(mTargetAngle);
+}
+
+
+Ref<ConstraintSettings> HingeConstraint::GetConstraintSettings() const
+{
+	HingeConstraintSettings *settings = new HingeConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mPoint1 = RVec3(mLocalSpacePosition1);
+	settings->mHingeAxis1 = mLocalSpaceHingeAxis1;
+	settings->mNormalAxis1 = mLocalSpaceNormalAxis1;
+	settings->mPoint2 = RVec3(mLocalSpacePosition2);
+	settings->mHingeAxis2 = mLocalSpaceHingeAxis2;
+	settings->mNormalAxis2 = mLocalSpaceNormalAxis2;
+	settings->mLimitsMin = mLimitsMin;
+	settings->mLimitsMax = mLimitsMax;
+	settings->mLimitsSpringSettings = mLimitsSpringSettings;
+	settings->mMaxFrictionTorque = mMaxFrictionTorque;
+	settings->mMotorSettings = mMotorSettings;
+	return settings;
+}
+
+Mat44 HingeConstraint::GetConstraintToBody1Matrix() const
+{
+	return Mat44(Vec4(mLocalSpaceHingeAxis1, 0), Vec4(mLocalSpaceNormalAxis1, 0), Vec4(mLocalSpaceHingeAxis1.Cross(mLocalSpaceNormalAxis1), 0), Vec4(mLocalSpacePosition1, 1));
+}
+
+Mat44 HingeConstraint::GetConstraintToBody2Matrix() const
+{
+	return Mat44(Vec4(mLocalSpaceHingeAxis2, 0), Vec4(mLocalSpaceNormalAxis2, 0), Vec4(mLocalSpaceHingeAxis2.Cross(mLocalSpaceNormalAxis2), 0), Vec4(mLocalSpacePosition2, 1));
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.h
new file mode 100644
index 0000000000..691bb1f5f4
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/HingeConstraint.h
@@ -0,0 +1,200 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Physics/Constraints/MotorSettings.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/HingeRotationConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Hinge constraint settings, used to create a hinge constraint
+class JPH_EXPORT HingeConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, HingeConstraintSettings)
+
+public:
+	// See: ConstraintSettings::SaveBinaryState
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+
+	/// Create an instance of this constraint
+	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
+
+	/// This determines in which space the constraint is setup, all properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	/// Body 1 constraint reference frame (space determined by mSpace).
+	/// Hinge axis is the axis where rotation is allowed.
+	/// When the normal axis of both bodies align in world space, the hinge angle is defined to be 0.
+	/// mHingeAxis1 and mNormalAxis1 should be perpendicular. mHingeAxis2 and mNormalAxis2 should also be perpendicular.
+	/// If you configure the joint in world space and create both bodies with a relative rotation you want to be defined as zero,
+	/// you can simply set mHingeAxis1 = mHingeAxis2 and mNormalAxis1 = mNormalAxis2.
+	RVec3						mPoint1 = RVec3::sZero();
+	Vec3						mHingeAxis1 = Vec3::sAxisY();
+	Vec3						mNormalAxis1 = Vec3::sAxisX();
+
+	/// Body 2 constraint reference frame (space determined by mSpace)
+	RVec3						mPoint2 = RVec3::sZero();
+	Vec3						mHingeAxis2 = Vec3::sAxisY();
+	Vec3						mNormalAxis2 = Vec3::sAxisX();
+
+	/// Rotation around the hinge axis will be limited between [mLimitsMin, mLimitsMax] where mLimitsMin e [-pi, 0] and mLimitsMax e [0, pi].
+	/// Both angles are in radians.
+	float						mLimitsMin = -JPH_PI;
+	float						mLimitsMax = JPH_PI;
+
+	/// When enabled, this makes the limits soft. When the constraint exceeds the limits, a spring force will pull it back.
+	SpringSettings				mLimitsSpringSettings;
+
+	/// Maximum amount of torque (N m) to apply as friction when the constraint is not powered by a motor
+	float						mMaxFrictionTorque = 0.0f;
+
+	/// In case the constraint is powered, this determines the motor settings around the hinge axis
+	MotorSettings				mMotorSettings;
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// A hinge constraint constrains 2 bodies on a single point and allows only a single axis of rotation
+class JPH_EXPORT HingeConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct hinge constraint
+								HingeConstraint(Body &inBody1, Body &inBody2, const HingeConstraintSettings &inSettings);
+
+	// Generic interface of a constraint
+	virtual EConstraintSubType	GetSubType() const override								{ return EConstraintSubType::Hinge; }
+	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
+	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
+	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
+	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
+	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void				DrawConstraint(DebugRenderer *inRenderer) const override;
+	virtual void				DrawConstraintLimits(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual void				SaveState(StateRecorder &inStream) const override;
+	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
+
+	// See: TwoBodyConstraint
+	virtual Mat44				GetConstraintToBody1Matrix() const override;
+	virtual Mat44				GetConstraintToBody2Matrix() const override;
+
+	/// Get the attachment point for body 1 relative to body 1 COM (transform by Body::GetCenterOfMassTransform to take to world space)
+	inline Vec3					GetLocalSpacePoint1() const								{ return mLocalSpacePosition1; }
+
+	/// Get the attachment point for body 2 relative to body 2 COM (transform by Body::GetCenterOfMassTransform to take to world space)
+	inline Vec3					GetLocalSpacePoint2() const								{ return mLocalSpacePosition2; }
+
+	// Local space hinge directions (transform direction by Body::GetCenterOfMassTransform to take to world space)
+	Vec3						GetLocalSpaceHingeAxis1() const							{ return mLocalSpaceHingeAxis1; }
+	Vec3						GetLocalSpaceHingeAxis2() const							{ return mLocalSpaceHingeAxis2; }
+
+	// Local space normal directions (transform direction by Body::GetCenterOfMassTransform to take to world space)
+	Vec3						GetLocalSpaceNormalAxis1() const						{ return mLocalSpaceNormalAxis1; }
+	Vec3						GetLocalSpaceNormalAxis2() const						{ return mLocalSpaceNormalAxis2; }
+
+	/// Get the current rotation angle from the rest position
+	float						GetCurrentAngle() const;
+
+	// Friction control
+	void						SetMaxFrictionTorque(float inFrictionTorque)			{ mMaxFrictionTorque = inFrictionTorque; }
+	float						GetMaxFrictionTorque() const							{ return mMaxFrictionTorque; }
+
+	// Motor settings
+	MotorSettings &				GetMotorSettings()										{ return mMotorSettings; }
+	const MotorSettings &		GetMotorSettings() const								{ return mMotorSettings; }
+
+	// Motor controls
+	void						SetMotorState(EMotorState inState)						{ JPH_ASSERT(inState == EMotorState::Off || mMotorSettings.IsValid()); mMotorState = inState; }
+	EMotorState					GetMotorState() const									{ return mMotorState; }
+	void						SetTargetAngularVelocity(float inAngularVelocity)		{ mTargetAngularVelocity = inAngularVelocity; } ///< rad/s
+	float						GetTargetAngularVelocity() const						{ return mTargetAngularVelocity; }
+	void						SetTargetAngle(float inAngle)							{ mTargetAngle = mHasLimits? Clamp(inAngle, mLimitsMin, mLimitsMax) : inAngle; } ///< rad
+	float						GetTargetAngle() const									{ return mTargetAngle; }
+
+	/// Update the rotation limits of the hinge, value in radians (see HingeConstraintSettings)
+	void						SetLimits(float inLimitsMin, float inLimitsMax);
+	float						GetLimitsMin() const									{ return mLimitsMin; }
+	float						GetLimitsMax() const									{ return mLimitsMax; }
+	bool						HasLimits() const										{ return mHasLimits; }
+
+	/// Update the limits spring settings
+	const SpringSettings &		GetLimitsSpringSettings() const							{ return mLimitsSpringSettings; }
+	SpringSettings &			GetLimitsSpringSettings()								{ return mLimitsSpringSettings; }
+	void						SetLimitsSpringSettings(const SpringSettings &inLimitsSpringSettings) { mLimitsSpringSettings = inLimitsSpringSettings; }
+
+	///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint)
+	inline Vec3					GetTotalLambdaPosition() const							{ return mPointConstraintPart.GetTotalLambda(); }
+	inline Vector<2>			GetTotalLambdaRotation() const							{ return mRotationConstraintPart.GetTotalLambda(); }
+	inline float				GetTotalLambdaRotationLimits() const					{ return mRotationLimitsConstraintPart.GetTotalLambda(); }
+	inline float				GetTotalLambdaMotor() const								{ return mMotorConstraintPart.GetTotalLambda(); }
+
+private:
+	// Internal helper function to calculate the values below
+	void						CalculateA1AndTheta();
+	void						CalculateRotationLimitsConstraintProperties(float inDeltaTime);
+	void						CalculateMotorConstraintProperties(float inDeltaTime);
+	inline float				GetSmallestAngleToLimit() const;
+	inline bool					IsMinLimitClosest() const;
+
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	// Local space constraint positions
+	Vec3						mLocalSpacePosition1;
+	Vec3						mLocalSpacePosition2;
+
+	// Local space hinge directions
+	Vec3						mLocalSpaceHingeAxis1;
+	Vec3						mLocalSpaceHingeAxis2;
+
+	// Local space normal direction (direction relative to which to draw constraint limits)
+	Vec3						mLocalSpaceNormalAxis1;
+	Vec3						mLocalSpaceNormalAxis2;
+
+	// Inverse of initial relative orientation between bodies (which defines hinge angle = 0)
+	Quat						mInvInitialOrientation;
+
+	// Hinge limits
+	bool						mHasLimits;
+	float						mLimitsMin;
+	float						mLimitsMax;
+
+	// Soft constraint limits
+	SpringSettings				mLimitsSpringSettings;
+
+	// Friction
+	float						mMaxFrictionTorque;
+
+	// Motor controls
+	MotorSettings				mMotorSettings;
+	EMotorState					mMotorState = EMotorState::Off;
+	float						mTargetAngularVelocity = 0.0f;
+	float						mTargetAngle = 0.0f;
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	// Current rotation around the hinge axis
+	float						mTheta = 0.0f;
+
+	// World space hinge axis for body 1
+	Vec3						mA1;
+
+	// The constraint parts
+	PointConstraintPart			mPointConstraintPart;
+	HingeRotationConstraintPart mRotationConstraintPart;
+	AngleConstraintPart			mRotationLimitsConstraintPart;
+	AngleConstraintPart			mMotorConstraintPart;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/MotorSettings.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/MotorSettings.cpp
new file mode 100644
index 0000000000..e4daecdd7d
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/MotorSettings.cpp
@@ -0,0 +1,43 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/MotorSettings.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(MotorSettings)
+{
+	JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(MotorSettings, mSpringSettings.mMode, "mSpringMode")
+	JPH_ADD_ATTRIBUTE_WITH_ALIAS(MotorSettings, mSpringSettings.mFrequency, "mFrequency") // Renaming attributes to stay compatible with old versions of the library
+	JPH_ADD_ATTRIBUTE_WITH_ALIAS(MotorSettings, mSpringSettings.mDamping, "mDamping")
+	JPH_ADD_ATTRIBUTE(MotorSettings, mMinForceLimit)
+	JPH_ADD_ATTRIBUTE(MotorSettings, mMaxForceLimit)
+	JPH_ADD_ATTRIBUTE(MotorSettings, mMinTorqueLimit)
+	JPH_ADD_ATTRIBUTE(MotorSettings, mMaxTorqueLimit)
+}
+
+void MotorSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	mSpringSettings.SaveBinaryState(inStream);
+	inStream.Write(mMinForceLimit);
+	inStream.Write(mMaxForceLimit);
+	inStream.Write(mMinTorqueLimit);
+	inStream.Write(mMaxTorqueLimit);
+}
+
+void MotorSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	mSpringSettings.RestoreBinaryState(inStream);
+	inStream.Read(mMinForceLimit);
+	inStream.Read(mMaxForceLimit);
+	inStream.Read(mMinTorqueLimit);
+	inStream.Read(mMaxTorqueLimit);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/MotorSettings.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/MotorSettings.h
new file mode 100644
index 0000000000..601fa3dc3f
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/MotorSettings.h
@@ -0,0 +1,66 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/Reference.h>
+#include <Jolt/ObjectStream/SerializableObject.h>
+#include <Jolt/Physics/Constraints/SpringSettings.h>
+
+JPH_NAMESPACE_BEGIN
+
+class StreamIn;
+class StreamOut;
+
+enum class EMotorState
+{
+	Off,																///< Motor is off
+	Velocity,															///< Motor will drive to target velocity
+	Position															///< Motor will drive to target position
+};
+
+/// Class that contains the settings for a constraint motor.
+/// See the main page of the API documentation for more information on how to configure a motor.
+class JPH_EXPORT MotorSettings
+{
+	JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, MotorSettings)
+
+public:
+	/// Constructor
+							MotorSettings() = default;
+							MotorSettings(const MotorSettings &) = default;
+	MotorSettings &			operator = (const MotorSettings &) = default;
+							MotorSettings(float inFrequency, float inDamping) : mSpringSettings(ESpringMode::FrequencyAndDamping, inFrequency, inDamping) { JPH_ASSERT(IsValid()); }
+							MotorSettings(float inFrequency, float inDamping, float inForceLimit, float inTorqueLimit) : mSpringSettings(ESpringMode::FrequencyAndDamping, inFrequency, inDamping), mMinForceLimit(-inForceLimit), mMaxForceLimit(inForceLimit), mMinTorqueLimit(-inTorqueLimit), mMaxTorqueLimit(inTorqueLimit) { JPH_ASSERT(IsValid()); }
+
+	/// Set asymmetric force limits
+	void					SetForceLimits(float inMin, float inMax)	{ JPH_ASSERT(inMin <= inMax); mMinForceLimit = inMin; mMaxForceLimit = inMax; }
+
+	/// Set asymmetric torque limits
+	void					SetTorqueLimits(float inMin, float inMax)	{ JPH_ASSERT(inMin <= inMax); mMinTorqueLimit = inMin; mMaxTorqueLimit = inMax; }
+
+	/// Set symmetric force limits
+	void					SetForceLimit(float inLimit)				{ mMinForceLimit = -inLimit; mMaxForceLimit = inLimit; }
+
+	/// Set symmetric torque limits
+	void					SetTorqueLimit(float inLimit)				{ mMinTorqueLimit = -inLimit; mMaxTorqueLimit = inLimit; }
+
+	/// Check if settings are valid
+	bool					IsValid() const								{ return mSpringSettings.mFrequency >= 0.0f && mSpringSettings.mDamping >= 0.0f && mMinForceLimit <= mMaxForceLimit && mMinTorqueLimit <= mMaxTorqueLimit; }
+
+	/// Saves the contents of the motor settings in binary form to inStream.
+	void					SaveBinaryState(StreamOut &inStream) const;
+
+	/// Restores contents from the binary stream inStream.
+	void					RestoreBinaryState(StreamIn &inStream);
+
+	// Settings
+	SpringSettings			mSpringSettings { ESpringMode::FrequencyAndDamping, 2.0f, 1.0f }; ///< Settings for the spring that is used to drive to the position target (not used when motor is a velocity motor).
+	float					mMinForceLimit = -FLT_MAX;					///< Minimum force to apply in case of a linear constraint (N). Usually this is -mMaxForceLimit unless you want a motor that can e.g. push but not pull. Not used when motor is an angular motor.
+	float					mMaxForceLimit = FLT_MAX;					///< Maximum force to apply in case of a linear constraint (N). Not used when motor is an angular motor.
+	float					mMinTorqueLimit = -FLT_MAX;					///< Minimum torque to apply in case of a angular constraint (N m). Usually this is -mMaxTorqueLimit unless you want a motor that can e.g. push but not pull. Not used when motor is a position motor.
+	float					mMaxTorqueLimit = FLT_MAX;					///< Maximum torque to apply in case of a angular constraint (N m). Not used when motor is a position motor.
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraint.cpp
new file mode 100644
index 0000000000..e5b6eb7da0
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraint.cpp
@@ -0,0 +1,458 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/PathConstraint.h>
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Core/StringTools.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PathConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(PathConstraintSettings, TwoBodyConstraintSettings)
+
+	JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPath)
+	JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPathPosition)
+	JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPathRotation)
+	JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPathFraction)
+	JPH_ADD_ATTRIBUTE(PathConstraintSettings, mMaxFrictionForce)
+	JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPositionMotorSettings)
+	JPH_ADD_ENUM_ATTRIBUTE(PathConstraintSettings, mRotationConstraintType)
+}
+
+void PathConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	ConstraintSettings::SaveBinaryState(inStream);
+
+	mPath->SaveBinaryState(inStream);
+	inStream.Write(mPathPosition);
+	inStream.Write(mPathRotation);
+	inStream.Write(mPathFraction);
+	inStream.Write(mMaxFrictionForce);
+	inStream.Write(mRotationConstraintType);
+	mPositionMotorSettings.SaveBinaryState(inStream);
+}
+
+void PathConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	ConstraintSettings::RestoreBinaryState(inStream);
+
+	PathConstraintPath::PathResult result = PathConstraintPath::sRestoreFromBinaryState(inStream);
+	if (!result.HasError())
+		mPath = result.Get();
+	inStream.Read(mPathPosition);
+	inStream.Read(mPathRotation);
+	inStream.Read(mPathFraction);
+	inStream.Read(mMaxFrictionForce);
+	inStream.Read(mRotationConstraintType);
+	mPositionMotorSettings.RestoreBinaryState(inStream);
+}
+
+TwoBodyConstraint *PathConstraintSettings::Create(Body &inBody1, Body &inBody2) const
+{
+	return new PathConstraint(inBody1, inBody2, *this);
+}
+
+PathConstraint::PathConstraint(Body &inBody1, Body &inBody2, const PathConstraintSettings &inSettings) :
+	TwoBodyConstraint(inBody1, inBody2, inSettings),
+	mRotationConstraintType(inSettings.mRotationConstraintType),
+	mMaxFrictionForce(inSettings.mMaxFrictionForce),
+	mPositionMotorSettings(inSettings.mPositionMotorSettings)
+{
+	// Calculate transform that takes us from the path start to center of mass space of body 1
+	mPathToBody1 = Mat44::sRotationTranslation(inSettings.mPathRotation, inSettings.mPathPosition - inBody1.GetShape()->GetCenterOfMass());
+
+	SetPath(inSettings.mPath, inSettings.mPathFraction);
+}
+
+void PathConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
+{
+	if (mBody1->GetID() == inBodyID)
+		mPathToBody1.SetTranslation(mPathToBody1.GetTranslation() - inDeltaCOM);
+	else if (mBody2->GetID() == inBodyID)
+		mPathToBody2.SetTranslation(mPathToBody2.GetTranslation() - inDeltaCOM);
+}
+
+void PathConstraint::SetPath(const PathConstraintPath *inPath, float inPathFraction)
+{
+	mPath = inPath;
+	mPathFraction = inPathFraction;
+
+	if (mPath != nullptr)
+	{
+		// Get the point on the path for this fraction
+		Vec3 path_point, path_tangent, path_normal, path_binormal;
+		mPath->GetPointOnPath(mPathFraction, path_point, path_tangent, path_normal, path_binormal);
+
+		// Construct the matrix that takes us from the closest point on the path to body 2 center of mass space
+		Mat44 closest_point_to_path(Vec4(path_tangent, 0), Vec4(path_binormal, 0), Vec4(path_normal, 0), Vec4(path_point, 1));
+		Mat44 cp_to_body1 = mPathToBody1 * closest_point_to_path;
+		mPathToBody2 = (mBody2->GetInverseCenterOfMassTransform() * mBody1->GetCenterOfMassTransform()).ToMat44() * cp_to_body1;
+
+		// Calculate initial orientation
+		if (mRotationConstraintType == EPathRotationConstraintType::FullyConstrained)
+			mInvInitialOrientation = RotationEulerConstraintPart::sGetInvInitialOrientation(*mBody1, *mBody2);
+	}
+}
+
+void PathConstraint::CalculateConstraintProperties(float inDeltaTime)
+{
+	// Get transforms of body 1 and 2
+	RMat44 transform1 = mBody1->GetCenterOfMassTransform();
+	RMat44 transform2 = mBody2->GetCenterOfMassTransform();
+
+	// Get the transform of the path transform as seen from body 1 in world space
+	RMat44 path_to_world_1 = transform1 * mPathToBody1;
+
+	// Get the transform of from the point on path that body 2 is attached to in world space
+	RMat44 path_to_world_2 = transform2 * mPathToBody2;
+
+	// Calculate new closest point on path
+	RVec3 position2 = path_to_world_2.GetTranslation();
+	Vec3 position2_local_to_path = Vec3(path_to_world_1.InversedRotationTranslation() * position2);
+	mPathFraction = mPath->GetClosestPoint(position2_local_to_path, mPathFraction);
+
+	// Get the point on the path for this fraction
+	Vec3 path_point, path_tangent, path_normal, path_binormal;
+	mPath->GetPointOnPath(mPathFraction, path_point, path_tangent, path_normal, path_binormal);
+
+	// Calculate R1 and R2
+	RVec3 path_point_ws = path_to_world_1 * path_point;
+	mR1 = Vec3(path_point_ws - mBody1->GetCenterOfMassPosition());
+	mR2 = Vec3(position2 - mBody2->GetCenterOfMassPosition());
+
+	// Calculate U = X2 + R2 - X1 - R1
+	mU = Vec3(position2 - path_point_ws);
+
+	// Calculate world space normals
+	mPathNormal = path_to_world_1.Multiply3x3(path_normal);
+	mPathBinormal = path_to_world_1.Multiply3x3(path_binormal);
+
+	// Calculate slide axis
+	mPathTangent = path_to_world_1.Multiply3x3(path_tangent);
+
+	// Prepare constraint part for position constraint to slide along the path
+	mPositionConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), mR1 + mU, *mBody2, transform2.GetRotation(), mR2, mPathNormal, mPathBinormal);
+
+	// Check if closest point is on the boundary of the path and if so apply limit
+	if (!mPath->IsLooping() && (mPathFraction <= 0.0f || mPathFraction >= mPath->GetPathMaxFraction()))
+		mPositionLimitsConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mPathTangent);
+	else
+		mPositionLimitsConstraintPart.Deactivate();
+
+	// Prepare rotation constraint part
+	switch (mRotationConstraintType)
+	{
+	case EPathRotationConstraintType::Free:
+		// No rotational limits
+		break;
+
+	case EPathRotationConstraintType::ConstrainAroundTangent:
+		mHingeConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), mPathTangent, *mBody2, transform2.GetRotation(), path_to_world_2.GetAxisX());
+		break;
+
+	case EPathRotationConstraintType::ConstrainAroundNormal:
+		mHingeConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), mPathNormal, *mBody2, transform2.GetRotation(), path_to_world_2.GetAxisZ());
+		break;
+
+	case EPathRotationConstraintType::ConstrainAroundBinormal:
+		mHingeConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), mPathBinormal, *mBody2, transform2.GetRotation(), path_to_world_2.GetAxisY());
+		break;
+
+	case EPathRotationConstraintType::ConstrainToPath:
+		// We need to calculate the inverse of the rotation from body 1 to body 2 for the current path position (see: RotationEulerConstraintPart::sGetInvInitialOrientation)
+		// RotationBody2 = RotationBody1 * InitialOrientation <=> InitialOrientation^-1 = RotationBody2^-1 * RotationBody1
+		// We can express RotationBody2 in terms of RotationBody1: RotationBody2 = RotationBody1 * PathToBody1 * RotationClosestPointOnPath * PathToBody2^-1
+		// Combining these two: InitialOrientation^-1 = PathToBody2 * (PathToBody1 * RotationClosestPointOnPath)^-1
+		mInvInitialOrientation = mPathToBody2.Multiply3x3RightTransposed(mPathToBody1.Multiply3x3(Mat44(Vec4(path_tangent, 0), Vec4(path_binormal, 0), Vec4(path_normal, 0), Vec4::sZero()))).GetQuaternion();
+		[[fallthrough]];
+
+	case EPathRotationConstraintType::FullyConstrained:
+		mRotationConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), *mBody2, transform2.GetRotation());
+		break;
+	}
+
+	// Motor properties
+	switch (mPositionMotorState)
+	{
+	case EMotorState::Off:
+		if (mMaxFrictionForce > 0.0f)
+			mPositionMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mPathTangent);
+		else
+			mPositionMotorConstraintPart.Deactivate();
+		break;
+
+	case EMotorState::Velocity:
+		mPositionMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mPathTangent, -mTargetVelocity);
+		break;
+
+	case EMotorState::Position:
+		if (mPositionMotorSettings.mSpringSettings.HasStiffness())
+		{
+			// Calculate constraint value to drive to
+			float c;
+			if (mPath->IsLooping())
+			{
+				float max_fraction = mPath->GetPathMaxFraction();
+				c = fmod(mPathFraction - mTargetPathFraction, max_fraction);
+				float half_max_fraction = 0.5f * max_fraction;
+				if (c > half_max_fraction)
+					c -= max_fraction;
+				else if (c < -half_max_fraction)
+					c += max_fraction;
+			}
+			else
+				c = mPathFraction - mTargetPathFraction;
+			mPositionMotorConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mPathTangent, 0.0f, c, mPositionMotorSettings.mSpringSettings);
+		}
+		else
+			mPositionMotorConstraintPart.Deactivate();
+		break;
+	}
+}
+
+void PathConstraint::SetupVelocityConstraint(float inDeltaTime)
+{
+	CalculateConstraintProperties(inDeltaTime);
+}
+
+void PathConstraint::ResetWarmStart()
+{
+	mPositionMotorConstraintPart.Deactivate();
+	mPositionConstraintPart.Deactivate();
+	mPositionLimitsConstraintPart.Deactivate();
+	mHingeConstraintPart.Deactivate();
+	mRotationConstraintPart.Deactivate();
+}
+
+void PathConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
+{
+	// Warm starting: Apply previous frame impulse
+	mPositionMotorConstraintPart.WarmStart(*mBody1, *mBody2, mPathTangent, inWarmStartImpulseRatio);
+	mPositionConstraintPart.WarmStart(*mBody1, *mBody2, mPathNormal, mPathBinormal, inWarmStartImpulseRatio);
+	mPositionLimitsConstraintPart.WarmStart(*mBody1, *mBody2, mPathTangent, inWarmStartImpulseRatio);
+
+	switch (mRotationConstraintType)
+	{
+	case EPathRotationConstraintType::Free:
+		// No rotational limits
+		break;
+
+	case EPathRotationConstraintType::ConstrainAroundTangent:
+	case EPathRotationConstraintType::ConstrainAroundNormal:
+	case EPathRotationConstraintType::ConstrainAroundBinormal:
+		mHingeConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+		break;
+
+	case EPathRotationConstraintType::ConstrainToPath:
+	case EPathRotationConstraintType::FullyConstrained:
+		mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+		break;
+	}
+}
+
+bool PathConstraint::SolveVelocityConstraint(float inDeltaTime)
+{
+	// Solve motor
+	bool motor = false;
+	if (mPositionMotorConstraintPart.IsActive())
+	{
+		switch (mPositionMotorState)
+		{
+		case EMotorState::Off:
+			{
+				float max_lambda = mMaxFrictionForce * inDeltaTime;
+				motor = mPositionMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathTangent, -max_lambda, max_lambda);
+				break;
+			}
+
+		case EMotorState::Velocity:
+		case EMotorState::Position:
+			motor = mPositionMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathTangent, inDeltaTime * mPositionMotorSettings.mMinForceLimit, inDeltaTime * mPositionMotorSettings.mMaxForceLimit);
+			break;
+		}
+	}
+
+	// Solve position constraint along 2 axis
+	bool pos = mPositionConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathNormal, mPathBinormal);
+
+	// Solve limits along path axis
+	bool limit = false;
+	if (mPositionLimitsConstraintPart.IsActive())
+	{
+		if (mPathFraction <= 0.0f)
+			limit = mPositionLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathTangent, 0, FLT_MAX);
+		else
+		{
+			JPH_ASSERT(mPathFraction >= mPath->GetPathMaxFraction());
+			limit = mPositionLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathTangent, -FLT_MAX, 0);
+		}
+	}
+
+	// Solve rotational constraint
+	// Note, this is not entirely correct, we should apply a velocity constraint so that the body will actually follow the path
+	// by looking at the derivative of the tangent, normal or binormal but we don't. This means the position constraint solver
+	// will need to correct the orientation error that builds up, which in turn means that the simulation is not physically correct.
+	bool rot = false;
+	switch (mRotationConstraintType)
+	{
+	case EPathRotationConstraintType::Free:
+		// No rotational limits
+		break;
+
+	case EPathRotationConstraintType::ConstrainAroundTangent:
+	case EPathRotationConstraintType::ConstrainAroundNormal:
+	case EPathRotationConstraintType::ConstrainAroundBinormal:
+		rot = mHingeConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+		break;
+
+	case EPathRotationConstraintType::ConstrainToPath:
+	case EPathRotationConstraintType::FullyConstrained:
+		rot = mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+		break;
+	}
+
+	return motor || pos || limit || rot;
+}
+
+bool PathConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
+{
+	// Update constraint properties (bodies may have moved)
+	CalculateConstraintProperties(inDeltaTime);
+
+	// Solve position constraint along 2 axis
+	bool pos = mPositionConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mU, mPathNormal, mPathBinormal, inBaumgarte);
+
+	// Solve limits along path axis
+	bool limit = false;
+	if (mPositionLimitsConstraintPart.IsActive())
+	{
+		if (mPathFraction <= 0.0f)
+			limit = mPositionLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mPathTangent, mU.Dot(mPathTangent), inBaumgarte);
+		else
+		{
+			JPH_ASSERT(mPathFraction >= mPath->GetPathMaxFraction());
+			limit = mPositionLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mPathTangent, mU.Dot(mPathTangent), inBaumgarte);
+		}
+	}
+
+	// Solve rotational constraint
+	bool rot = false;
+	switch (mRotationConstraintType)
+	{
+	case EPathRotationConstraintType::Free:
+		// No rotational limits
+		break;
+
+	case EPathRotationConstraintType::ConstrainAroundTangent:
+	case EPathRotationConstraintType::ConstrainAroundNormal:
+	case EPathRotationConstraintType::ConstrainAroundBinormal:
+		rot = mHingeConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte);
+		break;
+
+	case EPathRotationConstraintType::ConstrainToPath:
+	case EPathRotationConstraintType::FullyConstrained:
+		rot = mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mInvInitialOrientation, inBaumgarte);
+		break;
+	}
+
+	return pos || limit || rot;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void PathConstraint::DrawConstraint(DebugRenderer *inRenderer) const
+{
+	if (mPath != nullptr)
+	{
+		// Draw the path in world space
+		RMat44 path_to_world = mBody1->GetCenterOfMassTransform() * mPathToBody1;
+		mPath->DrawPath(inRenderer, path_to_world);
+
+		// Draw anchor point of both bodies in world space
+		RVec3 x1 = mBody1->GetCenterOfMassPosition() + mR1;
+		RVec3 x2 = mBody2->GetCenterOfMassPosition() + mR2;
+		inRenderer->DrawMarker(x1, Color::sYellow, 0.1f);
+		inRenderer->DrawMarker(x2, Color::sYellow, 0.1f);
+		inRenderer->DrawArrow(x1, x1 + mPathTangent, Color::sBlue, 0.1f);
+		inRenderer->DrawArrow(x1, x1 + mPathNormal, Color::sRed, 0.1f);
+		inRenderer->DrawArrow(x1, x1 + mPathBinormal, Color::sGreen, 0.1f);
+		inRenderer->DrawText3D(x1, StringFormat("%.1f", (double)mPathFraction));
+
+		// Draw motor
+		switch (mPositionMotorState)
+		{
+		case EMotorState::Position:
+			{
+				// Draw target marker
+				Vec3 position, tangent, normal, binormal;
+				mPath->GetPointOnPath(mTargetPathFraction, position, tangent, normal, binormal);
+				inRenderer->DrawMarker(path_to_world * position, Color::sYellow, 1.0f);
+				break;
+			}
+
+		case EMotorState::Velocity:
+			{
+				RVec3 position = mBody2->GetCenterOfMassPosition() + mR2;
+				inRenderer->DrawArrow(position, position + mPathTangent * mTargetVelocity, Color::sRed, 0.1f);
+				break;
+			}
+
+		case EMotorState::Off:
+			break;
+		}
+	}
+}
+#endif // JPH_DEBUG_RENDERER
+
+void PathConstraint::SaveState(StateRecorder &inStream) const
+{
+	TwoBodyConstraint::SaveState(inStream);
+
+	mPositionConstraintPart.SaveState(inStream);
+	mPositionLimitsConstraintPart.SaveState(inStream);
+	mPositionMotorConstraintPart.SaveState(inStream);
+	mHingeConstraintPart.SaveState(inStream);
+	mRotationConstraintPart.SaveState(inStream);
+
+	inStream.Write(mMaxFrictionForce);
+	inStream.Write(mPositionMotorSettings);
+	inStream.Write(mPositionMotorState);
+	inStream.Write(mTargetVelocity);
+	inStream.Write(mTargetPathFraction);
+	inStream.Write(mPathFraction);
+}
+
+void PathConstraint::RestoreState(StateRecorder &inStream)
+{
+	TwoBodyConstraint::RestoreState(inStream);
+
+	mPositionConstraintPart.RestoreState(inStream);
+	mPositionLimitsConstraintPart.RestoreState(inStream);
+	mPositionMotorConstraintPart.RestoreState(inStream);
+	mHingeConstraintPart.RestoreState(inStream);
+	mRotationConstraintPart.RestoreState(inStream);
+
+	inStream.Read(mMaxFrictionForce);
+	inStream.Read(mPositionMotorSettings);
+	inStream.Read(mPositionMotorState);
+	inStream.Read(mTargetVelocity);
+	inStream.Read(mTargetPathFraction);
+	inStream.Read(mPathFraction);
+}
+
+Ref<ConstraintSettings> PathConstraint::GetConstraintSettings() const
+{
+	JPH_ASSERT(false); // Not implemented yet
+	return nullptr;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraint.h
new file mode 100644
index 0000000000..3d15438bc9
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraint.h
@@ -0,0 +1,191 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Physics/Constraints/PathConstraintPath.h>
+#include <Jolt/Physics/Constraints/MotorSettings.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/DualAxisConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/HingeRotationConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_SUPPRESS_WARNING_PUSH
+JPH_GCC_SUPPRESS_WARNING("-Wshadow") // GCC complains about the 'Free' value conflicting with the 'Free' method
+
+/// How to constrain the rotation of the body to a PathConstraint
+enum class EPathRotationConstraintType
+{
+	Free,							///< Do not constrain the rotation of the body at all
+	ConstrainAroundTangent,			///< Only allow rotation around the tangent vector (following the path)
+	ConstrainAroundNormal,			///< Only allow rotation around the normal vector (perpendicular to the path)
+	ConstrainAroundBinormal,		///< Only allow rotation around the binormal vector (perpendicular to the path)
+	ConstrainToPath,				///< Fully constrain the rotation of body 2 to the path (following the tangent and normal of the path)
+	FullyConstrained,				///< Fully constrain the rotation of the body 2 to the rotation of body 1
+};
+
+JPH_SUPPRESS_WARNING_POP
+
+/// Path constraint settings, used to constrain the degrees of freedom between two bodies to a path
+///
+/// The requirements of the path are that:
+/// * Tangent, normal and bi-normal form an orthonormal basis with: tangent cross bi-normal = normal
+/// * The path points along the tangent vector
+/// * The path is continuous so doesn't contain any sharp corners
+///
+/// The reason for all this is that the constraint acts like a slider constraint with the sliding axis being the tangent vector (the assumption here is that delta time will be small enough so that the path is linear for that delta time).
+class JPH_EXPORT PathConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PathConstraintSettings)
+
+public:
+	// See: ConstraintSettings::SaveBinaryState
+	virtual void					SaveBinaryState(StreamOut &inStream) const override;
+
+	/// Create an instance of this constraint
+	virtual TwoBodyConstraint *		Create(Body &inBody1, Body &inBody2) const override;
+
+	/// The path that constrains the two bodies
+	RefConst<PathConstraintPath>	mPath;
+
+	/// The position of the path start relative to world transform of body 1
+	Vec3							mPathPosition = Vec3::sZero();
+
+	/// The rotation of the path start relative to world transform of body 1
+	Quat							mPathRotation = Quat::sIdentity();
+
+	/// The fraction along the path that corresponds to the initial position of body 2. Usually this is 0, the beginning of the path. But if you want to start an object halfway the path you can calculate this with mPath->GetClosestPoint(point on path to attach body to).
+	float							mPathFraction = 0.0f;
+
+	/// Maximum amount of friction force to apply (N) when not driven by a motor.
+	float							mMaxFrictionForce = 0.0f;
+
+	/// In case the constraint is powered, this determines the motor settings along the path
+	MotorSettings					mPositionMotorSettings;
+
+	/// How to constrain the rotation of the body to the path
+	EPathRotationConstraintType		mRotationConstraintType = EPathRotationConstraintType::Free;
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void					RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// Path constraint, used to constrain the degrees of freedom between two bodies to a path
+class JPH_EXPORT PathConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct point constraint
+									PathConstraint(Body &inBody1, Body &inBody2, const PathConstraintSettings &inSettings);
+
+	// Generic interface of a constraint
+	virtual EConstraintSubType		GetSubType() const override								{ return EConstraintSubType::Path; }
+	virtual void					NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
+	virtual void					SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void					ResetWarmStart() override;
+	virtual void					WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
+	virtual bool					SolveVelocityConstraint(float inDeltaTime) override;
+	virtual bool					SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void					DrawConstraint(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual void					SaveState(StateRecorder &inStream) const override;
+	virtual void					RestoreState(StateRecorder &inStream) override;
+	virtual bool					IsActive() const override								{ return TwoBodyConstraint::IsActive() && mPath != nullptr; }
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
+
+	// See: TwoBodyConstraint
+	virtual Mat44					GetConstraintToBody1Matrix() const override				{ return mPathToBody1; }
+	virtual Mat44					GetConstraintToBody2Matrix() const override				{ return mPathToBody2; }
+
+	/// Update the path for this constraint
+	void							SetPath(const PathConstraintPath *inPath, float inPathFraction);
+
+	/// Access to the current path
+	const PathConstraintPath *		GetPath() const											{ return mPath; }
+
+	/// Access to the current fraction along the path e [0, GetPath()->GetMaxPathFraction()]
+	float							GetPathFraction() const									{ return mPathFraction; }
+
+	/// Friction control
+	void							SetMaxFrictionForce(float inFrictionForce)				{ mMaxFrictionForce = inFrictionForce; }
+	float							GetMaxFrictionForce() const								{ return mMaxFrictionForce; }
+
+	/// Position motor settings
+	MotorSettings &					GetPositionMotorSettings()								{ return mPositionMotorSettings; }
+	const MotorSettings &			GetPositionMotorSettings() const						{ return mPositionMotorSettings; }
+
+	// Position motor controls (drives body 2 along the path)
+	void							SetPositionMotorState(EMotorState inState)				{ JPH_ASSERT(inState == EMotorState::Off || mPositionMotorSettings.IsValid()); mPositionMotorState = inState; }
+	EMotorState						GetPositionMotorState() const							{ return mPositionMotorState; }
+	void							SetTargetVelocity(float inVelocity)						{ mTargetVelocity = inVelocity; }
+	float							GetTargetVelocity() const								{ return mTargetVelocity; }
+	void							SetTargetPathFraction(float inFraction)					{ JPH_ASSERT(mPath->IsLooping() || (inFraction >= 0.0f && inFraction <= mPath->GetPathMaxFraction())); mTargetPathFraction = inFraction; }
+	float							GetTargetPathFraction() const							{ return mTargetPathFraction; }
+
+	///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint)
+	inline Vector<2>				GetTotalLambdaPosition() const							{ return mPositionConstraintPart.GetTotalLambda(); }
+	inline float					GetTotalLambdaPositionLimits() const					{ return mPositionLimitsConstraintPart.GetTotalLambda(); }
+	inline float					GetTotalLambdaMotor() const								{ return mPositionMotorConstraintPart.GetTotalLambda(); }
+	inline Vector<2>				GetTotalLambdaRotationHinge() const						{ return mHingeConstraintPart.GetTotalLambda(); }
+	inline Vec3						GetTotalLambdaRotation() const							{ return mRotationConstraintPart.GetTotalLambda(); }
+
+private:
+	// Internal helper function to calculate the values below
+	void							CalculateConstraintProperties(float inDeltaTime);
+
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	RefConst<PathConstraintPath>	mPath;													///< The path that attaches the two bodies
+	Mat44							mPathToBody1;											///< Transform that takes a quantity from path space to body 1 center of mass space
+	Mat44							mPathToBody2;											///< Transform that takes a quantity from path space to body 2 center of mass space
+	EPathRotationConstraintType		mRotationConstraintType;								///< How to constrain the rotation of the path
+
+	// Friction
+	float							mMaxFrictionForce;
+
+	// Motor controls
+	MotorSettings					mPositionMotorSettings;
+	EMotorState						mPositionMotorState = EMotorState::Off;
+	float							mTargetVelocity = 0.0f;
+	float							mTargetPathFraction = 0.0f;
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	// Positions where the point constraint acts on in world space
+	Vec3							mR1;
+	Vec3							mR2;
+
+	// X2 + R2 - X1 - R1
+	Vec3							mU;
+
+	// World space path tangent
+	Vec3							mPathTangent;
+
+	// Normals to the path tangent
+	Vec3							mPathNormal;
+	Vec3							mPathBinormal;
+
+	// Inverse of initial rotation from body 1 to body 2 in body 1 space (only used when rotation constraint type is FullyConstrained)
+	Quat							mInvInitialOrientation;
+
+	// Current fraction along the path where body 2 is attached
+	float							mPathFraction = 0.0f;
+
+	// Translation constraint parts
+	DualAxisConstraintPart			mPositionConstraintPart;								///< Constraint part that keeps the movement along the tangent of the path
+	AxisConstraintPart				mPositionLimitsConstraintPart;							///< Constraint part that prevents movement beyond the beginning and end of the path
+	AxisConstraintPart				mPositionMotorConstraintPart;							///< Constraint to drive the object along the path or to apply friction
+
+	// Rotation constraint parts
+	HingeRotationConstraintPart		mHingeConstraintPart;									///< Constraint part that removes 2 degrees of rotation freedom
+	RotationEulerConstraintPart		mRotationConstraintPart;								///< Constraint part that removes all rotational freedom
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPath.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPath.cpp
new file mode 100644
index 0000000000..69c0a82f3b
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPath.cpp
@@ -0,0 +1,85 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/PathConstraintPath.h>
+#include <Jolt/Core/StreamUtils.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(PathConstraintPath)
+{
+	JPH_ADD_BASE_CLASS(PathConstraintPath, SerializableObject)
+}
+
+#ifdef JPH_DEBUG_RENDERER
+// Helper function to transform the results of GetPointOnPath to world space
+static inline void sTransformPathPoint(RMat44Arg inTransform, Vec3Arg inPosition, RVec3 &outPosition, Vec3 &ioNormal, Vec3 &ioBinormal)
+{
+	outPosition = inTransform * inPosition;
+	ioNormal = inTransform.Multiply3x3(ioNormal);
+	ioBinormal = inTransform.Multiply3x3(ioBinormal);
+}
+
+// Helper function to draw a path segment
+static inline void sDrawPathSegment(DebugRenderer *inRenderer, RVec3Arg inPrevPosition, RVec3Arg inPosition, Vec3Arg inNormal, Vec3Arg inBinormal)
+{
+	inRenderer->DrawLine(inPrevPosition, inPosition, Color::sWhite);
+	inRenderer->DrawArrow(inPosition, inPosition + 0.1f * inNormal, Color::sRed, 0.02f);
+	inRenderer->DrawArrow(inPosition, inPosition + 0.1f * inBinormal, Color::sGreen, 0.02f);
+}
+
+void PathConstraintPath::DrawPath(DebugRenderer *inRenderer, RMat44Arg inBaseTransform) const
+{
+	// Calculate first point
+	Vec3 lfirst_pos, first_tangent, first_normal, first_binormal;
+	GetPointOnPath(0.0f, lfirst_pos, first_tangent, first_normal, first_binormal);
+	RVec3 first_pos;
+	sTransformPathPoint(inBaseTransform, lfirst_pos, first_pos, first_normal, first_binormal);
+
+	float t_max = GetPathMaxFraction();
+
+	// Draw the segments
+	RVec3 prev_pos = first_pos;
+	for (float t = 0.1f; t < t_max; t += 0.1f)
+	{
+		Vec3 lpos, tangent, normal, binormal;
+		GetPointOnPath(t, lpos, tangent, normal, binormal);
+		RVec3 pos;
+		sTransformPathPoint(inBaseTransform, lpos, pos, normal, binormal);
+		sDrawPathSegment(inRenderer, prev_pos, pos, normal, binormal);
+		prev_pos = pos;
+	}
+
+	// Draw last point
+	Vec3 lpos, tangent, normal, binormal;
+	GetPointOnPath(t_max, lpos, tangent, normal, binormal);
+	RVec3 pos;
+	sTransformPathPoint(inBaseTransform, lpos, pos, normal, binormal);
+	sDrawPathSegment(inRenderer, prev_pos, pos, normal, binormal);
+}
+#endif // JPH_DEBUG_RENDERER
+
+void PathConstraintPath::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(GetRTTI()->GetHash());
+	inStream.Write(mIsLooping);
+}
+
+void PathConstraintPath::RestoreBinaryState(StreamIn &inStream)
+{
+	// Type hash read by sRestoreFromBinaryState
+	inStream.Read(mIsLooping);
+}
+
+PathConstraintPath::PathResult PathConstraintPath::sRestoreFromBinaryState(StreamIn &inStream)
+{
+	return StreamUtils::RestoreObject<PathConstraintPath>(inStream, &PathConstraintPath::RestoreBinaryState);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPath.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPath.h
new file mode 100644
index 0000000000..2e05c2ce6f
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPath.h
@@ -0,0 +1,71 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/Reference.h>
+#include <Jolt/Core/Result.h>
+#include <Jolt/ObjectStream/SerializableObject.h>
+
+JPH_NAMESPACE_BEGIN
+
+class StreamIn;
+class StreamOut;
+#ifdef JPH_DEBUG_RENDERER
+class DebugRenderer;
+#endif // JPH_DEBUG_RENDERER
+
+/// The path for a path constraint. It allows attaching two bodies to each other while giving the second body the freedom to move along a path relative to the first.
+class JPH_EXPORT PathConstraintPath : public SerializableObject, public RefTarget<PathConstraintPath>
+{
+	JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, PathConstraintPath)
+
+public:
+	using PathResult = Result<Ref<PathConstraintPath>>;
+
+	/// Virtual destructor to ensure that derived types get their destructors called
+	virtual				~PathConstraintPath() override = default;
+
+	/// Gets the max fraction along the path. I.e. sort of the length of the path.
+	virtual float		GetPathMaxFraction() const = 0;
+
+	/// Get the globally closest point on the curve (Could be slow!)
+	/// @param inPosition Position to find closest point for
+	/// @param inFractionHint Last known fraction along the path (can be used to speed up the search)
+	/// @return Fraction of closest point along the path
+	virtual float		GetClosestPoint(Vec3Arg inPosition, float inFractionHint) const = 0;
+
+	/// Given the fraction along the path, get the point, tangent and normal.
+	/// @param inFraction Fraction along the path [0, GetPathMaxFraction()].
+	/// @param outPathPosition Returns the closest position to inSearchPosition on the path.
+	/// @param outPathTangent Returns the tangent to the path at outPathPosition (the vector that follows the direction of the path)
+	/// @param outPathNormal Return the normal to the path at outPathPosition (a vector that's perpendicular to outPathTangent)
+	/// @param outPathBinormal Returns the binormal to the path at outPathPosition (a vector so that normal cross tangent = binormal)
+	virtual void		GetPointOnPath(float inFraction, Vec3 &outPathPosition, Vec3 &outPathTangent, Vec3 &outPathNormal, Vec3 &outPathBinormal) const = 0;
+
+	/// If the path is looping or not. If a path is looping, the first and last point are automatically connected to each other. They should not be the same points.
+	void				SetIsLooping(bool inIsLooping)						{ mIsLooping = inIsLooping; }
+	bool				IsLooping() const									{ return mIsLooping; }
+
+#ifdef JPH_DEBUG_RENDERER
+	/// Draw the path relative to inBaseTransform. Used for debug purposes.
+	void				DrawPath(DebugRenderer *inRenderer, RMat44Arg inBaseTransform) const;
+#endif // JPH_DEBUG_RENDERER
+
+	/// Saves the contents of the path in binary form to inStream.
+	virtual void		SaveBinaryState(StreamOut &inStream) const;
+
+	/// Creates a Shape of the correct type and restores its contents from the binary stream inStream.
+	static PathResult	sRestoreFromBinaryState(StreamIn &inStream);
+
+protected:
+	/// This function should not be called directly, it is used by sRestoreFromBinaryState.
+	virtual void		RestoreBinaryState(StreamIn &inStream);
+
+private:
+	/// If the path is looping or not. If a path is looping, the first and last point are automatically connected to each other. They should not be the same points.
+	bool				mIsLooping = false;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPathHermite.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPathHermite.cpp
new file mode 100644
index 0000000000..132d3b25fc
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPathHermite.cpp
@@ -0,0 +1,308 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/PathConstraintPathHermite.h>
+#include <Jolt/Core/Profiler.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(PathConstraintPathHermite::Point)
+{
+	JPH_ADD_ATTRIBUTE(PathConstraintPathHermite::Point, mPosition)
+	JPH_ADD_ATTRIBUTE(PathConstraintPathHermite::Point, mTangent)
+	JPH_ADD_ATTRIBUTE(PathConstraintPathHermite::Point, mNormal)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PathConstraintPathHermite)
+{
+	JPH_ADD_BASE_CLASS(PathConstraintPathHermite, PathConstraintPath)
+
+	JPH_ADD_ATTRIBUTE(PathConstraintPathHermite, mPoints)
+}
+
+// Calculate position and tangent for a Cubic Hermite Spline segment
+static inline void sCalculatePositionAndTangent(Vec3Arg inP1, Vec3Arg inM1, Vec3Arg inP2, Vec3Arg inM2, float inT, Vec3 &outPosition, Vec3 &outTangent)
+{
+	// Calculate factors for Cubic Hermite Spline
+	// See: https://en.wikipedia.org/wiki/Cubic_Hermite_spline
+	float t2 = inT * inT;
+	float t3 = inT * t2;
+	float h00 = 2.0f * t3 - 3.0f * t2 + 1.0f;
+	float h10 = t3 - 2.0f * t2 + inT;
+	float h01 = -2.0f * t3 + 3.0f * t2;
+	float h11 = t3 - t2;
+
+	// Calculate d/dt for factors to calculate the tangent
+	float ddt_h00 = 6.0f * (t2 - inT);
+	float ddt_h10 = 3.0f * t2 - 4.0f * inT + 1.0f;
+	float ddt_h01 = -ddt_h00;
+	float ddt_h11 = 3.0f * t2 - 2.0f * inT;
+
+	outPosition = h00 * inP1 + h10 * inM1 + h01 * inP2 + h11 * inM2;
+	outTangent = ddt_h00 * inP1 + ddt_h10 * inM1 + ddt_h01 * inP2 + ddt_h11 * inM2;
+}
+
+// Calculate the closest point to the origin for a Cubic Hermite Spline segment
+// This is used to get an estimate for the interval in which the closest point can be found,
+// the interval [0, 1] is too big for Newton Raphson to work on because it is solving a 5th degree polynomial which may
+// have multiple local minima that are not the root. This happens especially when the path is straight (tangents aligned with inP2 - inP1).
+// Based on the bisection method: https://en.wikipedia.org/wiki/Bisection_method
+static inline void sCalculateClosestPointThroughBisection(Vec3Arg inP1, Vec3Arg inM1, Vec3Arg inP2, Vec3Arg inM2, float &outTMin, float &outTMax)
+{
+	outTMin = 0.0f;
+	outTMax = 1.0f;
+
+	// To get the closest point of the curve to the origin we need to solve:
+	// d/dt P(t) . P(t) = 0 for t, where P(t) is the point on the curve segment
+	// Using d/dt (a(t) . b(t)) = d/dt a(t) . b(t) + a(t) . d/dt b(t)
+	// See: https://proofwiki.org/wiki/Derivative_of_Dot_Product_of_Vector-Valued_Functions
+	// d/dt P(t) . P(t) = 2 P(t) d/dt P(t) = 2 P(t) . Tangent(t)
+
+	// Calculate the derivative at t = 0, we know P(0) = inP1 and Tangent(0) = inM1
+	float ddt_min = inP1.Dot(inM1); // Leaving out factor 2, we're only interested in the root
+	if (abs(ddt_min) < 1.0e-6f)
+	{
+		// Derivative is near zero, we found our root
+		outTMax = 0.0f;
+		return;
+	}
+	bool ddt_min_negative = ddt_min < 0.0f;
+
+	// Calculate derivative at t = 1, we know P(1) = inP2 and Tangent(1) = inM2
+	float ddt_max = inP2.Dot(inM2);
+	if (abs(ddt_max) < 1.0e-6f)
+	{
+		// Derivative is near zero, we found our root
+		outTMin = 1.0f;
+		return;
+	}
+	bool ddt_max_negative = ddt_max < 0.0f;
+
+	// If the signs of the derivative are not different, this algorithm can't find the root
+	if (ddt_min_negative == ddt_max_negative)
+		return;
+
+	// With 4 iterations we'll get a result accurate to 1 / 2^4 = 0.0625
+	for (int iteration = 0; iteration < 4; ++iteration)
+	{
+		float t_mid = 0.5f * (outTMin + outTMax);
+		Vec3 position, tangent;
+		sCalculatePositionAndTangent(inP1, inM1, inP2, inM2, t_mid, position, tangent);
+		float ddt_mid = position.Dot(tangent);
+		if (abs(ddt_mid) < 1.0e-6f)
+		{
+			// Derivative is near zero, we found our root
+			outTMin = outTMax = t_mid;
+			return;
+		}
+		bool ddt_mid_negative = ddt_mid < 0.0f;
+
+		// Update the search interval so that the signs of the derivative at both ends of the interval are still different
+		if (ddt_mid_negative == ddt_min_negative)
+			outTMin = t_mid;
+		else
+			outTMax = t_mid;
+	}
+}
+
+// Calculate the closest point to the origin for a Cubic Hermite Spline segment
+// Only considers the range t e [inTMin, inTMax] and will stop as soon as the closest point falls outside of that range
+static inline float sCalculateClosestPointThroughNewtonRaphson(Vec3Arg inP1, Vec3Arg inM1, Vec3Arg inP2, Vec3Arg inM2, float inTMin, float inTMax, float &outDistanceSq)
+{
+	// This is the closest position on the curve to the origin that we found
+	Vec3 position;
+
+	// Calculate the size of the interval
+	float interval = inTMax - inTMin;
+
+	// Start in the middle of the interval
+	float t = 0.5f * (inTMin + inTMax);
+
+	// Do max 10 iterations to prevent taking too much CPU time
+	for (int iteration = 0; iteration < 10; ++iteration)
+	{
+		// Calculate derivative at t, see comment at sCalculateClosestPointThroughBisection for derivation of the equations
+		Vec3 tangent;
+		sCalculatePositionAndTangent(inP1, inM1, inP2, inM2, t, position, tangent);
+		float ddt = position.Dot(tangent); // Leaving out factor 2, we're only interested in the root
+
+		// Calculate derivative of ddt: d^2/dt P(t) . P(t) = d/dt (2 P(t) . Tangent(t))
+		// = 2 (d/dt P(t)) . Tangent(t) + P(t) . d/dt Tangent(t)) = 2 (Tangent(t) . Tangent(t) + P(t) . d/dt Tangent(t))
+		float d2dt_h00 = 12.0f * t - 6.0f;
+		float d2dt_h10 = 6.0f * t - 4.0f;
+		float d2dt_h01 = -d2dt_h00;
+		float d2dt_h11 = 6.0f * t - 2.0f;
+		Vec3 ddt_tangent = d2dt_h00 * inP1 + d2dt_h10 * inM1 + d2dt_h01 * inP2 + d2dt_h11 * inM2;
+		float d2dt = tangent.Dot(tangent) + position.Dot(ddt_tangent); // Leaving out factor 2, because we left it out above too
+
+		// If d2dt is zero, the curve is flat and there are multiple t's for which we are closest to the origin, stop now
+		if (d2dt == 0.0f)
+			break;
+
+		// Do a Newton Raphson step
+		// See: https://en.wikipedia.org/wiki/Newton%27s_method
+		// Clamp against [-interval, interval] to avoid overshooting too much, we're not interested outside the interval
+		float delta = Clamp(-ddt / d2dt, -interval, interval);
+
+		// If we're stepping away further from t e [inTMin, inTMax] stop now
+		if ((t > inTMax && delta > 0.0f) || (t < inTMin && delta < 0.0f))
+			break;
+
+		// If we've converged, stop now
+		t += delta;
+		if (abs(delta) < 1.0e-4f)
+			break;
+	}
+
+	// Calculate the distance squared for the origin to the curve
+	outDistanceSq = position.LengthSq();
+	return t;
+}
+
+void PathConstraintPathHermite::GetIndexAndT(float inFraction, int &outIndex, float &outT) const
+{
+	int num_points = int(mPoints.size());
+
+	// Start by truncating the fraction to get the index and storing the remainder in t
+	int index = int(trunc(inFraction));
+	float t = inFraction - float(index);
+
+	if (IsLooping())
+	{
+		JPH_ASSERT(!mPoints.front().mPosition.IsClose(mPoints.back().mPosition), "A looping path should have a different first and last point!");
+
+		// Make sure index is positive by adding a multiple of num_points
+		if (index < 0)
+			index += (-index / num_points + 1) * num_points;
+
+		// Index needs to be modulo num_points
+		index = index % num_points;
+	}
+	else
+	{
+		// Clamp against range of points
+		if (index < 0)
+		{
+			index = 0;
+			t = 0.0f;
+		}
+		else if (index >= num_points - 1)
+		{
+			index = num_points - 2;
+			t = 1.0f;
+		}
+	}
+
+	outIndex = index;
+	outT = t;
+}
+
+float PathConstraintPathHermite::GetClosestPoint(Vec3Arg inPosition, float inFractionHint) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	int num_points = int(mPoints.size());
+
+	// Start with last point on the path, in the non-looping case we won't be visiting this point
+	float best_dist_sq = (mPoints[num_points - 1].mPosition - inPosition).LengthSq();
+	float best_t = float(num_points - 1);
+
+	// Loop over all points
+	for (int i = 0, max_i = IsLooping()? num_points : num_points - 1; i < max_i; ++i)
+	{
+		const Point &p1 = mPoints[i];
+		const Point &p2 = mPoints[(i + 1) % num_points];
+
+		// Make the curve relative to inPosition
+		Vec3 p1_pos = p1.mPosition - inPosition;
+		Vec3 p2_pos = p2.mPosition - inPosition;
+
+		// Get distance to p1
+		float dist_sq = p1_pos.LengthSq();
+		if (dist_sq < best_dist_sq)
+		{
+			best_t = float(i);
+			best_dist_sq = dist_sq;
+		}
+
+		// First find an interval for the closest point so that we can start doing Newton Raphson steps
+		float t_min, t_max;
+		sCalculateClosestPointThroughBisection(p1_pos, p1.mTangent, p2_pos, p2.mTangent, t_min, t_max);
+
+		if (t_min == t_max)
+		{
+			// If the function above returned no interval then it found the root already and we can just calculate the distance
+			Vec3 position, tangent;
+			sCalculatePositionAndTangent(p1_pos, p1.mTangent, p2_pos, p2.mTangent, t_min, position, tangent);
+			dist_sq = position.LengthSq();
+			if (dist_sq < best_dist_sq)
+			{
+				best_t = float(i) + t_min;
+				best_dist_sq = dist_sq;
+			}
+		}
+		else
+		{
+			// Get closest distance along curve segment
+			float t = sCalculateClosestPointThroughNewtonRaphson(p1_pos, p1.mTangent, p2_pos, p2.mTangent, t_min, t_max, dist_sq);
+			if (t >= 0.0f && t <= 1.0f && dist_sq < best_dist_sq)
+			{
+				best_t = float(i) + t;
+				best_dist_sq = dist_sq;
+			}
+		}
+	}
+
+	return best_t;
+}
+
+void PathConstraintPathHermite::GetPointOnPath(float inFraction, Vec3 &outPathPosition, Vec3 &outPathTangent, Vec3 &outPathNormal, Vec3 &outPathBinormal) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Determine which hermite spline segment we need
+	int index;
+	float t;
+	GetIndexAndT(inFraction, index, t);
+
+	// Get the points on the segment
+	const Point &p1 = mPoints[index];
+	const Point &p2 = mPoints[(index + 1) % int(mPoints.size())];
+
+	// Calculate the position and tangent on the path
+	Vec3 tangent;
+	sCalculatePositionAndTangent(p1.mPosition, p1.mTangent, p2.mPosition, p2.mTangent, t, outPathPosition, tangent);
+	outPathTangent = tangent.Normalized();
+
+	// Just linearly interpolate the normal
+	Vec3 normal = (1.0f - t) * p1.mNormal + t * p2.mNormal;
+
+	// Calculate binormal
+	outPathBinormal = normal.Cross(outPathTangent).Normalized();
+
+	// Recalculate normal so it is perpendicular to both (linear interpolation will cause it not to be)
+	outPathNormal = outPathTangent.Cross(outPathBinormal);
+	JPH_ASSERT(outPathNormal.IsNormalized());
+}
+
+void PathConstraintPathHermite::SaveBinaryState(StreamOut &inStream) const
+{
+	PathConstraintPath::SaveBinaryState(inStream);
+
+	inStream.Write(mPoints);
+}
+
+void PathConstraintPathHermite::RestoreBinaryState(StreamIn &inStream)
+{
+	PathConstraintPath::RestoreBinaryState(inStream);
+
+	inStream.Read(mPoints);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPathHermite.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPathHermite.h
new file mode 100644
index 0000000000..839909be8b
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PathConstraintPathHermite.h
@@ -0,0 +1,54 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/PathConstraintPath.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// A path that follows a Hermite spline
+class JPH_EXPORT PathConstraintPathHermite final : public PathConstraintPath
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PathConstraintPathHermite)
+
+public:
+	// See PathConstraintPath::GetPathMaxFraction
+	virtual float		GetPathMaxFraction() const override									{ return float(IsLooping()? mPoints.size() : mPoints.size() - 1); }
+
+	// See PathConstraintPath::GetClosestPoint
+	virtual float		GetClosestPoint(Vec3Arg inPosition, float inFractionHint) const override;
+
+	// See PathConstraintPath::GetPointOnPath
+	virtual void		GetPointOnPath(float inFraction, Vec3 &outPathPosition, Vec3 &outPathTangent, Vec3 &outPathNormal, Vec3 &outPathBinormal) const override;
+
+	/// Adds a point to the path
+	void				AddPoint(Vec3Arg inPosition, Vec3Arg inTangent, Vec3Arg inNormal)	{ mPoints.push_back({ inPosition, inTangent, inNormal}); }
+
+	// See: PathConstraintPath::SaveBinaryState
+	virtual void		SaveBinaryState(StreamOut &inStream) const override;
+
+	struct Point
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Point)
+
+		Vec3			mPosition;															///< Position on the path
+		Vec3			mTangent;															///< Tangent of the path, does not need to be normalized (in the direction of the path)
+		Vec3			mNormal;															///< Normal of the path (together with the tangent along the curve this forms a basis for the constraint)
+	};
+
+protected:
+	// See: PathConstraintPath::RestoreBinaryState
+	virtual void		RestoreBinaryState(StreamIn &inStream) override;
+
+private:
+	/// Helper function that returns the index of the path segment and the fraction t on the path segment based on the full path fraction
+	inline void			GetIndexAndT(float inFraction, int &outIndex, float &outT) const;
+
+	using Points = Array<Point>;
+
+	Points				mPoints;															///< Points on the Hermite spline
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PointConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PointConstraint.cpp
new file mode 100644
index 0000000000..74d0ecd7c7
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PointConstraint.cpp
@@ -0,0 +1,157 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/PointConstraint.h>
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PointConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(PointConstraintSettings, TwoBodyConstraintSettings)
+
+	JPH_ADD_ENUM_ATTRIBUTE(PointConstraintSettings, mSpace)
+	JPH_ADD_ATTRIBUTE(PointConstraintSettings, mPoint1)
+	JPH_ADD_ATTRIBUTE(PointConstraintSettings, mPoint2)
+}
+
+void PointConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	ConstraintSettings::SaveBinaryState(inStream);
+
+	inStream.Write(mSpace);
+	inStream.Write(mPoint1);
+	inStream.Write(mPoint2);
+}
+
+void PointConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	ConstraintSettings::RestoreBinaryState(inStream);
+
+	inStream.Read(mSpace);
+	inStream.Read(mPoint1);
+	inStream.Read(mPoint2);
+}
+
+TwoBodyConstraint *PointConstraintSettings::Create(Body &inBody1, Body &inBody2) const
+{
+	return new PointConstraint(inBody1, inBody2, *this);
+}
+
+PointConstraint::PointConstraint(Body &inBody1, Body &inBody2, const PointConstraintSettings &inSettings) :
+	TwoBodyConstraint(inBody1, inBody2, inSettings)
+{
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		// If all properties were specified in world space, take them to local space now
+		mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mPoint1);
+		mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mPoint2);
+	}
+	else
+	{
+		mLocalSpacePosition1 = Vec3(inSettings.mPoint1);
+		mLocalSpacePosition2 = Vec3(inSettings.mPoint2);
+	}
+}
+
+void PointConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
+{
+	if (mBody1->GetID() == inBodyID)
+		mLocalSpacePosition1 -= inDeltaCOM;
+	else if (mBody2->GetID() == inBodyID)
+		mLocalSpacePosition2 -= inDeltaCOM;
+}
+
+void PointConstraint::SetPoint1(EConstraintSpace inSpace, RVec3Arg inPoint1)
+{
+	if (inSpace == EConstraintSpace::WorldSpace)
+		mLocalSpacePosition1 = Vec3(mBody1->GetInverseCenterOfMassTransform() * inPoint1);
+	else
+		mLocalSpacePosition1 = Vec3(inPoint1);
+}
+
+void PointConstraint::SetPoint2(EConstraintSpace inSpace, RVec3Arg inPoint2)
+{
+	if (inSpace == EConstraintSpace::WorldSpace)
+		mLocalSpacePosition2 = Vec3(mBody2->GetInverseCenterOfMassTransform() * inPoint2);
+	else
+		mLocalSpacePosition2 = Vec3(inPoint2);
+}
+
+void PointConstraint::CalculateConstraintProperties()
+{
+	mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2);
+}
+
+void PointConstraint::SetupVelocityConstraint(float inDeltaTime)
+{
+	CalculateConstraintProperties();
+}
+
+void PointConstraint::ResetWarmStart()
+{
+	mPointConstraintPart.Deactivate();
+}
+
+void PointConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
+{
+	// Warm starting: Apply previous frame impulse
+	mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+}
+
+bool PointConstraint::SolveVelocityConstraint(float inDeltaTime)
+{
+	return mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+}
+
+bool PointConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
+{
+	// Update constraint properties (bodies may have moved)
+	CalculateConstraintProperties();
+
+	return mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte);
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void PointConstraint::DrawConstraint(DebugRenderer *inRenderer) const
+{
+	// Draw constraint
+	inRenderer->DrawMarker(mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1, Color::sRed, 0.1f);
+	inRenderer->DrawMarker(mBody2->GetCenterOfMassTransform() * mLocalSpacePosition2, Color::sGreen, 0.1f);
+}
+#endif // JPH_DEBUG_RENDERER
+
+void PointConstraint::SaveState(StateRecorder &inStream) const
+{
+	TwoBodyConstraint::SaveState(inStream);
+
+	mPointConstraintPart.SaveState(inStream);
+}
+
+void PointConstraint::RestoreState(StateRecorder &inStream)
+{
+	TwoBodyConstraint::RestoreState(inStream);
+
+	mPointConstraintPart.RestoreState(inStream);
+}
+
+Ref<ConstraintSettings> PointConstraint::GetConstraintSettings() const
+{
+	PointConstraintSettings *settings = new PointConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mPoint1 = RVec3(mLocalSpacePosition1);
+	settings->mPoint2 = RVec3(mLocalSpacePosition2);
+	return settings;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PointConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PointConstraint.h
new file mode 100644
index 0000000000..8ab943eec8
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PointConstraint.h
@@ -0,0 +1,94 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Point constraint settings, used to create a point constraint
+class JPH_EXPORT PointConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PointConstraintSettings)
+
+public:
+	// See: ConstraintSettings::SaveBinaryState
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+
+	/// Create an instance of this constraint
+	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
+
+	/// This determines in which space the constraint is setup, all properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	/// Body 1 constraint position (space determined by mSpace).
+	RVec3						mPoint1 = RVec3::sZero();
+
+	/// Body 2 constraint position (space determined by mSpace).
+	/// Note: Normally you would set mPoint1 = mPoint2 if the bodies are already placed how you want to constrain them (if mSpace = world space).
+	RVec3						mPoint2 = RVec3::sZero();
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// A point constraint constrains 2 bodies on a single point (removing 3 degrees of freedom)
+class JPH_EXPORT PointConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct point constraint
+								PointConstraint(Body &inBody1, Body &inBody2, const PointConstraintSettings &inSettings);
+
+	// Generic interface of a constraint
+	virtual EConstraintSubType	GetSubType() const override									{ return EConstraintSubType::Point; }
+	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
+	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
+	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
+	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
+	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void				DrawConstraint(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual void				SaveState(StateRecorder &inStream) const override;
+	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
+
+	/// Update the attachment point for body 1
+	void						SetPoint1(EConstraintSpace inSpace, RVec3Arg inPoint1);
+
+	/// Update the attachment point for body 2
+	void						SetPoint2(EConstraintSpace inSpace, RVec3Arg inPoint2);
+
+	/// Get the attachment point for body 1 relative to body 1 COM (transform by Body::GetCenterOfMassTransform to take to world space)
+	inline Vec3					GetLocalSpacePoint1() const									{ return mLocalSpacePosition1; }
+
+	/// Get the attachment point for body 2 relative to body 2 COM (transform by Body::GetCenterOfMassTransform to take to world space)
+	inline Vec3					GetLocalSpacePoint2() const									{ return mLocalSpacePosition2; }
+
+	// See: TwoBodyConstraint
+	virtual Mat44				GetConstraintToBody1Matrix() const override					{ return Mat44::sTranslation(mLocalSpacePosition1); }
+	virtual Mat44				GetConstraintToBody2Matrix() const override					{ return Mat44::sTranslation(mLocalSpacePosition2); } // Note: Incorrect rotation as we don't track the original rotation difference, should not matter though as the constraint is not limiting rotation.
+
+	///@name Get Lagrange multiplier from last physics update (the linear impulse applied to satisfy the constraint)
+	inline Vec3					GetTotalLambdaPosition() const								{ return mPointConstraintPart.GetTotalLambda(); }
+
+private:
+	// Internal helper function to calculate the values below
+	void						CalculateConstraintProperties();
+
+	// Local space constraint positions
+	Vec3						mLocalSpacePosition1;
+	Vec3						mLocalSpacePosition2;
+
+	// The constraint part
+	PointConstraintPart			mPointConstraintPart;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PulleyConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PulleyConstraint.cpp
new file mode 100644
index 0000000000..9d15c1d4dc
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PulleyConstraint.cpp
@@ -0,0 +1,253 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/PulleyConstraint.h>
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+using namespace literals;
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PulleyConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(PulleyConstraintSettings, TwoBodyConstraintSettings)
+
+	JPH_ADD_ENUM_ATTRIBUTE(PulleyConstraintSettings, mSpace)
+	JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mBodyPoint1)
+	JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mFixedPoint1)
+	JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mBodyPoint2)
+	JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mFixedPoint2)
+	JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mRatio)
+	JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mMinLength)
+	JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mMaxLength)
+}
+
+void PulleyConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	ConstraintSettings::SaveBinaryState(inStream);
+
+	inStream.Write(mSpace);
+	inStream.Write(mBodyPoint1);
+	inStream.Write(mFixedPoint1);
+	inStream.Write(mBodyPoint2);
+	inStream.Write(mFixedPoint2);
+	inStream.Write(mRatio);
+	inStream.Write(mMinLength);
+	inStream.Write(mMaxLength);
+}
+
+void PulleyConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	ConstraintSettings::RestoreBinaryState(inStream);
+
+	inStream.Read(mSpace);
+	inStream.Read(mBodyPoint1);
+	inStream.Read(mFixedPoint1);
+	inStream.Read(mBodyPoint2);
+	inStream.Read(mFixedPoint2);
+	inStream.Read(mRatio);
+	inStream.Read(mMinLength);
+	inStream.Read(mMaxLength);
+}
+
+TwoBodyConstraint *PulleyConstraintSettings::Create(Body &inBody1, Body &inBody2) const
+{
+	return new PulleyConstraint(inBody1, inBody2, *this);
+}
+
+PulleyConstraint::PulleyConstraint(Body &inBody1, Body &inBody2, const PulleyConstraintSettings &inSettings) :
+	TwoBodyConstraint(inBody1, inBody2, inSettings),
+	mFixedPosition1(inSettings.mFixedPoint1),
+	mFixedPosition2(inSettings.mFixedPoint2),
+	mRatio(inSettings.mRatio),
+	mMinLength(inSettings.mMinLength),
+	mMaxLength(inSettings.mMaxLength)
+{
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		// If all properties were specified in world space, take them to local space now
+		mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mBodyPoint1);
+		mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mBodyPoint2);
+		mWorldSpacePosition1 = inSettings.mBodyPoint1;
+		mWorldSpacePosition2 = inSettings.mBodyPoint2;
+	}
+	else
+	{
+		// If properties were specified in local space, we need to calculate world space positions
+		mLocalSpacePosition1 = Vec3(inSettings.mBodyPoint1);
+		mLocalSpacePosition2 = Vec3(inSettings.mBodyPoint2);
+		mWorldSpacePosition1 = inBody1.GetCenterOfMassTransform() * inSettings.mBodyPoint1;
+		mWorldSpacePosition2 = inBody2.GetCenterOfMassTransform() * inSettings.mBodyPoint2;
+	}
+
+	// Calculate min/max length if it was not provided
+	float current_length = GetCurrentLength();
+	if (mMinLength < 0.0f)
+		mMinLength = current_length;
+	if (mMaxLength < 0.0f)
+		mMaxLength = current_length;
+
+	// Initialize the normals to a likely valid axis in case the fixed points overlap with the attachment points (most likely the fixed points are above both bodies)
+	mWorldSpaceNormal1 = mWorldSpaceNormal2 = -Vec3::sAxisY();
+}
+
+void PulleyConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
+{
+	if (mBody1->GetID() == inBodyID)
+		mLocalSpacePosition1 -= inDeltaCOM;
+	else if (mBody2->GetID() == inBodyID)
+		mLocalSpacePosition2 -= inDeltaCOM;
+}
+
+float PulleyConstraint::CalculatePositionsNormalsAndLength()
+{
+	// Update world space positions (the bodies may have moved)
+	mWorldSpacePosition1 = mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1;
+	mWorldSpacePosition2 = mBody2->GetCenterOfMassTransform() * mLocalSpacePosition2;
+
+	// Calculate world space normals
+	Vec3 delta1 = Vec3(mWorldSpacePosition1 - mFixedPosition1);
+	float delta1_len = delta1.Length();
+	if (delta1_len > 0.0f)
+		mWorldSpaceNormal1 = delta1 / delta1_len;
+
+	Vec3 delta2 = Vec3(mWorldSpacePosition2 - mFixedPosition2);
+	float delta2_len = delta2.Length();
+	if (delta2_len > 0.0f)
+		mWorldSpaceNormal2 = delta2 / delta2_len;
+
+	// Calculate length
+	return delta1_len + mRatio * delta2_len;
+}
+
+void PulleyConstraint::CalculateConstraintProperties()
+{
+	// Calculate attachment points relative to COM
+	Vec3 r1 = Vec3(mWorldSpacePosition1 - mBody1->GetCenterOfMassPosition());
+	Vec3 r2 = Vec3(mWorldSpacePosition2 - mBody2->GetCenterOfMassPosition());
+
+	mIndependentAxisConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, r1, mWorldSpaceNormal1, r2, mWorldSpaceNormal2, mRatio);
+}
+
+void PulleyConstraint::SetupVelocityConstraint(float inDeltaTime)
+{
+	// Determine if the constraint is active
+	float current_length = CalculatePositionsNormalsAndLength();
+	bool min_length_violation = current_length <= mMinLength;
+	bool max_length_violation = current_length >= mMaxLength;
+	if (min_length_violation || max_length_violation)
+	{
+		// Determine max lambda based on if the length is too big or small
+		mMinLambda = max_length_violation? -FLT_MAX : 0.0f;
+		mMaxLambda = min_length_violation? FLT_MAX : 0.0f;
+
+		CalculateConstraintProperties();
+	}
+	else
+		mIndependentAxisConstraintPart.Deactivate();
+}
+
+void PulleyConstraint::ResetWarmStart()
+{
+	mIndependentAxisConstraintPart.Deactivate();
+}
+
+void PulleyConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
+{
+	mIndependentAxisConstraintPart.WarmStart(*mBody1, *mBody2, mWorldSpaceNormal1, mWorldSpaceNormal2, mRatio, inWarmStartImpulseRatio);
+}
+
+bool PulleyConstraint::SolveVelocityConstraint(float inDeltaTime)
+{
+	if (mIndependentAxisConstraintPart.IsActive())
+		return mIndependentAxisConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceNormal1, mWorldSpaceNormal2, mRatio, mMinLambda, mMaxLambda);
+	else
+		return false;
+}
+
+bool PulleyConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
+{
+	// Calculate new length (bodies may have changed)
+	float current_length = CalculatePositionsNormalsAndLength();
+
+	float position_error = 0.0f;
+	if (current_length < mMinLength)
+		position_error = current_length - mMinLength;
+	else if (current_length > mMaxLength)
+		position_error = current_length - mMaxLength;
+
+	if (position_error != 0.0f)
+	{
+		// Update constraint properties (bodies may have moved)
+		CalculateConstraintProperties();
+
+		return mIndependentAxisConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mWorldSpaceNormal1, mWorldSpaceNormal2, mRatio, position_error, inBaumgarte);
+	}
+
+	return false;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void PulleyConstraint::DrawConstraint(DebugRenderer *inRenderer) const
+{
+	// Color according to length vs min/max length
+	float current_length = GetCurrentLength();
+	Color color = Color::sGreen;
+	if (current_length < mMinLength)
+		color = Color::sYellow;
+	else if (current_length > mMaxLength)
+		color = Color::sRed;
+
+	// Draw constraint
+	inRenderer->DrawLine(mWorldSpacePosition1, mFixedPosition1, color);
+	inRenderer->DrawLine(mFixedPosition1, mFixedPosition2, color);
+	inRenderer->DrawLine(mFixedPosition2, mWorldSpacePosition2, color);
+
+	// Draw current length
+	inRenderer->DrawText3D(0.5_r * (mFixedPosition1 + mFixedPosition2), StringFormat("%.2f", (double)current_length));
+}
+#endif // JPH_DEBUG_RENDERER
+
+void PulleyConstraint::SaveState(StateRecorder &inStream) const
+{
+	TwoBodyConstraint::SaveState(inStream);
+
+	mIndependentAxisConstraintPart.SaveState(inStream);
+	inStream.Write(mWorldSpaceNormal1); // When distance to fixed point = 0, the normal is used from last frame so we need to store it
+	inStream.Write(mWorldSpaceNormal2);
+}
+
+void PulleyConstraint::RestoreState(StateRecorder &inStream)
+{
+	TwoBodyConstraint::RestoreState(inStream);
+
+	mIndependentAxisConstraintPart.RestoreState(inStream);
+	inStream.Read(mWorldSpaceNormal1);
+	inStream.Read(mWorldSpaceNormal2);
+}
+
+Ref<ConstraintSettings> PulleyConstraint::GetConstraintSettings() const
+{
+	PulleyConstraintSettings *settings = new PulleyConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mBodyPoint1 = RVec3(mLocalSpacePosition1);
+	settings->mFixedPoint1 = mFixedPosition1;
+	settings->mBodyPoint2 = RVec3(mLocalSpacePosition2);
+	settings->mFixedPoint2 = mFixedPosition2;
+	settings->mRatio = mRatio;
+	settings->mMinLength = mMinLength;
+	settings->mMaxLength = mMaxLength;
+	return settings;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/PulleyConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PulleyConstraint.h
new file mode 100644
index 0000000000..092be68c1b
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/PulleyConstraint.h
@@ -0,0 +1,137 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/IndependentAxisConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Pulley constraint settings, used to create a pulley constraint.
+/// A pulley connects two bodies via two fixed world points to each other similar to a distance constraint.
+/// We define Length1 = |BodyPoint1 - FixedPoint1| where Body1 is a point on body 1 in world space and FixedPoint1 a fixed point in world space
+/// Length2 = |BodyPoint2 - FixedPoint2|
+/// The constraint keeps the two line segments constrained so that
+/// MinDistance <= Length1 + Ratio * Length2 <= MaxDistance
+class JPH_EXPORT PulleyConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PulleyConstraintSettings)
+
+public:
+	// See: ConstraintSettings::SaveBinaryState
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+
+	/// Create an instance of this constraint
+	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
+
+	/// This determines in which space the constraint is setup, specified properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	/// Body 1 constraint attachment point (space determined by mSpace).
+	RVec3						mBodyPoint1 = RVec3::sZero();
+
+	/// Fixed world point to which body 1 is connected (always world space)
+	RVec3						mFixedPoint1 = RVec3::sZero();
+
+	/// Body 2 constraint attachment point (space determined by mSpace)
+	RVec3						mBodyPoint2 = RVec3::sZero();
+
+	/// Fixed world point to which body 2 is connected (always world space)
+	RVec3						mFixedPoint2 = RVec3::sZero();
+
+	/// Ratio between the two line segments (see formula above), can be used to create a block and tackle
+	float						mRatio = 1.0f;
+
+	/// The minimum length of the line segments (see formula above), use -1 to calculate the length based on the positions of the objects when the constraint is created.
+	float						mMinLength = 0.0f;
+
+	/// The maximum length of the line segments (see formula above), use -1 to calculate the length based on the positions of the objects when the constraint is created.
+	float						mMaxLength = -1.0f;
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// A pulley constraint.
+class JPH_EXPORT PulleyConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct distance constraint
+								PulleyConstraint(Body &inBody1, Body &inBody2, const PulleyConstraintSettings &inSettings);
+
+	// Generic interface of a constraint
+	virtual EConstraintSubType	GetSubType() const override									{ return EConstraintSubType::Pulley; }
+	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
+	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
+	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
+	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
+	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void				DrawConstraint(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual void				SaveState(StateRecorder &inStream) const override;
+	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
+
+	// See: TwoBodyConstraint
+	virtual Mat44				GetConstraintToBody1Matrix() const override					{ return Mat44::sTranslation(mLocalSpacePosition1); }
+	virtual Mat44				GetConstraintToBody2Matrix() const override					{ return Mat44::sTranslation(mLocalSpacePosition2); } // Note: Incorrect rotation as we don't track the original rotation difference, should not matter though as the constraint is not limiting rotation.
+
+	/// Update the minimum and maximum length for the constraint
+	void						SetLength(float inMinLength, float inMaxLength)				{ JPH_ASSERT(inMinLength >= 0.0f && inMinLength <= inMaxLength); mMinLength = inMinLength; mMaxLength = inMaxLength; }
+	float						GetMinLength() const										{ return mMinLength; }
+	float						GetMaxLength() const										{ return mMaxLength; }
+
+	/// Get the current length of both segments (multiplied by the ratio for segment 2)
+	float						GetCurrentLength() const									{ return Vec3(mWorldSpacePosition1 - mFixedPosition1).Length() + mRatio * Vec3(mWorldSpacePosition2 - mFixedPosition2).Length(); }
+
+	///@name Get Lagrange multiplier from last physics update (the linear impulse applied to satisfy the constraint)
+	inline float				GetTotalLambdaPosition() const								{ return mIndependentAxisConstraintPart.GetTotalLambda(); }
+
+private:
+	// Calculates world positions and normals and returns current length
+	float						CalculatePositionsNormalsAndLength();
+
+	// Internal helper function to calculate the values below
+	void						CalculateConstraintProperties();
+
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	// Local space constraint positions on the bodies
+	Vec3						mLocalSpacePosition1;
+	Vec3						mLocalSpacePosition2;
+
+	// World space fixed positions
+	RVec3						mFixedPosition1;
+	RVec3						mFixedPosition2;
+
+	/// Ratio between the two line segments
+	float						mRatio;
+
+	// The minimum/maximum length of the line segments
+	float						mMinLength;
+	float						mMaxLength;
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	// World space positions and normal
+	RVec3						mWorldSpacePosition1;
+	RVec3						mWorldSpacePosition2;
+	Vec3						mWorldSpaceNormal1;
+	Vec3						mWorldSpaceNormal2;
+
+	// Depending on if the length < min or length > max we can apply forces to prevent further violations
+	float						mMinLambda;
+	float						mMaxLambda;
+
+	// The constraint part
+	IndependentAxisConstraintPart mIndependentAxisConstraintPart;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/RackAndPinionConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/RackAndPinionConstraint.cpp
new file mode 100644
index 0000000000..d0bcb62feb
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/RackAndPinionConstraint.cpp
@@ -0,0 +1,189 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/RackAndPinionConstraint.h>
+#include <Jolt/Physics/Constraints/HingeConstraint.h>
+#include <Jolt/Physics/Constraints/SliderConstraint.h>
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(RackAndPinionConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(RackAndPinionConstraintSettings, TwoBodyConstraintSettings)
+
+	JPH_ADD_ENUM_ATTRIBUTE(RackAndPinionConstraintSettings, mSpace)
+	JPH_ADD_ATTRIBUTE(RackAndPinionConstraintSettings, mHingeAxis)
+	JPH_ADD_ATTRIBUTE(RackAndPinionConstraintSettings, mSliderAxis)
+	JPH_ADD_ATTRIBUTE(RackAndPinionConstraintSettings, mRatio)
+}
+
+void RackAndPinionConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	ConstraintSettings::SaveBinaryState(inStream);
+
+	inStream.Write(mSpace);
+	inStream.Write(mHingeAxis);
+	inStream.Write(mSliderAxis);
+	inStream.Write(mRatio);
+}
+
+void RackAndPinionConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	ConstraintSettings::RestoreBinaryState(inStream);
+
+	inStream.Read(mSpace);
+	inStream.Read(mHingeAxis);
+	inStream.Read(mSliderAxis);
+	inStream.Read(mRatio);
+}
+
+TwoBodyConstraint *RackAndPinionConstraintSettings::Create(Body &inBody1, Body &inBody2) const
+{
+	return new RackAndPinionConstraint(inBody1, inBody2, *this);
+}
+
+RackAndPinionConstraint::RackAndPinionConstraint(Body &inBody1, Body &inBody2, const RackAndPinionConstraintSettings &inSettings) :
+	TwoBodyConstraint(inBody1, inBody2, inSettings),
+	mLocalSpaceHingeAxis(inSettings.mHingeAxis),
+	mLocalSpaceSliderAxis(inSettings.mSliderAxis),
+	mRatio(inSettings.mRatio)
+{
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		// If all properties were specified in world space, take them to local space now
+		mLocalSpaceHingeAxis = inBody1.GetInverseCenterOfMassTransform().Multiply3x3(mLocalSpaceHingeAxis).Normalized();
+		mLocalSpaceSliderAxis = inBody2.GetInverseCenterOfMassTransform().Multiply3x3(mLocalSpaceSliderAxis).Normalized();
+	}
+}
+
+void RackAndPinionConstraint::CalculateConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2)
+{
+	// Calculate world space normals
+	mWorldSpaceHingeAxis = inRotation1 * mLocalSpaceHingeAxis;
+	mWorldSpaceSliderAxis = inRotation2 * mLocalSpaceSliderAxis;
+
+	mRackAndPinionConstraintPart.CalculateConstraintProperties(*mBody1, mWorldSpaceHingeAxis, *mBody2, mWorldSpaceSliderAxis, mRatio);
+}
+
+void RackAndPinionConstraint::SetupVelocityConstraint(float inDeltaTime)
+{
+	// Calculate constraint properties that are constant while bodies don't move
+	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
+	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
+	CalculateConstraintProperties(rotation1, rotation2);
+}
+
+void RackAndPinionConstraint::ResetWarmStart()
+{
+	mRackAndPinionConstraintPart.Deactivate();
+}
+
+void RackAndPinionConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
+{
+	// Warm starting: Apply previous frame impulse
+	mRackAndPinionConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+}
+
+bool RackAndPinionConstraint::SolveVelocityConstraint(float inDeltaTime)
+{
+	return mRackAndPinionConstraintPart.SolveVelocityConstraint(*mBody1, mWorldSpaceHingeAxis, *mBody2, mWorldSpaceSliderAxis, mRatio);
+}
+
+bool RackAndPinionConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
+{
+	if (mRackConstraint == nullptr || mPinionConstraint == nullptr)
+		return false;
+
+	float rotation;
+	if (mPinionConstraint->GetSubType() == EConstraintSubType::Hinge)
+	{
+		rotation = StaticCast<HingeConstraint>(mPinionConstraint)->GetCurrentAngle();
+	}
+	else
+	{
+		JPH_ASSERT(false, "Unsupported");
+		return false;
+	}
+
+	float translation;
+	if (mRackConstraint->GetSubType() == EConstraintSubType::Slider)
+	{
+		translation = StaticCast<SliderConstraint>(mRackConstraint)->GetCurrentPosition();
+	}
+	else
+	{
+		JPH_ASSERT(false, "Unsupported");
+		return false;
+	}
+
+	float error = CenterAngleAroundZero(fmod(rotation - mRatio * translation, 2.0f * JPH_PI));
+	if (error == 0.0f)
+		return false;
+
+	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
+	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
+	CalculateConstraintProperties(rotation1, rotation2);
+	return mRackAndPinionConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, error, inBaumgarte);
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void RackAndPinionConstraint::DrawConstraint(DebugRenderer *inRenderer) const
+{
+	RMat44 transform1 = mBody1->GetCenterOfMassTransform();
+	RMat44 transform2 = mBody2->GetCenterOfMassTransform();
+
+	// Draw constraint axis
+	inRenderer->DrawArrow(transform1.GetTranslation(), transform1 * mLocalSpaceHingeAxis, Color::sGreen, 0.01f);
+	inRenderer->DrawArrow(transform2.GetTranslation(), transform2 * mLocalSpaceSliderAxis, Color::sBlue, 0.01f);
+}
+
+#endif // JPH_DEBUG_RENDERER
+
+void RackAndPinionConstraint::SaveState(StateRecorder &inStream) const
+{
+	TwoBodyConstraint::SaveState(inStream);
+
+	mRackAndPinionConstraintPart.SaveState(inStream);
+}
+
+void RackAndPinionConstraint::RestoreState(StateRecorder &inStream)
+{
+	TwoBodyConstraint::RestoreState(inStream);
+
+	mRackAndPinionConstraintPart.RestoreState(inStream);
+}
+
+Ref<ConstraintSettings> RackAndPinionConstraint::GetConstraintSettings() const
+{
+	RackAndPinionConstraintSettings *settings = new RackAndPinionConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mHingeAxis = mLocalSpaceHingeAxis;
+	settings->mSliderAxis = mLocalSpaceSliderAxis;
+	settings->mRatio = mRatio;
+	return settings;
+}
+
+Mat44 RackAndPinionConstraint::GetConstraintToBody1Matrix() const
+{
+	Vec3 perp = mLocalSpaceHingeAxis.GetNormalizedPerpendicular();
+	return Mat44(Vec4(mLocalSpaceHingeAxis, 0), Vec4(perp, 0), Vec4(mLocalSpaceHingeAxis.Cross(perp), 0), Vec4(0, 0, 0, 1));
+}
+
+Mat44 RackAndPinionConstraint::GetConstraintToBody2Matrix() const
+{
+	Vec3 perp = mLocalSpaceSliderAxis.GetNormalizedPerpendicular();
+	return Mat44(Vec4(mLocalSpaceSliderAxis, 0), Vec4(perp, 0), Vec4(mLocalSpaceSliderAxis.Cross(perp), 0), Vec4(0, 0, 0, 1));
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/RackAndPinionConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/RackAndPinionConstraint.h
new file mode 100644
index 0000000000..f26af31f1b
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/RackAndPinionConstraint.h
@@ -0,0 +1,118 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/RackAndPinionConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Rack and pinion constraint (slider & gear) settings
+class JPH_EXPORT RackAndPinionConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, RackAndPinionConstraintSettings)
+
+public:
+	// See: ConstraintSettings::SaveBinaryState
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+
+	/// Create an instance of this constraint.
+	/// Body1 should be the pinion (gear) and body 2 the rack (slider).
+	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
+
+	/// Defines the ratio between the rotation of the pinion and the translation of the rack.
+	/// The ratio is defined as: PinionRotation(t) = ratio * RackTranslation(t)
+	/// @param inNumTeethRack Number of teeth that the rack has
+	/// @param inRackLength Length of the rack
+	/// @param inNumTeethPinion Number of teeth the pinion has
+	void						SetRatio(int inNumTeethRack, float inRackLength, int inNumTeethPinion)
+	{
+		mRatio = 2.0f * JPH_PI * inNumTeethRack / (inRackLength * inNumTeethPinion);
+	}
+
+	/// This determines in which space the constraint is setup, all properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	/// Body 1 (pinion) constraint reference frame (space determined by mSpace).
+	Vec3						mHingeAxis = Vec3::sAxisX();
+
+	/// Body 2 (rack) constraint reference frame (space determined by mSpace)
+	Vec3						mSliderAxis = Vec3::sAxisX();
+
+	/// Ratio between the rack and pinion, see SetRatio.
+	float						mRatio = 1.0f;
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// A rack and pinion constraint constrains the rotation of body1 to the translation of body 2.
+/// Note that this constraint needs to be used in conjunction with a hinge constraint for body 1 and a slider constraint for body 2.
+class JPH_EXPORT RackAndPinionConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct gear constraint
+								RackAndPinionConstraint(Body &inBody1, Body &inBody2, const RackAndPinionConstraintSettings &inSettings);
+
+	// Generic interface of a constraint
+	virtual EConstraintSubType	GetSubType() const override												{ return EConstraintSubType::RackAndPinion; }
+	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override { /* Nothing */ }
+	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
+	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
+	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
+	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void				DrawConstraint(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual void				SaveState(StateRecorder &inStream) const override;
+	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
+
+	// See: TwoBodyConstraint
+	virtual Mat44				GetConstraintToBody1Matrix() const override;
+	virtual Mat44				GetConstraintToBody2Matrix() const override;
+
+	/// The constraints that constrain the rack and pinion (a slider and a hinge), optional and used to calculate the position error and fix numerical drift.
+	void						SetConstraints(const Constraint *inPinion, const Constraint *inRack)	{ mPinionConstraint = inPinion; mRackConstraint = inRack; }
+
+	///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint)
+	inline float				GetTotalLambda() const													{ return mRackAndPinionConstraintPart.GetTotalLambda(); }
+
+private:
+	// Internal helper function to calculate the values below
+	void						CalculateConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2);
+
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	// Local space hinge axis
+	Vec3						mLocalSpaceHingeAxis;
+
+	// Local space sliding direction
+	Vec3						mLocalSpaceSliderAxis;
+
+	// Ratio between rack and pinion
+	float						mRatio;
+
+	// The constraints that constrain the rack and pinion (a slider and a hinge), optional and used to calculate the position error and fix numerical drift.
+	RefConst<Constraint>		mPinionConstraint;
+	RefConst<Constraint>		mRackConstraint;
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	// World space hinge axis
+	Vec3						mWorldSpaceHingeAxis;
+
+	// World space sliding direction
+	Vec3						mWorldSpaceSliderAxis;
+
+	// The constraint parts
+	RackAndPinionConstraintPart	mRackAndPinionConstraintPart;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/SixDOFConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SixDOFConstraint.cpp
new file mode 100644
index 0000000000..070a45e213
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SixDOFConstraint.cpp
@@ -0,0 +1,900 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/SixDOFConstraint.h>
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Geometry/Ellipse.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SixDOFConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(SixDOFConstraintSettings, TwoBodyConstraintSettings)
+
+	JPH_ADD_ENUM_ATTRIBUTE(SixDOFConstraintSettings, mSpace)
+	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mPosition1)
+	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mAxisX1)
+	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mAxisY1)
+	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mPosition2)
+	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mAxisX2)
+	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mAxisY2)
+	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mMaxFriction)
+	JPH_ADD_ENUM_ATTRIBUTE(SixDOFConstraintSettings, mSwingType)
+	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mLimitMin)
+	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mLimitMax)
+	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mLimitsSpringSettings)
+	JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mMotorSettings)
+}
+
+void SixDOFConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	ConstraintSettings::SaveBinaryState(inStream);
+
+	inStream.Write(mSpace);
+	inStream.Write(mPosition1);
+	inStream.Write(mAxisX1);
+	inStream.Write(mAxisY1);
+	inStream.Write(mPosition2);
+	inStream.Write(mAxisX2);
+	inStream.Write(mAxisY2);
+	inStream.Write(mMaxFriction);
+	inStream.Write(mSwingType);
+	inStream.Write(mLimitMin);
+	inStream.Write(mLimitMax);
+	for (const SpringSettings &s : mLimitsSpringSettings)
+		s.SaveBinaryState(inStream);
+	for (const MotorSettings &m : mMotorSettings)
+		m.SaveBinaryState(inStream);
+}
+
+void SixDOFConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	ConstraintSettings::RestoreBinaryState(inStream);
+
+	inStream.Read(mSpace);
+	inStream.Read(mPosition1);
+	inStream.Read(mAxisX1);
+	inStream.Read(mAxisY1);
+	inStream.Read(mPosition2);
+	inStream.Read(mAxisX2);
+	inStream.Read(mAxisY2);
+	inStream.Read(mMaxFriction);
+	inStream.Read(mSwingType);
+	inStream.Read(mLimitMin);
+	inStream.Read(mLimitMax);
+	for (SpringSettings &s : mLimitsSpringSettings)
+		s.RestoreBinaryState(inStream);
+	for (MotorSettings &m : mMotorSettings)
+		m.RestoreBinaryState(inStream);
+}
+
+TwoBodyConstraint *SixDOFConstraintSettings::Create(Body &inBody1, Body &inBody2) const
+{
+	return new SixDOFConstraint(inBody1, inBody2, *this);
+}
+
+void SixDOFConstraint::UpdateTranslationLimits()
+{
+	// Set to zero if the limits are inversed
+	for (int i = EAxis::TranslationX; i <= EAxis::TranslationZ; ++i)
+		if (mLimitMin[i] > mLimitMax[i])
+			mLimitMin[i] = mLimitMax[i] = 0.0f;
+}
+
+void SixDOFConstraint::UpdateRotationLimits()
+{
+	if (mSwingTwistConstraintPart.GetSwingType() == ESwingType::Cone)
+	{
+		// Cone swing upper limit needs to be positive
+		mLimitMax[EAxis::RotationY] = max(0.0f, mLimitMax[EAxis::RotationY]);
+		mLimitMax[EAxis::RotationZ] = max(0.0f, mLimitMax[EAxis::RotationZ]);
+
+		// Cone swing limits only support symmetric ranges
+		mLimitMin[EAxis::RotationY] = -mLimitMax[EAxis::RotationY];
+		mLimitMin[EAxis::RotationZ] = -mLimitMax[EAxis::RotationZ];
+	}
+
+	for (int i = EAxis::RotationX; i <= EAxis::RotationZ; ++i)
+	{
+		// Clamp to [-PI, PI] range
+		mLimitMin[i] = Clamp(mLimitMin[i], -JPH_PI, JPH_PI);
+		mLimitMax[i] = Clamp(mLimitMax[i], -JPH_PI, JPH_PI);
+
+		// Set to zero if the limits are inversed
+		if (mLimitMin[i] > mLimitMax[i])
+			mLimitMin[i] = mLimitMax[i] = 0.0f;
+	}
+
+	// Pass limits on to constraint part
+	mSwingTwistConstraintPart.SetLimits(mLimitMin[EAxis::RotationX], mLimitMax[EAxis::RotationX], mLimitMin[EAxis::RotationY], mLimitMax[EAxis::RotationY], mLimitMin[EAxis::RotationZ], mLimitMax[EAxis::RotationZ]);
+}
+
+void SixDOFConstraint::UpdateFixedFreeAxis()
+{
+	uint8 old_free_axis = mFreeAxis;
+	uint8 old_fixed_axis = mFixedAxis;
+
+	// Cache which axis are fixed and which ones are free
+	mFreeAxis = 0;
+	mFixedAxis = 0;
+	for (int a = 0; a < EAxis::Num; ++a)
+	{
+		float limit = a >= EAxis::RotationX? JPH_PI : FLT_MAX;
+
+		if (mLimitMin[a] >= mLimitMax[a])
+			mFixedAxis |= 1 << a;
+		else if (mLimitMin[a] <= -limit && mLimitMax[a] >= limit)
+			mFreeAxis |= 1 << a;
+	}
+
+	// On change we deactivate all constraints to reset warm starting
+	if (old_free_axis != mFreeAxis || old_fixed_axis != mFixedAxis)
+	{
+		for (AxisConstraintPart &c : mTranslationConstraintPart)
+			c.Deactivate();
+		mPointConstraintPart.Deactivate();
+		mSwingTwistConstraintPart.Deactivate();
+		mRotationConstraintPart.Deactivate();
+		for (AxisConstraintPart &c : mMotorTranslationConstraintPart)
+			c.Deactivate();
+		for (AngleConstraintPart &c : mMotorRotationConstraintPart)
+			c.Deactivate();
+	}
+}
+
+SixDOFConstraint::SixDOFConstraint(Body &inBody1, Body &inBody2, const SixDOFConstraintSettings &inSettings) :
+	TwoBodyConstraint(inBody1, inBody2, inSettings)
+{
+	// Override swing type
+	mSwingTwistConstraintPart.SetSwingType(inSettings.mSwingType);
+
+	// Calculate rotation needed to go from constraint space to body1 local space
+	Vec3 axis_z1 = inSettings.mAxisX1.Cross(inSettings.mAxisY1);
+	Mat44 c_to_b1(Vec4(inSettings.mAxisX1, 0), Vec4(inSettings.mAxisY1, 0), Vec4(axis_z1, 0), Vec4(0, 0, 0, 1));
+	mConstraintToBody1 = c_to_b1.GetQuaternion();
+
+	// Calculate rotation needed to go from constraint space to body2 local space
+	Vec3 axis_z2 = inSettings.mAxisX2.Cross(inSettings.mAxisY2);
+	Mat44 c_to_b2(Vec4(inSettings.mAxisX2, 0), Vec4(inSettings.mAxisY2, 0), Vec4(axis_z2, 0), Vec4(0, 0, 0, 1));
+	mConstraintToBody2 = c_to_b2.GetQuaternion();
+
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		// If all properties were specified in world space, take them to local space now
+		mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mPosition1);
+		mConstraintToBody1 = inBody1.GetRotation().Conjugated() * mConstraintToBody1;
+
+		mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mPosition2);
+		mConstraintToBody2 = inBody2.GetRotation().Conjugated() * mConstraintToBody2;
+	}
+	else
+	{
+		mLocalSpacePosition1 = Vec3(inSettings.mPosition1);
+		mLocalSpacePosition2 = Vec3(inSettings.mPosition2);
+	}
+
+	// Copy translation and rotation limits
+	memcpy(mLimitMin, inSettings.mLimitMin, sizeof(mLimitMin));
+	memcpy(mLimitMax, inSettings.mLimitMax, sizeof(mLimitMax));
+	memcpy(mLimitsSpringSettings, inSettings.mLimitsSpringSettings, sizeof(mLimitsSpringSettings));
+	UpdateTranslationLimits();
+	UpdateRotationLimits();
+	UpdateFixedFreeAxis();
+	CacheHasSpringLimits();
+
+	// Store friction settings
+	memcpy(mMaxFriction, inSettings.mMaxFriction, sizeof(mMaxFriction));
+
+	// Store motor settings
+	for (int i = 0; i < EAxis::Num; ++i)
+		mMotorSettings[i] = inSettings.mMotorSettings[i];
+
+	// Cache if motors are active (motors are off initially, but we may have friction)
+	CacheTranslationMotorActive();
+	CacheRotationMotorActive();
+}
+
+void SixDOFConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
+{
+	if (mBody1->GetID() == inBodyID)
+		mLocalSpacePosition1 -= inDeltaCOM;
+	else if (mBody2->GetID() == inBodyID)
+		mLocalSpacePosition2 -= inDeltaCOM;
+}
+
+void SixDOFConstraint::SetTranslationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax)
+{
+	mLimitMin[EAxis::TranslationX] = inLimitMin.GetX();
+	mLimitMin[EAxis::TranslationY] = inLimitMin.GetY();
+	mLimitMin[EAxis::TranslationZ] = inLimitMin.GetZ();
+	mLimitMax[EAxis::TranslationX] = inLimitMax.GetX();
+	mLimitMax[EAxis::TranslationY] = inLimitMax.GetY();
+	mLimitMax[EAxis::TranslationZ] = inLimitMax.GetZ();
+
+	UpdateTranslationLimits();
+	UpdateFixedFreeAxis();
+}
+
+void SixDOFConstraint::SetRotationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax)
+{
+	mLimitMin[EAxis::RotationX] = inLimitMin.GetX();
+	mLimitMin[EAxis::RotationY] = inLimitMin.GetY();
+	mLimitMin[EAxis::RotationZ] = inLimitMin.GetZ();
+	mLimitMax[EAxis::RotationX] = inLimitMax.GetX();
+	mLimitMax[EAxis::RotationY] = inLimitMax.GetY();
+	mLimitMax[EAxis::RotationZ] = inLimitMax.GetZ();
+
+	UpdateRotationLimits();
+	UpdateFixedFreeAxis();
+}
+
+void SixDOFConstraint::SetMaxFriction(EAxis inAxis, float inFriction)
+{
+	mMaxFriction[inAxis] = inFriction;
+
+	if (inAxis >= EAxis::TranslationX && inAxis <= EAxis::TranslationZ)
+		CacheTranslationMotorActive();
+	else
+		CacheRotationMotorActive();
+}
+
+void SixDOFConstraint::GetPositionConstraintProperties(Vec3 &outR1PlusU, Vec3 &outR2, Vec3 &outU) const
+{
+	RVec3 p1 = mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1;
+	RVec3 p2 = mBody2->GetCenterOfMassTransform() * mLocalSpacePosition2;
+	outR1PlusU = Vec3(p2 - mBody1->GetCenterOfMassPosition()); // r1 + u = (p1 - x1) + (p2 - p1) = p2 - x1
+	outR2 = Vec3(p2 - mBody2->GetCenterOfMassPosition());
+	outU = Vec3(p2 - p1);
+}
+
+Quat SixDOFConstraint::GetRotationInConstraintSpace() const
+{
+	// Let b1, b2 be the center of mass transform of body1 and body2 (For body1 this is mBody1->GetCenterOfMassTransform())
+	// Let c1, c2 be the transform that takes a vector from constraint space to local space of body1 and body2 (For body1 this is Mat44::sRotationTranslation(mConstraintToBody1, mLocalSpacePosition1))
+	// Let q be the rotation of the constraint in constraint space
+	// b2 takes a vector from the local space of body2 to world space
+	// To express this in terms of b1: b2 = b1 * c1 * q * c2^-1
+	// c2^-1 goes from local body 2 space to constraint space
+	// q rotates the constraint
+	// c1 goes from constraint space to body 1 local space
+	// b1 goes from body 1 local space to world space
+	// So when the body rotations are given, q = (b1 * c1)^-1 * b2 c2
+	// Or: q = (q1 * c1)^-1 * (q2 * c2) if we're only interested in rotations
+	return (mBody1->GetRotation() * mConstraintToBody1).Conjugated() * mBody2->GetRotation() * mConstraintToBody2;
+}
+
+void SixDOFConstraint::CacheTranslationMotorActive()
+{
+	mTranslationMotorActive = mMotorState[EAxis::TranslationX] != EMotorState::Off
+		|| mMotorState[EAxis::TranslationY] != EMotorState::Off
+		|| mMotorState[EAxis::TranslationZ] != EMotorState::Off
+		|| HasFriction(EAxis::TranslationX)
+		|| HasFriction(EAxis::TranslationY)
+		|| HasFriction(EAxis::TranslationZ);
+}
+
+void SixDOFConstraint::CacheRotationMotorActive()
+{
+	mRotationMotorActive = mMotorState[EAxis::RotationX] != EMotorState::Off
+		|| mMotorState[EAxis::RotationY] != EMotorState::Off
+		|| mMotorState[EAxis::RotationZ] != EMotorState::Off
+		|| HasFriction(EAxis::RotationX)
+		|| HasFriction(EAxis::RotationY)
+		|| HasFriction(EAxis::RotationZ);
+}
+
+void SixDOFConstraint::CacheRotationPositionMotorActive()
+{
+	mRotationPositionMotorActive = 0;
+	for (int i = 0; i < 3; ++i)
+		if (mMotorState[EAxis::RotationX + i] == EMotorState::Position)
+			mRotationPositionMotorActive |= 1 << i;
+}
+
+void SixDOFConstraint::CacheHasSpringLimits()
+{
+	mHasSpringLimits = mLimitsSpringSettings[EAxis::TranslationX].mFrequency > 0.0f
+		|| mLimitsSpringSettings[EAxis::TranslationY].mFrequency > 0.0f
+		|| mLimitsSpringSettings[EAxis::TranslationZ].mFrequency > 0.0f;
+}
+
+void SixDOFConstraint::SetMotorState(EAxis inAxis, EMotorState inState)
+{
+	JPH_ASSERT(inState == EMotorState::Off || mMotorSettings[inAxis].IsValid());
+
+	if (mMotorState[inAxis] != inState)
+	{
+		mMotorState[inAxis] = inState;
+
+		// Ensure that warm starting next frame doesn't apply any impulses (motor parts are repurposed for different modes)
+		if (inAxis >= EAxis::TranslationX && inAxis <= EAxis::TranslationZ)
+		{
+			mMotorTranslationConstraintPart[inAxis - EAxis::TranslationX].Deactivate();
+
+			CacheTranslationMotorActive();
+		}
+		else
+		{
+			JPH_ASSERT(inAxis >= EAxis::RotationX && inAxis <= EAxis::RotationZ);
+
+			mMotorRotationConstraintPart[inAxis - EAxis::RotationX].Deactivate();
+
+			CacheRotationMotorActive();
+			CacheRotationPositionMotorActive();
+		}
+	}
+}
+
+void SixDOFConstraint::SetTargetOrientationCS(QuatArg inOrientation)
+{
+	Quat q_swing, q_twist;
+	inOrientation.GetSwingTwist(q_swing, q_twist);
+
+	uint clamped_axis;
+	mSwingTwistConstraintPart.ClampSwingTwist(q_swing, q_twist, clamped_axis);
+
+	if (clamped_axis != 0)
+		mTargetOrientation = q_swing * q_twist;
+	else
+		mTargetOrientation = inOrientation;
+}
+
+void SixDOFConstraint::SetupVelocityConstraint(float inDeltaTime)
+{
+	// Get body rotations
+	Quat rotation1 = mBody1->GetRotation();
+	Quat rotation2 = mBody2->GetRotation();
+
+	// Quaternion that rotates from body1's constraint space to world space
+	Quat constraint_body1_to_world = rotation1 * mConstraintToBody1;
+
+	// Store world space axis of constraint space
+	Mat44 translation_axis_mat = Mat44::sRotation(constraint_body1_to_world);
+	for (int i = 0; i < 3; ++i)
+		mTranslationAxis[i] = translation_axis_mat.GetColumn3(i);
+
+	if (IsTranslationFullyConstrained())
+	{
+		// All translation locked: Setup point constraint
+		mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(rotation1), mLocalSpacePosition1, *mBody2, Mat44::sRotation(rotation2), mLocalSpacePosition2);
+	}
+	else if (IsTranslationConstrained() || mTranslationMotorActive)
+	{
+		// Update world space positions (the bodies may have moved)
+		Vec3 r1_plus_u, r2, u;
+		GetPositionConstraintProperties(r1_plus_u, r2, u);
+
+		// Setup axis constraint parts
+		for (int i = 0; i < 3; ++i)
+		{
+			EAxis axis = EAxis(EAxis::TranslationX + i);
+
+			Vec3 translation_axis = mTranslationAxis[i];
+
+			// Calculate displacement along this axis
+			float d = translation_axis.Dot(u);
+			mDisplacement[i] = d; // Store for SolveVelocityConstraint
+
+			// Setup limit constraint
+			bool constraint_active = false;
+			float constraint_value = 0.0f;
+			if (IsFixedAxis(axis))
+			{
+				// When constraint is fixed it is always active
+				constraint_value = d - mLimitMin[i];
+				constraint_active = true;
+			}
+			else if (!IsFreeAxis(axis))
+			{
+				// When constraint is limited, it is only active when outside of the allowed range
+				if (d <= mLimitMin[i])
+				{
+					constraint_value = d - mLimitMin[i];
+					constraint_active = true;
+				}
+				else if (d >= mLimitMax[i])
+				{
+					constraint_value = d - mLimitMax[i];
+					constraint_active = true;
+				}
+			}
+
+			if (constraint_active)
+				mTranslationConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, translation_axis, 0.0f, constraint_value, mLimitsSpringSettings[i]);
+			else
+				mTranslationConstraintPart[i].Deactivate();
+
+			// Setup motor constraint
+			switch (mMotorState[i])
+			{
+			case EMotorState::Off:
+				if (HasFriction(axis))
+					mMotorTranslationConstraintPart[i].CalculateConstraintProperties(*mBody1, r1_plus_u, *mBody2, r2, translation_axis);
+				else
+					mMotorTranslationConstraintPart[i].Deactivate();
+				break;
+
+			case EMotorState::Velocity:
+				mMotorTranslationConstraintPart[i].CalculateConstraintProperties(*mBody1, r1_plus_u, *mBody2, r2, translation_axis, -mTargetVelocity[i]);
+				break;
+
+			case EMotorState::Position:
+				{
+					const SpringSettings &spring_settings = mMotorSettings[i].mSpringSettings;
+					if (spring_settings.HasStiffness())
+						mMotorTranslationConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, translation_axis, 0.0f, translation_axis.Dot(u) - mTargetPosition[i], spring_settings);
+					else
+						mMotorTranslationConstraintPart[i].Deactivate();
+					break;
+				}
+			}
+		}
+	}
+
+	// Setup rotation constraints
+	if (IsRotationFullyConstrained())
+	{
+		// All rotation locked: Setup rotation constraint
+		mRotationConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), *mBody2, Mat44::sRotation(mBody2->GetRotation()));
+	}
+	else if (IsRotationConstrained() || mRotationMotorActive)
+	{
+		// GetRotationInConstraintSpace without redoing the calculation of constraint_body1_to_world
+		Quat constraint_body2_to_world = mBody2->GetRotation() * mConstraintToBody2;
+		Quat q = constraint_body1_to_world.Conjugated() * constraint_body2_to_world;
+
+		// Use swing twist constraint part
+		if (IsRotationConstrained())
+			mSwingTwistConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, q, constraint_body1_to_world);
+		else
+			mSwingTwistConstraintPart.Deactivate();
+
+		if (mRotationMotorActive)
+		{
+			// Calculate rotation motor axis
+			Mat44 ws_axis = Mat44::sRotation(constraint_body2_to_world);
+			for (int i = 0; i < 3; ++i)
+				mRotationAxis[i] = ws_axis.GetColumn3(i);
+
+			// Get target orientation along the shortest path from q
+			Quat target_orientation = q.Dot(mTargetOrientation) > 0.0f? mTargetOrientation : -mTargetOrientation;
+
+			// The definition of the constraint rotation q:
+			// R2 * ConstraintToBody2 = R1 * ConstraintToBody1 * q (1)
+			//
+			// R2' is the rotation of body 2 when reaching the target_orientation:
+			// R2' * ConstraintToBody2 = R1 * ConstraintToBody1 * target_orientation (2)
+			//
+			// The difference in body 2 space:
+			// R2' = R2 * diff_body2 (3)
+			//
+			// We want to specify the difference in the constraint space of body 2:
+			// diff_body2 = ConstraintToBody2 * diff * ConstraintToBody2^* (4)
+			//
+			// Extracting R2' from 2: R2' = R1 * ConstraintToBody1 * target_orientation * ConstraintToBody2^* (5)
+			// Combining 3 & 4: R2' = R2 * ConstraintToBody2 * diff * ConstraintToBody2^* (6)
+			// Combining 1 & 6: R2' = R1 * ConstraintToBody1 * q * diff * ConstraintToBody2^* (7)
+			// Combining 5 & 7: R1 * ConstraintToBody1 * target_orientation * ConstraintToBody2^* = R1 * ConstraintToBody1 * q * diff * ConstraintToBody2^*
+			// <=> target_orientation = q * diff
+			// <=> diff = q^* * target_orientation
+			Quat diff = q.Conjugated() * target_orientation;
+
+			// Project diff so that only rotation around axis that have a position motor are remaining
+			Quat projected_diff;
+			switch (mRotationPositionMotorActive)
+			{
+			case 0b001:
+				// Keep only rotation around X
+				projected_diff = diff.GetTwist(Vec3::sAxisX());
+				break;
+
+			case 0b010:
+				// Keep only rotation around Y
+				projected_diff = diff.GetTwist(Vec3::sAxisY());
+				break;
+
+			case 0b100:
+				// Keep only rotation around Z
+				projected_diff = diff.GetTwist(Vec3::sAxisZ());
+				break;
+
+			case 0b011:
+				// Remove rotation around Z
+				// q = swing_xy * twist_z <=> swing_xy = q * twist_z^*
+				projected_diff = diff * diff.GetTwist(Vec3::sAxisZ()).Conjugated();
+				break;
+
+			case 0b101:
+				// Remove rotation around Y
+				// q = swing_xz * twist_y <=> swing_xz = q * twist_y^*
+				projected_diff = diff * diff.GetTwist(Vec3::sAxisY()).Conjugated();
+				break;
+
+			case 0b110:
+				// Remove rotation around X
+				// q = swing_yz * twist_x <=> swing_yz = q * twist_x^*
+				projected_diff = diff * diff.GetTwist(Vec3::sAxisX()).Conjugated();
+				break;
+
+			case 0b111:
+			default: // All motors off is handled here but the results are unused
+				// Keep entire rotation
+				projected_diff = diff;
+				break;
+			}
+
+			// Approximate error angles
+			// The imaginary part of a quaternion is rotation_axis * sin(angle / 2)
+			// If angle is small, sin(x) = x so angle[i] ~ 2.0f * rotation_axis[i]
+			// We'll be making small time steps, so if the angle is not small at least the sign will be correct and we'll move in the right direction
+			Vec3 rotation_error = -2.0f * projected_diff.GetXYZ();
+
+			// Setup motors
+			for (int i = 0; i < 3; ++i)
+			{
+				EAxis axis = EAxis(EAxis::RotationX + i);
+
+				Vec3 rotation_axis = mRotationAxis[i];
+
+				switch (mMotorState[axis])
+				{
+				case EMotorState::Off:
+					if (HasFriction(axis))
+						mMotorRotationConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, rotation_axis);
+					else
+						mMotorRotationConstraintPart[i].Deactivate();
+					break;
+
+				case EMotorState::Velocity:
+					mMotorRotationConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, rotation_axis, -mTargetAngularVelocity[i]);
+					break;
+
+				case EMotorState::Position:
+					{
+						const SpringSettings &spring_settings = mMotorSettings[axis].mSpringSettings;
+						if (spring_settings.HasStiffness())
+							mMotorRotationConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, rotation_axis, 0.0f, rotation_error[i], spring_settings);
+						else
+							mMotorRotationConstraintPart[i].Deactivate();
+						break;
+					}
+				}
+			}
+		}
+	}
+}
+
+void SixDOFConstraint::ResetWarmStart()
+{
+	for (AxisConstraintPart &c : mMotorTranslationConstraintPart)
+		c.Deactivate();
+	for (AngleConstraintPart &c : mMotorRotationConstraintPart)
+		c.Deactivate();
+	mRotationConstraintPart.Deactivate();
+	mSwingTwistConstraintPart.Deactivate();
+	mPointConstraintPart.Deactivate();
+	for (AxisConstraintPart &c : mTranslationConstraintPart)
+		c.Deactivate();
+}
+
+void SixDOFConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
+{
+	// Warm start translation motors
+	if (mTranslationMotorActive)
+		for (int i = 0; i < 3; ++i)
+			if (mMotorTranslationConstraintPart[i].IsActive())
+				mMotorTranslationConstraintPart[i].WarmStart(*mBody1, *mBody2, mTranslationAxis[i], inWarmStartImpulseRatio);
+
+	// Warm start rotation motors
+	if (mRotationMotorActive)
+		for (AngleConstraintPart &c : mMotorRotationConstraintPart)
+			if (c.IsActive())
+				c.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+
+	// Warm start rotation constraints
+	if (IsRotationFullyConstrained())
+		mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+	else if (IsRotationConstrained())
+		mSwingTwistConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+
+	// Warm start translation constraints
+	if (IsTranslationFullyConstrained())
+		mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+	else if (IsTranslationConstrained())
+		for (int i = 0; i < 3; ++i)
+			if (mTranslationConstraintPart[i].IsActive())
+				mTranslationConstraintPart[i].WarmStart(*mBody1, *mBody2, mTranslationAxis[i], inWarmStartImpulseRatio);
+}
+
+bool SixDOFConstraint::SolveVelocityConstraint(float inDeltaTime)
+{
+	bool impulse = false;
+
+	// Solve translation motor
+	if (mTranslationMotorActive)
+		for (int i = 0; i < 3; ++i)
+			if (mMotorTranslationConstraintPart[i].IsActive())
+				switch (mMotorState[i])
+				{
+				case EMotorState::Off:
+				{
+					// Apply friction only
+					float max_lambda = mMaxFriction[i] * inDeltaTime;
+					impulse |= mMotorTranslationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mTranslationAxis[i], -max_lambda, max_lambda);
+					break;
+				}
+
+				case EMotorState::Velocity:
+				case EMotorState::Position:
+					// Drive motor
+					impulse |= mMotorTranslationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mTranslationAxis[i], inDeltaTime * mMotorSettings[i].mMinForceLimit, inDeltaTime * mMotorSettings[i].mMaxForceLimit);
+					break;
+				}
+
+	// Solve rotation motor
+	if (mRotationMotorActive)
+		for (int i = 0; i < 3; ++i)
+		{
+			EAxis axis = EAxis(EAxis::RotationX + i);
+			if (mMotorRotationConstraintPart[i].IsActive())
+				switch (mMotorState[axis])
+				{
+				case EMotorState::Off:
+				{
+					// Apply friction only
+					float max_lambda = mMaxFriction[axis] * inDeltaTime;
+					impulse |= mMotorRotationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mRotationAxis[i], -max_lambda, max_lambda);
+					break;
+				}
+
+				case EMotorState::Velocity:
+				case EMotorState::Position:
+					// Drive motor
+					impulse |= mMotorRotationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mRotationAxis[i], inDeltaTime * mMotorSettings[axis].mMinTorqueLimit, inDeltaTime * mMotorSettings[axis].mMaxTorqueLimit);
+					break;
+				}
+		}
+
+	// Solve rotation constraint
+	if (IsRotationFullyConstrained())
+		impulse |= mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+	else if (IsRotationConstrained())
+		impulse |= mSwingTwistConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+
+	// Solve position constraint
+	if (IsTranslationFullyConstrained())
+		impulse |= mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+	else if (IsTranslationConstrained())
+		for (int i = 0; i < 3; ++i)
+			if (mTranslationConstraintPart[i].IsActive())
+			{
+				// If the axis is not fixed it must be limited (or else the constraint would not be active)
+				// Calculate the min and max constraint force based on on which side we're limited
+				float limit_min = -FLT_MAX, limit_max = FLT_MAX;
+				if (!IsFixedAxis(EAxis(EAxis::TranslationX + i)))
+				{
+					JPH_ASSERT(!IsFreeAxis(EAxis(EAxis::TranslationX + i)));
+					if (mDisplacement[i] <= mLimitMin[i])
+						limit_min = 0;
+					else if (mDisplacement[i] >= mLimitMax[i])
+						limit_max = 0;
+				}
+
+				impulse |= mTranslationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mTranslationAxis[i], limit_min, limit_max);
+			}
+
+	return impulse;
+}
+
+bool SixDOFConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
+{
+	bool impulse = false;
+
+	if (IsRotationFullyConstrained())
+	{
+		// Rotation locked: Solve rotation constraint
+
+		// Inverse of initial rotation from body 1 to body 2 in body 1 space
+		// Definition of initial orientation r0: q2 = q1 r0
+		// Initial rotation (see: GetRotationInConstraintSpace): q2 = q1 c1 c2^-1
+		// So: r0^-1 = (c1 c2^-1)^-1 = c2 * c1^-1
+		Quat constraint_to_body1 = mConstraintToBody1 * Quat::sEulerAngles(GetRotationLimitsMin());
+		Quat inv_initial_orientation = mConstraintToBody2 * constraint_to_body1.Conjugated();
+
+		// Solve rotation violations
+		mRotationConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), *mBody2, Mat44::sRotation(mBody2->GetRotation()));
+		impulse |= mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inv_initial_orientation, inBaumgarte);
+	}
+	else if (IsRotationConstrained())
+	{
+		// Rotation partially constraint
+
+		// Solve rotation violations
+		Quat q = GetRotationInConstraintSpace();
+		impulse |= mSwingTwistConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, q, mConstraintToBody1, mConstraintToBody2, inBaumgarte);
+	}
+
+	// Solve position violations
+	if (IsTranslationFullyConstrained())
+	{
+		// Translation locked: Solve point constraint
+		Vec3 local_space_position1 = mLocalSpacePosition1 + mConstraintToBody1 * GetTranslationLimitsMin();
+		mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), local_space_position1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2);
+		impulse |= mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte);
+	}
+	else if (IsTranslationConstrained())
+	{
+		// Translation partially locked: Solve per axis
+		for (int i = 0; i < 3; ++i)
+			if (mLimitsSpringSettings[i].mFrequency <= 0.0f) // If not soft limit
+			{
+				// Update world space positions (the bodies may have moved)
+				Vec3 r1_plus_u, r2, u;
+				GetPositionConstraintProperties(r1_plus_u, r2, u);
+
+				// Quaternion that rotates from body1's constraint space to world space
+				Quat constraint_body1_to_world = mBody1->GetRotation() * mConstraintToBody1;
+
+				// Calculate axis
+				Vec3 translation_axis;
+				switch (i)
+				{
+				case 0:							translation_axis = constraint_body1_to_world.RotateAxisX(); break;
+				case 1:							translation_axis = constraint_body1_to_world.RotateAxisY(); break;
+				default:	JPH_ASSERT(i == 2); translation_axis = constraint_body1_to_world.RotateAxisZ(); break;
+				}
+
+				// Determine position error
+				float error = 0.0f;
+				EAxis axis(EAxis(EAxis::TranslationX + i));
+				if (IsFixedAxis(axis))
+					error = u.Dot(translation_axis) - mLimitMin[axis];
+				else if (!IsFreeAxis(axis))
+				{
+					float displacement = u.Dot(translation_axis);
+					if (displacement <= mLimitMin[axis])
+						error = displacement - mLimitMin[axis];
+					else if (displacement >= mLimitMax[axis])
+						error = displacement - mLimitMax[axis];
+				}
+
+				if (error != 0.0f)
+				{
+					// Setup axis constraint part and solve it
+					mTranslationConstraintPart[i].CalculateConstraintProperties(*mBody1, r1_plus_u, *mBody2, r2, translation_axis);
+					impulse |= mTranslationConstraintPart[i].SolvePositionConstraint(*mBody1, *mBody2, translation_axis, error, inBaumgarte);
+				}
+			}
+	}
+
+	return impulse;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void SixDOFConstraint::DrawConstraint(DebugRenderer *inRenderer) const
+{
+	// Get constraint properties in world space
+	RVec3 position1 = mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1;
+	Quat rotation1 = mBody1->GetRotation() * mConstraintToBody1;
+	Quat rotation2 = mBody2->GetRotation() * mConstraintToBody2;
+
+	// Draw constraint orientation
+	inRenderer->DrawCoordinateSystem(RMat44::sRotationTranslation(rotation1, position1), mDrawConstraintSize);
+
+	if ((IsRotationConstrained() || mRotationPositionMotorActive != 0) && !IsRotationFullyConstrained())
+	{
+		// Draw current swing and twist
+		Quat q = GetRotationInConstraintSpace();
+		Quat q_swing, q_twist;
+		q.GetSwingTwist(q_swing, q_twist);
+		inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * q_twist).RotateAxisY(), Color::sWhite);
+		inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * q_swing).RotateAxisX(), Color::sWhite);
+	}
+
+	// Draw target rotation
+	Quat m_swing, m_twist;
+	mTargetOrientation.GetSwingTwist(m_swing, m_twist);
+	if (mMotorState[EAxis::RotationX] == EMotorState::Position)
+		inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * m_twist).RotateAxisY(), Color::sYellow);
+	if (mMotorState[EAxis::RotationY] == EMotorState::Position || mMotorState[EAxis::RotationZ] == EMotorState::Position)
+		inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * m_swing).RotateAxisX(), Color::sYellow);
+
+	// Draw target angular velocity
+	Vec3 target_angular_velocity = Vec3::sZero();
+	for (int i = 0; i < 3; ++i)
+		if (mMotorState[EAxis::RotationX + i] == EMotorState::Velocity)
+			target_angular_velocity.SetComponent(i, mTargetAngularVelocity[i]);
+	if (target_angular_velocity != Vec3::sZero())
+		inRenderer->DrawArrow(position1, position1 + rotation2 * target_angular_velocity, Color::sRed, 0.1f);
+}
+
+void SixDOFConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const
+{
+	// Get matrix that transforms from constraint space to world space
+	RMat44 constraint_body1_to_world = RMat44::sRotationTranslation(mBody1->GetRotation() * mConstraintToBody1, mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1);
+
+	// Draw limits
+	if (mSwingTwistConstraintPart.GetSwingType() == ESwingType::Pyramid)
+		inRenderer->DrawSwingPyramidLimits(constraint_body1_to_world, mLimitMin[EAxis::RotationY], mLimitMax[EAxis::RotationY], mLimitMin[EAxis::RotationZ], mLimitMax[EAxis::RotationZ], mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off);
+	else
+		inRenderer->DrawSwingConeLimits(constraint_body1_to_world, mLimitMax[EAxis::RotationY], mLimitMax[EAxis::RotationZ], mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off);
+	inRenderer->DrawPie(constraint_body1_to_world.GetTranslation(), mDrawConstraintSize, constraint_body1_to_world.GetAxisX(), constraint_body1_to_world.GetAxisY(), mLimitMin[EAxis::RotationX], mLimitMax[EAxis::RotationX], Color::sPurple, DebugRenderer::ECastShadow::Off);
+}
+#endif // JPH_DEBUG_RENDERER
+
+void SixDOFConstraint::SaveState(StateRecorder &inStream) const
+{
+	TwoBodyConstraint::SaveState(inStream);
+
+	for (const AxisConstraintPart &c : mTranslationConstraintPart)
+		c.SaveState(inStream);
+	mPointConstraintPart.SaveState(inStream);
+	mSwingTwistConstraintPart.SaveState(inStream);
+	mRotationConstraintPart.SaveState(inStream);
+	for (const AxisConstraintPart &c : mMotorTranslationConstraintPart)
+		c.SaveState(inStream);
+	for (const AngleConstraintPart &c : mMotorRotationConstraintPart)
+		c.SaveState(inStream);
+
+	inStream.Write(mMotorState);
+	inStream.Write(mTargetVelocity);
+	inStream.Write(mTargetAngularVelocity);
+	inStream.Write(mTargetPosition);
+	inStream.Write(mTargetOrientation);
+}
+
+void SixDOFConstraint::RestoreState(StateRecorder &inStream)
+{
+	TwoBodyConstraint::RestoreState(inStream);
+
+	for (AxisConstraintPart &c : mTranslationConstraintPart)
+		c.RestoreState(inStream);
+	mPointConstraintPart.RestoreState(inStream);
+	mSwingTwistConstraintPart.RestoreState(inStream);
+	mRotationConstraintPart.RestoreState(inStream);
+	for (AxisConstraintPart &c : mMotorTranslationConstraintPart)
+		c.RestoreState(inStream);
+	for (AngleConstraintPart &c : mMotorRotationConstraintPart)
+		c.RestoreState(inStream);
+
+	inStream.Read(mMotorState);
+	inStream.Read(mTargetVelocity);
+	inStream.Read(mTargetAngularVelocity);
+	inStream.Read(mTargetPosition);
+	inStream.Read(mTargetOrientation);
+
+	CacheTranslationMotorActive();
+	CacheRotationMotorActive();
+	CacheRotationPositionMotorActive();
+}
+
+Ref<ConstraintSettings> SixDOFConstraint::GetConstraintSettings() const
+{
+	SixDOFConstraintSettings *settings = new SixDOFConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mPosition1 = RVec3(mLocalSpacePosition1);
+	settings->mAxisX1 = mConstraintToBody1.RotateAxisX();
+	settings->mAxisY1 = mConstraintToBody1.RotateAxisY();
+	settings->mPosition2 = RVec3(mLocalSpacePosition2);
+	settings->mAxisX2 = mConstraintToBody2.RotateAxisX();
+	settings->mAxisY2 = mConstraintToBody2.RotateAxisY();
+	settings->mSwingType = mSwingTwistConstraintPart.GetSwingType();
+	memcpy(settings->mLimitMin, mLimitMin, sizeof(mLimitMin));
+	memcpy(settings->mLimitMax, mLimitMax, sizeof(mLimitMax));
+	memcpy(settings->mMaxFriction, mMaxFriction, sizeof(mMaxFriction));
+	for (int i = 0; i < EAxis::Num; ++i)
+		settings->mMotorSettings[i] = mMotorSettings[i];
+	return settings;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/SixDOFConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SixDOFConstraint.h
new file mode 100644
index 0000000000..2ebb9856bd
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SixDOFConstraint.h
@@ -0,0 +1,289 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Physics/Constraints/MotorSettings.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/SwingTwistConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// 6 Degree Of Freedom Constraint setup structure. Allows control over each of the 6 degrees of freedom.
+class JPH_EXPORT SixDOFConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, SixDOFConstraintSettings)
+
+public:
+	/// Constraint is split up into translation/rotation around X, Y and Z axis.
+	enum EAxis
+	{
+		TranslationX,
+		TranslationY,
+		TranslationZ,
+
+		RotationX,
+		RotationY,
+		RotationZ,
+
+		Num,
+		NumTranslation = TranslationZ + 1,
+	};
+
+	// See: ConstraintSettings::SaveBinaryState
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+
+	/// Create an instance of this constraint
+	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
+
+	/// This determines in which space the constraint is setup, all properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	/// Body 1 constraint reference frame (space determined by mSpace)
+	RVec3						mPosition1 = RVec3::sZero();
+	Vec3						mAxisX1 = Vec3::sAxisX();
+	Vec3						mAxisY1 = Vec3::sAxisY();
+
+	/// Body 2 constraint reference frame (space determined by mSpace)
+	RVec3						mPosition2 = RVec3::sZero();
+	Vec3						mAxisX2 = Vec3::sAxisX();
+	Vec3						mAxisY2 = Vec3::sAxisY();
+
+	/// Friction settings.
+	/// For translation: Max friction force in N. 0 = no friction.
+	/// For rotation: Max friction torque in Nm. 0 = no friction.
+	float						mMaxFriction[EAxis::Num] = { 0, 0, 0, 0, 0, 0 };
+
+	/// The type of swing constraint that we want to use.
+	ESwingType					mSwingType = ESwingType::Cone;
+
+	/// Limits.
+	/// For translation: Min and max linear limits in m (0 is frame of body 1 and 2 coincide).
+	/// For rotation: Min and max angular limits in rad (0 is frame of body 1 and 2 coincide). See comments at Axis enum for limit ranges.
+	///
+	/// Remove degree of freedom by setting min = FLT_MAX and max = -FLT_MAX. The constraint will be driven to 0 for this axis.
+	///
+	/// Free movement over an axis is allowed when min = -FLT_MAX and max = FLT_MAX.
+	///
+	/// Rotation limit around X-Axis: When limited, should be \f$\in [-\pi, \pi]\f$. Can be asymmetric around zero.
+	///
+	/// Rotation limit around Y-Z Axis: Forms a pyramid or cone shaped limit:
+	/// * For pyramid, should be \f$\in [-\pi, \pi]\f$ and does not need to be symmetrical around zero.
+	/// * For cone should be \f$\in [0, \pi]\f$ and needs to be symmetrical around zero (min limit is assumed to be -max limit).
+	float						mLimitMin[EAxis::Num] = { -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX };
+	float						mLimitMax[EAxis::Num] = { FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX };
+
+	/// When enabled, this makes the limits soft. When the constraint exceeds the limits, a spring force will pull it back.
+	/// Only soft translation limits are supported, soft rotation limits are not currently supported.
+	SpringSettings				mLimitsSpringSettings[EAxis::NumTranslation];
+
+	/// Make axis free (unconstrained)
+	void						MakeFreeAxis(EAxis inAxis)									{ mLimitMin[inAxis] = -FLT_MAX; mLimitMax[inAxis] = FLT_MAX; }
+	bool						IsFreeAxis(EAxis inAxis) const								{ return mLimitMin[inAxis] == -FLT_MAX && mLimitMax[inAxis] == FLT_MAX; }
+
+	/// Make axis fixed (fixed at value 0)
+	void						MakeFixedAxis(EAxis inAxis)									{ mLimitMin[inAxis] = FLT_MAX; mLimitMax[inAxis] = -FLT_MAX; }
+	bool						IsFixedAxis(EAxis inAxis) const								{ return mLimitMin[inAxis] >= mLimitMax[inAxis]; }
+
+	/// Set a valid range for the constraint (if inMax < inMin, the axis will become fixed)
+	void						SetLimitedAxis(EAxis inAxis, float inMin, float inMax)		{ mLimitMin[inAxis] = inMin; mLimitMax[inAxis] = inMax; }
+
+	/// Motor settings for each axis
+	MotorSettings				mMotorSettings[EAxis::Num];
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// 6 Degree Of Freedom Constraint. Allows control over each of the 6 degrees of freedom.
+class JPH_EXPORT SixDOFConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Get Axis from settings class
+	using EAxis = SixDOFConstraintSettings::EAxis;
+
+	/// Construct six DOF constraint
+								SixDOFConstraint(Body &inBody1, Body &inBody2, const SixDOFConstraintSettings &inSettings);
+
+	/// Generic interface of a constraint
+	virtual EConstraintSubType	GetSubType() const override									{ return EConstraintSubType::SixDOF; }
+	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
+	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
+	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
+	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
+	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void				DrawConstraint(DebugRenderer *inRenderer) const override;
+	virtual void				DrawConstraintLimits(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual void				SaveState(StateRecorder &inStream) const override;
+	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
+
+	// See: TwoBodyConstraint
+	virtual Mat44				GetConstraintToBody1Matrix() const override					{ return Mat44::sRotationTranslation(mConstraintToBody1, mLocalSpacePosition1); }
+	virtual Mat44				GetConstraintToBody2Matrix() const override					{ return Mat44::sRotationTranslation(mConstraintToBody2, mLocalSpacePosition2); }
+
+	/// Update the translation limits for this constraint
+	void						SetTranslationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax);
+
+	/// Update the rotational limits for this constraint
+	void						SetRotationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax);
+
+	/// Get constraint Limits
+	float						GetLimitsMin(EAxis inAxis) const							{ return mLimitMin[inAxis]; }
+	float						GetLimitsMax(EAxis inAxis) const							{ return mLimitMax[inAxis]; }
+	Vec3						GetTranslationLimitsMin() const								{ return Vec3::sLoadFloat3Unsafe(*reinterpret_cast<const Float3 *>(&mLimitMin[EAxis::TranslationX])); }
+	Vec3						GetTranslationLimitsMax() const								{ return Vec3::sLoadFloat3Unsafe(*reinterpret_cast<const Float3 *>(&mLimitMax[EAxis::TranslationX])); }
+	Vec3						GetRotationLimitsMin() const								{ return Vec3::sLoadFloat3Unsafe(*reinterpret_cast<const Float3 *>(&mLimitMin[EAxis::RotationX])); }
+	Vec3						GetRotationLimitsMax() const								{ return Vec3::sLoadFloat3Unsafe(*reinterpret_cast<const Float3 *>(&mLimitMax[EAxis::RotationX])); }
+
+	/// Check which axis are fixed/free
+	inline bool					IsFixedAxis(EAxis inAxis) const								{ return (mFixedAxis & (1 << inAxis)) != 0; }
+	inline bool					IsFreeAxis(EAxis inAxis) const								{ return (mFreeAxis & (1 << inAxis)) != 0; }
+
+	/// Update the limits spring settings
+	const SpringSettings &		GetLimitsSpringSettings(EAxis inAxis) const					{ JPH_ASSERT(inAxis < EAxis::NumTranslation); return mLimitsSpringSettings[inAxis]; }
+	void						SetLimitsSpringSettings(EAxis inAxis, const SpringSettings& inLimitsSpringSettings) { JPH_ASSERT(inAxis < EAxis::NumTranslation); mLimitsSpringSettings[inAxis] = inLimitsSpringSettings; CacheHasSpringLimits(); }
+
+	/// Set the max friction for each axis
+	void						SetMaxFriction(EAxis inAxis, float inFriction);
+	float						GetMaxFriction(EAxis inAxis) const							{ return mMaxFriction[inAxis]; }
+
+	/// Get rotation of constraint in constraint space
+	Quat						GetRotationInConstraintSpace() const;
+
+	/// Motor settings
+	MotorSettings &				GetMotorSettings(EAxis inAxis)								{ return mMotorSettings[inAxis]; }
+	const MotorSettings &		GetMotorSettings(EAxis inAxis) const						{ return mMotorSettings[inAxis]; }
+
+	/// Motor controls.
+	/// Translation motors work in constraint space of body 1.
+	/// Rotation motors work in constraint space of body 2 (!).
+	void						SetMotorState(EAxis inAxis, EMotorState inState);
+	EMotorState					GetMotorState(EAxis inAxis) const							{ return mMotorState[inAxis]; }
+
+	/// Set the target velocity in body 1 constraint space
+	Vec3						GetTargetVelocityCS() const									{ return mTargetVelocity; }
+	void						SetTargetVelocityCS(Vec3Arg inVelocity)						{ mTargetVelocity = inVelocity; }
+
+	/// Set the target angular velocity in body 2 constraint space (!)
+	void						SetTargetAngularVelocityCS(Vec3Arg inAngularVelocity)		{ mTargetAngularVelocity = inAngularVelocity; }
+	Vec3						GetTargetAngularVelocityCS() const							{ return mTargetAngularVelocity; }
+
+	/// Set the target position in body 1 constraint space
+	Vec3						GetTargetPositionCS() const									{ return mTargetPosition; }
+	void						SetTargetPositionCS(Vec3Arg inPosition)						{ mTargetPosition = inPosition; }
+
+	/// Set the target orientation in body 1 constraint space
+	void						SetTargetOrientationCS(QuatArg inOrientation);
+	Quat						GetTargetOrientationCS() const								{ return mTargetOrientation; }
+
+	/// Set the target orientation in body space (R2 = R1 * inOrientation, where R1 and R2 are the world space rotations for body 1 and 2).
+	/// Solve: R2 * ConstraintToBody2 = R1 * ConstraintToBody1 * q (see SwingTwistConstraint::GetSwingTwist) and R2 = R1 * inOrientation for q.
+	void						SetTargetOrientationBS(QuatArg inOrientation)				{ SetTargetOrientationCS(mConstraintToBody1.Conjugated() * inOrientation * mConstraintToBody2); }
+
+	///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint)
+	inline Vec3					GetTotalLambdaPosition() const								{ return IsTranslationFullyConstrained()? mPointConstraintPart.GetTotalLambda() : Vec3(mTranslationConstraintPart[0].GetTotalLambda(), mTranslationConstraintPart[1].GetTotalLambda(), mTranslationConstraintPart[2].GetTotalLambda()); }
+	inline Vec3					GetTotalLambdaRotation() const								{ return IsRotationFullyConstrained()? mRotationConstraintPart.GetTotalLambda() : Vec3(mSwingTwistConstraintPart.GetTotalTwistLambda(), mSwingTwistConstraintPart.GetTotalSwingYLambda(), mSwingTwistConstraintPart.GetTotalSwingZLambda()); }
+	inline Vec3					GetTotalLambdaMotorTranslation() const						{ return Vec3(mMotorTranslationConstraintPart[0].GetTotalLambda(), mMotorTranslationConstraintPart[1].GetTotalLambda(), mMotorTranslationConstraintPart[2].GetTotalLambda()); }
+	inline Vec3					GetTotalLambdaMotorRotation() const							{ return Vec3(mMotorRotationConstraintPart[0].GetTotalLambda(), mMotorRotationConstraintPart[1].GetTotalLambda(), mMotorRotationConstraintPart[2].GetTotalLambda()); }
+
+private:
+	// Calculate properties needed for the position constraint
+	inline void					GetPositionConstraintProperties(Vec3 &outR1PlusU, Vec3 &outR2, Vec3 &outU) const;
+
+	// Sanitize the translation limits
+	inline void					UpdateTranslationLimits();
+
+	// Propagate the rotation limits to the constraint part
+	inline void					UpdateRotationLimits();
+
+	// Update the cached state of which axis are free and which ones are fixed
+	inline void					UpdateFixedFreeAxis();
+
+	// Cache the state of mTranslationMotorActive
+	void						CacheTranslationMotorActive();
+
+	// Cache the state of mRotationMotorActive
+	void						CacheRotationMotorActive();
+
+	// Cache the state of mRotationPositionMotorActive
+	void						CacheRotationPositionMotorActive();
+
+	/// Cache the state of mHasSpringLimits
+	void						CacheHasSpringLimits();
+
+	// Constraint settings helper functions
+	inline bool					IsTranslationConstrained() const							{ return (mFreeAxis & 0b111) != 0b111; }
+	inline bool					IsTranslationFullyConstrained() const						{ return (mFixedAxis & 0b111) == 0b111 && !mHasSpringLimits; }
+	inline bool					IsRotationConstrained() const								{ return (mFreeAxis & 0b111000) != 0b111000; }
+	inline bool					IsRotationFullyConstrained() const							{ return (mFixedAxis & 0b111000) == 0b111000; }
+	inline bool					HasFriction(EAxis inAxis) const								{ return !IsFixedAxis(inAxis) && mMaxFriction[inAxis] > 0.0f; }
+
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	// Local space constraint positions
+	Vec3						mLocalSpacePosition1;
+	Vec3						mLocalSpacePosition2;
+
+	// Transforms from constraint space to body space
+	Quat						mConstraintToBody1;
+	Quat						mConstraintToBody2;
+
+	// Limits
+	uint8						mFreeAxis = 0;												// Bitmask of free axis (bit 0 = TranslationX)
+	uint8						mFixedAxis = 0;												// Bitmask of fixed axis (bit 0 = TranslationX)
+	bool						mTranslationMotorActive = false;							// If any of the translational frictions / motors are active
+	bool						mRotationMotorActive = false;								// If any of the rotational frictions / motors are active
+	uint8						mRotationPositionMotorActive = 0;							// Bitmask of axis that have position motor active (bit 0 = RotationX)
+	bool						mHasSpringLimits = false;									// If any of the limit springs have a non-zero frequency/stiffness
+	float						mLimitMin[EAxis::Num];
+	float						mLimitMax[EAxis::Num];
+	SpringSettings				mLimitsSpringSettings[EAxis::NumTranslation];
+
+	// Motor settings for each axis
+	MotorSettings				mMotorSettings[EAxis::Num];
+
+	// Friction settings for each axis
+	float						mMaxFriction[EAxis::Num];
+
+	// Motor controls
+	EMotorState					mMotorState[EAxis::Num] = { EMotorState::Off, EMotorState::Off, EMotorState::Off, EMotorState::Off, EMotorState::Off, EMotorState::Off };
+	Vec3						mTargetVelocity = Vec3::sZero();
+	Vec3						mTargetAngularVelocity = Vec3::sZero();
+	Vec3						mTargetPosition = Vec3::sZero();
+	Quat						mTargetOrientation = Quat::sIdentity();
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	// Constraint space axis in world space
+	Vec3						mTranslationAxis[3];
+	Vec3						mRotationAxis[3];
+
+	// Translation displacement (valid when translation axis has a range limit)
+	float						mDisplacement[3];
+
+	// Individual constraint parts for translation, or a combined point constraint part if all axis are fixed
+	AxisConstraintPart			mTranslationConstraintPart[3];
+	PointConstraintPart			mPointConstraintPart;
+
+	// Individual constraint parts for rotation or a combined constraint part if rotation is fixed
+	SwingTwistConstraintPart	mSwingTwistConstraintPart;
+	RotationEulerConstraintPart	mRotationConstraintPart;
+
+	// Motor or friction constraints
+	AxisConstraintPart			mMotorTranslationConstraintPart[3];
+	AngleConstraintPart			mMotorRotationConstraintPart[3];
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/SliderConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SliderConstraint.cpp
new file mode 100644
index 0000000000..75335c32cb
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SliderConstraint.cpp
@@ -0,0 +1,501 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/SliderConstraint.h>
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+using namespace literals;
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SliderConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(SliderConstraintSettings, TwoBodyConstraintSettings)
+
+	JPH_ADD_ENUM_ATTRIBUTE(SliderConstraintSettings, mSpace)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mAutoDetectPoint)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mPoint1)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mSliderAxis1)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mNormalAxis1)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mPoint2)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mSliderAxis2)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mNormalAxis2)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mLimitsMin)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mLimitsMax)
+	JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(SliderConstraintSettings, mLimitsSpringSettings.mMode, "mSpringMode")
+	JPH_ADD_ATTRIBUTE_WITH_ALIAS(SliderConstraintSettings, mLimitsSpringSettings.mFrequency, "mFrequency") // Renaming attributes to stay compatible with old versions of the library
+	JPH_ADD_ATTRIBUTE_WITH_ALIAS(SliderConstraintSettings, mLimitsSpringSettings.mDamping, "mDamping")
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mMaxFrictionForce)
+	JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mMotorSettings)
+}
+
+void SliderConstraintSettings::SetSliderAxis(Vec3Arg inSliderAxis)
+{
+	JPH_ASSERT(mSpace == EConstraintSpace::WorldSpace);
+
+	mSliderAxis1 = mSliderAxis2 = inSliderAxis;
+	mNormalAxis1 = mNormalAxis2 = inSliderAxis.GetNormalizedPerpendicular();
+}
+
+void SliderConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	ConstraintSettings::SaveBinaryState(inStream);
+
+	inStream.Write(mSpace);
+	inStream.Write(mAutoDetectPoint);
+	inStream.Write(mPoint1);
+	inStream.Write(mSliderAxis1);
+	inStream.Write(mNormalAxis1);
+	inStream.Write(mPoint2);
+	inStream.Write(mSliderAxis2);
+	inStream.Write(mNormalAxis2);
+	inStream.Write(mLimitsMin);
+	inStream.Write(mLimitsMax);
+	inStream.Write(mMaxFrictionForce);
+	mLimitsSpringSettings.SaveBinaryState(inStream);
+	mMotorSettings.SaveBinaryState(inStream);
+}
+
+void SliderConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	ConstraintSettings::RestoreBinaryState(inStream);
+
+	inStream.Read(mSpace);
+	inStream.Read(mAutoDetectPoint);
+	inStream.Read(mPoint1);
+	inStream.Read(mSliderAxis1);
+	inStream.Read(mNormalAxis1);
+	inStream.Read(mPoint2);
+	inStream.Read(mSliderAxis2);
+	inStream.Read(mNormalAxis2);
+	inStream.Read(mLimitsMin);
+	inStream.Read(mLimitsMax);
+	inStream.Read(mMaxFrictionForce);
+	mLimitsSpringSettings.RestoreBinaryState(inStream);
+	mMotorSettings.RestoreBinaryState(inStream);
+}
+
+TwoBodyConstraint *SliderConstraintSettings::Create(Body &inBody1, Body &inBody2) const
+{
+	return new SliderConstraint(inBody1, inBody2, *this);
+}
+
+SliderConstraint::SliderConstraint(Body &inBody1, Body &inBody2, const SliderConstraintSettings &inSettings) :
+	TwoBodyConstraint(inBody1, inBody2, inSettings),
+	mMaxFrictionForce(inSettings.mMaxFrictionForce),
+	mMotorSettings(inSettings.mMotorSettings)
+{
+	// Store inverse of initial rotation from body 1 to body 2 in body 1 space
+	mInvInitialOrientation = RotationEulerConstraintPart::sGetInvInitialOrientationXY(inSettings.mSliderAxis1, inSettings.mNormalAxis1, inSettings.mSliderAxis2, inSettings.mNormalAxis2);
+
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		RMat44 inv_transform1 = inBody1.GetInverseCenterOfMassTransform();
+		RMat44 inv_transform2 = inBody2.GetInverseCenterOfMassTransform();
+
+		if (inSettings.mAutoDetectPoint)
+		{
+			// Determine anchor point: If any of the bodies can never be dynamic use the other body as anchor point
+			RVec3 anchor;
+			if (!inBody1.CanBeKinematicOrDynamic())
+				anchor = inBody2.GetCenterOfMassPosition();
+			else if (!inBody2.CanBeKinematicOrDynamic())
+				anchor = inBody1.GetCenterOfMassPosition();
+			else
+			{
+				// Otherwise use weighted anchor point towards the lightest body
+				Real inv_m1 = Real(inBody1.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked());
+				Real inv_m2 = Real(inBody2.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked());
+				Real total_inv_mass = inv_m1 + inv_m2;
+				if (total_inv_mass != 0.0_r)
+					anchor = (inv_m1 * inBody1.GetCenterOfMassPosition() + inv_m2 * inBody2.GetCenterOfMassPosition()) / total_inv_mass;
+				else
+					anchor = inBody1.GetCenterOfMassPosition();
+			}
+
+			// Store local positions
+			mLocalSpacePosition1 = Vec3(inv_transform1 * anchor);
+			mLocalSpacePosition2 = Vec3(inv_transform2 * anchor);
+		}
+		else
+		{
+			// Store local positions
+			mLocalSpacePosition1 = Vec3(inv_transform1 * inSettings.mPoint1);
+			mLocalSpacePosition2 = Vec3(inv_transform2 * inSettings.mPoint2);
+		}
+
+		// If all properties were specified in world space, take them to local space now
+		mLocalSpaceSliderAxis1 = inv_transform1.Multiply3x3(inSettings.mSliderAxis1).Normalized();
+		mLocalSpaceNormal1 = inv_transform1.Multiply3x3(inSettings.mNormalAxis1).Normalized();
+
+		// Constraints were specified in world space, so we should have replaced c1 with q10^-1 c1 and c2 with q20^-1 c2
+		// => r0^-1 = (q20^-1 c2) (q10^-1 c1)^1 = q20^-1 (c2 c1^-1) q10
+		mInvInitialOrientation = inBody2.GetRotation().Conjugated() * mInvInitialOrientation * inBody1.GetRotation();
+	}
+	else
+	{
+		// Store local positions
+		mLocalSpacePosition1 = Vec3(inSettings.mPoint1);
+		mLocalSpacePosition2 = Vec3(inSettings.mPoint2);
+
+		// Store local space axis
+		mLocalSpaceSliderAxis1 = inSettings.mSliderAxis1;
+		mLocalSpaceNormal1 = inSettings.mNormalAxis1;
+	}
+
+	// Calculate 2nd local space normal
+	mLocalSpaceNormal2 = mLocalSpaceSliderAxis1.Cross(mLocalSpaceNormal1);
+
+	// Store limits
+	JPH_ASSERT(inSettings.mLimitsMin != inSettings.mLimitsMax || inSettings.mLimitsSpringSettings.mFrequency > 0.0f, "Better use a fixed constraint");
+	SetLimits(inSettings.mLimitsMin, inSettings.mLimitsMax);
+
+	// Store spring settings
+	SetLimitsSpringSettings(inSettings.mLimitsSpringSettings);
+}
+
+void SliderConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
+{
+	if (mBody1->GetID() == inBodyID)
+		mLocalSpacePosition1 -= inDeltaCOM;
+	else if (mBody2->GetID() == inBodyID)
+		mLocalSpacePosition2 -= inDeltaCOM;
+}
+
+float SliderConstraint::GetCurrentPosition() const
+{
+	// See: CalculateR1R2U and CalculateSlidingAxisAndPosition
+	Vec3 r1 = mBody1->GetRotation() * mLocalSpacePosition1;
+	Vec3 r2 = mBody2->GetRotation() * mLocalSpacePosition2;
+	Vec3 u = Vec3(mBody2->GetCenterOfMassPosition() - mBody1->GetCenterOfMassPosition()) + r2 - r1;
+	return u.Dot(mBody1->GetRotation() * mLocalSpaceSliderAxis1);
+}
+
+void SliderConstraint::SetLimits(float inLimitsMin, float inLimitsMax)
+{
+	JPH_ASSERT(inLimitsMin <= 0.0f);
+	JPH_ASSERT(inLimitsMax >= 0.0f);
+	mLimitsMin = inLimitsMin;
+	mLimitsMax = inLimitsMax;
+	mHasLimits = mLimitsMin != -FLT_MAX || mLimitsMax != FLT_MAX;
+}
+
+void SliderConstraint::CalculateR1R2U(Mat44Arg inRotation1, Mat44Arg inRotation2)
+{
+	// Calculate points relative to body
+	mR1 = inRotation1 * mLocalSpacePosition1;
+	mR2 = inRotation2 * mLocalSpacePosition2;
+
+	// Calculate X2 + R2 - X1 - R1
+	mU = Vec3(mBody2->GetCenterOfMassPosition() - mBody1->GetCenterOfMassPosition()) + mR2 - mR1;
+}
+
+void SliderConstraint::CalculatePositionConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2)
+{
+	// Calculate world space normals
+	mN1 = inRotation1 * mLocalSpaceNormal1;
+	mN2 = inRotation1 * mLocalSpaceNormal2;
+
+	mPositionConstraintPart.CalculateConstraintProperties(*mBody1, inRotation1, mR1 + mU, *mBody2, inRotation2, mR2, mN1, mN2);
+}
+
+void SliderConstraint::CalculateSlidingAxisAndPosition(Mat44Arg inRotation1)
+{
+	if (mHasLimits || mMotorState != EMotorState::Off || mMaxFrictionForce > 0.0f)
+	{
+		// Calculate world space slider axis
+		mWorldSpaceSliderAxis = inRotation1 * mLocalSpaceSliderAxis1;
+
+		// Calculate slide distance along axis
+		mD = mU.Dot(mWorldSpaceSliderAxis);
+	}
+}
+
+void SliderConstraint::CalculatePositionLimitsConstraintProperties(float inDeltaTime)
+{
+	// Check if distance is within limits
+	bool below_min = mD <= mLimitsMin;
+	if (mHasLimits && (below_min || mD >= mLimitsMax))
+		mPositionLimitsConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis, 0.0f, mD - (below_min? mLimitsMin : mLimitsMax), mLimitsSpringSettings);
+	else
+		mPositionLimitsConstraintPart.Deactivate();
+}
+
+void SliderConstraint::CalculateMotorConstraintProperties(float inDeltaTime)
+{
+	switch (mMotorState)
+	{
+	case EMotorState::Off:
+		if (mMaxFrictionForce > 0.0f)
+			mMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis);
+		else
+			mMotorConstraintPart.Deactivate();
+		break;
+
+	case EMotorState::Velocity:
+		mMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis, -mTargetVelocity);
+		break;
+
+	case EMotorState::Position:
+		if (mMotorSettings.mSpringSettings.HasStiffness())
+			mMotorConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis, 0.0f, mD - mTargetPosition, mMotorSettings.mSpringSettings);
+		else
+			mMotorConstraintPart.Deactivate();
+		break;
+	}
+}
+
+void SliderConstraint::SetupVelocityConstraint(float inDeltaTime)
+{
+	// Calculate constraint properties that are constant while bodies don't move
+	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
+	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
+	CalculateR1R2U(rotation1, rotation2);
+	CalculatePositionConstraintProperties(rotation1, rotation2);
+	mRotationConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, *mBody2, rotation2);
+	CalculateSlidingAxisAndPosition(rotation1);
+	CalculatePositionLimitsConstraintProperties(inDeltaTime);
+	CalculateMotorConstraintProperties(inDeltaTime);
+}
+
+void SliderConstraint::ResetWarmStart()
+{
+	mMotorConstraintPart.Deactivate();
+	mPositionConstraintPart.Deactivate();
+	mRotationConstraintPart.Deactivate();
+	mPositionLimitsConstraintPart.Deactivate();
+}
+
+void SliderConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
+{
+	// Warm starting: Apply previous frame impulse
+	mMotorConstraintPart.WarmStart(*mBody1, *mBody2, mWorldSpaceSliderAxis, inWarmStartImpulseRatio);
+	mPositionConstraintPart.WarmStart(*mBody1, *mBody2, mN1, mN2, inWarmStartImpulseRatio);
+	mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+	mPositionLimitsConstraintPart.WarmStart(*mBody1, *mBody2, mWorldSpaceSliderAxis, inWarmStartImpulseRatio);
+}
+
+bool SliderConstraint::SolveVelocityConstraint(float inDeltaTime)
+{
+	// Solve motor
+	bool motor = false;
+	if (mMotorConstraintPart.IsActive())
+	{
+		switch (mMotorState)
+		{
+		case EMotorState::Off:
+			{
+				float max_lambda = mMaxFrictionForce * inDeltaTime;
+				motor = mMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, -max_lambda, max_lambda);
+				break;
+			}
+
+		case EMotorState::Velocity:
+		case EMotorState::Position:
+			motor = mMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, inDeltaTime * mMotorSettings.mMinForceLimit, inDeltaTime * mMotorSettings.mMaxForceLimit);
+			break;
+		}
+	}
+
+	// Solve position constraint along 2 axis
+	bool pos = mPositionConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mN1, mN2);
+
+	// Solve rotation constraint
+	bool rot = mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+
+	// Solve limits along slider axis
+	bool limit = false;
+	if (mPositionLimitsConstraintPart.IsActive())
+	{
+		float min_lambda, max_lambda;
+		if (mLimitsMin == mLimitsMax)
+		{
+			min_lambda = -FLT_MAX;
+			max_lambda = FLT_MAX;
+		}
+		else if (mD <= mLimitsMin)
+		{
+			min_lambda = 0.0f;
+			max_lambda = FLT_MAX;
+		}
+		else
+		{
+			min_lambda = -FLT_MAX;
+			max_lambda = 0.0f;
+		}
+		limit = mPositionLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, min_lambda, max_lambda);
+	}
+
+	return motor || pos || rot || limit;
+}
+
+bool SliderConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
+{
+	// Motor operates on velocities only, don't call SolvePositionConstraint
+
+	// Solve position constraint along 2 axis
+	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
+	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
+	CalculateR1R2U(rotation1, rotation2);
+	CalculatePositionConstraintProperties(rotation1, rotation2);
+	bool pos = mPositionConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mU, mN1, mN2, inBaumgarte);
+
+	// Solve rotation constraint
+	mRotationConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), *mBody2, Mat44::sRotation(mBody2->GetRotation()));
+	bool rot = mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mInvInitialOrientation, inBaumgarte);
+
+	// Solve limits along slider axis
+	bool limit = false;
+	if (mHasLimits && mLimitsSpringSettings.mFrequency <= 0.0f)
+	{
+		rotation1 = Mat44::sRotation(mBody1->GetRotation());
+		rotation2 = Mat44::sRotation(mBody2->GetRotation());
+		CalculateR1R2U(rotation1, rotation2);
+		CalculateSlidingAxisAndPosition(rotation1);
+		CalculatePositionLimitsConstraintProperties(inDeltaTime);
+		if (mPositionLimitsConstraintPart.IsActive())
+		{
+			if (mD <= mLimitsMin)
+				limit = mPositionLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, mD - mLimitsMin, inBaumgarte);
+			else
+			{
+				JPH_ASSERT(mD >= mLimitsMax);
+				limit = mPositionLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, mD - mLimitsMax, inBaumgarte);
+			}
+		}
+	}
+
+	return pos || rot || limit;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void SliderConstraint::DrawConstraint(DebugRenderer *inRenderer) const
+{
+	RMat44 transform1 = mBody1->GetCenterOfMassTransform();
+	RMat44 transform2 = mBody2->GetCenterOfMassTransform();
+
+	// Transform the local positions into world space
+	Vec3 slider_axis = transform1.Multiply3x3(mLocalSpaceSliderAxis1);
+	RVec3 position1 = transform1 * mLocalSpacePosition1;
+	RVec3 position2 = transform2 * mLocalSpacePosition2;
+
+	// Draw constraint
+	inRenderer->DrawMarker(position1, Color::sRed, 0.1f);
+	inRenderer->DrawMarker(position2, Color::sGreen, 0.1f);
+	inRenderer->DrawLine(position1, position2, Color::sGreen);
+
+	// Draw motor
+	switch (mMotorState)
+	{
+	case EMotorState::Position:
+		inRenderer->DrawMarker(position1 + mTargetPosition * slider_axis, Color::sYellow, 1.0f);
+		break;
+
+	case EMotorState::Velocity:
+		{
+			Vec3 cur_vel = (mBody2->GetLinearVelocity() - mBody1->GetLinearVelocity()).Dot(slider_axis) * slider_axis;
+			inRenderer->DrawLine(position2, position2 + cur_vel, Color::sBlue);
+			inRenderer->DrawArrow(position2 + cur_vel, position2 + mTargetVelocity * slider_axis, Color::sRed, 0.1f);
+			break;
+		}
+
+	case EMotorState::Off:
+		break;
+	}
+}
+
+void SliderConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const
+{
+	if (mHasLimits)
+	{
+		RMat44 transform1 = mBody1->GetCenterOfMassTransform();
+		RMat44 transform2 = mBody2->GetCenterOfMassTransform();
+
+		// Transform the local positions into world space
+		Vec3 slider_axis = transform1.Multiply3x3(mLocalSpaceSliderAxis1);
+		RVec3 position1 = transform1 * mLocalSpacePosition1;
+		RVec3 position2 = transform2 * mLocalSpacePosition2;
+
+		// Calculate the limits in world space
+		RVec3 limits_min = position1 + mLimitsMin * slider_axis;
+		RVec3 limits_max = position1 + mLimitsMax * slider_axis;
+
+		inRenderer->DrawLine(limits_min, position1, Color::sWhite);
+		inRenderer->DrawLine(position2, limits_max, Color::sWhite);
+
+		inRenderer->DrawMarker(limits_min, Color::sWhite, 0.1f);
+		inRenderer->DrawMarker(limits_max, Color::sWhite, 0.1f);
+	}
+}
+#endif // JPH_DEBUG_RENDERER
+
+void SliderConstraint::SaveState(StateRecorder &inStream) const
+{
+	TwoBodyConstraint::SaveState(inStream);
+
+	mMotorConstraintPart.SaveState(inStream);
+	mPositionConstraintPart.SaveState(inStream);
+	mRotationConstraintPart.SaveState(inStream);
+	mPositionLimitsConstraintPart.SaveState(inStream);
+
+	inStream.Write(mMotorState);
+	inStream.Write(mTargetVelocity);
+	inStream.Write(mTargetPosition);
+}
+
+void SliderConstraint::RestoreState(StateRecorder &inStream)
+{
+	TwoBodyConstraint::RestoreState(inStream);
+
+	mMotorConstraintPart.RestoreState(inStream);
+	mPositionConstraintPart.RestoreState(inStream);
+	mRotationConstraintPart.RestoreState(inStream);
+	mPositionLimitsConstraintPart.RestoreState(inStream);
+
+	inStream.Read(mMotorState);
+	inStream.Read(mTargetVelocity);
+	inStream.Read(mTargetPosition);
+}
+
+Ref<ConstraintSettings> SliderConstraint::GetConstraintSettings() const
+{
+	SliderConstraintSettings *settings = new SliderConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mPoint1 = RVec3(mLocalSpacePosition1);
+	settings->mSliderAxis1 = mLocalSpaceSliderAxis1;
+	settings->mNormalAxis1 = mLocalSpaceNormal1;
+	settings->mPoint2 = RVec3(mLocalSpacePosition2);
+	Mat44 inv_initial_rotation = Mat44::sRotation(mInvInitialOrientation);
+	settings->mSliderAxis2 = inv_initial_rotation.Multiply3x3(mLocalSpaceSliderAxis1);
+	settings->mNormalAxis2 = inv_initial_rotation.Multiply3x3(mLocalSpaceNormal1);
+	settings->mLimitsMin = mLimitsMin;
+	settings->mLimitsMax = mLimitsMax;
+	settings->mLimitsSpringSettings = mLimitsSpringSettings;
+	settings->mMaxFrictionForce = mMaxFrictionForce;
+	settings->mMotorSettings = mMotorSettings;
+	return settings;
+}
+
+Mat44 SliderConstraint::GetConstraintToBody1Matrix() const
+{
+	return Mat44(Vec4(mLocalSpaceSliderAxis1, 0), Vec4(mLocalSpaceNormal1, 0), Vec4(mLocalSpaceNormal2, 0), Vec4(mLocalSpacePosition1, 1));
+}
+
+Mat44 SliderConstraint::GetConstraintToBody2Matrix() const
+{
+	Mat44 mat = Mat44::sRotation(mInvInitialOrientation).Multiply3x3(Mat44(Vec4(mLocalSpaceSliderAxis1, 0), Vec4(mLocalSpaceNormal1, 0), Vec4(mLocalSpaceNormal2, 0), Vec4(0, 0, 0, 1)));
+	mat.SetTranslation(mLocalSpacePosition2);
+	return mat;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/SliderConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SliderConstraint.h
new file mode 100644
index 0000000000..1e126e57df
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SliderConstraint.h
@@ -0,0 +1,198 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Physics/Constraints/MotorSettings.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/DualAxisConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Slider constraint settings, used to create a slider constraint
+class JPH_EXPORT SliderConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, SliderConstraintSettings)
+
+public:
+	// See: ConstraintSettings::SaveBinaryState
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+
+	/// Create an instance of this constraint.
+	/// Note that the rotation constraint will be solved from body 1. This means that if body 1 and body 2 have different masses / inertias (kinematic body = infinite mass / inertia), body 1 should be the heaviest body.
+	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
+
+	/// Simple way of setting the slider and normal axis in world space (assumes the bodies are already oriented correctly when the constraint is created)
+	void						SetSliderAxis(Vec3Arg inSliderAxis);
+
+	/// This determines in which space the constraint is setup, all properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	/// When mSpace is WorldSpace mPoint1 and mPoint2 can be automatically calculated based on the positions of the bodies when the constraint is created (the current relative position/orientation is chosen as the '0' position). Set this to false if you want to supply the attachment points yourself.
+	bool						mAutoDetectPoint = false;
+
+	/// Body 1 constraint reference frame (space determined by mSpace).
+	/// Slider axis is the axis along which movement is possible (direction), normal axis is a perpendicular vector to define the frame.
+	RVec3						mPoint1 = RVec3::sZero();
+	Vec3						mSliderAxis1 = Vec3::sAxisX();
+	Vec3						mNormalAxis1 = Vec3::sAxisY();
+
+	/// Body 2 constraint reference frame (space determined by mSpace)
+	RVec3						mPoint2 = RVec3::sZero();
+	Vec3						mSliderAxis2 = Vec3::sAxisX();
+	Vec3						mNormalAxis2 = Vec3::sAxisY();
+
+	/// When the bodies move so that mPoint1 coincides with mPoint2 the slider position is defined to be 0, movement will be limited between [mLimitsMin, mLimitsMax] where mLimitsMin e [-inf, 0] and mLimitsMax e [0, inf]
+	float						mLimitsMin = -FLT_MAX;
+	float						mLimitsMax = FLT_MAX;
+
+	/// When enabled, this makes the limits soft. When the constraint exceeds the limits, a spring force will pull it back.
+	SpringSettings				mLimitsSpringSettings;
+
+	/// Maximum amount of friction force to apply (N) when not driven by a motor.
+	float						mMaxFrictionForce = 0.0f;
+
+	/// In case the constraint is powered, this determines the motor settings around the sliding axis
+	MotorSettings				mMotorSettings;
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// A slider constraint allows movement in only 1 axis (and no rotation). Also known as a prismatic constraint.
+class JPH_EXPORT SliderConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct slider constraint
+								SliderConstraint(Body &inBody1, Body &inBody2, const SliderConstraintSettings &inSettings);
+
+	// Generic interface of a constraint
+	virtual EConstraintSubType	GetSubType() const override								{ return EConstraintSubType::Slider; }
+	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
+	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
+	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
+	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
+	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void				DrawConstraint(DebugRenderer *inRenderer) const override;
+	virtual void				DrawConstraintLimits(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual void				SaveState(StateRecorder &inStream) const override;
+	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
+
+	// See: TwoBodyConstraint
+	virtual Mat44				GetConstraintToBody1Matrix() const override;
+	virtual Mat44				GetConstraintToBody2Matrix() const override;
+
+	/// Get the current distance from the rest position
+	float						GetCurrentPosition() const;
+
+	/// Friction control
+	void						SetMaxFrictionForce(float inFrictionForce)				{ mMaxFrictionForce = inFrictionForce; }
+	float						GetMaxFrictionForce() const								{ return mMaxFrictionForce; }
+
+	/// Motor settings
+	MotorSettings &				GetMotorSettings()										{ return mMotorSettings; }
+	const MotorSettings &		GetMotorSettings() const								{ return mMotorSettings; }
+
+	// Motor controls
+	void						SetMotorState(EMotorState inState)						{ JPH_ASSERT(inState == EMotorState::Off || mMotorSettings.IsValid()); mMotorState = inState; }
+	EMotorState					GetMotorState() const									{ return mMotorState; }
+	void						SetTargetVelocity(float inVelocity)						{ mTargetVelocity = inVelocity; }
+	float						GetTargetVelocity() const								{ return mTargetVelocity; }
+	void						SetTargetPosition(float inPosition)						{ mTargetPosition = mHasLimits? Clamp(inPosition, mLimitsMin, mLimitsMax) : inPosition; }
+	float						GetTargetPosition() const								{ return mTargetPosition; }
+
+	/// Update the limits of the slider constraint (see SliderConstraintSettings)
+	void						SetLimits(float inLimitsMin, float inLimitsMax);
+	float						GetLimitsMin() const									{ return mLimitsMin; }
+	float						GetLimitsMax() const									{ return mLimitsMax; }
+	bool						HasLimits() const										{ return mHasLimits; }
+
+	/// Update the limits spring settings
+	const SpringSettings &		GetLimitsSpringSettings() const							{ return mLimitsSpringSettings; }
+	SpringSettings &			GetLimitsSpringSettings()								{ return mLimitsSpringSettings; }
+	void						SetLimitsSpringSettings(const SpringSettings &inLimitsSpringSettings) { mLimitsSpringSettings = inLimitsSpringSettings; }
+
+	///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint)
+	inline Vector<2>			GetTotalLambdaPosition() const							{ return mPositionConstraintPart.GetTotalLambda(); }
+	inline float				GetTotalLambdaPositionLimits() const					{ return mPositionLimitsConstraintPart.GetTotalLambda(); }
+	inline Vec3					GetTotalLambdaRotation() const							{ return mRotationConstraintPart.GetTotalLambda(); }
+	inline float				GetTotalLambdaMotor() const								{ return mMotorConstraintPart.GetTotalLambda(); }
+
+private:
+	// Internal helper function to calculate the values below
+	void						CalculateR1R2U(Mat44Arg inRotation1, Mat44Arg inRotation2);
+	void						CalculateSlidingAxisAndPosition(Mat44Arg inRotation1);
+	void						CalculatePositionConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2);
+	void						CalculatePositionLimitsConstraintProperties(float inDeltaTime);
+	void						CalculateMotorConstraintProperties(float inDeltaTime);
+
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	// Local space constraint positions
+	Vec3						mLocalSpacePosition1;
+	Vec3						mLocalSpacePosition2;
+
+	// Local space sliding direction
+	Vec3						mLocalSpaceSliderAxis1;
+
+	// Local space normals to the sliding direction (in body 1 space)
+	Vec3						mLocalSpaceNormal1;
+	Vec3						mLocalSpaceNormal2;
+
+	// Inverse of initial rotation from body 1 to body 2 in body 1 space
+	Quat						mInvInitialOrientation;
+
+	// Slider limits
+	bool						mHasLimits;
+	float						mLimitsMin;
+	float						mLimitsMax;
+
+	// Soft constraint limits
+	SpringSettings				mLimitsSpringSettings;
+
+	// Friction
+	float						mMaxFrictionForce;
+
+	// Motor controls
+	MotorSettings				mMotorSettings;
+	EMotorState					mMotorState = EMotorState::Off;
+	float						mTargetVelocity = 0.0f;
+	float						mTargetPosition = 0.0f;
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	// Positions where the point constraint acts on (middle point between center of masses)
+	Vec3						mR1;
+	Vec3						mR2;
+
+	// X2 + R2 - X1 - R1
+	Vec3						mU;
+
+	// World space sliding direction
+	Vec3						mWorldSpaceSliderAxis;
+
+	// Normals to the slider axis
+	Vec3						mN1;
+	Vec3						mN2;
+
+	// Distance along the slide axis
+	float						mD = 0.0f;
+
+	// The constraint parts
+	DualAxisConstraintPart		mPositionConstraintPart;
+	RotationEulerConstraintPart	mRotationConstraintPart;
+	AxisConstraintPart			mPositionLimitsConstraintPart;
+	AxisConstraintPart			mMotorConstraintPart;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/SpringSettings.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SpringSettings.cpp
new file mode 100644
index 0000000000..c2c32400fe
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SpringSettings.cpp
@@ -0,0 +1,35 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/SpringSettings.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SpringSettings)
+{
+	JPH_ADD_ENUM_ATTRIBUTE(SpringSettings, mMode)
+	JPH_ADD_ATTRIBUTE(SpringSettings, mFrequency)
+	JPH_ADD_ATTRIBUTE(SpringSettings, mDamping)
+}
+
+void SpringSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(mMode);
+	inStream.Write(mFrequency);
+	inStream.Write(mDamping);
+}
+
+void SpringSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	inStream.Read(mMode);
+	inStream.Read(mFrequency);
+	inStream.Read(mDamping);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/SpringSettings.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SpringSettings.h
new file mode 100644
index 0000000000..bfa49caac0
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SpringSettings.h
@@ -0,0 +1,70 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/ObjectStream/SerializableObject.h>
+
+JPH_NAMESPACE_BEGIN
+
+class StreamIn;
+class StreamOut;
+
+/// Enum used by constraints to specify how the spring is defined
+enum class ESpringMode : uint8
+{
+	FrequencyAndDamping,		///< Frequency and damping are specified
+	StiffnessAndDamping,		///< Stiffness and damping are specified
+};
+
+/// Settings for a linear or angular spring
+class JPH_EXPORT SpringSettings
+{
+	JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SpringSettings)
+
+public:
+	/// Constructor
+								SpringSettings() = default;
+								SpringSettings(const SpringSettings &) = default;
+	SpringSettings &			operator = (const SpringSettings &) = default;
+								SpringSettings(ESpringMode inMode, float inFrequencyOrStiffness, float inDamping) : mMode(inMode), mFrequency(inFrequencyOrStiffness), mDamping(inDamping) { }
+
+	/// Saves the contents of the spring settings in binary form to inStream.
+	void						SaveBinaryState(StreamOut &inStream) const;
+
+	/// Restores contents from the binary stream inStream.
+	void						RestoreBinaryState(StreamIn &inStream);
+
+	/// Check if the spring has a valid frequency / stiffness, if not the spring will be hard
+	inline bool					HasStiffness() const							{ return mFrequency > 0.0f; }
+
+	/// Selects the way in which the spring is defined
+	/// If the mode is StiffnessAndDamping then mFrequency becomes the stiffness (k) and mDamping becomes the damping ratio (c) in the spring equation F = -k * x - c * v. Otherwise the properties are as documented.
+	ESpringMode					mMode = ESpringMode::FrequencyAndDamping;
+
+	union
+	{
+		/// Valid when mSpringMode = ESpringMode::FrequencyAndDamping.
+		/// If mFrequency > 0 the constraint will be soft and mFrequency specifies the oscillation frequency in Hz.
+		/// If mFrequency <= 0, mDamping is ignored and the constraint will have hard limits (as hard as the time step / the number of velocity / position solver steps allows).
+		float					mFrequency = 0.0f;
+
+		/// Valid when mSpringMode = ESpringMode::StiffnessAndDamping.
+		/// If mStiffness > 0 the constraint will be soft and mStiffness specifies the stiffness (k) in the spring equation F = -k * x - c * v for a linear or T = -k * theta - c * w for an angular spring.
+		/// If mStiffness <= 0, mDamping is ignored and the constraint will have hard limits (as hard as the time step / the number of velocity / position solver steps allows).
+		///
+		/// Note that stiffness values are large numbers. To calculate a ballpark value for the needed stiffness you can use:
+		/// force = stiffness * delta_spring_length = mass * gravity <=> stiffness = mass * gravity / delta_spring_length.
+		/// So if your object weighs 1500 kg and the spring compresses by 2 meters, you need a stiffness in the order of 1500 * 9.81 / 2 ~ 7500 N/m.
+		float					mStiffness;
+	};
+
+	/// When mSpringMode = ESpringMode::FrequencyAndDamping mDamping is the damping ratio (0 = no damping, 1 = critical damping).
+	/// When mSpringMode = ESpringMode::StiffnessAndDamping mDamping is the damping (c) in the spring equation F = -k * x - c * v for a linear or T = -k * theta - c * w for an angular spring.
+	/// Note that if you set mDamping = 0, you will not get an infinite oscillation. Because we integrate physics using an explicit Euler scheme, there is always energy loss.
+	/// This is done to keep the simulation from exploding, because with a damping of 0 and even the slightest rounding error, the oscillation could become bigger and bigger until the simulation explodes.
+	float						mDamping = 0.0f;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/SwingTwistConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SwingTwistConstraint.cpp
new file mode 100644
index 0000000000..bcd74ff305
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SwingTwistConstraint.cpp
@@ -0,0 +1,524 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/SwingTwistConstraint.h>
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SwingTwistConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(SwingTwistConstraintSettings, TwoBodyConstraintSettings)
+
+	JPH_ADD_ENUM_ATTRIBUTE(SwingTwistConstraintSettings, mSpace)
+	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPosition1)
+	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistAxis1)
+	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPlaneAxis1)
+	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPosition2)
+	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistAxis2)
+	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPlaneAxis2)
+	JPH_ADD_ENUM_ATTRIBUTE(SwingTwistConstraintSettings, mSwingType)
+	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mNormalHalfConeAngle)
+	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPlaneHalfConeAngle)
+	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistMinAngle)
+	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistMaxAngle)
+	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mMaxFrictionTorque)
+	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mSwingMotorSettings)
+	JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistMotorSettings)
+}
+
+void SwingTwistConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	ConstraintSettings::SaveBinaryState(inStream);
+
+	inStream.Write(mSpace);
+	inStream.Write(mPosition1);
+	inStream.Write(mTwistAxis1);
+	inStream.Write(mPlaneAxis1);
+	inStream.Write(mPosition2);
+	inStream.Write(mTwistAxis2);
+	inStream.Write(mPlaneAxis2);
+	inStream.Write(mSwingType);
+	inStream.Write(mNormalHalfConeAngle);
+	inStream.Write(mPlaneHalfConeAngle);
+	inStream.Write(mTwistMinAngle);
+	inStream.Write(mTwistMaxAngle);
+	inStream.Write(mMaxFrictionTorque);
+	mSwingMotorSettings.SaveBinaryState(inStream);
+	mTwistMotorSettings.SaveBinaryState(inStream);
+}
+
+void SwingTwistConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	ConstraintSettings::RestoreBinaryState(inStream);
+
+	inStream.Read(mSpace);
+	inStream.Read(mPosition1);
+	inStream.Read(mTwistAxis1);
+	inStream.Read(mPlaneAxis1);
+	inStream.Read(mPosition2);
+	inStream.Read(mTwistAxis2);
+	inStream.Read(mPlaneAxis2);
+	inStream.Read(mSwingType);
+	inStream.Read(mNormalHalfConeAngle);
+	inStream.Read(mPlaneHalfConeAngle);
+	inStream.Read(mTwistMinAngle);
+	inStream.Read(mTwistMaxAngle);
+	inStream.Read(mMaxFrictionTorque);
+	mSwingMotorSettings.RestoreBinaryState(inStream);
+	mTwistMotorSettings.RestoreBinaryState(inStream);
+}
+
+TwoBodyConstraint *SwingTwistConstraintSettings::Create(Body &inBody1, Body &inBody2) const
+{
+	return new SwingTwistConstraint(inBody1, inBody2, *this);
+}
+
+void SwingTwistConstraint::UpdateLimits()
+{
+	// Pass limits on to swing twist constraint part
+	mSwingTwistConstraintPart.SetLimits(mTwistMinAngle, mTwistMaxAngle, -mPlaneHalfConeAngle, mPlaneHalfConeAngle, -mNormalHalfConeAngle, mNormalHalfConeAngle);
+}
+
+SwingTwistConstraint::SwingTwistConstraint(Body &inBody1, Body &inBody2, const SwingTwistConstraintSettings &inSettings) :
+	TwoBodyConstraint(inBody1, inBody2, inSettings),
+	mNormalHalfConeAngle(inSettings.mNormalHalfConeAngle),
+	mPlaneHalfConeAngle(inSettings.mPlaneHalfConeAngle),
+	mTwistMinAngle(inSettings.mTwistMinAngle),
+	mTwistMaxAngle(inSettings.mTwistMaxAngle),
+	mMaxFrictionTorque(inSettings.mMaxFrictionTorque),
+	mSwingMotorSettings(inSettings.mSwingMotorSettings),
+	mTwistMotorSettings(inSettings.mTwistMotorSettings)
+{
+	// Override swing type
+	mSwingTwistConstraintPart.SetSwingType(inSettings.mSwingType);
+
+	// Calculate rotation needed to go from constraint space to body1 local space
+	Vec3 normal_axis1 = inSettings.mPlaneAxis1.Cross(inSettings.mTwistAxis1);
+	Mat44 c_to_b1(Vec4(inSettings.mTwistAxis1, 0), Vec4(normal_axis1, 0), Vec4(inSettings.mPlaneAxis1, 0), Vec4(0, 0, 0, 1));
+	mConstraintToBody1 = c_to_b1.GetQuaternion();
+
+	// Calculate rotation needed to go from constraint space to body2 local space
+	Vec3 normal_axis2 = inSettings.mPlaneAxis2.Cross(inSettings.mTwistAxis2);
+	Mat44 c_to_b2(Vec4(inSettings.mTwistAxis2, 0), Vec4(normal_axis2, 0), Vec4(inSettings.mPlaneAxis2, 0), Vec4(0, 0, 0, 1));
+	mConstraintToBody2 = c_to_b2.GetQuaternion();
+
+	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
+	{
+		// If all properties were specified in world space, take them to local space now
+		mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mPosition1);
+		mConstraintToBody1 = inBody1.GetRotation().Conjugated() * mConstraintToBody1;
+
+		mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mPosition2);
+		mConstraintToBody2 = inBody2.GetRotation().Conjugated() * mConstraintToBody2;
+	}
+	else
+	{
+		mLocalSpacePosition1 = Vec3(inSettings.mPosition1);
+		mLocalSpacePosition2 = Vec3(inSettings.mPosition2);
+	}
+
+	UpdateLimits();
+}
+
+void SwingTwistConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
+{
+	if (mBody1->GetID() == inBodyID)
+		mLocalSpacePosition1 -= inDeltaCOM;
+	else if (mBody2->GetID() == inBodyID)
+		mLocalSpacePosition2 -= inDeltaCOM;
+}
+
+Quat SwingTwistConstraint::GetRotationInConstraintSpace() const
+{
+	// Let b1, b2 be the center of mass transform of body1 and body2 (For body1 this is mBody1->GetCenterOfMassTransform())
+	// Let c1, c2 be the transform that takes a vector from constraint space to local space of body1 and body2 (For body1 this is Mat44::sRotationTranslation(mConstraintToBody1, mLocalSpacePosition1))
+	// Let q be the rotation of the constraint in constraint space
+	// b2 takes a vector from the local space of body2 to world space
+	// To express this in terms of b1: b2 = b1 * c1 * q * c2^-1
+	// c2^-1 goes from local body 2 space to constraint space
+	// q rotates the constraint
+	// c1 goes from constraint space to body 1 local space
+	// b1 goes from body 1 local space to world space
+	// So when the body rotations are given, q = (b1 * c1)^-1 * b2 c2
+	// Or: q = (q1 * c1)^-1 * (q2 * c2) if we're only interested in rotations
+	Quat constraint_body1_to_world = mBody1->GetRotation() * mConstraintToBody1;
+	Quat constraint_body2_to_world = mBody2->GetRotation() * mConstraintToBody2;
+	return constraint_body1_to_world.Conjugated() * constraint_body2_to_world;
+}
+
+void SwingTwistConstraint::SetSwingMotorState(EMotorState inState)
+{
+	JPH_ASSERT(inState == EMotorState::Off || mSwingMotorSettings.IsValid());
+
+	if (mSwingMotorState != inState)
+	{
+		mSwingMotorState = inState;
+
+		// Ensure that warm starting next frame doesn't apply any impulses (motor parts are repurposed for different modes)
+		for (AngleConstraintPart &c : mMotorConstraintPart)
+			c.Deactivate();
+	}
+}
+
+void SwingTwistConstraint::SetTwistMotorState(EMotorState inState)
+{
+	JPH_ASSERT(inState == EMotorState::Off || mTwistMotorSettings.IsValid());
+
+	if (mTwistMotorState != inState)
+	{
+		mTwistMotorState = inState;
+
+		// Ensure that warm starting next frame doesn't apply any impulses (motor parts are repurposed for different modes)
+		mMotorConstraintPart[0].Deactivate();
+	}
+}
+
+void SwingTwistConstraint::SetTargetOrientationCS(QuatArg inOrientation)
+{
+	Quat q_swing, q_twist;
+	inOrientation.GetSwingTwist(q_swing, q_twist);
+
+	uint clamped_axis;
+	mSwingTwistConstraintPart.ClampSwingTwist(q_swing, q_twist, clamped_axis);
+
+	if (clamped_axis != 0)
+		mTargetOrientation = q_swing * q_twist;
+	else
+		mTargetOrientation = inOrientation;
+}
+
+void SwingTwistConstraint::SetupVelocityConstraint(float inDeltaTime)
+{
+	// Setup point constraint
+	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
+	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
+	mPointConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, mLocalSpacePosition1, *mBody2, rotation2, mLocalSpacePosition2);
+
+	// GetRotationInConstraintSpace written out since we reuse the sub expressions
+	Quat constraint_body1_to_world = mBody1->GetRotation() * mConstraintToBody1;
+	Quat constraint_body2_to_world = mBody2->GetRotation() * mConstraintToBody2;
+	Quat q = constraint_body1_to_world.Conjugated() * constraint_body2_to_world;
+
+	// Calculate constraint properties for the swing twist limit
+	mSwingTwistConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, q, constraint_body1_to_world);
+
+	if (mSwingMotorState != EMotorState::Off || mTwistMotorState != EMotorState::Off || mMaxFrictionTorque > 0.0f)
+	{
+		// Calculate rotation motor axis
+		Mat44 ws_axis = Mat44::sRotation(constraint_body2_to_world);
+		for (int i = 0; i < 3; ++i)
+			mWorldSpaceMotorAxis[i] = ws_axis.GetColumn3(i);
+
+		Vec3 rotation_error;
+		if (mSwingMotorState == EMotorState::Position || mTwistMotorState == EMotorState::Position)
+		{
+			// Get target orientation along the shortest path from q
+			Quat target_orientation = q.Dot(mTargetOrientation) > 0.0f? mTargetOrientation : -mTargetOrientation;
+
+			// The definition of the constraint rotation q:
+			// R2 * ConstraintToBody2 = R1 * ConstraintToBody1 * q (1)
+			//
+			// R2' is the rotation of body 2 when reaching the target_orientation:
+			// R2' * ConstraintToBody2 = R1 * ConstraintToBody1 * target_orientation (2)
+			//
+			// The difference in body 2 space:
+			// R2' = R2 * diff_body2 (3)
+			//
+			// We want to specify the difference in the constraint space of body 2:
+			// diff_body2 = ConstraintToBody2 * diff * ConstraintToBody2^* (4)
+			//
+			// Extracting R2' from 2: R2' = R1 * ConstraintToBody1 * target_orientation * ConstraintToBody2^* (5)
+			// Combining 3 & 4: R2' = R2 * ConstraintToBody2 * diff * ConstraintToBody2^* (6)
+			// Combining 1 & 6: R2' = R1 * ConstraintToBody1 * q * diff * ConstraintToBody2^* (7)
+			// Combining 5 & 7: R1 * ConstraintToBody1 * target_orientation * ConstraintToBody2^* = R1 * ConstraintToBody1 * q * diff * ConstraintToBody2^*
+			// <=> target_orientation = q * diff
+			// <=> diff = q^* * target_orientation
+			Quat diff = q.Conjugated() * target_orientation;
+
+			// Approximate error angles
+			// The imaginary part of a quaternion is rotation_axis * sin(angle / 2)
+			// If angle is small, sin(x) = x so angle[i] ~ 2.0f * rotation_axis[i]
+			// We'll be making small time steps, so if the angle is not small at least the sign will be correct and we'll move in the right direction
+			rotation_error = -2.0f * diff.GetXYZ();
+		}
+
+		// Swing motor
+		switch (mSwingMotorState)
+		{
+		case EMotorState::Off:
+			if (mMaxFrictionTorque > 0.0f)
+			{
+				// Enable friction
+				for (int i = 1; i < 3; ++i)
+					mMotorConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceMotorAxis[i], 0.0f);
+			}
+			else
+			{
+				// Disable friction
+				for (AngleConstraintPart &c : mMotorConstraintPart)
+					c.Deactivate();
+			}
+			break;
+
+		case EMotorState::Velocity:
+			// Use motor to create angular velocity around desired axis
+			for (int i = 1; i < 3; ++i)
+				mMotorConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceMotorAxis[i], -mTargetAngularVelocity[i]);
+			break;
+
+		case EMotorState::Position:
+			// Use motor to drive rotation error to zero
+			if (mSwingMotorSettings.mSpringSettings.HasStiffness())
+			{
+				for (int i = 1; i < 3; ++i)
+					mMotorConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mWorldSpaceMotorAxis[i], 0.0f, rotation_error[i], mSwingMotorSettings.mSpringSettings);
+			}
+			else
+			{
+				for (int i = 1; i < 3; ++i)
+					mMotorConstraintPart[i].Deactivate();
+			}
+			break;
+		}
+
+		// Twist motor
+		switch (mTwistMotorState)
+		{
+		case EMotorState::Off:
+			if (mMaxFrictionTorque > 0.0f)
+			{
+				// Enable friction
+				mMotorConstraintPart[0].CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceMotorAxis[0], 0.0f);
+			}
+			else
+			{
+				// Disable friction
+				mMotorConstraintPart[0].Deactivate();
+			}
+			break;
+
+		case EMotorState::Velocity:
+			// Use motor to create angular velocity around desired axis
+			mMotorConstraintPart[0].CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceMotorAxis[0], -mTargetAngularVelocity[0]);
+			break;
+
+		case EMotorState::Position:
+			// Use motor to drive rotation error to zero
+			if (mTwistMotorSettings.mSpringSettings.HasStiffness())
+				mMotorConstraintPart[0].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mWorldSpaceMotorAxis[0], 0.0f, rotation_error[0], mTwistMotorSettings.mSpringSettings);
+			else
+				mMotorConstraintPart[0].Deactivate();
+			break;
+		}
+	}
+	else
+	{
+		// Disable rotation motor
+		for (AngleConstraintPart &c : mMotorConstraintPart)
+			c.Deactivate();
+	}
+}
+
+void SwingTwistConstraint::ResetWarmStart()
+{
+	for (AngleConstraintPart &c : mMotorConstraintPart)
+		c.Deactivate();
+	mSwingTwistConstraintPart.Deactivate();
+	mPointConstraintPart.Deactivate();
+}
+
+void SwingTwistConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
+{
+	// Warm starting: Apply previous frame impulse
+	for (AngleConstraintPart &c : mMotorConstraintPart)
+		c.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+	mSwingTwistConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+	mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
+}
+
+bool SwingTwistConstraint::SolveVelocityConstraint(float inDeltaTime)
+{
+	bool impulse = false;
+
+	// Solve twist rotation motor
+	if (mMotorConstraintPart[0].IsActive())
+	{
+		// Twist limits
+		float min_twist_limit, max_twist_limit;
+		if (mTwistMotorState == EMotorState::Off)
+		{
+			max_twist_limit = inDeltaTime * mMaxFrictionTorque;
+			min_twist_limit = -max_twist_limit;
+		}
+		else
+		{
+			min_twist_limit = inDeltaTime * mTwistMotorSettings.mMinTorqueLimit;
+			max_twist_limit = inDeltaTime * mTwistMotorSettings.mMaxTorqueLimit;
+		}
+
+		impulse |= mMotorConstraintPart[0].SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceMotorAxis[0], min_twist_limit, max_twist_limit);
+	}
+
+	// Solve swing rotation motor
+	if (mMotorConstraintPart[1].IsActive())
+	{
+		// Swing parts should turn on / off together
+		JPH_ASSERT(mMotorConstraintPart[2].IsActive());
+
+		// Swing limits
+		float min_swing_limit, max_swing_limit;
+		if (mSwingMotorState == EMotorState::Off)
+		{
+			max_swing_limit = inDeltaTime * mMaxFrictionTorque;
+			min_swing_limit = -max_swing_limit;
+		}
+		else
+		{
+			min_swing_limit = inDeltaTime * mSwingMotorSettings.mMinTorqueLimit;
+			max_swing_limit = inDeltaTime * mSwingMotorSettings.mMaxTorqueLimit;
+		}
+
+		for (int i = 1; i < 3; ++i)
+			impulse |= mMotorConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceMotorAxis[i], min_swing_limit, max_swing_limit);
+	}
+	else
+	{
+		// Swing parts should turn on / off together
+		JPH_ASSERT(!mMotorConstraintPart[2].IsActive());
+	}
+
+	// Solve rotation limits
+	impulse |= mSwingTwistConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+
+	// Solve position constraint
+	impulse |= mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
+
+	return impulse;
+}
+
+bool SwingTwistConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
+{
+	bool impulse = false;
+
+	// Solve rotation violations
+	Quat q = GetRotationInConstraintSpace();
+	impulse |= mSwingTwistConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, q, mConstraintToBody1, mConstraintToBody2, inBaumgarte);
+
+	// Solve position violations
+	mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2);
+	impulse |= mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte);
+
+	return impulse;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void SwingTwistConstraint::DrawConstraint(DebugRenderer *inRenderer) const
+{
+	// Get constraint properties in world space
+	RMat44 transform1 = mBody1->GetCenterOfMassTransform();
+	RVec3 position1 = transform1 * mLocalSpacePosition1;
+	Quat rotation1 = mBody1->GetRotation() * mConstraintToBody1;
+	Quat rotation2 = mBody2->GetRotation() * mConstraintToBody2;
+
+	// Draw constraint orientation
+	inRenderer->DrawCoordinateSystem(RMat44::sRotationTranslation(rotation1, position1), mDrawConstraintSize);
+
+	// Draw current swing and twist
+	Quat q = GetRotationInConstraintSpace();
+	Quat q_swing, q_twist;
+	q.GetSwingTwist(q_swing, q_twist);
+	inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * q_twist).RotateAxisY(), Color::sWhite);
+	inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * q_swing).RotateAxisX(), Color::sWhite);
+
+	if (mSwingMotorState == EMotorState::Velocity || mTwistMotorState == EMotorState::Velocity)
+	{
+		// Draw target angular velocity
+		inRenderer->DrawArrow(position1, position1 + rotation2 * mTargetAngularVelocity, Color::sRed, 0.1f);
+	}
+	if (mSwingMotorState == EMotorState::Position || mTwistMotorState == EMotorState::Position)
+	{
+		// Draw motor swing and twist
+		Quat swing, twist;
+		mTargetOrientation.GetSwingTwist(swing, twist);
+		inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * twist).RotateAxisY(), Color::sYellow);
+		inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * swing).RotateAxisX(), Color::sCyan);
+	}
+}
+
+void SwingTwistConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const
+{
+	// Get matrix that transforms from constraint space to world space
+	RMat44 constraint_to_world = RMat44::sRotationTranslation(mBody1->GetRotation() * mConstraintToBody1, mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1);
+
+	// Draw limits
+	if (mSwingTwistConstraintPart.GetSwingType() == ESwingType::Pyramid)
+		inRenderer->DrawSwingPyramidLimits(constraint_to_world, -mPlaneHalfConeAngle, mPlaneHalfConeAngle, -mNormalHalfConeAngle, mNormalHalfConeAngle, mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off);
+	else
+		inRenderer->DrawSwingConeLimits(constraint_to_world, mPlaneHalfConeAngle, mNormalHalfConeAngle, mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off);
+	inRenderer->DrawPie(constraint_to_world.GetTranslation(), mDrawConstraintSize, constraint_to_world.GetAxisX(), constraint_to_world.GetAxisY(), mTwistMinAngle, mTwistMaxAngle, Color::sPurple, DebugRenderer::ECastShadow::Off);
+}
+#endif // JPH_DEBUG_RENDERER
+
+void SwingTwistConstraint::SaveState(StateRecorder &inStream) const
+{
+	TwoBodyConstraint::SaveState(inStream);
+
+	mPointConstraintPart.SaveState(inStream);
+	mSwingTwistConstraintPart.SaveState(inStream);
+	for (const AngleConstraintPart &c : mMotorConstraintPart)
+		c.SaveState(inStream);
+
+	inStream.Write(mSwingMotorState);
+	inStream.Write(mTwistMotorState);
+	inStream.Write(mTargetAngularVelocity);
+	inStream.Write(mTargetOrientation);
+}
+
+void SwingTwistConstraint::RestoreState(StateRecorder &inStream)
+{
+	TwoBodyConstraint::RestoreState(inStream);
+
+	mPointConstraintPart.RestoreState(inStream);
+	mSwingTwistConstraintPart.RestoreState(inStream);
+	for (AngleConstraintPart &c : mMotorConstraintPart)
+		c.RestoreState(inStream);
+
+	inStream.Read(mSwingMotorState);
+	inStream.Read(mTwistMotorState);
+	inStream.Read(mTargetAngularVelocity);
+	inStream.Read(mTargetOrientation);
+}
+
+Ref<ConstraintSettings> SwingTwistConstraint::GetConstraintSettings() const
+{
+	SwingTwistConstraintSettings *settings = new SwingTwistConstraintSettings;
+	ToConstraintSettings(*settings);
+	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
+	settings->mPosition1 = RVec3(mLocalSpacePosition1);
+	settings->mTwistAxis1 = mConstraintToBody1.RotateAxisX();
+	settings->mPlaneAxis1 = mConstraintToBody1.RotateAxisZ();
+	settings->mPosition2 = RVec3(mLocalSpacePosition2);
+	settings->mTwistAxis2 = mConstraintToBody2.RotateAxisX();
+	settings->mPlaneAxis2 = mConstraintToBody2.RotateAxisZ();
+	settings->mSwingType = mSwingTwistConstraintPart.GetSwingType();
+	settings->mNormalHalfConeAngle = mNormalHalfConeAngle;
+	settings->mPlaneHalfConeAngle = mPlaneHalfConeAngle;
+	settings->mTwistMinAngle = mTwistMinAngle;
+	settings->mTwistMaxAngle = mTwistMaxAngle;
+	settings->mMaxFrictionTorque = mMaxFrictionTorque;
+	settings->mSwingMotorSettings = mSwingMotorSettings;
+	settings->mTwistMotorSettings = mTwistMotorSettings;
+	return settings;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/SwingTwistConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SwingTwistConstraint.h
new file mode 100644
index 0000000000..5e3e896f44
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/SwingTwistConstraint.h
@@ -0,0 +1,197 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Physics/Constraints/MotorSettings.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/SwingTwistConstraintPart.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Swing twist constraint settings, used to create a swing twist constraint
+/// All values in this structure are copied to the swing twist constraint and the settings object is no longer needed afterwards.
+///
+/// This image describes the limit settings:
+/// @image html Docs/SwingTwistConstraint.png
+class JPH_EXPORT SwingTwistConstraintSettings final : public TwoBodyConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, SwingTwistConstraintSettings)
+
+public:
+	// See: ConstraintSettings::SaveBinaryState
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+
+	/// Create an instance of this constraint
+	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
+
+	/// This determines in which space the constraint is setup, all properties below should be in the specified space
+	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
+
+	///@name Body 1 constraint reference frame (space determined by mSpace)
+	RVec3						mPosition1 = RVec3::sZero();
+	Vec3						mTwistAxis1 = Vec3::sAxisX();
+	Vec3						mPlaneAxis1 = Vec3::sAxisY();
+
+	///@name Body 2 constraint reference frame (space determined by mSpace)
+	RVec3						mPosition2 = RVec3::sZero();
+	Vec3						mTwistAxis2 = Vec3::sAxisX();
+	Vec3						mPlaneAxis2 = Vec3::sAxisY();
+
+	/// The type of swing constraint that we want to use.
+	ESwingType					mSwingType = ESwingType::Cone;
+
+	///@name Swing rotation limits
+	float						mNormalHalfConeAngle = 0.0f;								///< See image at Detailed Description. Angle in radians.
+	float						mPlaneHalfConeAngle = 0.0f;									///< See image at Detailed Description. Angle in radians.
+
+	///@name Twist rotation limits
+	float						mTwistMinAngle = 0.0f;										///< See image at Detailed Description. Angle in radians. Should be \f$\in [-\pi, \pi]\f$.
+	float						mTwistMaxAngle = 0.0f;										///< See image at Detailed Description. Angle in radians. Should be \f$\in [-\pi, \pi]\f$.
+
+	///@name Friction
+	float						mMaxFrictionTorque = 0.0f;									///< Maximum amount of torque (N m) to apply as friction when the constraint is not powered by a motor
+
+	///@name In case the constraint is powered, this determines the motor settings around the swing and twist axis
+	MotorSettings				mSwingMotorSettings;
+	MotorSettings				mTwistMotorSettings;
+
+protected:
+	// See: ConstraintSettings::RestoreBinaryState
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// A swing twist constraint is a specialized constraint for humanoid ragdolls that allows limited rotation only
+///
+/// @see SwingTwistConstraintSettings for a description of the limits
+class JPH_EXPORT SwingTwistConstraint final : public TwoBodyConstraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Construct swing twist constraint
+								SwingTwistConstraint(Body &inBody1, Body &inBody2, const SwingTwistConstraintSettings &inSettings);
+
+	///@name Generic interface of a constraint
+	virtual EConstraintSubType	GetSubType() const override									{ return EConstraintSubType::SwingTwist; }
+	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
+	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
+	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
+	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
+	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void				DrawConstraint(DebugRenderer *inRenderer) const override;
+	virtual void				DrawConstraintLimits(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual void				SaveState(StateRecorder &inStream) const override;
+	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
+
+	// See: TwoBodyConstraint
+	virtual Mat44				GetConstraintToBody1Matrix() const override					{ return Mat44::sRotationTranslation(mConstraintToBody1, mLocalSpacePosition1); }
+	virtual Mat44				GetConstraintToBody2Matrix() const override					{ return Mat44::sRotationTranslation(mConstraintToBody2, mLocalSpacePosition2); }
+
+	///@name Constraint reference frame
+	inline Vec3					GetLocalSpacePosition1() const								{ return mLocalSpacePosition1; }
+	inline Vec3					GetLocalSpacePosition2() const								{ return mLocalSpacePosition2; }
+	inline Quat					GetConstraintToBody1() const								{ return mConstraintToBody1; }
+	inline Quat					GetConstraintToBody2() const								{ return mConstraintToBody2; }
+
+	///@name Constraint limits
+	inline float				GetNormalHalfConeAngle() const								{ return mNormalHalfConeAngle; }
+	inline void					SetNormalHalfConeAngle(float inAngle)						{ mNormalHalfConeAngle = inAngle; UpdateLimits(); }
+	inline float				GetPlaneHalfConeAngle() const								{ return mPlaneHalfConeAngle; }
+	inline void					SetPlaneHalfConeAngle(float inAngle)						{ mPlaneHalfConeAngle = inAngle; UpdateLimits(); }
+	inline float				GetTwistMinAngle() const									{ return mTwistMinAngle; }
+	inline void					SetTwistMinAngle(float inAngle)								{ mTwistMinAngle = inAngle; UpdateLimits(); }
+	inline float				GetTwistMaxAngle() const									{ return mTwistMaxAngle; }
+	inline void					SetTwistMaxAngle(float inAngle)								{ mTwistMaxAngle = inAngle; UpdateLimits(); }
+
+	///@name Motor settings
+	const MotorSettings &		GetSwingMotorSettings() const								{ return mSwingMotorSettings; }
+	MotorSettings &				GetSwingMotorSettings()										{ return mSwingMotorSettings; }
+	const MotorSettings &		GetTwistMotorSettings() const								{ return mTwistMotorSettings; }
+	MotorSettings &				GetTwistMotorSettings()										{ return mTwistMotorSettings; }
+
+	///@name Friction control
+	void						SetMaxFrictionTorque(float inFrictionTorque)				{ mMaxFrictionTorque = inFrictionTorque; }
+	float						GetMaxFrictionTorque() const								{ return mMaxFrictionTorque; }
+
+	///@name Motor controls
+
+	/// Controls if the motors are on or off
+	void						SetSwingMotorState(EMotorState inState);
+	EMotorState					GetSwingMotorState() const									{ return mSwingMotorState; }
+	void						SetTwistMotorState(EMotorState inState);
+	EMotorState					GetTwistMotorState() const									{ return mTwistMotorState; }
+
+	/// Set the target angular velocity of body 2 in constraint space of body 2
+	void						SetTargetAngularVelocityCS(Vec3Arg inAngularVelocity)		{ mTargetAngularVelocity = inAngularVelocity; }
+	Vec3						GetTargetAngularVelocityCS() const							{ return mTargetAngularVelocity; }
+
+	/// Set the target orientation in constraint space (drives constraint to: GetRotationInConstraintSpace() == inOrientation)
+	void						SetTargetOrientationCS(QuatArg inOrientation);
+	Quat						GetTargetOrientationCS() const								{ return mTargetOrientation; }
+
+	/// Set the target orientation in body space (R2 = R1 * inOrientation, where R1 and R2 are the world space rotations for body 1 and 2).
+	/// Solve: R2 * ConstraintToBody2 = R1 * ConstraintToBody1 * q (see SwingTwistConstraint::GetSwingTwist) and R2 = R1 * inOrientation for q.
+	void						SetTargetOrientationBS(QuatArg inOrientation)				{ SetTargetOrientationCS(mConstraintToBody1.Conjugated() * inOrientation * mConstraintToBody2); }
+
+	/// Get current rotation of constraint in constraint space.
+	/// Solve: R2 * ConstraintToBody2 = R1 * ConstraintToBody1 * q for q.
+	Quat						GetRotationInConstraintSpace() const;
+
+	///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint)
+	inline Vec3					GetTotalLambdaPosition() const								{ return mPointConstraintPart.GetTotalLambda(); }
+	inline float				GetTotalLambdaTwist() const									{ return mSwingTwistConstraintPart.GetTotalTwistLambda(); }
+	inline float				GetTotalLambdaSwingY() const								{ return mSwingTwistConstraintPart.GetTotalSwingYLambda(); }
+	inline float				GetTotalLambdaSwingZ() const								{ return mSwingTwistConstraintPart.GetTotalSwingZLambda(); }
+	inline Vec3					GetTotalLambdaMotor() const									{ return Vec3(mMotorConstraintPart[0].GetTotalLambda(), mMotorConstraintPart[1].GetTotalLambda(), mMotorConstraintPart[2].GetTotalLambda()); }
+
+private:
+	// Update the limits in the swing twist constraint part
+	void						UpdateLimits();
+
+	// CONFIGURATION PROPERTIES FOLLOW
+
+	// Local space constraint positions
+	Vec3						mLocalSpacePosition1;
+	Vec3						mLocalSpacePosition2;
+
+	// Transforms from constraint space to body space
+	Quat						mConstraintToBody1;
+	Quat						mConstraintToBody2;
+
+	// Limits
+	float						mNormalHalfConeAngle;
+	float						mPlaneHalfConeAngle;
+	float						mTwistMinAngle;
+	float						mTwistMaxAngle;
+
+	// Friction
+	float						mMaxFrictionTorque;
+
+	// Motor controls
+	MotorSettings				mSwingMotorSettings;
+	MotorSettings				mTwistMotorSettings;
+	EMotorState					mSwingMotorState = EMotorState::Off;
+	EMotorState					mTwistMotorState = EMotorState::Off;
+	Vec3						mTargetAngularVelocity = Vec3::sZero();
+	Quat						mTargetOrientation = Quat::sIdentity();
+
+	// RUN TIME PROPERTIES FOLLOW
+
+	// Rotation axis for motor constraint parts
+	Vec3						mWorldSpaceMotorAxis[3];
+
+	// The constraint parts
+	PointConstraintPart			mPointConstraintPart;
+	SwingTwistConstraintPart	mSwingTwistConstraintPart;
+	AngleConstraintPart			mMotorConstraintPart[3];
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/TwoBodyConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/TwoBodyConstraint.cpp
new file mode 100644
index 0000000000..9ee7cc5d89
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/TwoBodyConstraint.cpp
@@ -0,0 +1,56 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Physics/IslandBuilder.h>
+#include <Jolt/Physics/LargeIslandSplitter.h>
+#include <Jolt/Physics/Body/BodyManager.h>
+
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(TwoBodyConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(TwoBodyConstraintSettings, ConstraintSettings)
+}
+
+void TwoBodyConstraint::BuildIslands(uint32 inConstraintIndex, IslandBuilder &ioBuilder, BodyManager &inBodyManager)
+{
+	// Activate bodies
+	BodyID body_ids[2];
+	int num_bodies = 0;
+	if (mBody1->IsDynamic() && !mBody1->IsActive())
+		body_ids[num_bodies++] = mBody1->GetID();
+	if (mBody2->IsDynamic() && !mBody2->IsActive())
+		body_ids[num_bodies++] = mBody2->GetID();
+	if (num_bodies > 0)
+		inBodyManager.ActivateBodies(body_ids, num_bodies);
+
+	// Link the bodies into the same island
+	ioBuilder.LinkConstraint(inConstraintIndex, mBody1->GetIndexInActiveBodiesInternal(), mBody2->GetIndexInActiveBodiesInternal());
+}
+
+uint TwoBodyConstraint::BuildIslandSplits(LargeIslandSplitter &ioSplitter) const
+{
+	return ioSplitter.AssignSplit(mBody1, mBody2);
+}
+
+#ifdef JPH_DEBUG_RENDERER
+
+void TwoBodyConstraint::DrawConstraintReferenceFrame(DebugRenderer *inRenderer) const
+{
+	RMat44 transform1 = mBody1->GetCenterOfMassTransform() * GetConstraintToBody1Matrix();
+	RMat44 transform2 = mBody2->GetCenterOfMassTransform() * GetConstraintToBody2Matrix();
+	inRenderer->DrawCoordinateSystem(transform1, 1.1f * mDrawConstraintSize);
+	inRenderer->DrawCoordinateSystem(transform2, mDrawConstraintSize);
+}
+
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/TwoBodyConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/TwoBodyConstraint.h
new file mode 100644
index 0000000000..24630eb6de
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/TwoBodyConstraint.h
@@ -0,0 +1,65 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/Constraint.h>
+#include <Jolt/Physics/Body/Body.h>
+
+JPH_NAMESPACE_BEGIN
+
+class TwoBodyConstraint;
+
+/// Base class for settings for all constraints that involve 2 bodies
+class JPH_EXPORT TwoBodyConstraintSettings : public ConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, TwoBodyConstraintSettings)
+
+public:
+	/// Create an instance of this constraint
+	/// You can use Body::sFixedToWorld for inBody1 if you want to attach inBody2 to the world
+	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const = 0;
+};
+
+/// Base class for all constraints that involve 2 bodies. Body1 is usually considered the parent, Body2 the child.
+class JPH_EXPORT TwoBodyConstraint : public Constraint
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+								TwoBodyConstraint(Body &inBody1, Body &inBody2, const TwoBodyConstraintSettings &inSettings) : Constraint(inSettings), mBody1(&inBody1), mBody2(&inBody2) { }
+
+	/// Get the type of a constraint
+	virtual EConstraintType		GetType() const override				{ return EConstraintType::TwoBodyConstraint; }
+
+	/// Solver interface
+	virtual bool				IsActive() const override				{ return Constraint::IsActive() && (mBody1->IsActive() || mBody2->IsActive()) && (mBody2->IsDynamic() || mBody1->IsDynamic()); }
+#ifdef JPH_DEBUG_RENDERER
+	virtual void				DrawConstraintReferenceFrame(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	/// Access to the connected bodies
+	Body *						GetBody1() const						{ return mBody1; }
+	Body *						GetBody2() const						{ return mBody2; }
+
+	/// Calculates the transform that transforms from constraint space to body 1 space. The first column of the matrix is the primary constraint axis (e.g. the hinge axis / slider direction), second column the secondary etc.
+	virtual Mat44				GetConstraintToBody1Matrix() const = 0;
+
+	/// Calculates the transform that transforms from constraint space to body 2 space. The first column of the matrix is the primary constraint axis (e.g. the hinge axis / slider direction), second column the secondary etc.
+	virtual Mat44				GetConstraintToBody2Matrix() const = 0;
+
+	/// Link bodies that are connected by this constraint in the island builder
+	virtual void				BuildIslands(uint32 inConstraintIndex, IslandBuilder &ioBuilder, BodyManager &inBodyManager) override;
+
+	/// Link bodies that are connected by this constraint in the same split. Returns the split index.
+	virtual uint				BuildIslandSplits(LargeIslandSplitter &ioSplitter) const override;
+
+protected:
+	/// The two bodies involved
+	Body *						mBody1;
+	Body *						mBody2;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/DeterminismLog.cpp b/thirdparty/jolt_physics/Jolt/Physics/DeterminismLog.cpp
new file mode 100644
index 0000000000..7985a36bf9
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/DeterminismLog.cpp
@@ -0,0 +1,17 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/DeterminismLog.h>
+
+#ifdef JPH_ENABLE_DETERMINISM_LOG
+
+JPH_NAMESPACE_BEGIN
+
+DeterminismLog DeterminismLog::sLog;
+
+JPH_NAMESPACE_END
+
+#endif // JPH_ENABLE_DETERMINISM_LOG
diff --git a/thirdparty/jolt_physics/Jolt/Physics/DeterminismLog.h b/thirdparty/jolt_physics/Jolt/Physics/DeterminismLog.h
new file mode 100644
index 0000000000..e2930ff3ce
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/DeterminismLog.h
@@ -0,0 +1,159 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+//#define JPH_ENABLE_DETERMINISM_LOG
+#ifdef JPH_ENABLE_DETERMINISM_LOG
+
+#include <Jolt/Physics/Body/BodyID.h>
+#include <Jolt/Physics/Collision/Shape/SubShapeID.h>
+
+JPH_SUPPRESS_WARNINGS_STD_BEGIN
+#include <iomanip>
+#include <fstream>
+JPH_SUPPRESS_WARNINGS_STD_END
+
+JPH_NAMESPACE_BEGIN
+
+/// A simple class that logs the state of the simulation. The resulting text file can be used to diff between platforms and find issues in determinism.
+class DeterminismLog
+{
+private:
+	JPH_INLINE uint32		Convert(float inValue) const
+	{
+		return *(uint32 *)&inValue;
+	}
+
+	JPH_INLINE uint64		Convert(double inValue) const
+	{
+		return *(uint64 *)&inValue;
+	}
+
+public:
+							DeterminismLog()
+	{
+		mLog.open("detlog.txt", std::ios::out | std::ios::trunc | std::ios::binary); // Binary because we don't want a difference between Unix and Windows line endings.
+		mLog.fill('0');
+	}
+
+	DeterminismLog &		operator << (char inValue)
+	{
+		mLog << inValue;
+		return *this;
+	}
+
+	DeterminismLog &		operator << (const char *inValue)
+	{
+		mLog << std::dec << inValue;
+		return *this;
+	}
+
+	DeterminismLog &		operator << (const string &inValue)
+	{
+		mLog << std::dec << inValue;
+		return *this;
+	}
+
+	DeterminismLog &		operator << (const BodyID &inValue)
+	{
+		mLog << std::hex << std::setw(8) << inValue.GetIndexAndSequenceNumber();
+		return *this;
+	}
+
+	DeterminismLog &		operator << (const SubShapeID &inValue)
+	{
+		mLog << std::hex << std::setw(8) << inValue.GetValue();
+		return *this;
+	}
+
+	DeterminismLog &		operator << (float inValue)
+	{
+		mLog << std::hex << std::setw(8) << Convert(inValue);
+		return *this;
+	}
+
+	DeterminismLog &		operator << (int inValue)
+	{
+		mLog << inValue;
+		return *this;
+	}
+
+	DeterminismLog &		operator << (uint32 inValue)
+	{
+		mLog << std::hex << std::setw(8) << inValue;
+		return *this;
+	}
+
+	DeterminismLog &		operator << (uint64 inValue)
+	{
+		mLog << std::hex << std::setw(16) << inValue;
+		return *this;
+	}
+
+	DeterminismLog &		operator << (Vec3Arg inValue)
+	{
+		mLog << std::hex << std::setw(8) << Convert(inValue.GetX()) << " " << std::setw(8) << Convert(inValue.GetY()) << " " << std::setw(8) << Convert(inValue.GetZ());
+		return *this;
+	}
+
+	DeterminismLog &		operator << (DVec3Arg inValue)
+	{
+		mLog << std::hex << std::setw(16) << Convert(inValue.GetX()) << " " << std::setw(16) << Convert(inValue.GetY()) << " " << std::setw(16) << Convert(inValue.GetZ());
+		return *this;
+	}
+
+	DeterminismLog &		operator << (Vec4Arg inValue)
+	{
+		mLog << std::hex << std::setw(8) << Convert(inValue.GetX()) << " " << std::setw(8) << Convert(inValue.GetY()) << " " << std::setw(8) << Convert(inValue.GetZ()) << " " << std::setw(8) << Convert(inValue.GetW());
+		return *this;
+	}
+
+	DeterminismLog &		operator << (const Float3 &inValue)
+	{
+		mLog << std::hex << std::setw(8) << Convert(inValue.x) << " " << std::setw(8) << Convert(inValue.y) << " " << std::setw(8) << Convert(inValue.z);
+		return *this;
+	}
+
+	DeterminismLog &		operator << (Mat44Arg inValue)
+	{
+		*this << inValue.GetColumn4(0) << " " << inValue.GetColumn4(1) << " " << inValue.GetColumn4(2) << " " << inValue.GetColumn4(3);
+		return *this;
+	}
+
+	DeterminismLog &		operator << (DMat44Arg inValue)
+	{
+		*this << inValue.GetColumn4(0) << " " << inValue.GetColumn4(1) << " " << inValue.GetColumn4(2) << " " << inValue.GetTranslation();
+		return *this;
+	}
+
+	DeterminismLog &		operator << (QuatArg inValue)
+	{
+		*this << inValue.GetXYZW();
+		return *this;
+	}
+
+	// Singleton instance
+	static DeterminismLog	sLog;
+
+private:
+	std::ofstream			mLog;
+};
+
+/// Will log something to the determinism log, usage: JPH_DET_LOG("label " << value);
+#define JPH_DET_LOG(...)	DeterminismLog::sLog << __VA_ARGS__ << '\n'
+
+JPH_NAMESPACE_END
+
+#else
+
+JPH_SUPPRESS_WARNING_PUSH
+JPH_SUPPRESS_WARNINGS
+
+/// By default we log nothing
+#define JPH_DET_LOG(...)
+
+JPH_SUPPRESS_WARNING_POP
+
+#endif // JPH_ENABLE_DETERMINISM_LOG
diff --git a/thirdparty/jolt_physics/Jolt/Physics/EActivation.h b/thirdparty/jolt_physics/Jolt/Physics/EActivation.h
new file mode 100644
index 0000000000..08c10c20dd
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/EActivation.h
@@ -0,0 +1,16 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Enum used by AddBody to determine if the body needs to be initially active
+enum class EActivation
+{
+	Activate,				///< Activate the body, making it part of the simulation
+	DontActivate			///< Leave activation state as it is (will not deactivate an active body)
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/EPhysicsUpdateError.h b/thirdparty/jolt_physics/Jolt/Physics/EPhysicsUpdateError.h
new file mode 100644
index 0000000000..c9edd6deda
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/EPhysicsUpdateError.h
@@ -0,0 +1,37 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Enum used by PhysicsSystem to report error conditions during the PhysicsSystem::Update call. This is a bit field, multiple errors can trigger in the same update.
+enum class EPhysicsUpdateError : uint32
+{
+	None					= 0,			///< No errors
+	ManifoldCacheFull		= 1 << 0,		///< The manifold cache is full, this means that the total number of contacts between bodies is too high. Some contacts were ignored. Increase inMaxContactConstraints in PhysicsSystem::Init.
+	BodyPairCacheFull		= 1 << 1,		///< The body pair cache is full, this means that too many bodies contacted. Some contacts were ignored. Increase inMaxBodyPairs in PhysicsSystem::Init.
+	ContactConstraintsFull	= 1 << 2,		///< The contact constraints buffer is full. Some contacts were ignored. Increase inMaxContactConstraints in PhysicsSystem::Init.
+};
+
+/// OR operator for EPhysicsUpdateError
+inline EPhysicsUpdateError operator | (EPhysicsUpdateError inA, EPhysicsUpdateError inB)
+{
+	return static_cast<EPhysicsUpdateError>(static_cast<uint32>(inA) | static_cast<uint32>(inB));
+}
+
+/// OR operator for EPhysicsUpdateError
+inline EPhysicsUpdateError operator |= (EPhysicsUpdateError &ioA, EPhysicsUpdateError inB)
+{
+	ioA = ioA | inB;
+	return ioA;
+}
+
+/// AND operator for EPhysicsUpdateError
+inline EPhysicsUpdateError operator & (EPhysicsUpdateError inA, EPhysicsUpdateError inB)
+{
+	return static_cast<EPhysicsUpdateError>(static_cast<uint32>(inA) & static_cast<uint32>(inB));
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.cpp b/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.cpp
new file mode 100644
index 0000000000..ed1064df99
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.cpp
@@ -0,0 +1,484 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/IslandBuilder.h>
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Physics/PhysicsSettings.h>
+#include <Jolt/Core/Profiler.h>
+#include <Jolt/Core/Atomics.h>
+#include <Jolt/Core/TempAllocator.h>
+#include <Jolt/Core/QuickSort.h>
+
+JPH_NAMESPACE_BEGIN
+
+IslandBuilder::~IslandBuilder()
+{
+	JPH_ASSERT(mConstraintLinks == nullptr);
+	JPH_ASSERT(mContactLinks == nullptr);
+	JPH_ASSERT(mBodyIslands == nullptr);
+	JPH_ASSERT(mBodyIslandEnds == nullptr);
+	JPH_ASSERT(mConstraintIslands == nullptr);
+	JPH_ASSERT(mConstraintIslandEnds == nullptr);
+	JPH_ASSERT(mContactIslands == nullptr);
+	JPH_ASSERT(mContactIslandEnds == nullptr);
+	JPH_ASSERT(mIslandsSorted == nullptr);
+
+	delete [] mBodyLinks;
+}
+
+void IslandBuilder::Init(uint32 inMaxActiveBodies)
+{
+	mMaxActiveBodies = inMaxActiveBodies;
+
+	// Link each body to itself, BuildBodyIslands() will restore this so that we don't need to do this each step
+	JPH_ASSERT(mBodyLinks == nullptr);
+	mBodyLinks = new BodyLink [mMaxActiveBodies];
+	for (uint32 i = 0; i < mMaxActiveBodies; ++i)
+		mBodyLinks[i].mLinkedTo.store(i, memory_order_relaxed);
+}
+
+void IslandBuilder::PrepareContactConstraints(uint32 inMaxContacts, TempAllocator *inTempAllocator)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Need to call Init first
+	JPH_ASSERT(mBodyLinks != nullptr);
+
+	// Check that the builder has been reset
+	JPH_ASSERT(mNumContacts == 0);
+	JPH_ASSERT(mNumIslands == 0);
+
+	// Create contact link buffer, not initialized so each contact needs to be explicitly set
+	JPH_ASSERT(mContactLinks == nullptr);
+	mContactLinks = (uint32 *)inTempAllocator->Allocate(inMaxContacts * sizeof(uint32));
+	mMaxContacts = inMaxContacts;
+
+#ifdef JPH_VALIDATE_ISLAND_BUILDER
+	// Create validation structures
+	JPH_ASSERT(mLinkValidation == nullptr);
+	mLinkValidation = (LinkValidation *)inTempAllocator->Allocate(inMaxContacts * sizeof(LinkValidation));
+	mNumLinkValidation = 0;
+#endif
+}
+
+void IslandBuilder::PrepareNonContactConstraints(uint32 inNumConstraints, TempAllocator *inTempAllocator)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Need to call Init first
+	JPH_ASSERT(mBodyLinks != nullptr);
+
+	// Check that the builder has been reset
+	JPH_ASSERT(mNumIslands == 0);
+
+	// Store number of constraints
+	mNumConstraints = inNumConstraints;
+
+	// Create constraint link buffer, not initialized so each constraint needs to be explicitly set
+	JPH_ASSERT(mConstraintLinks == nullptr);
+	mConstraintLinks = (uint32 *)inTempAllocator->Allocate(inNumConstraints * sizeof(uint32));
+}
+
+uint32 IslandBuilder::GetLowestBodyIndex(uint32 inActiveBodyIndex) const
+{
+	uint32 index = inActiveBodyIndex;
+	for (;;)
+	{
+		uint32 link_to = mBodyLinks[index].mLinkedTo.load(memory_order_relaxed);
+		if (link_to == index)
+			break;
+		index = link_to;
+	}
+	return index;
+}
+
+void IslandBuilder::LinkBodies(uint32 inFirst, uint32 inSecond)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Both need to be active, we don't want to create an island with static objects
+	if (inFirst >= mMaxActiveBodies || inSecond >= mMaxActiveBodies)
+		return;
+
+#ifdef JPH_VALIDATE_ISLAND_BUILDER
+	// Add link to the validation list
+	if (mNumLinkValidation < uint32(mMaxContacts))
+		mLinkValidation[mNumLinkValidation++] = { inFirst, inSecond };
+	else
+		JPH_ASSERT(false, "Out of links");
+#endif
+
+	// Start the algorithm with the two bodies
+	uint32 first_link_to = inFirst;
+	uint32 second_link_to = inSecond;
+
+	for (;;)
+	{
+		// Follow the chain until we get to the body with lowest index
+		// If the swap compare below fails, we'll keep searching from the lowest index for the new lowest index
+		first_link_to = GetLowestBodyIndex(first_link_to);
+		second_link_to = GetLowestBodyIndex(second_link_to);
+
+		// If the targets are the same, the bodies are already connected
+		if (first_link_to != second_link_to)
+		{
+			// We always link the highest to the lowest
+			if (first_link_to < second_link_to)
+			{
+				// Attempt to link the second to the first
+				// Since we found this body to be at the end of the chain it must point to itself, and if it
+				// doesn't it has been reparented and we need to retry the algorithm
+				if (!mBodyLinks[second_link_to].mLinkedTo.compare_exchange_weak(second_link_to, first_link_to, memory_order_relaxed))
+					continue;
+			}
+			else
+			{
+				// Attempt to link the first to the second
+				// Since we found this body to be at the end of the chain it must point to itself, and if it
+				// doesn't it has been reparented and we need to retry the algorithm
+				if (!mBodyLinks[first_link_to].mLinkedTo.compare_exchange_weak(first_link_to, second_link_to, memory_order_relaxed))
+					continue;
+			}
+		}
+
+		// Linking succeeded!
+		// Chains of bodies can become really long, resulting in an O(N) loop to find the lowest body index
+		// to prevent this we attempt to update the link of the bodies that were passed in to directly point
+		// to the lowest index that we found. If the value became lower than our lowest link, some other
+		// thread must have relinked these bodies in the mean time so we won't update the value.
+		uint32 lowest_link_to = min(first_link_to, second_link_to);
+		AtomicMin(mBodyLinks[inFirst].mLinkedTo, lowest_link_to, memory_order_relaxed);
+		AtomicMin(mBodyLinks[inSecond].mLinkedTo, lowest_link_to, memory_order_relaxed);
+		break;
+	}
+}
+
+void IslandBuilder::LinkConstraint(uint32 inConstraintIndex, uint32 inFirst, uint32 inSecond)
+{
+	LinkBodies(inFirst, inSecond);
+
+	JPH_ASSERT(inConstraintIndex < mNumConstraints);
+	uint32 min_value = min(inFirst, inSecond); // Use fact that invalid index is 0xffffffff, we want the active body of two
+	JPH_ASSERT(min_value != Body::cInactiveIndex); // At least one of the bodies must be active
+	mConstraintLinks[inConstraintIndex] = min_value;
+}
+
+void IslandBuilder::LinkContact(uint32 inContactIndex, uint32 inFirst, uint32 inSecond)
+{
+	JPH_ASSERT(inContactIndex < mMaxContacts);
+	mContactLinks[inContactIndex] = min(inFirst, inSecond); // Use fact that invalid index is 0xffffffff, we want the active body of two
+}
+
+#ifdef JPH_VALIDATE_ISLAND_BUILDER
+
+void IslandBuilder::ValidateIslands(uint32 inNumActiveBodies) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Go through all links so far
+	for (uint32 i = 0; i < mNumLinkValidation; ++i)
+	{
+		// If the bodies in this link ended up in different groups we have a problem
+		if (mBodyLinks[mLinkValidation[i].mFirst].mIslandIndex != mBodyLinks[mLinkValidation[i].mSecond].mIslandIndex)
+		{
+			Trace("Fail: %u, %u", mLinkValidation[i].mFirst, mLinkValidation[i].mSecond);
+			Trace("Num Active: %u", inNumActiveBodies);
+
+			for (uint32 j = 0; j < mNumLinkValidation; ++j)
+				Trace("builder.Link(%u, %u);", mLinkValidation[j].mFirst, mLinkValidation[j].mSecond);
+
+			IslandBuilder tmp;
+			tmp.Init(inNumActiveBodies);
+			for (uint32 j = 0; j < mNumLinkValidation; ++j)
+			{
+				Trace("Link %u -> %u", mLinkValidation[j].mFirst, mLinkValidation[j].mSecond);
+				tmp.LinkBodies(mLinkValidation[j].mFirst, mLinkValidation[j].mSecond);
+				for (uint32 t = 0; t < inNumActiveBodies; ++t)
+					Trace("%u -> %u", t, (uint32)tmp.mBodyLinks[t].mLinkedTo);
+			}
+
+			JPH_ASSERT(false, "IslandBuilder validation failed");
+		}
+	}
+}
+
+#endif
+
+void IslandBuilder::BuildBodyIslands(const BodyID *inActiveBodies, uint32 inNumActiveBodies, TempAllocator *inTempAllocator)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Store the amount of active bodies
+	mNumActiveBodies = inNumActiveBodies;
+
+	// Create output arrays for body ID's, don't call constructors
+	JPH_ASSERT(mBodyIslands == nullptr);
+	mBodyIslands = (BodyID *)inTempAllocator->Allocate(inNumActiveBodies * sizeof(BodyID));
+
+	// Create output array for start index of each island. At this point we don't know how many islands there will be, but we know it cannot be more than inNumActiveBodies.
+	// Note: We allocate 1 extra entry because we always increment the count of the next island.
+	uint32 *body_island_starts = (uint32 *)inTempAllocator->Allocate((inNumActiveBodies + 1) * sizeof(uint32));
+
+	// First island always starts at 0
+	body_island_starts[0] = 0;
+
+	// Calculate island index for all bodies
+	JPH_ASSERT(mNumIslands == 0);
+	for (uint32 i = 0; i < inNumActiveBodies; ++i)
+	{
+		BodyLink &link = mBodyLinks[i];
+		uint32 s = link.mLinkedTo.load(memory_order_relaxed);
+		if (s != i)
+		{
+			// Links to another body, take island index from other body (this must have been filled in already since we're looping from low to high)
+			JPH_ASSERT(s < uint32(i));
+			uint32 island_index = mBodyLinks[s].mIslandIndex;
+			link.mIslandIndex = island_index;
+
+			// Increment the start of the next island
+			body_island_starts[island_index + 1]++;
+		}
+		else
+		{
+			// Does not link to other body, this is the start of a new island
+			link.mIslandIndex = mNumIslands;
+			++mNumIslands;
+
+			// Set the start of the next island to 1
+			body_island_starts[mNumIslands] = 1;
+		}
+	}
+
+#ifdef JPH_VALIDATE_ISLAND_BUILDER
+	ValidateIslands(inNumActiveBodies);
+#endif
+
+	// Make the start array absolute (so far we only counted)
+	for (uint32 island = 1; island < mNumIslands; ++island)
+		body_island_starts[island] += body_island_starts[island - 1];
+
+	// Convert the to a linear list grouped by island
+	for (uint32 i = 0; i < inNumActiveBodies; ++i)
+	{
+		BodyLink &link = mBodyLinks[i];
+
+		// Copy the body to the correct location in the array and increment it
+		uint32 &start = body_island_starts[link.mIslandIndex];
+		mBodyIslands[start] = inActiveBodies[i];
+		start++;
+
+		// Reset linked to field for the next update
+		link.mLinkedTo.store(i, memory_order_relaxed);
+	}
+
+	// We should now have a full array
+	JPH_ASSERT(mNumIslands == 0 || body_island_starts[mNumIslands - 1] == inNumActiveBodies);
+
+	// We've incremented all body indices so that they now point at the end instead of the starts
+	JPH_ASSERT(mBodyIslandEnds == nullptr);
+	mBodyIslandEnds = body_island_starts;
+}
+
+void IslandBuilder::BuildConstraintIslands(const uint32 *inConstraintToBody, uint32 inNumConstraints, uint32 *&outConstraints, uint32 *&outConstraintsEnd, TempAllocator *inTempAllocator) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Check if there's anything to do
+	if (inNumConstraints == 0)
+		return;
+
+	// Create output arrays for constraints
+	// Note: For the end indices we allocate 1 extra entry so we don't have to do an if in the inner loop
+	uint32 *constraints = (uint32 *)inTempAllocator->Allocate(inNumConstraints * sizeof(uint32));
+	uint32 *constraint_ends = (uint32 *)inTempAllocator->Allocate((mNumIslands + 1) * sizeof(uint32));
+
+	// Reset sizes
+	for (uint32 island = 0; island < mNumIslands; ++island)
+		constraint_ends[island] = 0;
+
+	// Loop over array and increment start relative position for the next island
+	for (uint32 constraint = 0; constraint < inNumConstraints; ++constraint)
+	{
+		uint32 body_idx = inConstraintToBody[constraint];
+		uint32 next_island_idx = mBodyLinks[body_idx].mIslandIndex + 1;
+		JPH_ASSERT(next_island_idx <= mNumIslands);
+		constraint_ends[next_island_idx]++;
+	}
+
+	// Make start positions absolute
+	for (uint32 island = 1; island < mNumIslands; ++island)
+		constraint_ends[island] += constraint_ends[island - 1];
+
+	// Loop over array and collect constraints
+	for (uint32 constraint = 0; constraint < inNumConstraints; ++constraint)
+	{
+		uint32 body_idx = inConstraintToBody[constraint];
+		uint32 island_idx = mBodyLinks[body_idx].mIslandIndex;
+		constraints[constraint_ends[island_idx]++] = constraint;
+	}
+
+	JPH_ASSERT(outConstraints == nullptr);
+	outConstraints = constraints;
+	JPH_ASSERT(outConstraintsEnd == nullptr);
+	outConstraintsEnd = constraint_ends;
+}
+
+void IslandBuilder::SortIslands(TempAllocator *inTempAllocator)
+{
+	JPH_PROFILE_FUNCTION();
+
+	if (mNumContacts > 0 || mNumConstraints > 0)
+	{
+		// Allocate mapping table
+		JPH_ASSERT(mIslandsSorted == nullptr);
+		mIslandsSorted = (uint32 *)inTempAllocator->Allocate(mNumIslands * sizeof(uint32));
+
+		// Initialize index
+		for (uint32 island = 0; island < mNumIslands; ++island)
+			mIslandsSorted[island] = island;
+
+		// Determine the sum of contact constraints / constraints per island
+		uint32 *num_constraints = (uint32 *)inTempAllocator->Allocate(mNumIslands * sizeof(uint32));
+		if (mNumContacts > 0 && mNumConstraints > 0)
+		{
+			num_constraints[0] = mConstraintIslandEnds[0] + mContactIslandEnds[0];
+			for (uint32 island = 1; island < mNumIslands; ++island)
+				num_constraints[island] = mConstraintIslandEnds[island] - mConstraintIslandEnds[island - 1]
+										+ mContactIslandEnds[island] - mContactIslandEnds[island - 1];
+		}
+		else if (mNumContacts > 0)
+		{
+			num_constraints[0] = mContactIslandEnds[0];
+			for (uint32 island = 1; island < mNumIslands; ++island)
+				num_constraints[island] = mContactIslandEnds[island] - mContactIslandEnds[island - 1];
+		}
+		else
+		{
+			num_constraints[0] = mConstraintIslandEnds[0];
+			for (uint32 island = 1; island < mNumIslands; ++island)
+				num_constraints[island] = mConstraintIslandEnds[island] - mConstraintIslandEnds[island - 1];
+		}
+
+		// Sort so the biggest islands go first, this means that the jobs that take longest will be running
+		// first which improves the chance that all jobs finish at the same time.
+		QuickSort(mIslandsSorted, mIslandsSorted + mNumIslands, [num_constraints](uint32 inLHS, uint32 inRHS) {
+			return num_constraints[inLHS] > num_constraints[inRHS];
+		});
+
+		inTempAllocator->Free(num_constraints, mNumIslands * sizeof(uint32));
+	}
+}
+
+void IslandBuilder::Finalize(const BodyID *inActiveBodies, uint32 inNumActiveBodies, uint32 inNumContacts, TempAllocator *inTempAllocator)
+{
+	JPH_PROFILE_FUNCTION();
+
+	mNumContacts = inNumContacts;
+
+	BuildBodyIslands(inActiveBodies, inNumActiveBodies, inTempAllocator);
+	BuildConstraintIslands(mConstraintLinks, mNumConstraints, mConstraintIslands, mConstraintIslandEnds, inTempAllocator);
+	BuildConstraintIslands(mContactLinks, mNumContacts, mContactIslands, mContactIslandEnds, inTempAllocator);
+	SortIslands(inTempAllocator);
+
+	mNumPositionSteps = (uint8 *)inTempAllocator->Allocate(mNumIslands * sizeof(uint8));
+}
+
+void IslandBuilder::GetBodiesInIsland(uint32 inIslandIndex, BodyID *&outBodiesBegin, BodyID *&outBodiesEnd) const
+{
+	JPH_ASSERT(inIslandIndex < mNumIslands);
+	uint32 sorted_index = mIslandsSorted != nullptr? mIslandsSorted[inIslandIndex] : inIslandIndex;
+	outBodiesBegin = sorted_index > 0? mBodyIslands + mBodyIslandEnds[sorted_index - 1] : mBodyIslands;
+	outBodiesEnd = mBodyIslands + mBodyIslandEnds[sorted_index];
+}
+
+bool IslandBuilder::GetConstraintsInIsland(uint32 inIslandIndex, uint32 *&outConstraintsBegin, uint32 *&outConstraintsEnd) const
+{
+	JPH_ASSERT(inIslandIndex < mNumIslands);
+	if (mNumConstraints == 0)
+	{
+		outConstraintsBegin = nullptr;
+		outConstraintsEnd = nullptr;
+		return false;
+	}
+	else
+	{
+		uint32 sorted_index = mIslandsSorted[inIslandIndex];
+		outConstraintsBegin = sorted_index > 0? mConstraintIslands + mConstraintIslandEnds[sorted_index - 1] : mConstraintIslands;
+		outConstraintsEnd = mConstraintIslands + mConstraintIslandEnds[sorted_index];
+		return outConstraintsBegin != outConstraintsEnd;
+	}
+}
+
+bool IslandBuilder::GetContactsInIsland(uint32 inIslandIndex, uint32 *&outContactsBegin, uint32 *&outContactsEnd) const
+{
+	JPH_ASSERT(inIslandIndex < mNumIslands);
+	if (mNumContacts == 0)
+	{
+		outContactsBegin = nullptr;
+		outContactsEnd = nullptr;
+		return false;
+	}
+	else
+	{
+		uint32 sorted_index = mIslandsSorted[inIslandIndex];
+		outContactsBegin = sorted_index > 0? mContactIslands + mContactIslandEnds[sorted_index - 1] : mContactIslands;
+		outContactsEnd = mContactIslands + mContactIslandEnds[sorted_index];
+		return outContactsBegin != outContactsEnd;
+	}
+}
+
+void IslandBuilder::ResetIslands(TempAllocator *inTempAllocator)
+{
+	JPH_PROFILE_FUNCTION();
+
+	inTempAllocator->Free(mNumPositionSteps, mNumIslands * sizeof(uint8));
+
+	if (mIslandsSorted != nullptr)
+	{
+		inTempAllocator->Free(mIslandsSorted, mNumIslands * sizeof(uint32));
+		mIslandsSorted = nullptr;
+	}
+
+	if (mContactIslands != nullptr)
+	{
+		inTempAllocator->Free(mContactIslandEnds, (mNumIslands + 1) * sizeof(uint32));
+		mContactIslandEnds = nullptr;
+		inTempAllocator->Free(mContactIslands, mNumContacts * sizeof(uint32));
+		mContactIslands = nullptr;
+	}
+
+	if (mConstraintIslands != nullptr)
+	{
+		inTempAllocator->Free(mConstraintIslandEnds, (mNumIslands + 1) * sizeof(uint32));
+		mConstraintIslandEnds = nullptr;
+		inTempAllocator->Free(mConstraintIslands, mNumConstraints * sizeof(uint32));
+		mConstraintIslands = nullptr;
+	}
+
+	inTempAllocator->Free(mBodyIslandEnds, (mNumActiveBodies + 1) * sizeof(uint32));
+	mBodyIslandEnds = nullptr;
+	inTempAllocator->Free(mBodyIslands, mNumActiveBodies * sizeof(uint32));
+	mBodyIslands = nullptr;
+
+	inTempAllocator->Free(mConstraintLinks, mNumConstraints * sizeof(uint32));
+	mConstraintLinks = nullptr;
+
+#ifdef JPH_VALIDATE_ISLAND_BUILDER
+	inTempAllocator->Free(mLinkValidation, mMaxContacts * sizeof(LinkValidation));
+	mLinkValidation = nullptr;
+#endif
+
+	inTempAllocator->Free(mContactLinks, mMaxContacts * sizeof(uint32));
+	mContactLinks = nullptr;
+
+	mNumActiveBodies = 0;
+	mNumConstraints = 0;
+	mMaxContacts = 0;
+	mNumContacts = 0;
+	mNumIslands = 0;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.h b/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.h
new file mode 100644
index 0000000000..4c2f097d60
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/IslandBuilder.h
@@ -0,0 +1,125 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/BodyID.h>
+#include <Jolt/Core/NonCopyable.h>
+#include <Jolt/Core/Atomics.h>
+
+JPH_NAMESPACE_BEGIN
+
+class TempAllocator;
+
+//#define JPH_VALIDATE_ISLAND_BUILDER
+
+/// Keeps track of connected bodies and builds islands for multithreaded velocity/position update
+class IslandBuilder : public NonCopyable
+{
+public:
+	/// Destructor
+							~IslandBuilder();
+
+	/// Initialize the island builder with the maximum amount of bodies that could be active
+	void					Init(uint32 inMaxActiveBodies);
+
+	/// Prepare for simulation step by allocating space for the contact constraints
+	void					PrepareContactConstraints(uint32 inMaxContactConstraints, TempAllocator *inTempAllocator);
+
+	/// Prepare for simulation step by allocating space for the non-contact constraints
+	void					PrepareNonContactConstraints(uint32 inNumConstraints, TempAllocator *inTempAllocator);
+
+	/// Link two bodies by their index in the BodyManager::mActiveBodies list to form islands
+	void					LinkBodies(uint32 inFirst, uint32 inSecond);
+
+	/// Link a constraint to a body by their index in the BodyManager::mActiveBodies
+	void					LinkConstraint(uint32 inConstraintIndex, uint32 inFirst, uint32 inSecond);
+
+	/// Link a contact to a body by their index in the BodyManager::mActiveBodies
+	void					LinkContact(uint32 inContactIndex, uint32 inFirst, uint32 inSecond);
+
+	/// Finalize the islands after all bodies have been Link()-ed
+	void					Finalize(const BodyID *inActiveBodies, uint32 inNumActiveBodies, uint32 inNumContacts, TempAllocator *inTempAllocator);
+
+	/// Get the amount of islands formed
+	uint32					GetNumIslands() const							{ return mNumIslands; }
+
+	/// Get iterator for a particular island, return false if there are no constraints
+	void					GetBodiesInIsland(uint32 inIslandIndex, BodyID *&outBodiesBegin, BodyID *&outBodiesEnd) const;
+	bool					GetConstraintsInIsland(uint32 inIslandIndex, uint32 *&outConstraintsBegin, uint32 *&outConstraintsEnd) const;
+	bool					GetContactsInIsland(uint32 inIslandIndex, uint32 *&outContactsBegin, uint32 *&outContactsEnd) const;
+
+	/// The number of position iterations for each island
+	void					SetNumPositionSteps(uint32 inIslandIndex, uint inNumPositionSteps)	{ JPH_ASSERT(inIslandIndex < mNumIslands); JPH_ASSERT(inNumPositionSteps < 256); mNumPositionSteps[inIslandIndex] = uint8(inNumPositionSteps); }
+	uint					GetNumPositionSteps(uint32 inIslandIndex) const						{ JPH_ASSERT(inIslandIndex < mNumIslands); return mNumPositionSteps[inIslandIndex]; }
+
+	/// After you're done calling the three functions above, call this function to free associated data
+	void					ResetIslands(TempAllocator *inTempAllocator);
+
+private:
+	/// Returns the index of the lowest body in the group
+	uint32					GetLowestBodyIndex(uint32 inActiveBodyIndex) const;
+
+#ifdef JPH_VALIDATE_ISLAND_BUILDER
+	/// Helper function to validate all islands so far generated
+	void					ValidateIslands(uint32 inNumActiveBodies) const;
+#endif
+
+	// Helper functions to build various islands
+	void					BuildBodyIslands(const BodyID *inActiveBodies, uint32 inNumActiveBodies, TempAllocator *inTempAllocator);
+	void					BuildConstraintIslands(const uint32 *inConstraintToBody, uint32 inNumConstraints, uint32 *&outConstraints, uint32 *&outConstraintsEnd, TempAllocator *inTempAllocator) const;
+
+	/// Sorts the islands so that the islands with most constraints go first
+	void					SortIslands(TempAllocator *inTempAllocator);
+
+	/// Intermediate data structure that for each body keeps track what the lowest index of the body is that it is connected to
+	struct BodyLink
+	{
+		JPH_OVERRIDE_NEW_DELETE
+
+		atomic<uint32>		mLinkedTo;										///< An index in mBodyLinks pointing to another body in this island with a lower index than this body
+		uint32				mIslandIndex;									///< The island index of this body (filled in during Finalize)
+	};
+
+	// Intermediate data
+	BodyLink *				mBodyLinks = nullptr;							///< Maps bodies to the first body in the island
+	uint32 *				mConstraintLinks = nullptr;						///< Maps constraint index to body index (which maps to island index)
+	uint32 *				mContactLinks = nullptr;						///< Maps contact constraint index to body index (which maps to island index)
+
+	// Final data
+	BodyID *				mBodyIslands = nullptr;							///< Bodies ordered by island
+	uint32 *				mBodyIslandEnds = nullptr;						///< End index of each body island
+
+	uint32 *				mConstraintIslands = nullptr;					///< Constraints ordered by island
+	uint32 *				mConstraintIslandEnds = nullptr;				///< End index of each constraint island
+
+	uint32 *				mContactIslands = nullptr;						///< Contacts ordered by island
+	uint32 *				mContactIslandEnds = nullptr;					///< End index of each contact island
+
+	uint32 *				mIslandsSorted = nullptr;						///< A list of island indices in order of most constraints first
+
+	uint8 *					mNumPositionSteps = nullptr;					///< Number of position steps for each island
+
+	// Counters
+	uint32					mMaxActiveBodies;								///< Maximum size of the active bodies list (see BodyManager::mActiveBodies)
+	uint32					mNumActiveBodies = 0;							///< Number of active bodies passed to
+	uint32					mNumConstraints = 0;							///< Size of the constraint list (see ConstraintManager::mConstraints)
+	uint32					mMaxContacts = 0;								///< Maximum amount of contacts supported
+	uint32					mNumContacts = 0;								///< Size of the contacts list (see ContactConstraintManager::mNumConstraints)
+	uint32					mNumIslands = 0;								///< Final number of islands
+
+#ifdef JPH_VALIDATE_ISLAND_BUILDER
+	/// Structure to keep track of all added links to validate that islands were generated correctly
+	struct LinkValidation
+	{
+		uint32				mFirst;
+		uint32				mSecond;
+	};
+
+	LinkValidation *		mLinkValidation = nullptr;
+	atomic<uint32>			mNumLinkValidation;
+#endif
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/LargeIslandSplitter.cpp b/thirdparty/jolt_physics/Jolt/Physics/LargeIslandSplitter.cpp
new file mode 100644
index 0000000000..50ab1c583f
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/LargeIslandSplitter.cpp
@@ -0,0 +1,582 @@
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/LargeIslandSplitter.h>
+#include <Jolt/Physics/IslandBuilder.h>
+#include <Jolt/Physics/Constraints/CalculateSolverSteps.h>
+#include <Jolt/Physics/Constraints/Constraint.h>
+#include <Jolt/Physics/Constraints/ContactConstraintManager.h>
+#include <Jolt/Physics/Body/BodyManager.h>
+#include <Jolt/Core/Profiler.h>
+#include <Jolt/Core/TempAllocator.h>
+
+//#define JPH_LARGE_ISLAND_SPLITTER_DEBUG
+
+JPH_NAMESPACE_BEGIN
+
+LargeIslandSplitter::EStatus LargeIslandSplitter::Splits::FetchNextBatch(uint32 &outConstraintsBegin, uint32 &outConstraintsEnd, uint32 &outContactsBegin, uint32 &outContactsEnd, bool &outFirstIteration)
+{
+	{
+		// First check if we can get a new batch (doing a read to avoid hammering an atomic with an atomic subtract)
+		// Note this also avoids overflowing the status counter if we're done but there's still one thread processing items
+		uint64 status = mStatus.load(memory_order_acquire);
+
+		// Check for special value that indicates that the splits are still being built
+		// (note we do not check for this condition again below as we reset all splits before kicking off jobs that fetch batches of work)
+		if (status == StatusItemMask)
+			return EStatus::WaitingForBatch;
+
+		// Next check if all items have been processed. Note that we do this after checking if the job can be started
+		// as mNumIterations is not initialized until the split is started.
+		if (sGetIteration(status) >= mNumIterations)
+			return EStatus::AllBatchesDone;
+
+		uint item = sGetItem(status);
+		uint split_index = sGetSplit(status);
+		if (split_index == cNonParallelSplitIdx)
+		{
+			// Non parallel split needs to be taken as a single batch, only the thread that takes element 0 will do it
+			if (item != 0)
+				return EStatus::WaitingForBatch;
+		}
+		else
+		{
+			// Parallel split is split into batches
+			JPH_ASSERT(split_index < mNumSplits);
+			const Split &split = mSplits[split_index];
+			if (item >= split.GetNumItems())
+				return EStatus::WaitingForBatch;
+		}
+	}
+
+	// Then try to actually get the batch
+	uint64 status = mStatus.fetch_add(cBatchSize, memory_order_acquire);
+	int iteration = sGetIteration(status);
+	if (iteration >= mNumIterations)
+		return EStatus::AllBatchesDone;
+
+	uint split_index = sGetSplit(status);
+	JPH_ASSERT(split_index < mNumSplits || split_index == cNonParallelSplitIdx);
+	const Split &split = mSplits[split_index];
+	uint item_begin = sGetItem(status);
+	if (split_index == cNonParallelSplitIdx)
+	{
+		if (item_begin == 0)
+		{
+			// Non-parallel split always goes as a single batch
+			outConstraintsBegin = split.mConstraintBufferBegin;
+			outConstraintsEnd = split.mConstraintBufferEnd;
+			outContactsBegin = split.mContactBufferBegin;
+			outContactsEnd = split.mContactBufferEnd;
+			outFirstIteration = iteration == 0;
+			return EStatus::BatchRetrieved;
+		}
+		else
+		{
+			// Otherwise we're done with this split
+			return EStatus::WaitingForBatch;
+		}
+	}
+
+	// Parallel split is split into batches
+	uint num_constraints = split.GetNumConstraints();
+	uint num_contacts = split.GetNumContacts();
+	uint num_items = num_constraints + num_contacts;
+	if (item_begin >= num_items)
+		return EStatus::WaitingForBatch;
+
+	uint item_end = min(item_begin + cBatchSize, num_items);
+	if (item_end >= num_constraints)
+	{
+		if (item_begin < num_constraints)
+		{
+			// Partially from constraints and partially from contacts
+			outConstraintsBegin = split.mConstraintBufferBegin + item_begin;
+			outConstraintsEnd = split.mConstraintBufferEnd;
+		}
+		else
+		{
+			// Only contacts
+			outConstraintsBegin = 0;
+			outConstraintsEnd = 0;
+		}
+
+		outContactsBegin = split.mContactBufferBegin + (max(item_begin, num_constraints) - num_constraints);
+		outContactsEnd = split.mContactBufferBegin + (item_end - num_constraints);
+	}
+	else
+	{
+		// Only constraints
+		outConstraintsBegin = split.mConstraintBufferBegin + item_begin;
+		outConstraintsEnd = split.mConstraintBufferBegin + item_end;
+
+		outContactsBegin = 0;
+		outContactsEnd = 0;
+	}
+
+	outFirstIteration = iteration == 0;
+	return EStatus::BatchRetrieved;
+}
+
+void LargeIslandSplitter::Splits::MarkBatchProcessed(uint inNumProcessed, bool &outLastIteration, bool &outFinalBatch)
+{
+	// We fetched this batch, nobody should change the split and or iteration until we mark the last batch as processed so we can safely get the current status
+	uint64 status = mStatus.load(memory_order_relaxed);
+	uint split_index = sGetSplit(status);
+	JPH_ASSERT(split_index < mNumSplits || split_index == cNonParallelSplitIdx);
+	const Split &split = mSplits[split_index];
+	uint num_items_in_split = split.GetNumItems();
+
+	// Determine if this is the last iteration before possibly incrementing it
+	int iteration = sGetIteration(status);
+	outLastIteration = iteration == mNumIterations - 1;
+
+	// Add the number of items we processed to the total number of items processed
+	// Note: This needs to happen after we read the status as other threads may update the status after we mark items as processed
+	JPH_ASSERT(inNumProcessed > 0); // Logic will break if we mark a block of 0 items as processed
+	uint total_items_processed = mItemsProcessed.fetch_add(inNumProcessed, memory_order_acq_rel) + inNumProcessed;
+
+	// Check if we're at the end of the split
+	if (total_items_processed >= num_items_in_split)
+	{
+		JPH_ASSERT(total_items_processed == num_items_in_split); // Should not overflow, that means we're retiring more items than we should process
+
+		// Set items processed back to 0 for the next split/iteration
+		mItemsProcessed.store(0, memory_order_release);
+
+		// Determine next split
+		do
+		{
+			if (split_index == cNonParallelSplitIdx)
+			{
+				// At start of next iteration
+				split_index = 0;
+				++iteration;
+			}
+			else
+			{
+				// At start of next split
+				++split_index;
+			}
+
+			// If we're beyond the end of splits, go to the non-parallel split
+			if (split_index >= mNumSplits)
+				split_index = cNonParallelSplitIdx;
+		}
+		while (iteration < mNumIterations
+			&& mSplits[split_index].GetNumItems() == 0); // We don't support processing empty splits, skip to the next split in this case
+
+		mStatus.store((uint64(iteration) << StatusIterationShift) | (uint64(split_index) << StatusSplitShift), memory_order_release);
+	}
+
+	// Track if this is the final batch
+	outFinalBatch = iteration >= mNumIterations;
+}
+
+LargeIslandSplitter::~LargeIslandSplitter()
+{
+	JPH_ASSERT(mSplitMasks == nullptr);
+	JPH_ASSERT(mContactAndConstraintsSplitIdx == nullptr);
+	JPH_ASSERT(mContactAndConstraintIndices == nullptr);
+	JPH_ASSERT(mSplitIslands == nullptr);
+}
+
+void LargeIslandSplitter::Prepare(const IslandBuilder &inIslandBuilder, uint32 inNumActiveBodies, TempAllocator *inTempAllocator)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Count the total number of constraints and contacts that we will be putting in splits
+	mContactAndConstraintsSize = 0;
+	for (uint32 island = 0; island < inIslandBuilder.GetNumIslands(); ++island)
+	{
+		// Get the contacts in this island
+		uint32 *contacts_start, *contacts_end;
+		inIslandBuilder.GetContactsInIsland(island, contacts_start, contacts_end);
+		uint num_contacts_in_island = uint(contacts_end - contacts_start);
+
+		// Get the constraints in this island
+		uint32 *constraints_start, *constraints_end;
+		inIslandBuilder.GetConstraintsInIsland(island, constraints_start, constraints_end);
+		uint num_constraints_in_island = uint(constraints_end - constraints_start);
+
+		uint island_size = num_contacts_in_island + num_constraints_in_island;
+		if (island_size >= cLargeIslandTreshold)
+		{
+			mNumSplitIslands++;
+			mContactAndConstraintsSize += island_size;
+		}
+		else
+			break; // If this island doesn't have enough constraints, the next islands won't either since they're sorted from big to small
+	}
+
+	if (mContactAndConstraintsSize > 0)
+	{
+		mNumActiveBodies = inNumActiveBodies;
+
+		// Allocate split mask buffer
+		mSplitMasks = (SplitMask *)inTempAllocator->Allocate(mNumActiveBodies * sizeof(SplitMask));
+
+		// Allocate contact and constraint buffer
+		uint contact_and_constraint_indices_size = mContactAndConstraintsSize * sizeof(uint32);
+		mContactAndConstraintsSplitIdx = (uint32 *)inTempAllocator->Allocate(contact_and_constraint_indices_size);
+		mContactAndConstraintIndices = (uint32 *)inTempAllocator->Allocate(contact_and_constraint_indices_size);
+
+		// Allocate island split buffer
+		mSplitIslands = (Splits *)inTempAllocator->Allocate(mNumSplitIslands * sizeof(Splits));
+
+		// Prevent any of the splits from being picked up as work
+		for (uint i = 0; i < mNumSplitIslands; ++i)
+			mSplitIslands[i].ResetStatus();
+	}
+}
+
+uint LargeIslandSplitter::AssignSplit(const Body *inBody1, const Body *inBody2)
+{
+	uint32 idx1 = inBody1->GetIndexInActiveBodiesInternal();
+	uint32 idx2 = inBody2->GetIndexInActiveBodiesInternal();
+
+	// Test if either index is negative
+	if (idx1 == Body::cInactiveIndex || !inBody1->IsDynamic())
+	{
+		// Body 1 is not active or a kinematic body, so we only need to set 1 body
+		JPH_ASSERT(idx2 < mNumActiveBodies);
+		SplitMask &mask = mSplitMasks[idx2];
+		uint split = min(CountTrailingZeros(~uint32(mask)), cNonParallelSplitIdx);
+		mask |= SplitMask(1U << split);
+		return split;
+	}
+	else if (idx2 == Body::cInactiveIndex || !inBody2->IsDynamic())
+	{
+		// Body 2 is not active or a kinematic body, so we only need to set 1 body
+		JPH_ASSERT(idx1 < mNumActiveBodies);
+		SplitMask &mask = mSplitMasks[idx1];
+		uint split = min(CountTrailingZeros(~uint32(mask)), cNonParallelSplitIdx);
+		mask |= SplitMask(1U << split);
+		return split;
+	}
+	else
+	{
+		// If both bodies are active, we need to set 2 bodies
+		JPH_ASSERT(idx1 < mNumActiveBodies);
+		JPH_ASSERT(idx2 < mNumActiveBodies);
+		SplitMask &mask1 = mSplitMasks[idx1];
+		SplitMask &mask2 = mSplitMasks[idx2];
+		uint split = min(CountTrailingZeros((~uint32(mask1)) & (~uint32(mask2))), cNonParallelSplitIdx);
+		SplitMask mask = SplitMask(1U << split);
+		mask1 |= mask;
+		mask2 |= mask;
+		return split;
+	}
+}
+
+uint LargeIslandSplitter::AssignToNonParallelSplit(const Body *inBody)
+{
+	uint32 idx = inBody->GetIndexInActiveBodiesInternal();
+	if (idx != Body::cInactiveIndex)
+	{
+		JPH_ASSERT(idx < mNumActiveBodies);
+		mSplitMasks[idx] |= 1U << cNonParallelSplitIdx;
+	}
+
+	return cNonParallelSplitIdx;
+}
+
+bool LargeIslandSplitter::SplitIsland(uint32 inIslandIndex, const IslandBuilder &inIslandBuilder, const BodyManager &inBodyManager, const ContactConstraintManager &inContactManager, Constraint **inActiveConstraints, CalculateSolverSteps &ioStepsCalculator)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Get the contacts in this island
+	uint32 *contacts_start, *contacts_end;
+	inIslandBuilder.GetContactsInIsland(inIslandIndex, contacts_start, contacts_end);
+	uint num_contacts_in_island = uint(contacts_end - contacts_start);
+
+	// Get the constraints in this island
+	uint32 *constraints_start, *constraints_end;
+	inIslandBuilder.GetConstraintsInIsland(inIslandIndex, constraints_start, constraints_end);
+	uint num_constraints_in_island = uint(constraints_end - constraints_start);
+
+	// Check if it exceeds the threshold
+	uint island_size = num_contacts_in_island + num_constraints_in_island;
+	if (island_size < cLargeIslandTreshold)
+		return false;
+
+	// Get bodies in this island
+	BodyID *bodies_start, *bodies_end;
+	inIslandBuilder.GetBodiesInIsland(inIslandIndex, bodies_start, bodies_end);
+
+	// Reset the split mask for all bodies in this island
+	Body const * const *bodies = inBodyManager.GetBodies().data();
+	for (const BodyID *b = bodies_start; b < bodies_end; ++b)
+		mSplitMasks[bodies[b->GetIndex()]->GetIndexInActiveBodiesInternal()] = 0;
+
+	// Count the number of contacts and constraints per split
+	uint num_contacts_in_split[cNumSplits] = { };
+	uint num_constraints_in_split[cNumSplits] = { };
+
+	// Get space to store split indices
+	uint offset = mContactAndConstraintsNextFree.fetch_add(island_size, memory_order_relaxed);
+	uint32 *contact_split_idx = mContactAndConstraintsSplitIdx + offset;
+	uint32 *constraint_split_idx = contact_split_idx + num_contacts_in_island;
+
+	// Assign the contacts to a split
+	uint32 *cur_contact_split_idx = contact_split_idx;
+	for (const uint32 *c = contacts_start; c < contacts_end; ++c)
+	{
+		const Body *body1, *body2;
+		inContactManager.GetAffectedBodies(*c, body1, body2);
+		uint split = AssignSplit(body1, body2);
+		num_contacts_in_split[split]++;
+		*cur_contact_split_idx++ = split;
+
+		if (body1->IsDynamic())
+			ioStepsCalculator(body1->GetMotionPropertiesUnchecked());
+		if (body2->IsDynamic())
+			ioStepsCalculator(body2->GetMotionPropertiesUnchecked());
+	}
+
+	// Assign the constraints to a split
+	uint32 *cur_constraint_split_idx = constraint_split_idx;
+	for (const uint32 *c = constraints_start; c < constraints_end; ++c)
+	{
+		const Constraint *constraint = inActiveConstraints[*c];
+		uint split = constraint->BuildIslandSplits(*this);
+		num_constraints_in_split[split]++;
+		*cur_constraint_split_idx++ = split;
+
+		ioStepsCalculator(constraint);
+	}
+
+	ioStepsCalculator.Finalize();
+
+	// Start with 0 splits
+	uint split_remap_table[cNumSplits];
+	uint new_split_idx = mNextSplitIsland.fetch_add(1, memory_order_relaxed);
+	JPH_ASSERT(new_split_idx < mNumSplitIslands);
+	Splits &splits = mSplitIslands[new_split_idx];
+	splits.mIslandIndex = inIslandIndex;
+	splits.mNumSplits = 0;
+	splits.mNumIterations = ioStepsCalculator.GetNumVelocitySteps() + 1; // Iteration 0 is used for warm starting
+	splits.mNumVelocitySteps = ioStepsCalculator.GetNumVelocitySteps();
+	splits.mNumPositionSteps = ioStepsCalculator.GetNumPositionSteps();
+	splits.mItemsProcessed.store(0, memory_order_release);
+
+	// Allocate space to store the sorted constraint and contact indices per split
+	uint32 *constraint_buffer_cur[cNumSplits], *contact_buffer_cur[cNumSplits];
+	for (uint s = 0; s < cNumSplits; ++s)
+	{
+		// If this split doesn't contain enough constraints and contacts, we will combine it with the non parallel split
+		if (num_constraints_in_split[s] + num_contacts_in_split[s] < cSplitCombineTreshold
+			&& s < cNonParallelSplitIdx) // The non-parallel split cannot merge into itself
+		{
+			// Remap it
+			split_remap_table[s] = cNonParallelSplitIdx;
+
+			// Add the counts to the non parallel split
+			num_contacts_in_split[cNonParallelSplitIdx] += num_contacts_in_split[s];
+			num_constraints_in_split[cNonParallelSplitIdx] += num_constraints_in_split[s];
+		}
+		else
+		{
+			// This split is valid, map it to the next empty slot
+			uint target_split;
+			if (s < cNonParallelSplitIdx)
+				target_split = splits.mNumSplits++;
+			else
+				target_split = cNonParallelSplitIdx;
+			Split &split = splits.mSplits[target_split];
+			split_remap_table[s] = target_split;
+
+			// Allocate space for contacts
+			split.mContactBufferBegin = offset;
+			split.mContactBufferEnd = split.mContactBufferBegin + num_contacts_in_split[s];
+
+			// Allocate space for constraints
+			split.mConstraintBufferBegin = split.mContactBufferEnd;
+			split.mConstraintBufferEnd = split.mConstraintBufferBegin + num_constraints_in_split[s];
+
+			// Store start for each split
+			contact_buffer_cur[target_split] = mContactAndConstraintIndices + split.mContactBufferBegin;
+			constraint_buffer_cur[target_split] = mContactAndConstraintIndices + split.mConstraintBufferBegin;
+
+			// Update offset
+			offset = split.mConstraintBufferEnd;
+		}
+	}
+
+	// Split the contacts
+	for (uint c = 0; c < num_contacts_in_island; ++c)
+	{
+		uint split = split_remap_table[contact_split_idx[c]];
+		*contact_buffer_cur[split]++ = contacts_start[c];
+	}
+
+	// Split the constraints
+	for (uint c = 0; c < num_constraints_in_island; ++c)
+	{
+		uint split = split_remap_table[constraint_split_idx[c]];
+		*constraint_buffer_cur[split]++ = constraints_start[c];
+	}
+
+#ifdef JPH_LARGE_ISLAND_SPLITTER_DEBUG
+	// Trace the size of all splits
+	uint sum = 0;
+	String stats;
+	for (uint s = 0; s < cNumSplits; ++s)
+	{
+		// If we've processed all splits, jump to the non-parallel split
+		if (s >= splits.GetNumSplits())
+			s = cNonParallelSplitIdx;
+
+		const Split &split = splits.mSplits[s];
+		stats += StringFormat("g:%d:%d:%d, ", s, split.GetNumContacts(), split.GetNumConstraints());
+		sum += split.GetNumItems();
+	}
+	stats += StringFormat("sum: %d", sum);
+	Trace(stats.c_str());
+#endif // JPH_LARGE_ISLAND_SPLITTER_DEBUG
+
+#ifdef JPH_ENABLE_ASSERTS
+	for (uint s = 0; s < cNumSplits; ++s)
+	{
+		// If there are no more splits, process the non-parallel split
+		if (s >= splits.mNumSplits)
+			s = cNonParallelSplitIdx;
+
+		// Check that we wrote all elements
+		Split &split = splits.mSplits[s];
+		JPH_ASSERT(contact_buffer_cur[s] == mContactAndConstraintIndices + split.mContactBufferEnd);
+		JPH_ASSERT(constraint_buffer_cur[s] == mContactAndConstraintIndices + split.mConstraintBufferEnd);
+	}
+
+#ifdef JPH_DEBUG
+	// Validate that the splits are indeed not touching the same body
+	for (uint s = 0; s < splits.mNumSplits; ++s)
+	{
+		Array<bool> body_used(mNumActiveBodies, false);
+
+		// Validate contacts
+		uint32 split_contacts_begin, split_contacts_end;
+		splits.GetContactsInSplit(s, split_contacts_begin, split_contacts_end);
+		for (uint32 *c = mContactAndConstraintIndices + split_contacts_begin; c < mContactAndConstraintIndices + split_contacts_end; ++c)
+		{
+			const Body *body1, *body2;
+			inContactManager.GetAffectedBodies(*c, body1, body2);
+
+			uint32 idx1 = body1->GetIndexInActiveBodiesInternal();
+			if (idx1 != Body::cInactiveIndex && body1->IsDynamic())
+			{
+				JPH_ASSERT(!body_used[idx1]);
+				body_used[idx1] = true;
+			}
+
+			uint32 idx2 = body2->GetIndexInActiveBodiesInternal();
+			if (idx2 != Body::cInactiveIndex && body2->IsDynamic())
+			{
+				JPH_ASSERT(!body_used[idx2]);
+				body_used[idx2] = true;
+			}
+		}
+	}
+#endif // JPH_DEBUG
+#endif // JPH_ENABLE_ASSERTS
+
+	// Allow other threads to pick up this split island now
+	splits.StartFirstBatch();
+	return true;
+}
+
+LargeIslandSplitter::EStatus LargeIslandSplitter::FetchNextBatch(uint &outSplitIslandIndex, uint32 *&outConstraintsBegin, uint32 *&outConstraintsEnd, uint32 *&outContactsBegin, uint32 *&outContactsEnd, bool &outFirstIteration)
+{
+	// We can't be done when all islands haven't been submitted yet
+	uint num_splits_created = mNextSplitIsland.load(memory_order_acquire);
+	bool all_done = num_splits_created == mNumSplitIslands;
+
+	// Loop over all split islands to find work
+	uint32 constraints_begin, constraints_end, contacts_begin, contacts_end;
+	for (Splits *s = mSplitIslands; s < mSplitIslands + num_splits_created; ++s)
+		switch (s->FetchNextBatch(constraints_begin, constraints_end, contacts_begin, contacts_end, outFirstIteration))
+		{
+		case EStatus::AllBatchesDone:
+			break;
+
+		case EStatus::WaitingForBatch:
+			all_done = false;
+			break;
+
+		case EStatus::BatchRetrieved:
+			outSplitIslandIndex = uint(s - mSplitIslands);
+			outConstraintsBegin = mContactAndConstraintIndices + constraints_begin;
+			outConstraintsEnd = mContactAndConstraintIndices + constraints_end;
+			outContactsBegin = mContactAndConstraintIndices + contacts_begin;
+			outContactsEnd = mContactAndConstraintIndices + contacts_end;
+			return EStatus::BatchRetrieved;
+		}
+
+	return all_done? EStatus::AllBatchesDone : EStatus::WaitingForBatch;
+}
+
+void LargeIslandSplitter::MarkBatchProcessed(uint inSplitIslandIndex, const uint32 *inConstraintsBegin, const uint32 *inConstraintsEnd, const uint32 *inContactsBegin, const uint32 *inContactsEnd, bool &outLastIteration, bool &outFinalBatch)
+{
+	uint num_items_processed = uint(inConstraintsEnd - inConstraintsBegin) + uint(inContactsEnd - inContactsBegin);
+
+	JPH_ASSERT(inSplitIslandIndex < mNextSplitIsland.load(memory_order_relaxed));
+	Splits &splits = mSplitIslands[inSplitIslandIndex];
+	splits.MarkBatchProcessed(num_items_processed, outLastIteration, outFinalBatch);
+}
+
+void LargeIslandSplitter::PrepareForSolvePositions()
+{
+	for (Splits *s = mSplitIslands, *s_end = mSplitIslands + mNumSplitIslands; s < s_end; ++s)
+	{
+		// Set the number of iterations to the number of position steps
+		s->mNumIterations = s->mNumPositionSteps;
+
+		// We can start again from the first batch
+		s->StartFirstBatch();
+	}
+}
+
+void LargeIslandSplitter::Reset(TempAllocator *inTempAllocator)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Everything should have been used
+	JPH_ASSERT(mContactAndConstraintsNextFree.load(memory_order_relaxed) == mContactAndConstraintsSize);
+	JPH_ASSERT(mNextSplitIsland.load(memory_order_relaxed) == mNumSplitIslands);
+
+	// Free split islands
+	if (mNumSplitIslands > 0)
+	{
+		inTempAllocator->Free(mSplitIslands, mNumSplitIslands * sizeof(Splits));
+		mSplitIslands = nullptr;
+
+		mNumSplitIslands = 0;
+		mNextSplitIsland.store(0, memory_order_relaxed);
+	}
+
+	// Free contact and constraint buffers
+	if (mContactAndConstraintsSize > 0)
+	{
+		inTempAllocator->Free(mContactAndConstraintIndices, mContactAndConstraintsSize * sizeof(uint32));
+		mContactAndConstraintIndices = nullptr;
+
+		inTempAllocator->Free(mContactAndConstraintsSplitIdx, mContactAndConstraintsSize * sizeof(uint32));
+		mContactAndConstraintsSplitIdx = nullptr;
+
+		mContactAndConstraintsSize = 0;
+		mContactAndConstraintsNextFree.store(0, memory_order_relaxed);
+	}
+
+	// Free split masks
+	if (mSplitMasks != nullptr)
+	{
+		inTempAllocator->Free(mSplitMasks, mNumActiveBodies * sizeof(SplitMask));
+		mSplitMasks = nullptr;
+
+		mNumActiveBodies = 0;
+	}
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/LargeIslandSplitter.h b/thirdparty/jolt_physics/Jolt/Physics/LargeIslandSplitter.h
new file mode 100644
index 0000000000..36f0d61401
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/LargeIslandSplitter.h
@@ -0,0 +1,185 @@
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/NonCopyable.h>
+#include <Jolt/Core/Atomics.h>
+
+JPH_NAMESPACE_BEGIN
+
+class Body;
+class BodyID;
+class IslandBuilder;
+class TempAllocator;
+class Constraint;
+class BodyManager;
+class ContactConstraintManager;
+class CalculateSolverSteps;
+
+/// Assigns bodies in large islands to multiple groups that can run in parallel
+///
+/// This basically implements what is described in: High-Performance Physical Simulations on Next-Generation Architecture with Many Cores by Chen et al.
+/// See: http://web.eecs.umich.edu/~msmelyan/papers/physsim_onmanycore_itj.pdf section "PARALLELIZATION METHODOLOGY"
+class LargeIslandSplitter : public NonCopyable
+{
+private:
+	using					SplitMask = uint32;
+
+public:
+	static constexpr uint	cNumSplits = sizeof(SplitMask) * 8;
+	static constexpr uint	cNonParallelSplitIdx = cNumSplits - 1;
+	static constexpr uint	cLargeIslandTreshold = 128;							///< If the number of constraints + contacts in an island is larger than this, we will try to split the island
+
+	/// Status code for retrieving a batch
+	enum class EStatus
+	{
+		WaitingForBatch,														///< Work is expected to be available later
+		BatchRetrieved,															///< Work is being returned
+		AllBatchesDone,															///< No further work is expected from this
+	};
+
+	/// Describes a split of constraints and contacts
+	struct Split
+	{
+		inline uint			GetNumContacts() const								{ return mContactBufferEnd - mContactBufferBegin; }
+		inline uint			GetNumConstraints() const							{ return mConstraintBufferEnd - mConstraintBufferBegin; }
+		inline uint			GetNumItems() const									{ return GetNumContacts() + GetNumConstraints(); }
+
+		uint32				mContactBufferBegin;								///< Begin of the contact buffer (offset relative to mContactAndConstraintIndices)
+		uint32				mContactBufferEnd;									///< End of the contact buffer
+
+		uint32				mConstraintBufferBegin;								///< Begin of the constraint buffer (offset relative to mContactAndConstraintIndices)
+		uint32				mConstraintBufferEnd;								///< End of the constraint buffer
+	};
+
+	/// Structure that describes the resulting splits from the large island splitter
+	class Splits
+	{
+	public:
+		inline uint			GetNumSplits() const
+		{
+			return mNumSplits;
+		}
+
+		inline void			GetConstraintsInSplit(uint inSplitIndex, uint32 &outConstraintsBegin, uint32 &outConstraintsEnd) const
+		{
+			const Split &split = mSplits[inSplitIndex];
+			outConstraintsBegin = split.mConstraintBufferBegin;
+			outConstraintsEnd = split.mConstraintBufferEnd;
+		}
+
+		inline void			GetContactsInSplit(uint inSplitIndex, uint32 &outContactsBegin, uint32 &outContactsEnd) const
+		{
+			const Split &split = mSplits[inSplitIndex];
+			outContactsBegin = split.mContactBufferBegin;
+			outContactsEnd = split.mContactBufferEnd;
+		}
+
+		/// Reset current status so that no work can be picked up from this split
+		inline void			ResetStatus()
+		{
+			mStatus.store(StatusItemMask, memory_order_relaxed);
+		}
+
+		/// Make the first batch available to other threads
+		inline void			StartFirstBatch()
+		{
+			uint split_index = mNumSplits > 0? 0 : cNonParallelSplitIdx;
+			mStatus.store(uint64(split_index) << StatusSplitShift, memory_order_release);
+		}
+
+		/// Fetch the next batch to process
+		EStatus				FetchNextBatch(uint32 &outConstraintsBegin, uint32 &outConstraintsEnd, uint32 &outContactsBegin, uint32 &outContactsEnd, bool &outFirstIteration);
+
+		/// Mark a batch as processed
+		void				MarkBatchProcessed(uint inNumProcessed, bool &outLastIteration, bool &outFinalBatch);
+
+		enum EIterationStatus : uint64
+		{
+			StatusIterationMask		= 0xffff000000000000,
+			StatusIterationShift	= 48,
+			StatusSplitMask			= 0x0000ffff00000000,
+			StatusSplitShift		= 32,
+			StatusItemMask			= 0x00000000ffffffff,
+		};
+
+		static inline int	sGetIteration(uint64 inStatus)
+		{
+			return int((inStatus & StatusIterationMask) >> StatusIterationShift);
+		}
+
+		static inline uint	sGetSplit(uint64 inStatus)
+		{
+			return uint((inStatus & StatusSplitMask) >> StatusSplitShift);
+		}
+
+		static inline uint	sGetItem(uint64 inStatus)
+		{
+			return uint(inStatus & StatusItemMask);
+		}
+
+		Split				mSplits[cNumSplits];								///< Data per split
+		uint32				mIslandIndex;										///< Index of the island that was split
+		uint				mNumSplits;											///< Number of splits that were created (excluding the non-parallel split)
+		int					mNumIterations;										///< Number of iterations to do
+		int					mNumVelocitySteps;									///< Number of velocity steps to do (cached for 2nd sub step)
+		int					mNumPositionSteps;									///< Number of position steps to do
+		atomic<uint64>		mStatus;											///< Status of the split, see EIterationStatus
+		atomic<uint>		mItemsProcessed;									///< Number of items that have been marked as processed
+	};
+
+public:
+	/// Destructor
+							~LargeIslandSplitter();
+
+	/// Prepare the island splitter by allocating memory
+	void					Prepare(const IslandBuilder &inIslandBuilder, uint32 inNumActiveBodies, TempAllocator *inTempAllocator);
+
+	/// Assign two bodies to a split. Returns the split index.
+	uint					AssignSplit(const Body *inBody1, const Body *inBody2);
+
+	/// Force a body to be in a non parallel split. Returns the split index.
+	uint					AssignToNonParallelSplit(const Body *inBody);
+
+	/// Splits up an island, the created splits will be added to the list of batches and can be fetched with FetchNextBatch. Returns false if the island did not need splitting.
+	bool					SplitIsland(uint32 inIslandIndex, const IslandBuilder &inIslandBuilder, const BodyManager &inBodyManager, const ContactConstraintManager &inContactManager, Constraint **inActiveConstraints, CalculateSolverSteps &ioStepsCalculator);
+
+	/// Fetch the next batch to process, returns a handle in outSplitIslandIndex that must be provided to MarkBatchProcessed when complete
+	EStatus					FetchNextBatch(uint &outSplitIslandIndex, uint32 *&outConstraintsBegin, uint32 *&outConstraintsEnd, uint32 *&outContactsBegin, uint32 *&outContactsEnd, bool &outFirstIteration);
+
+	/// Mark a batch as processed
+	void					MarkBatchProcessed(uint inSplitIslandIndex, const uint32 *inConstraintsBegin, const uint32 *inConstraintsEnd, const uint32 *inContactsBegin, const uint32 *inContactsEnd, bool &outLastIteration, bool &outFinalBatch);
+
+	/// Get the island index of the island that was split for a particular split island index
+	inline uint32			GetIslandIndex(uint inSplitIslandIndex) const
+	{
+		JPH_ASSERT(inSplitIslandIndex < mNumSplitIslands);
+		return mSplitIslands[inSplitIslandIndex].mIslandIndex;
+	}
+
+	/// Prepare the island splitter for iterating over the split islands again for position solving. Marks all batches as startable.
+	void					PrepareForSolvePositions();
+
+	/// Reset the island splitter
+	void					Reset(TempAllocator *inTempAllocator);
+
+private:
+	static constexpr uint	cSplitCombineTreshold = 32;							///< If the number of constraints + contacts in a split is lower than this, we will merge this split into the 'non-parallel split'
+	static constexpr uint	cBatchSize = 16;									///< Number of items to process in a constraint batch
+
+	uint32					mNumActiveBodies = 0;								///< Cached number of active bodies
+
+	SplitMask *				mSplitMasks = nullptr;								///< Bits that indicate for each body in the BodyManager::mActiveBodies list which split they already belong to
+
+	uint32 *				mContactAndConstraintsSplitIdx = nullptr;			///< Buffer to store the split index per constraint or contact
+	uint32 *				mContactAndConstraintIndices = nullptr;				///< Buffer to store the ordered constraint indices per split
+	uint					mContactAndConstraintsSize = 0;						///< Total size of mContactAndConstraintsSplitIdx and mContactAndConstraintIndices
+	atomic<uint>			mContactAndConstraintsNextFree { 0 };				///< Next element that is free in both buffers
+
+	uint					mNumSplitIslands = 0;								///< Total number of islands that required splitting
+	Splits *				mSplitIslands = nullptr;							///< List of islands that required splitting
+	atomic<uint>			mNextSplitIsland = 0;								///< Next split island to pick from mSplitIslands
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsLock.h b/thirdparty/jolt_physics/Jolt/Physics/PhysicsLock.h
new file mode 100644
index 0000000000..1e83d18810
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsLock.h
@@ -0,0 +1,169 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/Mutex.h>
+
+JPH_NAMESPACE_BEGIN
+
+#ifdef JPH_ENABLE_ASSERTS
+
+/// This is the list of locks used by the physics engine, they need to be locked in a particular order (from top of the list to bottom of the list) in order to prevent deadlocks
+enum class EPhysicsLockTypes
+{
+	BroadPhaseQuery			= 1 << 0,
+	PerBody					= 1 << 1,
+	BodiesList				= 1 << 2,
+	BroadPhaseUpdate		= 1 << 3,
+	ConstraintsList			= 1 << 4,
+	ActiveBodiesList		= 1 << 5,
+};
+
+/// A token that indicates the context of a lock (we use 1 per physics system and we use the body manager pointer because it's convenient)
+class BodyManager;
+using PhysicsLockContext = const BodyManager *;
+
+#endif // !JPH_ENABLE_ASSERTS
+
+/// Helpers to safely lock the different mutexes that are part of the physics system while preventing deadlock
+/// Class that keeps track per thread which lock are taken and if the order of locking is correct
+class JPH_EXPORT PhysicsLock
+{
+public:
+#ifdef JPH_ENABLE_ASSERTS
+	/// Call before taking the lock
+	static inline void			sCheckLock(PhysicsLockContext inContext, EPhysicsLockTypes inType)
+	{
+		uint32 &mutexes = sGetLockedMutexes(inContext);
+		JPH_ASSERT(uint32(inType) > mutexes, "A lock of same or higher priority was already taken, this can create a deadlock!");
+		mutexes = mutexes | uint32(inType);
+	}
+
+	/// Call after releasing the lock
+	static inline void			sCheckUnlock(PhysicsLockContext inContext, EPhysicsLockTypes inType)
+	{
+		uint32 &mutexes = sGetLockedMutexes(inContext);
+		JPH_ASSERT((mutexes & uint32(inType)) != 0, "Mutex was not locked!");
+		mutexes = mutexes & ~uint32(inType);
+	}
+#endif // !JPH_ENABLE_ASSERTS
+
+	template <class LockType>
+	static inline void			sLock(LockType &inMutex JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType))
+	{
+		JPH_IF_ENABLE_ASSERTS(sCheckLock(inContext, inType);)
+		inMutex.lock();
+	}
+
+	template <class LockType>
+	static inline void			sUnlock(LockType &inMutex JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType))
+	{
+		JPH_IF_ENABLE_ASSERTS(sCheckUnlock(inContext, inType);)
+		inMutex.unlock();
+	}
+
+	template <class LockType>
+	static inline void			sLockShared(LockType &inMutex JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType))
+	{
+		JPH_IF_ENABLE_ASSERTS(sCheckLock(inContext, inType);)
+		inMutex.lock_shared();
+	}
+
+	template <class LockType>
+	static inline void			sUnlockShared(LockType &inMutex JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType))
+	{
+		JPH_IF_ENABLE_ASSERTS(sCheckUnlock(inContext, inType);)
+		inMutex.unlock_shared();
+	}
+
+#ifdef JPH_ENABLE_ASSERTS
+private:
+	struct LockData
+	{
+		uint32					mLockedMutexes = 0;
+		PhysicsLockContext		mContext = nullptr;
+	};
+
+	// Helper function to find the locked mutexes for a particular context
+	static uint32 &				sGetLockedMutexes(PhysicsLockContext inContext)
+	{
+		static thread_local LockData sLocks[4];
+
+		// If we find a matching context we can use it
+		for (LockData &l : sLocks)
+			if (l.mContext == inContext)
+				return l.mLockedMutexes;
+
+		// Otherwise we look for an entry that is not in use
+		for (LockData &l : sLocks)
+			if (l.mLockedMutexes == 0)
+			{
+				l.mContext = inContext;
+				return l.mLockedMutexes;
+			}
+
+		JPH_ASSERT(false, "Too many physics systems locked at the same time!");
+		return sLocks[0].mLockedMutexes;
+	}
+#endif // !JPH_ENABLE_ASSERTS
+};
+
+/// Helper class that is similar to std::unique_lock
+template <class LockType>
+class UniqueLock : public NonCopyable
+{
+public:
+	explicit					UniqueLock(LockType &inLock JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType)) :
+		mLock(inLock)
+#ifdef JPH_ENABLE_ASSERTS
+		, mContext(inContext),
+		mType(inType)
+#endif // JPH_ENABLE_ASSERTS
+	{
+		PhysicsLock::sLock(mLock JPH_IF_ENABLE_ASSERTS(, mContext, mType));
+	}
+
+								~UniqueLock()
+	{
+		PhysicsLock::sUnlock(mLock JPH_IF_ENABLE_ASSERTS(, mContext, mType));
+	}
+
+private:
+	LockType &					mLock;
+#ifdef JPH_ENABLE_ASSERTS
+	PhysicsLockContext			mContext;
+	EPhysicsLockTypes			mType;
+#endif // JPH_ENABLE_ASSERTS
+};
+
+/// Helper class that is similar to std::shared_lock
+template <class LockType>
+class SharedLock : public NonCopyable
+{
+public:
+	explicit					SharedLock(LockType &inLock JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType)) :
+		mLock(inLock)
+#ifdef JPH_ENABLE_ASSERTS
+		, mContext(inContext)
+		, mType(inType)
+#endif // JPH_ENABLE_ASSERTS
+	{
+		PhysicsLock::sLockShared(mLock JPH_IF_ENABLE_ASSERTS(, mContext, mType));
+	}
+
+								~SharedLock()
+	{
+		PhysicsLock::sUnlockShared(mLock JPH_IF_ENABLE_ASSERTS(, mContext, mType));
+	}
+
+private:
+	LockType &					mLock;
+#ifdef JPH_ENABLE_ASSERTS
+	PhysicsLockContext			mContext;
+	EPhysicsLockTypes			mType;
+#endif // JPH_ENABLE_ASSERTS
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsScene.cpp b/thirdparty/jolt_physics/Jolt/Physics/PhysicsScene.cpp
new file mode 100644
index 0000000000..1e9c60d230
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsScene.cpp
@@ -0,0 +1,261 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/PhysicsScene.h>
+#include <Jolt/Physics/PhysicsSystem.h>
+#include <Jolt/Physics/Body/BodyLockMulti.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(PhysicsScene)
+{
+	JPH_ADD_ATTRIBUTE(PhysicsScene, mBodies)
+	JPH_ADD_ATTRIBUTE(PhysicsScene, mConstraints)
+	JPH_ADD_ATTRIBUTE(PhysicsScene, mSoftBodies)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(PhysicsScene::ConnectedConstraint)
+{
+	JPH_ADD_ATTRIBUTE(PhysicsScene::ConnectedConstraint, mSettings)
+	JPH_ADD_ATTRIBUTE(PhysicsScene::ConnectedConstraint, mBody1)
+	JPH_ADD_ATTRIBUTE(PhysicsScene::ConnectedConstraint, mBody2)
+}
+
+void PhysicsScene::AddBody(const BodyCreationSettings &inBody)
+{
+	mBodies.push_back(inBody);
+}
+
+void PhysicsScene::AddConstraint(const TwoBodyConstraintSettings *inConstraint, uint32 inBody1, uint32 inBody2)
+{
+	mConstraints.emplace_back(inConstraint, inBody1, inBody2);
+}
+
+void PhysicsScene::AddSoftBody(const SoftBodyCreationSettings &inSoftBody)
+{
+	mSoftBodies.push_back(inSoftBody);
+}
+
+bool PhysicsScene::FixInvalidScales()
+{
+	const Vec3 unit_scale = Vec3::sReplicate(1.0f);
+
+	bool success = true;
+	for (BodyCreationSettings &b : mBodies)
+	{
+		// Test if there is an invalid scale in the shape hierarchy
+		const Shape *shape = b.GetShape();
+		if (!shape->IsValidScale(unit_scale))
+		{
+			// Fix it up
+			Shape::ShapeResult result = shape->ScaleShape(unit_scale);
+			if (result.IsValid())
+				b.SetShape(result.Get());
+			else
+				success = false;
+		}
+	}
+	return success;
+}
+
+bool PhysicsScene::CreateBodies(PhysicsSystem *inSystem) const
+{
+	BodyInterface &bi = inSystem->GetBodyInterface();
+
+	BodyIDVector body_ids;
+	body_ids.reserve(mBodies.size() + mSoftBodies.size());
+
+	// Create bodies
+	for (const BodyCreationSettings &b : mBodies)
+	{
+		const Body *body = bi.CreateBody(b);
+		if (body == nullptr)
+			break;
+		body_ids.push_back(body->GetID());
+	}
+
+	// Create soft bodies
+	for (const SoftBodyCreationSettings &b : mSoftBodies)
+	{
+		const Body *body = bi.CreateSoftBody(b);
+		if (body == nullptr)
+			break;
+		body_ids.push_back(body->GetID());
+	}
+
+	// Batch add bodies
+	BodyIDVector temp_body_ids = body_ids; // Body ID's get shuffled by AddBodiesPrepare
+	BodyInterface::AddState add_state = bi.AddBodiesPrepare(temp_body_ids.data(), (int)temp_body_ids.size());
+	bi.AddBodiesFinalize(temp_body_ids.data(), (int)temp_body_ids.size(), add_state, EActivation::Activate);
+
+	// If not all bodies are created, creating constraints will be unreliable
+	if (body_ids.size() != mBodies.size() + mSoftBodies.size())
+		return false;
+
+	// Create constraints
+	for (const ConnectedConstraint &cc : mConstraints)
+	{
+		BodyID body1_id = cc.mBody1 == cFixedToWorld? BodyID() : body_ids[cc.mBody1];
+		BodyID body2_id = cc.mBody2 == cFixedToWorld? BodyID() : body_ids[cc.mBody2];
+		Constraint *c = bi.CreateConstraint(cc.mSettings, body1_id, body2_id);
+		inSystem->AddConstraint(c);
+	}
+
+	// Everything was created
+	return true;
+}
+
+void PhysicsScene::SaveBinaryState(StreamOut &inStream, bool inSaveShapes, bool inSaveGroupFilter) const
+{
+	BodyCreationSettings::ShapeToIDMap shape_to_id;
+	BodyCreationSettings::MaterialToIDMap material_to_id;
+	BodyCreationSettings::GroupFilterToIDMap group_filter_to_id;
+	SoftBodyCreationSettings::SharedSettingsToIDMap settings_to_id;
+
+	// Save bodies
+	inStream.Write((uint32)mBodies.size());
+	for (const BodyCreationSettings &b : mBodies)
+		b.SaveWithChildren(inStream, inSaveShapes? &shape_to_id : nullptr, inSaveShapes? &material_to_id : nullptr, inSaveGroupFilter? &group_filter_to_id : nullptr);
+
+	// Save constraints
+	inStream.Write((uint32)mConstraints.size());
+	for (const ConnectedConstraint &cc : mConstraints)
+	{
+		cc.mSettings->SaveBinaryState(inStream);
+		inStream.Write(cc.mBody1);
+		inStream.Write(cc.mBody2);
+	}
+
+	// Save soft bodies
+	inStream.Write((uint32)mSoftBodies.size());
+	for (const SoftBodyCreationSettings &b : mSoftBodies)
+		b.SaveWithChildren(inStream, &settings_to_id, &material_to_id, inSaveGroupFilter? &group_filter_to_id : nullptr);
+}
+
+PhysicsScene::PhysicsSceneResult PhysicsScene::sRestoreFromBinaryState(StreamIn &inStream)
+{
+	PhysicsSceneResult result;
+
+	// Create scene
+	Ref<PhysicsScene> scene = new PhysicsScene();
+
+	BodyCreationSettings::IDToShapeMap id_to_shape;
+	BodyCreationSettings::IDToMaterialMap id_to_material;
+	BodyCreationSettings::IDToGroupFilterMap id_to_group_filter;
+	SoftBodyCreationSettings::IDToSharedSettingsMap id_to_settings;
+
+	// Reserve some memory to avoid frequent reallocations
+	id_to_shape.reserve(1024);
+	id_to_material.reserve(128);
+	id_to_group_filter.reserve(128);
+
+	// Read bodies
+	uint32 len = 0;
+	inStream.Read(len);
+	scene->mBodies.resize(len);
+	for (BodyCreationSettings &b : scene->mBodies)
+	{
+		// Read creation settings
+		BodyCreationSettings::BCSResult bcs_result = BodyCreationSettings::sRestoreWithChildren(inStream, id_to_shape, id_to_material, id_to_group_filter);
+		if (bcs_result.HasError())
+		{
+			result.SetError(bcs_result.GetError());
+			return result;
+		}
+		b = bcs_result.Get();
+	}
+
+	// Read constraints
+	len = 0;
+	inStream.Read(len);
+	scene->mConstraints.resize(len);
+	for (ConnectedConstraint &cc : scene->mConstraints)
+	{
+		ConstraintSettings::ConstraintResult c_result = ConstraintSettings::sRestoreFromBinaryState(inStream);
+		if (c_result.HasError())
+		{
+			result.SetError(c_result.GetError());
+			return result;
+		}
+		cc.mSettings = StaticCast<TwoBodyConstraintSettings>(c_result.Get());
+		inStream.Read(cc.mBody1);
+		inStream.Read(cc.mBody2);
+	}
+
+	// Read soft bodies
+	len = 0;
+	inStream.Read(len);
+	scene->mSoftBodies.resize(len);
+	for (SoftBodyCreationSettings &b : scene->mSoftBodies)
+	{
+		// Read creation settings
+		SoftBodyCreationSettings::SBCSResult sbcs_result = SoftBodyCreationSettings::sRestoreWithChildren(inStream, id_to_settings, id_to_material, id_to_group_filter);
+		if (sbcs_result.HasError())
+		{
+			result.SetError(sbcs_result.GetError());
+			return result;
+		}
+		b = sbcs_result.Get();
+	}
+
+	result.Set(scene);
+	return result;
+}
+
+void PhysicsScene::FromPhysicsSystem(const PhysicsSystem *inSystem)
+{
+	// This map will track where each body went in mBodies
+	using BodyIDToIdxMap = UnorderedMap<BodyID, uint32>;
+	BodyIDToIdxMap body_id_to_idx;
+
+	// Map invalid ID
+	body_id_to_idx[BodyID()] = cFixedToWorld;
+
+	// Get all bodies
+	BodyIDVector body_ids;
+	inSystem->GetBodies(body_ids);
+
+	// Loop over all bodies
+	const BodyLockInterface &bli = inSystem->GetBodyLockInterface();
+	for (const BodyID &id : body_ids)
+	{
+		BodyLockRead lock(bli, id);
+		if (lock.Succeeded())
+		{
+			// Store location of body
+			body_id_to_idx[id] = (uint32)mBodies.size();
+
+			const Body &body = lock.GetBody();
+
+			// Convert to body creation settings
+			if (body.IsRigidBody())
+				AddBody(body.GetBodyCreationSettings());
+			else
+				AddSoftBody(body.GetSoftBodyCreationSettings());
+		}
+	}
+
+	// Loop over all constraints
+	Constraints constraints = inSystem->GetConstraints();
+	for (const Constraint *c : constraints)
+		if (c->GetType() == EConstraintType::TwoBodyConstraint)
+		{
+			// Cast to two body constraint
+			const TwoBodyConstraint *tbc = static_cast<const TwoBodyConstraint *>(c);
+
+			// Find the body indices
+			BodyIDToIdxMap::const_iterator b1 = body_id_to_idx.find(tbc->GetBody1()->GetID());
+			BodyIDToIdxMap::const_iterator b2 = body_id_to_idx.find(tbc->GetBody2()->GetID());
+			JPH_ASSERT(b1 != body_id_to_idx.end() && b2 != body_id_to_idx.end());
+
+			// Create constraint settings and add the constraint
+			Ref<ConstraintSettings> settings = c->GetConstraintSettings();
+			AddConstraint(StaticCast<TwoBodyConstraintSettings>(settings), b1->second, b2->second);
+		}
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsScene.h b/thirdparty/jolt_physics/Jolt/Physics/PhysicsScene.h
new file mode 100644
index 0000000000..f974f83bd5
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsScene.h
@@ -0,0 +1,104 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/Reference.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Physics/SoftBody/SoftBodyCreationSettings.h>
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+
+JPH_NAMESPACE_BEGIN
+
+class PhysicsSystem;
+
+/// Contains the creation settings of a set of bodies
+class JPH_EXPORT PhysicsScene : public RefTarget<PhysicsScene>
+{
+	JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, PhysicsScene)
+
+public:
+	/// Add a body to the scene
+	void									AddBody(const BodyCreationSettings &inBody);
+
+	/// Body constant to use to indicate that the constraint is attached to the fixed world
+	static constexpr uint32					cFixedToWorld = 0xffffffff;
+
+	/// Add a constraint to the scene
+	/// @param inConstraint Constraint settings
+	/// @param inBody1 Index in the bodies list of first body to attach constraint to
+	/// @param inBody2 Index in the bodies list of the second body to attach constraint to
+	void									AddConstraint(const TwoBodyConstraintSettings *inConstraint, uint32 inBody1, uint32 inBody2);
+
+	/// Add a soft body to the scene
+	void									AddSoftBody(const SoftBodyCreationSettings &inSoftBody);
+
+	/// Get number of bodies in this scene
+	size_t									GetNumBodies() const							{ return mBodies.size(); }
+
+	/// Access to the body settings for this scene
+	const Array<BodyCreationSettings> &		GetBodies() const								{ return mBodies; }
+	Array<BodyCreationSettings> &			GetBodies()										{ return mBodies; }
+
+	/// A constraint and how it is connected to the bodies in the scene
+	class ConnectedConstraint
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, ConnectedConstraint)
+
+	public:
+											ConnectedConstraint() = default;
+											ConnectedConstraint(const TwoBodyConstraintSettings *inSettings, uint inBody1, uint inBody2) : mSettings(inSettings), mBody1(inBody1), mBody2(inBody2) { }
+
+		RefConst<TwoBodyConstraintSettings>	mSettings;										///< Constraint settings
+		uint32								mBody1;											///< Index of first body (in mBodies)
+		uint32								mBody2;											///< Index of second body (in mBodies)
+	};
+
+	/// Get number of constraints in this scene
+	size_t									GetNumConstraints() const						{ return mConstraints.size(); }
+
+	/// Access to the constraints for this scene
+	const Array<ConnectedConstraint> &		GetConstraints() const							{ return mConstraints; }
+	Array<ConnectedConstraint> &			GetConstraints()								{ return mConstraints; }
+
+	/// Get number of bodies in this scene
+	size_t									GetNumSoftBodies() const						{ return mSoftBodies.size(); }
+
+	/// Access to the soft body settings for this scene
+	const Array<SoftBodyCreationSettings> &	GetSoftBodies() const							{ return mSoftBodies; }
+	Array<SoftBodyCreationSettings> &		GetSoftBodies()									{ return mSoftBodies; }
+
+	/// Instantiate all bodies, returns false if not all bodies could be created
+	bool									CreateBodies(PhysicsSystem *inSystem) const;
+
+	/// Go through all body creation settings and fix shapes that are scaled incorrectly (note this will change the scene a bit).
+	/// @return False when not all scales could be fixed.
+	bool									FixInvalidScales();
+
+	/// Saves the state of this object in binary form to inStream.
+	/// @param inStream The stream to save the state to
+	/// @param inSaveShapes If the shapes should be saved as well (these could be shared between physics scenes, in which case the calling application may want to write custom code to restore them)
+	/// @param inSaveGroupFilter If the group filter should be saved as well (these could be shared)
+	void									SaveBinaryState(StreamOut &inStream, bool inSaveShapes, bool inSaveGroupFilter) const;
+
+	using PhysicsSceneResult = Result<Ref<PhysicsScene>>;
+
+	/// Restore a saved scene from inStream
+	static PhysicsSceneResult				sRestoreFromBinaryState(StreamIn &inStream);
+
+	/// For debugging purposes: Construct a scene from the current state of the physics system
+	void									FromPhysicsSystem(const PhysicsSystem *inSystem);
+
+private:
+	/// The bodies that are part of this scene
+	Array<BodyCreationSettings>				mBodies;
+
+	/// Constraints that are part of this scene
+	Array<ConnectedConstraint>				mConstraints;
+
+	/// Soft bodies that are part of this scene
+	Array<SoftBodyCreationSettings>			mSoftBodies;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsSettings.h b/thirdparty/jolt_physics/Jolt/Physics/PhysicsSettings.h
new file mode 100644
index 0000000000..eb8ecbbeaf
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsSettings.h
@@ -0,0 +1,119 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// If objects are closer than this distance, they are considered to be colliding (used for GJK) (unit: meter)
+constexpr float cDefaultCollisionTolerance = 1.0e-4f;
+
+/// A factor that determines the accuracy of the penetration depth calculation. If the change of the squared distance is less than tolerance * current_penetration_depth^2 the algorithm will terminate. (unit: dimensionless)
+constexpr float cDefaultPenetrationTolerance = 1.0e-4f; ///< Stop when there's less than 1% change
+
+/// How much padding to add around objects
+constexpr float cDefaultConvexRadius = 0.05f;
+
+/// Used by (Tapered)CapsuleShape to determine when supporting face is an edge rather than a point (unit: meter)
+static constexpr float cCapsuleProjectionSlop = 0.02f;
+
+/// Maximum amount of jobs to allow
+constexpr int cMaxPhysicsJobs = 2048;
+
+/// Maximum amount of barriers to allow
+constexpr int cMaxPhysicsBarriers = 8;
+
+struct PhysicsSettings
+{
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Size of body pairs array, corresponds to the maximum amount of potential body pairs that can be in flight at any time.
+	/// Setting this to a low value will use less memory but slow down simulation as threads may run out of narrow phase work.
+	int			mMaxInFlightBodyPairs = 16384;
+
+	/// How many PhysicsStepListeners to notify in 1 batch
+	int			mStepListenersBatchSize = 8;
+
+	/// How many step listener batches are needed before spawning another job (set to INT_MAX if no parallelism is desired)
+	int			mStepListenerBatchesPerJob = 1;
+
+	/// Baumgarte stabilization factor (how much of the position error to 'fix' in 1 update) (unit: dimensionless, 0 = nothing, 1 = 100%)
+	float		mBaumgarte = 0.2f;
+
+	/// Radius around objects inside which speculative contact points will be detected. Note that if this is too big
+	/// you will get ghost collisions as speculative contacts are based on the closest points during the collision detection
+	/// step which may not be the actual closest points by the time the two objects hit (unit: meters)
+	float		mSpeculativeContactDistance = 0.02f;
+
+	/// How much bodies are allowed to sink into each other (unit: meters)
+	float		mPenetrationSlop = 0.02f;
+
+	/// Fraction of its inner radius a body must move per step to enable casting for the LinearCast motion quality
+	float		mLinearCastThreshold = 0.75f;
+
+	/// Fraction of its inner radius a body may penetrate another body for the LinearCast motion quality
+	float		mLinearCastMaxPenetration = 0.25f;
+
+	/// Max squared distance to use to determine if two points are on the same plane for determining the contact manifold between two shape faces (unit: meter^2)
+	float		mManifoldToleranceSq = 1.0e-6f;
+
+	/// Maximum distance to correct in a single iteration when solving position constraints (unit: meters)
+	float		mMaxPenetrationDistance = 0.2f;
+
+	/// Maximum relative delta position for body pairs to be able to reuse collision results from last frame (units: meter^2)
+	float		mBodyPairCacheMaxDeltaPositionSq = Square(0.001f); ///< 1 mm
+
+	/// Maximum relative delta orientation for body pairs to be able to reuse collision results from last frame, stored as cos(max angle / 2)
+	float		mBodyPairCacheCosMaxDeltaRotationDiv2 = 0.99984769515639123915701155881391f; ///< cos(2 degrees / 2)
+
+	/// Maximum angle between normals that allows manifolds between different sub shapes of the same body pair to be combined
+	float		mContactNormalCosMaxDeltaRotation = 0.99619469809174553229501040247389f; ///< cos(5 degree)
+
+	/// Maximum allowed distance between old and new contact point to preserve contact forces for warm start (units: meter^2)
+	float		mContactPointPreserveLambdaMaxDistSq = Square(0.01f); ///< 1 cm
+
+	/// Number of solver velocity iterations to run
+	/// Note that this needs to be >= 2 in order for friction to work (friction is applied using the non-penetration impulse from the previous iteration)
+	uint		mNumVelocitySteps = 10;
+
+	/// Number of solver position iterations to run
+	uint		mNumPositionSteps = 2;
+
+	/// Minimal velocity needed before a collision can be elastic (unit: m)
+	float		mMinVelocityForRestitution = 1.0f;
+
+	/// Time before object is allowed to go to sleep (unit: seconds)
+	float		mTimeBeforeSleep = 0.5f;
+
+	/// Velocity of points on bounding box of object below which an object can be considered sleeping (unit: m/s)
+	float		mPointVelocitySleepThreshold = 0.03f;
+
+	/// By default the simulation is deterministic, it is possible to turn this off by setting this setting to false. This will make the simulation run faster but it will no longer be deterministic.
+	bool		mDeterministicSimulation = true;
+
+	///@name These variables are mainly for debugging purposes, they allow turning on/off certain subsystems. You probably want to leave them alone.
+	///@{
+
+	/// Whether or not to use warm starting for constraints (initially applying previous frames impulses)
+	bool		mConstraintWarmStart = true;
+
+	/// Whether or not to use the body pair cache, which removes the need for narrow phase collision detection when orientation between two bodies didn't change
+	bool		mUseBodyPairContactCache = true;
+
+	/// Whether or not to reduce manifolds with similar contact normals into one contact manifold (see description at Body::SetUseManifoldReduction)
+	bool		mUseManifoldReduction = true;
+
+	/// If we split up large islands into smaller parallel batches of work (to improve performance)
+	bool		mUseLargeIslandSplitter = true;
+
+	/// If objects can go to sleep or not
+	bool		mAllowSleeping = true;
+
+	/// When false, we prevent collision against non-active (shared) edges. Mainly for debugging the algorithm.
+	bool		mCheckActiveEdges = true;
+
+	///@}
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsStepListener.h b/thirdparty/jolt_physics/Jolt/Physics/PhysicsStepListener.h
new file mode 100644
index 0000000000..26c8eec34a
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsStepListener.h
@@ -0,0 +1,37 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+class PhysicsSystem;
+
+/// Context information for the step listener
+class JPH_EXPORT PhysicsStepListenerContext
+{
+public:
+	float					mDeltaTime;								///< Delta time of the current step
+	bool					mIsFirstStep;							///< True if this is the first step
+	bool					mIsLastStep;							///< True if this is the last step
+	PhysicsSystem *			mPhysicsSystem;							///< The physics system that is being stepped
+};
+
+/// A listener class that receives a callback before every physics simulation step
+class JPH_EXPORT PhysicsStepListener
+{
+public:
+	/// Ensure virtual destructor
+	virtual					~PhysicsStepListener() = default;
+
+	/// Called before every simulation step (received inCollisionSteps times for every PhysicsSystem::Update(...) call)
+	/// This is called while all body and constraint mutexes are locked. You can read/write bodies and constraints but not add/remove them.
+	/// Multiple listeners can be executed in parallel and it is the responsibility of the listener to avoid race conditions.
+	/// The best way to do this is to have each step listener operate on a subset of the bodies and constraints
+	/// and making sure that these bodies and constraints are not touched by any other step listener.
+	/// Note that this function is not called if there aren't any active bodies or when the physics system is updated with 0 delta time.
+	virtual void			OnStep(const PhysicsStepListenerContext &inContext) = 0;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.cpp b/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.cpp
new file mode 100644
index 0000000000..3ea6beabfa
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.cpp
@@ -0,0 +1,2754 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/PhysicsSystem.h>
+#include <Jolt/Physics/PhysicsSettings.h>
+#include <Jolt/Physics/PhysicsUpdateContext.h>
+#include <Jolt/Physics/PhysicsStepListener.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Physics/Collision/AABoxCast.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/CollideShape.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/CollideConvexVsTriangles.h>
+#include <Jolt/Physics/Collision/ManifoldBetweenTwoFaces.h>
+#include <Jolt/Physics/Collision/Shape/ConvexShape.h>
+#include <Jolt/Physics/Collision/SimShapeFilterWrapper.h>
+#include <Jolt/Physics/Collision/InternalEdgeRemovingCollector.h>
+#include <Jolt/Physics/Constraints/CalculateSolverSteps.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h>
+#include <Jolt/Physics/DeterminismLog.h>
+#include <Jolt/Physics/SoftBody/SoftBodyMotionProperties.h>
+#include <Jolt/Physics/SoftBody/SoftBodyShape.h>
+#include <Jolt/Geometry/RayAABox.h>
+#include <Jolt/Geometry/ClosestPoint.h>
+#include <Jolt/Core/JobSystem.h>
+#include <Jolt/Core/TempAllocator.h>
+#include <Jolt/Core/QuickSort.h>
+#include <Jolt/Core/ScopeExit.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+#ifdef JPH_DEBUG_RENDERER
+bool PhysicsSystem::sDrawMotionQualityLinearCast = false;
+#endif // JPH_DEBUG_RENDERER
+
+//#define BROAD_PHASE BroadPhaseBruteForce
+#define BROAD_PHASE BroadPhaseQuadTree
+
+static const Color cColorUpdateBroadPhaseFinalize = Color::sGetDistinctColor(1);
+static const Color cColorUpdateBroadPhasePrepare = Color::sGetDistinctColor(2);
+static const Color cColorFindCollisions = Color::sGetDistinctColor(3);
+static const Color cColorApplyGravity = Color::sGetDistinctColor(4);
+static const Color cColorSetupVelocityConstraints = Color::sGetDistinctColor(5);
+static const Color cColorBuildIslandsFromConstraints = Color::sGetDistinctColor(6);
+static const Color cColorDetermineActiveConstraints = Color::sGetDistinctColor(7);
+static const Color cColorFinalizeIslands = Color::sGetDistinctColor(8);
+static const Color cColorContactRemovedCallbacks = Color::sGetDistinctColor(9);
+static const Color cColorBodySetIslandIndex = Color::sGetDistinctColor(10);
+static const Color cColorStartNextStep = Color::sGetDistinctColor(11);
+static const Color cColorSolveVelocityConstraints = Color::sGetDistinctColor(12);
+static const Color cColorPreIntegrateVelocity = Color::sGetDistinctColor(13);
+static const Color cColorIntegrateVelocity = Color::sGetDistinctColor(14);
+static const Color cColorPostIntegrateVelocity = Color::sGetDistinctColor(15);
+static const Color cColorResolveCCDContacts = Color::sGetDistinctColor(16);
+static const Color cColorSolvePositionConstraints = Color::sGetDistinctColor(17);
+static const Color cColorFindCCDContacts = Color::sGetDistinctColor(18);
+static const Color cColorStepListeners = Color::sGetDistinctColor(19);
+static const Color cColorSoftBodyPrepare = Color::sGetDistinctColor(20);
+static const Color cColorSoftBodyCollide = Color::sGetDistinctColor(21);
+static const Color cColorSoftBodySimulate = Color::sGetDistinctColor(22);
+static const Color cColorSoftBodyFinalize = Color::sGetDistinctColor(23);
+
+PhysicsSystem::~PhysicsSystem()
+{
+	// Remove broadphase
+	delete mBroadPhase;
+}
+
+void PhysicsSystem::Init(uint inMaxBodies, uint inNumBodyMutexes, uint inMaxBodyPairs, uint inMaxContactConstraints, const BroadPhaseLayerInterface &inBroadPhaseLayerInterface, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter)
+{
+	JPH_ASSERT(inMaxBodies <= BodyID::cMaxBodyIndex, "Cannot support this many bodies");
+
+	mObjectVsBroadPhaseLayerFilter = &inObjectVsBroadPhaseLayerFilter;
+	mObjectLayerPairFilter = &inObjectLayerPairFilter;
+
+	// Initialize body manager
+	mBodyManager.Init(inMaxBodies, inNumBodyMutexes, inBroadPhaseLayerInterface);
+
+	// Create broadphase
+	mBroadPhase = new BROAD_PHASE();
+	mBroadPhase->Init(&mBodyManager, inBroadPhaseLayerInterface);
+
+	// Init contact constraint manager
+	mContactManager.Init(inMaxBodyPairs, inMaxContactConstraints);
+
+	// Init islands builder
+	mIslandBuilder.Init(inMaxBodies);
+
+	// Initialize body interface
+	mBodyInterfaceLocking.Init(mBodyLockInterfaceLocking, mBodyManager, *mBroadPhase);
+	mBodyInterfaceNoLock.Init(mBodyLockInterfaceNoLock, mBodyManager, *mBroadPhase);
+
+	// Initialize narrow phase query
+	mNarrowPhaseQueryLocking.Init(mBodyLockInterfaceLocking, *mBroadPhase);
+	mNarrowPhaseQueryNoLock.Init(mBodyLockInterfaceNoLock, *mBroadPhase);
+}
+
+void PhysicsSystem::OptimizeBroadPhase()
+{
+	mBroadPhase->Optimize();
+}
+
+void PhysicsSystem::AddStepListener(PhysicsStepListener *inListener)
+{
+	lock_guard lock(mStepListenersMutex);
+
+	JPH_ASSERT(std::find(mStepListeners.begin(), mStepListeners.end(), inListener) == mStepListeners.end());
+	mStepListeners.push_back(inListener);
+}
+
+void PhysicsSystem::RemoveStepListener(PhysicsStepListener *inListener)
+{
+	lock_guard lock(mStepListenersMutex);
+
+	StepListeners::iterator i = std::find(mStepListeners.begin(), mStepListeners.end(), inListener);
+	JPH_ASSERT(i != mStepListeners.end());
+	*i = mStepListeners.back();
+	mStepListeners.pop_back();
+}
+
+EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionSteps, TempAllocator *inTempAllocator, JobSystem *inJobSystem)
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_DET_LOG("PhysicsSystem::Update: dt: " << inDeltaTime << " steps: " << inCollisionSteps);
+
+	JPH_ASSERT(inCollisionSteps > 0);
+	JPH_ASSERT(inDeltaTime >= 0.0f);
+
+	// Sync point for the broadphase. This will allow it to do clean up operations without having any mutexes locked yet.
+	mBroadPhase->FrameSync();
+
+	// If there are no active bodies or there's no time delta
+	uint32 num_active_rigid_bodies = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody);
+	uint32 num_active_soft_bodies = mBodyManager.GetNumActiveBodies(EBodyType::SoftBody);
+	if ((num_active_rigid_bodies == 0 && num_active_soft_bodies == 0) || inDeltaTime <= 0.0f)
+	{
+		mBodyManager.LockAllBodies();
+
+		// Update broadphase
+		mBroadPhase->LockModifications();
+		BroadPhase::UpdateState update_state = mBroadPhase->UpdatePrepare();
+		mBroadPhase->UpdateFinalize(update_state);
+		mBroadPhase->UnlockModifications();
+
+		// Call contact removal callbacks from contacts that existed in the previous update
+		mContactManager.FinalizeContactCacheAndCallContactPointRemovedCallbacks(0, 0);
+
+		mBodyManager.UnlockAllBodies();
+		return EPhysicsUpdateError::None;
+	}
+
+	// Calculate ratio between current and previous frame delta time to scale initial constraint forces
+	float step_delta_time = inDeltaTime / inCollisionSteps;
+	float warm_start_impulse_ratio = mPhysicsSettings.mConstraintWarmStart && mPreviousStepDeltaTime > 0.0f? step_delta_time / mPreviousStepDeltaTime : 0.0f;
+	mPreviousStepDeltaTime = step_delta_time;
+
+	// Create the context used for passing information between jobs
+	PhysicsUpdateContext context(*inTempAllocator);
+	context.mPhysicsSystem = this;
+	context.mJobSystem = inJobSystem;
+	context.mBarrier = inJobSystem->CreateBarrier();
+	context.mIslandBuilder = &mIslandBuilder;
+	context.mStepDeltaTime = step_delta_time;
+	context.mWarmStartImpulseRatio = warm_start_impulse_ratio;
+	context.mSteps.resize(inCollisionSteps);
+
+	// Allocate space for body pairs
+	JPH_ASSERT(context.mBodyPairs == nullptr);
+	context.mBodyPairs = static_cast<BodyPair *>(inTempAllocator->Allocate(sizeof(BodyPair) * mPhysicsSettings.mMaxInFlightBodyPairs));
+
+	// Lock all bodies for write so that we can freely touch them
+	mStepListenersMutex.lock();
+	mBodyManager.LockAllBodies();
+	mBroadPhase->LockModifications();
+
+	// Get max number of concurrent jobs
+	int max_concurrency = context.GetMaxConcurrency();
+
+	// Calculate how many step listener jobs we spawn
+	int num_step_listener_jobs = mStepListeners.empty()? 0 : max(1, min((int)mStepListeners.size() / mPhysicsSettings.mStepListenersBatchSize / mPhysicsSettings.mStepListenerBatchesPerJob, max_concurrency));
+
+	// Number of gravity jobs depends on the amount of active bodies.
+	// Launch max 1 job per batch of active bodies
+	// Leave 1 thread for update broadphase prepare and 1 for determine active constraints
+	int num_apply_gravity_jobs = max(1, min(((int)num_active_rigid_bodies + cApplyGravityBatchSize - 1) / cApplyGravityBatchSize, max_concurrency - 2));
+
+	// Number of determine active constraints jobs to run depends on number of constraints.
+	// Leave 1 thread for update broadphase prepare and 1 for apply gravity
+	int num_determine_active_constraints_jobs = max(1, min(((int)mConstraintManager.GetNumConstraints() + cDetermineActiveConstraintsBatchSize - 1) / cDetermineActiveConstraintsBatchSize, max_concurrency - 2));
+
+	// Number of setup velocity constraints jobs to run depends on number of constraints.
+	int num_setup_velocity_constraints_jobs = max(1, min(((int)mConstraintManager.GetNumConstraints() + cSetupVelocityConstraintsBatchSize - 1) / cSetupVelocityConstraintsBatchSize, max_concurrency));
+
+	// Number of find collisions jobs to run depends on number of active bodies.
+	// Note that when we have more than 1 thread, we always spawn at least 2 find collisions jobs so that the first job can wait for build islands from constraints
+	// (which may activate additional bodies that need to be processed) while the second job can start processing collision work.
+	int num_find_collisions_jobs = max(max_concurrency == 1? 1 : 2, min(((int)num_active_rigid_bodies + cActiveBodiesBatchSize - 1) / cActiveBodiesBatchSize, max_concurrency));
+
+	// Number of integrate velocity jobs depends on number of active bodies.
+	int num_integrate_velocity_jobs = max(1, min(((int)num_active_rigid_bodies + cIntegrateVelocityBatchSize - 1) / cIntegrateVelocityBatchSize, max_concurrency));
+
+	{
+		JPH_PROFILE("Build Jobs");
+
+		// Iterate over collision steps
+		for (int step_idx = 0; step_idx < inCollisionSteps; ++step_idx)
+		{
+			bool is_first_step = step_idx == 0;
+			bool is_last_step = step_idx == inCollisionSteps - 1;
+
+			PhysicsUpdateContext::Step &step = context.mSteps[step_idx];
+			step.mContext = &context;
+			step.mIsFirst = is_first_step;
+			step.mIsLast = is_last_step;
+
+			// Create job to do broadphase finalization
+			// This job must finish before integrating velocities. Until then the positions will not be updated neither will bodies be added / removed.
+			step.mUpdateBroadphaseFinalize = inJobSystem->CreateJob("UpdateBroadPhaseFinalize", cColorUpdateBroadPhaseFinalize, [&context, &step]()
+				{
+					// Validate that all find collision jobs have stopped
+					JPH_ASSERT(step.mActiveFindCollisionJobs.load(memory_order_relaxed) == 0);
+
+					// Finalize the broadphase update
+					context.mPhysicsSystem->mBroadPhase->UpdateFinalize(step.mBroadPhaseUpdateState);
+
+					// Signal that it is done
+					step.mPreIntegrateVelocity.RemoveDependency();
+				}, num_find_collisions_jobs + 2); // depends on: find collisions, broadphase prepare update, finish building jobs
+
+			// The immediate jobs below are only immediate for the first step, the all finished job will kick them for the next step
+			int previous_step_dependency_count = is_first_step? 0 : 1;
+
+			// Start job immediately: Start the prepare broadphase
+			// Must be done under body lock protection since the order is body locks then broadphase mutex
+			// If this is turned around the RemoveBody call will hang since it locks in that order
+			step.mBroadPhasePrepare = inJobSystem->CreateJob("UpdateBroadPhasePrepare", cColorUpdateBroadPhasePrepare, [&context, &step]()
+				{
+					// Prepare the broadphase update
+					step.mBroadPhaseUpdateState = context.mPhysicsSystem->mBroadPhase->UpdatePrepare();
+
+					// Now the finalize can run (if other dependencies are met too)
+					step.mUpdateBroadphaseFinalize.RemoveDependency();
+				}, previous_step_dependency_count);
+
+			// This job will find all collisions
+			step.mBodyPairQueues.resize(max_concurrency);
+			step.mMaxBodyPairsPerQueue = mPhysicsSettings.mMaxInFlightBodyPairs / max_concurrency;
+			step.mActiveFindCollisionJobs.store(~PhysicsUpdateContext::JobMask(0) >> (sizeof(PhysicsUpdateContext::JobMask) * 8 - num_find_collisions_jobs), memory_order_release);
+			step.mFindCollisions.resize(num_find_collisions_jobs);
+			for (int i = 0; i < num_find_collisions_jobs; ++i)
+			{
+				// Build islands from constraints may activate additional bodies, so the first job will wait for this to finish in order to not miss any active bodies
+				int num_dep_build_islands_from_constraints = i == 0? 1 : 0;
+				step.mFindCollisions[i] = inJobSystem->CreateJob("FindCollisions", cColorFindCollisions, [&step, i]()
+					{
+						step.mContext->mPhysicsSystem->JobFindCollisions(&step, i);
+					}, num_apply_gravity_jobs + num_determine_active_constraints_jobs + 1 + num_dep_build_islands_from_constraints); // depends on: apply gravity, determine active constraints, finish building jobs, build islands from constraints
+			}
+
+			if (is_first_step)
+			{
+			#ifdef JPH_ENABLE_ASSERTS
+				// Don't allow write operations to the active bodies list
+				mBodyManager.SetActiveBodiesLocked(true);
+			#endif
+
+				// Store the number of active bodies at the start of the step
+				step.mNumActiveBodiesAtStepStart = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody);
+
+				// Lock all constraints
+				mConstraintManager.LockAllConstraints();
+
+				// Allocate memory for storing the active constraints
+				JPH_ASSERT(context.mActiveConstraints == nullptr);
+				context.mActiveConstraints = static_cast<Constraint **>(inTempAllocator->Allocate(mConstraintManager.GetNumConstraints() * sizeof(Constraint *)));
+
+				// Prepare contact buffer
+				mContactManager.PrepareConstraintBuffer(&context);
+
+				// Setup island builder
+				mIslandBuilder.PrepareContactConstraints(mContactManager.GetMaxConstraints(), context.mTempAllocator);
+			}
+
+			// This job applies gravity to all active bodies
+			step.mApplyGravity.resize(num_apply_gravity_jobs);
+			for (int i = 0; i < num_apply_gravity_jobs; ++i)
+				step.mApplyGravity[i] = inJobSystem->CreateJob("ApplyGravity", cColorApplyGravity, [&context, &step]()
+					{
+						context.mPhysicsSystem->JobApplyGravity(&context, &step);
+
+						JobHandle::sRemoveDependencies(step.mFindCollisions);
+					}, num_step_listener_jobs > 0? num_step_listener_jobs : previous_step_dependency_count); // depends on: step listeners (or previous step if no step listeners)
+
+			// This job will setup velocity constraints for non-collision constraints
+			step.mSetupVelocityConstraints.resize(num_setup_velocity_constraints_jobs);
+			for (int i = 0; i < num_setup_velocity_constraints_jobs; ++i)
+				step.mSetupVelocityConstraints[i] = inJobSystem->CreateJob("SetupVelocityConstraints", cColorSetupVelocityConstraints, [&context, &step]()
+					{
+						context.mPhysicsSystem->JobSetupVelocityConstraints(context.mStepDeltaTime, &step);
+
+						JobHandle::sRemoveDependencies(step.mSolveVelocityConstraints);
+					}, num_determine_active_constraints_jobs + 1); // depends on: determine active constraints, finish building jobs
+
+			// This job will build islands from constraints
+			step.mBuildIslandsFromConstraints = inJobSystem->CreateJob("BuildIslandsFromConstraints", cColorBuildIslandsFromConstraints, [&context, &step]()
+				{
+					context.mPhysicsSystem->JobBuildIslandsFromConstraints(&context, &step);
+
+					step.mFindCollisions[0].RemoveDependency(); // The first collisions job cannot start running until we've finished building islands and activated all bodies
+					step.mFinalizeIslands.RemoveDependency();
+				}, num_determine_active_constraints_jobs + 1); // depends on: determine active constraints, finish building jobs
+
+			// This job determines active constraints
+			step.mDetermineActiveConstraints.resize(num_determine_active_constraints_jobs);
+			for (int i = 0; i < num_determine_active_constraints_jobs; ++i)
+				step.mDetermineActiveConstraints[i] = inJobSystem->CreateJob("DetermineActiveConstraints", cColorDetermineActiveConstraints, [&context, &step]()
+					{
+						context.mPhysicsSystem->JobDetermineActiveConstraints(&step);
+
+						step.mBuildIslandsFromConstraints.RemoveDependency();
+
+						// Kick these jobs last as they will use up all CPU cores leaving no space for the previous job, we prefer setup velocity constraints to finish first so we kick it first
+						JobHandle::sRemoveDependencies(step.mSetupVelocityConstraints);
+						JobHandle::sRemoveDependencies(step.mFindCollisions);
+					}, num_step_listener_jobs > 0? num_step_listener_jobs : previous_step_dependency_count); // depends on: step listeners (or previous step if no step listeners)
+
+			// This job calls the step listeners
+			step.mStepListeners.resize(num_step_listener_jobs);
+			for (int i = 0; i < num_step_listener_jobs; ++i)
+				step.mStepListeners[i] = inJobSystem->CreateJob("StepListeners", cColorStepListeners, [&context, &step]()
+					{
+						// Call the step listeners
+						context.mPhysicsSystem->JobStepListeners(&step);
+
+						// Kick apply gravity and determine active constraint jobs
+						JobHandle::sRemoveDependencies(step.mApplyGravity);
+						JobHandle::sRemoveDependencies(step.mDetermineActiveConstraints);
+					}, previous_step_dependency_count);
+
+			// Unblock the previous step
+			if (!is_first_step)
+				context.mSteps[step_idx - 1].mStartNextStep.RemoveDependency();
+
+			// This job will finalize the simulation islands
+			step.mFinalizeIslands = inJobSystem->CreateJob("FinalizeIslands", cColorFinalizeIslands, [&context, &step]()
+				{
+					// Validate that all find collision jobs have stopped
+					JPH_ASSERT(step.mActiveFindCollisionJobs.load(memory_order_relaxed) == 0);
+
+					context.mPhysicsSystem->JobFinalizeIslands(&context);
+
+					JobHandle::sRemoveDependencies(step.mSolveVelocityConstraints);
+					step.mBodySetIslandIndex.RemoveDependency();
+				}, num_find_collisions_jobs + 2); // depends on: find collisions, build islands from constraints, finish building jobs
+
+			// Unblock previous job
+			// Note: technically we could release find collisions here but we don't want to because that could make them run before 'setup velocity constraints' which means that job won't have a thread left
+			step.mBuildIslandsFromConstraints.RemoveDependency();
+
+			// This job will call the contact removed callbacks
+			step.mContactRemovedCallbacks = inJobSystem->CreateJob("ContactRemovedCallbacks", cColorContactRemovedCallbacks, [&context, &step]()
+				{
+					context.mPhysicsSystem->JobContactRemovedCallbacks(&step);
+
+					if (step.mStartNextStep.IsValid())
+						step.mStartNextStep.RemoveDependency();
+				}, 1); // depends on the find ccd contacts
+
+			// This job will set the island index on each body (only used for debug drawing purposes)
+			// It will also delete any bodies that have been destroyed in the last frame
+			step.mBodySetIslandIndex = inJobSystem->CreateJob("BodySetIslandIndex", cColorBodySetIslandIndex, [&context, &step]()
+				{
+					context.mPhysicsSystem->JobBodySetIslandIndex();
+
+					JobHandle::sRemoveDependencies(step.mSolvePositionConstraints);
+				}, 2); // depends on: finalize islands, finish building jobs
+
+			// Job to start the next collision step
+			if (!is_last_step)
+			{
+				PhysicsUpdateContext::Step *next_step = &context.mSteps[step_idx + 1];
+				step.mStartNextStep = inJobSystem->CreateJob("StartNextStep", cColorStartNextStep, [this, next_step]()
+					{
+					#ifdef JPH_DEBUG
+						// Validate that the cached bounds are correct
+						mBodyManager.ValidateActiveBodyBounds();
+					#endif // JPH_DEBUG
+
+						// Store the number of active bodies at the start of the step
+						next_step->mNumActiveBodiesAtStepStart = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody);
+
+						// Clear the large island splitter
+						TempAllocator *temp_allocator = next_step->mContext->mTempAllocator;
+						mLargeIslandSplitter.Reset(temp_allocator);
+
+						// Clear the island builder
+						mIslandBuilder.ResetIslands(temp_allocator);
+
+						// Setup island builder
+						mIslandBuilder.PrepareContactConstraints(mContactManager.GetMaxConstraints(), temp_allocator);
+
+						// Restart the contact manager
+						mContactManager.RecycleConstraintBuffer();
+
+						// Kick the jobs of the next step (in the same order as the first step)
+						next_step->mBroadPhasePrepare.RemoveDependency();
+						if (next_step->mStepListeners.empty())
+						{
+							// Kick the gravity and active constraints jobs immediately
+							JobHandle::sRemoveDependencies(next_step->mApplyGravity);
+							JobHandle::sRemoveDependencies(next_step->mDetermineActiveConstraints);
+						}
+						else
+						{
+							// Kick the step listeners job first
+							JobHandle::sRemoveDependencies(next_step->mStepListeners);
+						}
+					}, 3); // depends on: update soft bodies, contact removed callbacks, finish building the previous step
+			}
+
+			// This job will solve the velocity constraints
+			step.mSolveVelocityConstraints.resize(max_concurrency);
+			for (int i = 0; i < max_concurrency; ++i)
+				step.mSolveVelocityConstraints[i] = inJobSystem->CreateJob("SolveVelocityConstraints", cColorSolveVelocityConstraints, [&context, &step]()
+					{
+						context.mPhysicsSystem->JobSolveVelocityConstraints(&context, &step);
+
+						step.mPreIntegrateVelocity.RemoveDependency();
+					}, num_setup_velocity_constraints_jobs + 2); // depends on: finalize islands, setup velocity constraints, finish building jobs.
+
+			// We prefer setup velocity constraints to finish first so we kick it first
+			JobHandle::sRemoveDependencies(step.mSetupVelocityConstraints);
+			JobHandle::sRemoveDependencies(step.mFindCollisions);
+
+			// Finalize islands is a dependency on find collisions so it can go last
+			step.mFinalizeIslands.RemoveDependency();
+
+			// This job will prepare the position update of all active bodies
+			step.mPreIntegrateVelocity = inJobSystem->CreateJob("PreIntegrateVelocity", cColorPreIntegrateVelocity, [&context, &step]()
+				{
+					context.mPhysicsSystem->JobPreIntegrateVelocity(&context, &step);
+
+					JobHandle::sRemoveDependencies(step.mIntegrateVelocity);
+				}, 2 + max_concurrency); // depends on: broadphase update finalize, solve velocity constraints, finish building jobs.
+
+			// Unblock previous jobs
+			step.mUpdateBroadphaseFinalize.RemoveDependency();
+			JobHandle::sRemoveDependencies(step.mSolveVelocityConstraints);
+
+			// This job will update the positions of all active bodies
+			step.mIntegrateVelocity.resize(num_integrate_velocity_jobs);
+			for (int i = 0; i < num_integrate_velocity_jobs; ++i)
+				step.mIntegrateVelocity[i] = inJobSystem->CreateJob("IntegrateVelocity", cColorIntegrateVelocity, [&context, &step]()
+					{
+						context.mPhysicsSystem->JobIntegrateVelocity(&context, &step);
+
+						step.mPostIntegrateVelocity.RemoveDependency();
+					}, 2); // depends on: pre integrate velocity, finish building jobs.
+
+			// Unblock previous job
+			step.mPreIntegrateVelocity.RemoveDependency();
+
+			// This job will finish the position update of all active bodies
+			step.mPostIntegrateVelocity = inJobSystem->CreateJob("PostIntegrateVelocity", cColorPostIntegrateVelocity, [&context, &step]()
+				{
+					context.mPhysicsSystem->JobPostIntegrateVelocity(&context, &step);
+
+					step.mResolveCCDContacts.RemoveDependency();
+				}, num_integrate_velocity_jobs + 1); // depends on: integrate velocity, finish building jobs
+
+			// Unblock previous jobs
+			JobHandle::sRemoveDependencies(step.mIntegrateVelocity);
+
+			// This job will update the positions and velocities for all bodies that need continuous collision detection
+			step.mResolveCCDContacts = inJobSystem->CreateJob("ResolveCCDContacts", cColorResolveCCDContacts, [&context, &step]()
+				{
+					context.mPhysicsSystem->JobResolveCCDContacts(&context, &step);
+
+					JobHandle::sRemoveDependencies(step.mSolvePositionConstraints);
+				}, 2); // depends on: integrate velocities, detect ccd contacts (added dynamically), finish building jobs.
+
+			// Unblock previous job
+			step.mPostIntegrateVelocity.RemoveDependency();
+
+			// Fixes up drift in positions and updates the broadphase with new body positions
+			step.mSolvePositionConstraints.resize(max_concurrency);
+			for (int i = 0; i < max_concurrency; ++i)
+				step.mSolvePositionConstraints[i] = inJobSystem->CreateJob("SolvePositionConstraints", cColorSolvePositionConstraints, [&context, &step]()
+					{
+						context.mPhysicsSystem->JobSolvePositionConstraints(&context, &step);
+
+						// Kick the next step
+						if (step.mSoftBodyPrepare.IsValid())
+							step.mSoftBodyPrepare.RemoveDependency();
+					}, 3); // depends on: resolve ccd contacts, body set island index, finish building jobs.
+
+			// Unblock previous jobs.
+			step.mResolveCCDContacts.RemoveDependency();
+			step.mBodySetIslandIndex.RemoveDependency();
+
+			// The soft body prepare job will create other jobs if needed
+			step.mSoftBodyPrepare = inJobSystem->CreateJob("SoftBodyPrepare", cColorSoftBodyPrepare, [&context, &step]()
+				{
+					context.mPhysicsSystem->JobSoftBodyPrepare(&context, &step);
+				}, max_concurrency); // depends on: solve position constraints.
+
+			// Unblock previous jobs
+			JobHandle::sRemoveDependencies(step.mSolvePositionConstraints);
+		}
+	}
+
+	// Build the list of jobs to wait for
+	JobSystem::Barrier *barrier = context.mBarrier;
+	{
+		JPH_PROFILE("Build job barrier");
+
+		StaticArray<JobHandle, cMaxPhysicsJobs> handles;
+		for (const PhysicsUpdateContext::Step &step : context.mSteps)
+		{
+			if (step.mBroadPhasePrepare.IsValid())
+				handles.push_back(step.mBroadPhasePrepare);
+			for (const JobHandle &h : step.mStepListeners)
+				handles.push_back(h);
+			for (const JobHandle &h : step.mDetermineActiveConstraints)
+				handles.push_back(h);
+			for (const JobHandle &h : step.mApplyGravity)
+				handles.push_back(h);
+			for (const JobHandle &h : step.mFindCollisions)
+				handles.push_back(h);
+			if (step.mUpdateBroadphaseFinalize.IsValid())
+				handles.push_back(step.mUpdateBroadphaseFinalize);
+			for (const JobHandle &h : step.mSetupVelocityConstraints)
+				handles.push_back(h);
+			handles.push_back(step.mBuildIslandsFromConstraints);
+			handles.push_back(step.mFinalizeIslands);
+			handles.push_back(step.mBodySetIslandIndex);
+			for (const JobHandle &h : step.mSolveVelocityConstraints)
+				handles.push_back(h);
+			handles.push_back(step.mPreIntegrateVelocity);
+			for (const JobHandle &h : step.mIntegrateVelocity)
+				handles.push_back(h);
+			handles.push_back(step.mPostIntegrateVelocity);
+			handles.push_back(step.mResolveCCDContacts);
+			for (const JobHandle &h : step.mSolvePositionConstraints)
+				handles.push_back(h);
+			handles.push_back(step.mContactRemovedCallbacks);
+			if (step.mSoftBodyPrepare.IsValid())
+				handles.push_back(step.mSoftBodyPrepare);
+			if (step.mStartNextStep.IsValid())
+				handles.push_back(step.mStartNextStep);
+		}
+		barrier->AddJobs(handles.data(), handles.size());
+	}
+
+	// Wait until all jobs finish
+	// Note we don't just wait for the last job. If we would and another job
+	// would be scheduled in between there is the possibility of a deadlock.
+	// The other job could try to e.g. add/remove a body which would try to
+	// lock a body mutex while this thread has already locked the mutex
+	inJobSystem->WaitForJobs(barrier);
+
+	// We're done with the barrier for this update
+	inJobSystem->DestroyBarrier(barrier);
+
+#ifdef JPH_DEBUG
+	// Validate that the cached bounds are correct
+	mBodyManager.ValidateActiveBodyBounds();
+#endif // JPH_DEBUG
+
+	// Clear the large island splitter
+	mLargeIslandSplitter.Reset(inTempAllocator);
+
+	// Clear the island builder
+	mIslandBuilder.ResetIslands(inTempAllocator);
+
+	// Clear the contact manager
+	mContactManager.FinishConstraintBuffer();
+
+	// Free active constraints
+	inTempAllocator->Free(context.mActiveConstraints, mConstraintManager.GetNumConstraints() * sizeof(Constraint *));
+	context.mActiveConstraints = nullptr;
+
+	// Free body pairs
+	inTempAllocator->Free(context.mBodyPairs, sizeof(BodyPair) * mPhysicsSettings.mMaxInFlightBodyPairs);
+	context.mBodyPairs = nullptr;
+
+	// Unlock the broadphase
+	mBroadPhase->UnlockModifications();
+
+	// Unlock all constraints
+	mConstraintManager.UnlockAllConstraints();
+
+#ifdef JPH_ENABLE_ASSERTS
+	// Allow write operations to the active bodies list
+	mBodyManager.SetActiveBodiesLocked(false);
+#endif
+
+	// Unlock all bodies
+	mBodyManager.UnlockAllBodies();
+
+	// Unlock step listeners
+	mStepListenersMutex.unlock();
+
+	// Return any errors
+	EPhysicsUpdateError errors = static_cast<EPhysicsUpdateError>(context.mErrors.load(memory_order_acquire));
+	JPH_ASSERT(errors == EPhysicsUpdateError::None, "An error occurred during the physics update, see EPhysicsUpdateError for more information");
+	return errors;
+}
+
+void PhysicsSystem::JobStepListeners(PhysicsUpdateContext::Step *ioStep)
+{
+#ifdef JPH_ENABLE_ASSERTS
+	// Read positions (broadphase updates concurrently so we can't write), read/write velocities
+	BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::Read);
+
+	// Can activate bodies only (we cache the amount of active bodies at the beginning of the step in mNumActiveBodiesAtStepStart so we cannot deactivate here)
+	BodyManager::GrantActiveBodiesAccess grant_active(true, false);
+#endif
+
+	PhysicsStepListenerContext context;
+	context.mDeltaTime = ioStep->mContext->mStepDeltaTime;
+	context.mIsFirstStep = ioStep->mIsFirst;
+	context.mIsLastStep = ioStep->mIsLast;
+	context.mPhysicsSystem = this;
+
+	uint32 batch_size = mPhysicsSettings.mStepListenersBatchSize;
+	for (;;)
+	{
+		// Get the start of a new batch
+		uint32 batch = ioStep->mStepListenerReadIdx.fetch_add(batch_size);
+		if (batch >= mStepListeners.size())
+			break;
+
+		// Call the listeners
+		for (uint32 i = batch, i_end = min((uint32)mStepListeners.size(), batch + batch_size); i < i_end; ++i)
+			mStepListeners[i]->OnStep(context);
+	}
+}
+
+void PhysicsSystem::JobDetermineActiveConstraints(PhysicsUpdateContext::Step *ioStep) const
+{
+#ifdef JPH_ENABLE_ASSERTS
+	// No body access
+	BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::None);
+#endif
+
+	uint32 num_constraints = mConstraintManager.GetNumConstraints();
+	uint32 num_active_constraints;
+	Constraint **active_constraints = (Constraint **)JPH_STACK_ALLOC(cDetermineActiveConstraintsBatchSize * sizeof(Constraint *));
+
+	for (;;)
+	{
+		// Atomically fetch a batch of constraints
+		uint32 constraint_idx = ioStep->mDetermineActiveConstraintReadIdx.fetch_add(cDetermineActiveConstraintsBatchSize);
+		if (constraint_idx >= num_constraints)
+			break;
+
+		// Calculate the end of the batch
+		uint32 constraint_idx_end = min(num_constraints, constraint_idx + cDetermineActiveConstraintsBatchSize);
+
+		// Store the active constraints at the start of the step (bodies get activated during the step which in turn may activate constraints leading to an inconsistent shapshot)
+		mConstraintManager.GetActiveConstraints(constraint_idx, constraint_idx_end, active_constraints, num_active_constraints);
+
+		// Copy the block of active constraints to the global list of active constraints
+		if (num_active_constraints > 0)
+		{
+			uint32 active_constraint_idx = ioStep->mNumActiveConstraints.fetch_add(num_active_constraints);
+			memcpy(ioStep->mContext->mActiveConstraints + active_constraint_idx, active_constraints, num_active_constraints * sizeof(Constraint *));
+		}
+	}
+}
+
+void PhysicsSystem::JobApplyGravity(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep)
+{
+#ifdef JPH_ENABLE_ASSERTS
+	// We update velocities and need the rotation to do so
+	BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::Read);
+#endif
+
+	// Get list of active bodies that we had at the start of the physics update.
+	// Any body that is activated as part of the simulation step does not receive gravity this frame.
+	// Note that bodies may be activated during this job but not deactivated, this means that only elements
+	// will be added to the array. Since the array is made to not reallocate, this is a safe operation.
+	const BodyID *active_bodies = mBodyManager.GetActiveBodiesUnsafe(EBodyType::RigidBody);
+	uint32 num_active_bodies_at_step_start = ioStep->mNumActiveBodiesAtStepStart;
+
+	// Fetch delta time once outside the loop
+	float delta_time = ioContext->mStepDeltaTime;
+
+	// Update velocities from forces
+	for (;;)
+	{
+		// Atomically fetch a batch of bodies
+		uint32 active_body_idx = ioStep->mApplyGravityReadIdx.fetch_add(cApplyGravityBatchSize);
+		if (active_body_idx >= num_active_bodies_at_step_start)
+			break;
+
+		// Calculate the end of the batch
+		uint32 active_body_idx_end = min(num_active_bodies_at_step_start, active_body_idx + cApplyGravityBatchSize);
+
+		// Process the batch
+		while (active_body_idx < active_body_idx_end)
+		{
+			Body &body = mBodyManager.GetBody(active_bodies[active_body_idx]);
+			if (body.IsDynamic())
+			{
+				MotionProperties *mp = body.GetMotionProperties();
+				Quat rotation = body.GetRotation();
+
+				if (body.GetApplyGyroscopicForce())
+					mp->ApplyGyroscopicForceInternal(rotation, delta_time);
+
+				mp->ApplyForceTorqueAndDragInternal(rotation, mGravity, delta_time);
+			}
+			active_body_idx++;
+		}
+	}
+}
+
+void PhysicsSystem::JobSetupVelocityConstraints(float inDeltaTime, PhysicsUpdateContext::Step *ioStep) const
+{
+#ifdef JPH_ENABLE_ASSERTS
+	// We only read positions
+	BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::Read);
+#endif
+
+	uint32 num_constraints = ioStep->mNumActiveConstraints;
+
+	for (;;)
+	{
+		// Atomically fetch a batch of constraints
+		uint32 constraint_idx = ioStep->mSetupVelocityConstraintsReadIdx.fetch_add(cSetupVelocityConstraintsBatchSize);
+		if (constraint_idx >= num_constraints)
+			break;
+
+		ConstraintManager::sSetupVelocityConstraints(ioStep->mContext->mActiveConstraints + constraint_idx, min<uint32>(cSetupVelocityConstraintsBatchSize, num_constraints - constraint_idx), inDeltaTime);
+	}
+}
+
+void PhysicsSystem::JobBuildIslandsFromConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep)
+{
+#ifdef JPH_ENABLE_ASSERTS
+	// We read constraints and positions
+	BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::Read);
+
+	// Can only activate bodies
+	BodyManager::GrantActiveBodiesAccess grant_active(true, false);
+#endif
+
+	// Prepare the island builder
+	mIslandBuilder.PrepareNonContactConstraints(ioStep->mNumActiveConstraints, ioContext->mTempAllocator);
+
+	// Build the islands
+	ConstraintManager::sBuildIslands(ioStep->mContext->mActiveConstraints, ioStep->mNumActiveConstraints, mIslandBuilder, mBodyManager);
+}
+
+void PhysicsSystem::TrySpawnJobFindCollisions(PhysicsUpdateContext::Step *ioStep) const
+{
+	// Get how many jobs we can spawn and check if we can spawn more
+	uint max_jobs = ioStep->mBodyPairQueues.size();
+	if (CountBits(ioStep->mActiveFindCollisionJobs.load(memory_order_relaxed)) >= max_jobs)
+		return;
+
+	// Count how many body pairs we have waiting
+	uint32 num_body_pairs = 0;
+	for (const PhysicsUpdateContext::BodyPairQueue &queue : ioStep->mBodyPairQueues)
+		num_body_pairs += queue.mWriteIdx - queue.mReadIdx;
+
+	// Count how many active bodies we have waiting
+	uint32 num_active_bodies = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody) - ioStep->mActiveBodyReadIdx;
+
+	// Calculate how many jobs we would like
+	uint desired_num_jobs = min((num_body_pairs + cNarrowPhaseBatchSize - 1) / cNarrowPhaseBatchSize + (num_active_bodies + cActiveBodiesBatchSize - 1) / cActiveBodiesBatchSize, max_jobs);
+
+	for (;;)
+	{
+		// Get the bit mask of active jobs and see if we can spawn more
+		PhysicsUpdateContext::JobMask current_active_jobs = ioStep->mActiveFindCollisionJobs.load(memory_order_relaxed);
+		uint job_index = CountTrailingZeros(~current_active_jobs);
+		if (job_index >= desired_num_jobs)
+			break;
+
+		// Try to claim the job index
+		PhysicsUpdateContext::JobMask job_mask = PhysicsUpdateContext::JobMask(1) << job_index;
+		PhysicsUpdateContext::JobMask prev_value = ioStep->mActiveFindCollisionJobs.fetch_or(job_mask, memory_order_acquire);
+		if ((prev_value & job_mask) == 0)
+		{
+			// Add dependencies from the find collisions job to the next jobs
+			ioStep->mUpdateBroadphaseFinalize.AddDependency();
+			ioStep->mFinalizeIslands.AddDependency();
+
+			// Start the job
+			JobHandle job = ioStep->mContext->mJobSystem->CreateJob("FindCollisions", cColorFindCollisions, [step = ioStep, job_index]()
+				{
+					step->mContext->mPhysicsSystem->JobFindCollisions(step, job_index);
+				});
+
+			// Add the job to the job barrier so the main updating thread can execute the job too
+			ioStep->mContext->mBarrier->AddJob(job);
+
+			// Spawn only 1 extra job at a time
+			return;
+		}
+	}
+}
+
+static void sFinalizeContactAllocator(PhysicsUpdateContext::Step &ioStep, const ContactConstraintManager::ContactAllocator &inAllocator)
+{
+	// Atomically accumulate the number of found manifolds and body pairs
+	ioStep.mNumBodyPairs.fetch_add(inAllocator.mNumBodyPairs, memory_order_relaxed);
+	ioStep.mNumManifolds.fetch_add(inAllocator.mNumManifolds, memory_order_relaxed);
+
+	// Combine update errors
+	ioStep.mContext->mErrors.fetch_or((uint32)inAllocator.mErrors, memory_order_relaxed);
+}
+
+// Disable TSAN for this function. It detects a false positive race condition on mBodyPairs.
+// We have written mBodyPairs before doing mWriteIdx++ and we check mWriteIdx before reading mBodyPairs, so this should be safe.
+JPH_TSAN_NO_SANITIZE
+void PhysicsSystem::JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int inJobIndex)
+{
+#ifdef JPH_ENABLE_ASSERTS
+	// We read positions and read velocities (for elastic collisions)
+	BodyAccess::Grant grant(BodyAccess::EAccess::Read, BodyAccess::EAccess::Read);
+
+	// Can only activate bodies
+	BodyManager::GrantActiveBodiesAccess grant_active(true, false);
+#endif
+
+	// Allocation context for allocating new contact points
+	ContactAllocator contact_allocator(mContactManager.GetContactAllocator());
+
+	// Determine initial queue to read pairs from if no broadphase work can be done
+	// (always start looking at results from the next job)
+	int read_queue_idx = (inJobIndex + 1) % ioStep->mBodyPairQueues.size();
+
+	for (;;)
+	{
+		// Check if there are active bodies to be processed
+		uint32 active_bodies_read_idx = ioStep->mActiveBodyReadIdx;
+		uint32 num_active_bodies = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody);
+		if (active_bodies_read_idx < num_active_bodies)
+		{
+			// Take a batch of active bodies
+			uint32 active_bodies_read_idx_end = min(num_active_bodies, active_bodies_read_idx + cActiveBodiesBatchSize);
+			if (ioStep->mActiveBodyReadIdx.compare_exchange_strong(active_bodies_read_idx, active_bodies_read_idx_end))
+			{
+				// Callback when a new body pair is found
+				class MyBodyPairCallback : public BodyPairCollector
+				{
+				public:
+					// Constructor
+											MyBodyPairCallback(PhysicsUpdateContext::Step *inStep, ContactAllocator &ioContactAllocator, int inJobIndex) :
+						mStep(inStep),
+						mContactAllocator(ioContactAllocator),
+						mJobIndex(inJobIndex)
+					{
+					}
+
+					// Callback function when a body pair is found
+					virtual void			AddHit(const BodyPair &inPair) override
+					{
+						// Check if we have space in our write queue
+						PhysicsUpdateContext::BodyPairQueue &queue = mStep->mBodyPairQueues[mJobIndex];
+						uint32 body_pairs_in_queue = queue.mWriteIdx - queue.mReadIdx;
+						if (body_pairs_in_queue >= mStep->mMaxBodyPairsPerQueue)
+						{
+							// Buffer full, process the pair now
+							mStep->mContext->mPhysicsSystem->ProcessBodyPair(mContactAllocator, inPair);
+						}
+						else
+						{
+							// Store the pair in our own queue
+							mStep->mContext->mBodyPairs[mJobIndex * mStep->mMaxBodyPairsPerQueue + queue.mWriteIdx % mStep->mMaxBodyPairsPerQueue] = inPair;
+							++queue.mWriteIdx;
+						}
+					}
+
+				private:
+					PhysicsUpdateContext::Step *	mStep;
+					ContactAllocator &				mContactAllocator;
+					int								mJobIndex;
+				};
+				MyBodyPairCallback add_pair(ioStep, contact_allocator, inJobIndex);
+
+				// Copy active bodies to temporary array, broadphase will reorder them
+				uint32 batch_size = active_bodies_read_idx_end - active_bodies_read_idx;
+				BodyID *active_bodies = (BodyID *)JPH_STACK_ALLOC(batch_size * sizeof(BodyID));
+				memcpy(active_bodies, mBodyManager.GetActiveBodiesUnsafe(EBodyType::RigidBody) + active_bodies_read_idx, batch_size * sizeof(BodyID));
+
+				// Find pairs in the broadphase
+				mBroadPhase->FindCollidingPairs(active_bodies, batch_size, mPhysicsSettings.mSpeculativeContactDistance, *mObjectVsBroadPhaseLayerFilter, *mObjectLayerPairFilter, add_pair);
+
+				// Check if we have enough pairs in the buffer to start a new job
+				const PhysicsUpdateContext::BodyPairQueue &queue = ioStep->mBodyPairQueues[inJobIndex];
+				uint32 body_pairs_in_queue = queue.mWriteIdx - queue.mReadIdx;
+				if (body_pairs_in_queue >= cNarrowPhaseBatchSize)
+					TrySpawnJobFindCollisions(ioStep);
+			}
+		}
+		else
+		{
+			// Lockless loop to get the next body pair from the pairs buffer
+			const PhysicsUpdateContext *context = ioStep->mContext;
+			int first_read_queue_idx = read_queue_idx;
+			for (;;)
+			{
+				PhysicsUpdateContext::BodyPairQueue &queue = ioStep->mBodyPairQueues[read_queue_idx];
+
+				// Get the next pair to process
+				uint32 pair_idx = queue.mReadIdx;
+
+				// If the pair hasn't been written yet
+				if (pair_idx >= queue.mWriteIdx)
+				{
+					// Go to the next queue
+					read_queue_idx = (read_queue_idx + 1) % ioStep->mBodyPairQueues.size();
+
+					// If we're back at the first queue, we've looked at all of them and found nothing
+					if (read_queue_idx == first_read_queue_idx)
+					{
+						// Collect information from the contact allocator and accumulate it in the step.
+						sFinalizeContactAllocator(*ioStep, contact_allocator);
+
+						// Mark this job as inactive
+						ioStep->mActiveFindCollisionJobs.fetch_and(~PhysicsUpdateContext::JobMask(1 << inJobIndex), memory_order_release);
+
+						// Trigger the next jobs
+						ioStep->mUpdateBroadphaseFinalize.RemoveDependency();
+						ioStep->mFinalizeIslands.RemoveDependency();
+						return;
+					}
+
+					// Try again reading from the next queue
+					continue;
+				}
+
+				// Copy the body pair out of the buffer
+				const BodyPair bp = context->mBodyPairs[read_queue_idx * ioStep->mMaxBodyPairsPerQueue + pair_idx % ioStep->mMaxBodyPairsPerQueue];
+
+				// Mark this pair as taken
+				if (queue.mReadIdx.compare_exchange_strong(pair_idx, pair_idx + 1))
+				{
+					// Process the actual body pair
+					ProcessBodyPair(contact_allocator, bp);
+					break;
+				}
+			}
+		}
+	}
+}
+
+void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const BodyPair &inBodyPair)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Fetch body pair
+	Body *body1 = &mBodyManager.GetBody(inBodyPair.mBodyA);
+	Body *body2 = &mBodyManager.GetBody(inBodyPair.mBodyB);
+	JPH_ASSERT(body1->IsActive());
+
+	JPH_DET_LOG("ProcessBodyPair: id1: " << inBodyPair.mBodyA << " id2: " << inBodyPair.mBodyB << " p1: " << body1->GetCenterOfMassPosition() << " p2: " << body2->GetCenterOfMassPosition() << " r1: " << body1->GetRotation() << " r2: " << body2->GetRotation());
+
+	// Check for soft bodies
+	if (body2->IsSoftBody())
+	{
+		// If the 2nd body is a soft body and not active, we activate it now
+		if (!body2->IsActive())
+			mBodyManager.ActivateBodies(&inBodyPair.mBodyB, 1);
+
+		// Soft body processing is done later in the pipeline
+		return;
+	}
+
+	// Ensure that body1 has the higher motion type (i.e. dynamic trumps kinematic), this ensures that we do the collision detection in the space of a moving body,
+	// which avoids accuracy problems when testing a very large static object against a small dynamic object
+	// Ensure that body1 id < body2 id when motion types are the same.
+	if (body1->GetMotionType() < body2->GetMotionType()
+		|| (body1->GetMotionType() == body2->GetMotionType() && inBodyPair.mBodyB < inBodyPair.mBodyA))
+		std::swap(body1, body2);
+
+	// Check if the contact points from the previous frame are reusable and if so copy them
+	bool pair_handled = false, constraint_created = false;
+	if (mPhysicsSettings.mUseBodyPairContactCache && !(body1->IsCollisionCacheInvalid() || body2->IsCollisionCacheInvalid()))
+		mContactManager.GetContactsFromCache(ioContactAllocator, *body1, *body2, pair_handled, constraint_created);
+
+	// If the cache hasn't handled this body pair do actual collision detection
+	if (!pair_handled)
+	{
+		// Create entry in the cache for this body pair
+		// Needs to happen irrespective if we found a collision or not (we want to remember that no collision was found too)
+		ContactConstraintManager::BodyPairHandle body_pair_handle = mContactManager.AddBodyPair(ioContactAllocator, *body1, *body2);
+		if (body_pair_handle == nullptr)
+			return; // Out of cache space
+
+		// If we want enhanced active edge detection for this body pair
+		bool enhanced_active_edges = body1->GetEnhancedInternalEdgeRemovalWithBody(*body2);
+
+		// Create the query settings
+		CollideShapeSettings settings;
+		settings.mCollectFacesMode = ECollectFacesMode::CollectFaces;
+		settings.mActiveEdgeMode = mPhysicsSettings.mCheckActiveEdges && !enhanced_active_edges? EActiveEdgeMode::CollideOnlyWithActive : EActiveEdgeMode::CollideWithAll;
+		settings.mMaxSeparationDistance = body1->IsSensor() || body2->IsSensor()? 0.0f : mPhysicsSettings.mSpeculativeContactDistance;
+		settings.mActiveEdgeMovementDirection = body1->GetLinearVelocity() - body2->GetLinearVelocity();
+
+		// Create shape filter
+		SimShapeFilterWrapperUnion shape_filter_union(mSimShapeFilter, body1);
+		SimShapeFilterWrapper &shape_filter = shape_filter_union.GetSimShapeFilterWrapper();
+		shape_filter.SetBody2(body2);
+
+		// Get transforms relative to body1
+		RVec3 offset = body1->GetCenterOfMassPosition();
+		Mat44 transform1 = Mat44::sRotation(body1->GetRotation());
+		Mat44 transform2 = body2->GetCenterOfMassTransform().PostTranslated(-offset).ToMat44();
+
+		if (mPhysicsSettings.mUseManifoldReduction				// Check global flag
+			&& body1->GetUseManifoldReductionWithBody(*body2))	// Check body flag
+		{
+			// Version WITH contact manifold reduction
+
+			class MyManifold : public ContactManifold
+			{
+			public:
+				Vec3				mFirstWorldSpaceNormal;
+			};
+
+			// A temporary structure that allows us to keep track of the all manifolds between this body pair
+			using Manifolds = StaticArray<MyManifold, 32>;
+
+			// Create collector
+			class ReductionCollideShapeCollector : public CollideShapeCollector
+			{
+			public:
+								ReductionCollideShapeCollector(PhysicsSystem *inSystem, const Body *inBody1, const Body *inBody2) :
+					mSystem(inSystem),
+					mBody1(inBody1),
+					mBody2(inBody2)
+				{
+				}
+
+				virtual void	AddHit(const CollideShapeResult &inResult) override
+				{
+					// The first body should be the one with the highest motion type
+					JPH_ASSERT(mBody1->GetMotionType() >= mBody2->GetMotionType());
+					JPH_ASSERT(!ShouldEarlyOut());
+
+					// Test if we want to accept this hit
+					if (mValidateBodyPair)
+					{
+						switch (mSystem->mContactManager.ValidateContactPoint(*mBody1, *mBody2, mBody1->GetCenterOfMassPosition(), inResult))
+						{
+						case ValidateResult::AcceptContact:
+							// We're just accepting this one, nothing to do
+							break;
+
+						case ValidateResult::AcceptAllContactsForThisBodyPair:
+							// Accept and stop calling the validate callback
+							mValidateBodyPair = false;
+							break;
+
+						case ValidateResult::RejectContact:
+							// Skip this contact
+							return;
+
+						case ValidateResult::RejectAllContactsForThisBodyPair:
+							// Skip this and early out
+							ForceEarlyOut();
+							return;
+						}
+					}
+
+					// Calculate normal
+					Vec3 world_space_normal = inResult.mPenetrationAxis.Normalized();
+
+					// Check if we can add it to an existing manifold
+					Manifolds::iterator manifold;
+					float contact_normal_cos_max_delta_rot = mSystem->mPhysicsSettings.mContactNormalCosMaxDeltaRotation;
+					for (manifold = mManifolds.begin(); manifold != mManifolds.end(); ++manifold)
+						if (world_space_normal.Dot(manifold->mFirstWorldSpaceNormal) >= contact_normal_cos_max_delta_rot)
+						{
+							// Update average normal
+							manifold->mWorldSpaceNormal += world_space_normal;
+							manifold->mPenetrationDepth = max(manifold->mPenetrationDepth, inResult.mPenetrationDepth);
+							break;
+						}
+					if (manifold == mManifolds.end())
+					{
+						// Check if array is full
+						if (mManifolds.size() == mManifolds.capacity())
+						{
+							// Full, find manifold with least amount of penetration
+							manifold = mManifolds.begin();
+							for (Manifolds::iterator m = mManifolds.begin() + 1; m < mManifolds.end(); ++m)
+								if (m->mPenetrationDepth < manifold->mPenetrationDepth)
+									manifold = m;
+
+							// If this contacts penetration is smaller than the smallest manifold, we skip this contact
+							if (inResult.mPenetrationDepth < manifold->mPenetrationDepth)
+								return;
+
+							// Replace the manifold
+							*manifold = { { mBody1->GetCenterOfMassPosition(), world_space_normal, inResult.mPenetrationDepth, inResult.mSubShapeID1, inResult.mSubShapeID2, { }, { } }, world_space_normal };
+						}
+						else
+						{
+							// Not full, create new manifold
+							mManifolds.push_back({ { mBody1->GetCenterOfMassPosition(), world_space_normal, inResult.mPenetrationDepth, inResult.mSubShapeID1, inResult.mSubShapeID2, { }, { } }, world_space_normal });
+							manifold = mManifolds.end() - 1;
+						}
+					}
+
+					// Determine contact points
+					const PhysicsSettings &settings = mSystem->mPhysicsSettings;
+					ManifoldBetweenTwoFaces(inResult.mContactPointOn1, inResult.mContactPointOn2, inResult.mPenetrationAxis, Square(settings.mSpeculativeContactDistance) + settings.mManifoldToleranceSq, inResult.mShape1Face, inResult.mShape2Face, manifold->mRelativeContactPointsOn1, manifold->mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, mBody1->GetCenterOfMassPosition()));
+
+					// Prune if we have more than 32 points (this means we could run out of space in the next iteration)
+					if (manifold->mRelativeContactPointsOn1.size() > 32)
+						PruneContactPoints(manifold->mFirstWorldSpaceNormal, manifold->mRelativeContactPointsOn1, manifold->mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold->mBaseOffset));
+				}
+
+				PhysicsSystem *		mSystem;
+				const Body *		mBody1;
+				const Body *		mBody2;
+				bool				mValidateBodyPair = true;
+				Manifolds			mManifolds;
+			};
+			ReductionCollideShapeCollector collector(this, body1, body2);
+
+			// Perform collision detection between the two shapes
+			SubShapeIDCreator part1, part2;
+			auto f = enhanced_active_edges? InternalEdgeRemovingCollector::sCollideShapeVsShape : CollisionDispatch::sCollideShapeVsShape;
+			f(body1->GetShape(), body2->GetShape(), Vec3::sReplicate(1.0f), Vec3::sReplicate(1.0f), transform1, transform2, part1, part2, settings, collector, shape_filter);
+
+			// Add the contacts
+			for (ContactManifold &manifold : collector.mManifolds)
+			{
+				// Normalize the normal (is a sum of all normals from merged manifolds)
+				manifold.mWorldSpaceNormal = manifold.mWorldSpaceNormal.Normalized();
+
+				// If we still have too many points, prune them now
+				if (manifold.mRelativeContactPointsOn1.size() > 4)
+					PruneContactPoints(manifold.mWorldSpaceNormal, manifold.mRelativeContactPointsOn1, manifold.mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold.mBaseOffset));
+
+				// Actually add the contact points to the manager
+				constraint_created |= mContactManager.AddContactConstraint(ioContactAllocator, body_pair_handle, *body1, *body2, manifold);
+			}
+		}
+		else
+		{
+			// Version WITHOUT contact manifold reduction
+
+			// Create collector
+			class NonReductionCollideShapeCollector : public CollideShapeCollector
+			{
+			public:
+								NonReductionCollideShapeCollector(PhysicsSystem *inSystem, ContactAllocator &ioContactAllocator, Body *inBody1, Body *inBody2, const ContactConstraintManager::BodyPairHandle &inPairHandle) :
+					mSystem(inSystem),
+					mContactAllocator(ioContactAllocator),
+					mBody1(inBody1),
+					mBody2(inBody2),
+					mBodyPairHandle(inPairHandle)
+				{
+				}
+
+				virtual void	AddHit(const CollideShapeResult &inResult) override
+				{
+					// The first body should be the one with the highest motion type
+					JPH_ASSERT(mBody1->GetMotionType() >= mBody2->GetMotionType());
+					JPH_ASSERT(!ShouldEarlyOut());
+
+					// Test if we want to accept this hit
+					if (mValidateBodyPair)
+					{
+						switch (mSystem->mContactManager.ValidateContactPoint(*mBody1, *mBody2, mBody1->GetCenterOfMassPosition(), inResult))
+						{
+						case ValidateResult::AcceptContact:
+							// We're just accepting this one, nothing to do
+							break;
+
+						case ValidateResult::AcceptAllContactsForThisBodyPair:
+							// Accept and stop calling the validate callback
+							mValidateBodyPair = false;
+							break;
+
+						case ValidateResult::RejectContact:
+							// Skip this contact
+							return;
+
+						case ValidateResult::RejectAllContactsForThisBodyPair:
+							// Skip this and early out
+							ForceEarlyOut();
+							return;
+						}
+					}
+
+					// Determine contact points
+					ContactManifold manifold;
+					manifold.mBaseOffset = mBody1->GetCenterOfMassPosition();
+					const PhysicsSettings &settings = mSystem->mPhysicsSettings;
+					ManifoldBetweenTwoFaces(inResult.mContactPointOn1, inResult.mContactPointOn2, inResult.mPenetrationAxis, Square(settings.mSpeculativeContactDistance) + settings.mManifoldToleranceSq, inResult.mShape1Face, inResult.mShape2Face, manifold.mRelativeContactPointsOn1, manifold.mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold.mBaseOffset));
+
+					// Calculate normal
+					manifold.mWorldSpaceNormal = inResult.mPenetrationAxis.Normalized();
+
+					// Store penetration depth
+					manifold.mPenetrationDepth = inResult.mPenetrationDepth;
+
+					// Prune if we have more than 4 points
+					if (manifold.mRelativeContactPointsOn1.size() > 4)
+						PruneContactPoints(manifold.mWorldSpaceNormal, manifold.mRelativeContactPointsOn1, manifold.mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold.mBaseOffset));
+
+					// Set other properties
+					manifold.mSubShapeID1 = inResult.mSubShapeID1;
+					manifold.mSubShapeID2 = inResult.mSubShapeID2;
+
+					// Actually add the contact points to the manager
+					mConstraintCreated |= mSystem->mContactManager.AddContactConstraint(mContactAllocator, mBodyPairHandle, *mBody1, *mBody2, manifold);
+				}
+
+				PhysicsSystem *		mSystem;
+				ContactAllocator &	mContactAllocator;
+				Body *				mBody1;
+				Body *				mBody2;
+				ContactConstraintManager::BodyPairHandle mBodyPairHandle;
+				bool				mValidateBodyPair = true;
+				bool				mConstraintCreated = false;
+			};
+			NonReductionCollideShapeCollector collector(this, ioContactAllocator, body1, body2, body_pair_handle);
+
+			// Perform collision detection between the two shapes
+			SubShapeIDCreator part1, part2;
+			auto f = enhanced_active_edges? InternalEdgeRemovingCollector::sCollideShapeVsShape : CollisionDispatch::sCollideShapeVsShape;
+			f(body1->GetShape(), body2->GetShape(), Vec3::sReplicate(1.0f), Vec3::sReplicate(1.0f), transform1, transform2, part1, part2, settings, collector, shape_filter);
+
+			constraint_created = collector.mConstraintCreated;
+		}
+	}
+
+	// If a contact constraint was created, we need to do some extra work
+	if (constraint_created)
+	{
+		// Wake up sleeping bodies
+		BodyID body_ids[2];
+		int num_bodies = 0;
+		if (body1->IsDynamic() && !body1->IsActive())
+			body_ids[num_bodies++] = body1->GetID();
+		if (body2->IsDynamic() && !body2->IsActive())
+			body_ids[num_bodies++] = body2->GetID();
+		if (num_bodies > 0)
+			mBodyManager.ActivateBodies(body_ids, num_bodies);
+
+		// Link the two bodies
+		mIslandBuilder.LinkBodies(body1->GetIndexInActiveBodiesInternal(), body2->GetIndexInActiveBodiesInternal());
+	}
+}
+
+void PhysicsSystem::JobFinalizeIslands(PhysicsUpdateContext *ioContext)
+{
+#ifdef JPH_ENABLE_ASSERTS
+	// We only touch island data
+	BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::None);
+#endif
+
+	// Finish collecting the islands, at this point the active body list doesn't change so it's safe to access
+	mIslandBuilder.Finalize(mBodyManager.GetActiveBodiesUnsafe(EBodyType::RigidBody), mBodyManager.GetNumActiveBodies(EBodyType::RigidBody), mContactManager.GetNumConstraints(), ioContext->mTempAllocator);
+
+	// Prepare the large island splitter
+	if (mPhysicsSettings.mUseLargeIslandSplitter)
+		mLargeIslandSplitter.Prepare(mIslandBuilder, mBodyManager.GetNumActiveBodies(EBodyType::RigidBody), ioContext->mTempAllocator);
+}
+
+void PhysicsSystem::JobBodySetIslandIndex()
+{
+#ifdef JPH_ENABLE_ASSERTS
+	// We only touch island data
+	BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::None);
+#endif
+
+	// Loop through the result and tag all bodies with an island index
+	for (uint32 island_idx = 0, n = mIslandBuilder.GetNumIslands(); island_idx < n; ++island_idx)
+	{
+		BodyID *body_start, *body_end;
+		mIslandBuilder.GetBodiesInIsland(island_idx, body_start, body_end);
+		for (const BodyID *body = body_start; body < body_end; ++body)
+			mBodyManager.GetBody(*body).GetMotionProperties()->SetIslandIndexInternal(island_idx);
+	}
+}
+
+JPH_SUPPRESS_WARNING_PUSH
+JPH_CLANG_SUPPRESS_WARNING("-Wundefined-func-template") // ConstraintManager::sWarmStartVelocityConstraints / ContactConstraintManager::WarmStartVelocityConstraints is instantiated in the cpp file
+
+void PhysicsSystem::JobSolveVelocityConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep)
+{
+#ifdef JPH_ENABLE_ASSERTS
+	// We update velocities and need to read positions to do so
+	BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::Read);
+#endif
+
+	float delta_time = ioContext->mStepDeltaTime;
+	Constraint **active_constraints = ioContext->mActiveConstraints;
+
+	// Only the first step to correct for the delta time difference in the previous update
+	float warm_start_impulse_ratio = ioStep->mIsFirst? ioContext->mWarmStartImpulseRatio : 1.0f;
+
+	bool check_islands = true, check_split_islands = mPhysicsSettings.mUseLargeIslandSplitter;
+	for (;;)
+	{
+		// First try to get work from large islands
+		if (check_split_islands)
+		{
+			bool first_iteration;
+			uint split_island_index;
+			uint32 *constraints_begin, *constraints_end, *contacts_begin, *contacts_end;
+			switch (mLargeIslandSplitter.FetchNextBatch(split_island_index, constraints_begin, constraints_end, contacts_begin, contacts_end, first_iteration))
+			{
+			case LargeIslandSplitter::EStatus::BatchRetrieved:
+				{
+					if (first_iteration)
+					{
+						// Iteration 0 is used to warm start the batch (we added 1 to the number of iterations in LargeIslandSplitter::SplitIsland)
+						DummyCalculateSolverSteps dummy;
+						ConstraintManager::sWarmStartVelocityConstraints(active_constraints, constraints_begin, constraints_end, warm_start_impulse_ratio, dummy);
+						mContactManager.WarmStartVelocityConstraints(contacts_begin, contacts_end, warm_start_impulse_ratio, dummy);
+					}
+					else
+					{
+						// Solve velocity constraints
+						ConstraintManager::sSolveVelocityConstraints(active_constraints, constraints_begin, constraints_end, delta_time);
+						mContactManager.SolveVelocityConstraints(contacts_begin, contacts_end);
+					}
+
+					// Mark the batch as processed
+					bool last_iteration, final_batch;
+					mLargeIslandSplitter.MarkBatchProcessed(split_island_index, constraints_begin, constraints_end, contacts_begin, contacts_end, last_iteration, final_batch);
+
+					// Save back the lambdas in the contact cache for the warm start of the next physics update
+					if (last_iteration)
+						mContactManager.StoreAppliedImpulses(contacts_begin, contacts_end);
+
+					// We processed work, loop again
+					continue;
+				}
+
+			case LargeIslandSplitter::EStatus::WaitingForBatch:
+				break;
+
+			case LargeIslandSplitter::EStatus::AllBatchesDone:
+				check_split_islands = false;
+				break;
+			}
+		}
+
+		// If that didn't succeed try to process an island
+		if (check_islands)
+		{
+			// Next island
+			uint32 island_idx = ioStep->mSolveVelocityConstraintsNextIsland++;
+			if (island_idx >= mIslandBuilder.GetNumIslands())
+			{
+				// We processed all islands, stop checking islands
+				check_islands = false;
+				continue;
+			}
+
+			JPH_PROFILE("Island");
+
+			// Get iterators for this island
+			uint32 *constraints_begin, *constraints_end, *contacts_begin, *contacts_end;
+			bool has_constraints = mIslandBuilder.GetConstraintsInIsland(island_idx, constraints_begin, constraints_end);
+			bool has_contacts = mIslandBuilder.GetContactsInIsland(island_idx, contacts_begin, contacts_end);
+
+			// If we don't have any contacts or constraints, we know that none of the following islands have any contacts or constraints
+			// (because they're sorted by most constraints first). This means we're done.
+			if (!has_contacts && !has_constraints)
+			{
+			#ifdef JPH_ENABLE_ASSERTS
+				// Validate our assumption that the next islands don't have any constraints or contacts
+				for (; island_idx < mIslandBuilder.GetNumIslands(); ++island_idx)
+				{
+					JPH_ASSERT(!mIslandBuilder.GetConstraintsInIsland(island_idx, constraints_begin, constraints_end));
+					JPH_ASSERT(!mIslandBuilder.GetContactsInIsland(island_idx, contacts_begin, contacts_end));
+				}
+			#endif // JPH_ENABLE_ASSERTS
+
+				check_islands = false;
+				continue;
+			}
+
+			// Sorting is costly but needed for a deterministic simulation, allow the user to turn this off
+			if (mPhysicsSettings.mDeterministicSimulation)
+			{
+				// Sort constraints to give a deterministic simulation
+				ConstraintManager::sSortConstraints(active_constraints, constraints_begin, constraints_end);
+
+				// Sort contacts to give a deterministic simulation
+				mContactManager.SortContacts(contacts_begin, contacts_end);
+			}
+
+			// Split up large islands
+			CalculateSolverSteps steps_calculator(mPhysicsSettings);
+			if (mPhysicsSettings.mUseLargeIslandSplitter
+				&& mLargeIslandSplitter.SplitIsland(island_idx, mIslandBuilder, mBodyManager, mContactManager, active_constraints, steps_calculator))
+				continue; // Loop again to try to fetch the newly split island
+
+			// We didn't create a split, just run the solver now for this entire island. Begin by warm starting.
+			ConstraintManager::sWarmStartVelocityConstraints(active_constraints, constraints_begin, constraints_end, warm_start_impulse_ratio, steps_calculator);
+			mContactManager.WarmStartVelocityConstraints(contacts_begin, contacts_end, warm_start_impulse_ratio, steps_calculator);
+			steps_calculator.Finalize();
+
+			// Store the number of position steps for later
+			mIslandBuilder.SetNumPositionSteps(island_idx, steps_calculator.GetNumPositionSteps());
+
+			// Solve velocity constraints
+			for (uint velocity_step = 0; velocity_step < steps_calculator.GetNumVelocitySteps(); ++velocity_step)
+			{
+				bool applied_impulse = ConstraintManager::sSolveVelocityConstraints(active_constraints, constraints_begin, constraints_end, delta_time);
+				applied_impulse |= mContactManager.SolveVelocityConstraints(contacts_begin, contacts_end);
+				if (!applied_impulse)
+					break;
+			}
+
+			// Save back the lambdas in the contact cache for the warm start of the next physics update
+			mContactManager.StoreAppliedImpulses(contacts_begin, contacts_end);
+
+			// We processed work, loop again
+			continue;
+		}
+
+		if (check_islands)
+		{
+			// If there are islands, we don't need to wait and can pick up new work
+			continue;
+		}
+		else if (check_split_islands)
+		{
+			// If there are split islands, but we did't do any work, give up a time slice
+			std::this_thread::yield();
+		}
+		else
+		{
+			// No more work
+			break;
+		}
+	}
+}
+
+JPH_SUPPRESS_WARNING_POP
+
+void PhysicsSystem::JobPreIntegrateVelocity(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep)
+{
+	// Reserve enough space for all bodies that may need a cast
+	TempAllocator *temp_allocator = ioContext->mTempAllocator;
+	JPH_ASSERT(ioStep->mCCDBodies == nullptr);
+	ioStep->mCCDBodiesCapacity = mBodyManager.GetNumActiveCCDBodies();
+	ioStep->mCCDBodies = (CCDBody *)temp_allocator->Allocate(ioStep->mCCDBodiesCapacity * sizeof(CCDBody));
+
+	// Initialize the mapping table between active body and CCD body
+	JPH_ASSERT(ioStep->mActiveBodyToCCDBody == nullptr);
+	ioStep->mNumActiveBodyToCCDBody = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody);
+	ioStep->mActiveBodyToCCDBody = (int *)temp_allocator->Allocate(ioStep->mNumActiveBodyToCCDBody * sizeof(int));
+
+	// Prepare the split island builder for solving the position constraints
+	mLargeIslandSplitter.PrepareForSolvePositions();
+}
+
+void PhysicsSystem::JobIntegrateVelocity(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep)
+{
+#ifdef JPH_ENABLE_ASSERTS
+	// We update positions and need velocity to do so, we also clamp velocities so need to write to them
+	BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::ReadWrite);
+#endif
+
+	float delta_time = ioContext->mStepDeltaTime;
+	const BodyID *active_bodies = mBodyManager.GetActiveBodiesUnsafe(EBodyType::RigidBody);
+	uint32 num_active_bodies = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody);
+	uint32 num_active_bodies_after_find_collisions = ioStep->mActiveBodyReadIdx;
+
+	// We can move bodies that are not part of an island. In this case we need to notify the broadphase of the movement.
+	static constexpr int cBodiesBatch = 64;
+	BodyID *bodies_to_update_bounds = (BodyID *)JPH_STACK_ALLOC(cBodiesBatch * sizeof(BodyID));
+	int num_bodies_to_update_bounds = 0;
+
+	for (;;)
+	{
+		// Atomically fetch a batch of bodies
+		uint32 active_body_idx = ioStep->mIntegrateVelocityReadIdx.fetch_add(cIntegrateVelocityBatchSize);
+		if (active_body_idx >= num_active_bodies)
+			break;
+
+		// Calculate the end of the batch
+		uint32 active_body_idx_end = min(num_active_bodies, active_body_idx + cIntegrateVelocityBatchSize);
+
+		// Process the batch
+		while (active_body_idx < active_body_idx_end)
+		{
+			// Update the positions using an Symplectic Euler step (which integrates using the updated velocity v1' rather
+			// than the original velocity v1):
+			// x1' = x1 + h * v1'
+			// At this point the active bodies list does not change, so it is safe to access the array.
+			BodyID body_id = active_bodies[active_body_idx];
+			Body &body = mBodyManager.GetBody(body_id);
+			MotionProperties *mp = body.GetMotionProperties();
+
+			JPH_DET_LOG("JobIntegrateVelocity: id: " << body_id << " v: " << body.GetLinearVelocity() << " w: " << body.GetAngularVelocity());
+
+			// Clamp velocities (not for kinematic bodies)
+			if (body.IsDynamic())
+			{
+				mp->ClampLinearVelocity();
+				mp->ClampAngularVelocity();
+			}
+
+			// Update the rotation of the body according to the angular velocity
+			// For motion type discrete we need to do this anyway, for motion type linear cast we have multiple choices
+			// 1. Rotate the body first and then sweep
+			// 2. First sweep and then rotate the body at the end
+			// 3. Pick some in between rotation (e.g. half way), then sweep and finally rotate the remainder
+			// (1) has some clear advantages as when a long thin body hits a surface away from the center of mass, this will result in a large angular velocity and a limited reduction in linear velocity.
+			// When simulation the rotation first before doing the translation, the body will be able to rotate away from the contact point allowing the center of mass to approach the surface. When using
+			// approach (2) in this case what will happen is that we will immediately detect the same collision again (the body has not rotated and the body was already colliding at the end of the previous
+			// time step) resulting in a lot of stolen time and the body appearing to be frozen in an unnatural pose (like it is glued at an angle to the surface). (2) obviously has some negative side effects
+			// too as simulating the rotation first may cause it to tunnel through a small object that the linear cast might have otherwise detected. In any case a linear cast is not good for detecting
+			// tunneling due to angular rotation, so we don't care about that too much (you'd need a full cast to take angular effects into account).
+			body.AddRotationStep(body.GetAngularVelocity() * delta_time);
+
+			// Get delta position
+			Vec3 delta_pos = body.GetLinearVelocity() * delta_time;
+
+			// If the position should be updated (or if it is delayed because of CCD)
+			bool update_position = true;
+
+			switch (mp->GetMotionQuality())
+			{
+			case EMotionQuality::Discrete:
+				// No additional collision checking to be done
+				break;
+
+			case EMotionQuality::LinearCast:
+				if (body.IsDynamic() // Kinematic bodies cannot be stopped
+					&& !body.IsSensor()) // We don't support CCD sensors
+				{
+					// Determine inner radius (the smallest sphere that fits into the shape)
+					float inner_radius = body.GetShape()->GetInnerRadius();
+					JPH_ASSERT(inner_radius > 0.0f, "The shape has no inner radius, this makes the shape unsuitable for the linear cast motion quality as we cannot move it without risking tunneling.");
+
+					// Measure translation in this step and check if it above the threshold to perform a linear cast
+					float linear_cast_threshold_sq = Square(mPhysicsSettings.mLinearCastThreshold * inner_radius);
+					if (delta_pos.LengthSq() > linear_cast_threshold_sq)
+					{
+						// This body needs a cast
+						uint32 ccd_body_idx = ioStep->mNumCCDBodies++;
+						JPH_ASSERT(active_body_idx < ioStep->mNumActiveBodyToCCDBody);
+						ioStep->mActiveBodyToCCDBody[active_body_idx] = ccd_body_idx;
+						new (&ioStep->mCCDBodies[ccd_body_idx]) CCDBody(body_id, delta_pos, linear_cast_threshold_sq, min(mPhysicsSettings.mPenetrationSlop, mPhysicsSettings.mLinearCastMaxPenetration * inner_radius));
+
+						update_position = false;
+					}
+				}
+				break;
+			}
+
+			if (update_position)
+			{
+				// Move the body now
+				body.AddPositionStep(delta_pos);
+
+				// If the body was activated due to an earlier CCD step it will have an index in the active
+				// body list that it higher than the highest one we processed during FindCollisions
+				// which means it hasn't been assigned an island and will not be updated by an island
+				// this means that we need to update its bounds manually
+				if (mp->GetIndexInActiveBodiesInternal() >= num_active_bodies_after_find_collisions)
+				{
+					body.CalculateWorldSpaceBoundsInternal();
+					bodies_to_update_bounds[num_bodies_to_update_bounds++] = body.GetID();
+					if (num_bodies_to_update_bounds == cBodiesBatch)
+					{
+						// Buffer full, flush now
+						mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false);
+						num_bodies_to_update_bounds = 0;
+					}
+				}
+
+				// We did not create a CCD body
+				ioStep->mActiveBodyToCCDBody[active_body_idx] = -1;
+			}
+
+			active_body_idx++;
+		}
+	}
+
+	// Notify change bounds on requested bodies
+	if (num_bodies_to_update_bounds > 0)
+		mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false);
+}
+
+void PhysicsSystem::JobPostIntegrateVelocity(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) const
+{
+	// Validate that our reservations were correct
+	JPH_ASSERT(ioStep->mNumCCDBodies <= mBodyManager.GetNumActiveCCDBodies());
+
+	if (ioStep->mNumCCDBodies == 0)
+	{
+		// No continuous collision detection jobs -> kick the next job ourselves
+		ioStep->mContactRemovedCallbacks.RemoveDependency();
+	}
+	else
+	{
+		// Run the continuous collision detection jobs
+		int num_continuous_collision_jobs = min(int(ioStep->mNumCCDBodies + cNumCCDBodiesPerJob - 1) / cNumCCDBodiesPerJob, ioContext->GetMaxConcurrency());
+		ioStep->mResolveCCDContacts.AddDependency(num_continuous_collision_jobs);
+		ioStep->mContactRemovedCallbacks.AddDependency(num_continuous_collision_jobs - 1); // Already had 1 dependency
+		for (int i = 0; i < num_continuous_collision_jobs; ++i)
+		{
+			JobHandle job = ioContext->mJobSystem->CreateJob("FindCCDContacts", cColorFindCCDContacts, [ioContext, ioStep]()
+			{
+				ioContext->mPhysicsSystem->JobFindCCDContacts(ioContext, ioStep);
+
+				ioStep->mResolveCCDContacts.RemoveDependency();
+				ioStep->mContactRemovedCallbacks.RemoveDependency();
+			});
+			ioContext->mBarrier->AddJob(job);
+		}
+	}
+}
+
+// Helper function to calculate the motion of a body during this CCD step
+inline static Vec3 sCalculateBodyMotion(const Body &inBody, float inDeltaTime)
+{
+	// If the body is linear casting, the body has not yet moved so we need to calculate its motion
+	if (inBody.IsDynamic() && inBody.GetMotionProperties()->GetMotionQuality() == EMotionQuality::LinearCast)
+		return inDeltaTime * inBody.GetLinearVelocity();
+
+	// Body has already moved, so we don't need to correct for anything
+	return Vec3::sZero();
+}
+
+// Helper function that finds the CCD body corresponding to a body (if it exists)
+inline static PhysicsUpdateContext::Step::CCDBody *sGetCCDBody(const Body &inBody, PhysicsUpdateContext::Step *inStep)
+{
+	// Only rigid bodies can have a CCD body
+	if (!inBody.IsRigidBody())
+		return nullptr;
+
+	// If the body has no motion properties it cannot have a CCD body
+	const MotionProperties *motion_properties = inBody.GetMotionPropertiesUnchecked();
+	if (motion_properties == nullptr)
+		return nullptr;
+
+	// If it is not active it cannot have a CCD body
+	uint32 active_index = motion_properties->GetIndexInActiveBodiesInternal();
+	if (active_index == Body::cInactiveIndex)
+		return nullptr;
+
+	// Check if the active body has a corresponding CCD body
+	JPH_ASSERT(active_index < inStep->mNumActiveBodyToCCDBody); // Ensure that the body has a mapping to CCD body
+	int ccd_index = inStep->mActiveBodyToCCDBody[active_index];
+	if (ccd_index < 0)
+		return nullptr;
+
+	PhysicsUpdateContext::Step::CCDBody *ccd_body = &inStep->mCCDBodies[ccd_index];
+	JPH_ASSERT(ccd_body->mBodyID1 == inBody.GetID(), "We found the wrong CCD body!");
+	return ccd_body;
+}
+
+void PhysicsSystem::JobFindCCDContacts(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep)
+{
+#ifdef JPH_ENABLE_ASSERTS
+	// We only read positions, but the validate callback may read body positions and velocities
+	BodyAccess::Grant grant(BodyAccess::EAccess::Read, BodyAccess::EAccess::Read);
+#endif
+
+	// Allocation context for allocating new contact points
+	ContactAllocator contact_allocator(mContactManager.GetContactAllocator());
+
+	// Settings
+	ShapeCastSettings settings;
+	settings.mUseShrunkenShapeAndConvexRadius = true;
+	settings.mBackFaceModeTriangles = EBackFaceMode::IgnoreBackFaces;
+	settings.mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces;
+	settings.mReturnDeepestPoint = true;
+	settings.mCollectFacesMode = ECollectFacesMode::CollectFaces;
+	settings.mActiveEdgeMode = mPhysicsSettings.mCheckActiveEdges? EActiveEdgeMode::CollideOnlyWithActive : EActiveEdgeMode::CollideWithAll;
+
+	for (;;)
+	{
+		// Fetch the next body to cast
+		uint32 idx = ioStep->mNextCCDBody++;
+		if (idx >= ioStep->mNumCCDBodies)
+			break;
+		CCDBody &ccd_body = ioStep->mCCDBodies[idx];
+		const Body &body = mBodyManager.GetBody(ccd_body.mBodyID1);
+
+		// Filter out layers
+		DefaultBroadPhaseLayerFilter broadphase_layer_filter = GetDefaultBroadPhaseLayerFilter(body.GetObjectLayer());
+		DefaultObjectLayerFilter object_layer_filter = GetDefaultLayerFilter(body.GetObjectLayer());
+
+	#ifdef JPH_DEBUG_RENDERER
+		// Draw start and end shape of cast
+		if (sDrawMotionQualityLinearCast)
+		{
+			RMat44 com = body.GetCenterOfMassTransform();
+			body.GetShape()->Draw(DebugRenderer::sInstance, com, Vec3::sReplicate(1.0f), Color::sGreen, false, true);
+			DebugRenderer::sInstance->DrawArrow(com.GetTranslation(), com.GetTranslation() + ccd_body.mDeltaPosition, Color::sGreen, 0.1f);
+			body.GetShape()->Draw(DebugRenderer::sInstance, com.PostTranslated(ccd_body.mDeltaPosition), Vec3::sReplicate(1.0f), Color::sRed, false, true);
+		}
+	#endif // JPH_DEBUG_RENDERER
+
+		// Create a collector that will find the maximum distance allowed to travel while not penetrating more than 'max penetration'
+		class CCDNarrowPhaseCollector : public CastShapeCollector
+		{
+		public:
+										CCDNarrowPhaseCollector(const BodyManager &inBodyManager, ContactConstraintManager &inContactConstraintManager, CCDBody &inCCDBody, ShapeCastResult &inResult, float inDeltaTime) :
+				mBodyManager(inBodyManager),
+				mContactConstraintManager(inContactConstraintManager),
+				mCCDBody(inCCDBody),
+				mResult(inResult),
+				mDeltaTime(inDeltaTime)
+			{
+			}
+
+			virtual void				AddHit(const ShapeCastResult &inResult) override
+			{
+				JPH_PROFILE_FUNCTION();
+
+				// Check if this is a possible earlier hit than the one before
+				float fraction = inResult.mFraction;
+				if (fraction < mCCDBody.mFractionPlusSlop)
+				{
+					// Normalize normal
+					Vec3 normal = inResult.mPenetrationAxis.Normalized();
+
+					// Calculate how much we can add to the fraction to penetrate the collision point by mMaxPenetration.
+					// Note that the normal is pointing towards body 2!
+					// Let the extra distance that we can travel along delta_pos be 'dist': mMaxPenetration / dist = cos(angle between normal and delta_pos) = normal . delta_pos / |delta_pos|
+					// <=> dist = mMaxPenetration * |delta_pos| / normal . delta_pos
+					// Converting to a faction: delta_fraction = dist / |delta_pos| = mLinearCastTreshold / normal . delta_pos
+					float denominator = normal.Dot(mCCDBody.mDeltaPosition);
+					if (denominator > mCCDBody.mMaxPenetration) // Avoid dividing by zero, if extra hit fraction > 1 there's also no point in continuing
+					{
+						float fraction_plus_slop = fraction + mCCDBody.mMaxPenetration / denominator;
+						if (fraction_plus_slop < mCCDBody.mFractionPlusSlop)
+						{
+							const Body &body2 = mBodyManager.GetBody(inResult.mBodyID2);
+
+							// Check if we've already accepted all hits from this body
+							if (mValidateBodyPair)
+							{
+								// Validate the contact result
+								const Body &body1 = mBodyManager.GetBody(mCCDBody.mBodyID1);
+								ValidateResult validate_result = mContactConstraintManager.ValidateContactPoint(body1, body2, body1.GetCenterOfMassPosition(), inResult); // Note that the center of mass of body 1 is the start of the sweep and is used as base offset below
+								switch (validate_result)
+								{
+								case ValidateResult::AcceptContact:
+									// Just continue
+									break;
+
+								case ValidateResult::AcceptAllContactsForThisBodyPair:
+									// Accept this and all following contacts from this body
+									mValidateBodyPair = false;
+									break;
+
+								case ValidateResult::RejectContact:
+									return;
+
+								case ValidateResult::RejectAllContactsForThisBodyPair:
+									// Reject this and all following contacts from this body
+									mRejectAll = true;
+									ForceEarlyOut();
+									return;
+								}
+							}
+
+							// This is the earliest hit so far, store it
+							mCCDBody.mContactNormal = normal;
+							mCCDBody.mBodyID2 = inResult.mBodyID2;
+							mCCDBody.mSubShapeID2 = inResult.mSubShapeID2;
+							mCCDBody.mFraction = fraction;
+							mCCDBody.mFractionPlusSlop = fraction_plus_slop;
+							mResult = inResult;
+
+							// Result was assuming body 2 is not moving, but it is, so we need to correct for it
+							Vec3 movement2 = fraction * sCalculateBodyMotion(body2, mDeltaTime);
+							if (!movement2.IsNearZero())
+							{
+								mResult.mContactPointOn1 += movement2;
+								mResult.mContactPointOn2 += movement2;
+								for (Vec3 &v : mResult.mShape1Face)
+									v += movement2;
+								for (Vec3 &v : mResult.mShape2Face)
+									v += movement2;
+							}
+
+							// Update early out fraction
+							UpdateEarlyOutFraction(fraction_plus_slop);
+						}
+					}
+				}
+			}
+
+			bool						mValidateBodyPair;				///< If we still have to call the ValidateContactPoint for this body pair
+			bool						mRejectAll;						///< Reject all further contacts between this body pair
+
+		private:
+			const BodyManager &			mBodyManager;
+			ContactConstraintManager &	mContactConstraintManager;
+			CCDBody &					mCCDBody;
+			ShapeCastResult &			mResult;
+			float						mDeltaTime;
+			BodyID						mAcceptedBodyID;
+		};
+
+		// Narrowphase collector
+		ShapeCastResult cast_shape_result;
+		CCDNarrowPhaseCollector np_collector(mBodyManager, mContactManager, ccd_body, cast_shape_result, ioContext->mStepDeltaTime);
+
+		// This collector wraps the narrowphase collector and collects the closest hit
+		class CCDBroadPhaseCollector : public CastShapeBodyCollector
+		{
+		public:
+										CCDBroadPhaseCollector(const CCDBody &inCCDBody, const Body &inBody1, const RShapeCast &inShapeCast, ShapeCastSettings &inShapeCastSettings, SimShapeFilterWrapper &inShapeFilter, CCDNarrowPhaseCollector &ioCollector, const BodyManager &inBodyManager, PhysicsUpdateContext::Step *inStep, float inDeltaTime) :
+				mCCDBody(inCCDBody),
+				mBody1(inBody1),
+				mBody1Extent(inShapeCast.mShapeWorldBounds.GetExtent()),
+				mShapeCast(inShapeCast),
+				mShapeCastSettings(inShapeCastSettings),
+				mShapeFilter(inShapeFilter),
+				mCollector(ioCollector),
+				mBodyManager(inBodyManager),
+				mStep(inStep),
+				mDeltaTime(inDeltaTime)
+			{
+			}
+
+			virtual void				AddHit(const BroadPhaseCastResult &inResult) override
+			{
+				JPH_PROFILE_FUNCTION();
+
+				JPH_ASSERT(inResult.mFraction <= GetEarlyOutFraction(), "This hit should not have been passed on to the collector");
+
+				// Test if we're colliding with ourselves
+				if (mBody1.GetID() == inResult.mBodyID)
+					return;
+
+				// Avoid treating duplicates, if both bodies are doing CCD then only consider collision if body ID < other body ID
+				const Body &body2 = mBodyManager.GetBody(inResult.mBodyID);
+				const CCDBody *ccd_body2 = sGetCCDBody(body2, mStep);
+				if (ccd_body2 != nullptr && mCCDBody.mBodyID1 > ccd_body2->mBodyID1)
+					return;
+
+				// Test group filter
+				if (!mBody1.GetCollisionGroup().CanCollide(body2.GetCollisionGroup()))
+					return;
+
+				// TODO: For now we ignore sensors
+				if (body2.IsSensor())
+					return;
+
+				// Get relative movement of these two bodies
+				Vec3 direction = mShapeCast.mDirection - sCalculateBodyMotion(body2, mDeltaTime);
+
+				// Test if the remaining movement is less than our movement threshold
+				if (direction.LengthSq() < mCCDBody.mLinearCastThresholdSq)
+					return;
+
+				// Get the bounds of 2, widen it by the extent of 1 and test a ray to see if it hits earlier than the current early out fraction
+				AABox bounds = body2.GetWorldSpaceBounds();
+				bounds.mMin -= mBody1Extent;
+				bounds.mMax += mBody1Extent;
+				float hit_fraction = RayAABox(Vec3(mShapeCast.mCenterOfMassStart.GetTranslation()), RayInvDirection(direction), bounds.mMin, bounds.mMax);
+				if (hit_fraction > GetPositiveEarlyOutFraction()) // If early out fraction <= 0, we have the possibility of finding a deeper hit so we need to clamp the early out fraction
+					return;
+
+				// Reset collector (this is a new body pair)
+				mCollector.ResetEarlyOutFraction(GetEarlyOutFraction());
+				mCollector.mValidateBodyPair = true;
+				mCollector.mRejectAll = false;
+
+				// Set body ID on shape filter
+				mShapeFilter.SetBody2(&body2);
+
+				// Provide direction as hint for the active edges algorithm
+				mShapeCastSettings.mActiveEdgeMovementDirection = direction;
+
+				// Do narrow phase collision check
+				RShapeCast relative_cast(mShapeCast.mShape, mShapeCast.mScale, mShapeCast.mCenterOfMassStart, direction, mShapeCast.mShapeWorldBounds);
+				body2.GetTransformedShape().CastShape(relative_cast, mShapeCastSettings, mShapeCast.mCenterOfMassStart.GetTranslation(), mCollector, mShapeFilter);
+
+				// Update early out fraction based on narrow phase collector
+				if (!mCollector.mRejectAll)
+					UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
+			}
+
+			const CCDBody &				mCCDBody;
+			const Body &				mBody1;
+			Vec3						mBody1Extent;
+			RShapeCast					mShapeCast;
+			ShapeCastSettings &			mShapeCastSettings;
+			SimShapeFilterWrapper &		mShapeFilter;
+			CCDNarrowPhaseCollector &	mCollector;
+			const BodyManager &			mBodyManager;
+			PhysicsUpdateContext::Step *mStep;
+			float						mDeltaTime;
+		};
+
+		// Create shape filter
+		SimShapeFilterWrapperUnion shape_filter_union(mSimShapeFilter, &body);
+		SimShapeFilterWrapper &shape_filter = shape_filter_union.GetSimShapeFilterWrapper();
+
+		// Check if we collide with any other body. Note that we use the non-locking interface as we know the broadphase cannot be modified at this point.
+		RShapeCast shape_cast(body.GetShape(), Vec3::sReplicate(1.0f), body.GetCenterOfMassTransform(), ccd_body.mDeltaPosition);
+		CCDBroadPhaseCollector bp_collector(ccd_body, body, shape_cast, settings, shape_filter, np_collector, mBodyManager, ioStep, ioContext->mStepDeltaTime);
+		mBroadPhase->CastAABoxNoLock({ shape_cast.mShapeWorldBounds, shape_cast.mDirection }, bp_collector, broadphase_layer_filter, object_layer_filter);
+
+		// Check if there was a hit
+		if (ccd_body.mFractionPlusSlop < 1.0f)
+		{
+			const Body &body2 = mBodyManager.GetBody(ccd_body.mBodyID2);
+
+			// Determine contact manifold
+			ContactManifold manifold;
+			manifold.mBaseOffset = shape_cast.mCenterOfMassStart.GetTranslation();
+			ManifoldBetweenTwoFaces(cast_shape_result.mContactPointOn1, cast_shape_result.mContactPointOn2, cast_shape_result.mPenetrationAxis, mPhysicsSettings.mManifoldToleranceSq, cast_shape_result.mShape1Face, cast_shape_result.mShape2Face, manifold.mRelativeContactPointsOn1, manifold.mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold.mBaseOffset));
+			manifold.mSubShapeID1 = cast_shape_result.mSubShapeID1;
+			manifold.mSubShapeID2 = cast_shape_result.mSubShapeID2;
+			manifold.mPenetrationDepth = cast_shape_result.mPenetrationDepth;
+			manifold.mWorldSpaceNormal = ccd_body.mContactNormal;
+
+			// Call contact point callbacks
+			mContactManager.OnCCDContactAdded(contact_allocator, body, body2, manifold, ccd_body.mContactSettings);
+
+			if (ccd_body.mContactSettings.mIsSensor)
+			{
+				// If this is a sensor, we don't want to solve the contact
+				ccd_body.mFractionPlusSlop = 1.0f;
+				ccd_body.mBodyID2 = BodyID();
+			}
+			else
+			{
+				// Calculate the average position from the manifold (this will result in the same impulse applied as when we apply impulses to all contact points)
+				if (manifold.mRelativeContactPointsOn2.size() > 1)
+				{
+					Vec3 average_contact_point = Vec3::sZero();
+					for (const Vec3 &v : manifold.mRelativeContactPointsOn2)
+						average_contact_point += v;
+					average_contact_point /= (float)manifold.mRelativeContactPointsOn2.size();
+					ccd_body.mContactPointOn2 = manifold.mBaseOffset + average_contact_point;
+				}
+				else
+					ccd_body.mContactPointOn2 = manifold.mBaseOffset + cast_shape_result.mContactPointOn2;
+			}
+		}
+	}
+
+	// Collect information from the contact allocator and accumulate it in the step.
+	sFinalizeContactAllocator(*ioStep, contact_allocator);
+}
+
+void PhysicsSystem::JobResolveCCDContacts(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep)
+{
+#ifdef JPH_ENABLE_ASSERTS
+	// Read/write body access
+	BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::ReadWrite);
+
+	// We activate bodies that we collide with
+	BodyManager::GrantActiveBodiesAccess grant_active(true, false);
+#endif
+
+	uint32 num_active_bodies_after_find_collisions = ioStep->mActiveBodyReadIdx;
+	TempAllocator *temp_allocator = ioContext->mTempAllocator;
+
+	// Check if there's anything to do
+	uint num_ccd_bodies = ioStep->mNumCCDBodies;
+	if (num_ccd_bodies > 0)
+	{
+		// Sort on fraction so that we process earliest collisions first
+		// This is needed to make the simulation deterministic and also to be able to stop contact processing
+		// between body pairs if an earlier hit was found involving the body by another CCD body
+		// (if it's body ID < this CCD body's body ID - see filtering logic in CCDBroadPhaseCollector)
+		CCDBody **sorted_ccd_bodies = (CCDBody **)temp_allocator->Allocate(num_ccd_bodies * sizeof(CCDBody *));
+		JPH_SCOPE_EXIT([temp_allocator, sorted_ccd_bodies, num_ccd_bodies]{ temp_allocator->Free(sorted_ccd_bodies, num_ccd_bodies * sizeof(CCDBody *)); });
+		{
+			JPH_PROFILE("Sort");
+
+			// We don't want to copy the entire struct (it's quite big), so we create a pointer array first
+			CCDBody *src_ccd_bodies = ioStep->mCCDBodies;
+			CCDBody **dst_ccd_bodies = sorted_ccd_bodies;
+			CCDBody **dst_ccd_bodies_end = dst_ccd_bodies + num_ccd_bodies;
+			while (dst_ccd_bodies < dst_ccd_bodies_end)
+				*(dst_ccd_bodies++) = src_ccd_bodies++;
+
+			// Which we then sort
+			QuickSort(sorted_ccd_bodies, sorted_ccd_bodies + num_ccd_bodies, [](const CCDBody *inBody1, const CCDBody *inBody2)
+				{
+					if (inBody1->mFractionPlusSlop != inBody2->mFractionPlusSlop)
+						return inBody1->mFractionPlusSlop < inBody2->mFractionPlusSlop;
+
+					return inBody1->mBodyID1 < inBody2->mBodyID1;
+				});
+		}
+
+		// We can collide with bodies that are not active, we track them here so we can activate them in one go at the end.
+		// This is also needed because we can't modify the active body array while we iterate it.
+		static constexpr int cBodiesBatch = 64;
+		BodyID *bodies_to_activate = (BodyID *)JPH_STACK_ALLOC(cBodiesBatch * sizeof(BodyID));
+		int num_bodies_to_activate = 0;
+
+		// We can move bodies that are not part of an island. In this case we need to notify the broadphase of the movement.
+		BodyID *bodies_to_update_bounds = (BodyID *)JPH_STACK_ALLOC(cBodiesBatch * sizeof(BodyID));
+		int num_bodies_to_update_bounds = 0;
+
+		for (uint i = 0; i < num_ccd_bodies; ++i)
+		{
+			const CCDBody *ccd_body = sorted_ccd_bodies[i];
+			Body &body1 = mBodyManager.GetBody(ccd_body->mBodyID1);
+			MotionProperties *body_mp = body1.GetMotionProperties();
+
+			// If there was a hit
+			if (!ccd_body->mBodyID2.IsInvalid())
+			{
+				Body &body2 = mBodyManager.GetBody(ccd_body->mBodyID2);
+
+				// Determine if the other body has a CCD body
+				CCDBody *ccd_body2 = sGetCCDBody(body2, ioStep);
+				if (ccd_body2 != nullptr)
+				{
+					JPH_ASSERT(ccd_body2->mBodyID2 != ccd_body->mBodyID1, "If we collided with another body, that other body should have ignored collisions with us!");
+
+					// Check if the other body found a hit that is further away
+					if (ccd_body2->mFraction > ccd_body->mFraction)
+					{
+						// Reset the colliding body of the other CCD body. The other body will shorten its distance traveled and will not do any collision response (we'll do that).
+						// This means that at this point we have triggered a contact point add/persist for our further hit by accident for the other body.
+						// We accept this as calling the contact point callbacks here would require persisting the manifolds up to this point and doing the callbacks single threaded.
+						ccd_body2->mBodyID2 = BodyID();
+						ccd_body2->mFractionPlusSlop = ccd_body->mFraction;
+					}
+				}
+
+				// If the other body moved less than us before hitting something, we're not colliding with it so we again have triggered contact point add/persist callbacks by accident.
+				// We'll just move to the collision position anyway (as that's the last position we know is good), but we won't do any collision response.
+				if (ccd_body2 == nullptr || ccd_body2->mFraction >= ccd_body->mFraction)
+				{
+					const ContactSettings &contact_settings = ccd_body->mContactSettings;
+
+					// Calculate contact point velocity for body 1
+					Vec3 r1_plus_u = Vec3(ccd_body->mContactPointOn2 - (body1.GetCenterOfMassPosition() + ccd_body->mFraction * ccd_body->mDeltaPosition));
+					Vec3 v1 = body1.GetPointVelocityCOM(r1_plus_u);
+
+					// Calculate inverse mass for body 1
+					float inv_m1 = contact_settings.mInvMassScale1 * body_mp->GetInverseMass();
+
+					if (body2.IsRigidBody())
+					{
+						// Calculate contact point velocity for body 2
+						Vec3 r2 = Vec3(ccd_body->mContactPointOn2 - body2.GetCenterOfMassPosition());
+						Vec3 v2 = body2.GetPointVelocityCOM(r2);
+
+						// Calculate relative contact velocity
+						Vec3 relative_velocity = v2 - v1;
+						float normal_velocity = relative_velocity.Dot(ccd_body->mContactNormal);
+
+						// Calculate velocity bias due to restitution
+						float normal_velocity_bias;
+						if (contact_settings.mCombinedRestitution > 0.0f && normal_velocity < -mPhysicsSettings.mMinVelocityForRestitution)
+							normal_velocity_bias = contact_settings.mCombinedRestitution * normal_velocity;
+						else
+							normal_velocity_bias = 0.0f;
+
+						// Get inverse mass of body 2
+						float inv_m2 = body2.GetMotionPropertiesUnchecked() != nullptr? contact_settings.mInvMassScale2 * body2.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked() : 0.0f;
+
+						// Solve contact constraint
+						AxisConstraintPart contact_constraint;
+						contact_constraint.CalculateConstraintPropertiesWithMassOverride(body1, inv_m1, contact_settings.mInvInertiaScale1, r1_plus_u, body2, inv_m2, contact_settings.mInvInertiaScale2, r2, ccd_body->mContactNormal, normal_velocity_bias);
+						contact_constraint.SolveVelocityConstraintWithMassOverride(body1, inv_m1, body2, inv_m2, ccd_body->mContactNormal, -FLT_MAX, FLT_MAX);
+
+						// Apply friction
+						if (contact_settings.mCombinedFriction > 0.0f)
+						{
+							// Calculate friction direction by removing normal velocity from the relative velocity
+							Vec3 friction_direction = relative_velocity - normal_velocity * ccd_body->mContactNormal;
+							float friction_direction_len_sq = friction_direction.LengthSq();
+							if (friction_direction_len_sq > 1.0e-12f)
+							{
+								// Normalize friction direction
+								friction_direction /= sqrt(friction_direction_len_sq);
+
+								// Calculate max friction impulse
+								float max_lambda_f = contact_settings.mCombinedFriction * contact_constraint.GetTotalLambda();
+
+								AxisConstraintPart friction;
+								friction.CalculateConstraintPropertiesWithMassOverride(body1, inv_m1, contact_settings.mInvInertiaScale1, r1_plus_u, body2, inv_m2, contact_settings.mInvInertiaScale2, r2, friction_direction);
+								friction.SolveVelocityConstraintWithMassOverride(body1, inv_m1, body2, inv_m2, friction_direction, -max_lambda_f, max_lambda_f);
+							}
+						}
+
+						// Clamp velocity of body 2
+						if (body2.IsDynamic())
+						{
+							MotionProperties *body2_mp = body2.GetMotionProperties();
+							body2_mp->ClampLinearVelocity();
+							body2_mp->ClampAngularVelocity();
+						}
+					}
+					else
+					{
+						SoftBodyMotionProperties *soft_mp = static_cast<SoftBodyMotionProperties *>(body2.GetMotionProperties());
+						const SoftBodyShape *soft_shape = static_cast<const SoftBodyShape *>(body2.GetShape());
+
+						// Convert the sub shape ID of the soft body to a face
+						uint32 face_idx = soft_shape->GetFaceIndex(ccd_body->mSubShapeID2);
+						const SoftBodyMotionProperties::Face &face = soft_mp->GetFace(face_idx);
+
+						// Get vertices of the face
+						SoftBodyMotionProperties::Vertex &vtx0 = soft_mp->GetVertex(face.mVertex[0]);
+						SoftBodyMotionProperties::Vertex &vtx1 = soft_mp->GetVertex(face.mVertex[1]);
+						SoftBodyMotionProperties::Vertex &vtx2 = soft_mp->GetVertex(face.mVertex[2]);
+
+						// Inverse mass of the face
+						float vtx0_mass = vtx0.mInvMass > 0.0f? 1.0f / vtx0.mInvMass : 1.0e10f;
+						float vtx1_mass = vtx1.mInvMass > 0.0f? 1.0f / vtx1.mInvMass : 1.0e10f;
+						float vtx2_mass = vtx2.mInvMass > 0.0f? 1.0f / vtx2.mInvMass : 1.0e10f;
+						float inv_m2 = 1.0f / (vtx0_mass + vtx1_mass + vtx2_mass);
+
+						// Calculate barycentric coordinates of the contact point on the soft body's face
+						float u, v, w;
+						RMat44 inv_body2_transform = body2.GetInverseCenterOfMassTransform();
+						Vec3 local_contact = Vec3(inv_body2_transform * ccd_body->mContactPointOn2);
+						ClosestPoint::GetBaryCentricCoordinates(vtx0.mPosition - local_contact, vtx1.mPosition - local_contact, vtx2.mPosition - local_contact, u, v, w);
+
+						// Calculate contact point velocity for the face
+						Vec3 v2 = inv_body2_transform.Multiply3x3Transposed(u * vtx0.mVelocity + v * vtx1.mVelocity + w * vtx2.mVelocity);
+						float normal_velocity = (v2 - v1).Dot(ccd_body->mContactNormal);
+
+						// Calculate velocity bias due to restitution
+						float normal_velocity_bias;
+						if (contact_settings.mCombinedRestitution > 0.0f && normal_velocity < -mPhysicsSettings.mMinVelocityForRestitution)
+							normal_velocity_bias = contact_settings.mCombinedRestitution * normal_velocity;
+						else
+							normal_velocity_bias = 0.0f;
+
+						// Calculate resulting velocity change (the math here is similar to AxisConstraintPart but without an inertia term for body 2 as we treat it as a point mass)
+						Vec3 r1_plus_u_x_n = r1_plus_u.Cross(ccd_body->mContactNormal);
+						Vec3 invi1_r1_plus_u_x_n = contact_settings.mInvInertiaScale1 * body1.GetInverseInertia().Multiply3x3(r1_plus_u_x_n);
+						float jv = r1_plus_u_x_n.Dot(body_mp->GetAngularVelocity()) - normal_velocity - normal_velocity_bias;
+						float inv_effective_mass = inv_m1 + inv_m2 + invi1_r1_plus_u_x_n.Dot(r1_plus_u_x_n);
+						float lambda = jv / inv_effective_mass;
+						body_mp->SubLinearVelocityStep((lambda * inv_m1) * ccd_body->mContactNormal);
+						body_mp->SubAngularVelocityStep(lambda * invi1_r1_plus_u_x_n);
+						Vec3 delta_v2 = inv_body2_transform.Multiply3x3(lambda * ccd_body->mContactNormal);
+						vtx0.mVelocity += delta_v2 * vtx0.mInvMass;
+						vtx1.mVelocity += delta_v2 * vtx1.mInvMass;
+						vtx2.mVelocity += delta_v2 * vtx2.mInvMass;
+					}
+
+					// Clamp velocity of body 1
+					body_mp->ClampLinearVelocity();
+					body_mp->ClampAngularVelocity();
+
+					// Activate the 2nd body if it is not already active
+					if (body2.IsDynamic() && !body2.IsActive())
+					{
+						bodies_to_activate[num_bodies_to_activate++] = ccd_body->mBodyID2;
+						if (num_bodies_to_activate == cBodiesBatch)
+						{
+							// Batch is full, activate now
+							mBodyManager.ActivateBodies(bodies_to_activate, num_bodies_to_activate);
+							num_bodies_to_activate = 0;
+						}
+					}
+
+				#ifdef JPH_DEBUG_RENDERER
+					if (sDrawMotionQualityLinearCast)
+					{
+						// Draw the collision location
+						RMat44 collision_transform = body1.GetCenterOfMassTransform().PostTranslated(ccd_body->mFraction * ccd_body->mDeltaPosition);
+						body1.GetShape()->Draw(DebugRenderer::sInstance, collision_transform, Vec3::sReplicate(1.0f), Color::sYellow, false, true);
+
+						// Draw the collision location + slop
+						RMat44 collision_transform_plus_slop = body1.GetCenterOfMassTransform().PostTranslated(ccd_body->mFractionPlusSlop * ccd_body->mDeltaPosition);
+						body1.GetShape()->Draw(DebugRenderer::sInstance, collision_transform_plus_slop, Vec3::sReplicate(1.0f), Color::sOrange, false, true);
+
+						// Draw contact normal
+						DebugRenderer::sInstance->DrawArrow(ccd_body->mContactPointOn2, ccd_body->mContactPointOn2 - ccd_body->mContactNormal, Color::sYellow, 0.1f);
+
+						// Draw post contact velocity
+						DebugRenderer::sInstance->DrawArrow(collision_transform.GetTranslation(), collision_transform.GetTranslation() + body1.GetLinearVelocity(), Color::sOrange, 0.1f);
+						DebugRenderer::sInstance->DrawArrow(collision_transform.GetTranslation(), collision_transform.GetTranslation() + body1.GetAngularVelocity(), Color::sPurple, 0.1f);
+					}
+				#endif // JPH_DEBUG_RENDERER
+				}
+			}
+
+			// Update body position
+			body1.AddPositionStep(ccd_body->mDeltaPosition * ccd_body->mFractionPlusSlop);
+
+			// If the body was activated due to an earlier CCD step it will have an index in the active
+			// body list that it higher than the highest one we processed during FindCollisions
+			// which means it hasn't been assigned an island and will not be updated by an island
+			// this means that we need to update its bounds manually
+			if (body_mp->GetIndexInActiveBodiesInternal() >= num_active_bodies_after_find_collisions)
+			{
+				body1.CalculateWorldSpaceBoundsInternal();
+				bodies_to_update_bounds[num_bodies_to_update_bounds++] = body1.GetID();
+				if (num_bodies_to_update_bounds == cBodiesBatch)
+				{
+					// Buffer full, flush now
+					mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false);
+					num_bodies_to_update_bounds = 0;
+				}
+			}
+		}
+
+		// Activate the requested bodies
+		if (num_bodies_to_activate > 0)
+			mBodyManager.ActivateBodies(bodies_to_activate, num_bodies_to_activate);
+
+		// Notify change bounds on requested bodies
+		if (num_bodies_to_update_bounds > 0)
+			mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false);
+	}
+
+	// Ensure we free the CCD bodies array now, will not call the destructor!
+	temp_allocator->Free(ioStep->mActiveBodyToCCDBody, ioStep->mNumActiveBodyToCCDBody * sizeof(int));
+	ioStep->mActiveBodyToCCDBody = nullptr;
+	ioStep->mNumActiveBodyToCCDBody = 0;
+	temp_allocator->Free(ioStep->mCCDBodies, ioStep->mCCDBodiesCapacity * sizeof(CCDBody));
+	ioStep->mCCDBodies = nullptr;
+	ioStep->mCCDBodiesCapacity = 0;
+}
+
+void PhysicsSystem::JobContactRemovedCallbacks(const PhysicsUpdateContext::Step *ioStep)
+{
+#ifdef JPH_ENABLE_ASSERTS
+	// We don't touch any bodies
+	BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::None);
+#endif
+
+	// Reset the Body::EFlags::InvalidateContactCache flag for all bodies
+	mBodyManager.ValidateContactCacheForAllBodies();
+
+	// Finalize the contact cache (this swaps the read and write versions of the contact cache)
+	// Trigger all contact removed callbacks by looking at last step contact points that have not been flagged as reused
+	mContactManager.FinalizeContactCacheAndCallContactPointRemovedCallbacks(ioStep->mNumBodyPairs, ioStep->mNumManifolds);
+}
+
+class PhysicsSystem::BodiesToSleep : public NonCopyable
+{
+public:
+	static constexpr int	cBodiesToSleepSize = 512;
+	static constexpr int	cMaxBodiesToPutInBuffer = 128;
+
+	inline					BodiesToSleep(BodyManager &inBodyManager, BodyID *inBodiesToSleepBuffer) : mBodyManager(inBodyManager), mBodiesToSleepBuffer(inBodiesToSleepBuffer), mBodiesToSleepCur(inBodiesToSleepBuffer) { }
+
+	inline					~BodiesToSleep()
+	{
+		// Flush the bodies to sleep buffer
+		int num_bodies_in_buffer = int(mBodiesToSleepCur - mBodiesToSleepBuffer);
+		if (num_bodies_in_buffer > 0)
+			mBodyManager.DeactivateBodies(mBodiesToSleepBuffer, num_bodies_in_buffer);
+	}
+
+	inline void				PutToSleep(const BodyID *inBegin, const BodyID *inEnd)
+	{
+		int num_bodies_to_sleep = int(inEnd - inBegin);
+		if (num_bodies_to_sleep > cMaxBodiesToPutInBuffer)
+		{
+			// Too many bodies, deactivate immediately
+			mBodyManager.DeactivateBodies(inBegin, num_bodies_to_sleep);
+		}
+		else
+		{
+			// Check if there's enough space in the bodies to sleep buffer
+			int num_bodies_in_buffer = int(mBodiesToSleepCur - mBodiesToSleepBuffer);
+			if (num_bodies_in_buffer + num_bodies_to_sleep > cBodiesToSleepSize)
+			{
+				// Flush the bodies to sleep buffer
+				mBodyManager.DeactivateBodies(mBodiesToSleepBuffer, num_bodies_in_buffer);
+				mBodiesToSleepCur = mBodiesToSleepBuffer;
+			}
+
+			// Copy the bodies in the buffer
+			memcpy(mBodiesToSleepCur, inBegin, num_bodies_to_sleep * sizeof(BodyID));
+			mBodiesToSleepCur += num_bodies_to_sleep;
+		}
+	}
+
+private:
+	BodyManager &			mBodyManager;
+	BodyID *				mBodiesToSleepBuffer;
+	BodyID *				mBodiesToSleepCur;
+};
+
+void PhysicsSystem::CheckSleepAndUpdateBounds(uint32 inIslandIndex, const PhysicsUpdateContext *ioContext, const PhysicsUpdateContext::Step *ioStep, BodiesToSleep &ioBodiesToSleep)
+{
+	// Get the bodies that belong to this island
+	BodyID *bodies_begin, *bodies_end;
+	mIslandBuilder.GetBodiesInIsland(inIslandIndex, bodies_begin, bodies_end);
+
+	// Only check sleeping in the last step
+	// Also resets force and torque used during the apply gravity phase
+	if (ioStep->mIsLast)
+	{
+		JPH_PROFILE("Check Sleeping");
+
+		static_assert(int(ECanSleep::CannotSleep) == 0 && int(ECanSleep::CanSleep) == 1, "Loop below makes this assumption");
+		int all_can_sleep = mPhysicsSettings.mAllowSleeping? int(ECanSleep::CanSleep) : int(ECanSleep::CannotSleep);
+
+		float time_before_sleep = mPhysicsSettings.mTimeBeforeSleep;
+		float max_movement = mPhysicsSettings.mPointVelocitySleepThreshold * time_before_sleep;
+
+		for (const BodyID *body_id = bodies_begin; body_id < bodies_end; ++body_id)
+		{
+			Body &body = mBodyManager.GetBody(*body_id);
+
+			// Update bounding box
+			body.CalculateWorldSpaceBoundsInternal();
+
+			// Update sleeping
+			all_can_sleep &= int(body.UpdateSleepStateInternal(ioContext->mStepDeltaTime, max_movement, time_before_sleep));
+
+			// Reset force and torque
+			MotionProperties *mp = body.GetMotionProperties();
+			mp->ResetForce();
+			mp->ResetTorque();
+		}
+
+		// If all bodies indicate they can sleep we can deactivate them
+		if (all_can_sleep == int(ECanSleep::CanSleep))
+			ioBodiesToSleep.PutToSleep(bodies_begin, bodies_end);
+	}
+	else
+	{
+		JPH_PROFILE("Update Bounds");
+
+		// Update bounding box only for all other steps
+		for (const BodyID *body_id = bodies_begin; body_id < bodies_end; ++body_id)
+		{
+			Body &body = mBodyManager.GetBody(*body_id);
+			body.CalculateWorldSpaceBoundsInternal();
+		}
+	}
+
+	// Notify broadphase of changed objects (find ccd contacts can do linear casts in the next step, so we need to do this every step)
+	// Note: Shuffles the BodyID's around!!!
+	mBroadPhase->NotifyBodiesAABBChanged(bodies_begin, int(bodies_end - bodies_begin), false);
+}
+
+void PhysicsSystem::JobSolvePositionConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep)
+{
+#ifdef JPH_ENABLE_ASSERTS
+	// We fix up position errors
+	BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::ReadWrite);
+
+	// Can only deactivate bodies
+	BodyManager::GrantActiveBodiesAccess grant_active(false, true);
+#endif
+
+	float delta_time = ioContext->mStepDeltaTime;
+	float baumgarte = mPhysicsSettings.mBaumgarte;
+	Constraint **active_constraints = ioContext->mActiveConstraints;
+
+	// Keep a buffer of bodies that need to go to sleep in order to not constantly lock the active bodies mutex and create contention between all solving threads
+	BodiesToSleep bodies_to_sleep(mBodyManager, (BodyID *)JPH_STACK_ALLOC(BodiesToSleep::cBodiesToSleepSize * sizeof(BodyID)));
+
+	bool check_islands = true, check_split_islands = mPhysicsSettings.mUseLargeIslandSplitter;
+	for (;;)
+	{
+		// First try to get work from large islands
+		if (check_split_islands)
+		{
+			bool first_iteration;
+			uint split_island_index;
+			uint32 *constraints_begin, *constraints_end, *contacts_begin, *contacts_end;
+			switch (mLargeIslandSplitter.FetchNextBatch(split_island_index, constraints_begin, constraints_end, contacts_begin, contacts_end, first_iteration))
+			{
+			case LargeIslandSplitter::EStatus::BatchRetrieved:
+				// Solve the batch
+				ConstraintManager::sSolvePositionConstraints(active_constraints, constraints_begin, constraints_end, delta_time, baumgarte);
+				mContactManager.SolvePositionConstraints(contacts_begin, contacts_end);
+
+				// Mark the batch as processed
+				bool last_iteration, final_batch;
+				mLargeIslandSplitter.MarkBatchProcessed(split_island_index, constraints_begin, constraints_end, contacts_begin, contacts_end, last_iteration, final_batch);
+
+				// The final batch will update all bounds and check sleeping
+				if (final_batch)
+					CheckSleepAndUpdateBounds(mLargeIslandSplitter.GetIslandIndex(split_island_index), ioContext, ioStep, bodies_to_sleep);
+
+				// We processed work, loop again
+				continue;
+
+			case LargeIslandSplitter::EStatus::WaitingForBatch:
+				break;
+
+			case LargeIslandSplitter::EStatus::AllBatchesDone:
+				check_split_islands = false;
+				break;
+			}
+		}
+
+		// If that didn't succeed try to process an island
+		if (check_islands)
+		{
+			// Next island
+			uint32 island_idx = ioStep->mSolvePositionConstraintsNextIsland++;
+			if (island_idx >= mIslandBuilder.GetNumIslands())
+			{
+				// We processed all islands, stop checking islands
+				check_islands = false;
+				continue;
+			}
+
+			JPH_PROFILE("Island");
+
+			// Get iterators for this island
+			uint32 *constraints_begin, *constraints_end, *contacts_begin, *contacts_end;
+			mIslandBuilder.GetConstraintsInIsland(island_idx, constraints_begin, constraints_end);
+			mIslandBuilder.GetContactsInIsland(island_idx, contacts_begin, contacts_end);
+
+			// If this island is a large island, it will be picked up as a batch and we don't need to do anything here
+			uint num_items = uint(constraints_end - constraints_begin) + uint(contacts_end - contacts_begin);
+			if (mPhysicsSettings.mUseLargeIslandSplitter
+				&& num_items >= LargeIslandSplitter::cLargeIslandTreshold)
+				continue;
+
+			// Check if this island needs solving
+			if (num_items > 0)
+			{
+				// Iterate
+				uint num_position_steps = mIslandBuilder.GetNumPositionSteps(island_idx);
+				for (uint position_step = 0; position_step < num_position_steps; ++position_step)
+				{
+					bool applied_impulse = ConstraintManager::sSolvePositionConstraints(active_constraints, constraints_begin, constraints_end, delta_time, baumgarte);
+					applied_impulse |= mContactManager.SolvePositionConstraints(contacts_begin, contacts_end);
+					if (!applied_impulse)
+						break;
+				}
+			}
+
+			// After solving we will update all bounds and check sleeping
+			CheckSleepAndUpdateBounds(island_idx, ioContext, ioStep, bodies_to_sleep);
+
+			// We processed work, loop again
+			continue;
+		}
+
+		if (check_islands)
+		{
+			// If there are islands, we don't need to wait and can pick up new work
+			continue;
+		}
+		else if (check_split_islands)
+		{
+			// If there are split islands, but we did't do any work, give up a time slice
+			std::this_thread::yield();
+		}
+		else
+		{
+			// No more work
+			break;
+		}
+	}
+}
+
+void PhysicsSystem::JobSoftBodyPrepare(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep)
+{
+	JPH_PROFILE_FUNCTION();
+
+	{
+	#ifdef JPH_ENABLE_ASSERTS
+		// Reading soft body positions
+		BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::Read);
+	#endif
+
+		// Get the active soft bodies
+		BodyIDVector active_bodies;
+		mBodyManager.GetActiveBodies(EBodyType::SoftBody, active_bodies);
+
+		// Quit if there are no active soft bodies
+		if (active_bodies.empty())
+		{
+			// Kick the next step
+			if (ioStep->mStartNextStep.IsValid())
+				ioStep->mStartNextStep.RemoveDependency();
+			return;
+		}
+
+		// Sort to get a deterministic update order
+		QuickSort(active_bodies.begin(), active_bodies.end());
+
+		// Allocate soft body contexts
+		ioContext->mNumSoftBodies = (uint)active_bodies.size();
+		ioContext->mSoftBodyUpdateContexts = (SoftBodyUpdateContext *)ioContext->mTempAllocator->Allocate(ioContext->mNumSoftBodies * sizeof(SoftBodyUpdateContext));
+
+		// Initialize soft body contexts
+		for (SoftBodyUpdateContext *sb_ctx = ioContext->mSoftBodyUpdateContexts, *sb_ctx_end = ioContext->mSoftBodyUpdateContexts + ioContext->mNumSoftBodies; sb_ctx < sb_ctx_end; ++sb_ctx)
+		{
+			new (sb_ctx) SoftBodyUpdateContext;
+			Body &body = mBodyManager.GetBody(active_bodies[sb_ctx - ioContext->mSoftBodyUpdateContexts]);
+			SoftBodyMotionProperties *mp = static_cast<SoftBodyMotionProperties *>(body.GetMotionProperties());
+			mp->InitializeUpdateContext(ioContext->mStepDeltaTime, body, *this, *sb_ctx);
+		}
+	}
+
+	// We're ready to collide the first soft body
+	ioContext->mSoftBodyToCollide.store(0, memory_order_release);
+
+	// Determine number of jobs to spawn
+	int num_soft_body_jobs = ioContext->GetMaxConcurrency();
+
+	// Create finalize job
+	ioStep->mSoftBodyFinalize = ioContext->mJobSystem->CreateJob("SoftBodyFinalize", cColorSoftBodyFinalize, [ioContext, ioStep]()
+	{
+		ioContext->mPhysicsSystem->JobSoftBodyFinalize(ioContext);
+
+		// Kick the next step
+		if (ioStep->mStartNextStep.IsValid())
+			ioStep->mStartNextStep.RemoveDependency();
+	}, num_soft_body_jobs); // depends on: soft body simulate
+	ioContext->mBarrier->AddJob(ioStep->mSoftBodyFinalize);
+
+	// Create simulate jobs
+	ioStep->mSoftBodySimulate.resize(num_soft_body_jobs);
+	for (int i = 0; i < num_soft_body_jobs; ++i)
+		ioStep->mSoftBodySimulate[i] = ioContext->mJobSystem->CreateJob("SoftBodySimulate", cColorSoftBodySimulate, [ioStep, i]()
+			{
+				ioStep->mContext->mPhysicsSystem->JobSoftBodySimulate(ioStep->mContext, i);
+
+				ioStep->mSoftBodyFinalize.RemoveDependency();
+			}, num_soft_body_jobs); // depends on: soft body collide
+	ioContext->mBarrier->AddJobs(ioStep->mSoftBodySimulate.data(), ioStep->mSoftBodySimulate.size());
+
+	// Create collision jobs
+	ioStep->mSoftBodyCollide.resize(num_soft_body_jobs);
+	for (int i = 0; i < num_soft_body_jobs; ++i)
+		ioStep->mSoftBodyCollide[i] = ioContext->mJobSystem->CreateJob("SoftBodyCollide", cColorSoftBodyCollide, [ioContext, ioStep]()
+			{
+				ioContext->mPhysicsSystem->JobSoftBodyCollide(ioContext);
+
+				for (const JobHandle &h : ioStep->mSoftBodySimulate)
+					h.RemoveDependency();
+			}); // depends on: nothing
+	ioContext->mBarrier->AddJobs(ioStep->mSoftBodyCollide.data(), ioStep->mSoftBodyCollide.size());
+}
+
+void PhysicsSystem::JobSoftBodyCollide(PhysicsUpdateContext *ioContext) const
+{
+#ifdef JPH_ENABLE_ASSERTS
+	// Reading rigid body positions and velocities
+	BodyAccess::Grant grant(BodyAccess::EAccess::Read, BodyAccess::EAccess::Read);
+#endif
+
+	for (;;)
+	{
+		// Fetch the next soft body
+		uint sb_idx = ioContext->mSoftBodyToCollide.fetch_add(1, std::memory_order_acquire);
+		if (sb_idx >= ioContext->mNumSoftBodies)
+			break;
+
+		// Do a broadphase check
+		SoftBodyUpdateContext &sb_ctx = ioContext->mSoftBodyUpdateContexts[sb_idx];
+		sb_ctx.mMotionProperties->DetermineCollidingShapes(sb_ctx, *this, GetBodyLockInterfaceNoLock());
+	}
+}
+
+void PhysicsSystem::JobSoftBodySimulate(PhysicsUpdateContext *ioContext, uint inThreadIndex) const
+{
+#ifdef JPH_ENABLE_ASSERTS
+	// Updating velocities of soft bodies, allow the contact listener to read the soft body state
+	BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::Read);
+#endif
+
+	// Calculate at which body we start to distribute the workload across the threads
+	uint num_soft_bodies = ioContext->mNumSoftBodies;
+	uint start_idx = inThreadIndex * num_soft_bodies / ioContext->GetMaxConcurrency();
+
+	// Keep running partial updates until everything has been updated
+	uint status;
+	do
+	{
+		// Reset status
+		status = 0;
+
+		// Update all soft bodies
+		for (uint i = 0; i < num_soft_bodies; ++i)
+		{
+			// Fetch the soft body context
+			SoftBodyUpdateContext &sb_ctx = ioContext->mSoftBodyUpdateContexts[(start_idx + i) % num_soft_bodies];
+
+			// To avoid trashing the cache too much, we prefer to stick to one soft body until we cannot progress it any further
+			uint sb_status;
+			do
+			{
+				sb_status = (uint)sb_ctx.mMotionProperties->ParallelUpdate(sb_ctx, mPhysicsSettings);
+				status |= sb_status;
+			} while (sb_status == (uint)SoftBodyMotionProperties::EStatus::DidWork);
+		}
+
+		// If we didn't perform any work, yield the thread so that something else can run
+		if (!(status & (uint)SoftBodyMotionProperties::EStatus::DidWork))
+			std::this_thread::yield();
+	}
+	while (status != (uint)SoftBodyMotionProperties::EStatus::Done);
+}
+
+void PhysicsSystem::JobSoftBodyFinalize(PhysicsUpdateContext *ioContext)
+{
+#ifdef JPH_ENABLE_ASSERTS
+	// Updating rigid body velocities and soft body positions / velocities
+	BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::ReadWrite);
+
+	// Can activate and deactivate bodies
+	BodyManager::GrantActiveBodiesAccess grant_active(true, true);
+#endif
+
+	static constexpr int cBodiesBatch = 64;
+	BodyID *bodies_to_update_bounds = (BodyID *)JPH_STACK_ALLOC(cBodiesBatch * sizeof(BodyID));
+	int num_bodies_to_update_bounds = 0;
+	BodyID *bodies_to_put_to_sleep = (BodyID *)JPH_STACK_ALLOC(cBodiesBatch * sizeof(BodyID));
+	int num_bodies_to_put_to_sleep = 0;
+
+	for (SoftBodyUpdateContext *sb_ctx = ioContext->mSoftBodyUpdateContexts, *sb_ctx_end = ioContext->mSoftBodyUpdateContexts + ioContext->mNumSoftBodies; sb_ctx < sb_ctx_end; ++sb_ctx)
+	{
+		// Apply the rigid body velocity deltas
+		sb_ctx->mMotionProperties->UpdateRigidBodyVelocities(*sb_ctx, GetBodyInterfaceNoLock());
+
+		// Update the position
+		sb_ctx->mBody->SetPositionAndRotationInternal(sb_ctx->mBody->GetPosition() + sb_ctx->mDeltaPosition, sb_ctx->mBody->GetRotation(), false);
+
+		BodyID id = sb_ctx->mBody->GetID();
+		bodies_to_update_bounds[num_bodies_to_update_bounds++] = id;
+		if (num_bodies_to_update_bounds == cBodiesBatch)
+		{
+			// Buffer full, flush now
+			mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false);
+			num_bodies_to_update_bounds = 0;
+		}
+
+		if (sb_ctx->mCanSleep == ECanSleep::CanSleep)
+		{
+			// This body should go to sleep
+			bodies_to_put_to_sleep[num_bodies_to_put_to_sleep++] = id;
+			if (num_bodies_to_put_to_sleep == cBodiesBatch)
+			{
+				mBodyManager.DeactivateBodies(bodies_to_put_to_sleep, num_bodies_to_put_to_sleep);
+				num_bodies_to_put_to_sleep = 0;
+			}
+		}
+	}
+
+	// Notify change bounds on requested bodies
+	if (num_bodies_to_update_bounds > 0)
+		mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false);
+
+	// Notify bodies to go to sleep
+	if (num_bodies_to_put_to_sleep > 0)
+		mBodyManager.DeactivateBodies(bodies_to_put_to_sleep, num_bodies_to_put_to_sleep);
+
+	// Free soft body contexts
+	ioContext->mTempAllocator->Free(ioContext->mSoftBodyUpdateContexts, ioContext->mNumSoftBodies * sizeof(SoftBodyUpdateContext));
+}
+
+void PhysicsSystem::SaveState(StateRecorder &inStream, EStateRecorderState inState, const StateRecorderFilter *inFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	inStream.Write(inState);
+
+	if (uint8(inState) & uint8(EStateRecorderState::Global))
+	{
+		inStream.Write(mPreviousStepDeltaTime);
+		inStream.Write(mGravity);
+	}
+
+	if (uint8(inState) & uint8(EStateRecorderState::Bodies))
+		mBodyManager.SaveState(inStream, inFilter);
+
+	if (uint8(inState) & uint8(EStateRecorderState::Contacts))
+		mContactManager.SaveState(inStream, inFilter);
+
+	if (uint8(inState) & uint8(EStateRecorderState::Constraints))
+		mConstraintManager.SaveState(inStream, inFilter);
+}
+
+bool PhysicsSystem::RestoreState(StateRecorder &inStream, const StateRecorderFilter *inFilter)
+{
+	JPH_PROFILE_FUNCTION();
+
+	EStateRecorderState state = EStateRecorderState::All; // Set this value for validation. If a partial state is saved, validation will not work anyway.
+	inStream.Read(state);
+
+	if (uint8(state) & uint8(EStateRecorderState::Global))
+	{
+		inStream.Read(mPreviousStepDeltaTime);
+		inStream.Read(mGravity);
+	}
+
+	if (uint8(state) & uint8(EStateRecorderState::Bodies))
+	{
+		if (!mBodyManager.RestoreState(inStream))
+			return false;
+
+		// Update bounding boxes for all bodies in the broadphase
+		if (inStream.IsLastPart())
+		{
+			Array<BodyID> bodies;
+			for (const Body *b : mBodyManager.GetBodies())
+				if (BodyManager::sIsValidBodyPointer(b) && b->IsInBroadPhase())
+					bodies.push_back(b->GetID());
+			if (!bodies.empty())
+				mBroadPhase->NotifyBodiesAABBChanged(&bodies[0], (int)bodies.size());
+		}
+	}
+
+	if (uint8(state) & uint8(EStateRecorderState::Contacts))
+	{
+		if (!mContactManager.RestoreState(inStream, inFilter))
+			return false;
+	}
+
+	if (uint8(state) & uint8(EStateRecorderState::Constraints))
+	{
+		if (!mConstraintManager.RestoreState(inStream))
+			return false;
+	}
+
+	return true;
+}
+
+void PhysicsSystem::SaveBodyState(const Body &inBody, StateRecorder &inStream) const
+{
+	mBodyManager.SaveBodyState(inBody, inStream);
+}
+
+void PhysicsSystem::RestoreBodyState(Body &ioBody, StateRecorder &inStream)
+{
+	mBodyManager.RestoreBodyState(ioBody, inStream);
+
+	BodyID id = ioBody.GetID();
+	mBroadPhase->NotifyBodiesAABBChanged(&id, 1);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.h b/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.h
new file mode 100644
index 0000000000..5b1d1acf54
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsSystem.h
@@ -0,0 +1,338 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/BodyInterface.h>
+#include <Jolt/Physics/Collision/NarrowPhaseQuery.h>
+#include <Jolt/Physics/Collision/ContactListener.h>
+#include <Jolt/Physics/Constraints/ContactConstraintManager.h>
+#include <Jolt/Physics/Constraints/ConstraintManager.h>
+#include <Jolt/Physics/IslandBuilder.h>
+#include <Jolt/Physics/LargeIslandSplitter.h>
+#include <Jolt/Physics/PhysicsUpdateContext.h>
+#include <Jolt/Physics/PhysicsSettings.h>
+
+JPH_NAMESPACE_BEGIN
+
+class JobSystem;
+class StateRecorder;
+class TempAllocator;
+class PhysicsStepListener;
+class SoftBodyContactListener;
+class SimShapeFilter;
+
+/// The main class for the physics system. It contains all rigid bodies and simulates them.
+///
+/// The main simulation is performed by the Update() call on multiple threads (if the JobSystem is configured to use them). Please refer to the general architecture overview in the Docs folder for more information.
+class JPH_EXPORT PhysicsSystem : public NonCopyable
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor / Destructor
+								PhysicsSystem()												: mContactManager(mPhysicsSettings) JPH_IF_ENABLE_ASSERTS(, mConstraintManager(&mBodyManager)) { }
+								~PhysicsSystem();
+
+	/// Initialize the system.
+	/// @param inMaxBodies Maximum number of bodies to support.
+	/// @param inNumBodyMutexes Number of body mutexes to use. Should be a power of 2 in the range [1, 64], use 0 to auto detect.
+	/// @param inMaxBodyPairs Maximum amount of body pairs to process (anything else will fall through the world), this number should generally be much higher than the max amount of contact points as there will be lots of bodies close that are not actually touching.
+	/// @param inMaxContactConstraints Maximum amount of contact constraints to process (anything else will fall through the world).
+	/// @param inBroadPhaseLayerInterface Information on the mapping of object layers to broad phase layers. Since this is a virtual interface, the instance needs to stay alive during the lifetime of the PhysicsSystem.
+	/// @param inObjectVsBroadPhaseLayerFilter Filter callback function that is used to determine if an object layer collides with a broad phase layer. Since this is a virtual interface, the instance needs to stay alive during the lifetime of the PhysicsSystem.
+	/// @param inObjectLayerPairFilter Filter callback function that is used to determine if two object layers collide. Since this is a virtual interface, the instance needs to stay alive during the lifetime of the PhysicsSystem.
+	void						Init(uint inMaxBodies, uint inNumBodyMutexes, uint inMaxBodyPairs, uint inMaxContactConstraints, const BroadPhaseLayerInterface &inBroadPhaseLayerInterface, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter);
+
+	/// Listener that is notified whenever a body is activated/deactivated
+	void						SetBodyActivationListener(BodyActivationListener *inListener) { mBodyManager.SetBodyActivationListener(inListener); }
+	BodyActivationListener *	GetBodyActivationListener() const							{ return mBodyManager.GetBodyActivationListener(); }
+
+	/// Listener that is notified whenever a contact point between two bodies is added/updated/removed.
+	/// You can't change contact listener during PhysicsSystem::Update but it can be changed at any other time.
+	void						SetContactListener(ContactListener *inListener)				{ mContactManager.SetContactListener(inListener); }
+	ContactListener *			GetContactListener() const									{ return mContactManager.GetContactListener(); }
+
+	/// Listener that is notified whenever a contact point between a soft body and another body
+	void						SetSoftBodyContactListener(SoftBodyContactListener *inListener) { mSoftBodyContactListener = inListener; }
+	SoftBodyContactListener *	GetSoftBodyContactListener() const							{ return mSoftBodyContactListener; }
+
+	/// Set the function that combines the friction of two bodies and returns it
+	/// Default method is the geometric mean: sqrt(friction1 * friction2).
+	void						SetCombineFriction(ContactConstraintManager::CombineFunction inCombineFriction) { mContactManager.SetCombineFriction(inCombineFriction); }
+	ContactConstraintManager::CombineFunction GetCombineFriction() const					{ return mContactManager.GetCombineFriction(); }
+
+	/// Set the function that combines the restitution of two bodies and returns it
+	/// Default method is max(restitution1, restitution1)
+	void						SetCombineRestitution(ContactConstraintManager::CombineFunction inCombineRestition) { mContactManager.SetCombineRestitution(inCombineRestition); }
+	ContactConstraintManager::CombineFunction GetCombineRestitution() const					{ return mContactManager.GetCombineRestitution(); }
+
+	/// Set/get the shape filter that will be used during simulation. This can be used to exclude shapes within a body from colliding with each other.
+	/// E.g. if you have a high detail and a low detail collision model, you can attach them to the same body in a StaticCompoundShape and use the ShapeFilter
+	/// to exclude the high detail collision model when simulating and exclude the low detail collision model when casting rays. Note that in this case
+	/// you would need to pass the inverse of inShapeFilter to the CastRay function. Pass a nullptr to disable the shape filter.
+	/// The PhysicsSystem does not own the ShapeFilter, make sure it stays alive during the lifetime of the PhysicsSystem.
+	void						SetSimShapeFilter(const SimShapeFilter *inShapeFilter)		{ mSimShapeFilter = inShapeFilter; }
+	const SimShapeFilter *		GetSimShapeFilter() const									{ return mSimShapeFilter; }
+
+	/// Control the main constants of the physics simulation
+	void						SetPhysicsSettings(const PhysicsSettings &inSettings)		{ mPhysicsSettings = inSettings; }
+	const PhysicsSettings &		GetPhysicsSettings() const									{ return mPhysicsSettings; }
+
+	/// Access to the body interface. This interface allows to to create / remove bodies and to change their properties.
+	const BodyInterface &		GetBodyInterface() const									{ return mBodyInterfaceLocking; }
+	BodyInterface &				GetBodyInterface()											{ return mBodyInterfaceLocking; }
+	const BodyInterface &		GetBodyInterfaceNoLock() const								{ return mBodyInterfaceNoLock; } ///< Version that does not lock the bodies, use with great care!
+	BodyInterface &				GetBodyInterfaceNoLock()									{ return mBodyInterfaceNoLock; } ///< Version that does not lock the bodies, use with great care!
+
+	/// Access to the broadphase interface that allows coarse collision queries
+	const BroadPhaseQuery &		GetBroadPhaseQuery() const									{ return *mBroadPhase; }
+
+	/// Interface that allows fine collision queries against first the broad phase and then the narrow phase.
+	const NarrowPhaseQuery &	GetNarrowPhaseQuery() const									{ return mNarrowPhaseQueryLocking; }
+	const NarrowPhaseQuery &	GetNarrowPhaseQueryNoLock() const							{ return mNarrowPhaseQueryNoLock; } ///< Version that does not lock the bodies, use with great care!
+
+	/// Add constraint to the world
+	void						AddConstraint(Constraint *inConstraint)						{ mConstraintManager.Add(&inConstraint, 1); }
+
+	/// Remove constraint from the world
+	void						RemoveConstraint(Constraint *inConstraint)					{ mConstraintManager.Remove(&inConstraint, 1); }
+
+	/// Batch add constraints.
+	void						AddConstraints(Constraint **inConstraints, int inNumber)	{ mConstraintManager.Add(inConstraints, inNumber); }
+
+	/// Batch remove constraints.
+	void						RemoveConstraints(Constraint **inConstraints, int inNumber)	{ mConstraintManager.Remove(inConstraints, inNumber); }
+
+	/// Get a list of all constraints
+	Constraints					GetConstraints() const										{ return mConstraintManager.GetConstraints(); }
+
+	/// Optimize the broadphase, needed only if you've added many bodies prior to calling Update() for the first time.
+	/// Don't call this every frame as PhysicsSystem::Update spreads out the same work over multiple frames.
+	/// If you add many bodies through BodyInterface::AddBodiesPrepare/AddBodiesFinalize and if the bodies in a batch are
+	/// in a roughly unoccupied space (e.g. a new level section) then a call to OptimizeBroadPhase is also not needed
+	/// as batch adding creates an efficient bounding volume hierarchy.
+	/// Don't call this function while bodies are being modified from another thread or use the locking BodyInterface to modify bodies.
+	void						OptimizeBroadPhase();
+
+	/// Adds a new step listener
+	void						AddStepListener(PhysicsStepListener *inListener);
+
+	/// Removes a step listener
+	void						RemoveStepListener(PhysicsStepListener *inListener);
+
+	/// Simulate the system.
+	/// The world steps for a total of inDeltaTime seconds. This is divided in inCollisionSteps iterations.
+	/// Each iteration consists of collision detection followed by an integration step.
+	/// This function internally spawns jobs using inJobSystem and waits for them to complete, so no jobs will be running when this function returns.
+	EPhysicsUpdateError			Update(float inDeltaTime, int inCollisionSteps, TempAllocator *inTempAllocator, JobSystem *inJobSystem);
+
+	/// Saving state for replay
+	void						SaveState(StateRecorder &inStream, EStateRecorderState inState = EStateRecorderState::All, const StateRecorderFilter *inFilter = nullptr) const;
+
+	/// Restoring state for replay. Returns false if failed.
+	bool						RestoreState(StateRecorder &inStream, const StateRecorderFilter *inFilter = nullptr);
+
+	/// Saving state of a single body.
+	void						SaveBodyState(const Body &inBody, StateRecorder &inStream) const;
+
+	/// Restoring state of a single body.
+	void						RestoreBodyState(Body &ioBody, StateRecorder &inStream);
+
+#ifdef JPH_DEBUG_RENDERER
+	// Drawing properties
+	static bool					sDrawMotionQualityLinearCast;								///< Draw debug info for objects that perform continuous collision detection through the linear cast motion quality
+
+	/// Draw the state of the bodies (debugging purposes)
+	void						DrawBodies(const BodyManager::DrawSettings &inSettings, DebugRenderer *inRenderer, const BodyDrawFilter *inBodyFilter = nullptr) { mBodyManager.Draw(inSettings, mPhysicsSettings, inRenderer, inBodyFilter); }
+
+	/// Draw the constraints only (debugging purposes)
+	void						DrawConstraints(DebugRenderer *inRenderer)					{ mConstraintManager.DrawConstraints(inRenderer); }
+
+	/// Draw the constraint limits only (debugging purposes)
+	void						DrawConstraintLimits(DebugRenderer *inRenderer)				{ mConstraintManager.DrawConstraintLimits(inRenderer); }
+
+	/// Draw the constraint reference frames only (debugging purposes)
+	void						DrawConstraintReferenceFrame(DebugRenderer *inRenderer)		{ mConstraintManager.DrawConstraintReferenceFrame(inRenderer); }
+#endif // JPH_DEBUG_RENDERER
+
+	/// Set gravity value
+	void						SetGravity(Vec3Arg inGravity)								{ mGravity = inGravity; }
+	Vec3						GetGravity() const											{ return mGravity; }
+
+	/// Returns a locking interface that won't actually lock the body. Use with great care!
+	inline const BodyLockInterfaceNoLock &	GetBodyLockInterfaceNoLock() const				{ return mBodyLockInterfaceNoLock; }
+
+	/// Returns a locking interface that locks the body so other threads cannot modify it.
+	inline const BodyLockInterfaceLocking &	GetBodyLockInterface() const					{ return mBodyLockInterfaceLocking; }
+
+	/// Get an broadphase layer filter that uses the default pair filter and a specified object layer to determine if broadphase layers collide
+	DefaultBroadPhaseLayerFilter GetDefaultBroadPhaseLayerFilter(ObjectLayer inLayer) const	{ return DefaultBroadPhaseLayerFilter(*mObjectVsBroadPhaseLayerFilter, inLayer); }
+
+	/// Get an object layer filter that uses the default pair filter and a specified layer to determine if layers collide
+	DefaultObjectLayerFilter	GetDefaultLayerFilter(ObjectLayer inLayer) const			{ return DefaultObjectLayerFilter(*mObjectLayerPairFilter, inLayer); }
+
+	/// Gets the current amount of bodies that are in the body manager
+	uint						GetNumBodies() const										{ return mBodyManager.GetNumBodies(); }
+
+	/// Gets the current amount of active bodies that are in the body manager
+	uint32						GetNumActiveBodies(EBodyType inType) const					{ return mBodyManager.GetNumActiveBodies(inType); }
+
+	/// Get the maximum amount of bodies that this physics system supports
+	uint						GetMaxBodies() const										{ return mBodyManager.GetMaxBodies(); }
+
+	/// Helper struct that counts the number of bodies of each type
+	using BodyStats = BodyManager::BodyStats;
+
+	/// Get stats about the bodies in the body manager (slow, iterates through all bodies)
+	BodyStats					GetBodyStats() const										{ return mBodyManager.GetBodyStats(); }
+
+	/// Get copy of the list of all bodies under protection of a lock.
+	/// @param outBodyIDs On return, this will contain the list of BodyIDs
+	void						GetBodies(BodyIDVector &outBodyIDs) const					{ return mBodyManager.GetBodyIDs(outBodyIDs); }
+
+	/// Get copy of the list of active bodies under protection of a lock.
+	/// @param inType The type of bodies to get
+	/// @param outBodyIDs On return, this will contain the list of BodyIDs
+	void						GetActiveBodies(EBodyType inType, BodyIDVector &outBodyIDs) const { return mBodyManager.GetActiveBodies(inType, outBodyIDs); }
+
+	/// Get the list of active bodies, use GetNumActiveBodies() to find out how long the list is.
+	/// Note: Not thread safe. The active bodies list can change at any moment when other threads are doing work. Use GetActiveBodies() if you need a thread safe version.
+	const BodyID *				GetActiveBodiesUnsafe(EBodyType inType) const				{ return mBodyManager.GetActiveBodiesUnsafe(inType); }
+
+	/// Check if 2 bodies were in contact during the last simulation step. Since contacts are only detected between active bodies, so at least one of the bodies must be active in order for this function to work.
+	/// It queries the state at the time of the last PhysicsSystem::Update and will return true if the bodies were in contact, even if one of the bodies was moved / removed afterwards.
+	/// This function can be called from any thread when the PhysicsSystem::Update is not running. During PhysicsSystem::Update this function is only valid during contact callbacks:
+	/// - During the ContactListener::OnContactAdded callback this function can be used to determine if a different contact pair between the bodies was active in the previous simulation step (function returns true) or if this is the first step that the bodies are touching (function returns false).
+	/// - During the ContactListener::OnContactRemoved callback this function can be used to determine if this is the last contact pair between the bodies (function returns false) or if there are other contacts still present (function returns true).
+	bool						WereBodiesInContact(const BodyID &inBody1ID, const BodyID &inBody2ID) const { return mContactManager.WereBodiesInContact(inBody1ID, inBody2ID); }
+
+	/// Get the bounding box of all bodies in the physics system
+	AABox						GetBounds() const											{ return mBroadPhase->GetBounds(); }
+
+#ifdef JPH_TRACK_BROADPHASE_STATS
+	/// Trace the accumulated broadphase stats to the TTY
+	void						ReportBroadphaseStats()										{ mBroadPhase->ReportStats(); }
+#endif // JPH_TRACK_BROADPHASE_STATS
+
+private:
+	using CCDBody = PhysicsUpdateContext::Step::CCDBody;
+
+	// Various job entry points
+	void						JobStepListeners(PhysicsUpdateContext::Step *ioStep);
+	void						JobDetermineActiveConstraints(PhysicsUpdateContext::Step *ioStep) const;
+	void						JobApplyGravity(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep);
+	void						JobSetupVelocityConstraints(float inDeltaTime, PhysicsUpdateContext::Step *ioStep) const;
+	void						JobBuildIslandsFromConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep);
+	void						JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int inJobIndex);
+	void						JobFinalizeIslands(PhysicsUpdateContext *ioContext);
+	void						JobBodySetIslandIndex();
+	void						JobSolveVelocityConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep);
+	void						JobPreIntegrateVelocity(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep);
+	void						JobIntegrateVelocity(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep);
+	void						JobPostIntegrateVelocity(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) const;
+	void						JobFindCCDContacts(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep);
+	void						JobResolveCCDContacts(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep);
+	void						JobContactRemovedCallbacks(const PhysicsUpdateContext::Step *ioStep);
+	void						JobSolvePositionConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep);
+	void						JobSoftBodyPrepare(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep);
+	void						JobSoftBodyCollide(PhysicsUpdateContext *ioContext) const;
+	void						JobSoftBodySimulate(PhysicsUpdateContext *ioContext, uint inThreadIndex) const;
+	void						JobSoftBodyFinalize(PhysicsUpdateContext *ioContext);
+
+	/// Tries to spawn a new FindCollisions job if max concurrency hasn't been reached yet
+	void						TrySpawnJobFindCollisions(PhysicsUpdateContext::Step *ioStep) const;
+
+	using ContactAllocator = ContactConstraintManager::ContactAllocator;
+
+	/// Process narrow phase for a single body pair
+	void						ProcessBodyPair(ContactAllocator &ioContactAllocator, const BodyPair &inBodyPair);
+
+	/// This helper batches up bodies that need to put to sleep to avoid contention on the activation mutex
+	class BodiesToSleep;
+
+	/// Called at the end of JobSolveVelocityConstraints to check if bodies need to go to sleep and to update their bounding box in the broadphase
+	void						CheckSleepAndUpdateBounds(uint32 inIslandIndex, const PhysicsUpdateContext *ioContext, const PhysicsUpdateContext::Step *ioStep, BodiesToSleep &ioBodiesToSleep);
+
+	/// Number of constraints to process at once in JobDetermineActiveConstraints
+	static constexpr int		cDetermineActiveConstraintsBatchSize = 64;
+
+	/// Number of constraints to process at once in JobSetupVelocityConstraints, we want a low number of threads working on this so we take fairly large batches
+	static constexpr int		cSetupVelocityConstraintsBatchSize = 256;
+
+	/// Number of bodies to process at once in JobApplyGravity
+	static constexpr int		cApplyGravityBatchSize = 64;
+
+	/// Number of active bodies to test for collisions per batch
+	static constexpr int		cActiveBodiesBatchSize = 16;
+
+	/// Number of active bodies to integrate velocities for
+	static constexpr int		cIntegrateVelocityBatchSize = 64;
+
+	/// Number of contacts that need to be queued before another narrow phase job is started
+	static constexpr int		cNarrowPhaseBatchSize = 16;
+
+	/// Number of continuous collision shape casts that need to be queued before another job is started
+	static constexpr int		cNumCCDBodiesPerJob = 4;
+
+	/// Broadphase layer filter that decides if two objects can collide
+	const ObjectVsBroadPhaseLayerFilter *mObjectVsBroadPhaseLayerFilter = nullptr;
+
+	/// Object layer filter that decides if two objects can collide
+	const ObjectLayerPairFilter *mObjectLayerPairFilter = nullptr;
+
+	/// The body manager keeps track which bodies are in the simulation
+	BodyManager					mBodyManager;
+
+	/// Body locking interfaces
+	BodyLockInterfaceNoLock		mBodyLockInterfaceNoLock { mBodyManager };
+	BodyLockInterfaceLocking	mBodyLockInterfaceLocking { mBodyManager };
+
+	/// Body interfaces
+	BodyInterface				mBodyInterfaceNoLock;
+	BodyInterface				mBodyInterfaceLocking;
+
+	/// Narrow phase query interface
+	NarrowPhaseQuery			mNarrowPhaseQueryNoLock;
+	NarrowPhaseQuery			mNarrowPhaseQueryLocking;
+
+	/// The broadphase does quick collision detection between body pairs
+	BroadPhase *				mBroadPhase = nullptr;
+
+	/// The soft body contact listener
+	SoftBodyContactListener *	mSoftBodyContactListener = nullptr;
+
+	/// The shape filter that is used to filter out sub shapes during simulation
+	const SimShapeFilter *		mSimShapeFilter = nullptr;
+
+	/// Simulation settings
+	PhysicsSettings				mPhysicsSettings;
+
+	/// The contact manager resolves all contacts during a simulation step
+	ContactConstraintManager	mContactManager;
+
+	/// All non-contact constraints
+	ConstraintManager			mConstraintManager;
+
+	/// Keeps track of connected bodies and builds islands for multithreaded velocity/position update
+	IslandBuilder				mIslandBuilder;
+
+	/// Will split large islands into smaller groups of bodies that can be processed in parallel
+	LargeIslandSplitter			mLargeIslandSplitter;
+
+	/// Mutex protecting mStepListeners
+	Mutex						mStepListenersMutex;
+
+	/// List of physics step listeners
+	using StepListeners = Array<PhysicsStepListener *>;
+	StepListeners				mStepListeners;
+
+	/// This is the global gravity vector
+	Vec3						mGravity = Vec3(0, -9.81f, 0);
+
+	/// Previous frame's delta time of one sub step to allow scaling previous frame's constraint impulses
+	float						mPreviousStepDeltaTime = 0.0f;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsUpdateContext.cpp b/thirdparty/jolt_physics/Jolt/Physics/PhysicsUpdateContext.cpp
new file mode 100644
index 0000000000..7c60ae6be6
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsUpdateContext.cpp
@@ -0,0 +1,23 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/PhysicsUpdateContext.h>
+
+JPH_NAMESPACE_BEGIN
+
+PhysicsUpdateContext::PhysicsUpdateContext(TempAllocator &inTempAllocator) :
+	mTempAllocator(&inTempAllocator),
+	mSteps(inTempAllocator)
+{
+}
+
+PhysicsUpdateContext::~PhysicsUpdateContext()
+{
+	JPH_ASSERT(mBodyPairs == nullptr);
+	JPH_ASSERT(mActiveConstraints == nullptr);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/PhysicsUpdateContext.h b/thirdparty/jolt_physics/Jolt/Physics/PhysicsUpdateContext.h
new file mode 100644
index 0000000000..fe99c46ccc
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/PhysicsUpdateContext.h
@@ -0,0 +1,172 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/BodyPair.h>
+#include <Jolt/Physics/Collision/ContactListener.h>
+#include <Jolt/Physics/Collision/BroadPhase/BroadPhase.h>
+#include <Jolt/Core/StaticArray.h>
+#include <Jolt/Core/JobSystem.h>
+#include <Jolt/Core/STLTempAllocator.h>
+
+JPH_NAMESPACE_BEGIN
+
+class PhysicsSystem;
+class IslandBuilder;
+class Constraint;
+class TempAllocator;
+class SoftBodyUpdateContext;
+
+/// Information used during the Update call
+class PhysicsUpdateContext : public NonCopyable
+{
+public:
+	/// Destructor
+	explicit				PhysicsUpdateContext(TempAllocator &inTempAllocator);
+							~PhysicsUpdateContext();
+
+	static constexpr int	cMaxConcurrency = 32;									///< Maximum supported amount of concurrent jobs
+
+	using JobHandleArray = StaticArray<JobHandle, cMaxConcurrency>;
+
+	struct Step;
+
+	struct BodyPairQueue
+	{
+		atomic<uint32>		mWriteIdx { 0 };										///< Next index to write in mBodyPair array (need to add thread index * mMaxBodyPairsPerQueue and modulo mMaxBodyPairsPerQueue)
+		uint8				mPadding1[JPH_CACHE_LINE_SIZE - sizeof(atomic<uint32>)];///< Moved to own cache line to avoid conflicts with consumer jobs
+
+		atomic<uint32>		mReadIdx { 0 };											///< Next index to read in mBodyPair array (need to add thread index * mMaxBodyPairsPerQueue and modulo mMaxBodyPairsPerQueue)
+		uint8				mPadding2[JPH_CACHE_LINE_SIZE - sizeof(atomic<uint32>)];///< Moved to own cache line to avoid conflicts with producer/consumer jobs
+	};
+
+	using BodyPairQueues = StaticArray<BodyPairQueue, cMaxConcurrency>;
+
+	using JobMask = uint32;															///< A mask that has as many bits as we can have concurrent jobs
+	static_assert(sizeof(JobMask) * 8 >= cMaxConcurrency);
+
+	/// Structure that contains data needed for each collision step.
+	struct Step
+	{
+							Step() = default;
+							Step(const Step &)										{ JPH_ASSERT(false); } // vector needs a copy constructor, but we're never going to call it
+
+		PhysicsUpdateContext *mContext;												///< The physics update context
+
+		bool				mIsFirst;												///< If this is the first step
+		bool				mIsLast;												///< If this is the last step
+
+		BroadPhase::UpdateState	mBroadPhaseUpdateState;								///< Handle returned by Broadphase::UpdatePrepare
+
+		uint32				mNumActiveBodiesAtStepStart;							///< Number of bodies that were active at the start of the physics update step. Only these bodies will receive gravity (they are the first N in the active body list).
+
+		atomic<uint32>		mDetermineActiveConstraintReadIdx { 0 };				///< Next constraint for determine active constraints
+		uint8				mPadding1[JPH_CACHE_LINE_SIZE - sizeof(atomic<uint32>)];///< Padding to avoid sharing cache line with the next atomic
+
+		atomic<uint32>		mNumActiveConstraints { 0 };							///< Number of constraints in the mActiveConstraints array
+		uint8				mPadding2[JPH_CACHE_LINE_SIZE - sizeof(atomic<uint32>)];///< Padding to avoid sharing cache line with the next atomic
+
+		atomic<uint32>		mSetupVelocityConstraintsReadIdx { 0 };					///< Next constraint for setting up velocity constraints
+		uint8				mPadding3[JPH_CACHE_LINE_SIZE - sizeof(atomic<uint32>)];///< Padding to avoid sharing cache line with the next atomic
+
+		atomic<uint32>		mStepListenerReadIdx { 0 };								///< Next step listener to call
+		uint8				mPadding4[JPH_CACHE_LINE_SIZE - sizeof(atomic<uint32>)];///< Padding to avoid sharing cache line with the next atomic
+
+		atomic<uint32>		mApplyGravityReadIdx { 0 };								///< Next body to apply gravity to
+		uint8				mPadding5[JPH_CACHE_LINE_SIZE - sizeof(atomic<uint32>)];///< Padding to avoid sharing cache line with the next atomic
+
+		atomic<uint32>		mActiveBodyReadIdx { 0 };								///< Index of fist active body that has not yet been processed by the broadphase
+		uint8				mPadding6[JPH_CACHE_LINE_SIZE - sizeof(atomic<uint32>)];///< Padding to avoid sharing cache line with the next atomic
+
+		BodyPairQueues		mBodyPairQueues;										///< Queues in which to put body pairs that need to be tested by the narrowphase
+
+		uint32				mMaxBodyPairsPerQueue;									///< Amount of body pairs that we can queue per queue
+
+		atomic<JobMask>		mActiveFindCollisionJobs;								///< A bitmask that indicates which jobs are still active
+
+		atomic<uint>		mNumBodyPairs { 0 };									///< The number of body pairs found in this step (used to size the contact cache in the next step)
+		atomic<uint>		mNumManifolds { 0 };									///< The number of manifolds found in this step (used to size the contact cache in the next step)
+
+		atomic<uint32>		mSolveVelocityConstraintsNextIsland { 0 };				///< Next island that needs to be processed for the solve velocity constraints step (doesn't need own cache line since position jobs don't run at same time)
+		atomic<uint32>		mSolvePositionConstraintsNextIsland { 0 };				///< Next island that needs to be processed for the solve position constraints step (doesn't need own cache line since velocity jobs don't run at same time)
+
+		/// Contains the information needed to cast a body through the scene to do continuous collision detection
+		struct CCDBody
+		{
+							CCDBody(BodyID inBodyID1, Vec3Arg inDeltaPosition, float inLinearCastThresholdSq, float inMaxPenetration) : mDeltaPosition(inDeltaPosition), mBodyID1(inBodyID1), mLinearCastThresholdSq(inLinearCastThresholdSq), mMaxPenetration(inMaxPenetration) { }
+
+			Vec3			mDeltaPosition;											///< Desired rotation step
+			Vec3			mContactNormal;											///< World space normal of closest hit (only valid if mFractionPlusSlop < 1)
+			RVec3			mContactPointOn2;										///< World space contact point on body 2 of closest hit (only valid if mFractionPlusSlop < 1)
+			BodyID			mBodyID1;												///< Body 1 (the body that is performing collision detection)
+			BodyID			mBodyID2;												///< Body 2 (the body of the closest hit, only valid if mFractionPlusSlop < 1)
+			SubShapeID		mSubShapeID2;											///< Sub shape of body 2 that was hit (only valid if mFractionPlusSlop < 1)
+			float			mFraction = 1.0f;										///< Fraction at which the hit occurred
+			float			mFractionPlusSlop = 1.0f;								///< Fraction at which the hit occurred + extra delta to allow body to penetrate by mMaxPenetration
+			float			mLinearCastThresholdSq;									///< Maximum allowed squared movement before doing a linear cast (determined by inner radius of shape)
+			float			mMaxPenetration;										///< Maximum allowed penetration (determined by inner radius of shape)
+			ContactSettings	mContactSettings;										///< The contact settings for this contact
+		};
+		atomic<uint32>		mIntegrateVelocityReadIdx { 0 };						///< Next active body index to take when integrating velocities
+		CCDBody *			mCCDBodies = nullptr;									///< List of bodies that need to do continuous collision detection
+		uint32				mCCDBodiesCapacity = 0;									///< Capacity of the mCCDBodies list
+		atomic<uint32>		mNumCCDBodies = 0;										///< Number of CCD bodies in mCCDBodies
+		atomic<uint32>		mNextCCDBody { 0 };										///< Next unprocessed body index in mCCDBodies
+		int *				mActiveBodyToCCDBody = nullptr;							///< A mapping between an index in BodyManager::mActiveBodies and the index in mCCDBodies
+		uint32				mNumActiveBodyToCCDBody = 0;							///< Number of indices in mActiveBodyToCCDBody
+
+		// Jobs in order of execution (some run in parallel)
+		JobHandle			mBroadPhasePrepare;										///< Prepares the new tree in the background
+		JobHandleArray		mStepListeners;											///< Listeners to notify of the beginning of a physics step
+		JobHandleArray		mDetermineActiveConstraints;							///< Determine which constraints will be active during this step
+		JobHandleArray		mApplyGravity;											///< Update velocities of bodies with gravity
+		JobHandleArray		mFindCollisions;										///< Find all collisions between active bodies an the world
+		JobHandle			mUpdateBroadphaseFinalize;								///< Swap the newly built tree with the current tree
+		JobHandleArray		mSetupVelocityConstraints;								///< Calculate properties for all constraints in the constraint manager
+		JobHandle			mBuildIslandsFromConstraints;							///< Go over all constraints and assign the bodies they're attached to to an island
+		JobHandle			mFinalizeIslands;										///< Finalize calculation simulation islands
+		JobHandle			mBodySetIslandIndex;									///< Set the current island index on each body (not used by the simulation, only for drawing purposes)
+		JobHandleArray		mSolveVelocityConstraints;								///< Solve the constraints in the velocity domain
+		JobHandle			mPreIntegrateVelocity;									///< Setup integration of all body positions
+		JobHandleArray		mIntegrateVelocity;										///< Integrate all body positions
+		JobHandle			mPostIntegrateVelocity;									///< Finalize integration of all body positions
+		JobHandle			mResolveCCDContacts;									///< Updates the positions and velocities for all bodies that need continuous collision detection
+		JobHandleArray		mSolvePositionConstraints;								///< Solve all constraints in the position domain
+		JobHandle			mContactRemovedCallbacks;								///< Calls the contact removed callbacks
+		JobHandle			mSoftBodyPrepare;										///< Prepares updating the soft bodies
+		JobHandleArray		mSoftBodyCollide;										///< Finds all colliding shapes for soft bodies
+		JobHandleArray		mSoftBodySimulate;										///< Simulates all particles
+		JobHandle			mSoftBodyFinalize;										///< Finalizes the soft body update
+		JobHandle			mStartNextStep;											///< Job that kicks the next step (empty for the last step)
+	};
+
+	using Steps = Array<Step, STLTempAllocator<Step>>;
+
+	/// Maximum amount of concurrent jobs on this machine
+	int						GetMaxConcurrency() const								{ const int max_concurrency = PhysicsUpdateContext::cMaxConcurrency; return min(max_concurrency, mJobSystem->GetMaxConcurrency()); } ///< Need to put max concurrency in temp var as min requires a reference
+
+	PhysicsSystem *			mPhysicsSystem;											///< The physics system we belong to
+	TempAllocator *			mTempAllocator;											///< Temporary allocator used during the update
+	JobSystem *				mJobSystem;												///< Job system that processes jobs
+	JobSystem::Barrier *	mBarrier;												///< Barrier used to wait for all physics jobs to complete
+
+	float					mStepDeltaTime;											///< Delta time for a simulation step (collision step)
+	float					mWarmStartImpulseRatio;									///< Ratio of this step delta time vs last step
+	atomic<uint32>			mErrors { 0 };											///< Errors that occurred during the update, actual type is EPhysicsUpdateError
+
+	Constraint **			mActiveConstraints = nullptr;							///< Constraints that were active at the start of the physics update step (activating bodies can activate constraints and we need a consistent snapshot). Only these constraints will be resolved.
+
+	BodyPair *				mBodyPairs = nullptr;									///< A list of body pairs found by the broadphase
+
+	IslandBuilder *			mIslandBuilder;											///< Keeps track of connected bodies and builds islands for multithreaded velocity/position update
+
+	Steps					mSteps;
+
+	uint					mNumSoftBodies;											///< Number of active soft bodies in the simulation
+	SoftBodyUpdateContext *	mSoftBodyUpdateContexts = nullptr;						///< Contexts for updating soft bodies
+	atomic<uint>			mSoftBodyToCollide { 0 };								///< Next soft body to take when running SoftBodyCollide jobs
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.cpp b/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.cpp
new file mode 100644
index 0000000000..9d7e5b675d
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.cpp
@@ -0,0 +1,705 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Ragdoll/Ragdoll.h>
+#include <Jolt/Physics/Constraints/SwingTwistConstraint.h>
+#include <Jolt/Physics/PhysicsSystem.h>
+#include <Jolt/Physics/Body/BodyLockMulti.h>
+#include <Jolt/Physics/Collision/GroupFilterTable.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/Collision/CollideShape.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(RagdollSettings::Part)
+{
+	JPH_ADD_BASE_CLASS(RagdollSettings::Part, BodyCreationSettings)
+
+	JPH_ADD_ATTRIBUTE(RagdollSettings::Part, mToParent)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(RagdollSettings::AdditionalConstraint)
+{
+	JPH_ADD_ATTRIBUTE(RagdollSettings::AdditionalConstraint, mBodyIdx)
+	JPH_ADD_ATTRIBUTE(RagdollSettings::AdditionalConstraint, mConstraint)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(RagdollSettings)
+{
+	JPH_ADD_ATTRIBUTE(RagdollSettings, mSkeleton)
+	JPH_ADD_ATTRIBUTE(RagdollSettings, mParts)
+	JPH_ADD_ATTRIBUTE(RagdollSettings, mAdditionalConstraints)
+}
+
+static inline BodyInterface &sGetBodyInterface(PhysicsSystem *inSystem, bool inLockBodies)
+{
+	return inLockBodies? inSystem->GetBodyInterface() : inSystem->GetBodyInterfaceNoLock();
+}
+
+static inline const BodyLockInterface &sGetBodyLockInterface(const PhysicsSystem *inSystem, bool inLockBodies)
+{
+	return inLockBodies? static_cast<const BodyLockInterface &>(inSystem->GetBodyLockInterface()) : static_cast<const BodyLockInterface &>(inSystem->GetBodyLockInterfaceNoLock());
+}
+
+bool RagdollSettings::Stabilize()
+{
+	// Based on: Stop my Constraints from Blowing Up! - Oliver Strunk (Havok)
+	// Do 2 things:
+	// 1. Limit the mass ratios between parents and children (slide 16)
+	// 2. Increase the inertia of parents so that they're bigger or equal to the sum of their children (slide 34)
+
+	// If we don't have any joints there's nothing to stabilize
+	if (mSkeleton->GetJointCount() == 0)
+		return true;
+
+	// The skeleton can contain one or more static bodies. We can't modify the mass for those so we start a new stabilization chain for each joint under a static body until we reach the next static body.
+	// This array keeps track of which joints have been processed.
+	Array<bool> visited;
+	visited.resize(mSkeleton->GetJointCount());
+	for (size_t v = 0; v < visited.size(); ++v)
+	{
+		// Mark static bodies as visited so we won't process these
+		Part &p = mParts[v];
+		bool has_mass_properties = p.HasMassProperties();
+		visited[v] = !has_mass_properties;
+
+		if (has_mass_properties && p.mOverrideMassProperties != EOverrideMassProperties::MassAndInertiaProvided)
+		{
+			// Mass properties not yet calculated, do it now
+			p.mMassPropertiesOverride = p.GetMassProperties();
+			p.mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided;
+		}
+	}
+
+	// Find first unvisited part that either has no parent or that has a parent that was visited
+	for (int first_idx = 0; first_idx < mSkeleton->GetJointCount(); ++first_idx)
+	{
+		int first_idx_parent = mSkeleton->GetJoint(first_idx).mParentJointIndex;
+		if (!visited[first_idx] && (first_idx_parent == -1 || visited[first_idx_parent]))
+		{
+			// Find all children of first_idx and their children up to the next static part
+			int next_to_process = 0;
+			Array<int> indices;
+			indices.reserve(mSkeleton->GetJointCount());
+			visited[first_idx] = true;
+			indices.push_back(first_idx);
+			do
+			{
+				int parent_idx = indices[next_to_process++];
+				for (int child_idx = 0; child_idx < mSkeleton->GetJointCount(); ++child_idx)
+					if (!visited[child_idx] && mSkeleton->GetJoint(child_idx).mParentJointIndex == parent_idx)
+					{
+						visited[child_idx] = true;
+						indices.push_back(child_idx);
+					}
+			} while (next_to_process < (int)indices.size());
+
+			// If there's only 1 body, we can't redistribute mass
+			if (indices.size() == 1)
+				continue;
+
+			const float cMinMassRatio = 0.8f;
+			const float cMaxMassRatio = 1.2f;
+
+			// Ensure that the mass ratio from parent to child is within a range
+			float total_mass_ratio = 1.0f;
+			Array<float> mass_ratios;
+			mass_ratios.resize(mSkeleton->GetJointCount());
+			mass_ratios[indices[0]] = 1.0f;
+			for (int i = 1; i < (int)indices.size(); ++i)
+			{
+				int child_idx = indices[i];
+				int parent_idx = mSkeleton->GetJoint(child_idx).mParentJointIndex;
+				float ratio = mParts[child_idx].mMassPropertiesOverride.mMass / mParts[parent_idx].mMassPropertiesOverride.mMass;
+				mass_ratios[child_idx] = mass_ratios[parent_idx] * Clamp(ratio, cMinMassRatio, cMaxMassRatio);
+				total_mass_ratio += mass_ratios[child_idx];
+			}
+
+			// Calculate total mass of this chain
+			float total_mass = 0.0f;
+			for (int idx : indices)
+				total_mass += mParts[idx].mMassPropertiesOverride.mMass;
+
+			// Calculate how much mass belongs to a ratio of 1
+			float ratio_to_mass = total_mass / total_mass_ratio;
+
+			// Adjust all masses and inertia tensors for the new mass
+			for (int i : indices)
+			{
+				Part &p = mParts[i];
+				float old_mass = p.mMassPropertiesOverride.mMass;
+				float new_mass = mass_ratios[i] * ratio_to_mass;
+				p.mMassPropertiesOverride.mMass = new_mass;
+				p.mMassPropertiesOverride.mInertia *= new_mass / old_mass;
+				p.mMassPropertiesOverride.mInertia.SetColumn4(3, Vec4(0, 0, 0, 1));
+			}
+
+			const float cMaxInertiaIncrease = 2.0f;
+
+			// Get the principal moments of inertia for all parts
+			struct Principal
+			{
+				Mat44	mRotation;
+				Vec3	mDiagonal;
+				float	mChildSum = 0.0f;
+			};
+			Array<Principal> principals;
+			principals.resize(mParts.size());
+			for (int i : indices)
+				if (!mParts[i].mMassPropertiesOverride.DecomposePrincipalMomentsOfInertia(principals[i].mRotation, principals[i].mDiagonal))
+				{
+					JPH_ASSERT(false, "Failed to decompose the inertia tensor!");
+					return false;
+				}
+
+			// Calculate sum of child inertias
+			// Walk backwards so we sum the leaves first
+			for (int i = (int)indices.size() - 1; i > 0; --i)
+			{
+				int child_idx = indices[i];
+				int parent_idx = mSkeleton->GetJoint(child_idx).mParentJointIndex;
+				principals[parent_idx].mChildSum += principals[child_idx].mDiagonal[0] + principals[child_idx].mChildSum;
+			}
+
+			// Adjust inertia tensors for all parts
+			for (int i : indices)
+			{
+				Part &p = mParts[i];
+				Principal &principal = principals[i];
+				if (principal.mChildSum != 0.0f)
+				{
+					// Calculate minimum inertia this object should have based on it children
+					float minimum = min(cMaxInertiaIncrease * principal.mDiagonal[0], principal.mChildSum);
+					principal.mDiagonal = Vec3::sMax(principal.mDiagonal, Vec3::sReplicate(minimum));
+
+					// Recalculate moment of inertia in body space
+					p.mMassPropertiesOverride.mInertia = principal.mRotation * Mat44::sScale(principal.mDiagonal) * principal.mRotation.Inversed3x3();
+				}
+			}
+		}
+	}
+
+	return true;
+}
+
+void RagdollSettings::DisableParentChildCollisions(const Mat44 *inJointMatrices, float inMinSeparationDistance)
+{
+	int joint_count = mSkeleton->GetJointCount();
+	JPH_ASSERT(joint_count == (int)mParts.size());
+
+	// Create a group filter table that disables collisions between parent and child
+	Ref<GroupFilterTable> group_filter = new GroupFilterTable(joint_count);
+	for (int joint_idx = 0; joint_idx < joint_count; ++joint_idx)
+	{
+		int parent_joint = mSkeleton->GetJoint(joint_idx).mParentJointIndex;
+		if (parent_joint >= 0)
+			group_filter->DisableCollision(joint_idx, parent_joint);
+	}
+
+	// If joint matrices are provided
+	if (inJointMatrices != nullptr)
+	{
+		// Loop over all joints
+		for (int j1 = 0; j1 < joint_count; ++j1)
+		{
+			// Shape and transform for joint 1
+			const Part &part1 = mParts[j1];
+			const Shape *shape1 = part1.GetShape();
+			Vec3 scale1;
+			Mat44 com1 = (inJointMatrices[j1].PreTranslated(shape1->GetCenterOfMass())).Decompose(scale1);
+
+			// Loop over all other joints
+			for (int j2 = j1 + 1; j2 < joint_count; ++j2)
+				if (group_filter->IsCollisionEnabled(j1, j2)) // Only if collision is still enabled we need to test
+				{
+					// Shape and transform for joint 2
+					const Part &part2 = mParts[j2];
+					const Shape *shape2 = part2.GetShape();
+					Vec3 scale2;
+					Mat44 com2 = (inJointMatrices[j2].PreTranslated(shape2->GetCenterOfMass())).Decompose(scale2);
+
+					// Collision settings
+					CollideShapeSettings settings;
+					settings.mActiveEdgeMode = EActiveEdgeMode::CollideWithAll;
+					settings.mBackFaceMode = EBackFaceMode::CollideWithBackFaces;
+					settings.mMaxSeparationDistance = inMinSeparationDistance;
+
+					// Only check if one of the two bodies can become dynamic
+					if (part1.HasMassProperties() || part2.HasMassProperties())
+					{
+						// If there is a collision, disable the collision between the joints
+						AnyHitCollisionCollector<CollideShapeCollector> collector;
+						if (part1.HasMassProperties()) // Ensure that the first shape is always a dynamic one (we can't check mesh vs convex but we can check convex vs mesh)
+							CollisionDispatch::sCollideShapeVsShape(shape1, shape2, scale1, scale2, com1, com2, SubShapeIDCreator(), SubShapeIDCreator(), settings, collector);
+						else
+							CollisionDispatch::sCollideShapeVsShape(shape2, shape1, scale2, scale1, com2, com1, SubShapeIDCreator(), SubShapeIDCreator(), settings, collector);
+						if (collector.HadHit())
+							group_filter->DisableCollision(j1, j2);
+					}
+				}
+		}
+	}
+
+	// Loop over the body parts and assign them a sub group ID and the group filter
+	for (int joint_idx = 0; joint_idx < joint_count; ++joint_idx)
+	{
+		Part &part = mParts[joint_idx];
+		part.mCollisionGroup.SetSubGroupID(joint_idx);
+		part.mCollisionGroup.SetGroupFilter(group_filter);
+	}
+}
+
+void RagdollSettings::SaveBinaryState(StreamOut &inStream, bool inSaveShapes, bool inSaveGroupFilter) const
+{
+	BodyCreationSettings::ShapeToIDMap shape_to_id;
+	BodyCreationSettings::MaterialToIDMap material_to_id;
+	BodyCreationSettings::GroupFilterToIDMap group_filter_to_id;
+
+	// Save skeleton
+	mSkeleton->SaveBinaryState(inStream);
+
+	// Save parts
+	inStream.Write((uint32)mParts.size());
+	for (const Part &p : mParts)
+	{
+		// Write body creation settings
+		p.SaveWithChildren(inStream, inSaveShapes? &shape_to_id : nullptr, inSaveShapes? &material_to_id : nullptr, inSaveGroupFilter? &group_filter_to_id : nullptr);
+
+		// Save constraint
+		inStream.Write(p.mToParent != nullptr);
+		if (p.mToParent != nullptr)
+			p.mToParent->SaveBinaryState(inStream);
+	}
+
+	// Save additional constraints
+	inStream.Write((uint32)mAdditionalConstraints.size());
+	for (const AdditionalConstraint &c : mAdditionalConstraints)
+	{
+		// Save bodies indices
+		inStream.Write(c.mBodyIdx);
+
+		// Save constraint
+		c.mConstraint->SaveBinaryState(inStream);
+	}
+}
+
+RagdollSettings::RagdollResult RagdollSettings::sRestoreFromBinaryState(StreamIn &inStream)
+{
+	RagdollResult result;
+
+	// Restore skeleton
+	Skeleton::SkeletonResult skeleton_result = Skeleton::sRestoreFromBinaryState(inStream);
+	if (skeleton_result.HasError())
+	{
+		result.SetError(skeleton_result.GetError());
+		return result;
+	}
+
+	// Create ragdoll
+	Ref<RagdollSettings> ragdoll = new RagdollSettings();
+	ragdoll->mSkeleton = skeleton_result.Get();
+
+	BodyCreationSettings::IDToShapeMap id_to_shape;
+	BodyCreationSettings::IDToMaterialMap id_to_material;
+	BodyCreationSettings::IDToGroupFilterMap id_to_group_filter;
+
+	// Reserve some memory to avoid frequent reallocations
+	id_to_shape.reserve(1024);
+	id_to_material.reserve(128);
+	id_to_group_filter.reserve(128);
+
+	// Read parts
+	uint32 len = 0;
+	inStream.Read(len);
+	ragdoll->mParts.resize(len);
+	for (Part &p : ragdoll->mParts)
+	{
+		// Read creation settings
+		BodyCreationSettings::BCSResult bcs_result = BodyCreationSettings::sRestoreWithChildren(inStream, id_to_shape, id_to_material, id_to_group_filter);
+		if (bcs_result.HasError())
+		{
+			result.SetError(bcs_result.GetError());
+			return result;
+		}
+		static_cast<BodyCreationSettings &>(p) = bcs_result.Get();
+
+		// Read constraint
+		bool has_constraint = false;
+		inStream.Read(has_constraint);
+		if (has_constraint)
+		{
+			ConstraintSettings::ConstraintResult constraint_result = ConstraintSettings::sRestoreFromBinaryState(inStream);
+			if (constraint_result.HasError())
+			{
+				result.SetError(constraint_result.GetError());
+				return result;
+			}
+			p.mToParent = DynamicCast<TwoBodyConstraintSettings>(constraint_result.Get());
+		}
+	}
+
+	// Read additional constraints
+	len = 0;
+	inStream.Read(len);
+	ragdoll->mAdditionalConstraints.resize(len);
+	for (AdditionalConstraint &c : ragdoll->mAdditionalConstraints)
+	{
+		// Read body indices
+		inStream.Read(c.mBodyIdx);
+
+		// Read constraint
+		ConstraintSettings::ConstraintResult constraint_result = ConstraintSettings::sRestoreFromBinaryState(inStream);
+		if (constraint_result.HasError())
+		{
+			result.SetError(constraint_result.GetError());
+			return result;
+		}
+		c.mConstraint = DynamicCast<TwoBodyConstraintSettings>(constraint_result.Get());
+	}
+
+	// Create mapping tables
+	ragdoll->CalculateBodyIndexToConstraintIndex();
+	ragdoll->CalculateConstraintIndexToBodyIdxPair();
+
+	result.Set(ragdoll);
+	return result;
+}
+
+Ragdoll *RagdollSettings::CreateRagdoll(CollisionGroup::GroupID inCollisionGroup, uint64 inUserData, PhysicsSystem *inSystem) const
+{
+	Ragdoll *r = new Ragdoll(inSystem);
+	r->mRagdollSettings = this;
+	r->mBodyIDs.reserve(mParts.size());
+	r->mConstraints.reserve(mParts.size() + mAdditionalConstraints.size());
+
+	// Create bodies and constraints
+	BodyInterface &bi = inSystem->GetBodyInterface();
+	Body **bodies = (Body **)JPH_STACK_ALLOC(mParts.size() * sizeof(Body *));
+	int joint_idx = 0;
+	for (const Part &p : mParts)
+	{
+		Body *body2 = bi.CreateBody(p);
+		if (body2 == nullptr)
+		{
+			// Out of bodies, failed to create ragdoll
+			delete r;
+			return nullptr;
+		}
+		body2->GetCollisionGroup().SetGroupID(inCollisionGroup);
+		body2->SetUserData(inUserData);
+
+		// Temporarily store body pointer for hooking up constraints
+		bodies[joint_idx] = body2;
+
+		// Create constraint
+		if (p.mToParent != nullptr)
+		{
+			Body *body1 = bodies[mSkeleton->GetJoint(joint_idx).mParentJointIndex];
+			r->mConstraints.push_back(p.mToParent->Create(*body1, *body2));
+		}
+
+		// Store body ID and constraint in parallel arrays
+		r->mBodyIDs.push_back(body2->GetID());
+
+		++joint_idx;
+	}
+
+	// Add additional constraints
+	for (const AdditionalConstraint &c : mAdditionalConstraints)
+	{
+		Body *body1 = bodies[c.mBodyIdx[0]];
+		Body *body2 = bodies[c.mBodyIdx[1]];
+		r->mConstraints.push_back(c.mConstraint->Create(*body1, *body2));
+	}
+
+	return r;
+}
+
+void RagdollSettings::CalculateBodyIndexToConstraintIndex()
+{
+	mBodyIndexToConstraintIndex.clear();
+	mBodyIndexToConstraintIndex.reserve(mParts.size());
+
+	int constraint_index = 0;
+	for (const Part &p : mParts)
+	{
+		if (p.mToParent != nullptr)
+			mBodyIndexToConstraintIndex.push_back(constraint_index++);
+		else
+			mBodyIndexToConstraintIndex.push_back(-1);
+	}
+}
+
+void RagdollSettings::CalculateConstraintIndexToBodyIdxPair()
+{
+	mConstraintIndexToBodyIdxPair.clear();
+	mConstraintIndexToBodyIdxPair.reserve(mParts.size() + mAdditionalConstraints.size());
+
+	// Add constraints between parts
+	int joint_idx = 0;
+	for (const Part &p : mParts)
+	{
+		if (p.mToParent != nullptr)
+		{
+			int parent_joint_idx = mSkeleton->GetJoint(joint_idx).mParentJointIndex;
+			mConstraintIndexToBodyIdxPair.emplace_back(parent_joint_idx, joint_idx);
+		}
+
+		++joint_idx;
+	}
+
+	// Add additional constraints
+	for (const AdditionalConstraint &c : mAdditionalConstraints)
+		mConstraintIndexToBodyIdxPair.emplace_back(c.mBodyIdx[0], c.mBodyIdx[1]);
+}
+
+Ragdoll::~Ragdoll()
+{
+	// Destroy all bodies
+	mSystem->GetBodyInterface().DestroyBodies(mBodyIDs.data(), (int)mBodyIDs.size());
+}
+
+void Ragdoll::AddToPhysicsSystem(EActivation inActivationMode, bool inLockBodies)
+{
+	// Scope for JPH_STACK_ALLOC
+	{
+		// Create copy of body ids since they will be shuffled
+		int num_bodies = (int)mBodyIDs.size();
+		BodyID *bodies = (BodyID *)JPH_STACK_ALLOC(num_bodies * sizeof(BodyID));
+		memcpy(bodies, mBodyIDs.data(), num_bodies * sizeof(BodyID));
+
+		// Insert bodies as a batch
+		BodyInterface &bi = sGetBodyInterface(mSystem, inLockBodies);
+		BodyInterface::AddState add_state = bi.AddBodiesPrepare(bodies, num_bodies);
+		bi.AddBodiesFinalize(bodies, num_bodies, add_state, inActivationMode);
+	}
+
+	// Add all constraints
+	mSystem->AddConstraints((Constraint **)mConstraints.data(), (int)mConstraints.size());
+}
+
+void Ragdoll::RemoveFromPhysicsSystem(bool inLockBodies)
+{
+	// Remove all constraints before removing the bodies
+	mSystem->RemoveConstraints((Constraint **)mConstraints.data(), (int)mConstraints.size());
+
+	// Scope for JPH_STACK_ALLOC
+	{
+		// Create copy of body ids since they will be shuffled
+		int num_bodies = (int)mBodyIDs.size();
+		BodyID *bodies = (BodyID *)JPH_STACK_ALLOC(num_bodies * sizeof(BodyID));
+		memcpy(bodies, mBodyIDs.data(), num_bodies * sizeof(BodyID));
+
+		// Remove all bodies as a batch
+		sGetBodyInterface(mSystem, inLockBodies).RemoveBodies(bodies, num_bodies);
+	}
+}
+
+void Ragdoll::Activate(bool inLockBodies)
+{
+	sGetBodyInterface(mSystem, inLockBodies).ActivateBodies(mBodyIDs.data(), (int)mBodyIDs.size());
+}
+
+bool Ragdoll::IsActive(bool inLockBodies) const
+{
+	// Lock the bodies
+	int body_count = (int)mBodyIDs.size();
+	BodyLockMultiRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyIDs.data(), body_count);
+
+	// Test if any body is active
+	for (int b = 0; b < body_count; ++b)
+	{
+		const Body *body = lock.GetBody(b);
+		if (body->IsActive())
+			return true;
+	}
+
+	return false;
+}
+
+void Ragdoll::SetGroupID(CollisionGroup::GroupID inGroupID, bool inLockBodies)
+{
+	// Lock the bodies
+	int body_count = (int)mBodyIDs.size();
+	BodyLockMultiWrite lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyIDs.data(), body_count);
+
+	// Update group ID
+	for (int b = 0; b < body_count; ++b)
+	{
+		Body *body = lock.GetBody(b);
+		body->GetCollisionGroup().SetGroupID(inGroupID);
+	}
+}
+
+void Ragdoll::SetPose(const SkeletonPose &inPose, bool inLockBodies)
+{
+	JPH_ASSERT(inPose.GetSkeleton() == mRagdollSettings->mSkeleton);
+
+	SetPose(inPose.GetRootOffset(), inPose.GetJointMatrices().data(), inLockBodies);
+}
+
+void Ragdoll::SetPose(RVec3Arg inRootOffset, const Mat44 *inJointMatrices, bool inLockBodies)
+{
+	// Move bodies instantly into the correct position
+	BodyInterface &bi = sGetBodyInterface(mSystem, inLockBodies);
+	for (int i = 0; i < (int)mBodyIDs.size(); ++i)
+	{
+		const Mat44 &joint = inJointMatrices[i];
+		bi.SetPositionAndRotation(mBodyIDs[i], inRootOffset + joint.GetTranslation(), joint.GetQuaternion(), EActivation::DontActivate);
+	}
+}
+
+void Ragdoll::GetPose(SkeletonPose &outPose, bool inLockBodies)
+{
+	JPH_ASSERT(outPose.GetSkeleton() == mRagdollSettings->mSkeleton);
+
+	RVec3 root_offset;
+	GetPose(root_offset, outPose.GetJointMatrices().data(), inLockBodies);
+	outPose.SetRootOffset(root_offset);
+}
+
+void Ragdoll::GetPose(RVec3 &outRootOffset, Mat44 *outJointMatrices, bool inLockBodies)
+{
+	// Lock the bodies
+	int body_count = (int)mBodyIDs.size();
+	if (body_count == 0)
+		return;
+	BodyLockMultiRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyIDs.data(), body_count);
+
+	// Get root matrix
+	const Body *root = lock.GetBody(0);
+	RMat44 root_transform = root->GetWorldTransform();
+	outRootOffset = root_transform.GetTranslation();
+	outJointMatrices[0] = Mat44(root_transform.GetColumn4(0), root_transform.GetColumn4(1), root_transform.GetColumn4(2), Vec4(0, 0, 0, 1));
+
+	// Get other matrices
+	for (int b = 1; b < body_count; ++b)
+	{
+		const Body *body = lock.GetBody(b);
+		RMat44 transform = body->GetWorldTransform();
+		outJointMatrices[b] = Mat44(transform.GetColumn4(0), transform.GetColumn4(1), transform.GetColumn4(2), Vec4(Vec3(transform.GetTranslation() - outRootOffset), 1));
+	}
+}
+
+void Ragdoll::ResetWarmStart()
+{
+	for (TwoBodyConstraint *c : mConstraints)
+		c->ResetWarmStart();
+}
+
+void Ragdoll::DriveToPoseUsingKinematics(const SkeletonPose &inPose, float inDeltaTime, bool inLockBodies)
+{
+	JPH_ASSERT(inPose.GetSkeleton() == mRagdollSettings->mSkeleton);
+
+	DriveToPoseUsingKinematics(inPose.GetRootOffset(), inPose.GetJointMatrices().data(), inDeltaTime, inLockBodies);
+}
+
+void Ragdoll::DriveToPoseUsingKinematics(RVec3Arg inRootOffset, const Mat44 *inJointMatrices, float inDeltaTime, bool inLockBodies)
+{
+	// Move bodies into the correct position using kinematics
+	BodyInterface &bi = sGetBodyInterface(mSystem, inLockBodies);
+	for (int i = 0; i < (int)mBodyIDs.size(); ++i)
+	{
+		const Mat44 &joint = inJointMatrices[i];
+		bi.MoveKinematic(mBodyIDs[i], inRootOffset + joint.GetTranslation(), joint.GetQuaternion(), inDeltaTime);
+	}
+}
+
+void Ragdoll::DriveToPoseUsingMotors(const SkeletonPose &inPose)
+{
+	JPH_ASSERT(inPose.GetSkeleton() == mRagdollSettings->mSkeleton);
+
+	// Move bodies into the correct position using constraints
+	for (int i = 0; i < (int)inPose.GetJointMatrices().size(); ++i)
+	{
+		int constraint_idx = mRagdollSettings->GetConstraintIndexForBodyIndex(i);
+		if (constraint_idx >= 0)
+		{
+			// Get desired rotation of this body relative to its parent
+			const SkeletalAnimation::JointState &joint_state = inPose.GetJoint(i);
+
+			// Drive constraint to target
+			TwoBodyConstraint *constraint = mConstraints[constraint_idx];
+			EConstraintSubType sub_type = constraint->GetSubType();
+			if (sub_type == EConstraintSubType::SwingTwist)
+			{
+				SwingTwistConstraint *st_constraint = static_cast<SwingTwistConstraint *>(constraint);
+				st_constraint->SetSwingMotorState(EMotorState::Position);
+				st_constraint->SetTwistMotorState(EMotorState::Position);
+				st_constraint->SetTargetOrientationBS(joint_state.mRotation);
+			}
+			else
+				JPH_ASSERT(false, "Constraint type not implemented!");
+		}
+	}
+}
+
+void Ragdoll::SetLinearAndAngularVelocity(Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, bool inLockBodies)
+{
+	BodyInterface &bi = sGetBodyInterface(mSystem, inLockBodies);
+	for (BodyID body_id : mBodyIDs)
+		bi.SetLinearAndAngularVelocity(body_id, inLinearVelocity, inAngularVelocity);
+}
+
+void Ragdoll::SetLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies)
+{
+	BodyInterface &bi = sGetBodyInterface(mSystem, inLockBodies);
+	for (BodyID body_id : mBodyIDs)
+		bi.SetLinearVelocity(body_id, inLinearVelocity);
+}
+
+void Ragdoll::AddLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies)
+{
+	BodyInterface &bi = sGetBodyInterface(mSystem, inLockBodies);
+	for (BodyID body_id : mBodyIDs)
+		bi.AddLinearVelocity(body_id, inLinearVelocity);
+}
+
+void Ragdoll::AddImpulse(Vec3Arg inImpulse, bool inLockBodies)
+{
+	BodyInterface &bi = sGetBodyInterface(mSystem, inLockBodies);
+	for (BodyID body_id : mBodyIDs)
+		bi.AddImpulse(body_id, inImpulse);
+}
+
+void Ragdoll::GetRootTransform(RVec3 &outPosition, Quat &outRotation, bool inLockBodies) const
+{
+	BodyLockRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyIDs[0]);
+	if (lock.Succeeded())
+	{
+		const Body &body = lock.GetBody();
+		outPosition = body.GetPosition();
+		outRotation = body.GetRotation();
+	}
+	else
+	{
+		outPosition = RVec3::sZero();
+		outRotation = Quat::sIdentity();
+	}
+}
+
+AABox Ragdoll::GetWorldSpaceBounds(bool inLockBodies) const
+{
+	// Lock the bodies
+	int body_count = (int)mBodyIDs.size();
+	BodyLockMultiRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyIDs.data(), body_count);
+
+	// Encapsulate all bodies
+	AABox bounds;
+	for (int b = 0; b < body_count; ++b)
+	{
+		const Body *body = lock.GetBody(b);
+		if (body != nullptr)
+			bounds.Encapsulate(body->GetWorldSpaceBounds());
+	}
+	return bounds;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.h b/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.h
new file mode 100644
index 0000000000..8fe401080f
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Ragdoll/Ragdoll.h
@@ -0,0 +1,240 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/Reference.h>
+#include <Jolt/Core/Result.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
+#include <Jolt/Skeleton/Skeleton.h>
+#include <Jolt/Skeleton/SkeletonPose.h>
+#include <Jolt/Physics/EActivation.h>
+
+JPH_NAMESPACE_BEGIN
+
+class Ragdoll;
+class PhysicsSystem;
+
+/// Contains the structure of a ragdoll
+class JPH_EXPORT RagdollSettings : public RefTarget<RagdollSettings>
+{
+	JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, RagdollSettings)
+
+public:
+	/// Stabilize the constraints of the ragdoll
+	/// @return True on success, false on failure.
+	bool								Stabilize();
+
+	/// After the ragdoll has been fully configured, call this function to automatically create and add a GroupFilterTable collision filter to all bodies
+	/// and configure them so that parent and children don't collide.
+	///
+	/// This will:
+	/// - Create a GroupFilterTable and assign it to all of the bodies in a ragdoll.
+	/// - Each body in your ragdoll will get a SubGroupID that is equal to the joint index in the Skeleton that it is attached to.
+	/// - Loop over all joints in the Skeleton and call GroupFilterTable::DisableCollision(joint index, parent joint index).
+	/// - When a pose is provided through inJointMatrices the function will detect collisions between joints
+	/// (they must be separated by more than inMinSeparationDistance to be treated as not colliding) and automatically disable collisions.
+	///
+	/// When you create an instance using Ragdoll::CreateRagdoll pass in a unique GroupID for each ragdoll (e.g. a simple counter), note that this number
+	/// should be unique throughout the PhysicsSystem, so if you have different types of ragdolls they should not share the same GroupID.
+	void								DisableParentChildCollisions(const Mat44 *inJointMatrices = nullptr, float inMinSeparationDistance = 0.0f);
+
+	/// Saves the state of this object in binary form to inStream.
+	/// @param inStream The stream to save the state to
+	/// @param inSaveShapes If the shapes should be saved as well (these could be shared between ragdolls, in which case the calling application may want to write custom code to restore them)
+	/// @param inSaveGroupFilter If the group filter should be saved as well (these could be shared)
+	void								SaveBinaryState(StreamOut &inStream, bool inSaveShapes, bool inSaveGroupFilter) const;
+
+	using RagdollResult = Result<Ref<RagdollSettings>>;
+
+	/// Restore a saved ragdoll from inStream
+	static RagdollResult				sRestoreFromBinaryState(StreamIn &inStream);
+
+	/// Create ragdoll instance from these settings
+	/// @return Newly created ragdoll or null when out of bodies
+	Ragdoll *							CreateRagdoll(CollisionGroup::GroupID inCollisionGroup, uint64 inUserData, PhysicsSystem *inSystem) const;
+
+	/// Access to the skeleton of this ragdoll
+	const Skeleton *					GetSkeleton() const												{ return mSkeleton; }
+	Skeleton *							GetSkeleton()													{ return mSkeleton; }
+
+	/// Calculate the map needed for GetBodyIndexToConstraintIndex()
+	void								CalculateBodyIndexToConstraintIndex();
+
+	/// Get table that maps a body index to the constraint index with which it is connected to its parent. -1 if there is no constraint associated with the body.
+	/// Note that this will only tell you which constraint connects the body to its parent, it will not look in the additional constraint list.
+	const Array<int> &					GetBodyIndexToConstraintIndex() const							{ return mBodyIndexToConstraintIndex; }
+
+	/// Map a single body index to a constraint index
+	int									GetConstraintIndexForBodyIndex(int inBodyIndex) const			{ return mBodyIndexToConstraintIndex[inBodyIndex]; }
+
+	/// Calculate the map needed for GetConstraintIndexToBodyIdxPair()
+	void								CalculateConstraintIndexToBodyIdxPair();
+
+	using BodyIdxPair = std::pair<int, int>;
+
+	/// Table that maps a constraint index (index in mConstraints) to the indices of the bodies that the constraint is connected to (index in mBodyIDs)
+	const Array<BodyIdxPair> &			GetConstraintIndexToBodyIdxPair() const							{ return mConstraintIndexToBodyIdxPair; }
+
+	/// Map a single constraint index (index in mConstraints) to the indices of the bodies that the constraint is connected to (index in mBodyIDs)
+	BodyIdxPair							GetBodyIndicesForConstraintIndex(int inConstraintIndex) const	{ return mConstraintIndexToBodyIdxPair[inConstraintIndex]; }
+
+	/// A single rigid body sub part of the ragdoll
+	class Part : public BodyCreationSettings
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Part)
+
+	public:
+		Ref<TwoBodyConstraintSettings>	mToParent;
+	};
+
+	/// List of ragdoll parts
+	using PartVector = Array<Part>;																	///< The constraint that connects this part to its parent part (should be null for the root)
+
+	/// A constraint that connects two bodies in a ragdoll (for non parent child related constraints)
+	class AdditionalConstraint
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, AdditionalConstraint)
+
+	public:
+		/// Constructors
+										AdditionalConstraint() = default;
+										AdditionalConstraint(int inBodyIdx1, int inBodyIdx2, TwoBodyConstraintSettings *inConstraint) : mBodyIdx { inBodyIdx1, inBodyIdx2 }, mConstraint(inConstraint) { }
+
+		int								mBodyIdx[2];												///< Indices of the bodies that this constraint connects
+		Ref<TwoBodyConstraintSettings>	mConstraint;												///< The constraint that connects these bodies
+	};
+
+	/// List of additional constraints
+	using AdditionalConstraintVector = Array<AdditionalConstraint>;
+
+	/// The skeleton for this ragdoll
+	Ref<Skeleton>						mSkeleton;
+
+	/// For each of the joints, the body and constraint attaching it to its parent body (1-on-1 with mSkeleton.GetJoints())
+	PartVector							mParts;
+
+	/// A list of constraints that connects two bodies in a ragdoll (for non parent child related constraints)
+	AdditionalConstraintVector			mAdditionalConstraints;
+
+private:
+	/// Table that maps a body index (index in mBodyIDs) to the constraint index with which it is connected to its parent. -1 if there is no constraint associated with the body.
+	Array<int>							mBodyIndexToConstraintIndex;
+
+	/// Table that maps a constraint index (index in mConstraints) to the indices of the bodies that the constraint is connected to (index in mBodyIDs)
+	Array<BodyIdxPair>					mConstraintIndexToBodyIdxPair;
+};
+
+/// Runtime ragdoll information
+class JPH_EXPORT Ragdoll : public RefTarget<Ragdoll>, public NonCopyable
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+	explicit							Ragdoll(PhysicsSystem *inSystem) : mSystem(inSystem) { }
+
+	/// Destructor
+										~Ragdoll();
+
+	/// Add bodies and constraints to the system and optionally activate the bodies
+	void								AddToPhysicsSystem(EActivation inActivationMode, bool inLockBodies = true);
+
+	/// Remove bodies and constraints from the system
+	void								RemoveFromPhysicsSystem(bool inLockBodies = true);
+
+	/// Wake up all bodies in the ragdoll
+	void								Activate(bool inLockBodies = true);
+
+	/// Check if one or more of the bodies in the ragdoll are active.
+	/// Note that this involves locking the bodies (if inLockBodies is true) and looping over them. An alternative and possibly faster
+	/// way could be to install a BodyActivationListener and count the number of active bodies of a ragdoll as they're activated / deactivated
+	/// (basically check if the body that activates / deactivates is in GetBodyIDs() and increment / decrement a counter).
+	bool								IsActive(bool inLockBodies = true) const;
+
+	/// Set the group ID on all bodies in the ragdoll
+	void								SetGroupID(CollisionGroup::GroupID inGroupID, bool inLockBodies = true);
+
+	/// Set the ragdoll to a pose (calls BodyInterface::SetPositionAndRotation to instantly move the ragdoll)
+	void								SetPose(const SkeletonPose &inPose, bool inLockBodies = true);
+
+	/// Lower level version of SetPose that directly takes the world space joint matrices
+	void								SetPose(RVec3Arg inRootOffset, const Mat44 *inJointMatrices, bool inLockBodies = true);
+
+	/// Get the ragdoll pose (uses the world transform of the bodies to calculate the pose)
+	void								GetPose(SkeletonPose &outPose, bool inLockBodies = true);
+
+	/// Lower level version of GetPose that directly returns the world space joint matrices
+	void								GetPose(RVec3 &outRootOffset, Mat44 *outJointMatrices, bool inLockBodies = true);
+
+	/// This function calls ResetWarmStart on all constraints. It can be used after calling SetPose to reset previous frames impulses. See: Constraint::ResetWarmStart.
+	void								ResetWarmStart();
+
+	/// Drive the ragdoll to a specific pose by setting velocities on each of the bodies so that it will reach inPose in inDeltaTime
+	void								DriveToPoseUsingKinematics(const SkeletonPose &inPose, float inDeltaTime, bool inLockBodies = true);
+
+	/// Lower level version of DriveToPoseUsingKinematics that directly takes the world space joint matrices
+	void								DriveToPoseUsingKinematics(RVec3Arg inRootOffset, const Mat44 *inJointMatrices, float inDeltaTime, bool inLockBodies = true);
+
+	/// Drive the ragdoll to a specific pose by activating the motors on each constraint
+	void								DriveToPoseUsingMotors(const SkeletonPose &inPose);
+
+	/// Control the linear and velocity of all bodies in the ragdoll
+	void								SetLinearAndAngularVelocity(Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, bool inLockBodies = true);
+
+	/// Set the world space linear velocity of all bodies in the ragdoll.
+	void								SetLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies = true);
+
+	/// Add a world space velocity (in m/s) to all bodies in the ragdoll.
+	void								AddLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies = true);
+
+	/// Add impulse to all bodies of the ragdoll (center of mass of each of them)
+	void								AddImpulse(Vec3Arg inImpulse, bool inLockBodies = true);
+
+	/// Get the position and orientation of the root of the ragdoll
+	void								GetRootTransform(RVec3 &outPosition, Quat &outRotation, bool inLockBodies = true) const;
+
+	/// Get number of bodies in the ragdoll
+	size_t								GetBodyCount() const									{ return mBodyIDs.size(); }
+
+	/// Access a body ID
+	BodyID								GetBodyID(int inBodyIndex) const						{ return mBodyIDs[inBodyIndex]; }
+
+	/// Access to the array of body IDs
+	const Array<BodyID> &				GetBodyIDs() const										{ return mBodyIDs; }
+
+	/// Get number of constraints in the ragdoll
+	size_t								GetConstraintCount() const								{ return mConstraints.size(); }
+
+	/// Access a constraint by index
+	TwoBodyConstraint *					GetConstraint(int inConstraintIndex)					{ return mConstraints[inConstraintIndex]; }
+
+	/// Access a constraint by index
+	const TwoBodyConstraint *			GetConstraint(int inConstraintIndex) const				{ return mConstraints[inConstraintIndex]; }
+
+	/// Get world space bounding box for all bodies of the ragdoll
+	AABox								GetWorldSpaceBounds(bool inLockBodies = true) const;
+
+	/// Get the settings object that created this ragdoll
+	const RagdollSettings *				GetRagdollSettings() const								{ return mRagdollSettings; }
+
+private:
+	/// For RagdollSettings::CreateRagdoll function
+	friend class RagdollSettings;
+
+	/// The settings that created this ragdoll
+	RefConst<RagdollSettings>			mRagdollSettings;
+
+	/// The bodies and constraints that this ragdoll consists of (1-on-1 with mRagdollSettings->mParts)
+	Array<BodyID>						mBodyIDs;
+
+	/// Array of constraints that connect the bodies together
+	Array<Ref<TwoBodyConstraint>>		mConstraints;
+
+	/// Cached physics system
+	PhysicsSystem *						mSystem;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyContactListener.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyContactListener.h
new file mode 100644
index 0000000000..27e185375f
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyContactListener.h
@@ -0,0 +1,55 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+class Body;
+class SoftBodyManifold;
+
+/// Return value for the OnSoftBodyContactValidate callback. Determines if the contact will be processed or not.
+enum class SoftBodyValidateResult
+{
+	AcceptContact,														///< Accept this contact
+	RejectContact,														///< Reject this contact
+};
+
+/// Contact settings for a soft body contact.
+/// The values are filled in with their defaults by the system so the callback doesn't need to modify anything, but it can if it wants to.
+class SoftBodyContactSettings
+{
+public:
+	float							mInvMassScale1 = 1.0f;				///< Scale factor for the inverse mass of the soft body (0 = infinite mass, 1 = use original mass, 2 = body has half the mass). For the same contact pair, you should strive to keep the value the same over time.
+	float							mInvMassScale2 = 1.0f;				///< Scale factor for the inverse mass of the other body (0 = infinite mass, 1 = use original mass, 2 = body has half the mass). For the same contact pair, you should strive to keep the value the same over time.
+	float							mInvInertiaScale2 = 1.0f;			///< Scale factor for the inverse inertia of the other body (usually same as mInvMassScale2)
+	bool							mIsSensor;							///< If the contact should be treated as a sensor vs body contact (no collision response)
+};
+
+/// A listener class that receives collision contact events for soft bodies against rigid bodies.
+/// It can be registered with the PhysicsSystem.
+class SoftBodyContactListener
+{
+public:
+	/// Ensure virtual destructor
+	virtual							~SoftBodyContactListener() = default;
+
+	/// Called whenever the soft body's aabox overlaps with another body's aabox (so receiving this callback doesn't tell if any of the vertices will collide).
+	/// This callback can be used to change the behavior of the collision response for all vertices in the soft body or to completely reject the contact.
+	/// Note that this callback is called when all bodies are locked, so don't use any locking functions!
+	/// @param inSoftBody The soft body that collided. It is safe to access this as the soft body is only updated on the current thread.
+	/// @param inOtherBody The other body that collided. Note that accessing the position/orientation/velocity of inOtherBody may result in a race condition as other threads may be modifying the body at the same time.
+	/// @param ioSettings The settings for all contact points that are generated by this collision.
+	/// @return Whether the contact should be processed or not.
+	virtual SoftBodyValidateResult	OnSoftBodyContactValidate([[maybe_unused]] const Body &inSoftBody, [[maybe_unused]] const Body &inOtherBody, [[maybe_unused]] SoftBodyContactSettings &ioSettings) { return SoftBodyValidateResult::AcceptContact; }
+
+	/// Called after all contact points for a soft body have been handled. You only receive one callback per body pair per simulation step and can use inManifold to iterate through all contacts.
+	/// Note that this callback is called when all bodies are locked, so don't use any locking functions!
+	/// You will receive a single callback for a soft body per simulation step for performance reasons, this callback will apply to all vertices in the soft body.
+	/// @param inSoftBody The soft body that collided. It is safe to access this as the soft body is only updated on the current thread.
+	/// @param inManifold The manifold that describes the contact surface between the two bodies. Other bodies may be modified by other threads during this callback.
+	virtual void					OnSoftBodyContactAdded([[maybe_unused]] const Body &inSoftBody, const SoftBodyManifold &inManifold) { /* Do nothing */ }
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.cpp b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.cpp
new file mode 100644
index 0000000000..3ae026df64
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.cpp
@@ -0,0 +1,122 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/SoftBody/SoftBodyCreationSettings.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodyCreationSettings)
+{
+	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mSettings)
+	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mPosition)
+	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mRotation)
+	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mUserData)
+	JPH_ADD_ENUM_ATTRIBUTE(SoftBodyCreationSettings, mObjectLayer)
+	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mCollisionGroup)
+	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mNumIterations)
+	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mLinearDamping)
+	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mMaxLinearVelocity)
+	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mRestitution)
+	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mFriction)
+	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mPressure)
+	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mGravityFactor)
+	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mUpdatePosition)
+	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mMakeRotationIdentity)
+	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mAllowSleeping)
+}
+
+void SoftBodyCreationSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(mPosition);
+	inStream.Write(mRotation);
+	inStream.Write(mUserData);
+	inStream.Write(mObjectLayer);
+	mCollisionGroup.SaveBinaryState(inStream);
+	inStream.Write(mNumIterations);
+	inStream.Write(mLinearDamping);
+	inStream.Write(mMaxLinearVelocity);
+	inStream.Write(mRestitution);
+	inStream.Write(mFriction);
+	inStream.Write(mPressure);
+	inStream.Write(mGravityFactor);
+	inStream.Write(mUpdatePosition);
+	inStream.Write(mMakeRotationIdentity);
+	inStream.Write(mAllowSleeping);
+}
+
+void SoftBodyCreationSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	inStream.Read(mPosition);
+	inStream.Read(mRotation);
+	inStream.Read(mUserData);
+	inStream.Read(mObjectLayer);
+	mCollisionGroup.RestoreBinaryState(inStream);
+	inStream.Read(mNumIterations);
+	inStream.Read(mLinearDamping);
+	inStream.Read(mMaxLinearVelocity);
+	inStream.Read(mRestitution);
+	inStream.Read(mFriction);
+	inStream.Read(mPressure);
+	inStream.Read(mGravityFactor);
+	inStream.Read(mUpdatePosition);
+	inStream.Read(mMakeRotationIdentity);
+	inStream.Read(mAllowSleeping);
+}
+
+void SoftBodyCreationSettings::SaveWithChildren(StreamOut &inStream, SharedSettingsToIDMap *ioSharedSettingsMap, MaterialToIDMap *ioMaterialMap, GroupFilterToIDMap *ioGroupFilterMap) const
+{
+	// Save creation settings
+	SaveBinaryState(inStream);
+
+	// Save shared settings
+	if (ioSharedSettingsMap != nullptr && ioMaterialMap != nullptr)
+		mSettings->SaveWithMaterials(inStream, *ioSharedSettingsMap, *ioMaterialMap);
+	else
+		inStream.Write(~uint32(0));
+
+	// Save group filter
+	StreamUtils::SaveObjectReference(inStream, mCollisionGroup.GetGroupFilter(), ioGroupFilterMap);
+}
+
+SoftBodyCreationSettings::SBCSResult SoftBodyCreationSettings::sRestoreWithChildren(StreamIn &inStream, IDToSharedSettingsMap &ioSharedSettingsMap, IDToMaterialMap &ioMaterialMap, IDToGroupFilterMap &ioGroupFilterMap)
+{
+	SBCSResult result;
+
+	// Read creation settings
+	SoftBodyCreationSettings settings;
+	settings.RestoreBinaryState(inStream);
+	if (inStream.IsEOF() || inStream.IsFailed())
+	{
+		result.SetError("Error reading body creation settings");
+		return result;
+	}
+
+	// Read shared settings
+	SoftBodySharedSettings::SettingsResult settings_result = SoftBodySharedSettings::sRestoreWithMaterials(inStream, ioSharedSettingsMap, ioMaterialMap);
+	if (settings_result.HasError())
+	{
+		result.SetError(settings_result.GetError());
+		return result;
+	}
+	settings.mSettings = settings_result.Get();
+
+	// Read group filter
+	Result gfresult = StreamUtils::RestoreObjectReference(inStream, ioGroupFilterMap);
+	if (gfresult.HasError())
+	{
+		result.SetError(gfresult.GetError());
+		return result;
+	}
+	settings.mCollisionGroup.SetGroupFilter(gfresult.Get());
+
+	result.Set(settings);
+	return result;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.h
new file mode 100644
index 0000000000..b5dc163767
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyCreationSettings.h
@@ -0,0 +1,73 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/SoftBody/SoftBodySharedSettings.h>
+#include <Jolt/Physics/Collision/ObjectLayer.h>
+#include <Jolt/Physics/Collision/CollisionGroup.h>
+#include <Jolt/ObjectStream/SerializableObject.h>
+#include <Jolt/Core/StreamUtils.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// This class contains the information needed to create a soft body object
+/// Note: Soft bodies are still in development and come with several caveats. Read the Architecture and API documentation for more information!
+class JPH_EXPORT SoftBodyCreationSettings
+{
+	JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SoftBodyCreationSettings)
+
+public:
+	/// Constructor
+						SoftBodyCreationSettings() = default;
+						SoftBodyCreationSettings(const SoftBodySharedSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, ObjectLayer inObjectLayer) : mSettings(inSettings), mPosition(inPosition), mRotation(inRotation), mObjectLayer(inObjectLayer) { }
+
+	/// Saves the state of this object in binary form to inStream. Doesn't store the shared settings nor the group filter.
+	void				SaveBinaryState(StreamOut &inStream) const;
+
+	/// Restore the state of this object from inStream. Doesn't restore the shared settings nor the group filter.
+	void				RestoreBinaryState(StreamIn &inStream);
+
+	using GroupFilterToIDMap = StreamUtils::ObjectToIDMap<GroupFilter>;
+	using IDToGroupFilterMap = StreamUtils::IDToObjectMap<GroupFilter>;
+	using SharedSettingsToIDMap = SoftBodySharedSettings::SharedSettingsToIDMap;
+	using IDToSharedSettingsMap = SoftBodySharedSettings::IDToSharedSettingsMap;
+	using MaterialToIDMap = StreamUtils::ObjectToIDMap<PhysicsMaterial>;
+	using IDToMaterialMap = StreamUtils::IDToObjectMap<PhysicsMaterial>;
+
+	/// Save this body creation settings, its shared settings and group filter. Pass in an empty map in ioSharedSettingsMap / ioMaterialMap / ioGroupFilterMap or reuse the same map while saving multiple shapes to the same stream in order to avoid writing duplicates.
+	/// Pass nullptr to ioSharedSettingsMap and ioMaterial map to skip saving shared settings and materials
+	/// Pass nullptr to ioGroupFilterMap to skip saving group filters
+	void				SaveWithChildren(StreamOut &inStream, SharedSettingsToIDMap *ioSharedSettingsMap, MaterialToIDMap *ioMaterialMap, GroupFilterToIDMap *ioGroupFilterMap) const;
+
+	using SBCSResult = Result<SoftBodyCreationSettings>;
+
+	/// Restore a shape, all its children and materials. Pass in an empty map in ioSharedSettingsMap / ioMaterialMap / ioGroupFilterMap or reuse the same map while reading multiple shapes from the same stream in order to restore duplicates.
+	static SBCSResult	sRestoreWithChildren(StreamIn &inStream, IDToSharedSettingsMap &ioSharedSettingsMap, IDToMaterialMap &ioMaterialMap, IDToGroupFilterMap &ioGroupFilterMap);
+
+	RefConst<SoftBodySharedSettings> mSettings;				///< Defines the configuration of this soft body
+
+	RVec3				mPosition { RVec3::sZero() };		///< Initial position of the soft body
+	Quat				mRotation { Quat::sIdentity() };	///< Initial rotation of the soft body
+
+	/// User data value (can be used by application)
+	uint64				mUserData = 0;
+
+	///@name Collision settings
+	ObjectLayer			mObjectLayer = 0;					///< The collision layer this body belongs to (determines if two objects can collide)
+	CollisionGroup		mCollisionGroup;					///< The collision group this body belongs to (determines if two objects can collide)
+
+	uint32				mNumIterations = 5;					///< Number of solver iterations
+	float				mLinearDamping = 0.1f;				///< Linear damping: dv/dt = -mLinearDamping * v
+	float				mMaxLinearVelocity = 500.0f;		///< Maximum linear velocity that a vertex can reach (m/s)
+	float				mRestitution = 0.0f;				///< Restitution when colliding
+	float				mFriction = 0.2f;					///< Friction coefficient when colliding
+	float				mPressure = 0.0f;					///< n * R * T, amount of substance * ideal gas constant * absolute temperature, see https://en.wikipedia.org/wiki/Pressure
+	float				mGravityFactor = 1.0f;				///< Value to multiply gravity with for this body
+	bool				mUpdatePosition = true;				///< Update the position of the body while simulating (set to false for something that is attached to the static world)
+	bool				mMakeRotationIdentity = true;		///< Bake specified mRotation in the vertices and set the body rotation to identity (simulation is slightly more accurate if the rotation of a soft body is kept to identity)
+	bool				mAllowSleeping = true;				///< If this body can go to sleep or not
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyManifold.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyManifold.h
new file mode 100644
index 0000000000..de21ec50de
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyManifold.h
@@ -0,0 +1,74 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/SoftBody/SoftBodyMotionProperties.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// An interface to query which vertices of a soft body are colliding with other bodies
+class SoftBodyManifold
+{
+public:
+	/// Get the vertices of the soft body for iterating
+	const Array<SoftBodyVertex> &	GetVertices() const							{ return mVertices; }
+
+	/// Check if a vertex has collided with something in this update
+	JPH_INLINE bool					HasContact(const SoftBodyVertex &inVertex) const
+	{
+		return inVertex.mHasContact;
+	}
+
+	/// Get the local space contact point (multiply by GetCenterOfMassTransform() of the soft body to get world space)
+	JPH_INLINE Vec3					GetLocalContactPoint(const SoftBodyVertex &inVertex) const
+	{
+		return inVertex.mPosition - inVertex.mCollisionPlane.SignedDistance(inVertex.mPosition) * inVertex.mCollisionPlane.GetNormal();
+	}
+
+	/// Get the contact normal for the vertex (assumes there is a contact).
+	JPH_INLINE Vec3					GetContactNormal(const SoftBodyVertex &inVertex) const
+	{
+		return -inVertex.mCollisionPlane.GetNormal();
+	}
+
+	/// Get the body with which the vertex has collided in this update
+	JPH_INLINE BodyID				GetContactBodyID(const SoftBodyVertex &inVertex) const
+	{
+		return inVertex.mHasContact? mCollidingShapes[inVertex.mCollidingShapeIndex].mBodyID : BodyID();
+	}
+
+	/// Get the number of sensors that are in contact with the soft body
+	JPH_INLINE uint					GetNumSensorContacts() const
+	{
+		return (uint)mCollidingSensors.size();
+	}
+
+	/// Get the i-th sensor that is in contact with the soft body
+	JPH_INLINE BodyID				GetSensorContactBodyID(uint inIndex) const
+	{
+		return mCollidingSensors[inIndex].mBodyID;
+	}
+
+private:
+	/// Allow SoftBodyMotionProperties to construct us
+	friend class SoftBodyMotionProperties;
+
+	/// Constructor
+	explicit						SoftBodyManifold(const SoftBodyMotionProperties *inMotionProperties) :
+										mVertices(inMotionProperties->mVertices),
+										mCollidingShapes(inMotionProperties->mCollidingShapes),
+										mCollidingSensors(inMotionProperties->mCollidingSensors)
+	{
+	}
+
+	using CollidingShape = SoftBodyMotionProperties::CollidingShape;
+	using CollidingSensor = SoftBodyMotionProperties::CollidingSensor;
+
+	const Array<SoftBodyVertex> &	mVertices;
+	const Array<CollidingShape>	&	mCollidingShapes;
+	const Array<CollidingSensor> &	mCollidingSensors;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp
new file mode 100644
index 0000000000..0aad94de88
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp
@@ -0,0 +1,1321 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/SoftBody/SoftBodyMotionProperties.h>
+#include <Jolt/Physics/SoftBody/SoftBodyCreationSettings.h>
+#include <Jolt/Physics/SoftBody/SoftBodyContactListener.h>
+#include <Jolt/Physics/SoftBody/SoftBodyManifold.h>
+#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
+#include <Jolt/Physics/Collision/SimShapeFilterWrapper.h>
+#include <Jolt/Physics/PhysicsSystem.h>
+#include <Jolt/Physics/Body/BodyManager.h>
+#include <Jolt/Core/ScopeExit.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+using namespace JPH::literals;
+
+void SoftBodyMotionProperties::CalculateMassAndInertia()
+{
+	MassProperties mp;
+
+	for (const Vertex &v : mVertices)
+		if (v.mInvMass > 0.0f)
+		{
+			Vec3 pos = v.mPosition;
+
+			// Accumulate mass
+			float mass = 1.0f / v.mInvMass;
+			mp.mMass += mass;
+
+			// Inertia tensor, diagonal
+			// See equations https://en.wikipedia.org/wiki/Moment_of_inertia section 'Inertia Tensor'
+			for (int i = 0; i < 3; ++i)
+				mp.mInertia(i, i) += mass * (Square(pos[(i + 1) % 3]) + Square(pos[(i + 2) % 3]));
+
+			// Inertia tensor off diagonal
+			for (int i = 0; i < 3; ++i)
+				for (int j = 0; j < 3; ++j)
+					if (i != j)
+						mp.mInertia(i, j) -= mass * pos[i] * pos[j];
+		}
+		else
+		{
+			// If one vertex is kinematic, the entire body will have infinite mass and inertia
+			SetInverseMass(0.0f);
+			SetInverseInertia(Vec3::sZero(), Quat::sIdentity());
+			return;
+		}
+
+	SetMassProperties(EAllowedDOFs::All, mp);
+}
+
+void SoftBodyMotionProperties::Initialize(const SoftBodyCreationSettings &inSettings)
+{
+	// Store settings
+	mSettings = inSettings.mSettings;
+	mNumIterations = inSettings.mNumIterations;
+	mPressure = inSettings.mPressure;
+	mUpdatePosition = inSettings.mUpdatePosition;
+
+	// Initialize vertices
+	mVertices.resize(inSettings.mSettings->mVertices.size());
+	Mat44 rotation = inSettings.mMakeRotationIdentity? Mat44::sRotation(inSettings.mRotation) : Mat44::sIdentity();
+	for (Array<Vertex>::size_type v = 0, s = mVertices.size(); v < s; ++v)
+	{
+		const SoftBodySharedSettings::Vertex &in_vertex = inSettings.mSettings->mVertices[v];
+		Vertex &out_vertex = mVertices[v];
+		out_vertex.mPreviousPosition = out_vertex.mPosition = rotation * Vec3(in_vertex.mPosition);
+		out_vertex.mVelocity = rotation.Multiply3x3(Vec3(in_vertex.mVelocity));
+		out_vertex.ResetCollision();
+		out_vertex.mInvMass = in_vertex.mInvMass;
+		mLocalBounds.Encapsulate(out_vertex.mPosition);
+	}
+
+	// Allocate space for skinned vertices
+	if (!inSettings.mSettings->mSkinnedConstraints.empty())
+		mSkinState.resize(mVertices.size());
+
+	// We don't know delta time yet, so we can't predict the bounds and use the local bounds as the predicted bounds
+	mLocalPredictedBounds = mLocalBounds;
+
+	CalculateMassAndInertia();
+}
+
+float SoftBodyMotionProperties::GetVolumeTimesSix() const
+{
+	float six_volume = 0.0f;
+	for (const Face &f : mSettings->mFaces)
+	{
+		Vec3 x1 = mVertices[f.mVertex[0]].mPosition;
+		Vec3 x2 = mVertices[f.mVertex[1]].mPosition;
+		Vec3 x3 = mVertices[f.mVertex[2]].mPosition;
+		six_volume += x1.Cross(x2).Dot(x3); // We pick zero as the origin as this is the center of the bounding box so should give good accuracy
+	}
+	return six_volume;
+}
+
+void SoftBodyMotionProperties::DetermineCollidingShapes(const SoftBodyUpdateContext &inContext, const PhysicsSystem &inSystem, const BodyLockInterface &inBodyLockInterface)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Reset flag prior to collision detection
+	mNeedContactCallback = false;
+
+	struct Collector : public CollideShapeBodyCollector
+	{
+									Collector(const SoftBodyUpdateContext &inContext, const PhysicsSystem &inSystem, const BodyLockInterface &inBodyLockInterface, const AABox &inLocalBounds, SimShapeFilterWrapper &inShapeFilter, Array<CollidingShape> &ioHits, Array<CollidingSensor> &ioSensors) :
+										mContext(inContext),
+										mInverseTransform(inContext.mCenterOfMassTransform.InversedRotationTranslation()),
+										mLocalBounds(inLocalBounds),
+										mBodyLockInterface(inBodyLockInterface),
+										mCombineFriction(inSystem.GetCombineFriction()),
+										mCombineRestitution(inSystem.GetCombineRestitution()),
+										mShapeFilter(inShapeFilter),
+										mHits(ioHits),
+										mSensors(ioSensors)
+		{
+		}
+
+		virtual void				AddHit(const BodyID &inResult) override
+		{
+			BodyLockRead lock(mBodyLockInterface, inResult);
+			if (lock.Succeeded())
+			{
+				const Body &soft_body = *mContext.mBody;
+				const Body &body = lock.GetBody();
+				if (body.IsRigidBody() // TODO: We should support soft body vs soft body
+					&& soft_body.GetCollisionGroup().CanCollide(body.GetCollisionGroup()))
+				{
+					SoftBodyContactSettings settings;
+					settings.mIsSensor = body.IsSensor();
+
+					if (mContext.mContactListener == nullptr)
+					{
+						// If we have no contact listener, we can ignore sensors
+						if (settings.mIsSensor)
+							return;
+					}
+					else
+					{
+						// Call the contact listener to see if we should accept this contact
+						if (mContext.mContactListener->OnSoftBodyContactValidate(soft_body, body, settings) != SoftBodyValidateResult::AcceptContact)
+							return;
+
+						// Check if there will be any interaction
+						if (!settings.mIsSensor
+							&& settings.mInvMassScale1 == 0.0f
+							&& (body.GetMotionType() != EMotionType::Dynamic || settings.mInvMassScale2 == 0.0f))
+							return;
+					}
+
+					// Calculate transform of this body relative to the soft body
+					Mat44 com = (mInverseTransform * body.GetCenterOfMassTransform()).ToMat44();
+
+					// Collect leaf shapes
+					mShapeFilter.SetBody2(&body);
+					struct LeafShapeCollector : public TransformedShapeCollector
+					{
+						virtual void		AddHit(const TransformedShape &inResult) override
+						{
+							mHits.emplace_back(Mat44::sRotationTranslation(inResult.mShapeRotation, Vec3(inResult.mShapePositionCOM)), inResult.GetShapeScale(), inResult.mShape);
+						}
+
+						Array<LeafShape>	mHits;
+					};
+					LeafShapeCollector collector;
+					body.GetShape()->CollectTransformedShapes(mLocalBounds, com.GetTranslation(), com.GetQuaternion(), Vec3::sReplicate(1.0f), SubShapeIDCreator(), collector, mShapeFilter);
+					if (collector.mHits.empty())
+						return;
+
+					if (settings.mIsSensor)
+					{
+						CollidingSensor cs;
+						cs.mCenterOfMassTransform = com;
+						cs.mShapes = std::move(collector.mHits);
+						cs.mBodyID = inResult;
+						mSensors.push_back(cs);
+					}
+					else
+					{
+						CollidingShape cs;
+						cs.mCenterOfMassTransform = com;
+						cs.mShapes = std::move(collector.mHits);
+						cs.mBodyID = inResult;
+						cs.mMotionType = body.GetMotionType();
+						cs.mUpdateVelocities = false;
+						cs.mFriction = mCombineFriction(soft_body, SubShapeID(), body, SubShapeID());
+						cs.mRestitution = mCombineRestitution(soft_body, SubShapeID(), body, SubShapeID());
+						cs.mSoftBodyInvMassScale = settings.mInvMassScale1;
+						if (cs.mMotionType == EMotionType::Dynamic)
+						{
+							const MotionProperties *mp = body.GetMotionProperties();
+							cs.mInvMass = settings.mInvMassScale2 * mp->GetInverseMass();
+							cs.mInvInertia = settings.mInvInertiaScale2 * mp->GetInverseInertiaForRotation(cs.mCenterOfMassTransform.GetRotation());
+							cs.mOriginalLinearVelocity = cs.mLinearVelocity = mInverseTransform.Multiply3x3(mp->GetLinearVelocity());
+							cs.mOriginalAngularVelocity = cs.mAngularVelocity = mInverseTransform.Multiply3x3(mp->GetAngularVelocity());
+						}
+						mHits.push_back(cs);
+					}
+				}
+			}
+		}
+
+	private:
+		const SoftBodyUpdateContext &mContext;
+		RMat44						mInverseTransform;
+		AABox						mLocalBounds;
+		const BodyLockInterface &	mBodyLockInterface;
+		ContactConstraintManager::CombineFunction mCombineFriction;
+		ContactConstraintManager::CombineFunction mCombineRestitution;
+		SimShapeFilterWrapper &		mShapeFilter;
+		Array<CollidingShape> &		mHits;
+		Array<CollidingSensor> &	mSensors;
+	};
+
+	// Calculate local bounding box
+	AABox local_bounds = mLocalBounds;
+	local_bounds.Encapsulate(mLocalPredictedBounds);
+	local_bounds.ExpandBy(Vec3::sReplicate(mSettings->mVertexRadius));
+
+	// Calculate world space bounding box
+	AABox world_bounds = local_bounds.Transformed(inContext.mCenterOfMassTransform);
+
+	// Create shape filter
+	SimShapeFilterWrapperUnion shape_filter_union(inContext.mSimShapeFilter, inContext.mBody);
+	SimShapeFilterWrapper &shape_filter = shape_filter_union.GetSimShapeFilterWrapper();
+
+	Collector collector(inContext, inSystem, inBodyLockInterface, local_bounds, shape_filter, mCollidingShapes, mCollidingSensors);
+	ObjectLayer layer = inContext.mBody->GetObjectLayer();
+	DefaultBroadPhaseLayerFilter broadphase_layer_filter = inSystem.GetDefaultBroadPhaseLayerFilter(layer);
+	DefaultObjectLayerFilter object_layer_filter = inSystem.GetDefaultLayerFilter(layer);
+	inSystem.GetBroadPhaseQuery().CollideAABox(world_bounds, collector, broadphase_layer_filter, object_layer_filter);
+}
+
+void SoftBodyMotionProperties::DetermineCollisionPlanes(uint inVertexStart, uint inNumVertices)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Generate collision planes
+	for (const CollidingShape &cs : mCollidingShapes)
+		for (const LeafShape &shape : cs.mShapes)
+			shape.mShape->CollideSoftBodyVertices(shape.mTransform, shape.mScale, CollideSoftBodyVertexIterator(mVertices.data() + inVertexStart), inNumVertices, int(&cs - mCollidingShapes.data()));
+}
+
+void SoftBodyMotionProperties::DetermineSensorCollisions(CollidingSensor &ioSensor)
+{
+	JPH_PROFILE_FUNCTION();
+
+	Plane collision_plane;
+	float largest_penetration = -FLT_MAX;
+	int colliding_shape_idx = -1;
+
+	// Collide sensor against all vertices
+	CollideSoftBodyVertexIterator vertex_iterator(
+		StridedPtr<const Vec3>(&mVertices[0].mPosition, sizeof(SoftBodyVertex)), // The position and mass come from the soft body vertex
+		StridedPtr<const float>(&mVertices[0].mInvMass, sizeof(SoftBodyVertex)),
+		StridedPtr<Plane>(&collision_plane, 0), // We want all vertices to result in a single collision so we pass stride 0
+		StridedPtr<float>(&largest_penetration, 0),
+		StridedPtr<int>(&colliding_shape_idx, 0));
+	for (const LeafShape &shape : ioSensor.mShapes)
+		shape.mShape->CollideSoftBodyVertices(shape.mTransform, shape.mScale, vertex_iterator, uint(mVertices.size()), 0);
+	ioSensor.mHasContact = largest_penetration > 0.0f;
+
+	// We need a contact callback if one of the sensors collided
+	if (ioSensor.mHasContact)
+		mNeedContactCallback = true;
+}
+
+void SoftBodyMotionProperties::ApplyPressure(const SoftBodyUpdateContext &inContext)
+{
+	JPH_PROFILE_FUNCTION();
+
+	float dt = inContext.mSubStepDeltaTime;
+	float pressure_coefficient = mPressure;
+	if (pressure_coefficient > 0.0f)
+	{
+		// Calculate total volume
+		float six_volume = GetVolumeTimesSix();
+		if (six_volume > 0.0f)
+		{
+			// Apply pressure
+			// p = F / A = n R T / V (see https://en.wikipedia.org/wiki/Pressure)
+			// Our pressure coefficient is n R T so the impulse is:
+			// P = F dt = pressure_coefficient / V * A * dt
+			float coefficient = pressure_coefficient * dt / six_volume; // Need to still multiply by 6 for the volume
+			for (const Face &f : mSettings->mFaces)
+			{
+				Vec3 x1 = mVertices[f.mVertex[0]].mPosition;
+				Vec3 x2 = mVertices[f.mVertex[1]].mPosition;
+				Vec3 x3 = mVertices[f.mVertex[2]].mPosition;
+
+				Vec3 impulse = coefficient * (x2 - x1).Cross(x3 - x1); // Area is half the cross product so need to still divide by 2
+				for (uint32 i : f.mVertex)
+				{
+					Vertex &v = mVertices[i];
+					v.mVelocity += v.mInvMass * impulse; // Want to divide by 3 because we spread over 3 vertices
+				}
+			}
+		}
+	}
+}
+
+void SoftBodyMotionProperties::IntegratePositions(const SoftBodyUpdateContext &inContext)
+{
+	JPH_PROFILE_FUNCTION();
+
+	float dt = inContext.mSubStepDeltaTime;
+	float linear_damping = max(0.0f, 1.0f - GetLinearDamping() * dt); // See: MotionProperties::ApplyForceTorqueAndDragInternal
+
+	// Integrate
+	Vec3 sub_step_gravity = inContext.mGravity * dt;
+	Vec3 sub_step_impulse = GetAccumulatedForce() * dt;
+	for (Vertex &v : mVertices)
+		if (v.mInvMass > 0.0f)
+		{
+			// Gravity
+			v.mVelocity += sub_step_gravity + sub_step_impulse * v.mInvMass;
+
+			// Damping
+			v.mVelocity *= linear_damping;
+
+			// Integrate
+			v.mPreviousPosition = v.mPosition;
+			v.mPosition += v.mVelocity * dt;
+		}
+		else
+		{
+			// Integrate
+			v.mPreviousPosition = v.mPosition;
+			v.mPosition += v.mVelocity * dt;
+		}
+}
+
+void SoftBodyMotionProperties::ApplyDihedralBendConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex)
+{
+	JPH_PROFILE_FUNCTION();
+
+	float inv_dt_sq = 1.0f / Square(inContext.mSubStepDeltaTime);
+
+	for (const DihedralBend *b = mSettings->mDihedralBendConstraints.data() + inStartIndex, *b_end = mSettings->mDihedralBendConstraints.data() + inEndIndex; b < b_end; ++b)
+	{
+		Vertex &v0 = mVertices[b->mVertex[0]];
+		Vertex &v1 = mVertices[b->mVertex[1]];
+		Vertex &v2 = mVertices[b->mVertex[2]];
+		Vertex &v3 = mVertices[b->mVertex[3]];
+
+		// Get positions
+		Vec3 x0 = v0.mPosition;
+		Vec3 x1 = v1.mPosition;
+		Vec3 x2 = v2.mPosition;
+		Vec3 x3 = v3.mPosition;
+
+		/*
+		   x2
+		e1/  \e3
+		 /    \
+		x0----x1
+		 \ e0 /
+		e2\  /e4
+		   x3
+		*/
+
+		// Calculate the shared edge of the triangles
+		Vec3 e = x1 - x0;
+		float e_len = e.Length();
+		if (e_len < 1.0e-6f)
+			continue;
+
+		// Calculate the normals of the triangles
+		Vec3 x1x2 = x2 - x1;
+		Vec3 x1x3 = x3 - x1;
+		Vec3 n1 = (x2 - x0).Cross(x1x2);
+		Vec3 n2 = x1x3.Cross(x3 - x0);
+		float n1_len_sq = n1.LengthSq();
+		float n2_len_sq = n2.LengthSq();
+		float n1_len_sq_n2_len_sq = n1_len_sq * n2_len_sq;
+		if (n1_len_sq_n2_len_sq < 1.0e-24f)
+			continue;
+
+		// Calculate constraint equation
+		// As per "Strain Based Dynamics" Appendix A we need to negate the gradients when (n1 x n2) . e > 0, instead we make sure that the sign of the constraint equation is correct
+		float sign = Sign(n2.Cross(n1).Dot(e));
+		float d = n1.Dot(n2) / sqrt(n1_len_sq_n2_len_sq);
+		float c = sign * ACosApproximate(d) - b->mInitialAngle;
+
+		// Ensure the range is -PI to PI
+		if (c > JPH_PI)
+			c -= 2.0f * JPH_PI;
+		else if (c < -JPH_PI)
+			c += 2.0f * JPH_PI;
+
+		// Calculate gradient of constraint equation
+		// Taken from "Strain Based Dynamics" - Matthias Muller et al. (Appendix A)
+		// with p1 = x2, p2 = x3, p3 = x0 and p4 = x1
+		// which in turn is based on "Simulation of Clothing with Folds and Wrinkles" - R. Bridson et al. (Section 4)
+		n1 /= n1_len_sq;
+		n2 /= n2_len_sq;
+		Vec3 d0c = (x1x2.Dot(e) * n1 + x1x3.Dot(e) * n2) / e_len;
+		Vec3 d2c = e_len * n1;
+		Vec3 d3c = e_len * n2;
+
+		// The sum of the gradients must be zero (see "Strain Based Dynamics" section 4)
+		Vec3 d1c = -d0c - d2c - d3c;
+
+		// Get masses
+		float w0 = v0.mInvMass;
+		float w1 = v1.mInvMass;
+		float w2 = v2.mInvMass;
+		float w3 = v3.mInvMass;
+
+		// Calculate -lambda
+		float denom = w0 * d0c.LengthSq() + w1 * d1c.LengthSq() + w2 * d2c.LengthSq() + w3 * d3c.LengthSq() + b->mCompliance * inv_dt_sq;
+		if (denom < 1.0e-12f)
+			continue;
+		float minus_lambda = c / denom;
+
+		// Apply correction
+		v0.mPosition = x0 - minus_lambda * w0 * d0c;
+		v1.mPosition = x1 - minus_lambda * w1 * d1c;
+		v2.mPosition = x2 - minus_lambda * w2 * d2c;
+		v3.mPosition = x3 - minus_lambda * w3 * d3c;
+	}
+}
+
+void SoftBodyMotionProperties::ApplyVolumeConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex)
+{
+	JPH_PROFILE_FUNCTION();
+
+	float inv_dt_sq = 1.0f / Square(inContext.mSubStepDeltaTime);
+
+	// Satisfy volume constraints
+	for (const Volume *v = mSettings->mVolumeConstraints.data() + inStartIndex, *v_end = mSettings->mVolumeConstraints.data() + inEndIndex; v < v_end; ++v)
+	{
+		Vertex &v1 = mVertices[v->mVertex[0]];
+		Vertex &v2 = mVertices[v->mVertex[1]];
+		Vertex &v3 = mVertices[v->mVertex[2]];
+		Vertex &v4 = mVertices[v->mVertex[3]];
+
+		Vec3 x1 = v1.mPosition;
+		Vec3 x2 = v2.mPosition;
+		Vec3 x3 = v3.mPosition;
+		Vec3 x4 = v4.mPosition;
+
+		// Calculate constraint equation
+		Vec3 x1x2 = x2 - x1;
+		Vec3 x1x3 = x3 - x1;
+		Vec3 x1x4 = x4 - x1;
+		float c = abs(x1x2.Cross(x1x3).Dot(x1x4)) - v->mSixRestVolume;
+
+		// Calculate gradient of constraint equation
+		Vec3 d1c = (x4 - x2).Cross(x3 - x2);
+		Vec3 d2c = x1x3.Cross(x1x4);
+		Vec3 d3c = x1x4.Cross(x1x2);
+		Vec3 d4c = x1x2.Cross(x1x3);
+
+		// Get masses
+		float w1 = v1.mInvMass;
+		float w2 = v2.mInvMass;
+		float w3 = v3.mInvMass;
+		float w4 = v4.mInvMass;
+
+		// Calculate -lambda
+		float denom = w1 * d1c.LengthSq() + w2 * d2c.LengthSq() + w3 * d3c.LengthSq() + w4 * d4c.LengthSq() + v->mCompliance * inv_dt_sq;
+		if (denom < 1.0e-12f)
+			continue;
+		float minus_lambda = c / denom;
+
+		// Apply correction
+		v1.mPosition = x1 - minus_lambda * w1 * d1c;
+		v2.mPosition = x2 - minus_lambda * w2 * d2c;
+		v3.mPosition = x3 - minus_lambda * w3 * d3c;
+		v4.mPosition = x4 - minus_lambda * w4 * d4c;
+	}
+}
+
+void SoftBodyMotionProperties::ApplySkinConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex)
+{
+	// Early out if nothing to do
+	if (mSettings->mSkinnedConstraints.empty() || !mEnableSkinConstraints)
+		return;
+
+	JPH_PROFILE_FUNCTION();
+
+	// We're going to iterate multiple times over the skin constraints, update the skinned position accordingly.
+	// If we don't do this, the simulation will see a big jump and the first iteration will cause a big velocity change in the system.
+	float factor = mSkinStatePreviousPositionValid? inContext.mNextIteration.load(std::memory_order_relaxed) / float(mNumIterations) : 1.0f;
+	float prev_factor = 1.0f - factor;
+
+	// Apply the constraints
+	Vertex *vertices = mVertices.data();
+	const SkinState *skin_states = mSkinState.data();
+	for (const Skinned *s = mSettings->mSkinnedConstraints.data() + inStartIndex, *s_end = mSettings->mSkinnedConstraints.data() + inEndIndex; s < s_end; ++s)
+	{
+		Vertex &vertex = vertices[s->mVertex];
+		const SkinState &skin_state = skin_states[s->mVertex];
+		float max_distance = s->mMaxDistance * mSkinnedMaxDistanceMultiplier;
+
+		// Calculate the skinned position by interpolating from previous to current position
+		Vec3 skin_pos = prev_factor * skin_state.mPreviousPosition + factor * skin_state.mPosition;
+
+		if (max_distance > 0.0f)
+		{
+			// Move vertex if it violated the back stop
+			if (s->mBackStopDistance < max_distance)
+			{
+				// Center of the back stop sphere
+				Vec3 center = skin_pos - skin_state.mNormal * (s->mBackStopDistance + s->mBackStopRadius);
+
+				// Check if we're inside the back stop sphere
+				Vec3 delta = vertex.mPosition - center;
+				float delta_len_sq = delta.LengthSq();
+				if (delta_len_sq < Square(s->mBackStopRadius))
+				{
+					// Push the vertex to the surface of the back stop sphere
+					float delta_len = sqrt(delta_len_sq);
+					vertex.mPosition = delta_len > 0.0f?
+						center + delta * (s->mBackStopRadius / delta_len)
+						: center + skin_state.mNormal * s->mBackStopRadius;
+				}
+			}
+
+			// Clamp vertex distance to max distance from skinned position
+			if (max_distance < FLT_MAX)
+			{
+				Vec3 delta = vertex.mPosition - skin_pos;
+				float delta_len_sq = delta.LengthSq();
+				float max_distance_sq = Square(max_distance);
+				if (delta_len_sq > max_distance_sq)
+					vertex.mPosition = skin_pos + delta * sqrt(max_distance_sq / delta_len_sq);
+			}
+		}
+		else
+		{
+			// Kinematic: Just update the vertex position
+			vertex.mPosition = skin_pos;
+		}
+	}
+}
+
+void SoftBodyMotionProperties::ApplyEdgeConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex)
+{
+	JPH_PROFILE_FUNCTION();
+
+	float inv_dt_sq = 1.0f / Square(inContext.mSubStepDeltaTime);
+
+	// Satisfy edge constraints
+	for (const Edge *e = mSettings->mEdgeConstraints.data() + inStartIndex, *e_end = mSettings->mEdgeConstraints.data() + inEndIndex; e < e_end; ++e)
+	{
+		Vertex &v0 = mVertices[e->mVertex[0]];
+		Vertex &v1 = mVertices[e->mVertex[1]];
+
+		// Get positions
+		Vec3 x0 = v0.mPosition;
+		Vec3 x1 = v1.mPosition;
+
+		// Calculate current length
+		Vec3 delta = x1 - x0;
+		float length = delta.Length();
+
+		// Apply correction
+		float denom = length * (v0.mInvMass + v1.mInvMass + e->mCompliance * inv_dt_sq);
+		if (denom < 1.0e-12f)
+			continue;
+		Vec3 correction = delta * (length - e->mRestLength) / denom;
+		v0.mPosition = x0 + v0.mInvMass * correction;
+		v1.mPosition = x1 - v1.mInvMass * correction;
+	}
+}
+
+void SoftBodyMotionProperties::ApplyLRAConstraints(uint inStartIndex, uint inEndIndex)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Satisfy LRA constraints
+	Vertex *vertices = mVertices.data();
+	for (const LRA *lra = mSettings->mLRAConstraints.data() + inStartIndex, *lra_end = mSettings->mLRAConstraints.data() + inEndIndex; lra < lra_end; ++lra)
+	{
+		JPH_ASSERT(lra->mVertex[0] < mVertices.size());
+		JPH_ASSERT(lra->mVertex[1] < mVertices.size());
+		const Vertex &vertex0 = vertices[lra->mVertex[0]];
+		Vertex &vertex1 = vertices[lra->mVertex[1]];
+
+		Vec3 x0 = vertex0.mPosition;
+		Vec3 delta = vertex1.mPosition - x0;
+		float delta_len_sq = delta.LengthSq();
+		if (delta_len_sq > Square(lra->mMaxDistance))
+			vertex1.mPosition = x0 + delta * lra->mMaxDistance / sqrt(delta_len_sq);
+	}
+}
+
+void SoftBodyMotionProperties::ApplyCollisionConstraintsAndUpdateVelocities(const SoftBodyUpdateContext &inContext)
+{
+	JPH_PROFILE_FUNCTION();
+
+	float dt = inContext.mSubStepDeltaTime;
+	float restitution_treshold = -2.0f * inContext.mGravity.Length() * dt;
+	float vertex_radius = mSettings->mVertexRadius;
+	for (Vertex &v : mVertices)
+		if (v.mInvMass > 0.0f)
+		{
+			// Remember previous velocity for restitution calculations
+			Vec3 prev_v = v.mVelocity;
+
+			// XPBD velocity update
+			v.mVelocity = (v.mPosition - v.mPreviousPosition) / dt;
+
+			// Satisfy collision constraint
+			if (v.mCollidingShapeIndex >= 0)
+			{
+				// Check if there is a collision
+				float projected_distance = -v.mCollisionPlane.SignedDistance(v.mPosition) + vertex_radius;
+				if (projected_distance > 0.0f)
+				{
+					// Remember that there was a collision
+					v.mHasContact = true;
+
+					// We need a contact callback if one of the vertices collided
+					mNeedContactCallback = true;
+
+					// Note that we already calculated the velocity, so this does not affect the velocity (next iteration starts by setting previous position to current position)
+					CollidingShape &cs = mCollidingShapes[v.mCollidingShapeIndex];
+					Vec3 contact_normal = v.mCollisionPlane.GetNormal();
+					v.mPosition += contact_normal * projected_distance;
+
+					// Apply friction as described in Detailed Rigid Body Simulation with Extended Position Based Dynamics - Matthias Muller et al.
+					// See section 3.6:
+					// Inverse mass: w1 = 1 / m1, w2 = 1 / m2 + (r2 x n)^T I^-1 (r2 x n) = 0 for a static object
+					// r2 are the contact point relative to the center of mass of body 2
+					// Lagrange multiplier for contact: lambda = -c / (w1 + w2)
+					// Where c is the constraint equation (the distance to the plane, negative because penetrating)
+					// Contact normal force: fn = lambda / dt^2
+					// Delta velocity due to friction dv = -vt / |vt| * min(dt * friction * fn * (w1 + w2), |vt|) = -vt * min(-friction * c / (|vt| * dt), 1)
+					// Note that I think there is an error in the paper, I added a mass term, see: https://github.com/matthias-research/pages/issues/29
+					// Relative velocity: vr = v1 - v2 - omega2 x r2
+					// Normal velocity: vn = vr . contact_normal
+					// Tangential velocity: vt = vr - contact_normal * vn
+					// Impulse: p = dv / (w1 + w2)
+					// Changes in particle velocities:
+					// v1 = v1 + p / m1
+					// v2 = v2 - p / m2 (no change when colliding with a static body)
+					// w2 = w2 - I^-1 (r2 x p) (no change when colliding with a static body)
+					if (cs.mMotionType == EMotionType::Dynamic)
+					{
+						// Calculate normal and tangential velocity (equation 30)
+						Vec3 r2 = v.mPosition - cs.mCenterOfMassTransform.GetTranslation();
+						Vec3 v2 = cs.GetPointVelocity(r2);
+						Vec3 relative_velocity = v.mVelocity - v2;
+						Vec3 v_normal = contact_normal * contact_normal.Dot(relative_velocity);
+						Vec3 v_tangential = relative_velocity - v_normal;
+						float v_tangential_length = v_tangential.Length();
+
+						// Calculate resulting inverse mass of vertex
+						float vertex_inv_mass = cs.mSoftBodyInvMassScale * v.mInvMass;
+
+						// Calculate inverse effective mass
+						Vec3 r2_cross_n = r2.Cross(contact_normal);
+						float w2 = cs.mInvMass + r2_cross_n.Dot(cs.mInvInertia * r2_cross_n);
+						float w1_plus_w2 = vertex_inv_mass + w2;
+						if (w1_plus_w2 > 0.0f)
+						{
+							// Calculate delta relative velocity due to friction (modified equation 31)
+							Vec3 dv;
+							if (v_tangential_length > 0.0f)
+								dv = v_tangential * min(cs.mFriction * projected_distance / (v_tangential_length * dt), 1.0f);
+							else
+								dv = Vec3::sZero();
+
+							// Calculate delta relative velocity due to restitution (equation 35)
+							dv += v_normal;
+							float prev_v_normal = (prev_v - v2).Dot(contact_normal);
+							if (prev_v_normal < restitution_treshold)
+								dv += cs.mRestitution * prev_v_normal * contact_normal;
+
+							// Calculate impulse
+							Vec3 p = dv / w1_plus_w2;
+
+							// Apply impulse to particle
+							v.mVelocity -= p * vertex_inv_mass;
+
+							// Apply impulse to rigid body
+							cs.mLinearVelocity += p * cs.mInvMass;
+							cs.mAngularVelocity += cs.mInvInertia * r2.Cross(p);
+
+							// Mark that the velocities of the body we hit need to be updated
+							cs.mUpdateVelocities = true;
+						}
+					}
+					else if (cs.mSoftBodyInvMassScale > 0.0f)
+					{
+						// Body is not movable, equations are simpler
+
+						// Calculate normal and tangential velocity (equation 30)
+						Vec3 v_normal = contact_normal * contact_normal.Dot(v.mVelocity);
+						Vec3 v_tangential = v.mVelocity - v_normal;
+						float v_tangential_length = v_tangential.Length();
+
+						// Apply friction (modified equation 31)
+						if (v_tangential_length > 0.0f)
+							v.mVelocity -= v_tangential * min(cs.mFriction * projected_distance / (v_tangential_length * dt), 1.0f);
+
+						// Apply restitution (equation 35)
+						v.mVelocity -= v_normal;
+						float prev_v_normal = prev_v.Dot(contact_normal);
+						if (prev_v_normal < restitution_treshold)
+							v.mVelocity -= cs.mRestitution * prev_v_normal * contact_normal;
+					}
+				}
+			}
+		}
+}
+
+void SoftBodyMotionProperties::UpdateSoftBodyState(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Contact callback
+	if (mNeedContactCallback && ioContext.mContactListener != nullptr)
+	{
+		// Remove non-colliding sensors from the list
+		for (int i = int(mCollidingSensors.size()) - 1; i >= 0; --i)
+			if (!mCollidingSensors[i].mHasContact)
+			{
+				mCollidingSensors[i] = std::move(mCollidingSensors.back());
+				mCollidingSensors.pop_back();
+			}
+
+		ioContext.mContactListener->OnSoftBodyContactAdded(*ioContext.mBody, SoftBodyManifold(this));
+	}
+
+	// Loop through vertices once more to update the global state
+	float dt = ioContext.mDeltaTime;
+	float max_linear_velocity_sq = Square(GetMaxLinearVelocity());
+	float max_v_sq = 0.0f;
+	Vec3 linear_velocity = Vec3::sZero(), angular_velocity = Vec3::sZero();
+	mLocalPredictedBounds = mLocalBounds = { };
+	for (Vertex &v : mVertices)
+	{
+		// Calculate max square velocity
+		float v_sq = v.mVelocity.LengthSq();
+		max_v_sq = max(max_v_sq, v_sq);
+
+		// Clamp if velocity is too high
+		if (v_sq > max_linear_velocity_sq)
+			v.mVelocity *= sqrt(max_linear_velocity_sq / v_sq);
+
+		// Calculate local linear/angular velocity
+		linear_velocity += v.mVelocity;
+		angular_velocity += v.mPosition.Cross(v.mVelocity);
+
+		// Update local bounding box
+		mLocalBounds.Encapsulate(v.mPosition);
+
+		// Create predicted position for the next frame in order to detect collisions before they happen
+		mLocalPredictedBounds.Encapsulate(v.mPosition + v.mVelocity * dt + ioContext.mDisplacementDueToGravity);
+
+		// Reset collision data for the next iteration
+		v.ResetCollision();
+	}
+
+	// Calculate linear/angular velocity of the body by averaging all vertices and bringing the value to world space
+	float num_vertices_divider = float(max(int(mVertices.size()), 1));
+	SetLinearVelocity(ioContext.mCenterOfMassTransform.Multiply3x3(linear_velocity / num_vertices_divider));
+	SetAngularVelocity(ioContext.mCenterOfMassTransform.Multiply3x3(angular_velocity / num_vertices_divider));
+
+	if (mUpdatePosition)
+	{
+		// Shift the body so that the position is the center of the local bounds
+		Vec3 delta = mLocalBounds.GetCenter();
+		ioContext.mDeltaPosition = ioContext.mCenterOfMassTransform.Multiply3x3(delta);
+		for (Vertex &v : mVertices)
+			v.mPosition -= delta;
+
+		// Update the skin state too since we will use this position as the previous position in the next update
+		for (SkinState &s : mSkinState)
+			s.mPosition -= delta;
+		JPH_IF_DEBUG_RENDERER(mSkinStateTransform.SetTranslation(mSkinStateTransform.GetTranslation() + ioContext.mDeltaPosition);)
+
+		// Offset bounds to match new position
+		mLocalBounds.Translate(-delta);
+		mLocalPredictedBounds.Translate(-delta);
+	}
+	else
+		ioContext.mDeltaPosition = Vec3::sZero();
+
+	// Test if we should go to sleep
+	if (GetAllowSleeping())
+	{
+		if (max_v_sq > inPhysicsSettings.mPointVelocitySleepThreshold)
+		{
+			ResetSleepTestTimer();
+			ioContext.mCanSleep = ECanSleep::CannotSleep;
+		}
+		else
+			ioContext.mCanSleep = AccumulateSleepTime(dt, inPhysicsSettings.mTimeBeforeSleep);
+	}
+	else
+		ioContext.mCanSleep = ECanSleep::CannotSleep;
+
+	// If SkinVertices is not called after this then don't use the previous position as the skin is static
+	mSkinStatePreviousPositionValid = false;
+
+	// Reset force accumulator
+	ResetForce();
+}
+
+void SoftBodyMotionProperties::UpdateRigidBodyVelocities(const SoftBodyUpdateContext &inContext, BodyInterface &inBodyInterface)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Write back velocity deltas
+	for (const CollidingShape &cs : mCollidingShapes)
+		if (cs.mUpdateVelocities)
+			inBodyInterface.AddLinearAndAngularVelocity(cs.mBodyID, inContext.mCenterOfMassTransform.Multiply3x3(cs.mLinearVelocity - cs.mOriginalLinearVelocity), inContext.mCenterOfMassTransform.Multiply3x3(cs.mAngularVelocity - cs.mOriginalAngularVelocity));
+
+	// Clear colliding shapes/sensors to avoid hanging on to references to shapes
+	mCollidingShapes.clear();
+	mCollidingSensors.clear();
+}
+
+void SoftBodyMotionProperties::InitializeUpdateContext(float inDeltaTime, Body &inSoftBody, const PhysicsSystem &inSystem, SoftBodyUpdateContext &ioContext)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Store body
+	ioContext.mBody = &inSoftBody;
+	ioContext.mMotionProperties = this;
+	ioContext.mContactListener = inSystem.GetSoftBodyContactListener();
+	ioContext.mSimShapeFilter = inSystem.GetSimShapeFilter();
+
+	// Convert gravity to local space
+	ioContext.mCenterOfMassTransform = inSoftBody.GetCenterOfMassTransform();
+	ioContext.mGravity = ioContext.mCenterOfMassTransform.Multiply3x3Transposed(GetGravityFactor() * inSystem.GetGravity());
+
+	// Calculate delta time for sub step
+	ioContext.mDeltaTime = inDeltaTime;
+	ioContext.mSubStepDeltaTime = inDeltaTime / mNumIterations;
+
+	// Calculate total displacement we'll have due to gravity over all sub steps
+	// The total displacement as produced by our integrator can be written as: Sum(i * g * dt^2, i = 0..mNumIterations).
+	// This is bigger than 0.5 * g * dt^2 because we first increment the velocity and then update the position
+	// Using Sum(i, i = 0..n) = n * (n + 1) / 2 we can write this as:
+	ioContext.mDisplacementDueToGravity = (0.5f * mNumIterations * (mNumIterations + 1) * Square(ioContext.mSubStepDeltaTime)) * ioContext.mGravity;
+}
+
+void SoftBodyMotionProperties::StartNextIteration(const SoftBodyUpdateContext &ioContext)
+{
+	ApplyPressure(ioContext);
+
+	IntegratePositions(ioContext);
+}
+
+void SoftBodyMotionProperties::StartFirstIteration(SoftBodyUpdateContext &ioContext)
+{
+	// Start the first iteration
+	JPH_IF_ENABLE_ASSERTS(uint iteration =) ioContext.mNextIteration.fetch_add(1, memory_order_relaxed);
+	JPH_ASSERT(iteration == 0);
+	StartNextIteration(ioContext);
+	ioContext.mState.store(SoftBodyUpdateContext::EState::ApplyConstraints, memory_order_release);
+}
+
+SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelDetermineCollisionPlanes(SoftBodyUpdateContext &ioContext)
+{
+	// Do a relaxed read first to see if there is any work to do (this prevents us from doing expensive atomic operations and also prevents us from continuously incrementing the counter and overflowing it)
+	uint num_vertices = (uint)mVertices.size();
+	if (ioContext.mNextCollisionVertex.load(memory_order_relaxed) < num_vertices)
+	{
+		// Fetch next batch of vertices to process
+		uint next_vertex = ioContext.mNextCollisionVertex.fetch_add(SoftBodyUpdateContext::cVertexCollisionBatch, memory_order_acquire);
+		if (next_vertex < num_vertices)
+		{
+			// Process collision planes
+			uint num_vertices_to_process = min(SoftBodyUpdateContext::cVertexCollisionBatch, num_vertices - next_vertex);
+			DetermineCollisionPlanes(next_vertex, num_vertices_to_process);
+			uint vertices_processed = ioContext.mNumCollisionVerticesProcessed.fetch_add(SoftBodyUpdateContext::cVertexCollisionBatch, memory_order_release) + num_vertices_to_process;
+			if (vertices_processed >= num_vertices)
+			{
+				// Determine next state
+				if (mCollidingSensors.empty())
+					StartFirstIteration(ioContext);
+				else
+					ioContext.mState.store(SoftBodyUpdateContext::EState::DetermineSensorCollisions, memory_order_release);
+			}
+			return EStatus::DidWork;
+		}
+	}
+
+	return EStatus::NoWork;
+}
+
+SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelDetermineSensorCollisions(SoftBodyUpdateContext &ioContext)
+{
+	// Do a relaxed read to see if there are more sensors to process
+	uint num_sensors = (uint)mCollidingSensors.size();
+	if (ioContext.mNextSensorIndex.load(memory_order_relaxed) < num_sensors)
+	{
+		// Fetch next sensor to process
+		uint sensor_index = ioContext.mNextSensorIndex.fetch_add(1, memory_order_acquire);
+		if (sensor_index < num_sensors)
+		{
+			// Process this sensor
+			DetermineSensorCollisions(mCollidingSensors[sensor_index]);
+
+			// Determine next state
+			uint sensors_processed = ioContext.mNumSensorsProcessed.fetch_add(1, memory_order_release) + 1;
+			if (sensors_processed >= num_sensors)
+				StartFirstIteration(ioContext);
+			return EStatus::DidWork;
+		}
+	}
+
+	return EStatus::NoWork;
+}
+
+void SoftBodyMotionProperties::ProcessGroup(const SoftBodyUpdateContext &ioContext, uint inGroupIndex)
+{
+	// Determine start and end
+	SoftBodySharedSettings::UpdateGroup start { 0, 0, 0, 0, 0 };
+	const SoftBodySharedSettings::UpdateGroup &prev = inGroupIndex > 0? mSettings->mUpdateGroups[inGroupIndex - 1] : start;
+	const SoftBodySharedSettings::UpdateGroup &current = mSettings->mUpdateGroups[inGroupIndex];
+
+	// Process volume constraints
+	ApplyVolumeConstraints(ioContext, prev.mVolumeEndIndex, current.mVolumeEndIndex);
+
+	// Process bend constraints
+	ApplyDihedralBendConstraints(ioContext, prev.mDihedralBendEndIndex, current.mDihedralBendEndIndex);
+
+	// Process skinned constraints
+	ApplySkinConstraints(ioContext, prev.mSkinnedEndIndex, current.mSkinnedEndIndex);
+
+	// Process edges
+	ApplyEdgeConstraints(ioContext, prev.mEdgeEndIndex, current.mEdgeEndIndex);
+
+	// Process LRA constraints
+	ApplyLRAConstraints(prev.mLRAEndIndex, current.mLRAEndIndex);
+}
+
+SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelApplyConstraints(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings)
+{
+	uint num_groups = (uint)mSettings->mUpdateGroups.size();
+	JPH_ASSERT(num_groups > 0, "SoftBodySharedSettings::Optimize should have been called!");
+	--num_groups; // Last group is the non-parallel group, we don't want to execute it in parallel
+
+	// Do a relaxed read first to see if there is any work to do (this prevents us from doing expensive atomic operations and also prevents us from continuously incrementing the counter and overflowing it)
+	uint next_group = ioContext.mNextConstraintGroup.load(memory_order_relaxed);
+	if (next_group < num_groups || (num_groups == 0 && next_group == 0))
+	{
+		// Fetch the next group process
+		next_group = ioContext.mNextConstraintGroup.fetch_add(1, memory_order_acquire);
+		if (next_group < num_groups || (num_groups == 0 && next_group == 0))
+		{
+			uint num_groups_processed = 0;
+			if (num_groups > 0)
+			{
+				// Process this group
+				ProcessGroup(ioContext, next_group);
+
+				// Increment total number of groups processed
+				num_groups_processed = ioContext.mNumConstraintGroupsProcessed.fetch_add(1, memory_order_relaxed) + 1;
+			}
+
+			if (num_groups_processed >= num_groups)
+			{
+				// Finish the iteration
+				JPH_PROFILE("FinishIteration");
+
+				// Process non-parallel group
+				ProcessGroup(ioContext, num_groups);
+
+				ApplyCollisionConstraintsAndUpdateVelocities(ioContext);
+
+				uint iteration = ioContext.mNextIteration.fetch_add(1, memory_order_relaxed);
+				if (iteration < mNumIterations)
+				{
+					// Start a new iteration
+					StartNextIteration(ioContext);
+
+					// Reset group logic
+					ioContext.mNumConstraintGroupsProcessed.store(0, memory_order_relaxed);
+					ioContext.mNextConstraintGroup.store(0, memory_order_release);
+				}
+				else
+				{
+					// On final iteration we update the state
+					UpdateSoftBodyState(ioContext, inPhysicsSettings);
+
+					ioContext.mState.store(SoftBodyUpdateContext::EState::Done, memory_order_release);
+					return EStatus::Done;
+				}
+			}
+
+			return EStatus::DidWork;
+		}
+	}
+	return EStatus::NoWork;
+}
+
+SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelUpdate(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings)
+{
+	switch (ioContext.mState.load(memory_order_relaxed))
+	{
+	case SoftBodyUpdateContext::EState::DetermineCollisionPlanes:
+		return ParallelDetermineCollisionPlanes(ioContext);
+
+	case SoftBodyUpdateContext::EState::DetermineSensorCollisions:
+		return ParallelDetermineSensorCollisions(ioContext);
+
+	case SoftBodyUpdateContext::EState::ApplyConstraints:
+		return ParallelApplyConstraints(ioContext, inPhysicsSettings);
+
+	case SoftBodyUpdateContext::EState::Done:
+		return EStatus::Done;
+
+	default:
+		JPH_ASSERT(false);
+		return EStatus::NoWork;
+	}
+}
+
+void SoftBodyMotionProperties::SkinVertices([[maybe_unused]] RMat44Arg inCenterOfMassTransform, const Mat44 *inJointMatrices, [[maybe_unused]] uint inNumJoints, bool inHardSkinAll, TempAllocator &ioTempAllocator)
+{
+	// Calculate the skin matrices
+	uint num_skin_matrices = uint(mSettings->mInvBindMatrices.size());
+	uint skin_matrices_size = num_skin_matrices * sizeof(Mat44);
+	Mat44 *skin_matrices = (Mat44 *)ioTempAllocator.Allocate(skin_matrices_size);
+	JPH_SCOPE_EXIT([&ioTempAllocator, skin_matrices, skin_matrices_size]{ ioTempAllocator.Free(skin_matrices, skin_matrices_size); });
+	const Mat44 *skin_matrices_end = skin_matrices + num_skin_matrices;
+	const InvBind *inv_bind_matrix = mSettings->mInvBindMatrices.data();
+	for (Mat44 *s = skin_matrices; s < skin_matrices_end; ++s, ++inv_bind_matrix)
+		*s = inJointMatrices[inv_bind_matrix->mJointIndex] * inv_bind_matrix->mInvBind;
+
+	// Skin the vertices
+	JPH_IF_DEBUG_RENDERER(mSkinStateTransform = inCenterOfMassTransform;)
+	JPH_IF_ENABLE_ASSERTS(uint num_vertices = uint(mSettings->mVertices.size());)
+	JPH_ASSERT(mSkinState.size() == num_vertices);
+	const SoftBodySharedSettings::Vertex *in_vertices = mSettings->mVertices.data();
+	for (const Skinned &s : mSettings->mSkinnedConstraints)
+	{
+		// Get bind pose
+		JPH_ASSERT(s.mVertex < num_vertices);
+		Vec3 bind_pos = Vec3::sLoadFloat3Unsafe(in_vertices[s.mVertex].mPosition);
+
+		// Skin vertex
+		Vec3 pos = Vec3::sZero();
+		for (const SkinWeight &w : s.mWeights)
+		{
+			// We assume that the first zero weight is the end of the list
+			if (w.mWeight == 0.0f)
+				break;
+
+			JPH_ASSERT(w.mInvBindIndex < num_skin_matrices);
+			pos += w.mWeight * (skin_matrices[w.mInvBindIndex] * bind_pos);
+		}
+		SkinState &skin_state = mSkinState[s.mVertex];
+		skin_state.mPreviousPosition = skin_state.mPosition;
+		skin_state.mPosition = pos;
+	}
+
+	// Calculate the normals
+	for (const Skinned &s : mSettings->mSkinnedConstraints)
+	{
+		Vec3 normal = Vec3::sZero();
+		uint32 num_faces = s.mNormalInfo >> 24;
+		if (num_faces > 0)
+		{
+			// Calculate normal
+			const uint32 *f = &mSettings->mSkinnedConstraintNormals[s.mNormalInfo & 0xffffff];
+			const uint32 *f_end = f + num_faces;
+			while (f < f_end)
+			{
+				const Face &face = mSettings->mFaces[*f];
+				Vec3 v0 = mSkinState[face.mVertex[0]].mPosition;
+				Vec3 v1 = mSkinState[face.mVertex[1]].mPosition;
+				Vec3 v2 = mSkinState[face.mVertex[2]].mPosition;
+				normal += (v1 - v0).Cross(v2 - v0).NormalizedOr(Vec3::sZero());
+				++f;
+			}
+			normal = normal.NormalizedOr(Vec3::sZero());
+		}
+		mSkinState[s.mVertex].mNormal = normal;
+	}
+
+	if (inHardSkinAll)
+	{
+		// Hard skin all vertices and reset their velocities
+		for (const Skinned &s : mSettings->mSkinnedConstraints)
+		{
+			Vertex &vertex = mVertices[s.mVertex];
+			SkinState &skin_state = mSkinState[s.mVertex];
+			skin_state.mPreviousPosition = skin_state.mPosition;
+			vertex.mPosition = skin_state.mPosition;
+			vertex.mVelocity = Vec3::sZero();
+		}
+	}
+	else if (!mEnableSkinConstraints)
+	{
+		// Hard skin only the kinematic vertices as we will not solve the skin constraints later
+		for (const Skinned &s : mSettings->mSkinnedConstraints)
+			if (s.mMaxDistance == 0.0f)
+			{
+				Vertex &vertex = mVertices[s.mVertex];
+				vertex.mPosition = mSkinState[s.mVertex].mPosition;
+			}
+	}
+
+	// Indicate that the previous positions are valid for the coming update
+	mSkinStatePreviousPositionValid = true;
+}
+
+void SoftBodyMotionProperties::CustomUpdate(float inDeltaTime, Body &ioSoftBody, PhysicsSystem &inSystem)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Create update context
+	SoftBodyUpdateContext context;
+	InitializeUpdateContext(inDeltaTime, ioSoftBody, inSystem, context);
+
+	// Determine bodies we're colliding with
+	DetermineCollidingShapes(context, inSystem, inSystem.GetBodyLockInterface());
+
+	// Call the internal update until it finishes
+	EStatus status;
+	const PhysicsSettings &settings = inSystem.GetPhysicsSettings();
+	while ((status = ParallelUpdate(context, settings)) == EStatus::DidWork)
+		continue;
+	JPH_ASSERT(status == EStatus::Done);
+
+	// Update the state of the bodies we've collided with
+	UpdateRigidBodyVelocities(context, inSystem.GetBodyInterface());
+
+	// Update position of the soft body
+	if (mUpdatePosition)
+		inSystem.GetBodyInterface().SetPosition(ioSoftBody.GetID(), ioSoftBody.GetPosition() + context.mDeltaPosition, EActivation::DontActivate);
+}
+
+#ifdef JPH_DEBUG_RENDERER
+
+void SoftBodyMotionProperties::DrawVertices(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const
+{
+	for (const Vertex &v : mVertices)
+		inRenderer->DrawMarker(inCenterOfMassTransform * v.mPosition, v.mInvMass > 0.0f? Color::sGreen : Color::sRed, 0.05f);
+}
+
+void SoftBodyMotionProperties::DrawVertexVelocities(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const
+{
+	for (const Vertex &v : mVertices)
+		inRenderer->DrawArrow(inCenterOfMassTransform * v.mPosition, inCenterOfMassTransform * (v.mPosition + v.mVelocity), Color::sYellow, 0.01f);
+}
+
+template <typename GetEndIndex, typename DrawConstraint>
+inline void SoftBodyMotionProperties::DrawConstraints(ESoftBodyConstraintColor inConstraintColor, const GetEndIndex &inGetEndIndex, const DrawConstraint &inDrawConstraint, ColorArg inBaseColor) const
+{
+	uint start = 0;
+	for (uint i = 0; i < (uint)mSettings->mUpdateGroups.size(); ++i)
+	{
+		uint end = inGetEndIndex(mSettings->mUpdateGroups[i]);
+
+		Color base_color;
+		if (inConstraintColor != ESoftBodyConstraintColor::ConstraintType)
+			base_color = Color::sGetDistinctColor((uint)mSettings->mUpdateGroups.size() - i - 1); // Ensure that color 0 is always the last group
+		else
+			base_color = inBaseColor;
+
+		for (uint idx = start; idx < end; ++idx)
+		{
+			Color color = inConstraintColor == ESoftBodyConstraintColor::ConstraintOrder? base_color * (float(idx - start) / (end - start)) : base_color;
+			inDrawConstraint(idx, color);
+		}
+
+		start = end;
+	}
+}
+
+void SoftBodyMotionProperties::DrawEdgeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const
+{
+	DrawConstraints(inConstraintColor,
+		[](const SoftBodySharedSettings::UpdateGroup &inGroup) {
+			return inGroup.mEdgeEndIndex;
+		},
+		[this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) {
+			const Edge &e = mSettings->mEdgeConstraints[inIndex];
+			inRenderer->DrawLine(inCenterOfMassTransform * mVertices[e.mVertex[0]].mPosition, inCenterOfMassTransform * mVertices[e.mVertex[1]].mPosition, inColor);
+		},
+		Color::sWhite);
+}
+
+void SoftBodyMotionProperties::DrawBendConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const
+{
+	DrawConstraints(inConstraintColor,
+		[](const SoftBodySharedSettings::UpdateGroup &inGroup) {
+			return inGroup.mDihedralBendEndIndex;
+		},
+		[this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) {
+			const DihedralBend &b = mSettings->mDihedralBendConstraints[inIndex];
+
+			RVec3 x0 = inCenterOfMassTransform * mVertices[b.mVertex[0]].mPosition;
+			RVec3 x1 = inCenterOfMassTransform * mVertices[b.mVertex[1]].mPosition;
+			RVec3 x2 = inCenterOfMassTransform * mVertices[b.mVertex[2]].mPosition;
+			RVec3 x3 = inCenterOfMassTransform * mVertices[b.mVertex[3]].mPosition;
+			RVec3 c_edge = 0.5_r * (x0 + x1);
+			RVec3 c0 = (x0 + x1 + x2) / 3.0_r;
+			RVec3 c1 = (x0 + x1 + x3) / 3.0_r;
+
+			inRenderer->DrawArrow(0.9_r * x0 + 0.1_r * x1, 0.1_r * x0 + 0.9_r * x1, inColor, 0.01f);
+			inRenderer->DrawLine(c_edge, 0.1_r * c_edge + 0.9_r * c0, inColor);
+			inRenderer->DrawLine(c_edge, 0.1_r * c_edge + 0.9_r * c1, inColor);
+		},
+		Color::sGreen);
+}
+
+void SoftBodyMotionProperties::DrawVolumeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const
+{
+	DrawConstraints(inConstraintColor,
+		[](const SoftBodySharedSettings::UpdateGroup &inGroup) {
+			return inGroup.mVolumeEndIndex;
+		},
+		[this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) {
+			const Volume &v = mSettings->mVolumeConstraints[inIndex];
+
+			RVec3 x1 = inCenterOfMassTransform * mVertices[v.mVertex[0]].mPosition;
+			RVec3 x2 = inCenterOfMassTransform * mVertices[v.mVertex[1]].mPosition;
+			RVec3 x3 = inCenterOfMassTransform * mVertices[v.mVertex[2]].mPosition;
+			RVec3 x4 = inCenterOfMassTransform * mVertices[v.mVertex[3]].mPosition;
+
+			inRenderer->DrawTriangle(x1, x3, x2, inColor, DebugRenderer::ECastShadow::On);
+			inRenderer->DrawTriangle(x2, x3, x4, inColor, DebugRenderer::ECastShadow::On);
+			inRenderer->DrawTriangle(x1, x4, x3, inColor, DebugRenderer::ECastShadow::On);
+			inRenderer->DrawTriangle(x1, x2, x4, inColor, DebugRenderer::ECastShadow::On);
+		},
+		Color::sYellow);
+}
+
+void SoftBodyMotionProperties::DrawSkinConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const
+{
+	DrawConstraints(inConstraintColor,
+		[](const SoftBodySharedSettings::UpdateGroup &inGroup) {
+			return inGroup.mSkinnedEndIndex;
+		},
+		[this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) {
+			const Skinned &s = mSettings->mSkinnedConstraints[inIndex];
+			const SkinState &skin_state = mSkinState[s.mVertex];
+			inRenderer->DrawArrow(mSkinStateTransform * skin_state.mPosition, mSkinStateTransform * (skin_state.mPosition + 0.1f * skin_state.mNormal), inColor, 0.01f);
+			inRenderer->DrawLine(mSkinStateTransform * skin_state.mPosition, inCenterOfMassTransform * mVertices[s.mVertex].mPosition, Color::sBlue);
+		},
+		Color::sOrange);
+}
+
+void SoftBodyMotionProperties::DrawLRAConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const
+{
+	DrawConstraints(inConstraintColor,
+		[](const SoftBodySharedSettings::UpdateGroup &inGroup) {
+			return inGroup.mLRAEndIndex;
+		},
+		[this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) {
+			const LRA &l = mSettings->mLRAConstraints[inIndex];
+			inRenderer->DrawLine(inCenterOfMassTransform * mVertices[l.mVertex[0]].mPosition, inCenterOfMassTransform * mVertices[l.mVertex[1]].mPosition, inColor);
+		},
+		Color::sGrey);
+}
+
+void SoftBodyMotionProperties::DrawPredictedBounds(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const
+{
+	inRenderer->DrawWireBox(inCenterOfMassTransform, mLocalPredictedBounds, Color::sRed);
+}
+
+#endif // JPH_DEBUG_RENDERER
+
+void SoftBodyMotionProperties::SaveState(StateRecorder &inStream) const
+{
+	MotionProperties::SaveState(inStream);
+
+	for (const Vertex &v : mVertices)
+	{
+		inStream.Write(v.mPreviousPosition);
+		inStream.Write(v.mPosition);
+		inStream.Write(v.mVelocity);
+	}
+
+	for (const SkinState &s : mSkinState)
+	{
+		inStream.Write(s.mPreviousPosition);
+		inStream.Write(s.mPosition);
+		inStream.Write(s.mNormal);
+	}
+
+	inStream.Write(mLocalBounds.mMin);
+	inStream.Write(mLocalBounds.mMax);
+	inStream.Write(mLocalPredictedBounds.mMin);
+	inStream.Write(mLocalPredictedBounds.mMax);
+}
+
+void SoftBodyMotionProperties::RestoreState(StateRecorder &inStream)
+{
+	MotionProperties::RestoreState(inStream);
+
+	for (Vertex &v : mVertices)
+	{
+		inStream.Read(v.mPreviousPosition);
+		inStream.Read(v.mPosition);
+		inStream.Read(v.mVelocity);
+	}
+
+	for (SkinState &s : mSkinState)
+	{
+		inStream.Read(s.mPreviousPosition);
+		inStream.Read(s.mPosition);
+		inStream.Read(s.mNormal);
+	}
+
+	inStream.Read(mLocalBounds.mMin);
+	inStream.Read(mLocalBounds.mMax);
+	inStream.Read(mLocalPredictedBounds.mMin);
+	inStream.Read(mLocalPredictedBounds.mMax);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.h
new file mode 100644
index 0000000000..af66b7a2e4
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyMotionProperties.h
@@ -0,0 +1,297 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/AABox.h>
+#include <Jolt/Physics/Body/BodyID.h>
+#include <Jolt/Physics/Body/MotionProperties.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/SoftBody/SoftBodySharedSettings.h>
+#include <Jolt/Physics/SoftBody/SoftBodyVertex.h>
+#include <Jolt/Physics/SoftBody/SoftBodyUpdateContext.h>
+
+JPH_NAMESPACE_BEGIN
+
+class PhysicsSystem;
+class BodyInterface;
+class BodyLockInterface;
+struct PhysicsSettings;
+class Body;
+class Shape;
+class SoftBodyCreationSettings;
+class TempAllocator;
+#ifdef JPH_DEBUG_RENDERER
+class DebugRenderer;
+enum class ESoftBodyConstraintColor;
+#endif // JPH_DEBUG_RENDERER
+
+/// This class contains the runtime information of a soft body.
+//
+// Based on: XPBD, Extended Position Based Dynamics, Matthias Muller, Ten Minute Physics
+// See: https://matthias-research.github.io/pages/tenMinutePhysics/09-xpbd.pdf
+class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties
+{
+public:
+	using Vertex = SoftBodyVertex;
+	using Edge = SoftBodySharedSettings::Edge;
+	using Face = SoftBodySharedSettings::Face;
+	using DihedralBend = SoftBodySharedSettings::DihedralBend;
+	using Volume = SoftBodySharedSettings::Volume;
+	using InvBind = SoftBodySharedSettings::InvBind;
+	using SkinWeight = SoftBodySharedSettings::SkinWeight;
+	using Skinned = SoftBodySharedSettings::Skinned;
+	using LRA = SoftBodySharedSettings::LRA;
+
+	/// Initialize the soft body motion properties
+	void								Initialize(const SoftBodyCreationSettings &inSettings);
+
+	/// Get the shared settings of the soft body
+	const SoftBodySharedSettings *		GetSettings() const							{ return mSettings; }
+
+	/// Get the vertices of the soft body
+	const Array<Vertex> &				GetVertices() const							{ return mVertices; }
+	Array<Vertex> &						GetVertices()								{ return mVertices; }
+
+	/// Access an individual vertex
+	const Vertex &						GetVertex(uint inIndex) const				{ return mVertices[inIndex]; }
+	Vertex &							GetVertex(uint inIndex)						{ return mVertices[inIndex]; }
+
+	/// Get the materials of the soft body
+	const PhysicsMaterialList &			GetMaterials() const						{ return mSettings->mMaterials; }
+
+	/// Get the faces of the soft body
+	const Array<Face> &					GetFaces() const							{ return mSettings->mFaces; }
+
+	/// Access to an individual face
+	const Face &						GetFace(uint inIndex) const					{ return mSettings->mFaces[inIndex]; }
+
+	/// Get the number of solver iterations
+	uint32								GetNumIterations() const					{ return mNumIterations; }
+	void								SetNumIterations(uint32 inNumIterations)	{ mNumIterations = inNumIterations; }
+
+	/// Get the pressure of the soft body
+	float								GetPressure() const							{ return mPressure; }
+	void								SetPressure(float inPressure)				{ mPressure = inPressure; }
+
+	/// Update the position of the body while simulating (set to false for something that is attached to the static world)
+	bool								GetUpdatePosition() const					{ return mUpdatePosition; }
+	void								SetUpdatePosition(bool inUpdatePosition)	{ mUpdatePosition = inUpdatePosition; }
+
+	/// Global setting to turn on/off skin constraints
+	bool								GetEnableSkinConstraints() const			{ return mEnableSkinConstraints; }
+	void								SetEnableSkinConstraints(bool inEnableSkinConstraints) { mEnableSkinConstraints = inEnableSkinConstraints; }
+
+	/// Multiplier applied to Skinned::mMaxDistance to allow tightening or loosening of the skin constraints. 0 to hard skin all vertices.
+	float								GetSkinnedMaxDistanceMultiplier() const		{ return mSkinnedMaxDistanceMultiplier; }
+	void								SetSkinnedMaxDistanceMultiplier(float inSkinnedMaxDistanceMultiplier) { mSkinnedMaxDistanceMultiplier = inSkinnedMaxDistanceMultiplier; }
+
+	/// Get local bounding box
+	const AABox &						GetLocalBounds() const						{ return mLocalBounds; }
+
+	/// Get the volume of the soft body. Note can become negative if the shape is inside out!
+	float								GetVolume() const							{ return GetVolumeTimesSix() / 6.0f; }
+
+	/// Calculate the total mass and inertia of this body based on the current state of the vertices
+	void								CalculateMassAndInertia();
+
+#ifdef JPH_DEBUG_RENDERER
+	/// Draw the state of a soft body
+	void								DrawVertices(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const;
+	void								DrawVertexVelocities(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const;
+	void								DrawEdgeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const;
+	void								DrawBendConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const;
+	void								DrawVolumeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const;
+	void								DrawSkinConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const;
+	void								DrawLRAConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const;
+	void								DrawPredictedBounds(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const;
+#endif // JPH_DEBUG_RENDERER
+
+	/// Saving state for replay
+	void								SaveState(StateRecorder &inStream) const;
+
+	/// Restoring state for replay
+	void								RestoreState(StateRecorder &inStream);
+
+	/// Skin vertices to supplied joints, information is used by the skinned constraints.
+	/// @param inCenterOfMassTransform Value of Body::GetCenterOfMassTransform().
+	/// @param inJointMatrices The joint matrices must be expressed relative to inCenterOfMassTransform.
+	/// @param inNumJoints Indicates how large the inJointMatrices array is (used only for validating out of bounds).
+	/// @param inHardSkinAll Can be used to position all vertices on the skinned vertices and can be used to hard reset the soft body.
+	/// @param ioTempAllocator Allocator.
+	void								SkinVertices(RMat44Arg inCenterOfMassTransform, const Mat44 *inJointMatrices, uint inNumJoints, bool inHardSkinAll, TempAllocator &ioTempAllocator);
+
+	/// This function allows you to update the soft body immediately without going through the PhysicsSystem.
+	/// This is useful if the soft body is teleported and needs to 'settle' or it can be used if a the soft body
+	/// is not added to the PhysicsSystem and needs to be updated manually. One reason for not adding it to the
+	/// PhyicsSystem is that you might want to update a soft body immediately after updating an animated object
+	/// that has the soft body attached to it. If the soft body is added to the PhysicsSystem it will be updated
+	/// by it, so calling this function will effectively update it twice. Note that when you use this function,
+	/// only the current thread will be used, whereas if you update through the PhysicsSystem, multiple threads may
+	/// be used.
+	/// Note that this will bypass any sleep checks. Since the dynamic objects that the soft body touches
+	/// will not move during this call, there can be simulation artifacts if you call this function multiple times
+	/// without running the physics simulation step.
+	void								CustomUpdate(float inDeltaTime, Body &ioSoftBody, PhysicsSystem &inSystem);
+
+	////////////////////////////////////////////////////////////
+	// FUNCTIONS BELOW THIS LINE ARE FOR INTERNAL USE ONLY
+	////////////////////////////////////////////////////////////
+
+	/// Initialize the update context. Not part of the public API.
+	void								InitializeUpdateContext(float inDeltaTime, Body &inSoftBody, const PhysicsSystem &inSystem, SoftBodyUpdateContext &ioContext);
+
+	/// Do a broad phase check and collect all bodies that can possibly collide with this soft body. Not part of the public API.
+	void								DetermineCollidingShapes(const SoftBodyUpdateContext &inContext, const PhysicsSystem &inSystem, const BodyLockInterface &inBodyLockInterface);
+
+	/// Return code for ParallelUpdate
+	enum class EStatus
+	{
+		NoWork	= 1 << 0,				///< No work was done because other threads were still working on a batch that cannot run concurrently
+		DidWork	= 1 << 1,				///< Work was done to progress the update
+		Done	= 1 << 2,				///< All work is done
+	};
+
+	/// Update the soft body, will process a batch of work. Not part of the public API.
+	EStatus								ParallelUpdate(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings);
+
+	/// Update the velocities of all rigid bodies that we collided with. Not part of the public API.
+	void								UpdateRigidBodyVelocities(const SoftBodyUpdateContext &inContext, BodyInterface &inBodyInterface);
+
+private:
+	// SoftBodyManifold needs to have access to CollidingShape
+	friend class SoftBodyManifold;
+
+	// Information about a leaf shape that we're colliding with
+	struct LeafShape
+	{
+										LeafShape() = default;
+										LeafShape(Mat44Arg inTransform, Vec3Arg inScale, const Shape *inShape) : mTransform(inTransform), mScale(inScale), mShape(inShape) { }
+
+		Mat44							mTransform;									///< Transform of the shape relative to the soft body
+		Vec3							mScale;										///< Scale of the shape
+		RefConst<Shape>					mShape;										///< Shape
+	};
+
+	// Collect information about the colliding bodies
+	struct CollidingShape
+	{
+		/// Get the velocity of a point on this body
+		Vec3							GetPointVelocity(Vec3Arg inPointRelativeToCOM) const
+		{
+			return mLinearVelocity + mAngularVelocity.Cross(inPointRelativeToCOM);
+		}
+
+		Mat44							mCenterOfMassTransform;						///< Transform of the body relative to the soft body
+		Array<LeafShape>				mShapes;									///< Leaf shapes of the body we hit
+		BodyID							mBodyID;									///< Body ID of the body we hit
+		EMotionType						mMotionType;								///< Motion type of the body we hit
+		float							mInvMass;									///< Inverse mass of the body we hit
+		float							mFriction;									///< Combined friction of the two bodies
+		float							mRestitution;								///< Combined restitution of the two bodies
+		float							mSoftBodyInvMassScale;						///< Scale factor for the inverse mass of the soft body vertices
+		bool							mUpdateVelocities;							///< If the linear/angular velocity changed and the body needs to be updated
+		Mat44							mInvInertia;								///< Inverse inertia in local space to the soft body
+		Vec3							mLinearVelocity;							///< Linear velocity of the body in local space to the soft body
+		Vec3							mAngularVelocity;							///< Angular velocity of the body in local space to the soft body
+		Vec3							mOriginalLinearVelocity;					///< Linear velocity of the body in local space to the soft body at start
+		Vec3							mOriginalAngularVelocity;					///< Angular velocity of the body in local space to the soft body at start
+	};
+
+	// Collect information about the colliding sensors
+	struct CollidingSensor
+	{
+		Mat44							mCenterOfMassTransform;						///< Transform of the body relative to the soft body
+		Array<LeafShape>				mShapes;									///< Leaf shapes of the body we hit
+		BodyID							mBodyID;									///< Body ID of the body we hit
+		bool							mHasContact;								///< If the sensor collided with the soft body
+	};
+
+	// Information about the state of all skinned vertices
+	struct SkinState
+	{
+		Vec3							mPreviousPosition = Vec3::sZero();			///< Previous position of the skinned vertex, used to interpolate between the previous and current position
+		Vec3							mPosition = Vec3::sNaN();					///< Current position of the skinned vertex
+		Vec3							mNormal = Vec3::sNaN();						///< Normal of the skinned vertex
+	};
+
+	/// Do a narrow phase check and determine the closest feature that we can collide with
+	void								DetermineCollisionPlanes(uint inVertexStart, uint inNumVertices);
+
+	/// Do a narrow phase check between a single sensor and the soft body
+	void								DetermineSensorCollisions(CollidingSensor &ioSensor);
+
+	/// Apply pressure force and update the vertex velocities
+	void								ApplyPressure(const SoftBodyUpdateContext &inContext);
+
+	/// Integrate the positions of all vertices by 1 sub step
+	void								IntegratePositions(const SoftBodyUpdateContext &inContext);
+
+	/// Enforce all bend constraints
+	void								ApplyDihedralBendConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex);
+
+	/// Enforce all volume constraints
+	void								ApplyVolumeConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex);
+
+	/// Enforce all skin constraints
+	void								ApplySkinConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex);
+
+	/// Enforce all edge constraints
+	void								ApplyEdgeConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex);
+
+	/// Enforce all LRA constraints
+	void								ApplyLRAConstraints(uint inStartIndex, uint inEndIndex);
+
+	/// Enforce all collision constraints & update all velocities according the XPBD algorithm
+	void								ApplyCollisionConstraintsAndUpdateVelocities(const SoftBodyUpdateContext &inContext);
+
+	/// Update the state of the soft body (position, velocity, bounds)
+	void								UpdateSoftBodyState(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings);
+
+	/// Start the first solver iteration
+	void								StartFirstIteration(SoftBodyUpdateContext &ioContext);
+
+	/// Executes tasks that need to run on the start of an iteration (i.e. the stuff that can't run in parallel)
+	void								StartNextIteration(const SoftBodyUpdateContext &ioContext);
+
+	/// Helper function for ParallelUpdate that works on batches of collision planes
+	EStatus								ParallelDetermineCollisionPlanes(SoftBodyUpdateContext &ioContext);
+
+	/// Helper function for ParallelUpdate that works on sensor collisions
+	EStatus								ParallelDetermineSensorCollisions(SoftBodyUpdateContext &ioContext);
+
+	/// Helper function for ParallelUpdate that works on batches of constraints
+	EStatus								ParallelApplyConstraints(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings);
+
+	/// Helper function to update a single group of constraints
+	void								ProcessGroup(const SoftBodyUpdateContext &ioContext, uint inGroupIndex);
+
+	/// Returns 6 times the volume of the soft body
+	float								GetVolumeTimesSix() const;
+
+#ifdef JPH_DEBUG_RENDERER
+	/// Helper function to draw constraints
+	template <typename GetEndIndex, typename DrawConstraint>
+		inline void						DrawConstraints(ESoftBodyConstraintColor inConstraintColor, const GetEndIndex &inGetEndIndex, const DrawConstraint &inDrawConstraint, ColorArg inBaseColor) const;
+
+	RMat44								mSkinStateTransform = RMat44::sIdentity();	///< The matrix that transforms mSkinState to world space
+#endif // JPH_DEBUG_RENDERER
+
+	RefConst<SoftBodySharedSettings>	mSettings;									///< Configuration of the particles and constraints
+	Array<Vertex>						mVertices;									///< Current state of all vertices in the simulation
+	Array<CollidingShape>				mCollidingShapes;							///< List of colliding shapes retrieved during the last update
+	Array<CollidingSensor>				mCollidingSensors;							///< List of colliding sensors retrieved during the last update
+	Array<SkinState>					mSkinState;									///< List of skinned positions (1-on-1 with mVertices but only those that are used by the skinning constraints are filled in)
+	AABox								mLocalBounds;								///< Bounding box of all vertices
+	AABox								mLocalPredictedBounds;						///< Predicted bounding box for all vertices using extrapolation of velocity by last step delta time
+	uint32								mNumIterations;								///< Number of solver iterations
+	float								mPressure;									///< n * R * T, amount of substance * ideal gas constant * absolute temperature, see https://en.wikipedia.org/wiki/Pressure
+	float								mSkinnedMaxDistanceMultiplier = 1.0f;		///< Multiplier applied to Skinned::mMaxDistance to allow tightening or loosening of the skin constraints
+	bool								mUpdatePosition;							///< Update the position of the body while simulating (set to false for something that is attached to the static world)
+	bool								mNeedContactCallback = false;						///< True if the soft body has collided with anything in the last update
+	bool								mEnableSkinConstraints = true;				///< If skin constraints are enabled
+	bool								mSkinStatePreviousPositionValid = false;	///< True if the skinning was updated in the last update so that the previous position of the skin state is valid
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyShape.cpp b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyShape.cpp
new file mode 100644
index 0000000000..6692b223eb
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyShape.cpp
@@ -0,0 +1,338 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/SoftBody/SoftBodyShape.h>
+#include <Jolt/Core/Profiler.h>
+#include <Jolt/Geometry/RayTriangle.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/SoftBody/SoftBodyMotionProperties.h>
+#include <Jolt/Physics/Collision/CastConvexVsTriangles.h>
+#include <Jolt/Physics/Collision/CastSphereVsTriangles.h>
+#include <Jolt/Physics/Collision/CollideConvexVsTriangles.h>
+#include <Jolt/Physics/Collision/CollideSphereVsTriangles.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+uint SoftBodyShape::GetSubShapeIDBits() const
+{
+	// Ensure we have enough bits to encode our shape [0, n - 1]
+	uint32 n = (uint32)mSoftBodyMotionProperties->GetFaces().size() - 1;
+	return 32 - CountLeadingZeros(n);
+}
+
+uint32 SoftBodyShape::GetFaceIndex(const SubShapeID &inSubShapeID) const
+{
+	SubShapeID remainder;
+	uint32 face_index = inSubShapeID.PopID(GetSubShapeIDBits(), remainder);
+	JPH_ASSERT(remainder.IsEmpty());
+	return face_index;
+}
+
+AABox SoftBodyShape::GetLocalBounds() const
+{
+	return mSoftBodyMotionProperties->GetLocalBounds();
+}
+
+bool SoftBodyShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	uint num_triangle_bits = GetSubShapeIDBits();
+	uint triangle_idx = uint(-1);
+
+	const Array<SoftBodyVertex> &vertices = mSoftBodyMotionProperties->GetVertices();
+	for (const SoftBodyMotionProperties::Face &f : mSoftBodyMotionProperties->GetFaces())
+	{
+		Vec3 x1 = vertices[f.mVertex[0]].mPosition;
+		Vec3 x2 = vertices[f.mVertex[1]].mPosition;
+		Vec3 x3 = vertices[f.mVertex[2]].mPosition;
+
+		float fraction = RayTriangle(inRay.mOrigin, inRay.mDirection, x1, x2, x3);
+		if (fraction < ioHit.mFraction)
+		{
+			// Store fraction
+			ioHit.mFraction = fraction;
+
+			// Store triangle index
+			triangle_idx = uint(&f - mSoftBodyMotionProperties->GetFaces().data());
+		}
+	}
+
+	if (triangle_idx == uint(-1))
+		return false;
+
+	ioHit.mSubShapeID2 = inSubShapeIDCreator.PushID(triangle_idx, num_triangle_bits).GetID();
+	return true;
+}
+
+void SoftBodyShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	uint num_triangle_bits = GetSubShapeIDBits();
+
+	const Array<SoftBodyVertex> &vertices = mSoftBodyMotionProperties->GetVertices();
+	for (const SoftBodyMotionProperties::Face &f : mSoftBodyMotionProperties->GetFaces())
+	{
+		Vec3 x1 = vertices[f.mVertex[0]].mPosition;
+		Vec3 x2 = vertices[f.mVertex[1]].mPosition;
+		Vec3 x3 = vertices[f.mVertex[2]].mPosition;
+
+		// Back facing check
+		if (inRayCastSettings.mBackFaceModeTriangles == EBackFaceMode::IgnoreBackFaces && (x2 - x1).Cross(x3 - x1).Dot(inRay.mDirection) > 0.0f)
+			continue;
+
+		// Test ray against triangle
+		float fraction = RayTriangle(inRay.mOrigin, inRay.mDirection, x1, x2, x3);
+		if (fraction < ioCollector.GetEarlyOutFraction())
+		{
+			// Better hit than the current hit
+			RayCastResult hit;
+			hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext());
+			hit.mFraction = fraction;
+			hit.mSubShapeID2 = inSubShapeIDCreator.PushID(uint(&f - mSoftBodyMotionProperties->GetFaces().data()), num_triangle_bits).GetID();
+			ioCollector.AddHit(hit);
+		}
+	}
+}
+
+void SoftBodyShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	sCollidePointUsingRayCast(*this, inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter);
+}
+
+void SoftBodyShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
+{
+	/* Not implemented */
+}
+
+const PhysicsMaterial *SoftBodyShape::GetMaterial(const SubShapeID &inSubShapeID) const
+{
+	SubShapeID remainder;
+	uint triangle_idx = inSubShapeID.PopID(GetSubShapeIDBits(), remainder);
+	JPH_ASSERT(remainder.IsEmpty());
+
+	const SoftBodyMotionProperties::Face &f = mSoftBodyMotionProperties->GetFace(triangle_idx);
+	return mSoftBodyMotionProperties->GetMaterials()[f.mMaterialIndex];
+}
+
+Vec3 SoftBodyShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	SubShapeID remainder;
+	uint triangle_idx = inSubShapeID.PopID(GetSubShapeIDBits(), remainder);
+	JPH_ASSERT(remainder.IsEmpty());
+
+	const SoftBodyMotionProperties::Face &f = mSoftBodyMotionProperties->GetFace(triangle_idx);
+	const Array<SoftBodyVertex> &vertices = mSoftBodyMotionProperties->GetVertices();
+
+	Vec3 x1 = vertices[f.mVertex[0]].mPosition;
+	Vec3 x2 = vertices[f.mVertex[1]].mPosition;
+	Vec3 x3 = vertices[f.mVertex[2]].mPosition;
+
+	return (x2 - x1).Cross(x3 - x1).NormalizedOr(Vec3::sAxisY());
+}
+
+void SoftBodyShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
+	SubShapeID remainder;
+	uint triangle_idx = inSubShapeID.PopID(GetSubShapeIDBits(), remainder);
+	JPH_ASSERT(remainder.IsEmpty());
+
+	const SoftBodyMotionProperties::Face &f = mSoftBodyMotionProperties->GetFace(triangle_idx);
+	const Array<SoftBodyVertex> &vertices = mSoftBodyMotionProperties->GetVertices();
+
+	for (uint32 i : f.mVertex)
+		outVertices.push_back(inCenterOfMassTransform * (inScale * vertices[i].mPosition));
+}
+
+void SoftBodyShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const
+{
+	outSubmergedVolume = 0.0f;
+	outTotalVolume = mSoftBodyMotionProperties->GetVolume();
+	outCenterOfBuoyancy = Vec3::sZero();
+}
+
+#ifdef JPH_DEBUG_RENDERER
+
+void SoftBodyShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	const Array<SoftBodyVertex> &vertices = mSoftBodyMotionProperties->GetVertices();
+	for (const SoftBodyMotionProperties::Face &f : mSoftBodyMotionProperties->GetFaces())
+	{
+		RVec3 x1 = inCenterOfMassTransform * vertices[f.mVertex[0]].mPosition;
+		RVec3 x2 = inCenterOfMassTransform * vertices[f.mVertex[1]].mPosition;
+		RVec3 x3 = inCenterOfMassTransform * vertices[f.mVertex[2]].mPosition;
+
+		inRenderer->DrawTriangle(x1, x2, x3, inColor, DebugRenderer::ECastShadow::On);
+	}
+}
+
+#endif // JPH_DEBUG_RENDERER
+
+struct SoftBodyShape::SBSGetTrianglesContext
+{
+	Mat44		mCenterOfMassTransform;
+	int			mTriangleIndex;
+};
+
+void SoftBodyShape::GetTrianglesStart(GetTrianglesContext &ioContext, [[maybe_unused]] const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const
+{
+	SBSGetTrianglesContext &context = reinterpret_cast<SBSGetTrianglesContext &>(ioContext);
+	context.mCenterOfMassTransform = Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale);
+	context.mTriangleIndex = 0;
+}
+
+int SoftBodyShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const
+{
+	SBSGetTrianglesContext &context = reinterpret_cast<SBSGetTrianglesContext &>(ioContext);
+
+	const Array<SoftBodyMotionProperties::Face> &faces = mSoftBodyMotionProperties->GetFaces();
+	const Array<SoftBodyVertex> &vertices = mSoftBodyMotionProperties->GetVertices();
+	const PhysicsMaterialList &materials = mSoftBodyMotionProperties->GetMaterials();
+
+	int num_triangles = min(inMaxTrianglesRequested, (int)faces.size() - context.mTriangleIndex);
+	for (int i = 0; i < num_triangles; ++i)
+	{
+		const SoftBodyMotionProperties::Face &f = faces[context.mTriangleIndex + i];
+
+		Vec3 x1 = context.mCenterOfMassTransform * vertices[f.mVertex[0]].mPosition;
+		Vec3 x2 = context.mCenterOfMassTransform * vertices[f.mVertex[1]].mPosition;
+		Vec3 x3 = context.mCenterOfMassTransform * vertices[f.mVertex[2]].mPosition;
+
+		x1.StoreFloat3(outTriangleVertices++);
+		x2.StoreFloat3(outTriangleVertices++);
+		x3.StoreFloat3(outTriangleVertices++);
+
+		if (outMaterials != nullptr)
+			*outMaterials++ = materials[f.mMaterialIndex];
+	}
+
+	context.mTriangleIndex += num_triangles;
+	return num_triangles;
+}
+
+Shape::Stats SoftBodyShape::GetStats() const
+{
+	return Stats(sizeof(*this), (uint)mSoftBodyMotionProperties->GetFaces().size());
+}
+
+float SoftBodyShape::GetVolume() const
+{
+	return mSoftBodyMotionProperties->GetVolume();
+}
+
+void SoftBodyShape::sCollideConvexVsSoftBody(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter)
+{
+	JPH_ASSERT(inShape1->GetType() == EShapeType::Convex);
+	const ConvexShape *shape1 = static_cast<const ConvexShape *>(inShape1);
+	JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::SoftBody);
+	const SoftBodyShape *shape2 = static_cast<const SoftBodyShape *>(inShape2);
+
+	const Array<SoftBodyVertex> &vertices = shape2->mSoftBodyMotionProperties->GetVertices();
+	const Array<SoftBodyMotionProperties::Face> &faces = shape2->mSoftBodyMotionProperties->GetFaces();
+	uint num_triangle_bits = shape2->GetSubShapeIDBits();
+
+	CollideConvexVsTriangles collider(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector);
+	for (const SoftBodyMotionProperties::Face &f : faces)
+	{
+		Vec3 x1 = vertices[f.mVertex[0]].mPosition;
+		Vec3 x2 = vertices[f.mVertex[1]].mPosition;
+		Vec3 x3 = vertices[f.mVertex[2]].mPosition;
+
+		collider.Collide(x1, x2, x3, 0b111, inSubShapeIDCreator2.PushID(uint(&f - faces.data()), num_triangle_bits).GetID());
+	}
+}
+
+void SoftBodyShape::sCollideSphereVsSoftBody(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter)
+{
+	JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::Sphere);
+	const SphereShape *shape1 = static_cast<const SphereShape *>(inShape1);
+	JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::SoftBody);
+	const SoftBodyShape *shape2 = static_cast<const SoftBodyShape *>(inShape2);
+
+	const Array<SoftBodyVertex> &vertices = shape2->mSoftBodyMotionProperties->GetVertices();
+	const Array<SoftBodyMotionProperties::Face> &faces = shape2->mSoftBodyMotionProperties->GetFaces();
+	uint num_triangle_bits = shape2->GetSubShapeIDBits();
+
+	CollideSphereVsTriangles collider(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector);
+	for (const SoftBodyMotionProperties::Face &f : faces)
+	{
+		Vec3 x1 = vertices[f.mVertex[0]].mPosition;
+		Vec3 x2 = vertices[f.mVertex[1]].mPosition;
+		Vec3 x3 = vertices[f.mVertex[2]].mPosition;
+
+		collider.Collide(x1, x2, x3, 0b111, inSubShapeIDCreator2.PushID(uint(&f - faces.data()), num_triangle_bits).GetID());
+	}
+}
+
+void SoftBodyShape::sCastConvexVsSoftBody(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::SoftBody);
+	const SoftBodyShape *shape = static_cast<const SoftBodyShape *>(inShape);
+
+	const Array<SoftBodyVertex> &vertices = shape->mSoftBodyMotionProperties->GetVertices();
+	const Array<SoftBodyMotionProperties::Face> &faces = shape->mSoftBodyMotionProperties->GetFaces();
+	uint num_triangle_bits = shape->GetSubShapeIDBits();
+
+	CastConvexVsTriangles caster(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector);
+	for (const SoftBodyMotionProperties::Face &f : faces)
+	{
+		Vec3 x1 = vertices[f.mVertex[0]].mPosition;
+		Vec3 x2 = vertices[f.mVertex[1]].mPosition;
+		Vec3 x3 = vertices[f.mVertex[2]].mPosition;
+
+		caster.Cast(x1, x2, x3, 0b111, inSubShapeIDCreator2.PushID(uint(&f - faces.data()), num_triangle_bits).GetID());
+	}
+}
+
+void SoftBodyShape::sCastSphereVsSoftBody(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::SoftBody);
+	const SoftBodyShape *shape = static_cast<const SoftBodyShape *>(inShape);
+
+	const Array<SoftBodyVertex> &vertices = shape->mSoftBodyMotionProperties->GetVertices();
+	const Array<SoftBodyMotionProperties::Face> &faces = shape->mSoftBodyMotionProperties->GetFaces();
+	uint num_triangle_bits = shape->GetSubShapeIDBits();
+
+	CastSphereVsTriangles caster(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector);
+	for (const SoftBodyMotionProperties::Face &f : faces)
+	{
+		Vec3 x1 = vertices[f.mVertex[0]].mPosition;
+		Vec3 x2 = vertices[f.mVertex[1]].mPosition;
+		Vec3 x3 = vertices[f.mVertex[2]].mPosition;
+
+		caster.Cast(x1, x2, x3, 0b111, inSubShapeIDCreator2.PushID(uint(&f - faces.data()), num_triangle_bits).GetID());
+	}
+}
+
+void SoftBodyShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::SoftBody);
+	f.mConstruct = nullptr; // Not supposed to be constructed by users!
+	f.mColor = Color::sDarkGreen;
+
+	for (EShapeSubType s : sConvexSubShapeTypes)
+	{
+		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::SoftBody, sCollideConvexVsSoftBody);
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::SoftBody, sCastConvexVsSoftBody);
+	}
+
+	// Specialized collision functions
+	CollisionDispatch::sRegisterCollideShape(EShapeSubType::Sphere, EShapeSubType::SoftBody, sCollideSphereVsSoftBody);
+	CollisionDispatch::sRegisterCastShape(EShapeSubType::Sphere, EShapeSubType::SoftBody, sCastSphereVsSoftBody);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyShape.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyShape.h
new file mode 100644
index 0000000000..70605dae40
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyShape.h
@@ -0,0 +1,73 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+
+JPH_NAMESPACE_BEGIN
+
+class SoftBodyMotionProperties;
+class CollideShapeSettings;
+
+/// Shape used exclusively for soft bodies. Adds the ability to perform collision checks against soft bodies.
+class JPH_EXPORT SoftBodyShape final : public Shape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+									SoftBodyShape()											: Shape(EShapeType::SoftBody, EShapeSubType::SoftBody) { }
+
+	/// Determine amount of bits needed to encode sub shape id
+	uint							GetSubShapeIDBits() const;
+
+	/// Convert a sub shape ID back to a face index
+	uint32							GetFaceIndex(const SubShapeID &inSubShapeID) const;
+
+	// See Shape
+	virtual bool					MustBeStatic() const override							{ return false; }
+	virtual Vec3					GetCenterOfMass() const override						{ return Vec3::sZero(); }
+	virtual AABox					GetLocalBounds() const override;
+	virtual uint					GetSubShapeIDBitsRecursive() const override				{ return GetSubShapeIDBits(); }
+	virtual float					GetInnerRadius() const override							{ return 0.0f; }
+	virtual MassProperties			GetMassProperties() const override						{ return MassProperties(); }
+	virtual const PhysicsMaterial *	GetMaterial(const SubShapeID &inSubShapeID) const override;
+	virtual Vec3					GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override;
+	virtual void					GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
+	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy
+#ifdef JPH_DEBUG_RENDERER // Not using JPH_IF_DEBUG_RENDERER for Doxygen
+		, RVec3Arg inBaseOffset
+#endif
+		) const override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void					Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual bool					CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+	virtual void					CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const override;
+	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
+	virtual int						GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override;
+	virtual Stats					GetStats() const override;
+	virtual float					GetVolume() const override;
+
+	// Register shape functions with the registry
+	static void						sRegister();
+
+private:
+	// Helper functions called by CollisionDispatch
+	static void						sCollideConvexVsSoftBody(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCollideSphereVsSoftBody(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCastConvexVsSoftBody(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+	static void						sCastSphereVsSoftBody(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+
+	struct SBSGetTrianglesContext;
+
+	friend class BodyManager;
+
+	const SoftBodyMotionProperties *mSoftBodyMotionProperties;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp
new file mode 100644
index 0000000000..7eb8dd555c
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp
@@ -0,0 +1,1032 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/SoftBody/SoftBodySharedSettings.h>
+#include <Jolt/Physics/SoftBody/SoftBodyUpdateContext.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/Core/QuickSort.h>
+#include <Jolt/Core/UnorderedMap.h>
+#include <Jolt/Core/UnorderedSet.h>
+#include <Jolt/Core/BinaryHeap.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Vertex)
+{
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Vertex, mPosition)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Vertex, mVelocity)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Vertex, mInvMass)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Face)
+{
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Face, mVertex)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Face, mMaterialIndex)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Edge)
+{
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Edge, mVertex)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Edge, mRestLength)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Edge, mCompliance)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::DihedralBend)
+{
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::DihedralBend, mVertex)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::DihedralBend, mCompliance)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::DihedralBend, mInitialAngle)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Volume)
+{
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Volume, mVertex)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Volume, mSixRestVolume)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Volume, mCompliance)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::InvBind)
+{
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::InvBind, mJointIndex)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::InvBind, mInvBind)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::SkinWeight)
+{
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::SkinWeight, mInvBindIndex)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::SkinWeight, mWeight)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Skinned)
+{
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Skinned, mVertex)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Skinned, mWeights)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Skinned, mMaxDistance)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Skinned, mBackStopDistance)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Skinned, mBackStopRadius)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::LRA)
+{
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::LRA, mVertex)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::LRA, mMaxDistance)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings)
+{
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mVertices)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mFaces)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mEdgeConstraints)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mDihedralBendConstraints)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mVolumeConstraints)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mSkinnedConstraints)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mInvBindMatrices)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mLRAConstraints)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mMaterials)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mVertexRadius)
+}
+
+void SoftBodySharedSettings::CalculateClosestKinematic()
+{
+	// Check if we already calculated this
+	if (!mClosestKinematic.empty())
+		return;
+
+	// Reserve output size
+	mClosestKinematic.resize(mVertices.size());
+
+	// Create a list of connected vertices
+	Array<Array<uint32>> connectivity;
+	connectivity.resize(mVertices.size());
+	for (const Edge &e : mEdgeConstraints)
+	{
+		connectivity[e.mVertex[0]].push_back(e.mVertex[1]);
+		connectivity[e.mVertex[1]].push_back(e.mVertex[0]);
+	}
+
+	// Use Dijkstra's algorithm to find the closest kinematic vertex for each vertex
+	// See: https://en.wikipedia.org/wiki/Dijkstra's_algorithm
+	//
+	// An element in the open list
+	struct Open
+	{
+		// Order so that we get the shortest distance first
+		bool	operator < (const Open &inRHS) const
+		{
+			return mDistance > inRHS.mDistance;
+		}
+
+		uint32	mVertex;
+		float	mDistance;
+	};
+
+	// Start with all kinematic elements
+	Array<Open> to_visit;
+	for (uint32 v = 0; v < mVertices.size(); ++v)
+		if (mVertices[v].mInvMass == 0.0f)
+		{
+			mClosestKinematic[v].mVertex = v;
+			mClosestKinematic[v].mDistance = 0.0f;
+			to_visit.push_back({ v, 0.0f });
+			BinaryHeapPush(to_visit.begin(), to_visit.end(), std::less<Open> { });
+		}
+
+	// Visit all vertices remembering the closest kinematic vertex and its distance
+	JPH_IF_ENABLE_ASSERTS(float last_closest = 0.0f;)
+	while (!to_visit.empty())
+	{
+		// Pop element from the open list
+		BinaryHeapPop(to_visit.begin(), to_visit.end(), std::less<Open> { });
+		Open current = to_visit.back();
+		to_visit.pop_back();
+		JPH_ASSERT(current.mDistance >= last_closest);
+		JPH_IF_ENABLE_ASSERTS(last_closest = current.mDistance;)
+
+		// Loop through all of its connected vertices
+		for (uint32 v : connectivity[current.mVertex])
+		{
+			// Calculate distance from the current vertex to this target vertex and check if it is smaller
+			float new_distance = current.mDistance + (Vec3(mVertices[v].mPosition) - Vec3(mVertices[current.mVertex].mPosition)).Length();
+			if (new_distance < mClosestKinematic[v].mDistance)
+			{
+				// Remember new closest vertex
+				mClosestKinematic[v].mVertex = mClosestKinematic[current.mVertex].mVertex;
+				mClosestKinematic[v].mDistance = new_distance;
+				to_visit.push_back({ v, new_distance });
+				BinaryHeapPush(to_visit.begin(), to_visit.end(), std::less<Open> { });
+			}
+		}
+	}
+}
+
+void SoftBodySharedSettings::CreateConstraints(const VertexAttributes *inVertexAttributes, uint inVertexAttributesLength, EBendType inBendType, float inAngleTolerance)
+{
+	struct EdgeHelper
+	{
+		uint32	mVertex[2];
+		uint32	mEdgeIdx;
+	};
+
+	// Create list of all edges
+	Array<EdgeHelper> edges;
+	edges.reserve(mFaces.size() * 3);
+	for (const Face &f : mFaces)
+		for (int i = 0; i < 3; ++i)
+		{
+			uint32 v0 = f.mVertex[i];
+			uint32 v1 = f.mVertex[(i + 1) % 3];
+
+			EdgeHelper e;
+			e.mVertex[0] = min(v0, v1);
+			e.mVertex[1] = max(v0, v1);
+			e.mEdgeIdx = uint32(&f - mFaces.data()) * 3 + i;
+			edges.push_back(e);
+		}
+
+	// Sort the edges
+	QuickSort(edges.begin(), edges.end(), [](const EdgeHelper &inLHS, const EdgeHelper &inRHS) { return inLHS.mVertex[0] < inRHS.mVertex[0] || (inLHS.mVertex[0] == inRHS.mVertex[0] && inLHS.mVertex[1] < inRHS.mVertex[1]); });
+
+	// Only add edges if one of the vertices is movable
+	auto add_edge = [this](uint32 inVtx1, uint32 inVtx2, float inCompliance1, float inCompliance2) {
+		if ((mVertices[inVtx1].mInvMass > 0.0f || mVertices[inVtx2].mInvMass > 0.0f)
+			&& inCompliance1 < FLT_MAX && inCompliance2 < FLT_MAX)
+		{
+			Edge temp_edge;
+			temp_edge.mVertex[0] = inVtx1;
+			temp_edge.mVertex[1] = inVtx2;
+			temp_edge.mCompliance = 0.5f * (inCompliance1 + inCompliance2);
+			temp_edge.mRestLength = (Vec3(mVertices[inVtx2].mPosition) - Vec3(mVertices[inVtx1].mPosition)).Length();
+			JPH_ASSERT(temp_edge.mRestLength > 0.0f);
+			mEdgeConstraints.push_back(temp_edge);
+		}
+	};
+
+	// Helper function to get the attributes of a vertex
+	auto attr = [inVertexAttributes, inVertexAttributesLength](uint32 inVertex) {
+		return inVertexAttributes[min(inVertex, inVertexAttributesLength - 1)];
+	};
+
+	// Create the constraints
+	float sq_sin_tolerance = Square(Sin(inAngleTolerance));
+	float sq_cos_tolerance = Square(Cos(inAngleTolerance));
+	mEdgeConstraints.clear();
+	mEdgeConstraints.reserve(edges.size());
+	for (Array<EdgeHelper>::size_type i = 0; i < edges.size(); ++i)
+	{
+		const EdgeHelper &e0 = edges[i];
+
+		// Get attributes for the vertices of the edge
+		const VertexAttributes &a0 = attr(e0.mVertex[0]);
+		const VertexAttributes &a1 = attr(e0.mVertex[1]);
+
+		// Flag that indicates if this edge is a shear edge (if 2 triangles form a quad-like shape and this edge is on the diagonal)
+		bool is_shear = false;
+
+		// Test if there are any shared edges
+		for (Array<EdgeHelper>::size_type j = i + 1; j < edges.size(); ++j)
+		{
+			const EdgeHelper &e1 = edges[j];
+			if (e0.mVertex[0] == e1.mVertex[0] && e0.mVertex[1] == e1.mVertex[1])
+			{
+				// Get opposing vertices
+				const Face &f0 = mFaces[e0.mEdgeIdx / 3];
+				const Face &f1 = mFaces[e1.mEdgeIdx / 3];
+				uint32 vopposite0 = f0.mVertex[(e0.mEdgeIdx + 2) % 3];
+				uint32 vopposite1 = f1.mVertex[(e1.mEdgeIdx + 2) % 3];
+				const VertexAttributes &a_opposite0 = attr(vopposite0);
+				const VertexAttributes &a_opposite1 = attr(vopposite1);
+
+				// Faces should be roughly in a plane
+				Vec3 n0 = (Vec3(mVertices[f0.mVertex[2]].mPosition) - Vec3(mVertices[f0.mVertex[0]].mPosition)).Cross(Vec3(mVertices[f0.mVertex[1]].mPosition) - Vec3(mVertices[f0.mVertex[0]].mPosition));
+				Vec3 n1 = (Vec3(mVertices[f1.mVertex[2]].mPosition) - Vec3(mVertices[f1.mVertex[0]].mPosition)).Cross(Vec3(mVertices[f1.mVertex[1]].mPosition) - Vec3(mVertices[f1.mVertex[0]].mPosition));
+				if (Square(n0.Dot(n1)) > sq_cos_tolerance * n0.LengthSq() * n1.LengthSq())
+				{
+					// Faces should approximately form a quad
+					Vec3 e0_dir = Vec3(mVertices[vopposite0].mPosition) - Vec3(mVertices[e0.mVertex[0]].mPosition);
+					Vec3 e1_dir = Vec3(mVertices[vopposite1].mPosition) - Vec3(mVertices[e0.mVertex[0]].mPosition);
+					if (Square(e0_dir.Dot(e1_dir)) < sq_sin_tolerance * e0_dir.LengthSq() * e1_dir.LengthSq())
+					{
+						// Shear constraint
+						add_edge(vopposite0, vopposite1, a_opposite0.mShearCompliance, a_opposite1.mShearCompliance);
+						is_shear = true;
+					}
+				}
+
+				// Bend constraint
+				switch (inBendType)
+				{
+				case EBendType::None:
+					// Do nothing
+					break;
+
+				case EBendType::Distance:
+					// Create an edge constraint to represent the bend constraint
+					// Use the bend compliance of the shared edge
+					if (!is_shear)
+						add_edge(vopposite0, vopposite1, a0.mBendCompliance, a1.mBendCompliance);
+					break;
+
+				case EBendType::Dihedral:
+					// Test if both opposite vertices are free to move
+					if ((mVertices[vopposite0].mInvMass > 0.0f || mVertices[vopposite1].mInvMass > 0.0f)
+						&& a0.mBendCompliance < FLT_MAX && a1.mBendCompliance < FLT_MAX)
+					{
+						// Create a bend constraint
+						// Use the bend compliance of the shared edge
+						mDihedralBendConstraints.emplace_back(e0.mVertex[0], e0.mVertex[1], vopposite0, vopposite1, 0.5f * (a0.mBendCompliance + a1.mBendCompliance));
+					}
+					break;
+				}
+			}
+			else
+			{
+				// Start iterating from the first non-shared edge
+				i = j - 1;
+				break;
+			}
+		}
+
+		// Create a edge constraint for the current edge
+		add_edge(e0.mVertex[0], e0.mVertex[1], is_shear? a0.mShearCompliance : a0.mCompliance, is_shear? a1.mShearCompliance : a1.mCompliance);
+	}
+	mEdgeConstraints.shrink_to_fit();
+
+	// Calculate the initial angle for all bend constraints
+	CalculateBendConstraintConstants();
+
+	// Check if any vertices have LRA constraints
+	bool has_lra_constraints = false;
+	for (const VertexAttributes *va = inVertexAttributes; va < inVertexAttributes + inVertexAttributesLength; ++va)
+		if (va->mLRAType != ELRAType::None)
+		{
+			has_lra_constraints = true;
+			break;
+		}
+	if (has_lra_constraints)
+	{
+		// Ensure we have calculated the closest kinematic vertex for each vertex
+		CalculateClosestKinematic();
+
+		// Find non-kinematic vertices
+		for (uint32 v = 0; v < (uint32)mVertices.size(); ++v)
+			if (mVertices[v].mInvMass > 0.0f)
+			{
+				// Check if a closest vertex was found
+				uint32 closest = mClosestKinematic[v].mVertex;
+				if (closest != 0xffffffff)
+				{
+					// Check which LRA constraint to create
+					const VertexAttributes &va = attr(v);
+					switch (va.mLRAType)
+					{
+					case ELRAType::None:
+						break;
+
+					case ELRAType::EuclideanDistance:
+						mLRAConstraints.emplace_back(closest, v, va.mLRAMaxDistanceMultiplier * (Vec3(mVertices[closest].mPosition) - Vec3(mVertices[v].mPosition)).Length());
+						break;
+
+					case ELRAType::GeodesicDistance:
+						mLRAConstraints.emplace_back(closest, v, va.mLRAMaxDistanceMultiplier * mClosestKinematic[v].mDistance);
+						break;
+					}
+				}
+			}
+	}
+}
+
+void SoftBodySharedSettings::CalculateEdgeLengths()
+{
+	for (Edge &e : mEdgeConstraints)
+	{
+		e.mRestLength = (Vec3(mVertices[e.mVertex[1]].mPosition) - Vec3(mVertices[e.mVertex[0]].mPosition)).Length();
+		JPH_ASSERT(e.mRestLength > 0.0f);
+	}
+}
+
+void SoftBodySharedSettings::CalculateLRALengths(float inMaxDistanceMultiplier)
+{
+	for (LRA &l : mLRAConstraints)
+	{
+		l.mMaxDistance = inMaxDistanceMultiplier * (Vec3(mVertices[l.mVertex[1]].mPosition) - Vec3(mVertices[l.mVertex[0]].mPosition)).Length();
+		JPH_ASSERT(l.mMaxDistance > 0.0f);
+	}
+}
+
+void SoftBodySharedSettings::CalculateBendConstraintConstants()
+{
+	for (DihedralBend &b : mDihedralBendConstraints)
+	{
+		// Get positions
+		Vec3 x0 = Vec3(mVertices[b.mVertex[0]].mPosition);
+		Vec3 x1 = Vec3(mVertices[b.mVertex[1]].mPosition);
+		Vec3 x2 = Vec3(mVertices[b.mVertex[2]].mPosition);
+		Vec3 x3 = Vec3(mVertices[b.mVertex[3]].mPosition);
+
+		/*
+		   x2
+		e1/  \e3
+		 /    \
+		x0----x1
+		 \ e0 /
+		e2\  /e4
+		   x3
+		*/
+
+		// Calculate edges
+		Vec3 e0 = x1 - x0;
+		Vec3 e1 = x2 - x0;
+		Vec3 e2 = x3 - x0;
+
+		// Normals of both triangles
+		Vec3 n1 = e0.Cross(e1);
+		Vec3 n2 = e2.Cross(e0);
+		float denom = sqrt(n1.LengthSq() * n2.LengthSq());
+		if (denom < 1.0e-12f)
+			b.mInitialAngle = 0.0f;
+		else
+		{
+			float sign = Sign(n2.Cross(n1).Dot(e0));
+			b.mInitialAngle = sign * ACosApproximate(n1.Dot(n2) / denom); // Runtime uses the approximation too
+		}
+	}
+}
+
+void SoftBodySharedSettings::CalculateVolumeConstraintVolumes()
+{
+	for (Volume &v : mVolumeConstraints)
+	{
+		Vec3 x1(mVertices[v.mVertex[0]].mPosition);
+		Vec3 x2(mVertices[v.mVertex[1]].mPosition);
+		Vec3 x3(mVertices[v.mVertex[2]].mPosition);
+		Vec3 x4(mVertices[v.mVertex[3]].mPosition);
+
+		Vec3 x1x2 = x2 - x1;
+		Vec3 x1x3 = x3 - x1;
+		Vec3 x1x4 = x4 - x1;
+
+		v.mSixRestVolume = abs(x1x2.Cross(x1x3).Dot(x1x4));
+	}
+}
+
+void SoftBodySharedSettings::CalculateSkinnedConstraintNormals()
+{
+	// Clear any previous results
+	mSkinnedConstraintNormals.clear();
+
+	// If there are no skinned constraints, we're done
+	if (mSkinnedConstraints.empty())
+		return;
+
+	// First collect all vertices that are skinned
+	using VertexIndexSet = UnorderedSet<uint32>;
+	VertexIndexSet skinned_vertices;
+	skinned_vertices.reserve(VertexIndexSet::size_type(mSkinnedConstraints.size()));
+	for (const Skinned &s : mSkinnedConstraints)
+		skinned_vertices.insert(s.mVertex);
+
+	// Now collect all faces that connect only to skinned vertices
+	using ConnectedFacesMap = UnorderedMap<uint32, VertexIndexSet>;
+	ConnectedFacesMap connected_faces;
+	connected_faces.reserve(ConnectedFacesMap::size_type(mVertices.size()));
+	for (const Face &f : mFaces)
+	{
+		// Must connect to only skinned vertices
+		bool valid = true;
+		for (uint32 v : f.mVertex)
+			valid &= skinned_vertices.find(v) != skinned_vertices.end();
+		if (!valid)
+			continue;
+
+		// Store faces that connect to vertices
+		for (uint32 v : f.mVertex)
+			connected_faces[v].insert(uint32(&f - mFaces.data()));
+	}
+
+	// Populate the list of connecting faces per skinned vertex
+	mSkinnedConstraintNormals.reserve(mFaces.size());
+	for (Skinned &s : mSkinnedConstraints)
+	{
+		uint32 start = uint32(mSkinnedConstraintNormals.size());
+		JPH_ASSERT((start >> 24) == 0);
+		ConnectedFacesMap::const_iterator connected_faces_it = connected_faces.find(s.mVertex);
+		if (connected_faces_it != connected_faces.cend())
+		{
+			const VertexIndexSet &faces = connected_faces_it->second;
+			uint32 num = uint32(faces.size());
+			JPH_ASSERT(num < 256);
+			mSkinnedConstraintNormals.insert(mSkinnedConstraintNormals.end(), faces.begin(), faces.end());
+			QuickSort(mSkinnedConstraintNormals.begin() + start, mSkinnedConstraintNormals.begin() + start + num);
+			s.mNormalInfo = start + (num << 24);
+		}
+		else
+			s.mNormalInfo = 0;
+	}
+	mSkinnedConstraintNormals.shrink_to_fit();
+}
+
+void SoftBodySharedSettings::Optimize(OptimizationResults &outResults)
+{
+	// Clear any previous results
+	mUpdateGroups.clear();
+
+	// Create a list of connected vertices
+	struct Connection
+	{
+		uint32	mVertex;
+		uint32	mCount;
+	};
+	Array<Array<Connection>> connectivity;
+	connectivity.resize(mVertices.size());
+	auto add_connection = [&connectivity](uint inV1, uint inV2) {
+			for (int i = 0; i < 2; ++i)
+			{
+				bool found = false;
+				for (Connection &c : connectivity[inV1])
+					if (c.mVertex == inV2)
+					{
+						c.mCount++;
+						found = true;
+						break;
+					}
+				if (!found)
+					connectivity[inV1].push_back({ inV2, 1 });
+
+				std::swap(inV1, inV2);
+			}
+		};
+	for (const Edge &c : mEdgeConstraints)
+		add_connection(c.mVertex[0], c.mVertex[1]);
+	for (const LRA &c : mLRAConstraints)
+		add_connection(c.mVertex[0], c.mVertex[1]);
+	for (const DihedralBend &c : mDihedralBendConstraints)
+	{
+		add_connection(c.mVertex[0], c.mVertex[1]);
+		add_connection(c.mVertex[0], c.mVertex[2]);
+		add_connection(c.mVertex[0], c.mVertex[3]);
+		add_connection(c.mVertex[1], c.mVertex[2]);
+		add_connection(c.mVertex[1], c.mVertex[3]);
+		add_connection(c.mVertex[2], c.mVertex[3]);
+	}
+	for (const Volume &c : mVolumeConstraints)
+	{
+		add_connection(c.mVertex[0], c.mVertex[1]);
+		add_connection(c.mVertex[0], c.mVertex[2]);
+		add_connection(c.mVertex[0], c.mVertex[3]);
+		add_connection(c.mVertex[1], c.mVertex[2]);
+		add_connection(c.mVertex[1], c.mVertex[3]);
+		add_connection(c.mVertex[2], c.mVertex[3]);
+	}
+	// Skinned constraints only update 1 vertex, so we don't need special logic here
+
+	// Maps each of the vertices to a group index
+	Array<int> group_idx;
+	group_idx.resize(mVertices.size(), -1);
+
+	// Which group we are currently filling and its vertices
+	int current_group_idx = 0;
+	Array<uint> current_group;
+
+	// Start greedy algorithm to group vertices
+	for (;;)
+	{
+		// Find the bounding box of the ungrouped vertices
+		AABox bounds;
+		for (uint i = 0; i < (uint)mVertices.size(); ++i)
+			if (group_idx[i] == -1)
+				bounds.Encapsulate(Vec3(mVertices[i].mPosition));
+
+		// Determine longest and shortest axis
+		Vec3 bounds_size = bounds.GetSize();
+		uint max_axis = bounds_size.GetHighestComponentIndex();
+		uint min_axis = bounds_size.GetLowestComponentIndex();
+		if (min_axis == max_axis)
+			min_axis = (min_axis + 1) % 3;
+		uint mid_axis = 3 - min_axis - max_axis;
+
+		// Find the vertex that has the lowest value on the axis with the largest extent
+		uint current_vertex = UINT_MAX;
+		Float3 current_vertex_position { FLT_MAX, FLT_MAX, FLT_MAX };
+		for (uint i = 0; i < (uint)mVertices.size(); ++i)
+			if (group_idx[i] == -1)
+			{
+				const Float3 &vertex_position = mVertices[i].mPosition;
+				float max_axis_value = vertex_position[max_axis];
+				float mid_axis_value = vertex_position[mid_axis];
+				float min_axis_value = vertex_position[min_axis];
+
+				if (max_axis_value < current_vertex_position[max_axis]
+					|| (max_axis_value == current_vertex_position[max_axis]
+						&& (mid_axis_value < current_vertex_position[mid_axis]
+							|| (mid_axis_value == current_vertex_position[mid_axis]
+								&& min_axis_value < current_vertex_position[min_axis]))))
+				{
+					current_vertex_position = mVertices[i].mPosition;
+					current_vertex = i;
+				}
+			}
+		if (current_vertex == UINT_MAX)
+			break;
+
+		// Initialize the current group with 1 vertex
+		current_group.push_back(current_vertex);
+		group_idx[current_vertex] = current_group_idx;
+
+		// Fill up the group
+		for (;;)
+		{
+			// Find the vertex that is most connected to the current group
+			uint best_vertex = UINT_MAX;
+			uint best_num_connections = 0;
+			float best_dist_sq = FLT_MAX;
+			for (uint i = 0; i < (uint)current_group.size(); ++i) // For all vertices in the current group
+				for (const Connection &c : connectivity[current_group[i]]) // For all connections to other vertices
+				{
+					uint v = c.mVertex;
+					if (group_idx[v] == -1) // Ungrouped vertices only
+					{
+						// Count the number of connections to this group
+						uint num_connections = 0;
+						for (const Connection &v2 : connectivity[v])
+							if (group_idx[v2.mVertex] == current_group_idx)
+								num_connections += v2.mCount;
+
+						// Calculate distance to group centroid
+						float dist_sq = (Vec3(mVertices[v].mPosition) - Vec3(mVertices[current_group.front()].mPosition)).LengthSq();
+
+						if (best_vertex == UINT_MAX
+							|| num_connections > best_num_connections
+							|| (num_connections == best_num_connections && dist_sq < best_dist_sq))
+						{
+							best_vertex = v;
+							best_num_connections = num_connections;
+							best_dist_sq = dist_sq;
+						}
+					}
+				}
+
+			// Add the best vertex to the current group
+			if (best_vertex != UINT_MAX)
+			{
+				current_group.push_back(best_vertex);
+				group_idx[best_vertex] = current_group_idx;
+			}
+
+			// Create a new group?
+			if (current_group.size() >= SoftBodyUpdateContext::cVertexConstraintBatch // If full, yes
+				|| (current_group.size() > SoftBodyUpdateContext::cVertexConstraintBatch / 2 && best_vertex == UINT_MAX)) // If half full and we found no connected vertex, yes
+			{
+				current_group.clear();
+				current_group_idx++;
+				break;
+			}
+
+			// If we didn't find a connected vertex, we need to find a new starting vertex
+			if (best_vertex == UINT_MAX)
+				break;
+		}
+	}
+
+	// If the last group is more than half full, we'll keep it as a separate group, otherwise we merge it with the 'non parallel' group
+	if (current_group.size() > SoftBodyUpdateContext::cVertexConstraintBatch / 2)
+		++current_group_idx;
+
+	// We no longer need the current group array, free the memory
+	current_group.clear();
+	current_group.shrink_to_fit();
+
+	// We're done with the connectivity list, free the memory
+	connectivity.clear();
+	connectivity.shrink_to_fit();
+
+	// Assign the constraints to their groups
+	struct Group
+	{
+		uint			GetSize() const
+		{
+			return (uint)mEdgeConstraints.size() + (uint)mLRAConstraints.size() + (uint)mDihedralBendConstraints.size() + (uint)mVolumeConstraints.size() + (uint)mSkinnedConstraints.size();
+		}
+
+		Array<uint>		mEdgeConstraints;
+		Array<uint>		mLRAConstraints;
+		Array<uint>		mDihedralBendConstraints;
+		Array<uint>		mVolumeConstraints;
+		Array<uint>		mSkinnedConstraints;
+	};
+	Array<Group> groups;
+	groups.resize(current_group_idx + 1); // + non parallel group
+	for (const Edge &e : mEdgeConstraints)
+	{
+		int g1 = group_idx[e.mVertex[0]];
+		int g2 = group_idx[e.mVertex[1]];
+		JPH_ASSERT(g1 >= 0 && g2 >= 0);
+		if (g1 == g2) // In the same group
+			groups[g1].mEdgeConstraints.push_back(uint(&e - mEdgeConstraints.data()));
+		else // In different groups -> parallel group
+			groups.back().mEdgeConstraints.push_back(uint(&e - mEdgeConstraints.data()));
+	}
+	for (const LRA &l : mLRAConstraints)
+	{
+		int g1 = group_idx[l.mVertex[0]];
+		int g2 = group_idx[l.mVertex[1]];
+		JPH_ASSERT(g1 >= 0 && g2 >= 0);
+		if (g1 == g2) // In the same group
+			groups[g1].mLRAConstraints.push_back(uint(&l - mLRAConstraints.data()));
+		else // In different groups -> parallel group
+			groups.back().mLRAConstraints.push_back(uint(&l - mLRAConstraints.data()));
+	}
+	for (const DihedralBend &d : mDihedralBendConstraints)
+	{
+		int g1 = group_idx[d.mVertex[0]];
+		int g2 = group_idx[d.mVertex[1]];
+		int g3 = group_idx[d.mVertex[2]];
+		int g4 = group_idx[d.mVertex[3]];
+		JPH_ASSERT(g1 >= 0 && g2 >= 0 && g3 >= 0 && g4 >= 0);
+		if (g1 == g2 && g1 == g3 && g1 == g4) // In the same group
+			groups[g1].mDihedralBendConstraints.push_back(uint(&d - mDihedralBendConstraints.data()));
+		else // In different groups -> parallel group
+			groups.back().mDihedralBendConstraints.push_back(uint(&d - mDihedralBendConstraints.data()));
+	}
+	for (const Volume &v : mVolumeConstraints)
+	{
+		int g1 = group_idx[v.mVertex[0]];
+		int g2 = group_idx[v.mVertex[1]];
+		int g3 = group_idx[v.mVertex[2]];
+		int g4 = group_idx[v.mVertex[3]];
+		JPH_ASSERT(g1 >= 0 && g2 >= 0 && g3 >= 0 && g4 >= 0);
+		if (g1 == g2 && g1 == g3 && g1 == g4) // In the same group
+			groups[g1].mVolumeConstraints.push_back(uint(&v - mVolumeConstraints.data()));
+		else // In different groups -> parallel group
+			groups.back().mVolumeConstraints.push_back(uint(&v - mVolumeConstraints.data()));
+	}
+	for (const Skinned &s : mSkinnedConstraints)
+	{
+		int g1 = group_idx[s.mVertex];
+		JPH_ASSERT(g1 >= 0);
+		groups[g1].mSkinnedConstraints.push_back(uint(&s - mSkinnedConstraints.data()));
+	}
+
+	// Sort the parallel groups from big to small (this means the big groups will be scheduled first and have more time to complete)
+	QuickSort(groups.begin(), groups.end() - 1, [](const Group &inLHS, const Group &inRHS) { return inLHS.GetSize() > inRHS.GetSize(); });
+
+	// Make sure we know the closest kinematic vertex so we can sort
+	CalculateClosestKinematic();
+
+	// Sort within each group
+	for (Group &group : groups)
+	{
+		// Sort the edge constraints
+		QuickSort(group.mEdgeConstraints.begin(), group.mEdgeConstraints.end(), [this](uint inLHS, uint inRHS)
+			{
+				const Edge &e1 = mEdgeConstraints[inLHS];
+				const Edge &e2 = mEdgeConstraints[inRHS];
+
+				// First sort so that the edge with the smallest distance to a kinematic vertex comes first
+				float d1 = min(mClosestKinematic[e1.mVertex[0]].mDistance, mClosestKinematic[e1.mVertex[1]].mDistance);
+				float d2 = min(mClosestKinematic[e2.mVertex[0]].mDistance, mClosestKinematic[e2.mVertex[1]].mDistance);
+				if (d1 != d2)
+					return d1 < d2;
+
+				// Order the edges so that the ones with the smallest index go first (hoping to get better cache locality when we process the edges).
+				// Note we could also re-order the vertices but that would be much more of a burden to the end user
+				uint32 m1 = e1.GetMinVertexIndex();
+				uint32 m2 = e2.GetMinVertexIndex();
+				if (m1 != m2)
+					return m1 < m2;
+
+				return inLHS < inRHS;
+			});
+
+		// Sort the LRA constraints
+		QuickSort(group.mLRAConstraints.begin(), group.mLRAConstraints.end(), [this](uint inLHS, uint inRHS)
+			{
+				const LRA &l1 = mLRAConstraints[inLHS];
+				const LRA &l2 = mLRAConstraints[inRHS];
+
+				// First sort so that the longest constraint comes first (meaning the shortest constraint has the most influence on the end result)
+				// Most of the time there will be a single LRA constraint per vertex and since the LRA constraint only modifies a single vertex,
+				// updating one constraint will not violate another constraint.
+				if (l1.mMaxDistance != l2.mMaxDistance)
+					return l1.mMaxDistance > l2.mMaxDistance;
+
+				// Order constraints so that the ones with the smallest index go first
+				uint32 m1 = l1.GetMinVertexIndex();
+				uint32 m2 = l2.GetMinVertexIndex();
+				if (m1 != m2)
+					return m1 < m2;
+
+				return inLHS < inRHS;
+			});
+
+		// Sort the dihedral bend constraints
+		QuickSort(group.mDihedralBendConstraints.begin(), group.mDihedralBendConstraints.end(), [this](uint inLHS, uint inRHS)
+		{
+			const DihedralBend &b1 = mDihedralBendConstraints[inLHS];
+			const DihedralBend &b2 = mDihedralBendConstraints[inRHS];
+
+			// First sort so that the constraint with the smallest distance to a kinematic vertex comes first
+			float d1 = min(
+						min(mClosestKinematic[b1.mVertex[0]].mDistance, mClosestKinematic[b1.mVertex[1]].mDistance),
+						min(mClosestKinematic[b1.mVertex[2]].mDistance, mClosestKinematic[b1.mVertex[3]].mDistance));
+			float d2 = min(
+						min(mClosestKinematic[b2.mVertex[0]].mDistance, mClosestKinematic[b2.mVertex[1]].mDistance),
+						min(mClosestKinematic[b2.mVertex[2]].mDistance, mClosestKinematic[b2.mVertex[3]].mDistance));
+			if (d1 != d2)
+				return d1 < d2;
+
+			// Order constraints so that the ones with the smallest index go first
+			uint32 m1 = b1.GetMinVertexIndex();
+			uint32 m2 = b2.GetMinVertexIndex();
+			if (m1 != m2)
+				return m1 < m2;
+
+			return inLHS < inRHS;
+		});
+
+		// Sort the volume constraints
+		QuickSort(group.mVolumeConstraints.begin(), group.mVolumeConstraints.end(), [this](uint inLHS, uint inRHS)
+		{
+			const Volume &v1 = mVolumeConstraints[inLHS];
+			const Volume &v2 = mVolumeConstraints[inRHS];
+
+			// First sort so that the constraint with the smallest distance to a kinematic vertex comes first
+			float d1 = min(
+						min(mClosestKinematic[v1.mVertex[0]].mDistance, mClosestKinematic[v1.mVertex[1]].mDistance),
+						min(mClosestKinematic[v1.mVertex[2]].mDistance, mClosestKinematic[v1.mVertex[3]].mDistance));
+			float d2 = min(
+						min(mClosestKinematic[v2.mVertex[0]].mDistance, mClosestKinematic[v2.mVertex[1]].mDistance),
+						min(mClosestKinematic[v2.mVertex[2]].mDistance, mClosestKinematic[v2.mVertex[3]].mDistance));
+			if (d1 != d2)
+				return d1 < d2;
+
+			// Order constraints so that the ones with the smallest index go first
+			uint32 m1 = v1.GetMinVertexIndex();
+			uint32 m2 = v2.GetMinVertexIndex();
+			if (m1 != m2)
+				return m1 < m2;
+
+			return inLHS < inRHS;
+		});
+
+		// Sort the skinned constraints
+		QuickSort(group.mSkinnedConstraints.begin(), group.mSkinnedConstraints.end(), [this](uint inLHS, uint inRHS)
+			{
+				const Skinned &s1 = mSkinnedConstraints[inLHS];
+				const Skinned &s2 = mSkinnedConstraints[inRHS];
+
+				// Order the skinned constraints so that the ones with the smallest index go first (hoping to get better cache locality when we process the edges).
+				if (s1.mVertex != s2.mVertex)
+					return s1.mVertex < s2.mVertex;
+
+				return inLHS < inRHS;
+			});
+	}
+
+	// Temporary store constraints as we reorder them
+	Array<Edge> temp_edges;
+	temp_edges.swap(mEdgeConstraints);
+	mEdgeConstraints.reserve(temp_edges.size());
+	outResults.mEdgeRemap.reserve(temp_edges.size());
+
+	Array<LRA> temp_lra;
+	temp_lra.swap(mLRAConstraints);
+	mLRAConstraints.reserve(temp_lra.size());
+	outResults.mLRARemap.reserve(temp_lra.size());
+
+	Array<DihedralBend> temp_dihedral_bend;
+	temp_dihedral_bend.swap(mDihedralBendConstraints);
+	mDihedralBendConstraints.reserve(temp_dihedral_bend.size());
+	outResults.mDihedralBendRemap.reserve(temp_dihedral_bend.size());
+
+	Array<Volume> temp_volume;
+	temp_volume.swap(mVolumeConstraints);
+	mVolumeConstraints.reserve(temp_volume.size());
+	outResults.mVolumeRemap.reserve(temp_volume.size());
+
+	Array<Skinned> temp_skinned;
+	temp_skinned.swap(mSkinnedConstraints);
+	mSkinnedConstraints.reserve(temp_skinned.size());
+	outResults.mSkinnedRemap.reserve(temp_skinned.size());
+
+	// Finalize update groups
+	for (const Group &group : groups)
+	{
+		// Reorder edge constraints for this group
+		for (uint idx : group.mEdgeConstraints)
+		{
+			mEdgeConstraints.push_back(temp_edges[idx]);
+			outResults.mEdgeRemap.push_back(idx);
+		}
+
+		// Reorder LRA constraints for this group
+		for (uint idx : group.mLRAConstraints)
+		{
+			mLRAConstraints.push_back(temp_lra[idx]);
+			outResults.mLRARemap.push_back(idx);
+		}
+
+		// Reorder dihedral bend constraints for this group
+		for (uint idx : group.mDihedralBendConstraints)
+		{
+			mDihedralBendConstraints.push_back(temp_dihedral_bend[idx]);
+			outResults.mDihedralBendRemap.push_back(idx);
+		}
+
+		// Reorder volume constraints for this group
+		for (uint idx : group.mVolumeConstraints)
+		{
+			mVolumeConstraints.push_back(temp_volume[idx]);
+			outResults.mVolumeRemap.push_back(idx);
+		}
+
+		// Reorder skinned constraints for this group
+		for (uint idx : group.mSkinnedConstraints)
+		{
+			mSkinnedConstraints.push_back(temp_skinned[idx]);
+			outResults.mSkinnedRemap.push_back(idx);
+		}
+
+		// Store end indices
+		mUpdateGroups.push_back({ (uint)mEdgeConstraints.size(), (uint)mLRAConstraints.size(), (uint)mDihedralBendConstraints.size(), (uint)mVolumeConstraints.size(), (uint)mSkinnedConstraints.size() });
+	}
+
+	// Free closest kinematic buffer
+	mClosestKinematic.clear();
+	mClosestKinematic.shrink_to_fit();
+}
+
+Ref<SoftBodySharedSettings> SoftBodySharedSettings::Clone() const
+{
+	Ref<SoftBodySharedSettings> clone = new SoftBodySharedSettings;
+	clone->mVertices = mVertices;
+	clone->mFaces = mFaces;
+	clone->mEdgeConstraints = mEdgeConstraints;
+	clone->mDihedralBendConstraints = mDihedralBendConstraints;
+	clone->mVolumeConstraints = mVolumeConstraints;
+	clone->mSkinnedConstraints = mSkinnedConstraints;
+	clone->mSkinnedConstraintNormals = mSkinnedConstraintNormals;
+	clone->mInvBindMatrices = mInvBindMatrices;
+	clone->mLRAConstraints = mLRAConstraints;
+	clone->mMaterials = mMaterials;
+	clone->mVertexRadius = mVertexRadius;
+	clone->mUpdateGroups = mUpdateGroups;
+	return clone;
+}
+
+void SoftBodySharedSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(mVertices);
+	inStream.Write(mFaces);
+	inStream.Write(mEdgeConstraints);
+	inStream.Write(mDihedralBendConstraints);
+	inStream.Write(mVolumeConstraints);
+	inStream.Write(mSkinnedConstraints);
+	inStream.Write(mSkinnedConstraintNormals);
+	inStream.Write(mLRAConstraints);
+	inStream.Write(mVertexRadius);
+	inStream.Write(mUpdateGroups);
+
+	// Can't write mInvBindMatrices directly because the class contains padding
+	inStream.Write(mInvBindMatrices, [](const InvBind &inElement, StreamOut &inS) {
+		inS.Write(inElement.mJointIndex);
+		inS.Write(inElement.mInvBind);
+	});
+}
+
+void SoftBodySharedSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	inStream.Read(mVertices);
+	inStream.Read(mFaces);
+	inStream.Read(mEdgeConstraints);
+	inStream.Read(mDihedralBendConstraints);
+	inStream.Read(mVolumeConstraints);
+	inStream.Read(mSkinnedConstraints);
+	inStream.Read(mSkinnedConstraintNormals);
+	inStream.Read(mLRAConstraints);
+	inStream.Read(mVertexRadius);
+	inStream.Read(mUpdateGroups);
+
+	inStream.Read(mInvBindMatrices, [](StreamIn &inS, InvBind &outElement) {
+		inS.Read(outElement.mJointIndex);
+		inS.Read(outElement.mInvBind);
+	});
+}
+
+void SoftBodySharedSettings::SaveWithMaterials(StreamOut &inStream, SharedSettingsToIDMap &ioSettingsMap, MaterialToIDMap &ioMaterialMap) const
+{
+	SharedSettingsToIDMap::const_iterator settings_iter = ioSettingsMap.find(this);
+	if (settings_iter == ioSettingsMap.end())
+	{
+		// Write settings ID
+		uint32 settings_id = ioSettingsMap.size();
+		ioSettingsMap[this] = settings_id;
+		inStream.Write(settings_id);
+
+		// Write the settings
+		SaveBinaryState(inStream);
+
+		// Write materials
+		StreamUtils::SaveObjectArray(inStream, mMaterials, &ioMaterialMap);
+	}
+	else
+	{
+		// Known settings, just write the ID
+		inStream.Write(settings_iter->second);
+	}
+}
+
+SoftBodySharedSettings::SettingsResult SoftBodySharedSettings::sRestoreWithMaterials(StreamIn &inStream, IDToSharedSettingsMap &ioSettingsMap, IDToMaterialMap &ioMaterialMap)
+{
+	SettingsResult result;
+
+	// Read settings id
+	uint32 settings_id;
+	inStream.Read(settings_id);
+	if (inStream.IsEOF() || inStream.IsFailed())
+	{
+		result.SetError("Failed to read settings id");
+		return result;
+	}
+
+	// Check nullptr settings
+	if (settings_id == ~uint32(0))
+	{
+		result.Set(nullptr);
+		return result;
+	}
+
+	// Check if we already read this settings
+	if (settings_id < ioSettingsMap.size())
+	{
+		result.Set(ioSettingsMap[settings_id]);
+		return result;
+	}
+
+	// Create new object
+	Ref<SoftBodySharedSettings> settings = new SoftBodySharedSettings;
+
+	// Read state
+	settings->RestoreBinaryState(inStream);
+
+	// Read materials
+	Result mlresult = StreamUtils::RestoreObjectArray<PhysicsMaterialList>(inStream, ioMaterialMap);
+	if (mlresult.HasError())
+	{
+		result.SetError(mlresult.GetError());
+		return result;
+	}
+	settings->mMaterials = mlresult.Get();
+
+	// Add the settings to the map
+	ioSettingsMap.push_back(settings);
+
+	result.Set(settings);
+	return result;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.h
new file mode 100644
index 0000000000..d4b05a6ccd
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodySharedSettings.h
@@ -0,0 +1,335 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/Reference.h>
+#include <Jolt/Physics/Collision/PhysicsMaterial.h>
+#include <Jolt/Core/StreamUtils.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// This class defines the setup of all particles and their constraints.
+/// It is used during the simulation and can be shared between multiple soft bodies.
+class JPH_EXPORT SoftBodySharedSettings : public RefTarget<SoftBodySharedSettings>
+{
+	JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SoftBodySharedSettings)
+
+public:
+	/// Which type of bend constraint should be created
+	enum class EBendType
+	{
+		None,														///< No bend constraints will be created
+		Distance,													///< A simple distance constraint
+		Dihedral,													///< A dihedral bend constraint (most expensive, but also supports triangles that are initially not in the same plane)
+	};
+
+	/// The type of long range attachment constraint to create
+	enum class ELRAType
+	{
+		None,														///< Don't create a LRA constraint
+		EuclideanDistance,											///< Create a LRA constraint based on Euclidean distance between the closest kinematic vertex and this vertex
+		GeodesicDistance,											///< Create a LRA constraint based on the geodesic distance between the closest kinematic vertex and this vertex (follows the edge constraints)
+	};
+
+	/// Per vertex attributes used during the CreateConstraints function.
+	/// For an edge or shear constraint, the compliance is averaged between the two attached vertices.
+	/// For a bend constraint, the compliance is averaged between the two vertices on the shared edge.
+	struct JPH_EXPORT VertexAttributes
+	{
+		/// Constructor
+						VertexAttributes() = default;
+						VertexAttributes(float inCompliance, float inShearCompliance, float inBendCompliance, ELRAType inLRAType = ELRAType::None, float inLRAMaxDistanceMultiplier = 1.0f) : mCompliance(inCompliance), mShearCompliance(inShearCompliance), mBendCompliance(inBendCompliance), mLRAType(inLRAType), mLRAMaxDistanceMultiplier(inLRAMaxDistanceMultiplier) { }
+
+		float			mCompliance = 0.0f;							///< The compliance of the normal edges. Set to FLT_MAX to disable regular edges for any edge involving this vertex.
+		float			mShearCompliance = 0.0f;					///< The compliance of the shear edges. Set to FLT_MAX to disable shear edges for any edge involving this vertex.
+		float			mBendCompliance = FLT_MAX;					///< The compliance of the bend edges. Set to FLT_MAX to disable bend edges for any bend constraint involving this vertex.
+		ELRAType		mLRAType = ELRAType::None;					///< The type of long range attachment constraint to create.
+		float			mLRAMaxDistanceMultiplier = 1.0f;			///< Multiplier for the max distance of the LRA constraint, e.g. 1.01 means the max distance is 1% longer than the calculated distance in the rest pose.
+	};
+
+	/// Automatically create constraints based on the faces of the soft body
+	/// @param inVertexAttributes A list of attributes for each vertex (1-on-1 with mVertices, note that if the list is smaller than mVertices the last element will be repeated). This defines the properties of the constraints that are created.
+	/// @param inVertexAttributesLength The length of inVertexAttributes
+	/// @param inBendType The type of bend constraint to create
+	/// @param inAngleTolerance Shear edges are created when two connected triangles form a quad (are roughly in the same plane and form a square with roughly 90 degree angles). This defines the tolerance (in radians).
+	void				CreateConstraints(const VertexAttributes *inVertexAttributes, uint inVertexAttributesLength, EBendType inBendType = EBendType::Distance, float inAngleTolerance = DegreesToRadians(8.0f));
+
+	/// Calculate the initial lengths of all springs of the edges of this soft body (if you use CreateConstraint, this is already done)
+	void				CalculateEdgeLengths();
+
+	/// Calculate the max lengths for the long range attachment constraints based on Euclidean distance (if you use CreateConstraints, this is already done)
+	/// @param inMaxDistanceMultiplier Multiplier for the max distance of the LRA constraint, e.g. 1.01 means the max distance is 1% longer than the calculated distance in the rest pose.
+	void				CalculateLRALengths(float inMaxDistanceMultiplier = 1.0f);
+
+	/// Calculate the constants for the bend constraints (if you use CreateConstraints, this is already done)
+	void				CalculateBendConstraintConstants();
+
+	/// Calculates the initial volume of all tetrahedra of this soft body
+	void				CalculateVolumeConstraintVolumes();
+
+	/// Calculate information needed to be able to calculate the skinned constraint normals at run-time
+	void				CalculateSkinnedConstraintNormals();
+
+	/// Information about the optimization of the soft body, the indices of certain elements may have changed.
+	class OptimizationResults
+	{
+	public:
+		Array<uint>		mEdgeRemap;									///< Maps old edge index to new edge index
+		Array<uint>		mLRARemap;									///< Maps old LRA index to new LRA index
+		Array<uint>		mDihedralBendRemap;							///< Maps old dihedral bend index to new dihedral bend index
+		Array<uint>		mVolumeRemap;								///< Maps old volume constraint index to new volume constraint index
+		Array<uint>		mSkinnedRemap;								///< Maps old skinned constraint index to new skinned constraint index
+	};
+
+	/// Optimize the soft body settings for simulation. This will reorder constraints so they can be executed in parallel.
+	void				Optimize(OptimizationResults &outResults);
+
+	/// Optimize the soft body settings without results
+	void				Optimize()									{ OptimizationResults results; Optimize(results); }
+
+	/// Clone this object
+	Ref<SoftBodySharedSettings> Clone() const;
+
+	/// Saves the state of this object in binary form to inStream. Doesn't store the material list.
+	void				SaveBinaryState(StreamOut &inStream) const;
+
+	/// Restore the state of this object from inStream. Doesn't restore the material list.
+	void				RestoreBinaryState(StreamIn &inStream);
+
+	using SharedSettingsToIDMap = StreamUtils::ObjectToIDMap<SoftBodySharedSettings>;
+	using IDToSharedSettingsMap = StreamUtils::IDToObjectMap<SoftBodySharedSettings>;
+	using MaterialToIDMap = StreamUtils::ObjectToIDMap<PhysicsMaterial>;
+	using IDToMaterialMap = StreamUtils::IDToObjectMap<PhysicsMaterial>;
+
+	/// Save this shared settings and its materials. Pass in an empty map ioSettingsMap / ioMaterialMap or reuse the same map while saving multiple settings objects to the same stream in order to avoid writing duplicates.
+	void				SaveWithMaterials(StreamOut &inStream, SharedSettingsToIDMap &ioSettingsMap, MaterialToIDMap &ioMaterialMap) const;
+
+	using SettingsResult = Result<Ref<SoftBodySharedSettings>>;
+
+	/// Restore a shape and materials. Pass in an empty map in ioSettingsMap / ioMaterialMap or reuse the same map while reading multiple settings objects from the same stream in order to restore duplicates.
+	static SettingsResult sRestoreWithMaterials(StreamIn &inStream, IDToSharedSettingsMap &ioSettingsMap, IDToMaterialMap &ioMaterialMap);
+
+	/// A vertex is a particle, the data in this structure is only used during creation of the soft body and not during simulation
+	struct JPH_EXPORT Vertex
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Vertex)
+
+		/// Constructor
+						Vertex() = default;
+						Vertex(const Float3 &inPosition, const Float3 &inVelocity = Float3(0, 0, 0), float inInvMass = 1.0f) : mPosition(inPosition), mVelocity(inVelocity), mInvMass(inInvMass) { }
+
+		Float3			mPosition { 0, 0, 0 };						///< Initial position of the vertex
+		Float3			mVelocity { 0, 0, 0 };						///< Initial velocity of the vertex
+		float			mInvMass = 1.0f;							///< Initial inverse of the mass of the vertex
+	};
+
+	/// A face defines the surface of the body
+	struct JPH_EXPORT Face
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Face)
+
+		/// Constructor
+						Face() = default;
+						Face(uint32 inVertex1, uint32 inVertex2, uint32 inVertex3, uint32 inMaterialIndex = 0) : mVertex { inVertex1, inVertex2, inVertex3 }, mMaterialIndex(inMaterialIndex) { }
+
+		/// Check if this is a degenerate face (a face which points to the same vertex twice)
+		bool			IsDegenerate() const						{ return mVertex[0] == mVertex[1] || mVertex[0] == mVertex[2] || mVertex[1] == mVertex[2]; }
+
+		uint32			mVertex[3];									///< Indices of the vertices that form the face
+		uint32			mMaterialIndex = 0;							///< Index of the material of the face in SoftBodySharedSettings::mMaterials
+	};
+
+	/// An edge keeps two vertices at a constant distance using a spring: |x1 - x2| = rest length
+	struct JPH_EXPORT Edge
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Edge)
+
+		/// Constructor
+						Edge() = default;
+						Edge(uint32 inVertex1, uint32 inVertex2, float inCompliance = 0.0f) : mVertex { inVertex1, inVertex2 }, mCompliance(inCompliance) { }
+
+		/// Return the lowest vertex index of this constraint
+		uint32			GetMinVertexIndex() const					{ return min(mVertex[0], mVertex[1]); }
+
+		uint32			mVertex[2];									///< Indices of the vertices that form the edge
+		float			mRestLength = 1.0f;							///< Rest length of the spring
+		float			mCompliance = 0.0f;							///< Inverse of the stiffness of the spring
+	};
+
+	/**
+	 * A dihedral bend constraint keeps the angle between two triangles constant along their shared edge.
+	 *
+	 *        x2
+	 *       /  \
+	 *      / t0 \
+	 *     x0----x1
+	 *      \ t1 /
+	 *       \  /
+	 *        x3
+	 *
+	 * x0..x3 are the vertices, t0 and t1 are the triangles that share the edge x0..x1
+	 *
+	 * Based on:
+	 * - "Position Based Dynamics" - Matthias Muller et al.
+	 * - "Strain Based Dynamics" - Matthias Muller et al.
+	 * - "Simulation of Clothing with Folds and Wrinkles" - R. Bridson et al.
+	 */
+	struct JPH_EXPORT DihedralBend
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, DihedralBend)
+
+		/// Constructor
+						DihedralBend() = default;
+						DihedralBend(uint32 inVertex1, uint32 inVertex2, uint32 inVertex3, uint32 inVertex4, float inCompliance = 0.0f) : mVertex { inVertex1, inVertex2, inVertex3, inVertex4 }, mCompliance(inCompliance) { }
+
+		/// Return the lowest vertex index of this constraint
+		uint32			GetMinVertexIndex() const					{ return min(min(mVertex[0], mVertex[1]), min(mVertex[2], mVertex[3])); }
+
+		uint32			mVertex[4];									///< Indices of the vertices of the 2 triangles that share an edge (the first 2 vertices are the shared edge)
+		float			mCompliance = 0.0f;							///< Inverse of the stiffness of the constraint
+		float			mInitialAngle = 0.0f;						///< Initial angle between the normals of the triangles (pi - dihedral angle).
+	};
+
+	/// Volume constraint, keeps the volume of a tetrahedron constant
+	struct JPH_EXPORT Volume
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Volume)
+
+		/// Constructor
+						Volume() = default;
+						Volume(uint32 inVertex1, uint32 inVertex2, uint32 inVertex3, uint32 inVertex4, float inCompliance = 0.0f) : mVertex { inVertex1, inVertex2, inVertex3, inVertex4 }, mCompliance(inCompliance) { }
+
+		/// Return the lowest vertex index of this constraint
+		uint32			GetMinVertexIndex() const					{ return min(min(mVertex[0], mVertex[1]), min(mVertex[2], mVertex[3])); }
+
+		uint32			mVertex[4];									///< Indices of the vertices that form the tetrhedron
+		float			mSixRestVolume = 1.0f;						///< 6 times the rest volume of the tetrahedron (calculated by CalculateVolumeConstraintVolumes())
+		float			mCompliance = 0.0f;							///< Inverse of the stiffness of the constraint
+	};
+
+	/// An inverse bind matrix take a skinned vertex from its bind pose into joint local space
+	class JPH_EXPORT InvBind
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, InvBind)
+
+	public:
+		/// Constructor
+						InvBind() = default;
+						InvBind(uint32 inJointIndex, Mat44Arg inInvBind) : mJointIndex(inJointIndex), mInvBind(inInvBind) { }
+
+		uint32			mJointIndex = 0;							///< Joint index to which this is attached
+		Mat44			mInvBind = Mat44::sIdentity();				///< The inverse bind matrix, this takes a vertex in its bind pose (Vertex::mPosition) to joint local space
+	};
+
+	/// A joint and its skin weight
+	class JPH_EXPORT SkinWeight
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SkinWeight)
+
+	public:
+		/// Constructor
+						SkinWeight() = default;
+						SkinWeight(uint32 inInvBindIndex, float inWeight) : mInvBindIndex(inInvBindIndex), mWeight(inWeight) { }
+
+		uint32			mInvBindIndex = 0;							///< Index in mInvBindMatrices
+		float			mWeight = 0.0f;								///< Weight with which it is skinned
+	};
+
+	/// A constraint that skins a vertex to joints and limits the distance that the simulated vertex can travel from this vertex
+	class JPH_EXPORT Skinned
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Skinned)
+
+	public:
+		/// Constructor
+						Skinned() = default;
+						Skinned(uint32 inVertex, float inMaxDistance, float inBackStopDistance, float inBackStopRadius) : mVertex(inVertex), mMaxDistance(inMaxDistance), mBackStopDistance(inBackStopDistance), mBackStopRadius(inBackStopRadius) { }
+
+		/// Normalize the weights so that they add up to 1
+		void			NormalizeWeights()
+		{
+			// Get the total weight
+			float total = 0.0f;
+			for (const SkinWeight &w : mWeights)
+				total += w.mWeight;
+
+			// Normalize
+			if (total > 0.0f)
+				for (SkinWeight &w : mWeights)
+					w.mWeight /= total;
+		}
+
+		/// Maximum number of skin weights
+		static constexpr uint cMaxSkinWeights = 4;
+
+		uint32			mVertex = 0;								///< Index in mVertices which indicates which vertex is being skinned
+		SkinWeight		mWeights[cMaxSkinWeights];					///< Skin weights, the bind pose of the vertex is assumed to be stored in Vertex::mPosition. The first weight that is zero indicates the end of the list. Weights should add up to 1.
+		float			mMaxDistance = FLT_MAX;						///< Maximum distance that this vertex can reach from the skinned vertex, disabled when FLT_MAX. 0 when you want to hard skin the vertex to the skinned vertex.
+		float			mBackStopDistance = FLT_MAX;				///< Disabled if mBackStopDistance >= mMaxDistance. The faces surrounding mVertex determine an average normal. mBackStopDistance behind the vertex in the opposite direction of this normal, the back stop sphere starts. The simulated vertex will be pushed out of this sphere and it can be used to approximate the volume of the skinned mesh behind the skinned vertex.
+		float			mBackStopRadius = 40.0f;					///< Radius of the backstop sphere. By default this is a fairly large radius so the sphere approximates a plane.
+		uint32			mNormalInfo = 0;							///< Information needed to calculate the normal of this vertex, lowest 24 bit is start index in mSkinnedConstraintNormals, highest 8 bit is number of faces (generated by CalculateSkinnedConstraintNormals())
+	};
+
+	/// A long range attachment constraint, this is a constraint that sets a max distance between a kinematic vertex and a dynamic vertex
+	/// See: "Long Range Attachments - A Method to Simulate Inextensible Clothing in Computer Games", Tae-Yong Kim, Nuttapong Chentanez and Matthias Mueller-Fischer
+	class JPH_EXPORT LRA
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, LRA)
+
+	public:
+		/// Constructor
+						LRA() = default;
+						LRA(uint32 inVertex1, uint32 inVertex2, float inMaxDistance) : mVertex { inVertex1, inVertex2 }, mMaxDistance(inMaxDistance) { }
+
+		/// Return the lowest vertex index of this constraint
+		uint32			GetMinVertexIndex() const					{ return min(mVertex[0], mVertex[1]); }
+
+		uint32			mVertex[2];									///< The vertices that are connected. The first vertex should be kinematic, the 2nd dynamic.
+		float			mMaxDistance = 0.0f;						///< The maximum distance between the vertices
+	};
+
+	/// Add a face to this soft body
+	void				AddFace(const Face &inFace)					{ JPH_ASSERT(!inFace.IsDegenerate()); mFaces.push_back(inFace); }
+
+	Array<Vertex>		mVertices;									///< The list of vertices or particles of the body
+	Array<Face>			mFaces;										///< The list of faces of the body
+	Array<Edge>			mEdgeConstraints;							///< The list of edges or springs of the body
+	Array<DihedralBend>	mDihedralBendConstraints;					///< The list of dihedral bend constraints of the body
+	Array<Volume>		mVolumeConstraints;							///< The list of volume constraints of the body that keep the volume of tetrahedra in the soft body constant
+	Array<Skinned>		mSkinnedConstraints;						///< The list of vertices that are constrained to a skinned vertex
+	Array<InvBind>		mInvBindMatrices;							///< The list of inverse bind matrices for skinning vertices
+	Array<LRA>			mLRAConstraints;							///< The list of long range attachment constraints
+	PhysicsMaterialList mMaterials { PhysicsMaterial::sDefault };	///< The materials of the faces of the body, referenced by Face::mMaterialIndex
+	float				mVertexRadius = 0.0f;						///< How big the particles are, can be used to push the vertices a little bit away from the surface of other bodies to prevent z-fighting
+
+private:
+	friend class SoftBodyMotionProperties;
+
+	/// Calculate the closest kinematic vertex array
+	void				CalculateClosestKinematic();
+
+	/// Tracks the closest kinematic vertex
+	struct ClosestKinematic
+	{
+		uint32			mVertex = 0xffffffff;						///< Vertex index of closest kinematic vertex
+		float			mDistance = FLT_MAX;						///< Distance to the closest kinematic vertex
+	};
+
+	/// Tracks the end indices of the various constraint groups
+	struct UpdateGroup
+	{
+		uint			mEdgeEndIndex;								///< The end index of the edge constraints in this group
+		uint			mLRAEndIndex;								///< The end index of the LRA constraints in this group
+		uint			mDihedralBendEndIndex;						///< The end index of the dihedral bend constraints in this group
+		uint			mVolumeEndIndex;							///< The end index of the volume constraints in this group
+		uint			mSkinnedEndIndex;							///< The end index of the skinned constraints in this group
+	};
+
+	Array<ClosestKinematic> mClosestKinematic;						///< The closest kinematic vertex to each vertex in mVertices
+	Array<UpdateGroup>	mUpdateGroups;								///< The end indices for each group of constraints that can be updated in parallel
+	Array<uint32>		mSkinnedConstraintNormals;					///< A list of indices in the mFaces array used by mSkinnedConstraints, calculated by CalculateSkinnedConstraintNormals()
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyUpdateContext.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyUpdateContext.h
new file mode 100644
index 0000000000..2f565f5693
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyUpdateContext.h
@@ -0,0 +1,59 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/NonCopyable.h>
+#include <Jolt/Physics/Body/MotionProperties.h>
+
+JPH_NAMESPACE_BEGIN
+
+class Body;
+class SoftBodyMotionProperties;
+class SoftBodyContactListener;
+class SimShapeFilter;
+
+/// Temporary data used by the update of a soft body
+class SoftBodyUpdateContext : public NonCopyable
+{
+public:
+	static constexpr uint				cVertexCollisionBatch = 64;					///< Number of vertices to process in a batch in DetermineCollisionPlanes
+	static constexpr uint				cVertexConstraintBatch = 256;				///< Number of vertices to group for processing batches of constraints in ApplyEdgeConstraints
+
+	// Input
+	Body *								mBody;										///< Body that is being updated
+	SoftBodyMotionProperties *			mMotionProperties;							///< Motion properties of that body
+	SoftBodyContactListener *			mContactListener;							///< Contact listener to fire callbacks to
+	const SimShapeFilter *				mSimShapeFilter;							///< Shape filter to use for collision detection
+	RMat44								mCenterOfMassTransform;						///< Transform of the body relative to the soft body
+	Vec3								mGravity;									///< Gravity vector in local space of the soft body
+	Vec3								mDisplacementDueToGravity;					///< Displacement of the center of mass due to gravity in the current time step
+	float								mDeltaTime;									///< Delta time for the current time step
+	float								mSubStepDeltaTime;							///< Delta time for each sub step
+
+	/// Describes progress in the current update
+	enum class EState
+	{
+		DetermineCollisionPlanes,													///< Determine collision planes for vertices in parallel
+		DetermineSensorCollisions,													///< Determine collisions with sensors in parallel
+		ApplyConstraints,															///< Apply constraints in parallel
+		Done																		///< Update is finished
+	};
+
+	// State of the update
+	atomic<EState>						mState { EState::DetermineCollisionPlanes };///< Current state of the update
+	atomic<uint>						mNextCollisionVertex { 0 };					///< Next vertex to process for DetermineCollisionPlanes
+	atomic<uint>						mNumCollisionVerticesProcessed { 0 };		///< Number of vertices processed by DetermineCollisionPlanes, used to determine if we can go to the next step
+	atomic<uint>						mNextSensorIndex { 0 };						///< Next sensor to process for DetermineCollisionPlanes
+	atomic<uint>						mNumSensorsProcessed { 0 };					///< Number of sensors processed by DetermineSensorCollisions, used to determine if we can go to the next step
+	atomic<uint>						mNextIteration { 0 };						///< Next simulation iteration to process
+	atomic<uint>						mNextConstraintGroup { 0 };					///< Next constraint group to process
+	atomic<uint>						mNumConstraintGroupsProcessed { 0 };		///< Number of groups processed, used to determine if we can go to the next iteration
+
+	// Output
+	Vec3								mDeltaPosition;								///< Delta position of the body in the current time step, should be applied after the update
+	ECanSleep							mCanSleep;									///< Can the body sleep? Should be applied after the update
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyVertex.h b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyVertex.h
new file mode 100644
index 0000000000..72d4291e21
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/SoftBody/SoftBodyVertex.h
@@ -0,0 +1,36 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/Plane.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Run time information for a single particle of a soft body
+/// Note that at run-time you should only modify the inverse mass and/or velocity of a vertex to control the soft body.
+/// Modifying the position can lead to missed collisions.
+/// The other members are used internally by the soft body solver.
+class SoftBodyVertex
+{
+public:
+	/// Reset collision information to prepare for a new collision check
+	inline void		ResetCollision()
+	{
+		mLargestPenetration = -FLT_MAX;
+		mCollidingShapeIndex = -1;
+		mHasContact = false;
+	}
+
+	Vec3			mPreviousPosition;					///< Internal use only. Position at the previous time step
+	Vec3			mPosition;							///< Position, relative to the center of mass of the soft body
+	Vec3			mVelocity;							///< Velocity, relative to the center of mass of the soft body
+	Plane			mCollisionPlane;					///< Internal use only. Nearest collision plane, relative to the center of mass of the soft body
+	int				mCollidingShapeIndex;				///< Internal use only. Index in the colliding shapes list of the body we may collide with
+	bool			mHasContact;						///< True if the vertex has collided with anything in the last update
+	float			mLargestPenetration;				///< Internal use only. Used while finding the collision plane, stores the largest penetration found so far
+	float			mInvMass;							///< Inverse mass (1 / mass)
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/StateRecorder.h b/thirdparty/jolt_physics/Jolt/Physics/StateRecorder.h
new file mode 100644
index 0000000000..c3bd477107
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/StateRecorder.h
@@ -0,0 +1,136 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+class Body;
+class Constraint;
+class BodyID;
+
+JPH_SUPPRESS_WARNING_PUSH
+JPH_GCC_SUPPRESS_WARNING("-Wshadow") // GCC complains about the 'Constraints' value conflicting with the 'Constraints' typedef
+
+/// A bit field that determines which aspects of the simulation to save
+enum class EStateRecorderState : uint8
+{
+	None				= 0,														///< Save nothing
+	Global				= 1,														///< Save global physics system state (delta time, gravity, etc.)
+	Bodies				= 2,														///< Save the state of bodies
+	Contacts			= 4,														///< Save the state of contacts
+	Constraints			= 8,														///< Save the state of constraints
+	All					= Global | Bodies | Contacts | Constraints					///< Save all state
+};
+
+JPH_SUPPRESS_WARNING_POP
+
+/// Bitwise OR operator for EStateRecorderState
+constexpr EStateRecorderState operator | (EStateRecorderState inLHS, EStateRecorderState inRHS)
+{
+	return EStateRecorderState(uint8(inLHS) | uint8(inRHS));
+}
+
+/// Bitwise AND operator for EStateRecorderState
+constexpr EStateRecorderState operator & (EStateRecorderState inLHS, EStateRecorderState inRHS)
+{
+	return EStateRecorderState(uint8(inLHS) & uint8(inRHS));
+}
+
+/// Bitwise XOR operator for EStateRecorderState
+constexpr EStateRecorderState operator ^ (EStateRecorderState inLHS, EStateRecorderState inRHS)
+{
+	return EStateRecorderState(uint8(inLHS) ^ uint8(inRHS));
+}
+
+/// Bitwise NOT operator for EStateRecorderState
+constexpr EStateRecorderState operator ~ (EStateRecorderState inAllowedDOFs)
+{
+	return EStateRecorderState(~uint8(inAllowedDOFs));
+}
+
+/// Bitwise OR assignment operator for EStateRecorderState
+constexpr EStateRecorderState & operator |= (EStateRecorderState &ioLHS, EStateRecorderState inRHS)
+{
+	ioLHS = ioLHS | inRHS;
+	return ioLHS;
+}
+
+/// Bitwise AND assignment operator for EStateRecorderState
+constexpr EStateRecorderState & operator &= (EStateRecorderState &ioLHS, EStateRecorderState inRHS)
+{
+	ioLHS = ioLHS & inRHS;
+	return ioLHS;
+}
+
+/// Bitwise XOR assignment operator for EStateRecorderState
+constexpr EStateRecorderState & operator ^= (EStateRecorderState &ioLHS, EStateRecorderState inRHS)
+{
+	ioLHS = ioLHS ^ inRHS;
+	return ioLHS;
+}
+
+/// User callbacks that allow determining which parts of the simulation should be saved by a StateRecorder
+class JPH_EXPORT StateRecorderFilter
+{
+public:
+	/// Destructor
+	virtual				~StateRecorderFilter() = default;
+
+	///@name Functions called during SaveState
+	///@{
+
+	/// If the state of a specific body should be saved
+	virtual bool		ShouldSaveBody([[maybe_unused]] const Body &inBody) const					{ return true; }
+
+	/// If the state of a specific constraint should be saved
+	virtual bool		ShouldSaveConstraint([[maybe_unused]] const Constraint &inConstraint) const	{ return true; }
+
+	/// If the state of a specific contact should be saved
+	virtual bool		ShouldSaveContact([[maybe_unused]] const BodyID &inBody1, [[maybe_unused]] const BodyID &inBody2) const { return true; }
+
+	///@}
+	///@name Functions called during RestoreState
+	///@{
+
+	/// If the state of a specific contact should be restored
+	virtual bool		ShouldRestoreContact([[maybe_unused]] const BodyID &inBody1, [[maybe_unused]] const BodyID &inBody2) const { return true; }
+
+	///@}
+};
+
+/// Class that records the state of a physics system. Can be used to check if the simulation is deterministic by putting the recorder in validation mode.
+/// Can be used to restore the state to an earlier point in time. Note that only the state that is modified by the simulation is saved, configuration settings
+/// like body friction or restitution, motion quality etc. are not saved and need to be saved by the user if desired.
+class JPH_EXPORT StateRecorder : public StreamIn, public StreamOut
+{
+public:
+	/// Constructor
+						StateRecorder() = default;
+						StateRecorder(const StateRecorder &inRHS)					: mIsValidating(inRHS.mIsValidating) { }
+
+	/// Sets the stream in validation mode. In this case the physics system ensures that before it calls ReadBytes that it will
+	/// ensure that those bytes contain the current state. This makes it possible to step and save the state, restore to the previous
+	/// step and step again and when the recorded state is not the same it can restore the expected state and any byte that changes
+	/// due to a ReadBytes function can be caught to find out which part of the simulation is not deterministic.
+	/// Note that validation only works when saving the full state of the simulation (EStateRecorderState::All, StateRecorderFilter == nullptr).
+	void				SetValidating(bool inValidating)							{ mIsValidating = inValidating; }
+	bool				IsValidating() const										{ return mIsValidating; }
+
+	/// This allows splitting the state in multiple parts. While restoring, only the last part should have this flag set to true.
+	/// Note that you should ensure that the different parts contain information for disjoint sets of bodies, constraints and contacts.
+	/// E.g. if you restore the same contact twice, you get undefined behavior. In order to create disjoint sets you can use the StateRecorderFilter.
+	/// Note that validation is not compatible with restoring a simulation state in multiple parts.
+	void				SetIsLastPart(bool inIsLastPart)							{ mIsLastPart = inIsLastPart; }
+	bool				IsLastPart() const											{ return mIsLastPart; }
+
+private:
+	bool				mIsValidating = false;
+	bool				mIsLastPart = true;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/StateRecorderImpl.cpp b/thirdparty/jolt_physics/Jolt/Physics/StateRecorderImpl.cpp
new file mode 100644
index 0000000000..7624135896
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/StateRecorderImpl.cpp
@@ -0,0 +1,90 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/StateRecorderImpl.h>
+
+JPH_NAMESPACE_BEGIN
+
+void StateRecorderImpl::WriteBytes(const void *inData, size_t inNumBytes)
+{
+	mStream.write((const char *)inData, inNumBytes);
+}
+
+void StateRecorderImpl::Rewind()
+{
+	mStream.seekg(0, std::stringstream::beg);
+}
+
+void StateRecorderImpl::Clear()
+{
+	mStream.clear();
+	mStream.str({});
+}
+
+void StateRecorderImpl::ReadBytes(void *outData, size_t inNumBytes)
+{
+	if (IsValidating())
+	{
+		// Read data in temporary buffer to compare with current value
+		void *data = JPH_STACK_ALLOC(inNumBytes);
+		mStream.read((char *)data, inNumBytes);
+		if (memcmp(data, outData, inNumBytes) != 0)
+		{
+			// Mismatch, print error
+			Trace("Mismatch reading %u bytes", (uint)inNumBytes);
+			for (size_t i = 0; i < inNumBytes; ++i)
+			{
+				int b1 = reinterpret_cast<uint8 *>(outData)[i];
+				int b2 = reinterpret_cast<uint8 *>(data)[i];
+				if (b1 != b2)
+					Trace("Offset %d: %02X -> %02X", i, b1, b2);
+			}
+			JPH_BREAKPOINT;
+		}
+
+		// Copy the temporary data to the final destination
+		memcpy(outData, data, inNumBytes);
+		return;
+	}
+
+	mStream.read((char *)outData, inNumBytes);
+}
+
+bool StateRecorderImpl::IsEqual(StateRecorderImpl &inReference)
+{
+	// Get length of new state
+	mStream.seekg(0, std::stringstream::end);
+	std::streamoff this_len = mStream.tellg();
+	mStream.seekg(0, std::stringstream::beg);
+
+	// Get length of old state
+	inReference.mStream.seekg(0, std::stringstream::end);
+	std::streamoff reference_len = inReference.mStream.tellg();
+	inReference.mStream.seekg(0, std::stringstream::beg);
+
+	// Compare size
+	bool fail = reference_len != this_len;
+	if (fail)
+	{
+		Trace("Failed to properly recover state, different stream length!");
+		return false;
+	}
+
+	// Compare byte by byte
+	for (std::streamoff i = 0, l = this_len; !fail && i < l; ++i)
+	{
+		fail = inReference.mStream.get() != mStream.get();
+		if (fail)
+		{
+			Trace("Failed to properly recover state, different at offset %d!", (int)i);
+			return false;
+		}
+	}
+
+	return true;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/StateRecorderImpl.h b/thirdparty/jolt_physics/Jolt/Physics/StateRecorderImpl.h
new file mode 100644
index 0000000000..c994ece3e4
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/StateRecorderImpl.h
@@ -0,0 +1,50 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/StateRecorder.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Implementation of the StateRecorder class that uses a stringstream as underlying store and that implements checking if the state doesn't change upon reading
+class JPH_EXPORT StateRecorderImpl final : public StateRecorder
+{
+public:
+	/// Constructor
+						StateRecorderImpl() = default;
+						StateRecorderImpl(StateRecorderImpl &&inRHS)				: StateRecorder(inRHS), mStream(std::move(inRHS.mStream)) { }
+
+	/// Write a string of bytes to the binary stream
+	virtual void		WriteBytes(const void *inData, size_t inNumBytes) override;
+
+	/// Rewind the stream for reading
+	void				Rewind();
+
+	/// Clear the stream for reuse
+	void				Clear();
+
+	/// Read a string of bytes from the binary stream
+	virtual void		ReadBytes(void *outData, size_t inNumBytes) override;
+
+	// See StreamIn
+	virtual bool		IsEOF() const override										{ return mStream.eof(); }
+
+	// See StreamIn / StreamOut
+	virtual bool		IsFailed() const override									{ return mStream.fail(); }
+
+	/// Compare this state with a reference state and ensure they are the same
+	bool				IsEqual(StateRecorderImpl &inReference);
+
+	/// Convert the binary data to a string
+	std::string			GetData() const												{ return mStream.str(); }
+
+	/// Get size of the binary data in bytes
+	size_t				GetDataSize()												{ return size_t(mStream.tellp()); }
+
+private:
+	std::stringstream	mStream;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/MotorcycleController.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/MotorcycleController.cpp
new file mode 100644
index 0000000000..1bfa564f76
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/MotorcycleController.cpp
@@ -0,0 +1,293 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Vehicle/MotorcycleController.h>
+#include <Jolt/Physics/PhysicsSystem.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(MotorcycleControllerSettings)
+{
+	JPH_ADD_BASE_CLASS(MotorcycleControllerSettings, VehicleControllerSettings)
+
+	JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mMaxLeanAngle)
+	JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSpringConstant)
+	JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSpringDamping)
+	JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSpringIntegrationCoefficient)
+	JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSpringIntegrationCoefficientDecay)
+	JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSmoothingFactor)
+}
+
+VehicleController *MotorcycleControllerSettings::ConstructController(VehicleConstraint &inConstraint) const
+{
+	return new MotorcycleController(*this, inConstraint);
+}
+
+void MotorcycleControllerSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	WheeledVehicleControllerSettings::SaveBinaryState(inStream);
+
+	inStream.Write(mMaxLeanAngle);
+	inStream.Write(mLeanSpringConstant);
+	inStream.Write(mLeanSpringDamping);
+	inStream.Write(mLeanSpringIntegrationCoefficient);
+	inStream.Write(mLeanSpringIntegrationCoefficientDecay);
+	inStream.Write(mLeanSmoothingFactor);
+}
+
+void MotorcycleControllerSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	WheeledVehicleControllerSettings::RestoreBinaryState(inStream);
+
+	inStream.Read(mMaxLeanAngle);
+	inStream.Read(mLeanSpringConstant);
+	inStream.Read(mLeanSpringDamping);
+	inStream.Read(mLeanSpringIntegrationCoefficient);
+	inStream.Read(mLeanSpringIntegrationCoefficientDecay);
+	inStream.Read(mLeanSmoothingFactor);
+}
+
+MotorcycleController::MotorcycleController(const MotorcycleControllerSettings &inSettings, VehicleConstraint &inConstraint) :
+	WheeledVehicleController(inSettings, inConstraint),
+	mMaxLeanAngle(inSettings.mMaxLeanAngle),
+	mLeanSpringConstant(inSettings.mLeanSpringConstant),
+	mLeanSpringDamping(inSettings.mLeanSpringDamping),
+	mLeanSpringIntegrationCoefficient(inSettings.mLeanSpringIntegrationCoefficient),
+	mLeanSpringIntegrationCoefficientDecay(inSettings.mLeanSpringIntegrationCoefficientDecay),
+	mLeanSmoothingFactor(inSettings.mLeanSmoothingFactor)
+{
+}
+
+float MotorcycleController::GetWheelBase() const
+{
+	float low = FLT_MAX, high = -FLT_MAX;
+
+	for (const Wheel *w : mConstraint.GetWheels())
+	{
+		const WheelSettings *s = w->GetSettings();
+
+		// Measure distance along the forward axis by looking at the fully extended suspension.
+		// If the suspension force point is active, use that instead.
+		Vec3 force_point = s->mEnableSuspensionForcePoint? s->mSuspensionForcePoint : s->mPosition + s->mSuspensionDirection * s->mSuspensionMaxLength;
+		float value = force_point.Dot(mConstraint.GetLocalForward());
+
+		// Update min and max
+		low = min(low, value);
+		high = max(high, value);
+	}
+
+	return high - low;
+}
+
+void MotorcycleController::PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem)
+{
+	WheeledVehicleController::PreCollide(inDeltaTime, inPhysicsSystem);
+
+	const Body *body = mConstraint.GetVehicleBody();
+	Vec3 forward = body->GetRotation() * mConstraint.GetLocalForward();
+	float wheel_base = GetWheelBase();
+	Vec3 world_up = mConstraint.GetWorldUp();
+
+	if (mEnableLeanController)
+	{
+		// Calculate the target lean vector, this is in the direction of the total applied impulse by the ground on the wheels
+		Vec3 target_lean = Vec3::sZero();
+		for (const Wheel *w : mConstraint.GetWheels())
+			if (w->HasContact())
+				target_lean += w->GetContactNormal() * w->GetSuspensionLambda() + w->GetContactLateral() * w->GetLateralLambda();
+
+		// Normalize the impulse
+		target_lean = target_lean.NormalizedOr(world_up);
+
+		// Smooth the impulse to avoid jittery behavior
+		mTargetLean = mLeanSmoothingFactor * mTargetLean + (1.0f - mLeanSmoothingFactor) * target_lean;
+
+		// Remove forward component, we can only lean sideways
+		mTargetLean -= forward * mTargetLean.Dot(forward);
+		mTargetLean = mTargetLean.NormalizedOr(world_up);
+
+		// Clamp the target lean against the max lean angle
+		Vec3 adjusted_world_up = world_up - forward * world_up.Dot(forward);
+		adjusted_world_up = adjusted_world_up.NormalizedOr(world_up);
+		float w_angle = -Sign(mTargetLean.Cross(adjusted_world_up).Dot(forward)) * ACos(mTargetLean.Dot(adjusted_world_up));
+		if (abs(w_angle) > mMaxLeanAngle)
+			mTargetLean = Quat::sRotation(forward, Sign(w_angle) * mMaxLeanAngle) * adjusted_world_up;
+
+		// Integrate the delta angle
+		Vec3 up = body->GetRotation() * mConstraint.GetLocalUp();
+		float d_angle = -Sign(mTargetLean.Cross(up).Dot(forward)) * ACos(mTargetLean.Dot(up));
+		mLeanSpringIntegratedDeltaAngle += d_angle * inDeltaTime;
+	}
+	else
+	{
+		// Controller not enabled, reset target lean
+		mTargetLean = world_up;
+
+		// Reset integrated delta angle
+		mLeanSpringIntegratedDeltaAngle = 0;
+	}
+
+	JPH_DET_LOG("WheeledVehicleController::PreCollide: mTargetLean: " << mTargetLean);
+
+	// Calculate max steering angle based on the max lean angle we're willing to take
+	// See: https://en.wikipedia.org/wiki/Bicycle_and_motorcycle_dynamics#Leaning
+	// LeanAngle = Atan(Velocity^2 / (Gravity * TurnRadius))
+	// And: https://en.wikipedia.org/wiki/Turning_radius (we're ignoring the tire width)
+	// The CasterAngle is the added according to https://en.wikipedia.org/wiki/Bicycle_and_motorcycle_dynamics#Turning (this is the same formula but without small angle approximation)
+	// TurnRadius = WheelBase / (Sin(SteerAngle) * Cos(CasterAngle))
+	// => SteerAngle = ASin(WheelBase * Tan(LeanAngle) * Gravity / (Velocity^2 * Cos(CasterAngle))
+	// The caster angle is different for each wheel so we can only calculate part of the equation here
+	float max_steer_angle_factor = wheel_base * Tan(mMaxLeanAngle) * (mConstraint.IsGravityOverridden()? mConstraint.GetGravityOverride() : inPhysicsSystem.GetGravity()).Length();
+
+	// Calculate forward velocity
+	float velocity = body->GetLinearVelocity().Dot(forward);
+	float velocity_sq = Square(velocity);
+
+	// Decompose steering into sign and direction
+	float steer_strength = abs(mRightInput);
+	float steer_sign = -Sign(mRightInput);
+
+	for (Wheel *w_base : mConstraint.GetWheels())
+	{
+		WheelWV *w = static_cast<WheelWV *>(w_base);
+		const WheelSettingsWV *s = w->GetSettings();
+
+		// Check if this wheel can steer
+		if (s->mMaxSteerAngle != 0.0f)
+		{
+			// Calculate cos(caster angle), the angle between the steering axis and the up vector
+			float cos_caster_angle = s->mSteeringAxis.Dot(mConstraint.GetLocalUp());
+
+			// Calculate steer angle
+			float steer_angle = steer_strength * w->GetSettings()->mMaxSteerAngle;
+
+			// Clamp to max steering angle
+			if (mEnableLeanSteeringLimit
+				&& velocity_sq > 1.0e-6f && cos_caster_angle > 1.0e-6f)
+			{
+				float max_steer_angle = ASin(max_steer_angle_factor / (velocity_sq * cos_caster_angle));
+				steer_angle = min(steer_angle, max_steer_angle);
+			}
+
+			// Set steering angle
+			w->SetSteerAngle(steer_sign * steer_angle);
+		}
+	}
+
+	// Reset applied impulse
+	mAppliedImpulse = 0;
+}
+
+bool MotorcycleController::SolveLongitudinalAndLateralConstraints(float inDeltaTime)
+{
+	bool impulse = WheeledVehicleController::SolveLongitudinalAndLateralConstraints(inDeltaTime);
+
+	if (mEnableLeanController)
+	{
+		// Only apply a lean impulse if all wheels are in contact, otherwise we can easily spin out
+		bool all_in_contact = true;
+		for (const Wheel *w : mConstraint.GetWheels())
+			if (!w->HasContact() || w->GetSuspensionLambda() <= 0.0f)
+			{
+				all_in_contact = false;
+				break;
+			}
+
+		if (all_in_contact)
+		{
+			Body *body = mConstraint.GetVehicleBody();
+			const MotionProperties *mp = body->GetMotionProperties();
+
+			Vec3 forward = body->GetRotation() * mConstraint.GetLocalForward();
+			Vec3 up = body->GetRotation() * mConstraint.GetLocalUp();
+
+			// Calculate delta to target angle and derivative
+			float d_angle = -Sign(mTargetLean.Cross(up).Dot(forward)) * ACos(mTargetLean.Dot(up));
+			float ddt_angle = body->GetAngularVelocity().Dot(forward);
+
+			// Calculate impulse to apply to get to target lean angle
+			float total_impulse = (mLeanSpringConstant * d_angle - mLeanSpringDamping * ddt_angle + mLeanSpringIntegrationCoefficient * mLeanSpringIntegratedDeltaAngle) * inDeltaTime;
+
+			// Remember angular velocity pre angular impulse
+			Vec3 old_w = mp->GetAngularVelocity();
+
+			// Apply impulse taking into account the impulse we've applied earlier
+			float delta_impulse = total_impulse - mAppliedImpulse;
+			body->AddAngularImpulse(delta_impulse * forward);
+			mAppliedImpulse = total_impulse;
+
+			// Calculate delta angular velocity due to angular impulse
+			Vec3 dw = mp->GetAngularVelocity() - old_w;
+			Vec3 linear_acceleration = Vec3::sZero();
+			float total_lambda = 0.0f;
+			for (Wheel *w_base : mConstraint.GetWheels())
+			{
+				const WheelWV *w = static_cast<WheelWV *>(w_base);
+
+				// We weigh the importance of each contact point according to the contact force
+				float lambda = w->GetSuspensionLambda();
+				total_lambda += lambda;
+
+				// Linear acceleration of contact point is dw x com_to_contact
+				Vec3 r = Vec3(w->GetContactPosition() - body->GetCenterOfMassPosition());
+				linear_acceleration += lambda * dw.Cross(r);
+			}
+
+			// Apply linear impulse to COM to cancel the average velocity change on the wheels due to the angular impulse
+			Vec3 linear_impulse = -linear_acceleration / (total_lambda * mp->GetInverseMass());
+			body->AddImpulse(linear_impulse);
+
+			// Return true if we applied an impulse
+			impulse |= delta_impulse != 0.0f;
+		}
+		else
+		{
+			// Decay the integrated angle because we won't be applying a torque this frame
+			// Uses 1st order Taylor approximation of e^(-decay * dt) = 1 - decay * dt
+			mLeanSpringIntegratedDeltaAngle *= max(0.0f, 1.0f - mLeanSpringIntegrationCoefficientDecay * inDeltaTime);
+		}
+	}
+
+	return impulse;
+}
+
+void MotorcycleController::SaveState(StateRecorder &inStream) const
+{
+	WheeledVehicleController::SaveState(inStream);
+
+	inStream.Write(mTargetLean);
+}
+
+void MotorcycleController::RestoreState(StateRecorder &inStream)
+{
+	WheeledVehicleController::RestoreState(inStream);
+
+	inStream.Read(mTargetLean);
+}
+
+#ifdef JPH_DEBUG_RENDERER
+
+void MotorcycleController::Draw(DebugRenderer *inRenderer) const
+{
+	WheeledVehicleController::Draw(inRenderer);
+
+	// Draw current and desired lean angle
+	Body *body = mConstraint.GetVehicleBody();
+	RVec3 center_of_mass = body->GetCenterOfMassPosition();
+	Vec3 up = body->GetRotation() * mConstraint.GetLocalUp();
+	inRenderer->DrawArrow(center_of_mass, center_of_mass + up, Color::sYellow, 0.1f);
+	inRenderer->DrawArrow(center_of_mass, center_of_mass + mTargetLean, Color::sRed, 0.1f);
+}
+
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/MotorcycleController.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/MotorcycleController.h
new file mode 100644
index 0000000000..bf8ebfa43f
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/MotorcycleController.h
@@ -0,0 +1,116 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Vehicle/WheeledVehicleController.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Settings of a two wheeled motorcycle (adds a spring to balance the motorcycle)
+/// Note: The motor cycle controller is still in development and may need a lot of tweaks/hacks to work properly!
+class JPH_EXPORT MotorcycleControllerSettings : public WheeledVehicleControllerSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, MotorcycleControllerSettings)
+
+public:
+	// See: VehicleControllerSettings
+	virtual VehicleController *	ConstructController(VehicleConstraint &inConstraint) const override;
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+
+	/// How far we're willing to make the bike lean over in turns (in radians)
+	float						mMaxLeanAngle = DegreesToRadians(45.0f);
+
+	/// Spring constant for the lean spring
+	float						mLeanSpringConstant = 5000.0f;
+
+	/// Spring damping constant for the lean spring
+	float						mLeanSpringDamping = 1000.0f;
+
+	/// The lean spring applies an additional force equal to this coefficient * Integral(delta angle, 0, t), this effectively makes the lean spring a PID controller
+	float						mLeanSpringIntegrationCoefficient = 0.0f;
+
+	/// How much to decay the angle integral when the wheels are not touching the floor: new_value = e^(-decay * t) * initial_value
+	float						mLeanSpringIntegrationCoefficientDecay = 4.0f;
+
+	/// How much to smooth the lean angle (0 = no smoothing, 1 = lean angle never changes)
+	/// Note that this is frame rate dependent because the formula is: smoothing_factor * previous + (1 - smoothing_factor) * current
+	float						mLeanSmoothingFactor = 0.8f;
+};
+
+/// Runtime controller class
+class JPH_EXPORT MotorcycleController : public WheeledVehicleController
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+								MotorcycleController(const MotorcycleControllerSettings &inSettings, VehicleConstraint &inConstraint);
+
+	/// Get the distance between the front and back wheels
+	float						GetWheelBase() const;
+
+	/// Enable or disable the lean spring. This allows you to temporarily disable the lean spring to allow the motorcycle to fall over.
+	void						EnableLeanController(bool inEnable)					{ mEnableLeanController = inEnable; }
+
+	/// Check if the lean spring is enabled.
+	bool						IsLeanControllerEnabled() const						{ return mEnableLeanController; }
+
+	/// Enable or disable the lean steering limit. When enabled (default) the steering angle is limited based on the vehicle speed to prevent steering that would cause an inertial force that causes the motorcycle to topple over.
+	void						EnableLeanSteeringLimit(bool inEnable)				{ mEnableLeanSteeringLimit = inEnable; }
+	bool						IsLeanSteeringLimitEnabled() const					{ return mEnableLeanSteeringLimit; }
+
+	/// Spring constant for the lean spring
+	void						SetLeanSpringConstant(float inConstant)				{ mLeanSpringConstant = inConstant; }
+	float						GetLeanSpringConstant() const						{ return mLeanSpringConstant; }
+
+	/// Spring damping constant for the lean spring
+	void						SetLeanSpringDamping(float inDamping)				{ mLeanSpringDamping = inDamping; }
+	float						GetLeanSpringDamping() const						{ return mLeanSpringDamping; }
+
+	/// The lean spring applies an additional force equal to this coefficient * Integral(delta angle, 0, t), this effectively makes the lean spring a PID controller
+	void						SetLeanSpringIntegrationCoefficient(float inCoefficient) { mLeanSpringIntegrationCoefficient = inCoefficient; }
+	float						GetLeanSpringIntegrationCoefficient() const			{ return mLeanSpringIntegrationCoefficient; }
+
+	/// How much to decay the angle integral when the wheels are not touching the floor: new_value = e^(-decay * t) * initial_value
+	void						SetLeanSpringIntegrationCoefficientDecay(float inDecay) { mLeanSpringIntegrationCoefficientDecay = inDecay; }
+	float						GetLeanSpringIntegrationCoefficientDecay() const	{ return mLeanSpringIntegrationCoefficientDecay; }
+
+	/// How much to smooth the lean angle (0 = no smoothing, 1 = lean angle never changes)
+	/// Note that this is frame rate dependent because the formula is: smoothing_factor * previous + (1 - smoothing_factor) * current
+	void						SetLeanSmoothingFactor(float inFactor)				{ mLeanSmoothingFactor = inFactor; }
+	float						GetLeanSmoothingFactor() const						{ return mLeanSmoothingFactor; }
+
+protected:
+	// See: VehicleController
+	virtual void				PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override;
+	virtual bool				SolveLongitudinalAndLateralConstraints(float inDeltaTime) override;
+	virtual void				SaveState(StateRecorder &inStream) const override;
+	virtual void				RestoreState(StateRecorder &inStream) override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void				Draw(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// Configuration properties
+	bool						mEnableLeanController = true;
+	bool						mEnableLeanSteeringLimit = true;
+	float						mMaxLeanAngle;
+	float						mLeanSpringConstant;
+	float						mLeanSpringDamping;
+	float						mLeanSpringIntegrationCoefficient;
+	float						mLeanSpringIntegrationCoefficientDecay;
+	float						mLeanSmoothingFactor;
+
+	// Run-time calculated target lean vector
+	Vec3						mTargetLean = Vec3::sZero();
+
+	// Integrated error for the lean spring
+	float						mLeanSpringIntegratedDeltaAngle = 0.0f;
+
+	// Run-time total angular impulse applied to turn the cycle towards the target lean angle
+	float						mAppliedImpulse = 0.0f;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/TrackedVehicleController.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/TrackedVehicleController.cpp
new file mode 100644
index 0000000000..d3cfcffc54
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/TrackedVehicleController.cpp
@@ -0,0 +1,531 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Vehicle/TrackedVehicleController.h>
+#include <Jolt/Physics/PhysicsSystem.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(TrackedVehicleControllerSettings)
+{
+	JPH_ADD_BASE_CLASS(TrackedVehicleControllerSettings, VehicleControllerSettings)
+
+	JPH_ADD_ATTRIBUTE(TrackedVehicleControllerSettings, mEngine)
+	JPH_ADD_ATTRIBUTE(TrackedVehicleControllerSettings, mTransmission)
+	JPH_ADD_ATTRIBUTE(TrackedVehicleControllerSettings, mTracks)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(WheelSettingsTV)
+{
+	JPH_ADD_ATTRIBUTE(WheelSettingsTV, mLongitudinalFriction)
+	JPH_ADD_ATTRIBUTE(WheelSettingsTV, mLateralFriction)
+}
+
+void WheelSettingsTV::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(mLongitudinalFriction);
+	inStream.Write(mLateralFriction);
+}
+
+void WheelSettingsTV::RestoreBinaryState(StreamIn &inStream)
+{
+	inStream.Read(mLongitudinalFriction);
+	inStream.Read(mLateralFriction);
+}
+
+WheelTV::WheelTV(const WheelSettingsTV &inSettings) :
+	Wheel(inSettings)
+{
+}
+
+void WheelTV::CalculateAngularVelocity(const VehicleConstraint &inConstraint)
+{
+	const WheelSettingsTV *settings = GetSettings();
+	const Wheels &wheels = inConstraint.GetWheels();
+	const VehicleTrack &track = static_cast<const TrackedVehicleController *>(inConstraint.GetController())->GetTracks()[mTrackIndex];
+
+	// Calculate angular velocity of this wheel
+	mAngularVelocity = track.mAngularVelocity * wheels[track.mDrivenWheel]->GetSettings()->mRadius / settings->mRadius;
+}
+
+void WheelTV::Update(uint inWheelIndex, float inDeltaTime, const VehicleConstraint &inConstraint)
+{
+	CalculateAngularVelocity(inConstraint);
+
+	// Update rotation of wheel
+	mAngle = fmod(mAngle + mAngularVelocity * inDeltaTime, 2.0f * JPH_PI);
+
+	// Reset brake impulse, will be set during post collision again
+	mBrakeImpulse = 0.0f;
+
+	if (mContactBody != nullptr)
+	{
+		// Friction at the point of this wheel between track and floor
+		const WheelSettingsTV *settings = GetSettings();
+		VehicleConstraint::CombineFunction combine_friction = inConstraint.GetCombineFriction();
+		mCombinedLongitudinalFriction = settings->mLongitudinalFriction;
+		mCombinedLateralFriction = settings->mLateralFriction;
+		combine_friction(inWheelIndex, mCombinedLongitudinalFriction, mCombinedLateralFriction, *mContactBody, mContactSubShapeID);
+	}
+	else
+	{
+		// No collision
+		mCombinedLongitudinalFriction = mCombinedLateralFriction = 0.0f;
+	}
+}
+
+VehicleController *TrackedVehicleControllerSettings::ConstructController(VehicleConstraint &inConstraint) const
+{
+	return new TrackedVehicleController(*this, inConstraint);
+}
+
+TrackedVehicleControllerSettings::TrackedVehicleControllerSettings()
+{
+	// Numbers guestimated from: https://en.wikipedia.org/wiki/M1_Abrams
+	mEngine.mMinRPM = 500.0f;
+	mEngine.mMaxRPM = 4000.0f;
+	mEngine.mMaxTorque = 500.0f; // Note actual torque for M1 is around 5000 but we need a reduced mass in order to keep the simulation sane
+
+	mTransmission.mShiftDownRPM = 1000.0f;
+	mTransmission.mShiftUpRPM = 3500.0f;
+	mTransmission.mGearRatios = { 4.0f, 3.0f, 2.0f, 1.0f };
+	mTransmission.mReverseGearRatios = { -4.0f, -3.0f };
+}
+
+void TrackedVehicleControllerSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	mEngine.SaveBinaryState(inStream);
+
+	mTransmission.SaveBinaryState(inStream);
+
+	for (const VehicleTrackSettings &t : mTracks)
+		t.SaveBinaryState(inStream);
+}
+
+void TrackedVehicleControllerSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	mEngine.RestoreBinaryState(inStream);
+
+	mTransmission.RestoreBinaryState(inStream);
+
+	for (VehicleTrackSettings &t : mTracks)
+		t.RestoreBinaryState(inStream);
+}
+
+TrackedVehicleController::TrackedVehicleController(const TrackedVehicleControllerSettings &inSettings, VehicleConstraint &inConstraint) :
+	VehicleController(inConstraint)
+{
+	// Copy engine settings
+	static_cast<VehicleEngineSettings &>(mEngine) = inSettings.mEngine;
+	JPH_ASSERT(inSettings.mEngine.mMinRPM >= 0.0f);
+	JPH_ASSERT(inSettings.mEngine.mMinRPM <= inSettings.mEngine.mMaxRPM);
+	mEngine.SetCurrentRPM(mEngine.mMinRPM);
+
+	// Copy transmission settings
+	static_cast<VehicleTransmissionSettings &>(mTransmission) = inSettings.mTransmission;
+#ifdef JPH_ENABLE_ASSERTS
+	for (float r : inSettings.mTransmission.mGearRatios)
+		JPH_ASSERT(r > 0.0f);
+	for (float r : inSettings.mTransmission.mReverseGearRatios)
+		JPH_ASSERT(r < 0.0f);
+#endif // JPH_ENABLE_ASSERTS
+	JPH_ASSERT(inSettings.mTransmission.mSwitchTime >= 0.0f);
+	JPH_ASSERT(inSettings.mTransmission.mShiftDownRPM > 0.0f);
+	JPH_ASSERT(inSettings.mTransmission.mMode != ETransmissionMode::Auto || inSettings.mTransmission.mShiftUpRPM < inSettings.mEngine.mMaxRPM);
+	JPH_ASSERT(inSettings.mTransmission.mShiftUpRPM > inSettings.mTransmission.mShiftDownRPM);
+
+	// Copy track settings
+	for (uint i = 0; i < std::size(mTracks); ++i)
+	{
+		const VehicleTrackSettings &d = inSettings.mTracks[i];
+		static_cast<VehicleTrackSettings &>(mTracks[i]) = d;
+		JPH_ASSERT(d.mInertia >= 0.0f);
+		JPH_ASSERT(d.mAngularDamping >= 0.0f);
+		JPH_ASSERT(d.mMaxBrakeTorque >= 0.0f);
+		JPH_ASSERT(d.mDifferentialRatio > 0.0f);
+	}
+}
+
+bool TrackedVehicleController::AllowSleep() const
+{
+	return mForwardInput == 0.0f								// No user input
+		&& mTransmission.AllowSleep()							// Transmission is not shifting
+		&& mEngine.AllowSleep();								// Engine is idling
+}
+
+void TrackedVehicleController::PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem)
+{
+	Wheels &wheels = mConstraint.GetWheels();
+
+	// Fill in track index
+	for (size_t t = 0; t < std::size(mTracks); ++t)
+		for (uint w : mTracks[t].mWheels)
+			static_cast<WheelTV *>(wheels[w])->mTrackIndex = (uint)t;
+
+	// Angular damping: dw/dt = -c * w
+	// Solution: w(t) = w(0) * e^(-c * t) or w2 = w1 * e^(-c * dt)
+	// Taylor expansion of e^(-c * dt) = 1 - c * dt + ...
+	// Since dt is usually in the order of 1/60 and c is a low number too this approximation is good enough
+	for (VehicleTrack &t : mTracks)
+		t.mAngularVelocity *= max(0.0f, 1.0f - t.mAngularDamping * inDeltaTime);
+}
+
+void TrackedVehicleController::SyncLeftRightTracks()
+{
+	// Apply left to right ratio according to track inertias
+	VehicleTrack &tl = mTracks[(int)ETrackSide::Left];
+	VehicleTrack &tr = mTracks[(int)ETrackSide::Right];
+
+	if (mLeftRatio * mRightRatio > 0.0f)
+	{
+		// Solve: (tl.mAngularVelocity + dl) / (tr.mAngularVelocity + dr) = mLeftRatio / mRightRatio and dl * tr.mInertia = -dr * tl.mInertia, where dl/dr are the delta angular velocities for left and right tracks
+		float impulse = (mLeftRatio * tr.mAngularVelocity - mRightRatio * tl.mAngularVelocity) / (mLeftRatio * tr.mInertia + mRightRatio * tl.mInertia);
+		tl.mAngularVelocity += impulse * tl.mInertia;
+		tr.mAngularVelocity -= impulse * tr.mInertia;
+	}
+	else
+	{
+		// Solve: (tl.mAngularVelocity + dl) / (tr.mAngularVelocity + dr) = mLeftRatio / mRightRatio and dl * tr.mInertia = dr * tl.mInertia, where dl/dr are the delta angular velocities for left and right tracks
+		float impulse = (mLeftRatio * tr.mAngularVelocity - mRightRatio * tl.mAngularVelocity) / (mRightRatio * tl.mInertia - mLeftRatio * tr.mInertia);
+		tl.mAngularVelocity += impulse * tl.mInertia;
+		tr.mAngularVelocity += impulse * tr.mInertia;
+	}
+}
+
+void TrackedVehicleController::PostCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem)
+{
+	JPH_PROFILE_FUNCTION();
+
+	Wheels &wheels = mConstraint.GetWheels();
+
+	// Update wheel angle, do this before applying torque to the wheels (as friction will slow them down again)
+	for (uint wheel_index = 0, num_wheels = (uint)wheels.size(); wheel_index < num_wheels; ++wheel_index)
+	{
+		WheelTV *w = static_cast<WheelTV *>(wheels[wheel_index]);
+		w->Update(wheel_index, inDeltaTime, mConstraint);
+	}
+
+	// First calculate engine speed based on speed of all wheels
+	bool can_engine_apply_torque = false;
+	if (mTransmission.GetCurrentGear() != 0 && mTransmission.GetClutchFriction() > 1.0e-3f)
+	{
+		float transmission_ratio = mTransmission.GetCurrentRatio();
+		bool forward = transmission_ratio >= 0.0f;
+		float fastest_wheel_speed = forward? -FLT_MAX : FLT_MAX;
+		for (const VehicleTrack &t : mTracks)
+		{
+			if (forward)
+				fastest_wheel_speed = max(fastest_wheel_speed, t.mAngularVelocity * t.mDifferentialRatio);
+			else
+				fastest_wheel_speed = min(fastest_wheel_speed, t.mAngularVelocity * t.mDifferentialRatio);
+			for (uint w : t.mWheels)
+				if (wheels[w]->HasContact())
+				{
+					can_engine_apply_torque = true;
+					break;
+				}
+		}
+
+		// Update RPM only if the tracks are connected to the engine
+		if (fastest_wheel_speed > -FLT_MAX && fastest_wheel_speed < FLT_MAX)
+			mEngine.SetCurrentRPM(fastest_wheel_speed * mTransmission.GetCurrentRatio() * VehicleEngine::cAngularVelocityToRPM);
+	}
+	else
+	{
+		// Update engine with damping
+		mEngine.ApplyDamping(inDeltaTime);
+
+		// In auto transmission mode, don't accelerate the engine when switching gears
+		float forward_input = mTransmission.mMode == ETransmissionMode::Manual? abs(mForwardInput) : 0.0f;
+
+		// Engine not connected to wheels, update RPM based on engine inertia alone
+		mEngine.ApplyTorque(mEngine.GetTorque(forward_input), inDeltaTime);
+	}
+
+	// Update transmission
+	// Note: only allow switching gears up when the tracks are rolling in the same direction
+	mTransmission.Update(inDeltaTime, mEngine.GetCurrentRPM(), mForwardInput, mLeftRatio * mRightRatio > 0.0f && can_engine_apply_torque);
+
+	// Calculate the amount of torque the transmission gives to the differentials
+	float transmission_ratio = mTransmission.GetCurrentRatio();
+	float transmission_torque = mTransmission.GetClutchFriction() * transmission_ratio * mEngine.GetTorque(abs(mForwardInput));
+	if (transmission_torque != 0.0f)
+	{
+		// Apply the transmission torque to the wheels
+		for (uint i = 0; i < std::size(mTracks); ++i)
+		{
+			VehicleTrack &t = mTracks[i];
+
+			// Get wheel rotation ratio for this track
+			float ratio = i == 0? mLeftRatio : mRightRatio;
+
+			// Calculate the max angular velocity of the driven wheel of the track given current engine RPM
+			// Note this adds 0.1% slop to avoid numerical accuracy issues
+			float track_max_angular_velocity = mEngine.GetCurrentRPM() / (transmission_ratio * t.mDifferentialRatio * ratio * VehicleEngine::cAngularVelocityToRPM) * 1.001f;
+
+			// Calculate torque on the driven wheel
+			float differential_torque = t.mDifferentialRatio * ratio * transmission_torque;
+
+			// Apply torque to driven wheel
+			if (t.mAngularVelocity * track_max_angular_velocity < 0.0f || abs(t.mAngularVelocity) < abs(track_max_angular_velocity))
+				t.mAngularVelocity += differential_torque * inDeltaTime / t.mInertia;
+		}
+	}
+
+	// Ensure that we have the correct ratio between the two tracks
+	SyncLeftRightTracks();
+
+	// Braking
+	for (VehicleTrack &t : mTracks)
+	{
+		// Calculate brake torque
+		float brake_torque = mBrakeInput * t.mMaxBrakeTorque;
+		if (brake_torque > 0.0f)
+		{
+			// Calculate how much torque is needed to stop the track from rotating in this time step
+			float brake_torque_to_lock_track = abs(t.mAngularVelocity) * t.mInertia / inDeltaTime;
+			if (brake_torque > brake_torque_to_lock_track)
+			{
+				// Wheels are locked
+				t.mAngularVelocity = 0.0f;
+				brake_torque -= brake_torque_to_lock_track;
+			}
+			else
+			{
+				// Slow down the track
+				t.mAngularVelocity -= Sign(t.mAngularVelocity) * brake_torque * inDeltaTime / t.mInertia;
+			}
+		}
+
+		if (brake_torque > 0.0f)
+		{
+			// Sum the radius of all wheels touching the floor
+			float total_radius = 0.0f;
+			for (uint wheel_index : t.mWheels)
+			{
+				const WheelTV *w = static_cast<WheelTV *>(wheels[wheel_index]);
+
+				if (w->HasContact())
+					total_radius += w->GetSettings()->mRadius;
+			}
+
+			if (total_radius > 0.0f)
+			{
+				brake_torque /= total_radius;
+				for (uint wheel_index : t.mWheels)
+				{
+					WheelTV *w = static_cast<WheelTV *>(wheels[wheel_index]);
+					if (w->HasContact())
+					{
+						// Impulse: p = F * dt = Torque / Wheel_Radius * dt, Torque = Total_Torque * Wheel_Radius / Summed_Radius => p = Total_Torque * dt / Summed_Radius
+						w->mBrakeImpulse = brake_torque * inDeltaTime;
+					}
+				}
+			}
+		}
+	}
+
+	// Update wheel angular velocity based on that of the track
+	for (Wheel *w_base : wheels)
+	{
+		WheelTV *w = static_cast<WheelTV *>(w_base);
+		w->CalculateAngularVelocity(mConstraint);
+	}
+}
+
+bool TrackedVehicleController::SolveLongitudinalAndLateralConstraints(float inDeltaTime)
+{
+	bool impulse = false;
+
+	for (Wheel *w_base : mConstraint.GetWheels())
+		if (w_base->HasContact())
+		{
+			WheelTV *w = static_cast<WheelTV *>(w_base);
+			const WheelSettingsTV *settings = w->GetSettings();
+			VehicleTrack &track = mTracks[w->mTrackIndex];
+
+			// Calculate max impulse that we can apply on the ground
+			float max_longitudinal_friction_impulse = w->mCombinedLongitudinalFriction * w->GetSuspensionLambda();
+
+			// Calculate relative velocity between wheel contact point and floor in longitudinal direction
+			Vec3 relative_velocity = mConstraint.GetVehicleBody()->GetPointVelocity(w->GetContactPosition()) - w->GetContactPointVelocity();
+			float relative_longitudinal_velocity = relative_velocity.Dot(w->GetContactLongitudinal());
+
+			// Calculate brake force to apply
+			float min_longitudinal_impulse, max_longitudinal_impulse;
+			if (w->mBrakeImpulse != 0.0f)
+			{
+				// Limit brake force by max tire friction
+				float brake_impulse = min(w->mBrakeImpulse, max_longitudinal_friction_impulse);
+
+				// Check which direction the brakes should be applied (we don't want to apply an impulse that would accelerate the vehicle)
+				if (relative_longitudinal_velocity >= 0.0f)
+				{
+					min_longitudinal_impulse = -brake_impulse;
+					max_longitudinal_impulse = 0.0f;
+				}
+				else
+				{
+					min_longitudinal_impulse = 0.0f;
+					max_longitudinal_impulse = brake_impulse;
+				}
+
+				// Longitudinal impulse, note that we assume that once the wheels are locked that the brakes have more than enough torque to keep the wheels locked so we exclude any rotation deltas
+				impulse |= w->SolveLongitudinalConstraintPart(mConstraint, min_longitudinal_impulse, max_longitudinal_impulse);
+			}
+			else
+			{
+				// Assume we want to apply an angular impulse that makes the delta velocity between track and ground zero in one time step, calculate the amount of linear impulse needed to do that
+				float desired_angular_velocity = relative_longitudinal_velocity / settings->mRadius;
+				float linear_impulse = (track.mAngularVelocity - desired_angular_velocity) * track.mInertia / settings->mRadius;
+
+				// Limit the impulse by max track friction
+				float prev_lambda = w->GetLongitudinalLambda();
+				min_longitudinal_impulse = max_longitudinal_impulse = Clamp(prev_lambda + linear_impulse, -max_longitudinal_friction_impulse, max_longitudinal_friction_impulse);
+
+				// Longitudinal impulse
+				impulse |= w->SolveLongitudinalConstraintPart(mConstraint, min_longitudinal_impulse, max_longitudinal_impulse);
+
+				// Update the angular velocity of the track according to the lambda that was applied
+				track.mAngularVelocity -= (w->GetLongitudinalLambda() - prev_lambda) * settings->mRadius / track.mInertia;
+				SyncLeftRightTracks();
+			}
+		}
+
+	for (Wheel *w_base : mConstraint.GetWheels())
+		if (w_base->HasContact())
+		{
+			WheelTV *w = static_cast<WheelTV *>(w_base);
+
+			// Update angular velocity of wheel for the next iteration
+			w->CalculateAngularVelocity(mConstraint);
+
+			// Lateral friction
+			float max_lateral_friction_impulse = w->mCombinedLateralFriction * w->GetSuspensionLambda();
+			impulse |= w->SolveLateralConstraintPart(mConstraint, -max_lateral_friction_impulse, max_lateral_friction_impulse);
+		}
+
+	return impulse;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+
+void TrackedVehicleController::Draw(DebugRenderer *inRenderer) const
+{
+	float constraint_size = mConstraint.GetDrawConstraintSize();
+
+	// Draw RPM
+	Body *body = mConstraint.GetVehicleBody();
+	Vec3 rpm_meter_up = body->GetRotation() * mConstraint.GetLocalUp();
+	RVec3 rpm_meter_pos = body->GetPosition() + body->GetRotation() * mRPMMeterPosition;
+	Vec3 rpm_meter_fwd = body->GetRotation() * mConstraint.GetLocalForward();
+	mEngine.DrawRPM(inRenderer, rpm_meter_pos, rpm_meter_fwd, rpm_meter_up, mRPMMeterSize, mTransmission.mShiftDownRPM, mTransmission.mShiftUpRPM);
+
+	// Draw current vehicle state
+	String status = StringFormat("Forward: %.1f, LRatio: %.1f, RRatio: %.1f, Brake: %.1f\n"
+								 "Gear: %d, Clutch: %.1f, EngineRPM: %.0f, V: %.1f km/h",
+								 (double)mForwardInput, (double)mLeftRatio, (double)mRightRatio, (double)mBrakeInput,
+								 mTransmission.GetCurrentGear(), (double)mTransmission.GetClutchFriction(), (double)mEngine.GetCurrentRPM(), (double)body->GetLinearVelocity().Length() * 3.6);
+	inRenderer->DrawText3D(body->GetPosition(), status, Color::sWhite, constraint_size);
+
+	for (const VehicleTrack &t : mTracks)
+	{
+		const WheelTV *w = static_cast<const WheelTV *>(mConstraint.GetWheels()[t.mDrivenWheel]);
+		const WheelSettings *settings = w->GetSettings();
+
+		// Calculate where the suspension attaches to the body in world space
+		RVec3 ws_position = body->GetCenterOfMassPosition() + body->GetRotation() * (settings->mPosition - body->GetShape()->GetCenterOfMass());
+
+		DebugRenderer::sInstance->DrawText3D(ws_position, StringFormat("W: %.1f", (double)t.mAngularVelocity), Color::sWhite, constraint_size);
+	}
+
+	RMat44 body_transform = body->GetWorldTransform();
+
+	for (const Wheel *w_base : mConstraint.GetWheels())
+	{
+		const WheelTV *w = static_cast<const WheelTV *>(w_base);
+		const WheelSettings *settings = w->GetSettings();
+
+		// Calculate where the suspension attaches to the body in world space
+		RVec3 ws_position = body_transform * settings->mPosition;
+		Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection);
+
+		// Draw suspension
+		RVec3 min_suspension_pos = ws_position + ws_direction * settings->mSuspensionMinLength;
+		RVec3 max_suspension_pos = ws_position + ws_direction * settings->mSuspensionMaxLength;
+		inRenderer->DrawLine(ws_position, min_suspension_pos, Color::sRed);
+		inRenderer->DrawLine(min_suspension_pos, max_suspension_pos, Color::sGreen);
+
+		// Draw current length
+		RVec3 wheel_pos = ws_position + ws_direction * w->GetSuspensionLength();
+		inRenderer->DrawMarker(wheel_pos, w->GetSuspensionLength() < settings->mSuspensionMinLength? Color::sRed : Color::sGreen, constraint_size);
+
+		// Draw wheel basis
+		Vec3 wheel_forward, wheel_up, wheel_right;
+		mConstraint.GetWheelLocalBasis(w, wheel_forward, wheel_up, wheel_right);
+		wheel_forward = body_transform.Multiply3x3(wheel_forward);
+		wheel_up = body_transform.Multiply3x3(wheel_up);
+		wheel_right = body_transform.Multiply3x3(wheel_right);
+		Vec3 steering_axis = body_transform.Multiply3x3(settings->mSteeringAxis);
+		inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_forward, Color::sRed);
+		inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_up, Color::sGreen);
+		inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_right, Color::sBlue);
+		inRenderer->DrawLine(wheel_pos, wheel_pos + steering_axis, Color::sYellow);
+
+		// Draw wheel
+		RMat44 wheel_transform(Vec4(wheel_up, 0.0f), Vec4(wheel_right, 0.0f), Vec4(wheel_forward, 0.0f), wheel_pos);
+		wheel_transform.SetRotation(wheel_transform.GetRotation() * Mat44::sRotationY(-w->GetRotationAngle()));
+		inRenderer->DrawCylinder(wheel_transform, settings->mWidth * 0.5f, settings->mRadius, w->GetSuspensionLength() <= settings->mSuspensionMinLength? Color::sRed : Color::sGreen, DebugRenderer::ECastShadow::Off, DebugRenderer::EDrawMode::Wireframe);
+
+		if (w->HasContact())
+		{
+			// Draw contact
+			inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactNormal(), Color::sYellow);
+			inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactLongitudinal(), Color::sRed);
+			inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactLateral(), Color::sBlue);
+
+			DebugRenderer::sInstance->DrawText3D(w->GetContactPosition(), StringFormat("S: %.2f", (double)w->GetSuspensionLength()), Color::sWhite, constraint_size);
+		}
+	}
+}
+
+#endif // JPH_DEBUG_RENDERER
+
+void TrackedVehicleController::SaveState(StateRecorder &inStream) const
+{
+	inStream.Write(mForwardInput);
+	inStream.Write(mLeftRatio);
+	inStream.Write(mRightRatio);
+	inStream.Write(mBrakeInput);
+
+	mEngine.SaveState(inStream);
+	mTransmission.SaveState(inStream);
+
+	for (const VehicleTrack &t : mTracks)
+		t.SaveState(inStream);
+}
+
+void TrackedVehicleController::RestoreState(StateRecorder &inStream)
+{
+	inStream.Read(mForwardInput);
+	inStream.Read(mLeftRatio);
+	inStream.Read(mRightRatio);
+	inStream.Read(mBrakeInput);
+
+	mEngine.RestoreState(inStream);
+	mTransmission.RestoreState(inStream);
+
+	for (VehicleTrack &t : mTracks)
+		t.RestoreState(inStream);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/TrackedVehicleController.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/TrackedVehicleController.h
new file mode 100644
index 0000000000..8567c97073
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/TrackedVehicleController.h
@@ -0,0 +1,166 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Vehicle/VehicleConstraint.h>
+#include <Jolt/Physics/Vehicle/VehicleController.h>
+#include <Jolt/Physics/Vehicle/VehicleEngine.h>
+#include <Jolt/Physics/Vehicle/VehicleTransmission.h>
+#include <Jolt/Physics/Vehicle/VehicleTrack.h>
+
+JPH_NAMESPACE_BEGIN
+
+class PhysicsSystem;
+
+/// WheelSettings object specifically for TrackedVehicleController
+class JPH_EXPORT WheelSettingsTV : public WheelSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, WheelSettingsTV)
+
+public:
+	// See: WheelSettings
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+
+	float						mLongitudinalFriction = 4.0f;				///< Friction in forward direction of tire
+	float						mLateralFriction = 2.0f;					///< Friction in sideway direction of tire
+};
+
+/// Wheel object specifically for TrackedVehicleController
+class JPH_EXPORT WheelTV : public Wheel
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+	explicit					WheelTV(const WheelSettingsTV &inWheel);
+
+	/// Override GetSettings and cast to the correct class
+	const WheelSettingsTV *		GetSettings() const							{ return StaticCast<WheelSettingsTV>(mSettings); }
+
+	/// Update the angular velocity of the wheel based on the angular velocity of the track
+	void						CalculateAngularVelocity(const VehicleConstraint &inConstraint);
+
+	/// Update the wheel rotation based on the current angular velocity
+	void						Update(uint inWheelIndex, float inDeltaTime, const VehicleConstraint &inConstraint);
+
+	int							mTrackIndex = -1;							///< Index in mTracks to which this wheel is attached (calculated on initialization)
+	float						mCombinedLongitudinalFriction = 0.0f;		///< Combined friction coefficient in longitudinal direction (combines terrain and track)
+	float						mCombinedLateralFriction = 0.0f;			///< Combined friction coefficient in lateral direction (combines terrain and track)
+	float						mBrakeImpulse = 0.0f;						///< Amount of impulse that the brakes can apply to the floor (excluding friction), spread out from brake impulse applied on track
+};
+
+/// Settings of a vehicle with tank tracks
+///
+/// Default settings are based around what I could find about the M1 Abrams tank.
+/// Note to avoid issues with very heavy objects vs very light objects the mass of the tank should be a lot lower (say 10x) than that of a real tank. That means that the engine/brake torque is also 10x less.
+class JPH_EXPORT TrackedVehicleControllerSettings : public VehicleControllerSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, TrackedVehicleControllerSettings)
+
+public:
+	// Constructor
+								TrackedVehicleControllerSettings();
+
+	// See: VehicleControllerSettings
+	virtual VehicleController *	ConstructController(VehicleConstraint &inConstraint) const override;
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+
+	VehicleEngineSettings		mEngine;									///< The properties of the engine
+	VehicleTransmissionSettings	mTransmission;								///< The properties of the transmission (aka gear box)
+	VehicleTrackSettings		mTracks[(int)ETrackSide::Num];				///< List of tracks and their properties
+};
+
+/// Runtime controller class for vehicle with tank tracks
+class JPH_EXPORT TrackedVehicleController : public VehicleController
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+								TrackedVehicleController(const TrackedVehicleControllerSettings &inSettings, VehicleConstraint &inConstraint);
+
+	/// Set input from driver
+	/// @param inForward Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed
+	/// @param inLeftRatio Value between -1 and 1 indicating an extra multiplier to the rotation rate of the left track (used for steering)
+	/// @param inRightRatio Value between -1 and 1 indicating an extra multiplier to the rotation rate of the right track (used for steering)
+	/// @param inBrake Value between 0 and 1 indicating how strong the brake pedal is pressed
+	void						SetDriverInput(float inForward, float inLeftRatio, float inRightRatio, float inBrake) { JPH_ASSERT(inLeftRatio != 0.0f && inRightRatio != 0.0f); mForwardInput = inForward; mLeftRatio = inLeftRatio; mRightRatio = inRightRatio; mBrakeInput = inBrake; }
+
+	/// Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed
+	void						SetForwardInput(float inForward)			{ mForwardInput = inForward; }
+	float						GetForwardInput() const						{ return mForwardInput; }
+
+	/// Value between -1 and 1 indicating an extra multiplier to the rotation rate of the left track (used for steering)
+	void						SetLeftRatio(float inLeftRatio)				{ JPH_ASSERT(inLeftRatio != 0.0f); mLeftRatio = inLeftRatio; }
+	float						GetLeftRatio() const						{ return mLeftRatio; }
+
+	/// Value between -1 and 1 indicating an extra multiplier to the rotation rate of the right track (used for steering)
+	void						SetRightRatio(float inRightRatio)			{ JPH_ASSERT(inRightRatio != 0.0f); mRightRatio = inRightRatio; }
+	float						GetRightRatio() const						{ return mRightRatio; }
+
+	/// Value between 0 and 1 indicating how strong the brake pedal is pressed
+	void						SetBrakeInput(float inBrake)				{ mBrakeInput = inBrake; }
+	float						GetBrakeInput() const						{ return mBrakeInput; }
+
+	/// Get current engine state
+	const VehicleEngine &		GetEngine() const							{ return mEngine; }
+
+	/// Get current engine state (writable interface, allows you to make changes to the configuration which will take effect the next time step)
+	VehicleEngine &				GetEngine()									{ return mEngine; }
+
+	/// Get current transmission state
+	const VehicleTransmission &	GetTransmission() const						{ return mTransmission; }
+
+	/// Get current transmission state (writable interface, allows you to make changes to the configuration which will take effect the next time step)
+	VehicleTransmission &		GetTransmission()							{ return mTransmission; }
+
+	/// Get the tracks this vehicle has
+	const VehicleTracks	&		GetTracks() const							{ return mTracks; }
+
+	/// Get the tracks this vehicle has (writable interface, allows you to make changes to the configuration which will take effect the next time step)
+	VehicleTracks &				GetTracks()									{ return mTracks; }
+
+#ifdef JPH_DEBUG_RENDERER
+	/// Debug drawing of RPM meter
+	void						SetRPMMeter(Vec3Arg inPosition, float inSize) { mRPMMeterPosition = inPosition; mRPMMeterSize = inSize; }
+#endif // JPH_DEBUG_RENDERER
+
+protected:
+	/// Synchronize angular velocities of left and right tracks according to their ratios
+	void						SyncLeftRightTracks();
+
+	// See: VehicleController
+	virtual Wheel *				ConstructWheel(const WheelSettings &inWheel) const override { JPH_ASSERT(IsKindOf(&inWheel, JPH_RTTI(WheelSettingsTV))); return new WheelTV(static_cast<const WheelSettingsTV &>(inWheel)); }
+	virtual bool				AllowSleep() const override;
+	virtual void				PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override;
+	virtual void				PostCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override;
+	virtual bool				SolveLongitudinalAndLateralConstraints(float inDeltaTime) override;
+	virtual void				SaveState(StateRecorder &inStream) const override;
+	virtual void				RestoreState(StateRecorder &inStream) override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void				Draw(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// Control information
+	float						mForwardInput = 0.0f;						///< Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed
+	float						mLeftRatio = 1.0f;							///< Value between -1 and 1 indicating an extra multiplier to the rotation rate of the left track (used for steering)
+	float						mRightRatio = 1.0f;							///< Value between -1 and 1 indicating an extra multiplier to the rotation rate of the right track (used for steering)
+	float						mBrakeInput = 0.0f;							///< Value between 0 and 1 indicating how strong the brake pedal is pressed
+
+	// Simulation information
+	VehicleEngine				mEngine;									///< Engine state of the vehicle
+	VehicleTransmission			mTransmission;								///< Transmission state of the vehicle
+	VehicleTracks				mTracks;									///< Tracks of the vehicle
+
+#ifdef JPH_DEBUG_RENDERER
+	// Debug settings
+	Vec3						mRPMMeterPosition { 0, 1, 0 };				///< Position (in local space of the body) of the RPM meter when drawing the constraint
+	float						mRPMMeterSize = 0.5f;						///< Size of the RPM meter when drawing the constraint
+#endif // JPH_DEBUG_RENDERER
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleAntiRollBar.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleAntiRollBar.cpp
new file mode 100644
index 0000000000..859bcafa17
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleAntiRollBar.cpp
@@ -0,0 +1,33 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Vehicle/VehicleAntiRollBar.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(VehicleAntiRollBar)
+{
+	JPH_ADD_ATTRIBUTE(VehicleAntiRollBar, mLeftWheel)
+	JPH_ADD_ATTRIBUTE(VehicleAntiRollBar, mRightWheel)
+	JPH_ADD_ATTRIBUTE(VehicleAntiRollBar, mStiffness)
+}
+
+void VehicleAntiRollBar::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(mLeftWheel);
+	inStream.Write(mRightWheel);
+	inStream.Write(mStiffness);
+}
+
+void VehicleAntiRollBar::RestoreBinaryState(StreamIn &inStream)
+{
+	inStream.Read(mLeftWheel);
+	inStream.Read(mRightWheel);
+	inStream.Read(mStiffness);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleAntiRollBar.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleAntiRollBar.h
new file mode 100644
index 0000000000..4a9c6707d4
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleAntiRollBar.h
@@ -0,0 +1,31 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/ObjectStream/SerializableObject.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// An anti rollbar is a stiff spring that connects two wheels to reduce the amount of roll the vehicle makes in sharp corners
+/// See: https://en.wikipedia.org/wiki/Anti-roll_bar
+class JPH_EXPORT VehicleAntiRollBar
+{
+	JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, VehicleAntiRollBar)
+
+public:
+	/// Saves the contents in binary form to inStream.
+	void					SaveBinaryState(StreamOut &inStream) const;
+
+	/// Restores the contents in binary form to inStream.
+	void					RestoreBinaryState(StreamIn &inStream);
+
+	int						mLeftWheel = 0;								///< Index (in mWheels) that represents the left wheel of this anti-rollbar
+	int						mRightWheel = 1;							///< Index (in mWheels) that represents the right wheel of this anti-rollbar
+	float					mStiffness = 1000.0f;						///< Stiffness (spring constant in N/m) of anti rollbar, can be 0 to disable the anti-rollbar
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleCollisionTester.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleCollisionTester.cpp
new file mode 100644
index 0000000000..5624683579
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleCollisionTester.cpp
@@ -0,0 +1,376 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Vehicle/VehicleCollisionTester.h>
+#include <Jolt/Physics/Vehicle/VehicleConstraint.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/ShapeCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/CylinderShape.h>
+#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
+#include <Jolt/Physics/PhysicsSystem.h>
+
+JPH_NAMESPACE_BEGIN
+
+bool VehicleCollisionTesterRay::Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const
+{
+	const DefaultBroadPhaseLayerFilter default_broadphase_layer_filter = inPhysicsSystem.GetDefaultBroadPhaseLayerFilter(mObjectLayer);
+	const BroadPhaseLayerFilter &broadphase_layer_filter = mBroadPhaseLayerFilter != nullptr? *mBroadPhaseLayerFilter : default_broadphase_layer_filter;
+
+	const DefaultObjectLayerFilter default_object_layer_filter = inPhysicsSystem.GetDefaultLayerFilter(mObjectLayer);
+	const ObjectLayerFilter &object_layer_filter = mObjectLayerFilter != nullptr? *mObjectLayerFilter : default_object_layer_filter;
+
+	const IgnoreSingleBodyFilter default_body_filter(inVehicleBodyID);
+	const BodyFilter &body_filter = mBodyFilter != nullptr? *mBodyFilter : default_body_filter;
+
+	const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings();
+	float wheel_radius = wheel_settings->mRadius;
+	float ray_length = wheel_settings->mSuspensionMaxLength + wheel_radius;
+	RRayCast ray { inOrigin, ray_length * inDirection };
+
+	class MyCollector : public CastRayCollector
+	{
+	public:
+							MyCollector(PhysicsSystem &inPhysicsSystem, const RRayCast &inRay, Vec3Arg inUpDirection, float inCosMaxSlopeAngle) :
+			mPhysicsSystem(inPhysicsSystem),
+			mRay(inRay),
+			mUpDirection(inUpDirection),
+			mCosMaxSlopeAngle(inCosMaxSlopeAngle)
+		{
+		}
+
+		virtual void		AddHit(const RayCastResult &inResult) override
+		{
+			// Test if this collision is closer than the previous one
+			if (inResult.mFraction < GetEarlyOutFraction())
+			{
+				// Lock the body
+				BodyLockRead lock(mPhysicsSystem.GetBodyLockInterfaceNoLock(), inResult.mBodyID);
+				JPH_ASSERT(lock.Succeeded()); // When this runs all bodies are locked so this should not fail
+				const Body *body = &lock.GetBody();
+
+				if (body->IsSensor())
+					return;
+
+				// Test that we're not hitting a vertical wall
+				RVec3 contact_pos = mRay.GetPointOnRay(inResult.mFraction);
+				Vec3 normal = body->GetWorldSpaceSurfaceNormal(inResult.mSubShapeID2, contact_pos);
+				if (normal.Dot(mUpDirection) > mCosMaxSlopeAngle)
+				{
+					// Update early out fraction to this hit
+					UpdateEarlyOutFraction(inResult.mFraction);
+
+					// Get the contact properties
+					mBody = body;
+					mSubShapeID2 = inResult.mSubShapeID2;
+					mContactPosition = contact_pos;
+					mContactNormal = normal;
+				}
+			}
+		}
+
+		// Configuration
+		PhysicsSystem &		mPhysicsSystem;
+		RRayCast			mRay;
+		Vec3				mUpDirection;
+		float				mCosMaxSlopeAngle;
+
+		// Resulting closest collision
+		const Body *		mBody = nullptr;
+		SubShapeID			mSubShapeID2;
+		RVec3				mContactPosition;
+		Vec3				mContactNormal;
+	};
+
+	RayCastSettings settings;
+
+	MyCollector collector(inPhysicsSystem, ray, mUp, mCosMaxSlopeAngle);
+	inPhysicsSystem.GetNarrowPhaseQueryNoLock().CastRay(ray, settings, collector, broadphase_layer_filter, object_layer_filter, body_filter);
+	if (collector.mBody == nullptr)
+		return false;
+
+	outBody = const_cast<Body *>(collector.mBody);
+	outSubShapeID = collector.mSubShapeID2;
+	outContactPosition = collector.mContactPosition;
+	outContactNormal = collector.mContactNormal;
+	outSuspensionLength = max(0.0f, ray_length * collector.GetEarlyOutFraction() - wheel_radius);
+
+	return true;
+}
+
+void VehicleCollisionTesterRay::PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const
+{
+	// Recalculate the contact points assuming the contact point is on an infinite plane
+	const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings();
+	float d_dot_n = inDirection.Dot(ioContactNormal);
+	if (d_dot_n < -1.0e-6f)
+	{
+		// Reproject the contact position using the suspension ray and the plane formed by the contact position and normal
+		ioContactPosition = inOrigin + Vec3(ioContactPosition - inOrigin).Dot(ioContactNormal) / d_dot_n * inDirection;
+
+		// The suspension length is simply the distance between the contact position and the suspension origin excluding the wheel radius
+		ioSuspensionLength = Clamp(Vec3(ioContactPosition - inOrigin).Dot(inDirection) - wheel_settings->mRadius, 0.0f, wheel_settings->mSuspensionMaxLength);
+	}
+	else
+	{
+		// If the normal is pointing away we assume there's no collision anymore
+		ioSuspensionLength = wheel_settings->mSuspensionMaxLength;
+	}
+}
+
+bool VehicleCollisionTesterCastSphere::Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const
+{
+	const DefaultBroadPhaseLayerFilter default_broadphase_layer_filter = inPhysicsSystem.GetDefaultBroadPhaseLayerFilter(mObjectLayer);
+	const BroadPhaseLayerFilter &broadphase_layer_filter = mBroadPhaseLayerFilter != nullptr? *mBroadPhaseLayerFilter : default_broadphase_layer_filter;
+
+	const DefaultObjectLayerFilter default_object_layer_filter = inPhysicsSystem.GetDefaultLayerFilter(mObjectLayer);
+	const ObjectLayerFilter &object_layer_filter = mObjectLayerFilter != nullptr? *mObjectLayerFilter : default_object_layer_filter;
+
+	const IgnoreSingleBodyFilter default_body_filter(inVehicleBodyID);
+	const BodyFilter &body_filter = mBodyFilter != nullptr? *mBodyFilter : default_body_filter;
+
+	SphereShape sphere(mRadius);
+	sphere.SetEmbedded();
+
+	const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings();
+	float wheel_radius = wheel_settings->mRadius;
+	float shape_cast_length = wheel_settings->mSuspensionMaxLength + wheel_radius - mRadius;
+	RShapeCast shape_cast(&sphere, Vec3::sReplicate(1.0f), RMat44::sTranslation(inOrigin), inDirection * shape_cast_length);
+
+	ShapeCastSettings settings;
+	settings.mUseShrunkenShapeAndConvexRadius = true;
+	settings.mReturnDeepestPoint = true;
+
+	class MyCollector : public CastShapeCollector
+	{
+	public:
+							MyCollector(PhysicsSystem &inPhysicsSystem, const RShapeCast &inShapeCast, Vec3Arg inUpDirection, float inCosMaxSlopeAngle) :
+			mPhysicsSystem(inPhysicsSystem),
+			mShapeCast(inShapeCast),
+			mUpDirection(inUpDirection),
+			mCosMaxSlopeAngle(inCosMaxSlopeAngle)
+		{
+		}
+
+		virtual void		AddHit(const ShapeCastResult &inResult) override
+		{
+			// Test if this collision is closer/deeper than the previous one
+			float early_out = inResult.GetEarlyOutFraction();
+			if (early_out < GetEarlyOutFraction())
+			{
+				// Lock the body
+				BodyLockRead lock(mPhysicsSystem.GetBodyLockInterfaceNoLock(), inResult.mBodyID2);
+				JPH_ASSERT(lock.Succeeded()); // When this runs all bodies are locked so this should not fail
+				const Body *body = &lock.GetBody();
+
+				if (body->IsSensor())
+					return;
+
+				// Test that we're not hitting a vertical wall
+				Vec3 normal = -inResult.mPenetrationAxis.Normalized();
+				if (normal.Dot(mUpDirection) > mCosMaxSlopeAngle)
+				{
+					// Update early out fraction to this hit
+					UpdateEarlyOutFraction(early_out);
+
+					// Get the contact properties
+					mBody = body;
+					mSubShapeID2 = inResult.mSubShapeID2;
+					mContactPosition = mShapeCast.mCenterOfMassStart.GetTranslation() + inResult.mContactPointOn2;
+					mContactNormal = normal;
+					mFraction = inResult.mFraction;
+				}
+			}
+		}
+
+		// Configuration
+		PhysicsSystem &		mPhysicsSystem;
+		const RShapeCast &	mShapeCast;
+		Vec3				mUpDirection;
+		float				mCosMaxSlopeAngle;
+
+		// Resulting closest collision
+		const Body *		mBody = nullptr;
+		SubShapeID			mSubShapeID2;
+		RVec3				mContactPosition;
+		Vec3				mContactNormal;
+		float				mFraction;
+	};
+
+	MyCollector collector(inPhysicsSystem, shape_cast, mUp, mCosMaxSlopeAngle);
+	inPhysicsSystem.GetNarrowPhaseQueryNoLock().CastShape(shape_cast, settings, shape_cast.mCenterOfMassStart.GetTranslation(), collector, broadphase_layer_filter, object_layer_filter, body_filter);
+	if (collector.mBody == nullptr)
+		return false;
+
+	outBody = const_cast<Body *>(collector.mBody);
+	outSubShapeID = collector.mSubShapeID2;
+	outContactPosition = collector.mContactPosition;
+	outContactNormal = collector.mContactNormal;
+	outSuspensionLength = max(0.0f, shape_cast_length * collector.mFraction + mRadius - wheel_radius);
+
+	return true;
+}
+
+void VehicleCollisionTesterCastSphere::PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const
+{
+	// Recalculate the contact points assuming the contact point is on an infinite plane
+	const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings();
+	float d_dot_n = inDirection.Dot(ioContactNormal);
+	if (d_dot_n < -1.0e-6f)
+	{
+		// Reproject the contact position using the suspension cast sphere and the plane formed by the contact position and normal
+		// This solves x = inOrigin + fraction * inDirection and (x - ioContactPosition) . ioContactNormal = mRadius for fraction
+		float oc_dot_n = Vec3(ioContactPosition - inOrigin).Dot(ioContactNormal);
+		float fraction = (mRadius + oc_dot_n) / d_dot_n;
+		ioContactPosition = inOrigin + fraction * inDirection - mRadius * ioContactNormal;
+
+		// Calculate the new suspension length in the same way as the cast sphere normally does
+		ioSuspensionLength = Clamp(fraction + mRadius - wheel_settings->mRadius, 0.0f, wheel_settings->mSuspensionMaxLength);
+	}
+	else
+	{
+		// If the normal is pointing away we assume there's no collision anymore
+		ioSuspensionLength = wheel_settings->mSuspensionMaxLength;
+	}
+}
+
+bool VehicleCollisionTesterCastCylinder::Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const
+{
+	const DefaultBroadPhaseLayerFilter default_broadphase_layer_filter = inPhysicsSystem.GetDefaultBroadPhaseLayerFilter(mObjectLayer);
+	const BroadPhaseLayerFilter &broadphase_layer_filter = mBroadPhaseLayerFilter != nullptr? *mBroadPhaseLayerFilter : default_broadphase_layer_filter;
+
+	const DefaultObjectLayerFilter default_object_layer_filter = inPhysicsSystem.GetDefaultLayerFilter(mObjectLayer);
+	const ObjectLayerFilter &object_layer_filter = mObjectLayerFilter != nullptr? *mObjectLayerFilter : default_object_layer_filter;
+
+	const IgnoreSingleBodyFilter default_body_filter(inVehicleBodyID);
+	const BodyFilter &body_filter = mBodyFilter != nullptr? *mBodyFilter : default_body_filter;
+
+	const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings();
+	float max_suspension_length = wheel_settings->mSuspensionMaxLength;
+
+	// Get the wheel transform given that the cylinder rotates around the Y axis
+	RMat44 shape_cast_start = inVehicleConstraint.GetWheelWorldTransform(inWheelIndex, Vec3::sAxisY(), Vec3::sAxisX());
+	shape_cast_start.SetTranslation(inOrigin);
+
+	// Construct a cylinder with the dimensions of the wheel
+	float wheel_half_width = 0.5f * wheel_settings->mWidth;
+	CylinderShape cylinder(wheel_half_width, wheel_settings->mRadius, min(wheel_half_width, wheel_settings->mRadius) * mConvexRadiusFraction);
+	cylinder.SetEmbedded();
+
+	RShapeCast shape_cast(&cylinder, Vec3::sReplicate(1.0f), shape_cast_start, inDirection * max_suspension_length);
+
+	ShapeCastSettings settings;
+	settings.mUseShrunkenShapeAndConvexRadius = true;
+	settings.mReturnDeepestPoint = true;
+
+	class MyCollector : public CastShapeCollector
+	{
+	public:
+							MyCollector(PhysicsSystem &inPhysicsSystem, const RShapeCast &inShapeCast) :
+			mPhysicsSystem(inPhysicsSystem),
+			mShapeCast(inShapeCast)
+		{
+		}
+
+		virtual void		AddHit(const ShapeCastResult &inResult) override
+		{
+			// Test if this collision is closer/deeper than the previous one
+			float early_out = inResult.GetEarlyOutFraction();
+			if (early_out < GetEarlyOutFraction())
+			{
+				// Lock the body
+				BodyLockRead lock(mPhysicsSystem.GetBodyLockInterfaceNoLock(), inResult.mBodyID2);
+				JPH_ASSERT(lock.Succeeded()); // When this runs all bodies are locked so this should not fail
+				const Body *body = &lock.GetBody();
+
+				if (body->IsSensor())
+					return;
+
+				// Update early out fraction to this hit
+				UpdateEarlyOutFraction(early_out);
+
+				// Get the contact properties
+				mBody = body;
+				mSubShapeID2 = inResult.mSubShapeID2;
+				mContactPosition = mShapeCast.mCenterOfMassStart.GetTranslation() + inResult.mContactPointOn2;
+				mContactNormal = -inResult.mPenetrationAxis.Normalized();
+				mFraction = inResult.mFraction;
+			}
+		}
+
+		// Configuration
+		PhysicsSystem &		mPhysicsSystem;
+		const RShapeCast &	mShapeCast;
+
+		// Resulting closest collision
+		const Body *		mBody = nullptr;
+		SubShapeID			mSubShapeID2;
+		RVec3				mContactPosition;
+		Vec3				mContactNormal;
+		float				mFraction;
+	};
+
+	MyCollector collector(inPhysicsSystem, shape_cast);
+	inPhysicsSystem.GetNarrowPhaseQueryNoLock().CastShape(shape_cast, settings, shape_cast.mCenterOfMassStart.GetTranslation(), collector, broadphase_layer_filter, object_layer_filter, body_filter);
+	if (collector.mBody == nullptr)
+		return false;
+
+	outBody = const_cast<Body *>(collector.mBody);
+	outSubShapeID = collector.mSubShapeID2;
+	outContactPosition = collector.mContactPosition;
+	outContactNormal = collector.mContactNormal;
+	outSuspensionLength = max_suspension_length * collector.mFraction;
+
+	return true;
+}
+
+void VehicleCollisionTesterCastCylinder::PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const
+{
+	// Recalculate the contact points assuming the contact point is on an infinite plane
+	const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings();
+	float d_dot_n = inDirection.Dot(ioContactNormal);
+	if (d_dot_n < -1.0e-6f)
+	{
+		// Wheel size
+		float half_width = 0.5f * wheel_settings->mWidth;
+		float radius = wheel_settings->mRadius;
+
+		// Get the inverse local space contact normal for a cylinder pointing along Y
+		RMat44 wheel_transform = inVehicleConstraint.GetWheelWorldTransform(inWheelIndex, Vec3::sAxisY(), Vec3::sAxisX());
+		Vec3 inverse_local_normal = -wheel_transform.Multiply3x3Transposed(ioContactNormal);
+
+		// Get the support point of this normal in local space of the cylinder
+		// See CylinderShape::Cylinder::GetSupport
+		float x = inverse_local_normal.GetX(), y = inverse_local_normal.GetY(), z = inverse_local_normal.GetZ();
+		float o = sqrt(Square(x) + Square(z));
+		Vec3 support_point;
+		if (o > 0.0f)
+			support_point = Vec3((radius * x) / o, Sign(y) * half_width, (radius * z) / o);
+		else
+			support_point = Vec3(0, Sign(y) * half_width, 0);
+
+		// Rotate back to world space
+		support_point = wheel_transform.Multiply3x3(support_point);
+
+		// Now we can use inOrigin + support_point as the start of a ray of our suspension to the contact plane
+		// as know that it is the first point on the wheel that will hit the plane
+		RVec3 origin = inOrigin + support_point;
+
+		// Calculate contact position and suspension length, the is the same as VehicleCollisionTesterRay
+		// but we don't need to take the radius into account anymore
+		Vec3 oc(ioContactPosition - origin);
+		ioContactPosition = origin + oc.Dot(ioContactNormal) / d_dot_n * inDirection;
+		ioSuspensionLength = Clamp(oc.Dot(inDirection), 0.0f, wheel_settings->mSuspensionMaxLength);
+	}
+	else
+	{
+		// If the normal is pointing away we assume there's no collision anymore
+		ioSuspensionLength = wheel_settings->mSuspensionMaxLength;
+	}
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleCollisionTester.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleCollisionTester.h
new file mode 100644
index 0000000000..7c222756c8
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleCollisionTester.h
@@ -0,0 +1,146 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Core/NonCopyable.h>
+
+JPH_NAMESPACE_BEGIN
+
+class PhysicsSystem;
+class VehicleConstraint;
+class BroadPhaseLayerFilter;
+class ObjectLayerFilter;
+class BodyFilter;
+
+/// Class that does collision detection between wheels and ground
+class JPH_EXPORT VehicleCollisionTester : public RefTarget<VehicleCollisionTester>, public NonCopyable
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructors
+									VehicleCollisionTester() = default;
+	explicit						VehicleCollisionTester(ObjectLayer inObjectLayer) : mObjectLayer(inObjectLayer) { }
+
+	/// Virtual destructor
+	virtual							~VehicleCollisionTester() = default;
+
+	/// Object layer to use for collision detection, this is used when the filters are not overridden
+	ObjectLayer						GetObjectLayer() const												{ return mObjectLayer; }
+	void							SetObjectLayer(ObjectLayer inObjectLayer)							{ mObjectLayer = inObjectLayer; }
+
+	/// Access to the broad phase layer filter, when set this overrides the object layer supplied in the constructor
+	void							SetBroadPhaseLayerFilter(const BroadPhaseLayerFilter *inFilter)		{ mBroadPhaseLayerFilter = inFilter; }
+	const BroadPhaseLayerFilter *	GetBroadPhaseLayerFilter() const									{ return mBroadPhaseLayerFilter; }
+
+	/// Access to the object layer filter, when set this overrides the object layer supplied in the constructor
+	void							SetObjectLayerFilter(const ObjectLayerFilter *inFilter)				{ mObjectLayerFilter = inFilter; }
+	const ObjectLayerFilter *		GetObjectLayerFilter() const										{ return mObjectLayerFilter; }
+
+	/// Access to the body filter, when set this overrides the default filter that filters out the vehicle body
+	void							SetBodyFilter(const BodyFilter *inFilter)							{ mBodyFilter = inFilter; }
+	const BodyFilter *				GetBodyFilter() const												{ return mBodyFilter; }
+
+	/// Do a collision test with the world
+	/// @param inPhysicsSystem The physics system that should be tested against
+	/// @param inVehicleConstraint The vehicle constraint
+	/// @param inWheelIndex Index of the wheel that we're testing collision for
+	/// @param inOrigin Origin for the test, corresponds to the world space position for the suspension attachment point
+	/// @param inDirection Direction for the test (unit vector, world space)
+	/// @param inVehicleBodyID This body should be filtered out during collision detection to avoid self collisions
+	/// @param outBody Body that the wheel collided with
+	/// @param outSubShapeID Sub shape ID that the wheel collided with
+	/// @param outContactPosition Contact point between wheel and floor, in world space
+	/// @param outContactNormal Contact normal between wheel and floor, pointing away from the floor
+	/// @param outSuspensionLength New length of the suspension [0, inSuspensionMaxLength]
+	/// @return True when collision found, false if not
+	virtual bool					Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const = 0;
+
+	/// Do a cheap contact properties prediction based on the contact properties from the last collision test (provided as input parameters)
+	/// @param inPhysicsSystem The physics system that should be tested against
+	/// @param inVehicleConstraint The vehicle constraint
+	/// @param inWheelIndex Index of the wheel that we're testing collision for
+	/// @param inOrigin Origin for the test, corresponds to the world space position for the suspension attachment point
+	/// @param inDirection Direction for the test (unit vector, world space)
+	/// @param inVehicleBodyID The body ID for the vehicle itself
+	/// @param ioBody Body that the wheel previously collided with
+	/// @param ioSubShapeID Sub shape ID that the wheel collided with during the last check
+	/// @param ioContactPosition Contact point between wheel and floor during the last check, in world space
+	/// @param ioContactNormal Contact normal between wheel and floor during the last check, pointing away from the floor
+	/// @param ioSuspensionLength New length of the suspension [0, inSuspensionMaxLength]
+	virtual void					PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const = 0;
+
+protected:
+	const BroadPhaseLayerFilter	*	mBroadPhaseLayerFilter = nullptr;
+	const ObjectLayerFilter *		mObjectLayerFilter = nullptr;
+	const BodyFilter *				mBodyFilter = nullptr;
+	ObjectLayer						mObjectLayer = cObjectLayerInvalid;
+};
+
+/// Collision tester that tests collision using a raycast
+class JPH_EXPORT VehicleCollisionTesterRay : public VehicleCollisionTester
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+	/// @param inObjectLayer Object layer to test collision with
+	/// @param inUp World space up vector, used to avoid colliding with vertical walls.
+	/// @param inMaxSlopeAngle Max angle (rad) that is considered for colliding wheels. This is to avoid colliding with vertical walls.
+									VehicleCollisionTesterRay(ObjectLayer inObjectLayer, Vec3Arg inUp = Vec3::sAxisY(), float inMaxSlopeAngle = DegreesToRadians(80.0f)) : VehicleCollisionTester(inObjectLayer), mUp(inUp), mCosMaxSlopeAngle(Cos(inMaxSlopeAngle)) { }
+
+	// See: VehicleCollisionTester
+	virtual bool					Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const override;
+	virtual void					PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const override;
+
+private:
+	Vec3							mUp;
+	float							mCosMaxSlopeAngle;
+};
+
+/// Collision tester that tests collision using a sphere cast
+class JPH_EXPORT VehicleCollisionTesterCastSphere : public VehicleCollisionTester
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+	/// @param inObjectLayer Object layer to test collision with
+	/// @param inUp World space up vector, used to avoid colliding with vertical walls.
+	/// @param inRadius Radius of sphere
+	/// @param inMaxSlopeAngle Max angle (rad) that is considered for colliding wheels. This is to avoid colliding with vertical walls.
+									VehicleCollisionTesterCastSphere(ObjectLayer inObjectLayer, float inRadius, Vec3Arg inUp = Vec3::sAxisY(), float inMaxSlopeAngle = DegreesToRadians(80.0f)) : VehicleCollisionTester(inObjectLayer), mRadius(inRadius), mUp(inUp), mCosMaxSlopeAngle(Cos(inMaxSlopeAngle)) { }
+
+	// See: VehicleCollisionTester
+	virtual bool					Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const override;
+	virtual void					PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const override;
+
+private:
+	float							mRadius;
+	Vec3							mUp;
+	float							mCosMaxSlopeAngle;
+};
+
+/// Collision tester that tests collision using a cylinder shape
+class JPH_EXPORT VehicleCollisionTesterCastCylinder : public VehicleCollisionTester
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+	/// @param inObjectLayer Object layer to test collision with
+	/// @param inConvexRadiusFraction Fraction of half the wheel width (or wheel radius if it is smaller) that is used as the convex radius
+									VehicleCollisionTesterCastCylinder(ObjectLayer inObjectLayer, float inConvexRadiusFraction = 0.1f) : VehicleCollisionTester(inObjectLayer), mConvexRadiusFraction(inConvexRadiusFraction) { JPH_ASSERT(mConvexRadiusFraction >= 0.0f && mConvexRadiusFraction <= 1.0f); }
+
+	// See: VehicleCollisionTester
+	virtual bool					Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const override;
+	virtual void					PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const override;
+
+private:
+	float							mConvexRadiusFraction;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.cpp
new file mode 100644
index 0000000000..fb76aa6740
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.cpp
@@ -0,0 +1,697 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Vehicle/VehicleConstraint.h>
+#include <Jolt/Physics/Vehicle/VehicleController.h>
+#include <Jolt/Physics/PhysicsSystem.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/Core/Factory.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(VehicleConstraintSettings)
+{
+	JPH_ADD_BASE_CLASS(VehicleConstraintSettings, ConstraintSettings)
+
+	JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mUp)
+	JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mForward)
+	JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mMaxPitchRollAngle)
+	JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mWheels)
+	JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mAntiRollBars)
+	JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mController)
+}
+
+void VehicleConstraintSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	ConstraintSettings::SaveBinaryState(inStream);
+
+	inStream.Write(mUp);
+	inStream.Write(mForward);
+	inStream.Write(mMaxPitchRollAngle);
+
+	uint32 num_anti_rollbars = (uint32)mAntiRollBars.size();
+	inStream.Write(num_anti_rollbars);
+	for (const VehicleAntiRollBar &r : mAntiRollBars)
+		r.SaveBinaryState(inStream);
+
+	uint32 num_wheels = (uint32)mWheels.size();
+	inStream.Write(num_wheels);
+	for (const WheelSettings *w : mWheels)
+		w->SaveBinaryState(inStream);
+
+	inStream.Write(mController->GetRTTI()->GetHash());
+	mController->SaveBinaryState(inStream);
+}
+
+void VehicleConstraintSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	ConstraintSettings::RestoreBinaryState(inStream);
+
+	inStream.Read(mUp);
+	inStream.Read(mForward);
+	inStream.Read(mMaxPitchRollAngle);
+
+	uint32 num_anti_rollbars = 0;
+	inStream.Read(num_anti_rollbars);
+	mAntiRollBars.resize(num_anti_rollbars);
+	for (VehicleAntiRollBar &r : mAntiRollBars)
+		r.RestoreBinaryState(inStream);
+
+	uint32 num_wheels = 0;
+	inStream.Read(num_wheels);
+	mWheels.resize(num_wheels);
+	for (WheelSettings *w : mWheels)
+		w->RestoreBinaryState(inStream);
+
+	uint32 hash = 0;
+	inStream.Read(hash);
+	const RTTI *rtti = Factory::sInstance->Find(hash);
+	mController = reinterpret_cast<VehicleControllerSettings *>(rtti->CreateObject());
+	mController->RestoreBinaryState(inStream);
+}
+
+VehicleConstraint::VehicleConstraint(Body &inVehicleBody, const VehicleConstraintSettings &inSettings) :
+	Constraint(inSettings),
+	mBody(&inVehicleBody),
+	mForward(inSettings.mForward),
+	mUp(inSettings.mUp),
+	mWorldUp(inSettings.mUp)
+{
+	// Check sanity of incoming settings
+	JPH_ASSERT(inSettings.mUp.IsNormalized());
+	JPH_ASSERT(inSettings.mForward.IsNormalized());
+	JPH_ASSERT(!inSettings.mWheels.empty());
+
+	// Store max pitch/roll angle
+	SetMaxPitchRollAngle(inSettings.mMaxPitchRollAngle);
+
+	// Copy anti-rollbar settings
+	mAntiRollBars.resize(inSettings.mAntiRollBars.size());
+	for (uint i = 0; i < mAntiRollBars.size(); ++i)
+	{
+		const VehicleAntiRollBar &r = inSettings.mAntiRollBars[i];
+		mAntiRollBars[i] = r;
+		JPH_ASSERT(r.mStiffness >= 0.0f);
+	}
+
+	// Construct our controller class
+	mController = inSettings.mController->ConstructController(*this);
+
+	// Create wheels
+	mWheels.resize(inSettings.mWheels.size());
+	for (uint i = 0; i < mWheels.size(); ++i)
+		mWheels[i] = mController->ConstructWheel(*inSettings.mWheels[i]);
+
+	// Use the body ID as a seed for the step counter so that not all vehicles will update at the same time
+	mCurrentStep = uint32(Hash64(inVehicleBody.GetID().GetIndex()));
+}
+
+VehicleConstraint::~VehicleConstraint()
+{
+	// Destroy controller
+	delete mController;
+
+	// Destroy our wheels
+	for (Wheel *w : mWheels)
+		delete w;
+}
+
+void VehicleConstraint::GetWheelLocalBasis(const Wheel *inWheel, Vec3 &outForward, Vec3 &outUp, Vec3 &outRight) const
+{
+	const WheelSettings *settings = inWheel->mSettings;
+
+	Quat steer_rotation = Quat::sRotation(settings->mSteeringAxis, inWheel->mSteerAngle);
+	outUp = steer_rotation * settings->mWheelUp;
+	outForward = steer_rotation * settings->mWheelForward;
+	outRight = outForward.Cross(outUp).Normalized();
+	outForward = outUp.Cross(outRight).Normalized();
+}
+
+Mat44 VehicleConstraint::GetWheelLocalTransform(uint inWheelIndex, Vec3Arg inWheelRight, Vec3Arg inWheelUp) const
+{
+	JPH_ASSERT(inWheelIndex < mWheels.size());
+
+	const Wheel *wheel = mWheels[inWheelIndex];
+	const WheelSettings *settings = wheel->mSettings;
+
+	// Use the two vectors provided to calculate a matrix that takes us from wheel model space to X = right, Y = up, Z = forward (the space where we will rotate the wheel)
+	Mat44 wheel_to_rotational = Mat44(Vec4(inWheelRight, 0), Vec4(inWheelUp, 0), Vec4(inWheelUp.Cross(inWheelRight), 0), Vec4(0, 0, 0, 1)).Transposed();
+
+	// Calculate the matrix that takes us from the rotational space to vehicle local space
+	Vec3 local_forward, local_up, local_right;
+	GetWheelLocalBasis(wheel, local_forward, local_up, local_right);
+	Vec3 local_wheel_pos = settings->mPosition + settings->mSuspensionDirection * wheel->mSuspensionLength;
+	Mat44 rotational_to_local(Vec4(local_right, 0), Vec4(local_up, 0), Vec4(local_forward, 0), Vec4(local_wheel_pos, 1));
+
+	// Calculate transform of rotated wheel
+	return rotational_to_local * Mat44::sRotationX(wheel->mAngle) * wheel_to_rotational;
+}
+
+RMat44 VehicleConstraint::GetWheelWorldTransform(uint inWheelIndex, Vec3Arg inWheelRight, Vec3Arg inWheelUp) const
+{
+	return mBody->GetWorldTransform() * GetWheelLocalTransform(inWheelIndex, inWheelRight, inWheelUp);
+}
+
+void VehicleConstraint::OnStep(const PhysicsStepListenerContext &inContext)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Callback to higher-level systems. We do it before PreCollide, in case steering changes.
+	if (mPreStepCallback != nullptr)
+		mPreStepCallback(*this, inContext);
+
+	if (mIsGravityOverridden)
+	{
+		// If gravity is overridden, we replace the normal gravity calculations
+		if (mBody->IsActive())
+		{
+			MotionProperties *mp = mBody->GetMotionProperties();
+			mp->SetGravityFactor(0.0f);
+			mBody->AddForce(mGravityOverride / mp->GetInverseMass());
+		}
+
+		// And we calculate the world up using the custom gravity
+		mWorldUp = (-mGravityOverride).NormalizedOr(mWorldUp);
+	}
+	else
+	{
+		// Calculate new world up vector by inverting gravity
+		mWorldUp = (-inContext.mPhysicsSystem->GetGravity()).NormalizedOr(mWorldUp);
+	}
+
+	// Callback on our controller
+	mController->PreCollide(inContext.mDeltaTime, *inContext.mPhysicsSystem);
+
+	// Calculate if this constraint is active by checking if our main vehicle body is active or any of the bodies we touch are active
+	mIsActive = mBody->IsActive();
+
+	// Test how often we need to update the wheels
+	uint num_steps_between_collisions = mIsActive? mNumStepsBetweenCollisionTestActive : mNumStepsBetweenCollisionTestInactive;
+
+	RMat44 body_transform = mBody->GetWorldTransform();
+
+	// Test collision for wheels
+	for (uint wheel_index = 0; wheel_index < mWheels.size(); ++wheel_index)
+	{
+		Wheel *w = mWheels[wheel_index];
+		const WheelSettings *settings = w->mSettings;
+
+		// Calculate suspension origin and direction
+		RVec3 ws_origin = body_transform * settings->mPosition;
+		Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection);
+
+		// Test if we need to update this wheel
+		if (num_steps_between_collisions == 0
+			|| (mCurrentStep + wheel_index) % num_steps_between_collisions != 0)
+		{
+			// Simplified wheel contact test
+			if (!w->mContactBodyID.IsInvalid())
+			{
+				// Test if the body is still valid
+				w->mContactBody = inContext.mPhysicsSystem->GetBodyLockInterfaceNoLock().TryGetBody(w->mContactBodyID);
+				if (w->mContactBody == nullptr)
+				{
+					// It's not, forget the contact
+					w->mContactBodyID = BodyID();
+					w->mContactSubShapeID = SubShapeID();
+					w->mSuspensionLength = settings->mSuspensionMaxLength;
+				}
+				else
+				{
+					// Extrapolate the wheel contact properties
+					mVehicleCollisionTester->PredictContactProperties(*inContext.mPhysicsSystem, *this, wheel_index, ws_origin, ws_direction, mBody->GetID(), w->mContactBody, w->mContactSubShapeID, w->mContactPosition, w->mContactNormal, w->mSuspensionLength);
+				}
+			}
+		}
+		else
+		{
+			// Full wheel contact test, start by resetting the contact data
+			w->mContactBodyID = BodyID();
+			w->mContactBody = nullptr;
+			w->mContactSubShapeID = SubShapeID();
+			w->mSuspensionLength = settings->mSuspensionMaxLength;
+
+			// Test collision to find the floor
+			if (mVehicleCollisionTester->Collide(*inContext.mPhysicsSystem, *this, wheel_index, ws_origin, ws_direction, mBody->GetID(), w->mContactBody, w->mContactSubShapeID, w->mContactPosition, w->mContactNormal, w->mSuspensionLength))
+			{
+				// Store ID (pointer is not valid outside of the simulation step)
+				w->mContactBodyID = w->mContactBody->GetID();
+			}
+		}
+
+		if (w->mContactBody != nullptr)
+		{
+			// Store contact velocity, cache this as the contact body may be removed
+			w->mContactPointVelocity = w->mContactBody->GetPointVelocity(w->mContactPosition);
+
+			// Determine plane constant for axle contact plane
+			w->mAxlePlaneConstant = RVec3(w->mContactNormal).Dot(ws_origin + w->mSuspensionLength * ws_direction);
+
+			// Check if body is active, if so the entire vehicle should be active
+			mIsActive |= w->mContactBody->IsActive();
+
+			// Determine world space forward using steering angle and body rotation
+			Vec3 forward, up, right;
+			GetWheelLocalBasis(w, forward, up, right);
+			forward = body_transform.Multiply3x3(forward);
+			right = body_transform.Multiply3x3(right);
+
+			// The longitudinal axis is in the up/forward plane
+			w->mContactLongitudinal = w->mContactNormal.Cross(right);
+
+			// Make sure that the longitudinal axis is aligned with the forward axis
+			if (w->mContactLongitudinal.Dot(forward) < 0.0f)
+				w->mContactLongitudinal = -w->mContactLongitudinal;
+
+			// Normalize it
+			w->mContactLongitudinal = w->mContactLongitudinal.NormalizedOr(w->mContactNormal.GetNormalizedPerpendicular());
+
+			// The lateral axis is perpendicular to contact normal and longitudinal axis
+			w->mContactLateral = w->mContactLongitudinal.Cross(w->mContactNormal).Normalized();
+		}
+	}
+
+	// Callback to higher-level systems. We do it immediately after wheel collision.
+	if (mPostCollideCallback != nullptr)
+		mPostCollideCallback(*this, inContext);
+
+	// Calculate anti-rollbar impulses
+	for (const VehicleAntiRollBar &r : mAntiRollBars)
+	{
+		Wheel *lw = mWheels[r.mLeftWheel];
+		Wheel *rw = mWheels[r.mRightWheel];
+
+		if (lw->mContactBody != nullptr && rw->mContactBody != nullptr)
+		{
+			// Calculate the impulse to apply based on the difference in suspension length
+			float difference = rw->mSuspensionLength - lw->mSuspensionLength;
+			float impulse = difference * r.mStiffness * inContext.mDeltaTime;
+			lw->mAntiRollBarImpulse = -impulse;
+			rw->mAntiRollBarImpulse = impulse;
+		}
+		else
+		{
+			// When one of the wheels is not on the ground we don't apply any impulses
+			lw->mAntiRollBarImpulse = rw->mAntiRollBarImpulse = 0.0f;
+		}
+	}
+
+	// Callback on our controller
+	mController->PostCollide(inContext.mDeltaTime, *inContext.mPhysicsSystem);
+
+	// Callback to higher-level systems. We do it before the sleep section, in case velocities change.
+	if (mPostStepCallback != nullptr)
+		mPostStepCallback(*this, inContext);
+
+	// If the wheels are rotating, we don't want to go to sleep yet
+	bool allow_sleep = mController->AllowSleep();
+	if (allow_sleep)
+		for (const Wheel *w : mWheels)
+			if (abs(w->mAngularVelocity) > DegreesToRadians(10.0f))
+			{
+				allow_sleep = false;
+				break;
+			}
+	if (mBody->GetAllowSleeping() != allow_sleep)
+		mBody->SetAllowSleeping(allow_sleep);
+
+	// Increment step counter
+	++mCurrentStep;
+}
+
+void VehicleConstraint::BuildIslands(uint32 inConstraintIndex, IslandBuilder &ioBuilder, BodyManager &inBodyManager)
+{
+	// Find dynamic bodies that our wheels are touching
+	BodyID *body_ids = (BodyID *)JPH_STACK_ALLOC((mWheels.size() + 1) * sizeof(BodyID));
+	int num_bodies = 0;
+	bool needs_to_activate = false;
+	for (const Wheel *w : mWheels)
+		if (w->mContactBody != nullptr)
+		{
+			// Avoid adding duplicates
+			bool duplicate = false;
+			BodyID id = w->mContactBody->GetID();
+			for (int i = 0; i < num_bodies; ++i)
+				if (body_ids[i] == id)
+				{
+					duplicate = true;
+					break;
+				}
+			if (duplicate)
+				continue;
+
+			if (w->mContactBody->IsDynamic())
+			{
+				body_ids[num_bodies++] = id;
+				needs_to_activate |= !w->mContactBody->IsActive();
+			}
+		}
+
+	// Activate bodies, note that if we get here we have already told the system that we're active so that means our main body needs to be active too
+	if (!mBody->IsActive())
+	{
+		// Our main body is not active, activate it too
+		body_ids[num_bodies] = mBody->GetID();
+		inBodyManager.ActivateBodies(body_ids, num_bodies + 1);
+	}
+	else if (needs_to_activate)
+	{
+		// Only activate bodies the wheels are touching
+		inBodyManager.ActivateBodies(body_ids, num_bodies);
+	}
+
+	// Link the bodies into the same island
+	uint32 min_active_index = Body::cInactiveIndex;
+	for (int i = 0; i < num_bodies; ++i)
+	{
+		const Body &body = inBodyManager.GetBody(body_ids[i]);
+		min_active_index = min(min_active_index, body.GetIndexInActiveBodiesInternal());
+		ioBuilder.LinkBodies(mBody->GetIndexInActiveBodiesInternal(), body.GetIndexInActiveBodiesInternal());
+	}
+
+	// Link the constraint in the island
+	ioBuilder.LinkConstraint(inConstraintIndex, mBody->GetIndexInActiveBodiesInternal(), min_active_index);
+}
+
+uint VehicleConstraint::BuildIslandSplits(LargeIslandSplitter &ioSplitter) const
+{
+	return ioSplitter.AssignToNonParallelSplit(mBody);
+}
+
+void VehicleConstraint::CalculateSuspensionForcePoint(const Wheel &inWheel, Vec3 &outR1PlusU, Vec3 &outR2) const
+{
+	// Determine point to apply force to
+	RVec3 force_point;
+	if (inWheel.mSettings->mEnableSuspensionForcePoint)
+		force_point = mBody->GetWorldTransform() * inWheel.mSettings->mSuspensionForcePoint;
+	else
+		force_point = inWheel.mContactPosition;
+
+	// Calculate r1 + u and r2
+	outR1PlusU = Vec3(force_point - mBody->GetCenterOfMassPosition());
+	outR2 = Vec3(force_point - inWheel.mContactBody->GetCenterOfMassPosition());
+}
+
+void VehicleConstraint::CalculatePitchRollConstraintProperties(RMat44Arg inBodyTransform)
+{
+	// Check if a limit was specified
+	if (mCosMaxPitchRollAngle > -1.0f)
+	{
+		// Calculate cos of angle between world up vector and vehicle up vector
+		Vec3 vehicle_up = inBodyTransform.Multiply3x3(mUp);
+		mCosPitchRollAngle = mWorldUp.Dot(vehicle_up);
+		if (mCosPitchRollAngle < mCosMaxPitchRollAngle)
+		{
+			// Calculate rotation axis to rotate vehicle towards up
+			Vec3 rotation_axis = mWorldUp.Cross(vehicle_up);
+			float len = rotation_axis.Length();
+			if (len > 0.0f)
+				mPitchRollRotationAxis = rotation_axis / len;
+
+			mPitchRollPart.CalculateConstraintProperties(*mBody, Body::sFixedToWorld, mPitchRollRotationAxis);
+		}
+		else
+			mPitchRollPart.Deactivate();
+	}
+	else
+		mPitchRollPart.Deactivate();
+}
+
+void VehicleConstraint::SetupVelocityConstraint(float inDeltaTime)
+{
+	RMat44 body_transform = mBody->GetWorldTransform();
+
+	for (Wheel *w : mWheels)
+		if (w->mContactBody != nullptr)
+		{
+			const WheelSettings *settings = w->mSettings;
+
+			Vec3 neg_contact_normal = -w->mContactNormal;
+
+			Vec3 r1_plus_u, r2;
+			CalculateSuspensionForcePoint(*w, r1_plus_u, r2);
+
+			// Suspension spring
+			if (settings->mSuspensionMaxLength > settings->mSuspensionMinLength)
+			{
+				float stiffness, damping;
+				if (settings->mSuspensionSpring.mMode == ESpringMode::FrequencyAndDamping)
+				{
+					// Calculate effective mass based on vehicle configuration (the stiffness of the spring should not be affected by the dynamics of the vehicle): K = 1 / (J M^-1 J^T)
+					// Note that if no suspension force point is supplied we don't know where the force is applied so we assume it is applied at average suspension length
+					Vec3 force_point = settings->mEnableSuspensionForcePoint? settings->mSuspensionForcePoint : settings->mPosition + 0.5f * (settings->mSuspensionMinLength + settings->mSuspensionMaxLength) * settings->mSuspensionDirection;
+					Vec3 force_point_x_neg_up = force_point.Cross(-mUp);
+					const MotionProperties *mp = mBody->GetMotionProperties();
+					float effective_mass = 1.0f / (mp->GetInverseMass() + force_point_x_neg_up.Dot(mp->GetLocalSpaceInverseInertia().Multiply3x3(force_point_x_neg_up)));
+
+					// Convert frequency and damping to stiffness and damping
+					float omega = 2.0f * JPH_PI * settings->mSuspensionSpring.mFrequency;
+					stiffness = effective_mass * Square(omega);
+					damping = 2.0f * effective_mass * settings->mSuspensionSpring.mDamping * omega;
+				}
+				else
+				{
+					// In this case we can simply copy the properties
+					stiffness = settings->mSuspensionSpring.mStiffness;
+					damping = settings->mSuspensionSpring.mDamping;
+				}
+
+				// Calculate the damping and frequency of the suspension spring given the angle between the suspension direction and the contact normal
+				// If the angle between the suspension direction and the inverse of the contact normal is alpha then the force on the spring relates to the force along the contact normal as:
+				//
+				// Fspring = Fnormal * cos(alpha)
+				//
+				// The spring force is:
+				//
+				// Fspring = -k * x
+				//
+				// where k is the spring constant and x is the displacement of the spring. So we have:
+				//
+				// Fnormal * cos(alpha) = -k * x <=> Fnormal = -k / cos(alpha) * x
+				//
+				// So we can see this as a spring with spring constant:
+				//
+				// k' = k / cos(alpha)
+				//
+				// In the same way the velocity relates like:
+				//
+				// Vspring = Vnormal * cos(alpha)
+				//
+				// Which results in the modified damping constant c:
+				//
+				// c' = c / cos(alpha)
+				//
+				// Note that we clamp 1 / cos(alpha) to the range [0.1, 1] in order not to increase the stiffness / damping by too much.
+				Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection);
+				float cos_angle = max(0.1f, ws_direction.Dot(neg_contact_normal));
+				stiffness /= cos_angle;
+				damping /= cos_angle;
+
+				// Get the value of the constraint equation
+				float c = w->mSuspensionLength - settings->mSuspensionMaxLength - settings->mSuspensionPreloadLength;
+
+				w->mSuspensionPart.CalculateConstraintPropertiesWithStiffnessAndDamping(inDeltaTime, *mBody, r1_plus_u, *w->mContactBody, r2, neg_contact_normal, w->mAntiRollBarImpulse, c, stiffness, damping);
+			}
+			else
+				w->mSuspensionPart.Deactivate();
+
+			// Check if we reached the 'max up' position and if so add a hard velocity constraint that stops any further movement in the normal direction
+			if (w->mSuspensionLength < settings->mSuspensionMinLength)
+				w->mSuspensionMaxUpPart.CalculateConstraintProperties(*mBody, r1_plus_u, *w->mContactBody, r2, neg_contact_normal);
+			else
+				w->mSuspensionMaxUpPart.Deactivate();
+
+			// Friction and propulsion
+			w->mLongitudinalPart.CalculateConstraintProperties(*mBody, r1_plus_u, *w->mContactBody, r2, -w->mContactLongitudinal);
+			w->mLateralPart.CalculateConstraintProperties(*mBody, r1_plus_u, *w->mContactBody, r2, -w->mContactLateral);
+		}
+		else
+		{
+			// No contact -> disable everything
+			w->mSuspensionPart.Deactivate();
+			w->mSuspensionMaxUpPart.Deactivate();
+			w->mLongitudinalPart.Deactivate();
+			w->mLateralPart.Deactivate();
+		}
+
+	CalculatePitchRollConstraintProperties(body_transform);
+}
+
+void VehicleConstraint::ResetWarmStart()
+{
+	for (Wheel *w : mWheels)
+	{
+		w->mSuspensionPart.Deactivate();
+		w->mSuspensionMaxUpPart.Deactivate();
+		w->mLongitudinalPart.Deactivate();
+		w->mLateralPart.Deactivate();
+	}
+
+	mPitchRollPart.Deactivate();
+}
+
+void VehicleConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
+{
+	for (Wheel *w : mWheels)
+		if (w->mContactBody != nullptr)
+		{
+			Vec3 neg_contact_normal = -w->mContactNormal;
+
+			w->mSuspensionPart.WarmStart(*mBody, *w->mContactBody, neg_contact_normal, inWarmStartImpulseRatio);
+			w->mSuspensionMaxUpPart.WarmStart(*mBody, *w->mContactBody, neg_contact_normal, inWarmStartImpulseRatio);
+			w->mLongitudinalPart.WarmStart(*mBody, *w->mContactBody, -w->mContactLongitudinal, 0.0f); // Don't warm start the longitudinal part (the engine/brake force, we don't want to preserve anything from the last frame)
+			w->mLateralPart.WarmStart(*mBody, *w->mContactBody, -w->mContactLateral, inWarmStartImpulseRatio);
+		}
+
+	mPitchRollPart.WarmStart(*mBody, Body::sFixedToWorld, inWarmStartImpulseRatio);
+}
+
+bool VehicleConstraint::SolveVelocityConstraint(float inDeltaTime)
+{
+	bool impulse = false;
+
+	// Solve suspension
+	for (Wheel *w : mWheels)
+		if (w->mContactBody != nullptr)
+		{
+			Vec3 neg_contact_normal = -w->mContactNormal;
+
+			// Suspension spring, note that it can only push and not pull
+			if (w->mSuspensionPart.IsActive())
+				impulse |= w->mSuspensionPart.SolveVelocityConstraint(*mBody, *w->mContactBody, neg_contact_normal, 0.0f, FLT_MAX);
+
+			// When reaching the minimal suspension length only allow forces pushing the bodies away
+			if (w->mSuspensionMaxUpPart.IsActive())
+				impulse |= w->mSuspensionMaxUpPart.SolveVelocityConstraint(*mBody, *w->mContactBody, neg_contact_normal, 0.0f, FLT_MAX);
+		}
+
+	// Solve the horizontal movement of the vehicle
+	impulse |= mController->SolveLongitudinalAndLateralConstraints(inDeltaTime);
+
+	// Apply the pitch / roll constraint to avoid the vehicle from toppling over
+	if (mPitchRollPart.IsActive())
+		impulse |= mPitchRollPart.SolveVelocityConstraint(*mBody, Body::sFixedToWorld, mPitchRollRotationAxis, 0, FLT_MAX);
+
+	return impulse;
+}
+
+bool VehicleConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
+{
+	bool impulse = false;
+
+	RMat44 body_transform = mBody->GetWorldTransform();
+
+	for (Wheel *w : mWheels)
+		if (w->mContactBody != nullptr)
+		{
+			const WheelSettings *settings = w->mSettings;
+
+			// Check if we reached the 'max up' position now that the body has possibly moved
+			// We do this by calculating the axle position at minimum suspension length and making sure it does not go through the
+			// plane defined by the contact normal and the axle position when the contact happened
+			// TODO: This assumes that only the vehicle moved and not the ground as we kept the axle contact plane in world space
+			Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection);
+			RVec3 ws_position = body_transform * settings->mPosition;
+			RVec3 min_suspension_pos = ws_position + settings->mSuspensionMinLength * ws_direction;
+			float max_up_error = float(RVec3(w->mContactNormal).Dot(min_suspension_pos) - w->mAxlePlaneConstant);
+			if (max_up_error < 0.0f)
+			{
+				Vec3 neg_contact_normal = -w->mContactNormal;
+
+				// Recalculate constraint properties since the body may have moved
+				Vec3 r1_plus_u, r2;
+				CalculateSuspensionForcePoint(*w, r1_plus_u, r2);
+				w->mSuspensionMaxUpPart.CalculateConstraintProperties(*mBody, r1_plus_u, *w->mContactBody, r2, neg_contact_normal);
+
+				impulse |= w->mSuspensionMaxUpPart.SolvePositionConstraint(*mBody, *w->mContactBody, neg_contact_normal, max_up_error, inBaumgarte);
+			}
+		}
+
+	// Apply the pitch / roll constraint to avoid the vehicle from toppling over
+	CalculatePitchRollConstraintProperties(body_transform);
+	if (mPitchRollPart.IsActive())
+		impulse |= mPitchRollPart.SolvePositionConstraint(*mBody, Body::sFixedToWorld, mCosPitchRollAngle - mCosMaxPitchRollAngle, inBaumgarte);
+
+	return impulse;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+
+void VehicleConstraint::DrawConstraint(DebugRenderer *inRenderer) const
+{
+	mController->Draw(inRenderer);
+}
+
+void VehicleConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const
+{
+}
+
+#endif // JPH_DEBUG_RENDERER
+
+void VehicleConstraint::SaveState(StateRecorder &inStream) const
+{
+	Constraint::SaveState(inStream);
+
+	mController->SaveState(inStream);
+
+	for (const Wheel *w : mWheels)
+	{
+		inStream.Write(w->mAngularVelocity);
+		inStream.Write(w->mAngle);
+		inStream.Write(w->mContactBodyID); // Used by MotorcycleController::PreCollide
+		inStream.Write(w->mContactPosition); // Used by VehicleCollisionTester::PredictContactProperties
+		inStream.Write(w->mContactNormal); // Used by MotorcycleController::PreCollide
+		inStream.Write(w->mContactLateral); // Used by MotorcycleController::PreCollide
+		inStream.Write(w->mSuspensionLength); // Used by VehicleCollisionTester::PredictContactProperties
+
+		w->mSuspensionPart.SaveState(inStream);
+		w->mSuspensionMaxUpPart.SaveState(inStream);
+		w->mLongitudinalPart.SaveState(inStream);
+		w->mLateralPart.SaveState(inStream);
+	}
+
+	inStream.Write(mPitchRollRotationAxis); // When rotation is too small we use last frame so we need to store it
+	mPitchRollPart.SaveState(inStream);
+	inStream.Write(mCurrentStep);
+}
+
+void VehicleConstraint::RestoreState(StateRecorder &inStream)
+{
+	Constraint::RestoreState(inStream);
+
+	mController->RestoreState(inStream);
+
+	for (Wheel *w : mWheels)
+	{
+		inStream.Read(w->mAngularVelocity);
+		inStream.Read(w->mAngle);
+		inStream.Read(w->mContactBodyID);
+		inStream.Read(w->mContactPosition);
+		inStream.Read(w->mContactNormal);
+		inStream.Read(w->mContactLateral);
+		inStream.Read(w->mSuspensionLength);
+		w->mContactBody = nullptr; // No longer valid
+
+		w->mSuspensionPart.RestoreState(inStream);
+		w->mSuspensionMaxUpPart.RestoreState(inStream);
+		w->mLongitudinalPart.RestoreState(inStream);
+		w->mLateralPart.RestoreState(inStream);
+	}
+
+	inStream.Read(mPitchRollRotationAxis);
+	mPitchRollPart.RestoreState(inStream);
+	inStream.Read(mCurrentStep);
+}
+
+Ref<ConstraintSettings> VehicleConstraint::GetConstraintSettings() const
+{
+	JPH_ASSERT(false); // Not implemented yet
+	return nullptr;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.h
new file mode 100644
index 0000000000..9459f8e437
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleConstraint.h
@@ -0,0 +1,246 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Constraints/Constraint.h>
+#include <Jolt/Physics/PhysicsStepListener.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h>
+#include <Jolt/Physics/Vehicle/VehicleCollisionTester.h>
+#include <Jolt/Physics/Vehicle/VehicleAntiRollBar.h>
+#include <Jolt/Physics/Vehicle/Wheel.h>
+#include <Jolt/Physics/Vehicle/VehicleController.h>
+
+JPH_NAMESPACE_BEGIN
+
+class PhysicsSystem;
+
+/// Configuration for constraint that simulates a wheeled vehicle.
+///
+/// The properties in this constraint are largely based on "Car Physics for Games" by Marco Monster.
+/// See: https://www.asawicki.info/Mirror/Car%20Physics%20for%20Games/Car%20Physics%20for%20Games.html
+class JPH_EXPORT VehicleConstraintSettings : public ConstraintSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, VehicleConstraintSettings)
+
+public:
+	/// Saves the contents of the constraint settings in binary form to inStream.
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+
+	Vec3						mUp { 0, 1, 0 };							///< Vector indicating the up direction of the vehicle (in local space to the body)
+	Vec3						mForward { 0, 0, 1 };						///< Vector indicating forward direction of the vehicle (in local space to the body)
+	float						mMaxPitchRollAngle = JPH_PI;				///< Defines the maximum pitch/roll angle (rad), can be used to avoid the car from getting upside down. The vehicle up direction will stay within a cone centered around the up axis with half top angle mMaxPitchRollAngle, set to pi to turn off.
+	Array<Ref<WheelSettings>>	mWheels;									///< List of wheels and their properties
+	Array<VehicleAntiRollBar>	mAntiRollBars;								///< List of anti rollbars and their properties
+	Ref<VehicleControllerSettings> mController;								///< Defines how the vehicle can accelerate / decelerate
+
+protected:
+	/// This function should not be called directly, it is used by sRestoreFromBinaryState.
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+};
+
+/// Constraint that simulates a vehicle
+/// Note: Don't forget to register the constraint as a StepListener with the PhysicsSystem!
+///
+/// When the vehicle drives over very light objects (rubble) you may see the car body dip down. This is a known issue and is an artifact of the iterative solver that Jolt is using.
+/// Basically if a light object is sandwiched between two heavy objects (the static floor and the car body), the light object is not able to transfer enough force from the ground to
+/// the car body to keep the car body up. You can see this effect in the HeavyOnLightTest sample, the boxes on the right have a lot of penetration because they're on top of light objects.
+///
+/// There are a couple of ways to improve this:
+///
+/// 1. You can increase the number of velocity steps (global settings PhysicsSettings::mNumVelocitySteps or if you only want to increase it on
+/// the vehicle you can use VehicleConstraintSettings::mNumVelocityStepsOverride). E.g. going from 10 to 30 steps in the HeavyOnLightTest sample makes the penetration a lot less.
+/// The number of position steps can also be increased (the first prevents the body from going down, the second corrects it if the problem did
+/// occur which inevitably happens due to numerical drift). This solution costs CPU cycles.
+///
+/// 2. You can reduce the mass difference between the vehicle body and the rubble on the floor (by making the rubble heavier or the car lighter).
+///
+/// 3. You could filter out collisions between the vehicle collision test and the rubble completely. This would make the wheels ignore the rubble but would cause the vehicle to drive
+/// through it as if nothing happened. You could create fake wheels (keyframed bodies) that move along with the vehicle and that only collide with rubble (and not the vehicle or the ground).
+/// This would cause the vehicle to push away the rubble without the rubble being able to affect the vehicle (unless it hits the main body of course).
+///
+/// Note that when driving over rubble, you may see the wheel jump up and down quite quickly because one frame a collision is found and the next frame not.
+/// To alleviate this, it may be needed to smooth the motion of the visual mesh for the wheel.
+class JPH_EXPORT VehicleConstraint : public Constraint, public PhysicsStepListener
+{
+public:
+	/// Constructor / destructor
+								VehicleConstraint(Body &inVehicleBody, const VehicleConstraintSettings &inSettings);
+	virtual						~VehicleConstraint() override;
+
+	/// Get the type of a constraint
+	virtual EConstraintSubType	GetSubType() const override					{ return EConstraintSubType::Vehicle; }
+
+	/// Defines the maximum pitch/roll angle (rad), can be used to avoid the car from getting upside down. The vehicle up direction will stay within a cone centered around the up axis with half top angle mMaxPitchRollAngle, set to pi to turn off.
+	void						SetMaxPitchRollAngle(float inMaxPitchRollAngle) { mCosMaxPitchRollAngle = Cos(inMaxPitchRollAngle); }
+
+	/// Set the interface that tests collision between wheel and ground
+	void						SetVehicleCollisionTester(const VehicleCollisionTester *inTester) { mVehicleCollisionTester = inTester; }
+
+	/// Callback function to combine the friction of a tire with the friction of the body it is colliding with.
+	/// On input ioLongitudinalFriction and ioLateralFriction contain the friction of the tire, on output they should contain the combined friction with inBody2.
+	using CombineFunction = function<void(uint inWheelIndex, float &ioLongitudinalFriction, float &ioLateralFriction, const Body &inBody2, const SubShapeID &inSubShapeID2)>;
+
+	/// Set the function that combines the friction of two bodies and returns it
+	/// Default method is the geometric mean: sqrt(friction1 * friction2).
+	void						SetCombineFriction(const CombineFunction &inCombineFriction) { mCombineFriction = inCombineFriction; }
+	const CombineFunction &		GetCombineFriction() const					{ return mCombineFriction; }
+
+	/// Callback function to notify of current stage in PhysicsStepListener::OnStep.
+	using StepCallback = function<void(VehicleConstraint &inVehicle, const PhysicsStepListenerContext &inContext)>;
+
+	/// Callback function to notify that PhysicsStepListener::OnStep has started for this vehicle. Default is to do nothing.
+	/// Can be used to allow higher-level code to e.g. control steering. This is the last moment that the position/orientation of the vehicle can be changed.
+	/// Wheel collision checks have not been performed yet.
+	const StepCallback &		GetPreStepCallback() const					{ return mPreStepCallback; }
+	void						SetPreStepCallback(const StepCallback &inPreStepCallback) { mPreStepCallback = inPreStepCallback; }
+
+	/// Callback function to notify that PhysicsStepListener::OnStep has just completed wheel collision checks. Default is to do nothing.
+	/// Can be used to allow higher-level code to e.g. detect tire contact or to modify the velocity of the vehicle based on the wheel contacts.
+	/// You should not change the position of the vehicle in this callback as the wheel collision checks have already been performed.
+	const StepCallback &		GetPostCollideCallback() const				{ return mPostCollideCallback; }
+	void						SetPostCollideCallback(const StepCallback &inPostCollideCallback) { mPostCollideCallback = inPostCollideCallback; }
+
+	/// Callback function to notify that PhysicsStepListener::OnStep has completed for this vehicle. Default is to do nothing.
+	/// Can be used to allow higher-level code to e.g. control the vehicle in the air.
+	/// You should not change the position of the vehicle in this callback as the wheel collision checks have already been performed.
+	const StepCallback &		GetPostStepCallback() const					{ return mPostStepCallback; }
+	void						SetPostStepCallback(const StepCallback &inPostStepCallback) { mPostStepCallback = inPostStepCallback; }
+
+	/// Override gravity for this vehicle. Note that overriding gravity will set the gravity factor of the vehicle body to 0 and apply gravity in the PhysicsStepListener instead.
+	void						OverrideGravity(Vec3Arg inGravity)			{ mGravityOverride = inGravity; mIsGravityOverridden = true; }
+	bool						IsGravityOverridden() const					{ return mIsGravityOverridden; }
+	Vec3						GetGravityOverride() const					{ return mGravityOverride; }
+	void						ResetGravityOverride()						{ mIsGravityOverridden = false; mBody->GetMotionProperties()->SetGravityFactor(1.0f); } ///< Note that resetting the gravity override will restore the gravity factor of the vehicle body to 1.
+
+	/// Get the local space forward vector of the vehicle
+	Vec3						GetLocalForward() const						{ return mForward; }
+
+	/// Get the local space up vector of the vehicle
+	Vec3						GetLocalUp() const							{ return mUp; }
+
+	/// Vector indicating the world space up direction (used to limit vehicle pitch/roll), calculated every frame by inverting gravity
+	Vec3						GetWorldUp() const							{ return mWorldUp; }
+
+	/// Access to the vehicle body
+	Body *						GetVehicleBody() const						{ return mBody; }
+
+	/// Access to the vehicle controller interface (determines acceleration / deceleration)
+	const VehicleController *	GetController() const						{ return mController; }
+
+	/// Access to the vehicle controller interface (determines acceleration / deceleration)
+	VehicleController *			GetController()								{ return mController; }
+
+	/// Get the state of the wheels
+	const Wheels &				GetWheels() const							{ return mWheels; }
+
+	/// Get the state of a wheels (writable interface, allows you to make changes to the configuration which will take effect the next time step)
+	Wheels &					GetWheels()									{ return mWheels; }
+
+	/// Get the state of a wheel
+	Wheel *						GetWheel(uint inIdx)						{ return mWheels[inIdx]; }
+	const Wheel *				GetWheel(uint inIdx) const					{ return mWheels[inIdx]; }
+
+	/// Get the basis vectors for the wheel in local space to the vehicle body (note: basis does not rotate when the wheel rotates around its axis)
+	/// @param inWheel Wheel to fetch basis for
+	/// @param outForward Forward vector for the wheel
+	/// @param outUp Up vector for the wheel
+	/// @param outRight Right vector for the wheel
+	void						GetWheelLocalBasis(const Wheel *inWheel, Vec3 &outForward, Vec3 &outUp, Vec3 &outRight) const;
+
+	/// Get the transform of a wheel in local space to the vehicle body, returns a matrix that transforms a cylinder aligned with the Y axis in body space (not COM space)
+	/// @param inWheelIndex Index of the wheel to fetch
+	/// @param inWheelRight Unit vector that indicates right in model space of the wheel (so if you only have 1 wheel model, you probably want to specify the opposite direction for the left and right wheels)
+	/// @param inWheelUp Unit vector that indicates up in model space of the wheel
+	Mat44						GetWheelLocalTransform(uint inWheelIndex, Vec3Arg inWheelRight, Vec3Arg inWheelUp) const;
+
+	/// Get the transform of a wheel in world space, returns a matrix that transforms a cylinder aligned with the Y axis in world space
+	/// @param inWheelIndex Index of the wheel to fetch
+	/// @param inWheelRight Unit vector that indicates right in model space of the wheel (so if you only have 1 wheel model, you probably want to specify the opposite direction for the left and right wheels)
+	/// @param inWheelUp Unit vector that indicates up in model space of the wheel
+	RMat44						GetWheelWorldTransform(uint inWheelIndex, Vec3Arg inWheelRight, Vec3Arg inWheelUp) const;
+
+	/// Number of simulation steps between wheel collision tests when the vehicle is active. Default is 1. 0 = never, 1 = every step, 2 = every other step, etc.
+	/// Note that if a vehicle has multiple wheels and the number of steps > 1, the wheels will be tested in a round robin fashion.
+	/// If there are multiple vehicles, the tests will be spread out based on the BodyID of the vehicle.
+	/// If you set this to test less than every step, you may see simulation artifacts. This setting can be used to reduce the cost of simulating vehicles in the distance.
+	void						SetNumStepsBetweenCollisionTestActive(uint inSteps) { mNumStepsBetweenCollisionTestActive = inSteps; }
+	uint						GetNumStepsBetweenCollisionTestActive() const { return mNumStepsBetweenCollisionTestActive; }
+
+	/// Number of simulation steps between wheel collision tests when the vehicle is inactive. Default is 1. 0 = never, 1 = every step, 2 = every other step, etc.
+	/// Note that if a vehicle has multiple wheels and the number of steps > 1, the wheels will be tested in a round robin fashion.
+	/// If there are multiple vehicles, the tests will be spread out based on the BodyID of the vehicle.
+	/// This number can be lower than the number of steps when the vehicle is active as the only purpose of this test is
+	/// to allow the vehicle to wake up in response to bodies moving into the wheels but not touching the body of the vehicle.
+	void						SetNumStepsBetweenCollisionTestInactive(uint inSteps) { mNumStepsBetweenCollisionTestInactive = inSteps; }
+	uint						GetNumStepsBetweenCollisionTestInactive() const { return mNumStepsBetweenCollisionTestInactive; }
+
+	// Generic interface of a constraint
+	virtual bool				IsActive() const override					{ return mIsActive && Constraint::IsActive(); }
+	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override { /* Do nothing */ }
+	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
+	virtual void				ResetWarmStart() override;
+	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
+	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
+	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
+	virtual void				BuildIslands(uint32 inConstraintIndex, IslandBuilder &ioBuilder, BodyManager &inBodyManager) override;
+	virtual uint				BuildIslandSplits(LargeIslandSplitter &ioSplitter) const override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void				DrawConstraint(DebugRenderer *inRenderer) const override;
+	virtual void				DrawConstraintLimits(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual void				SaveState(StateRecorder &inStream) const override;
+	virtual void				RestoreState(StateRecorder &inStream) override;
+	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
+
+private:
+	// See: PhysicsStepListener
+	virtual void				OnStep(const PhysicsStepListenerContext &inContext) override;
+
+	// Calculate the position where the suspension and traction forces should be applied in world space, relative to the center of mass of both bodies
+	void						CalculateSuspensionForcePoint(const Wheel &inWheel, Vec3 &outR1PlusU, Vec3 &outR2) const;
+
+	// Calculate the constraint properties for mPitchRollPart
+	void						CalculatePitchRollConstraintProperties(RMat44Arg inBodyTransform);
+
+	// Gravity override
+	bool						mIsGravityOverridden = false;				///< If the gravity is currently overridden
+	Vec3						mGravityOverride = Vec3::sZero();			///< Gravity override value, replaces PhysicsSystem::GetGravity() when mIsGravityOverridden is true
+
+	// Simulation information
+	Body *						mBody;										///< Body of the vehicle
+	Vec3						mForward;									///< Local space forward vector for the vehicle
+	Vec3						mUp;										///< Local space up vector for the vehicle
+	Vec3						mWorldUp;									///< Vector indicating the world space up direction (used to limit vehicle pitch/roll)
+	Wheels						mWheels;									///< Wheel states of the vehicle
+	Array<VehicleAntiRollBar>	mAntiRollBars;								///< Anti rollbars of the vehicle
+	VehicleController *			mController;								///< Controls the acceleration / deceleration of the vehicle
+	bool						mIsActive = false;							///< If this constraint is active
+	uint						mNumStepsBetweenCollisionTestActive = 1;	///< Number of simulation steps between wheel collision tests when the vehicle is active
+	uint						mNumStepsBetweenCollisionTestInactive = 1;	///< Number of simulation steps between wheel collision tests when the vehicle is inactive
+	uint						mCurrentStep = 0;							///< Current step number, used to determine when to test a wheel
+
+	// Prevent vehicle from toppling over
+	float						mCosMaxPitchRollAngle;						///< Cos of the max pitch/roll angle
+	float						mCosPitchRollAngle;							///< Cos of the current pitch/roll angle
+	Vec3						mPitchRollRotationAxis { 0, 1, 0 };			///< Current axis along which to apply torque to prevent the car from toppling over
+	AngleConstraintPart			mPitchRollPart;								///< Constraint part that prevents the car from toppling over
+
+	// Interfaces
+	RefConst<VehicleCollisionTester> mVehicleCollisionTester;				///< Class that performs testing of collision for the wheels
+	CombineFunction				mCombineFriction = [](uint, float &ioLongitudinalFriction, float &ioLateralFriction, const Body &inBody2, const SubShapeID &)
+	{
+		float body_friction = inBody2.GetFriction();
+
+		ioLongitudinalFriction = sqrt(ioLongitudinalFriction * body_friction);
+		ioLateralFriction = sqrt(ioLateralFriction * body_friction);
+	};
+
+	// Callbacks
+	StepCallback				mPreStepCallback;
+	StepCallback				mPostCollideCallback;
+	StepCallback				mPostStepCallback;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleController.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleController.cpp
new file mode 100644
index 0000000000..ffa8ff1a64
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleController.cpp
@@ -0,0 +1,17 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Vehicle/VehicleController.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(VehicleControllerSettings)
+{
+	JPH_ADD_BASE_CLASS(VehicleControllerSettings, SerializableObject)
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleController.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleController.h
new file mode 100644
index 0000000000..0d9095b189
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleController.h
@@ -0,0 +1,84 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/ObjectStream/SerializableObject.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+class PhysicsSystem;
+class VehicleController;
+class VehicleConstraint;
+class WheelSettings;
+class Wheel;
+class StateRecorder;
+
+/// Basic settings object for interface that controls acceleration / deceleration of the vehicle
+class JPH_EXPORT VehicleControllerSettings : public SerializableObject, public RefTarget<VehicleControllerSettings>
+{
+	JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, VehicleControllerSettings)
+
+public:
+	/// Saves the contents of the controller settings in binary form to inStream.
+	virtual void				SaveBinaryState(StreamOut &inStream) const = 0;
+
+	/// Restore the contents of the controller settings in binary form from inStream.
+	virtual void				RestoreBinaryState(StreamIn &inStream) = 0;
+
+	/// Create an instance of the vehicle controller class
+	virtual VehicleController *	ConstructController(VehicleConstraint &inConstraint) const = 0;
+};
+
+/// Runtime data for interface that controls acceleration / deceleration of the vehicle
+class JPH_EXPORT VehicleController : public RefTarget<VehicleController>, public NonCopyable
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor / destructor
+	explicit					VehicleController(VehicleConstraint &inConstraint) : mConstraint(inConstraint) { }
+	virtual						~VehicleController() = default;
+
+	/// Access the vehicle constraint that this controller is part of
+	VehicleConstraint &			GetConstraint()								{ return mConstraint; }
+	const VehicleConstraint &	GetConstraint() const						{ return mConstraint; }
+
+protected:
+	// The functions below are only for the VehicleConstraint
+	friend class VehicleConstraint;
+
+	// Create a new instance of wheel
+	virtual Wheel *				ConstructWheel(const WheelSettings &inWheel) const = 0;
+
+	// If the vehicle is allowed to go to sleep
+	virtual bool				AllowSleep() const = 0;
+
+	// Called before the wheel probes have been done
+	virtual void				PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) = 0;
+
+	// Called after the wheel probes have been done
+	virtual void				PostCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) = 0;
+
+	// Solve longitudinal and lateral constraint parts for all of the wheels
+	virtual bool				SolveLongitudinalAndLateralConstraints(float inDeltaTime) = 0;
+
+	// Saving state for replay
+	virtual void				SaveState(StateRecorder &inStream) const = 0;
+	virtual void				RestoreState(StateRecorder &inStream) = 0;
+
+#ifdef JPH_DEBUG_RENDERER
+	// Drawing interface
+	virtual void				Draw(DebugRenderer *inRenderer) const = 0;
+#endif // JPH_DEBUG_RENDERER
+
+	VehicleConstraint &			mConstraint;								///< The vehicle constraint we belong to
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleDifferential.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleDifferential.cpp
new file mode 100644
index 0000000000..ef7cf4cb91
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleDifferential.cpp
@@ -0,0 +1,81 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Vehicle/VehicleDifferential.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(VehicleDifferentialSettings)
+{
+	JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mLeftWheel)
+	JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mRightWheel)
+	JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mDifferentialRatio)
+	JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mLeftRightSplit)
+	JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mLimitedSlipRatio)
+	JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mEngineTorqueRatio)
+}
+
+void VehicleDifferentialSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(mLeftWheel);
+	inStream.Write(mRightWheel);
+	inStream.Write(mDifferentialRatio);
+	inStream.Write(mLeftRightSplit);
+	inStream.Write(mLimitedSlipRatio);
+	inStream.Write(mEngineTorqueRatio);
+}
+
+void VehicleDifferentialSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	inStream.Read(mLeftWheel);
+	inStream.Read(mRightWheel);
+	inStream.Read(mDifferentialRatio);
+	inStream.Read(mLeftRightSplit);
+	inStream.Read(mLimitedSlipRatio);
+	inStream.Read(mEngineTorqueRatio);
+}
+
+void VehicleDifferentialSettings::CalculateTorqueRatio(float inLeftAngularVelocity, float inRightAngularVelocity, float &outLeftTorqueFraction, float &outRightTorqueFraction) const
+{
+	// Start with the default torque ratio
+	outLeftTorqueFraction = 1.0f - mLeftRightSplit;
+	outRightTorqueFraction = mLeftRightSplit;
+
+	if (mLimitedSlipRatio < FLT_MAX)
+	{
+		JPH_ASSERT(mLimitedSlipRatio > 1.0f);
+
+		// This is a limited slip differential, adjust torque ratios according to wheel speeds
+		float omega_l = max(1.0e-3f, abs(inLeftAngularVelocity)); // prevent div by zero by setting a minimum velocity and ignoring that the wheels may be rotating in different directions
+		float omega_r = max(1.0e-3f, abs(inRightAngularVelocity));
+		float omega_min = min(omega_l, omega_r);
+		float omega_max = max(omega_l, omega_r);
+
+		// Map into a value that is 0 when the wheels are turning at an equal rate and 1 when the wheels are turning at mLimitedSlipRotationRatio
+		float alpha = min((omega_max / omega_min - 1.0f) / (mLimitedSlipRatio - 1.0f), 1.0f);
+		JPH_ASSERT(alpha >= 0.0f);
+		float one_min_alpha = 1.0f - alpha;
+
+		if (omega_l < omega_r)
+		{
+			// Redirect more power to the left wheel
+			outLeftTorqueFraction = outLeftTorqueFraction * one_min_alpha + alpha;
+			outRightTorqueFraction = outRightTorqueFraction * one_min_alpha;
+		}
+		else
+		{
+			// Redirect more power to the right wheel
+			outLeftTorqueFraction = outLeftTorqueFraction * one_min_alpha;
+			outRightTorqueFraction = outRightTorqueFraction * one_min_alpha + alpha;
+		}
+	}
+
+	// Assert the values add up to 1
+	JPH_ASSERT(abs(outLeftTorqueFraction + outRightTorqueFraction - 1.0f) < 1.0e-6f);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleDifferential.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleDifferential.h
new file mode 100644
index 0000000000..3043371717
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleDifferential.h
@@ -0,0 +1,39 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/ObjectStream/SerializableObject.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+class JPH_EXPORT VehicleDifferentialSettings
+{
+	JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, VehicleDifferentialSettings)
+
+public:
+	/// Saves the contents in binary form to inStream.
+	void					SaveBinaryState(StreamOut &inStream) const;
+
+	/// Restores the contents in binary form to inStream.
+	void					RestoreBinaryState(StreamIn &inStream);
+
+	/// Calculate the torque ratio between left and right wheel
+	/// @param inLeftAngularVelocity Angular velocity of left wheel (rad / s)
+	/// @param inRightAngularVelocity Angular velocity of right wheel (rad / s)
+	/// @param outLeftTorqueFraction Fraction of torque that should go to the left wheel
+	/// @param outRightTorqueFraction Fraction of torque that should go to the right wheel
+	void					CalculateTorqueRatio(float inLeftAngularVelocity, float inRightAngularVelocity, float &outLeftTorqueFraction, float &outRightTorqueFraction) const;
+
+	int						mLeftWheel = -1;							///< Index (in mWheels) that represents the left wheel of this differential (can be -1 to indicate no wheel)
+	int						mRightWheel = -1;							///< Index (in mWheels) that represents the right wheel of this differential (can be -1 to indicate no wheel)
+	float					mDifferentialRatio = 3.42f;					///< Ratio between rotation speed of gear box and wheels
+	float					mLeftRightSplit = 0.5f;						///< Defines how the engine torque is split across the left and right wheel (0 = left, 0.5 = center, 1 = right)
+	float					mLimitedSlipRatio = 1.4f;					///< Ratio max / min wheel speed. When this ratio is exceeded, all torque gets distributed to the slowest moving wheel. This allows implementing a limited slip differential. Set to FLT_MAX for an open differential. Value should be > 1.
+	float					mEngineTorqueRatio = 1.0f;					///< How much of the engines torque is applied to this differential (0 = none, 1 = full), make sure the sum of all differentials is 1.
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleEngine.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleEngine.cpp
new file mode 100644
index 0000000000..1352cc0d51
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleEngine.cpp
@@ -0,0 +1,122 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Vehicle/VehicleEngine.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(VehicleEngineSettings)
+{
+	JPH_ADD_ATTRIBUTE(VehicleEngineSettings, mMaxTorque)
+	JPH_ADD_ATTRIBUTE(VehicleEngineSettings, mMinRPM)
+	JPH_ADD_ATTRIBUTE(VehicleEngineSettings, mMaxRPM)
+	JPH_ADD_ATTRIBUTE(VehicleEngineSettings, mNormalizedTorque)
+}
+
+VehicleEngineSettings::VehicleEngineSettings()
+{
+	mNormalizedTorque.Reserve(3);
+	mNormalizedTorque.AddPoint(0.0f, 0.8f);
+	mNormalizedTorque.AddPoint(0.66f, 1.0f);
+	mNormalizedTorque.AddPoint(1.0f, 0.8f);
+}
+
+void VehicleEngineSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(mMaxTorque);
+	inStream.Write(mMinRPM);
+	inStream.Write(mMaxRPM);
+	mNormalizedTorque.SaveBinaryState(inStream);
+}
+
+void VehicleEngineSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	inStream.Read(mMaxTorque);
+	inStream.Read(mMinRPM);
+	inStream.Read(mMaxRPM);
+	mNormalizedTorque.RestoreBinaryState(inStream);
+}
+
+void VehicleEngine::ApplyTorque(float inTorque, float inDeltaTime)
+{
+	// Accelerate engine using torque
+	mCurrentRPM += cAngularVelocityToRPM * inTorque * inDeltaTime / mInertia;
+	ClampRPM();
+}
+
+void VehicleEngine::ApplyDamping(float inDeltaTime)
+{
+	// Angular damping: dw/dt = -c * w
+	// Solution: w(t) = w(0) * e^(-c * t) or w2 = w1 * e^(-c * dt)
+	// Taylor expansion of e^(-c * dt) = 1 - c * dt + ...
+	// Since dt is usually in the order of 1/60 and c is a low number too this approximation is good enough
+	mCurrentRPM *= max(0.0f, 1.0f - mAngularDamping * inDeltaTime);
+	ClampRPM();
+}
+
+#ifdef JPH_DEBUG_RENDERER
+
+void VehicleEngine::DrawRPM(DebugRenderer *inRenderer, RVec3Arg inPosition, Vec3Arg inForward, Vec3Arg inUp, float inSize, float inShiftDownRPM, float inShiftUpRPM) const
+{
+	// Function to draw part of a pie
+	auto draw_pie = [this, inRenderer, inSize, inPosition, inForward, inUp](float inMinRPM, float inMaxRPM, Color inColor) {
+		inRenderer->DrawPie(inPosition, inSize, inForward, inUp, ConvertRPMToAngle(inMinRPM), ConvertRPMToAngle(inMaxRPM), inColor, DebugRenderer::ECastShadow::Off);
+	};
+
+	// Draw segment under min RPM
+	draw_pie(0, mMinRPM, Color::sGrey);
+
+	// Draw segment until inShiftDownRPM
+	if (mCurrentRPM < inShiftDownRPM)
+	{
+		draw_pie(mMinRPM, mCurrentRPM, Color::sRed);
+		draw_pie(mCurrentRPM, inShiftDownRPM, Color::sDarkRed);
+	}
+	else
+	{
+		draw_pie(mMinRPM, inShiftDownRPM, Color::sRed);
+	}
+
+	// Draw segment between inShiftDownRPM and inShiftUpRPM
+	if (mCurrentRPM > inShiftDownRPM && mCurrentRPM < inShiftUpRPM)
+	{
+		draw_pie(inShiftDownRPM, mCurrentRPM, Color::sOrange);
+		draw_pie(mCurrentRPM, inShiftUpRPM, Color::sDarkOrange);
+	}
+	else
+	{
+		draw_pie(inShiftDownRPM, inShiftUpRPM, mCurrentRPM <= inShiftDownRPM? Color::sDarkOrange : Color::sOrange);
+	}
+
+	// Draw segment above inShiftUpRPM
+	if (mCurrentRPM > inShiftUpRPM)
+	{
+		draw_pie(inShiftUpRPM, mCurrentRPM, Color::sGreen);
+		draw_pie(mCurrentRPM, mMaxRPM, Color::sDarkGreen);
+	}
+	else
+	{
+		draw_pie(inShiftUpRPM, mMaxRPM, Color::sDarkGreen);
+	}
+}
+
+#endif // JPH_DEBUG_RENDERER
+
+void VehicleEngine::SaveState(StateRecorder &inStream) const
+{
+	inStream.Write(mCurrentRPM);
+}
+
+void VehicleEngine::RestoreState(StateRecorder &inStream)
+{
+	inStream.Read(mCurrentRPM);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleEngine.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleEngine.h
new file mode 100644
index 0000000000..29212ffdab
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleEngine.h
@@ -0,0 +1,93 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/ObjectStream/SerializableObject.h>
+#include <Jolt/Core/LinearCurve.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/Physics/StateRecorder.h>
+
+JPH_NAMESPACE_BEGIN
+
+#ifdef JPH_DEBUG_RENDERER
+	class DebugRenderer;
+#endif // JPH_DEBUG_RENDERER
+
+/// Generic properties for a vehicle engine
+class JPH_EXPORT VehicleEngineSettings
+{
+	JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, VehicleEngineSettings)
+
+public:
+	/// Constructor
+							VehicleEngineSettings();
+
+	/// Saves the contents in binary form to inStream.
+	void					SaveBinaryState(StreamOut &inStream) const;
+
+	/// Restores the contents in binary form to inStream.
+	void					RestoreBinaryState(StreamIn &inStream);
+
+	float					mMaxTorque = 500.0f;						///< Max amount of torque (Nm) that the engine can deliver
+	float					mMinRPM = 1000.0f;							///< Min amount of revolutions per minute (rpm) the engine can produce without stalling
+	float					mMaxRPM = 6000.0f;							///< Max amount of revolutions per minute (rpm) the engine can generate
+	LinearCurve				mNormalizedTorque;							///< Y-axis: Curve that describes a ratio of the max torque the engine can produce (0 = 0, 1 = mMaxTorque). X-axis: the fraction of the RPM of the engine (0 = mMinRPM, 1 = mMaxRPM)
+	float					mInertia = 0.5f;							///< Moment of inertia (kg m^2) of the engine
+	float					mAngularDamping = 0.2f;						///< Angular damping factor of the wheel: dw/dt = -c * w
+};
+
+/// Runtime data for engine
+class JPH_EXPORT VehicleEngine : public VehicleEngineSettings
+{
+public:
+	/// Multiply an angular velocity (rad/s) with this value to get rounds per minute (RPM)
+	static constexpr float	cAngularVelocityToRPM = 60.0f / (2.0f * JPH_PI);
+
+	/// Clamp the RPM between min and max RPM
+	inline void				ClampRPM()									{ mCurrentRPM = Clamp(mCurrentRPM, mMinRPM, mMaxRPM); }
+
+	/// Current rotation speed of engine in rounds per minute
+	float					GetCurrentRPM() const						{ return mCurrentRPM; }
+
+	/// Update rotation speed of engine in rounds per minute
+	void					SetCurrentRPM(float inRPM)					{ mCurrentRPM = inRPM; ClampRPM(); }
+
+	/// Get current angular velocity of the engine in radians / second
+	inline float			GetAngularVelocity() const					{ return mCurrentRPM / cAngularVelocityToRPM; }
+
+	/// Get the amount of torque (N m) that the engine can supply
+	/// @param inAcceleration How much the gas pedal is pressed [0, 1]
+	float					GetTorque(float inAcceleration) const		{ return inAcceleration * mMaxTorque * mNormalizedTorque.GetValue(mCurrentRPM / mMaxRPM); }
+
+	/// Apply a torque to the engine rotation speed
+	/// @param inTorque Torque in N m
+	/// @param inDeltaTime Delta time in seconds
+	void					ApplyTorque(float inTorque, float inDeltaTime);
+
+	/// Update the engine RPM for damping
+	/// @param inDeltaTime Delta time in seconds
+	void					ApplyDamping(float inDeltaTime);
+
+#ifdef JPH_DEBUG_RENDERER
+	// Function that converts RPM to an angle in radians for debugging purposes
+	float					ConvertRPMToAngle(float inRPM) const		{ return (-0.75f + 1.5f * inRPM / mMaxRPM) * JPH_PI; }
+
+	/// Debug draw a RPM meter
+	void					DrawRPM(DebugRenderer *inRenderer, RVec3Arg inPosition, Vec3Arg inForward, Vec3Arg inUp, float inSize, float inShiftDownRPM, float inShiftUpRPM) const;
+#endif // JPH_DEBUG_RENDERER
+
+	/// If the engine is idle we allow the vehicle to sleep
+	bool					AllowSleep() const							{ return mCurrentRPM <= 1.01f * mMinRPM; }
+
+	/// Saving state for replay
+	void					SaveState(StateRecorder &inStream) const;
+	void					RestoreState(StateRecorder &inStream);
+
+private:
+	float					mCurrentRPM = mMinRPM;						///< Current rotation speed of engine in rounds per minute
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTrack.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTrack.cpp
new file mode 100644
index 0000000000..3bef2e8049
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTrack.cpp
@@ -0,0 +1,52 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Vehicle/VehicleTrack.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(VehicleTrackSettings)
+{
+	JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mDrivenWheel)
+	JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mWheels)
+	JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mInertia)
+	JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mAngularDamping)
+	JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mMaxBrakeTorque)
+	JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mDifferentialRatio)
+}
+
+void VehicleTrackSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(mDrivenWheel);
+	inStream.Write(mWheels);
+	inStream.Write(mInertia);
+	inStream.Write(mAngularDamping);
+	inStream.Write(mMaxBrakeTorque);
+	inStream.Write(mDifferentialRatio);
+}
+
+void VehicleTrackSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	inStream.Read(mDrivenWheel);
+	inStream.Read(mWheels);
+	inStream.Read(mInertia);
+	inStream.Read(mAngularDamping);
+	inStream.Read(mMaxBrakeTorque);
+	inStream.Read(mDifferentialRatio);
+}
+
+void VehicleTrack::SaveState(StateRecorder &inStream) const
+{
+	inStream.Write(mAngularVelocity);
+}
+
+void VehicleTrack::RestoreState(StateRecorder &inStream)
+{
+	inStream.Read(mAngularVelocity);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTrack.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTrack.h
new file mode 100644
index 0000000000..dae4f9842e
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTrack.h
@@ -0,0 +1,56 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/ObjectStream/SerializableObject.h>
+#include <Jolt/Core/LinearCurve.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/Physics/StateRecorder.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// On which side of the vehicle the track is located (for steering)
+enum class ETrackSide : uint
+{
+	Left = 0,
+	Right = 1,
+	Num = 2
+};
+
+/// Generic properties for tank tracks
+class JPH_EXPORT VehicleTrackSettings
+{
+	JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, VehicleTrackSettings)
+
+public:
+	/// Saves the contents in binary form to inStream.
+	void					SaveBinaryState(StreamOut &inStream) const;
+
+	/// Restores the contents in binary form to inStream.
+	void					RestoreBinaryState(StreamIn &inStream);
+
+	uint					mDrivenWheel;								///< Which wheel on the track is connected to the engine
+	Array<uint>				mWheels;									///< Indices of wheels that are inside this track, should include the driven wheel too
+	float					mInertia = 10.0f;							///< Moment of inertia (kg m^2) of the track and its wheels as seen on the driven wheel
+	float					mAngularDamping = 0.5f;						///< Damping factor of track and its wheels: dw/dt = -c * w as seen on the driven wheel
+	float					mMaxBrakeTorque = 15000.0f;					///< How much torque (Nm) the brakes can apply on the driven wheel
+	float					mDifferentialRatio = 6.0f;					///< Ratio between rotation speed of gear box and driven wheel of track
+};
+
+/// Runtime data for tank tracks
+class JPH_EXPORT VehicleTrack : public VehicleTrackSettings
+{
+public:
+	/// Saving state for replay
+	void					SaveState(StateRecorder &inStream) const;
+	void					RestoreState(StateRecorder &inStream);
+
+	float					mAngularVelocity = 0.0f;					///< Angular velocity of the driven wheel, will determine the speed of the entire track
+};
+
+using VehicleTracks = VehicleTrack[(int)ETrackSide::Num];
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTransmission.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTransmission.cpp
new file mode 100644
index 0000000000..43336a537f
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTransmission.cpp
@@ -0,0 +1,159 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Vehicle/VehicleTransmission.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(VehicleTransmissionSettings)
+{
+	JPH_ADD_ENUM_ATTRIBUTE(VehicleTransmissionSettings, mMode)
+	JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mGearRatios)
+	JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mReverseGearRatios)
+	JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mSwitchTime)
+	JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mClutchReleaseTime)
+	JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mSwitchLatency)
+	JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mShiftUpRPM)
+	JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mShiftDownRPM)
+	JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mClutchStrength)
+}
+
+void VehicleTransmissionSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(mMode);
+	inStream.Write(mGearRatios);
+	inStream.Write(mReverseGearRatios);
+	inStream.Write(mSwitchTime);
+	inStream.Write(mClutchReleaseTime);
+	inStream.Write(mSwitchLatency);
+	inStream.Write(mShiftUpRPM);
+	inStream.Write(mShiftDownRPM);
+	inStream.Write(mClutchStrength);
+}
+
+void VehicleTransmissionSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	inStream.Read(mMode);
+	inStream.Read(mGearRatios);
+	inStream.Read(mReverseGearRatios);
+	inStream.Read(mSwitchTime);
+	inStream.Read(mClutchReleaseTime);
+	inStream.Read(mSwitchLatency);
+	inStream.Read(mShiftUpRPM);
+	inStream.Read(mShiftDownRPM);
+	inStream.Read(mClutchStrength);
+}
+
+void VehicleTransmission::Update(float inDeltaTime, float inCurrentRPM, float inForwardInput, bool inCanShiftUp)
+{
+	// Update current gear and calculate clutch friction
+	if (mMode == ETransmissionMode::Auto)
+	{
+		// Switch gears based on rpm
+		int old_gear = mCurrentGear;
+		if (mCurrentGear == 0 // In neutral
+			|| inForwardInput * float(mCurrentGear) < 0.0f) // Changing between forward / reverse
+		{
+			// Switch to first gear or reverse depending on input
+			mCurrentGear = inForwardInput > 0.0f? 1 : (inForwardInput < 0.0f? -1 : 0);
+		}
+		else if (mGearSwitchLatencyTimeLeft == 0.0f) // If not in the timout after switching gears
+		{
+			if (inCanShiftUp && inCurrentRPM > mShiftUpRPM)
+			{
+				if (mCurrentGear < 0)
+				{
+					// Shift up, reverse
+					if (mCurrentGear > -(int)mReverseGearRatios.size())
+						mCurrentGear--;
+				}
+				else
+				{
+					// Shift up, forward
+					if (mCurrentGear < (int)mGearRatios.size())
+						mCurrentGear++;
+				}
+			}
+			else if (inCurrentRPM < mShiftDownRPM)
+			{
+				if (mCurrentGear < 0)
+				{
+					// Shift down, reverse
+					int max_gear = inForwardInput != 0.0f? -1 : 0;
+					if (mCurrentGear < max_gear)
+						mCurrentGear++;
+				}
+				else
+				{
+					// Shift down, forward
+					int min_gear = inForwardInput != 0.0f? 1 : 0;
+					if (mCurrentGear > min_gear)
+						mCurrentGear--;
+				}
+			}
+		}
+
+		if (old_gear != mCurrentGear)
+		{
+			// We've shifted gear, start switch countdown
+			mGearSwitchTimeLeft = old_gear != 0? mSwitchTime : 0.0f;
+			mClutchReleaseTimeLeft = mClutchReleaseTime;
+			mGearSwitchLatencyTimeLeft = mSwitchLatency;
+			mClutchFriction = 0.0f;
+		}
+		else if (mGearSwitchTimeLeft > 0.0f)
+		{
+			// If still switching gears, count down
+			mGearSwitchTimeLeft = max(0.0f, mGearSwitchTimeLeft - inDeltaTime);
+			mClutchFriction = 0.0f;
+		}
+		else if (mClutchReleaseTimeLeft > 0.0f)
+		{
+			// After switching the gears we slowly release the clutch
+			mClutchReleaseTimeLeft = max(0.0f, mClutchReleaseTimeLeft - inDeltaTime);
+			mClutchFriction = 1.0f - mClutchReleaseTimeLeft / mClutchReleaseTime;
+		}
+		else
+		{
+			// Clutch has full friction
+			mClutchFriction = 1.0f;
+
+			// Count down switch latency
+			mGearSwitchLatencyTimeLeft = max(0.0f, mGearSwitchLatencyTimeLeft - inDeltaTime);
+		}
+	}
+}
+
+float VehicleTransmission::GetCurrentRatio() const
+{
+	if (mCurrentGear < 0)
+		return mReverseGearRatios[-mCurrentGear - 1];
+	else if (mCurrentGear == 0)
+		return 0.0f;
+	else
+		return mGearRatios[mCurrentGear - 1];
+}
+
+void VehicleTransmission::SaveState(StateRecorder &inStream) const
+{
+	inStream.Write(mCurrentGear);
+	inStream.Write(mClutchFriction);
+	inStream.Write(mGearSwitchTimeLeft);
+	inStream.Write(mClutchReleaseTimeLeft);
+	inStream.Write(mGearSwitchLatencyTimeLeft);
+}
+
+void VehicleTransmission::RestoreState(StateRecorder &inStream)
+{
+	inStream.Read(mCurrentGear);
+	inStream.Read(mClutchFriction);
+	inStream.Read(mGearSwitchTimeLeft);
+	inStream.Read(mClutchReleaseTimeLeft);
+	inStream.Read(mGearSwitchLatencyTimeLeft);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTransmission.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTransmission.h
new file mode 100644
index 0000000000..3360217abb
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleTransmission.h
@@ -0,0 +1,87 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/ObjectStream/SerializableObject.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/Physics/StateRecorder.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// How gears are shifted
+enum class ETransmissionMode : uint8
+{
+	Auto,																///< Automatically shift gear up and down
+	Manual,																///< Manual gear shift (call SetTransmissionInput)
+};
+
+/// Configuration for the transmission of a vehicle (gear box)
+class JPH_EXPORT VehicleTransmissionSettings
+{
+	JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, VehicleTransmissionSettings)
+
+public:
+	/// Saves the contents in binary form to inStream.
+	void					SaveBinaryState(StreamOut &inStream) const;
+
+	/// Restores the contents in binary form to inStream.
+	void					RestoreBinaryState(StreamIn &inStream);
+
+	ETransmissionMode		mMode = ETransmissionMode::Auto;			///< How to switch gears
+	Array<float>			mGearRatios { 2.66f, 1.78f, 1.3f, 1.0f, 0.74f }; ///< Ratio in rotation rate between engine and gear box, first element is 1st gear, 2nd element 2nd gear etc.
+	Array<float>			mReverseGearRatios { -2.90f };				///< Ratio in rotation rate between engine and gear box when driving in reverse
+	float					mSwitchTime = 0.5f;							///< How long it takes to switch gears (s), only used in auto mode
+	float					mClutchReleaseTime = 0.3f;					///< How long it takes to release the clutch (go to full friction), only used in auto mode
+	float					mSwitchLatency = 0.5f;						///< How long to wait after releasing the clutch before another switch is attempted (s), only used in auto mode
+	float					mShiftUpRPM = 4000.0f;						///< If RPM of engine is bigger then this we will shift a gear up, only used in auto mode
+	float					mShiftDownRPM = 2000.0f;					///< If RPM of engine is smaller then this we will shift a gear down, only used in auto mode
+	float					mClutchStrength = 10.0f;					///< Strength of the clutch when fully engaged. Total torque a clutch applies is Torque = ClutchStrength * (Velocity Engine - Avg Velocity Wheels At Clutch) (units: k m^2 s^-1)
+};
+
+/// Runtime data for transmission
+class JPH_EXPORT VehicleTransmission : public VehicleTransmissionSettings
+{
+public:
+	/// Set input from driver regarding the transmission (only relevant when transmission is set to manual mode)
+	/// @param inCurrentGear Current gear, -1 = reverse, 0 = neutral, 1 = 1st gear etc.
+	/// @param inClutchFriction Value between 0 and 1 indicating how much friction the clutch gives (0 = no friction, 1 = full friction)
+	void					Set(int inCurrentGear, float inClutchFriction) { mCurrentGear = inCurrentGear; mClutchFriction = inClutchFriction; }
+
+	/// Update the current gear and clutch friction if the transmission is in auto mode
+	/// @param inDeltaTime Time step delta time in s
+	/// @param inCurrentRPM Current RPM for engine
+	/// @param inForwardInput Hint if the user wants to drive forward (> 0) or backwards (< 0)
+	/// @param inCanShiftUp Indicates if we want to allow the transmission to shift up (e.g. pass false if wheels are slipping)
+	void					Update(float inDeltaTime, float inCurrentRPM, float inForwardInput, bool inCanShiftUp);
+
+	/// Current gear, -1 = reverse, 0 = neutral, 1 = 1st gear etc.
+	int						GetCurrentGear() const						{ return mCurrentGear; }
+
+	/// Value between 0 and 1 indicating how much friction the clutch gives (0 = no friction, 1 = full friction)
+	float					GetClutchFriction() const					{ return mClutchFriction; }
+
+	/// If the auto box is currently switching gears
+	bool					IsSwitchingGear() const						{ return mGearSwitchTimeLeft > 0.0f; }
+
+	/// Return the transmission ratio based on the current gear (ratio between engine and differential)
+	float					GetCurrentRatio() const;
+
+	/// Only allow sleeping when the transmission is idle
+	bool					AllowSleep() const							{ return mGearSwitchTimeLeft <= 0.0f && mClutchReleaseTimeLeft <= 0.0f && mGearSwitchLatencyTimeLeft <= 0.0f; }
+
+	/// Saving state for replay
+	void					SaveState(StateRecorder &inStream) const;
+	void					RestoreState(StateRecorder &inStream);
+
+private:
+	int						mCurrentGear = 0;							///< Current gear, -1 = reverse, 0 = neutral, 1 = 1st gear etc.
+	float					mClutchFriction = 1.0f;						///< Value between 0 and 1 indicating how much friction the clutch gives (0 = no friction, 1 = full friction)
+	float					mGearSwitchTimeLeft = 0.0f;					///< When switching gears this will be > 0 and will cause the engine to not provide any torque to the wheels for a short time (used for automatic gear switching only)
+	float					mClutchReleaseTimeLeft = 0.0f;				///< After switching gears this will be > 0 and will cause the clutch friction to go from 0 to 1 (used for automatic gear switching only)
+	float					mGearSwitchLatencyTimeLeft = 0.0f;			///< After releasing the clutch this will be > 0 and will prevent another gear switch (used for automatic gear switching only)
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/Wheel.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/Wheel.cpp
new file mode 100644
index 0000000000..e43ffb8c9a
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/Wheel.cpp
@@ -0,0 +1,93 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Vehicle/Wheel.h>
+#include <Jolt/Physics/Vehicle/VehicleConstraint.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(WheelSettings)
+{
+	JPH_ADD_ATTRIBUTE(WheelSettings, mSuspensionForcePoint)
+	JPH_ADD_ATTRIBUTE(WheelSettings, mPosition)
+	JPH_ADD_ATTRIBUTE(WheelSettings, mSuspensionDirection)
+	JPH_ADD_ATTRIBUTE(WheelSettings, mSteeringAxis)
+	JPH_ADD_ATTRIBUTE(WheelSettings, mWheelForward)
+	JPH_ADD_ATTRIBUTE(WheelSettings, mWheelUp)
+	JPH_ADD_ATTRIBUTE(WheelSettings, mSuspensionMinLength)
+	JPH_ADD_ATTRIBUTE(WheelSettings, mSuspensionMaxLength)
+	JPH_ADD_ATTRIBUTE(WheelSettings, mSuspensionPreloadLength)
+	JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(WheelSettings, mSuspensionSpring.mMode, "mSuspensionSpringMode")
+	JPH_ADD_ATTRIBUTE_WITH_ALIAS(WheelSettings, mSuspensionSpring.mFrequency, "mSuspensionFrequency") // Renaming attributes to stay compatible with old versions of the library
+	JPH_ADD_ATTRIBUTE_WITH_ALIAS(WheelSettings, mSuspensionSpring.mDamping, "mSuspensionDamping")
+	JPH_ADD_ATTRIBUTE(WheelSettings, mRadius)
+	JPH_ADD_ATTRIBUTE(WheelSettings, mWidth)
+	JPH_ADD_ATTRIBUTE(WheelSettings, mEnableSuspensionForcePoint)
+}
+
+void WheelSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(mSuspensionForcePoint);
+	inStream.Write(mPosition);
+	inStream.Write(mSuspensionDirection);
+	inStream.Write(mSteeringAxis);
+	inStream.Write(mWheelForward);
+	inStream.Write(mWheelUp);
+	inStream.Write(mSuspensionMinLength);
+	inStream.Write(mSuspensionMaxLength);
+	inStream.Write(mSuspensionPreloadLength);
+	mSuspensionSpring.SaveBinaryState(inStream);
+	inStream.Write(mRadius);
+	inStream.Write(mWidth);
+	inStream.Write(mEnableSuspensionForcePoint);
+}
+
+void WheelSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	inStream.Read(mSuspensionForcePoint);
+	inStream.Read(mPosition);
+	inStream.Read(mSuspensionDirection);
+	inStream.Read(mSteeringAxis);
+	inStream.Read(mWheelForward);
+	inStream.Read(mWheelUp);
+	inStream.Read(mSuspensionMinLength);
+	inStream.Read(mSuspensionMaxLength);
+	inStream.Read(mSuspensionPreloadLength);
+	mSuspensionSpring.RestoreBinaryState(inStream);
+	inStream.Read(mRadius);
+	inStream.Read(mWidth);
+	inStream.Read(mEnableSuspensionForcePoint);
+}
+
+Wheel::Wheel(const WheelSettings &inSettings) :
+	mSettings(&inSettings),
+	mSuspensionLength(inSettings.mSuspensionMaxLength)
+{
+	JPH_ASSERT(inSettings.mSuspensionDirection.IsNormalized());
+	JPH_ASSERT(inSettings.mSteeringAxis.IsNormalized());
+	JPH_ASSERT(inSettings.mWheelForward.IsNormalized());
+	JPH_ASSERT(inSettings.mWheelUp.IsNormalized());
+	JPH_ASSERT(inSettings.mSuspensionMinLength >= 0.0f);
+	JPH_ASSERT(inSettings.mSuspensionMaxLength >= inSettings.mSuspensionMinLength);
+	JPH_ASSERT(inSettings.mSuspensionPreloadLength >= 0.0f);
+	JPH_ASSERT(inSettings.mSuspensionSpring.mFrequency > 0.0f);
+	JPH_ASSERT(inSettings.mSuspensionSpring.mDamping >= 0.0f);
+	JPH_ASSERT(inSettings.mRadius > 0.0f);
+	JPH_ASSERT(inSettings.mWidth >= 0.0f);
+}
+
+bool Wheel::SolveLongitudinalConstraintPart(const VehicleConstraint &inConstraint, float inMinImpulse, float inMaxImpulse)
+{
+	return mLongitudinalPart.SolveVelocityConstraint(*inConstraint.GetVehicleBody(), *mContactBody, -mContactLongitudinal, inMinImpulse, inMaxImpulse);
+}
+
+bool Wheel::SolveLateralConstraintPart(const VehicleConstraint &inConstraint, float inMinImpulse, float inMaxImpulse)
+{
+	return mLateralPart.SolveVelocityConstraint(*inConstraint.GetVehicleBody(), *mContactBody, -mContactLateral, inMinImpulse, inMaxImpulse);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/Wheel.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/Wheel.h
new file mode 100644
index 0000000000..0e92caa7a4
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/Wheel.h
@@ -0,0 +1,148 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Body/Body.h>
+#include <Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h>
+#include <Jolt/ObjectStream/SerializableObject.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+class VehicleConstraint;
+
+/// Base class for wheel settings, each VehicleController can implement a derived class of this
+class JPH_EXPORT WheelSettings : public SerializableObject, public RefTarget<WheelSettings>
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, WheelSettings)
+
+public:
+	/// Saves the contents in binary form to inStream.
+	virtual void			SaveBinaryState(StreamOut &inStream) const;
+
+	/// Restores the contents in binary form to inStream.
+	virtual void			RestoreBinaryState(StreamIn &inStream);
+
+	Vec3					mPosition { 0, 0, 0 };						///< Attachment point of wheel suspension in local space of the body
+	Vec3					mSuspensionForcePoint { 0, 0, 0 };			///< Where tire forces (suspension and traction) are applied, in local space of the body. A good default is the center of the wheel in its neutral pose. See mEnableSuspensionForcePoint.
+	Vec3					mSuspensionDirection { 0, -1, 0 };			///< Direction of the suspension in local space of the body, should point down
+	Vec3					mSteeringAxis { 0, 1, 0 };					///< Direction of the steering axis in local space of the body, should point up (e.g. for a bike would be -mSuspensionDirection)
+	Vec3					mWheelUp { 0, 1, 0 };						///< Up direction when the wheel is in the neutral steering position (usually VehicleConstraintSettings::mUp but can be used to give the wheel camber or for a bike would be -mSuspensionDirection)
+	Vec3					mWheelForward { 0, 0, 1 };					///< Forward direction when the wheel is in the neutral steering position (usually VehicleConstraintSettings::mForward but can be used to give the wheel toe, does not need to be perpendicular to mWheelUp)
+	float					mSuspensionMinLength = 0.3f;				///< How long the suspension is in max raised position relative to the attachment point (m)
+	float					mSuspensionMaxLength = 0.5f;				///< How long the suspension is in max droop position relative to the attachment point (m)
+	float					mSuspensionPreloadLength = 0.0f;			///< The natural length (m) of the suspension spring is defined as mSuspensionMaxLength + mSuspensionPreloadLength. Can be used to preload the suspension as the spring is compressed by mSuspensionPreloadLength when the suspension is in max droop position. Note that this means when the vehicle touches the ground there is a discontinuity so it will also make the vehicle more bouncy as we're updating with discrete time steps.
+	SpringSettings			mSuspensionSpring { ESpringMode::FrequencyAndDamping, 1.5f, 0.5f }; ///< Settings for the suspension spring
+	float					mRadius = 0.3f;								///< Radius of the wheel (m)
+	float					mWidth = 0.1f;								///< Width of the wheel (m)
+	bool					mEnableSuspensionForcePoint = false;		///< Enables mSuspensionForcePoint, if disabled, the forces are applied at the collision contact point. This leads to a more accurate simulation when interacting with dynamic objects but makes the vehicle less stable. When setting this to true, all forces will be applied to a fixed point on the vehicle body.
+};
+
+/// Base class for runtime data for a wheel, each VehicleController can implement a derived class of this
+class JPH_EXPORT Wheel : public NonCopyable
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor / destructor
+	explicit				Wheel(const WheelSettings &inSettings);
+	virtual					~Wheel() = default;
+
+	/// Get settings for the wheel
+	const WheelSettings *	GetSettings() const							{ return mSettings; }
+
+	/// Get the angular velocity (rad/s) for this wheel, note that positive means the wheel is rotating such that the car moves forward
+	float					GetAngularVelocity() const					{ return mAngularVelocity; }
+
+	/// Update the angular velocity (rad/s)
+	void					SetAngularVelocity(float inVel)				{ mAngularVelocity = inVel; }
+
+	/// Get the current rotation angle of the wheel in radians [0, 2 pi]
+	float					GetRotationAngle() const					{ return mAngle; }
+
+	/// Set the current rotation angle of the wheel in radians [0, 2 pi]
+	void					SetRotationAngle(float inAngle)				{ mAngle = inAngle; }
+
+	/// Get the current steer angle of the wheel in radians [-pi, pi], positive is to the left
+	float					GetSteerAngle() const						{ return mSteerAngle; }
+
+	/// Set the current steer angle of the wheel in radians [-pi, pi]
+	void					SetSteerAngle(float inAngle)				{ mSteerAngle = inAngle; }
+
+	/// Returns true if the wheel is touching an object
+	inline bool				HasContact() const							{ return !mContactBodyID.IsInvalid(); }
+
+	/// Returns the body ID of the body that this wheel is touching
+	BodyID					GetContactBodyID() const					{ return mContactBodyID; }
+
+	/// Returns the sub shape ID where we're contacting the body
+	SubShapeID				GetContactSubShapeID() const				{ return mContactSubShapeID; }
+
+	/// Returns the current contact position in world space (note by the time you call this the vehicle has moved)
+	RVec3					GetContactPosition() const					{ JPH_ASSERT(HasContact()); return mContactPosition; }
+
+	/// Velocity of the contact point (m / s, not relative to the wheel but in world space)
+	Vec3					GetContactPointVelocity() const				{ JPH_ASSERT(HasContact()); return mContactPointVelocity; }
+
+	/// Returns the current contact normal in world space (note by the time you call this the vehicle has moved)
+	Vec3					GetContactNormal() const					{ JPH_ASSERT(HasContact()); return mContactNormal; }
+
+	/// Returns longitudinal direction (direction along the wheel relative to floor) in world space (note by the time you call this the vehicle has moved)
+	Vec3					GetContactLongitudinal() const				{ JPH_ASSERT(HasContact()); return mContactLongitudinal; }
+
+	/// Returns lateral direction (sideways direction) in world space (note by the time you call this the vehicle has moved)
+	Vec3					GetContactLateral() const					{ JPH_ASSERT(HasContact()); return mContactLateral; }
+
+	/// Get the length of the suspension for a wheel (m) relative to the suspension attachment point (hard point)
+	float					GetSuspensionLength() const					{ return mSuspensionLength; }
+
+	/// Check if the suspension hit its upper limit
+	bool					HasHitHardPoint() const						{ return mSuspensionMaxUpPart.IsActive(); }
+
+	/// Get the total impulse (N s) that was applied by the suspension
+	float					GetSuspensionLambda() const					{ return mSuspensionPart.GetTotalLambda() + mSuspensionMaxUpPart.GetTotalLambda(); }
+
+	/// Get total impulse (N s) applied along the forward direction of the wheel
+	float					GetLongitudinalLambda() const				{ return mLongitudinalPart.GetTotalLambda(); }
+
+	/// Get total impulse (N s) applied along the sideways direction of the wheel
+	float					GetLateralLambda() const					{ return mLateralPart.GetTotalLambda(); }
+
+	/// Internal function that should only be called by the controller. Used to apply impulses in the forward direction of the vehicle.
+	bool					SolveLongitudinalConstraintPart(const VehicleConstraint &inConstraint, float inMinImpulse, float inMaxImpulse);
+
+	/// Internal function that should only be called by the controller. Used to apply impulses in the sideways direction of the vehicle.
+	bool					SolveLateralConstraintPart(const VehicleConstraint &inConstraint, float inMinImpulse, float inMaxImpulse);
+
+protected:
+	friend class VehicleConstraint;
+
+	RefConst<WheelSettings>	mSettings;									///< Configuration settings for this wheel
+	BodyID					mContactBodyID;								///< ID of body for ground
+	SubShapeID				mContactSubShapeID;							///< Sub shape ID for ground
+	Body *					mContactBody = nullptr;						///< Body for ground
+	float					mSuspensionLength;							///< Current length of the suspension
+	RVec3					mContactPosition;							///< Position of the contact point between wheel and ground
+	Vec3					mContactPointVelocity;						///< Velocity of the contact point (m / s, not relative to the wheel but in world space)
+	Vec3					mContactNormal;								///< Normal of the contact point between wheel and ground
+	Vec3					mContactLongitudinal;						///< Vector perpendicular to normal in the forward direction
+	Vec3					mContactLateral;							///< Vector perpendicular to normal and longitudinal direction in the right direction
+	Real					mAxlePlaneConstant;							///< Constant for the contact plane of the axle, defined as ContactNormal . (WorldSpaceSuspensionPoint + SuspensionLength * WorldSpaceSuspensionDirection)
+	float					mAntiRollBarImpulse = 0.0f;					///< Amount of impulse applied to the suspension from the anti-rollbars
+
+	float					mSteerAngle = 0.0f;							///< Rotation around the suspension direction, positive is to the left
+	float					mAngularVelocity = 0.0f;					///< Rotation speed of wheel, positive when the wheels cause the vehicle to move forwards (rad/s)
+	float					mAngle = 0.0f;								///< Current rotation of the wheel (rad, [0, 2 pi])
+
+	AxisConstraintPart		mSuspensionPart;							///< Controls movement up/down along the contact normal
+	AxisConstraintPart		mSuspensionMaxUpPart;						///< Adds a hard limit when reaching the minimal suspension length
+	AxisConstraintPart		mLongitudinalPart;							///< Controls movement forward/backward
+	AxisConstraintPart		mLateralPart;								///< Controls movement sideways (slip)
+};
+
+using Wheels = Array<Wheel *>;
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.cpp b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.cpp
new file mode 100644
index 0000000000..8e4a71a4fb
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.cpp
@@ -0,0 +1,845 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/Vehicle/WheeledVehicleController.h>
+#include <Jolt/Physics/PhysicsSystem.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/Math/DynMatrix.h>
+#include <Jolt/Math/GaussianElimination.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+//#define JPH_TRACE_VEHICLE_STATS
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(WheeledVehicleControllerSettings)
+{
+	JPH_ADD_BASE_CLASS(WheeledVehicleControllerSettings, VehicleControllerSettings)
+
+	JPH_ADD_ATTRIBUTE(WheeledVehicleControllerSettings, mEngine)
+	JPH_ADD_ATTRIBUTE(WheeledVehicleControllerSettings, mTransmission)
+	JPH_ADD_ATTRIBUTE(WheeledVehicleControllerSettings, mDifferentials)
+	JPH_ADD_ATTRIBUTE(WheeledVehicleControllerSettings, mDifferentialLimitedSlipRatio)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(WheelSettingsWV)
+{
+	JPH_ADD_ATTRIBUTE(WheelSettingsWV, mInertia)
+	JPH_ADD_ATTRIBUTE(WheelSettingsWV, mAngularDamping)
+	JPH_ADD_ATTRIBUTE(WheelSettingsWV, mMaxSteerAngle)
+	JPH_ADD_ATTRIBUTE(WheelSettingsWV, mLongitudinalFriction)
+	JPH_ADD_ATTRIBUTE(WheelSettingsWV, mLateralFriction)
+	JPH_ADD_ATTRIBUTE(WheelSettingsWV, mMaxBrakeTorque)
+	JPH_ADD_ATTRIBUTE(WheelSettingsWV, mMaxHandBrakeTorque)
+}
+
+WheelSettingsWV::WheelSettingsWV()
+{
+	mLongitudinalFriction.Reserve(3);
+	mLongitudinalFriction.AddPoint(0.0f, 0.0f);
+	mLongitudinalFriction.AddPoint(0.06f, 1.2f);
+	mLongitudinalFriction.AddPoint(0.2f, 1.0f);
+
+	mLateralFriction.Reserve(3);
+	mLateralFriction.AddPoint(0.0f, 0.0f);
+	mLateralFriction.AddPoint(3.0f, 1.2f);
+	mLateralFriction.AddPoint(20.0f, 1.0f);
+}
+
+void WheelSettingsWV::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(mInertia);
+	inStream.Write(mAngularDamping);
+	inStream.Write(mMaxSteerAngle);
+	mLongitudinalFriction.SaveBinaryState(inStream);
+	mLateralFriction.SaveBinaryState(inStream);
+	inStream.Write(mMaxBrakeTorque);
+	inStream.Write(mMaxHandBrakeTorque);
+}
+
+void WheelSettingsWV::RestoreBinaryState(StreamIn &inStream)
+{
+	inStream.Read(mInertia);
+	inStream.Read(mAngularDamping);
+	inStream.Read(mMaxSteerAngle);
+	mLongitudinalFriction.RestoreBinaryState(inStream);
+	mLateralFriction.RestoreBinaryState(inStream);
+	inStream.Read(mMaxBrakeTorque);
+	inStream.Read(mMaxHandBrakeTorque);
+}
+
+WheelWV::WheelWV(const WheelSettingsWV &inSettings) :
+	Wheel(inSettings)
+{
+	JPH_ASSERT(inSettings.mInertia >= 0.0f);
+	JPH_ASSERT(inSettings.mAngularDamping >= 0.0f);
+	JPH_ASSERT(abs(inSettings.mMaxSteerAngle) <= 0.5f * JPH_PI);
+	JPH_ASSERT(inSettings.mMaxBrakeTorque >= 0.0f);
+	JPH_ASSERT(inSettings.mMaxHandBrakeTorque >= 0.0f);
+}
+
+void WheelWV::Update(uint inWheelIndex, float inDeltaTime, const VehicleConstraint &inConstraint)
+{
+	const WheelSettingsWV *settings = GetSettings();
+
+	// Angular damping: dw/dt = -c * w
+	// Solution: w(t) = w(0) * e^(-c * t) or w2 = w1 * e^(-c * dt)
+	// Taylor expansion of e^(-c * dt) = 1 - c * dt + ...
+	// Since dt is usually in the order of 1/60 and c is a low number too this approximation is good enough
+	mAngularVelocity *= max(0.0f, 1.0f - settings->mAngularDamping * inDeltaTime);
+
+	// Update rotation of wheel
+	mAngle = fmod(mAngle + mAngularVelocity * inDeltaTime, 2.0f * JPH_PI);
+
+	if (mContactBody != nullptr)
+	{
+		const Body *body = inConstraint.GetVehicleBody();
+
+		// Calculate relative velocity between wheel contact point and floor
+		Vec3 relative_velocity = body->GetPointVelocity(mContactPosition) - mContactPointVelocity;
+
+		// Cancel relative velocity in the normal plane
+		relative_velocity -= mContactNormal.Dot(relative_velocity) * mContactNormal;
+		float relative_longitudinal_velocity = relative_velocity.Dot(mContactLongitudinal);
+
+		// Calculate longitudinal friction based on difference between velocity of rolling wheel and drive surface
+		float relative_longitudinal_velocity_denom = Sign(relative_longitudinal_velocity) * max(1.0e-3f, abs(relative_longitudinal_velocity)); // Ensure we don't divide by zero
+		mLongitudinalSlip = abs((mAngularVelocity * settings->mRadius - relative_longitudinal_velocity) / relative_longitudinal_velocity_denom);
+		float longitudinal_slip_friction = settings->mLongitudinalFriction.GetValue(mLongitudinalSlip);
+
+		// Calculate lateral friction based on slip angle
+		float relative_velocity_len = relative_velocity.Length();
+		mLateralSlip = relative_velocity_len < 1.0e-3f ? 0.0f : ACos(abs(relative_longitudinal_velocity) / relative_velocity_len);
+		float lateral_slip_angle = RadiansToDegrees(mLateralSlip);
+		float lateral_slip_friction = settings->mLateralFriction.GetValue(lateral_slip_angle);
+
+		// Tire friction
+		VehicleConstraint::CombineFunction combine_friction = inConstraint.GetCombineFriction();
+		mCombinedLongitudinalFriction = longitudinal_slip_friction;
+		mCombinedLateralFriction = lateral_slip_friction;
+		combine_friction(inWheelIndex, mCombinedLongitudinalFriction, mCombinedLateralFriction, *mContactBody, mContactSubShapeID);
+	}
+	else
+	{
+		// No collision
+		mLongitudinalSlip = 0.0f;
+		mLateralSlip = 0.0f;
+		mCombinedLongitudinalFriction = mCombinedLateralFriction = 0.0f;
+	}
+}
+
+VehicleController *WheeledVehicleControllerSettings::ConstructController(VehicleConstraint &inConstraint) const
+{
+	return new WheeledVehicleController(*this, inConstraint);
+}
+
+void WheeledVehicleControllerSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	mEngine.SaveBinaryState(inStream);
+
+	mTransmission.SaveBinaryState(inStream);
+
+	uint32 num_differentials = (uint32)mDifferentials.size();
+	inStream.Write(num_differentials);
+	for (const VehicleDifferentialSettings &d : mDifferentials)
+		d.SaveBinaryState(inStream);
+
+	inStream.Write(mDifferentialLimitedSlipRatio);
+}
+
+void WheeledVehicleControllerSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	mEngine.RestoreBinaryState(inStream);
+
+	mTransmission.RestoreBinaryState(inStream);
+
+	uint32 num_differentials = 0;
+	inStream.Read(num_differentials);
+	mDifferentials.resize(num_differentials);
+	for (VehicleDifferentialSettings &d : mDifferentials)
+		d.RestoreBinaryState(inStream);
+
+	inStream.Read(mDifferentialLimitedSlipRatio);
+}
+
+WheeledVehicleController::WheeledVehicleController(const WheeledVehicleControllerSettings &inSettings, VehicleConstraint &inConstraint) :
+	VehicleController(inConstraint)
+{
+	// Copy engine settings
+	static_cast<VehicleEngineSettings &>(mEngine) = inSettings.mEngine;
+	JPH_ASSERT(inSettings.mEngine.mMinRPM >= 0.0f);
+	JPH_ASSERT(inSettings.mEngine.mMinRPM <= inSettings.mEngine.mMaxRPM);
+	mEngine.SetCurrentRPM(mEngine.mMinRPM);
+
+	// Copy transmission settings
+	static_cast<VehicleTransmissionSettings &>(mTransmission) = inSettings.mTransmission;
+#ifdef JPH_ENABLE_ASSERTS
+	for (float r : inSettings.mTransmission.mGearRatios)
+		JPH_ASSERT(r > 0.0f);
+	for (float r : inSettings.mTransmission.mReverseGearRatios)
+		JPH_ASSERT(r < 0.0f);
+#endif // JPH_ENABLE_ASSERTS
+	JPH_ASSERT(inSettings.mTransmission.mSwitchTime >= 0.0f);
+	JPH_ASSERT(inSettings.mTransmission.mShiftDownRPM > 0.0f);
+	JPH_ASSERT(inSettings.mTransmission.mMode != ETransmissionMode::Auto || inSettings.mTransmission.mShiftUpRPM < inSettings.mEngine.mMaxRPM);
+	JPH_ASSERT(inSettings.mTransmission.mShiftUpRPM > inSettings.mTransmission.mShiftDownRPM);
+	JPH_ASSERT(inSettings.mTransmission.mClutchStrength > 0.0f);
+
+	// Copy differential settings
+	mDifferentials.resize(inSettings.mDifferentials.size());
+	for (uint i = 0; i < mDifferentials.size(); ++i)
+	{
+		const VehicleDifferentialSettings &d = inSettings.mDifferentials[i];
+		mDifferentials[i] = d;
+		JPH_ASSERT(d.mDifferentialRatio > 0.0f);
+		JPH_ASSERT(d.mLeftRightSplit >= 0.0f && d.mLeftRightSplit <= 1.0f);
+		JPH_ASSERT(d.mEngineTorqueRatio >= 0.0f);
+		JPH_ASSERT(d.mLimitedSlipRatio > 1.0f);
+	}
+
+	mDifferentialLimitedSlipRatio = inSettings.mDifferentialLimitedSlipRatio;
+	JPH_ASSERT(mDifferentialLimitedSlipRatio > 1.0f);
+}
+
+float WheeledVehicleController::GetWheelSpeedAtClutch() const
+{
+	float wheel_speed_at_clutch = 0.0f;
+	int num_driven_wheels = 0;
+	for (const VehicleDifferentialSettings &d : mDifferentials)
+	{
+		int wheels[] = { d.mLeftWheel, d.mRightWheel };
+		for (int w : wheels)
+			if (w >= 0)
+			{
+				wheel_speed_at_clutch += mConstraint.GetWheel(w)->GetAngularVelocity() * d.mDifferentialRatio;
+				num_driven_wheels++;
+			}
+	}
+	return wheel_speed_at_clutch / float(num_driven_wheels) * VehicleEngine::cAngularVelocityToRPM * mTransmission.GetCurrentRatio();
+}
+
+bool WheeledVehicleController::AllowSleep() const
+{
+	return mForwardInput == 0.0f								// No user input
+		&& mTransmission.AllowSleep()							// Transmission is not shifting
+		&& mEngine.AllowSleep();								// Engine is idling
+}
+
+void WheeledVehicleController::PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem)
+{
+	JPH_PROFILE_FUNCTION();
+
+#ifdef JPH_TRACE_VEHICLE_STATS
+	static bool sTracedHeader = false;
+	if (!sTracedHeader)
+	{
+		Trace("Time, ForwardInput, Gear, ClutchFriction, EngineRPM, WheelRPM, Velocity (km/h)");
+		sTracedHeader = true;
+	}
+	static float sTime = 0.0f;
+	sTime += inDeltaTime;
+	Trace("%.3f, %.1f, %d, %.1f, %.1f, %.1f, %.1f", sTime, mForwardInput, mTransmission.GetCurrentGear(), mTransmission.GetClutchFriction(), mEngine.GetCurrentRPM(), GetWheelSpeedAtClutch(), mConstraint.GetVehicleBody()->GetLinearVelocity().Length() * 3.6f);
+#endif // JPH_TRACE_VEHICLE_STATS
+
+	for (Wheel *w_base : mConstraint.GetWheels())
+	{
+		WheelWV *w = static_cast<WheelWV *>(w_base);
+
+		// Set steering angle
+		w->SetSteerAngle(-mRightInput * w->GetSettings()->mMaxSteerAngle);
+	}
+}
+
+void WheeledVehicleController::PostCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Remember old RPM so we can detect if we're increasing or decreasing
+	float old_engine_rpm = mEngine.GetCurrentRPM();
+
+	Wheels &wheels = mConstraint.GetWheels();
+
+	// Update wheel angle, do this before applying torque to the wheels (as friction will slow them down again)
+	for (uint wheel_index = 0, num_wheels = (uint)wheels.size(); wheel_index < num_wheels; ++wheel_index)
+	{
+		WheelWV *w = static_cast<WheelWV *>(wheels[wheel_index]);
+		w->Update(wheel_index, inDeltaTime, mConstraint);
+	}
+
+	// In auto transmission mode, don't accelerate the engine when switching gears
+	float forward_input = abs(mForwardInput);
+	if (mTransmission.mMode == ETransmissionMode::Auto)
+		forward_input *= mTransmission.GetClutchFriction();
+
+	// Apply engine damping
+	mEngine.ApplyDamping(inDeltaTime);
+
+	// Calculate engine torque
+	float engine_torque = mEngine.GetTorque(forward_input);
+
+	// Define a struct that contains information about driven differentials (i.e. that have wheels connected)
+	struct DrivenDifferential
+	{
+		const VehicleDifferentialSettings *	mDifferential;
+		float								mAngularVelocity;
+		float								mClutchToDifferentialTorqueRatio;
+		float								mTempTorqueFactor;
+	};
+
+	// Collect driven differentials and their speeds
+	Array<DrivenDifferential> driven_differentials;
+	driven_differentials.reserve(mDifferentials.size());
+	float differential_omega_min = FLT_MAX, differential_omega_max = 0.0f;
+	for (const VehicleDifferentialSettings &d : mDifferentials)
+	{
+		float avg_omega = 0.0f;
+		int avg_omega_denom = 0;
+		int indices[] = { d.mLeftWheel, d.mRightWheel };
+		for (int idx : indices)
+			if (idx != -1)
+			{
+				avg_omega += wheels[idx]->GetAngularVelocity();
+				avg_omega_denom++;
+			}
+
+		if (avg_omega_denom > 0)
+		{
+			avg_omega = abs(avg_omega * d.mDifferentialRatio / float(avg_omega_denom)); // ignoring that the differentials may be rotating in different directions
+			driven_differentials.push_back({ &d, avg_omega, d.mEngineTorqueRatio, 0 });
+
+			// Remember min and max velocity
+			differential_omega_min = min(differential_omega_min, avg_omega);
+			differential_omega_max = max(differential_omega_max, avg_omega);
+		}
+	}
+
+	if (mDifferentialLimitedSlipRatio < FLT_MAX					// Limited slip differential needs to be turned on
+		&& differential_omega_max > differential_omega_min)		// There needs to be a velocity difference
+	{
+		// Calculate factor based on relative speed of a differential
+		float sum_factor = 0.0f;
+		for (DrivenDifferential &d : driven_differentials)
+		{
+			// Differential with max velocity gets factor 0, differential with min velocity 1
+			d.mTempTorqueFactor = (differential_omega_max - d.mAngularVelocity) / (differential_omega_max - differential_omega_min);
+			sum_factor += d.mTempTorqueFactor;
+		}
+
+		// Normalize the result
+		for (DrivenDifferential &d : driven_differentials)
+			d.mTempTorqueFactor /= sum_factor;
+
+		// Prevent div by zero
+		differential_omega_min = max(1.0e-3f, differential_omega_min);
+		differential_omega_max = max(1.0e-3f, differential_omega_max);
+
+		// Map into a value that is 0 when the wheels are turning at an equal rate and 1 when the wheels are turning at mDifferentialLimitedSlipRatio
+		float alpha = min((differential_omega_max / differential_omega_min - 1.0f) / (mDifferentialLimitedSlipRatio - 1.0f), 1.0f);
+		JPH_ASSERT(alpha >= 0.0f);
+		float one_min_alpha = 1.0f - alpha;
+
+		// Update torque ratio for all differentials
+		for (DrivenDifferential &d : driven_differentials)
+			d.mClutchToDifferentialTorqueRatio = one_min_alpha * d.mClutchToDifferentialTorqueRatio + alpha * d.mTempTorqueFactor;
+	}
+
+#ifdef JPH_ENABLE_ASSERTS
+	// Assert the values add up to 1
+	float sum_torque_factors = 0.0f;
+	for (DrivenDifferential &d : driven_differentials)
+		sum_torque_factors += d.mClutchToDifferentialTorqueRatio;
+	JPH_ASSERT(abs(sum_torque_factors - 1.0f) < 1.0e-6f);
+#endif // JPH_ENABLE_ASSERTS
+
+	// Define a struct that collects information about the wheels that connect to the engine
+	struct DrivenWheel
+	{
+		WheelWV *				mWheel;
+		float					mClutchToWheelRatio;
+		float					mClutchToWheelTorqueRatio;
+		float					mEstimatedAngularImpulse;
+	};
+	Array<DrivenWheel> driven_wheels;
+	driven_wheels.reserve(wheels.size());
+
+	// Collect driven wheels
+	float transmission_ratio = mTransmission.GetCurrentRatio();
+	for (const DrivenDifferential &dd : driven_differentials)
+	{
+		VehicleDifferentialSettings d = *dd.mDifferential;
+
+		WheelWV *wl = d.mLeftWheel != -1? static_cast<WheelWV *>(wheels[d.mLeftWheel]) : nullptr;
+		WheelWV *wr = d.mRightWheel != -1? static_cast<WheelWV *>(wheels[d.mRightWheel]) : nullptr;
+
+		float clutch_to_wheel_ratio = transmission_ratio * d.mDifferentialRatio;
+
+		if (wl != nullptr && wr != nullptr)
+		{
+			// Calculate torque ratio
+			float ratio_l, ratio_r;
+			d.CalculateTorqueRatio(wl->GetAngularVelocity(), wr->GetAngularVelocity(), ratio_l, ratio_r);
+
+			// Add both wheels
+			driven_wheels.push_back({ wl, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio * ratio_l, 0.0f });
+			driven_wheels.push_back({ wr, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio * ratio_r, 0.0f });
+		}
+		else if (wl != nullptr)
+		{
+			// Only left wheel, all power to left
+			driven_wheels.push_back({ wl, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio, 0.0f });
+		}
+		else if (wr != nullptr)
+		{
+			// Only right wheel, all power to right
+			driven_wheels.push_back({ wr, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio, 0.0f });
+		}
+	}
+
+	bool solved = false;
+	if (!driven_wheels.empty())
+	{
+		// Define the torque at the clutch at time t as:
+		//
+		// tc(t):=S*(we(t)-sum(R(j)*ww(j,t),j,1,N)/N)
+		//
+		// Where:
+		// S is the total strength of clutch (= friction * strength)
+		// we(t) is the engine angular velocity at time t
+		// R(j) is the total gear ratio of clutch to wheel for wheel j
+		// ww(j,t) is the angular velocity of wheel j at time t
+		// N is the amount of wheels
+		//
+		// The torque that increases the engine angular velocity at time t is:
+		//
+		// te(t):=TE-tc(t)
+		//
+		// Where:
+		// TE is the torque delivered by the engine
+		//
+		// The torque that increases the wheel angular velocity for wheel i at time t is:
+		//
+		// tw(i,t):=TW(i)+R(i)*F(i)*tc(t)
+		//
+		// Where:
+		// TW(i) is the torque applied to the wheel outside of the engine (brake + torque due to friction with the ground)
+		// F(i) is the fraction of the engine torque applied from engine to wheel i
+		//
+		// Because the angular acceleration and torque are connected through: Torque = I * dw/dt
+		//
+		// We have the angular acceleration of the engine at time t:
+		//
+		// ddt_we(t):=te(t)/Ie
+		//
+		// Where:
+		// Ie is the inertia of the engine
+		//
+		// We have the angular acceleration of wheel i at time t:
+		//
+		// ddt_ww(i,t):=tw(i,t)/Iw(i)
+		//
+		// Where:
+		// Iw(i) is the inertia of wheel i
+		//
+		// We could take a simple Euler step to calculate the resulting accelerations but because the system is very stiff this turns out to be unstable, so we need to use implicit Euler instead:
+		//
+		// we(t+dt)=we(t)+dt*ddt_we(t+dt)
+		//
+		// and:
+		//
+		// ww(i,t+dt)=ww(i,t)+dt*ddt_ww(i,t+dt)
+		//
+		// Expanding both equations (the equations above are in wxMaxima format and this can easily be done by expand(%)):
+		//
+		// For wheel:
+		//
+		// ww(i,t+dt) + (S*dt*F(i)*R(i)*sum(R(j)*ww(j,t+dt),j,1,N))/(N*Iw(i)) - (S*dt*F(i)*R(i)*we(t+dt))/Iw(i) = ww(i,t)+(dt*TW(i))/Iw(i)
+		//
+		// For engine:
+		//
+		// we(t+dt) + (S*dt*we(t+dt))/Ie - (S*dt*sum(R(j)*ww(j,t+dt),j,1,N))/(Ie*N) = we(t)+(TE*dt)/Ie
+		//
+		// Defining a vector w(t) = (ww(1, t), ww(2, t), ..., ww(N, t), we(t)) we can write both equations as a matrix multiplication:
+		//
+		// a * w(t + dt) = b
+		//
+		// We then invert the matrix to get the new angular velocities.
+
+		// Dimension of matrix is N + 1
+		int n = (int)driven_wheels.size() + 1;
+
+		// Last column of w is for the engine angular velocity
+		int engine = n - 1;
+
+		// Define a and b
+		DynMatrix a(n, n);
+		DynMatrix b(n, 1);
+
+		// Get number of driven wheels as a float
+		float num_driven_wheels_float = float(driven_wheels.size());
+
+		// Angular velocity of engine
+		float w_engine = mEngine.GetAngularVelocity();
+
+		// Calculate the total strength of the clutch
+		float clutch_strength = transmission_ratio != 0.0f? mTransmission.GetClutchFriction() * mTransmission.mClutchStrength : 0.0f;
+
+		// dt / Ie
+		float dt_div_ie = inDeltaTime / mEngine.mInertia;
+
+		// Calculate scale factor for impulses based on previous delta time
+		float impulse_scale = mPreviousDeltaTime > 0.0f? inDeltaTime / mPreviousDeltaTime : 0.0f;
+
+		// Iterate the rows for the wheels
+		for (int i = 0; i < (int)driven_wheels.size(); ++i)
+		{
+			DrivenWheel &w_i = driven_wheels[i];
+			const WheelSettingsWV *settings = w_i.mWheel->GetSettings();
+
+			// Get wheel inertia
+			float inertia = settings->mInertia;
+
+			// S * R(i)
+			float s_r = clutch_strength * w_i.mClutchToWheelRatio;
+
+			// dt * S * R(i) * F(i) / Iw
+			float dt_s_r_f_div_iw = inDeltaTime * s_r * w_i.mClutchToWheelTorqueRatio / inertia;
+
+			// Fill in the columns of a for wheel j
+			for (int j = 0; j < (int)driven_wheels.size(); ++j)
+			{
+				const DrivenWheel &w_j = driven_wheels[j];
+				a(i, j) = dt_s_r_f_div_iw * w_j.mClutchToWheelRatio / num_driven_wheels_float;
+			}
+
+			// Add ww(i, t+dt)
+			a(i, i) += 1.0f;
+
+			// Add the column for the engine
+			a(i, engine) = -dt_s_r_f_div_iw;
+
+			// Calculate external angular impulse operating on the wheel: TW(i) * dt
+			float dt_tw = 0.0f;
+
+			// Combine brake with hand brake torque
+			float brake_torque = mBrakeInput * settings->mMaxBrakeTorque + mHandBrakeInput * settings->mMaxHandBrakeTorque;
+			if (brake_torque > 0.0f)
+			{
+				// We're braking
+				// Calculate brake angular impulse
+				float sign;
+				if (w_i.mWheel->GetAngularVelocity() != 0.0f)
+					sign = Sign(w_i.mWheel->GetAngularVelocity());
+				else
+					sign = Sign(mTransmission.GetCurrentRatio()); // When wheels have locked up use the transmission ratio to determine the sign
+				dt_tw = sign * inDeltaTime * brake_torque;
+			}
+
+			if (w_i.mWheel->HasContact())
+			{
+				// We have wheel contact with the floor
+				// Note that we don't know the torque due to the ground contact yet, so we use the impulse applied from the last frame to estimate it
+				// Wheel torque TW = force * radius = lambda / dt * radius
+				dt_tw += impulse_scale * w_i.mWheel->GetLongitudinalLambda() * settings->mRadius;
+			}
+
+			w_i.mEstimatedAngularImpulse = dt_tw;
+
+			// Fill in the constant b = ww(i,t)+(dt*TW(i))/Iw(i)
+			b(i, 0) = w_i.mWheel->GetAngularVelocity() - dt_tw / inertia;
+
+			// To avoid looping over the wheels again, we also fill in the wheel columns of the engine row here
+			a(engine, i) = -dt_div_ie * s_r / num_driven_wheels_float;
+		}
+
+		// Finalize the engine row
+		a(engine, engine) = (1.0f + dt_div_ie * clutch_strength);
+		b(engine, 0) = w_engine + dt_div_ie * engine_torque;
+
+		// Solve the linear equation
+		if (GaussianElimination(a, b))
+		{
+			// Update the angular velocities for the wheels
+			for (int i = 0; i < (int)driven_wheels.size(); ++i)
+			{
+				DrivenWheel &w_i = driven_wheels[i];
+				const WheelSettingsWV *settings = w_i.mWheel->GetSettings();
+
+				// Get solved wheel angular velocity
+				float angular_velocity = b(i, 0);
+
+				// We estimated TW and applied it in the equation above, but we haven't actually applied this torque yet so we undo it here.
+				// It will be applied when we solve the actual braking / the constraints with the floor.
+				angular_velocity += w_i.mEstimatedAngularImpulse / settings->mInertia;
+
+				// Update angular velocity
+				w_i.mWheel->SetAngularVelocity(angular_velocity);
+			}
+
+			// Update the engine RPM
+			mEngine.SetCurrentRPM(b(engine, 0) * VehicleEngine::cAngularVelocityToRPM);
+
+			// The speeds have been solved
+			solved = true;
+		}
+		else
+		{
+			JPH_ASSERT(false, "New engine/wheel speeds could not be calculated!");
+		}
+	}
+
+	if (!solved)
+	{
+		// Engine not connected to wheels, apply all torque to engine rotation
+		mEngine.ApplyTorque(engine_torque, inDeltaTime);
+	}
+
+	// Calculate if any of the wheels are slipping, this is used to prevent gear switching
+	bool wheels_slipping = false;
+	for (const DrivenWheel &w : driven_wheels)
+		wheels_slipping |= w.mClutchToWheelTorqueRatio > 0.0f && (!w.mWheel->HasContact() || w.mWheel->mLongitudinalSlip > 0.1f);
+
+	// Only allow shifting up when we're not slipping and we're increasing our RPM.
+	// After a jump, we have a very high engine RPM but once we hit the ground the RPM should be decreasing and we don't want to shift up
+	// during that time.
+	bool can_shift_up = !wheels_slipping && mEngine.GetCurrentRPM() >= old_engine_rpm;
+
+	// Update transmission
+	mTransmission.Update(inDeltaTime, mEngine.GetCurrentRPM(), mForwardInput, can_shift_up);
+
+	// Braking
+	for (Wheel *w_base : wheels)
+	{
+		WheelWV *w = static_cast<WheelWV *>(w_base);
+		const WheelSettingsWV *settings = w->GetSettings();
+
+		// Combine brake with hand brake torque
+		float brake_torque = mBrakeInput * settings->mMaxBrakeTorque + mHandBrakeInput * settings->mMaxHandBrakeTorque;
+		if (brake_torque > 0.0f)
+		{
+			// Calculate how much torque is needed to stop the wheels from rotating in this time step
+			float brake_torque_to_lock_wheels = abs(w->GetAngularVelocity()) * settings->mInertia / inDeltaTime;
+			if (brake_torque > brake_torque_to_lock_wheels)
+			{
+				// Wheels are locked
+				w->SetAngularVelocity(0.0f);
+				w->mBrakeImpulse = (brake_torque - brake_torque_to_lock_wheels) * inDeltaTime / settings->mRadius;
+			}
+			else
+			{
+				// Slow down the wheels
+				w->ApplyTorque(-Sign(w->GetAngularVelocity()) * brake_torque, inDeltaTime);
+				w->mBrakeImpulse = 0.0f;
+			}
+		}
+		else
+		{
+			// Not braking
+			w->mBrakeImpulse = 0.0f;
+		}
+	}
+
+	// Remember previous delta time so we can scale the impulses correctly
+	mPreviousDeltaTime = inDeltaTime;
+}
+
+bool WheeledVehicleController::SolveLongitudinalAndLateralConstraints(float inDeltaTime)
+{
+	bool impulse = false;
+
+	float *max_lateral_friction_impulse = (float *)JPH_STACK_ALLOC(mConstraint.GetWheels().size() * sizeof(float));
+
+	uint wheel_index = 0;
+	for (Wheel *w_base : mConstraint.GetWheels())
+	{
+		if (w_base->HasContact())
+		{
+			WheelWV *w = static_cast<WheelWV *>(w_base);
+			const WheelSettingsWV *settings = w->GetSettings();
+
+			// Calculate max impulse that we can apply on the ground
+			float max_longitudinal_friction_impulse;
+			mTireMaxImpulseCallback(wheel_index,
+				max_longitudinal_friction_impulse, max_lateral_friction_impulse[wheel_index], w->GetSuspensionLambda(),
+				w->mCombinedLongitudinalFriction, w->mCombinedLateralFriction, w->mLongitudinalSlip, w->mLateralSlip, inDeltaTime);
+
+			// Calculate relative velocity between wheel contact point and floor in longitudinal direction
+			Vec3 relative_velocity = mConstraint.GetVehicleBody()->GetPointVelocity(w->GetContactPosition()) - w->GetContactPointVelocity();
+			float relative_longitudinal_velocity = relative_velocity.Dot(w->GetContactLongitudinal());
+
+			// Calculate brake force to apply
+			float min_longitudinal_impulse, max_longitudinal_impulse;
+			if (w->mBrakeImpulse != 0.0f)
+			{
+				// Limit brake force by max tire friction
+				float brake_impulse = min(w->mBrakeImpulse, max_longitudinal_friction_impulse);
+
+				// Check which direction the brakes should be applied (we don't want to apply an impulse that would accelerate the vehicle)
+				if (relative_longitudinal_velocity >= 0.0f)
+				{
+					min_longitudinal_impulse = -brake_impulse;
+					max_longitudinal_impulse = 0.0f;
+				}
+				else
+				{
+					min_longitudinal_impulse = 0.0f;
+					max_longitudinal_impulse = brake_impulse;
+				}
+
+				// Longitudinal impulse, note that we assume that once the wheels are locked that the brakes have more than enough torque to keep the wheels locked so we exclude any rotation deltas
+				impulse |= w->SolveLongitudinalConstraintPart(mConstraint, min_longitudinal_impulse, max_longitudinal_impulse);
+			}
+			else
+			{
+				// Assume we want to apply an angular impulse that makes the delta velocity between wheel and ground zero in one time step, calculate the amount of linear impulse needed to do that
+				float desired_angular_velocity = relative_longitudinal_velocity / settings->mRadius;
+				float linear_impulse = (w->GetAngularVelocity() - desired_angular_velocity) * settings->mInertia / settings->mRadius;
+
+				// Limit the impulse by max tire friction
+				float prev_lambda = w->GetLongitudinalLambda();
+				min_longitudinal_impulse = max_longitudinal_impulse = Clamp(prev_lambda + linear_impulse, -max_longitudinal_friction_impulse, max_longitudinal_friction_impulse);
+
+				// Longitudinal impulse
+				impulse |= w->SolveLongitudinalConstraintPart(mConstraint, min_longitudinal_impulse, max_longitudinal_impulse);
+
+				// Update the angular velocity of the wheels according to the lambda that was applied
+				w->SetAngularVelocity(w->GetAngularVelocity() - (w->GetLongitudinalLambda() - prev_lambda) * settings->mRadius / settings->mInertia);
+			}
+		}
+		++wheel_index;
+	}
+
+	wheel_index = 0;
+	for (Wheel *w_base : mConstraint.GetWheels())
+	{
+		if (w_base->HasContact())
+		{
+			WheelWV *w = static_cast<WheelWV *>(w_base);
+
+			// Lateral friction
+			float max_lateral_impulse = max_lateral_friction_impulse[wheel_index];
+			impulse |= w->SolveLateralConstraintPart(mConstraint, -max_lateral_impulse, max_lateral_impulse);
+		}
+		++wheel_index;
+	}
+
+	return impulse;
+}
+
+#ifdef JPH_DEBUG_RENDERER
+
+void WheeledVehicleController::Draw(DebugRenderer *inRenderer) const
+{
+	float constraint_size = mConstraint.GetDrawConstraintSize();
+
+	// Draw RPM
+	Body *body = mConstraint.GetVehicleBody();
+	Vec3 rpm_meter_up = body->GetRotation() * mConstraint.GetLocalUp();
+	RVec3 rpm_meter_pos = body->GetPosition() + body->GetRotation() * mRPMMeterPosition;
+	Vec3 rpm_meter_fwd = body->GetRotation() * mConstraint.GetLocalForward();
+	mEngine.DrawRPM(inRenderer, rpm_meter_pos, rpm_meter_fwd, rpm_meter_up, mRPMMeterSize, mTransmission.mShiftDownRPM, mTransmission.mShiftUpRPM);
+
+	if (mTransmission.GetCurrentRatio() != 0.0f)
+	{
+		// Calculate average wheel speed at clutch
+		float wheel_speed_at_clutch = GetWheelSpeedAtClutch();
+
+		// Draw the average wheel speed measured at clutch to compare engine RPM with wheel RPM
+		inRenderer->DrawLine(rpm_meter_pos, rpm_meter_pos + Quat::sRotation(rpm_meter_fwd, mEngine.ConvertRPMToAngle(wheel_speed_at_clutch)) * (rpm_meter_up * 1.1f * mRPMMeterSize), Color::sYellow);
+	}
+
+	// Draw current vehicle state
+	String status = StringFormat("Forward: %.1f, Right: %.1f\nBrake: %.1f, HandBrake: %.1f\n"
+								 "Gear: %d, Clutch: %.1f\nEngineRPM: %.0f, V: %.1f km/h",
+								 (double)mForwardInput, (double)mRightInput, (double)mBrakeInput, (double)mHandBrakeInput,
+								 mTransmission.GetCurrentGear(), (double)mTransmission.GetClutchFriction(), (double)mEngine.GetCurrentRPM(), (double)body->GetLinearVelocity().Length() * 3.6);
+	inRenderer->DrawText3D(body->GetPosition(), status, Color::sWhite, constraint_size);
+
+	RMat44 body_transform = body->GetWorldTransform();
+
+	for (const Wheel *w_base : mConstraint.GetWheels())
+	{
+		const WheelWV *w = static_cast<const WheelWV *>(w_base);
+		const WheelSettings *settings = w->GetSettings();
+
+		// Calculate where the suspension attaches to the body in world space
+		RVec3 ws_position = body_transform * settings->mPosition;
+		Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection);
+
+		// Draw suspension
+		RVec3 min_suspension_pos = ws_position + ws_direction * settings->mSuspensionMinLength;
+		RVec3 max_suspension_pos = ws_position + ws_direction * settings->mSuspensionMaxLength;
+		inRenderer->DrawLine(ws_position, min_suspension_pos, Color::sRed);
+		inRenderer->DrawLine(min_suspension_pos, max_suspension_pos, Color::sGreen);
+
+		// Draw current length
+		RVec3 wheel_pos = ws_position + ws_direction * w->GetSuspensionLength();
+		inRenderer->DrawMarker(wheel_pos, w->GetSuspensionLength() < settings->mSuspensionMinLength? Color::sRed : Color::sGreen, constraint_size);
+
+		// Draw wheel basis
+		Vec3 wheel_forward, wheel_up, wheel_right;
+		mConstraint.GetWheelLocalBasis(w, wheel_forward, wheel_up, wheel_right);
+		wheel_forward = body_transform.Multiply3x3(wheel_forward);
+		wheel_up = body_transform.Multiply3x3(wheel_up);
+		wheel_right = body_transform.Multiply3x3(wheel_right);
+		Vec3 steering_axis = body_transform.Multiply3x3(settings->mSteeringAxis);
+		inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_forward, Color::sRed);
+		inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_up, Color::sGreen);
+		inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_right, Color::sBlue);
+		inRenderer->DrawLine(wheel_pos, wheel_pos + steering_axis, Color::sYellow);
+
+		// Draw wheel
+		RMat44 wheel_transform(Vec4(wheel_up, 0.0f), Vec4(wheel_right, 0.0f), Vec4(wheel_forward, 0.0f), wheel_pos);
+		wheel_transform.SetRotation(wheel_transform.GetRotation() * Mat44::sRotationY(-w->GetRotationAngle()));
+		inRenderer->DrawCylinder(wheel_transform, settings->mWidth * 0.5f, settings->mRadius, w->GetSuspensionLength() <= settings->mSuspensionMinLength? Color::sRed : Color::sGreen, DebugRenderer::ECastShadow::Off, DebugRenderer::EDrawMode::Wireframe);
+
+		if (w->HasContact())
+		{
+			// Draw contact
+			inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactNormal(), Color::sYellow);
+			inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactLongitudinal(), Color::sRed);
+			inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactLateral(), Color::sBlue);
+
+			DebugRenderer::sInstance->DrawText3D(wheel_pos, StringFormat("W: %.1f, S: %.2f\nSlipLateral: %.1f, SlipLong: %.2f\nFrLateral: %.1f, FrLong: %.1f", (double)w->GetAngularVelocity(), (double)w->GetSuspensionLength(), (double)RadiansToDegrees(w->mLateralSlip), (double)w->mLongitudinalSlip, (double)w->mCombinedLateralFriction, (double)w->mCombinedLongitudinalFriction), Color::sWhite, constraint_size);
+		}
+		else
+		{
+			// Draw 'no hit'
+			DebugRenderer::sInstance->DrawText3D(wheel_pos, StringFormat("W: %.1f", (double)w->GetAngularVelocity()), Color::sRed, constraint_size);
+		}
+	}
+}
+
+#endif // JPH_DEBUG_RENDERER
+
+void WheeledVehicleController::SaveState(StateRecorder &inStream) const
+{
+	inStream.Write(mForwardInput);
+	inStream.Write(mRightInput);
+	inStream.Write(mBrakeInput);
+	inStream.Write(mHandBrakeInput);
+	inStream.Write(mPreviousDeltaTime);
+
+	mEngine.SaveState(inStream);
+	mTransmission.SaveState(inStream);
+}
+
+void WheeledVehicleController::RestoreState(StateRecorder &inStream)
+{
+	inStream.Read(mForwardInput);
+	inStream.Read(mRightInput);
+	inStream.Read(mBrakeInput);
+	inStream.Read(mHandBrakeInput);
+	inStream.Read(mPreviousDeltaTime);
+
+	mEngine.RestoreState(inStream);
+	mTransmission.RestoreState(inStream);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.h b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.h
new file mode 100644
index 0000000000..0bfd66fd8b
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Physics/Vehicle/WheeledVehicleController.h
@@ -0,0 +1,199 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Vehicle/VehicleConstraint.h>
+#include <Jolt/Physics/Vehicle/VehicleController.h>
+#include <Jolt/Physics/Vehicle/VehicleEngine.h>
+#include <Jolt/Physics/Vehicle/VehicleTransmission.h>
+#include <Jolt/Physics/Vehicle/VehicleDifferential.h>
+#include <Jolt/Core/LinearCurve.h>
+
+JPH_NAMESPACE_BEGIN
+
+class PhysicsSystem;
+
+/// WheelSettings object specifically for WheeledVehicleController
+class JPH_EXPORT WheelSettingsWV : public WheelSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, WheelSettingsWV)
+
+public:
+	/// Constructor
+								WheelSettingsWV();
+
+	// See: WheelSettings
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+
+	float						mInertia = 0.9f;							///< Moment of inertia (kg m^2), for a cylinder this would be 0.5 * M * R^2 which is 0.9 for a wheel with a mass of 20 kg and radius 0.3 m
+	float						mAngularDamping = 0.2f;						///< Angular damping factor of the wheel: dw/dt = -c * w
+	float						mMaxSteerAngle = DegreesToRadians(70.0f);	///< How much this wheel can steer (radians)
+	LinearCurve					mLongitudinalFriction;						///< On the Y-axis: friction in the forward direction of the tire. Friction is normally between 0 (no friction) and 1 (full friction) although friction can be a little bit higher than 1 because of the profile of a tire. On the X-axis: the slip ratio (fraction) defined as (omega_wheel * r_wheel - v_longitudinal) / |v_longitudinal|. You can see slip ratio as the amount the wheel is spinning relative to the floor: 0 means the wheel has full traction and is rolling perfectly in sync with the ground, 1 is for example when the wheel is locked and sliding over the ground.
+	LinearCurve					mLateralFriction;							///< On the Y-axis: friction in the sideways direction of the tire. Friction is normally between 0 (no friction) and 1 (full friction) although friction can be a little bit higher than 1 because of the profile of a tire. On the X-axis: the slip angle (degrees) defined as angle between relative contact velocity and tire direction.
+	float						mMaxBrakeTorque = 1500.0f;					///< How much torque (Nm) the brakes can apply to this wheel
+	float						mMaxHandBrakeTorque = 4000.0f;				///< How much torque (Nm) the hand brake can apply to this wheel (usually only applied to the rear wheels)
+};
+
+/// Wheel object specifically for WheeledVehicleController
+class JPH_EXPORT WheelWV : public Wheel
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+	explicit					WheelWV(const WheelSettingsWV &inWheel);
+
+	/// Override GetSettings and cast to the correct class
+	const WheelSettingsWV *		GetSettings() const							{ return StaticCast<WheelSettingsWV>(mSettings); }
+
+	/// Apply a torque (N m) to the wheel for a particular delta time
+	void						ApplyTorque(float inTorque, float inDeltaTime)
+	{
+		mAngularVelocity += inTorque * inDeltaTime / GetSettings()->mInertia;
+	}
+
+	/// Update the wheel rotation based on the current angular velocity
+	void						Update(uint inWheelIndex, float inDeltaTime, const VehicleConstraint &inConstraint);
+
+	float						mLongitudinalSlip = 0.0f;					///< Velocity difference between ground and wheel relative to ground velocity
+	float						mLateralSlip = 0.0f;						///< Angular difference (in radians) between ground and wheel relative to ground velocity
+	float						mCombinedLongitudinalFriction = 0.0f;		///< Combined friction coefficient in longitudinal direction (combines terrain and tires)
+	float						mCombinedLateralFriction = 0.0f;			///< Combined friction coefficient in lateral direction (combines terrain and tires)
+	float						mBrakeImpulse = 0.0f;						///< Amount of impulse that the brakes can apply to the floor (excluding friction)
+};
+
+/// Settings of a vehicle with regular wheels
+///
+/// The properties in this controller are largely based on "Car Physics for Games" by Marco Monster.
+/// See: https://www.asawicki.info/Mirror/Car%20Physics%20for%20Games/Car%20Physics%20for%20Games.html
+class JPH_EXPORT WheeledVehicleControllerSettings : public VehicleControllerSettings
+{
+	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, WheeledVehicleControllerSettings)
+
+public:
+	// See: VehicleControllerSettings
+	virtual VehicleController *	ConstructController(VehicleConstraint &inConstraint) const override;
+	virtual void				SaveBinaryState(StreamOut &inStream) const override;
+	virtual void				RestoreBinaryState(StreamIn &inStream) override;
+
+	VehicleEngineSettings		mEngine;									///< The properties of the engine
+	VehicleTransmissionSettings	mTransmission;								///< The properties of the transmission (aka gear box)
+	Array<VehicleDifferentialSettings> mDifferentials;						///< List of differentials and their properties
+	float						mDifferentialLimitedSlipRatio = 1.4f;		///< Ratio max / min average wheel speed of each differential (measured at the clutch). When the ratio is exceeded all torque gets distributed to the differential with the minimal average velocity. This allows implementing a limited slip differential between differentials. Set to FLT_MAX for an open differential. Value should be > 1.
+};
+
+/// Runtime controller class
+class JPH_EXPORT WheeledVehicleController : public VehicleController
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+								WheeledVehicleController(const WheeledVehicleControllerSettings &inSettings, VehicleConstraint &inConstraint);
+
+	/// Typedefs
+	using Differentials = Array<VehicleDifferentialSettings>;
+
+	/// Set input from driver
+	/// @param inForward Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed
+	/// @param inRight Value between -1 and 1 indicating desired steering angle (1 = right)
+	/// @param inBrake Value between 0 and 1 indicating how strong the brake pedal is pressed
+	/// @param inHandBrake Value between 0 and 1 indicating how strong the hand brake is pulled
+	void						SetDriverInput(float inForward, float inRight, float inBrake, float inHandBrake) { mForwardInput = inForward; mRightInput = inRight; mBrakeInput = inBrake; mHandBrakeInput = inHandBrake; }
+
+	/// Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed
+	void						SetForwardInput(float inForward)			{ mForwardInput = inForward; }
+	float						GetForwardInput() const						{ return mForwardInput; }
+
+	/// Value between -1 and 1 indicating desired steering angle (1 = right)
+	void						SetRightInput(float inRight)				{ mRightInput = inRight; }
+	float						GetRightInput() const						{ return mRightInput; }
+
+	/// Value between 0 and 1 indicating how strong the brake pedal is pressed
+	void						SetBrakeInput(float inBrake)				{ mBrakeInput = inBrake; }
+	float						GetBrakeInput() const						{ return mBrakeInput; }
+
+	/// Value between 0 and 1 indicating how strong the hand brake is pulled
+	void						SetHandBrakeInput(float inHandBrake)		{ mHandBrakeInput = inHandBrake; }
+	float						GetHandBrakeInput() const					{ return mHandBrakeInput; }
+
+	/// Get current engine state
+	const VehicleEngine &		GetEngine() const							{ return mEngine; }
+
+	/// Get current engine state (writable interface, allows you to make changes to the configuration which will take effect the next time step)
+	VehicleEngine &				GetEngine()									{ return mEngine; }
+
+	/// Get current transmission state
+	const VehicleTransmission &	GetTransmission() const						{ return mTransmission; }
+
+	/// Get current transmission state (writable interface, allows you to make changes to the configuration which will take effect the next time step)
+	VehicleTransmission &		GetTransmission()							{ return mTransmission; }
+
+	/// Get the differentials this vehicle has
+	const Differentials &		GetDifferentials() const					{ return mDifferentials; }
+
+	/// Get the differentials this vehicle has (writable interface, allows you to make changes to the configuration which will take effect the next time step)
+	Differentials &				GetDifferentials()							{ return mDifferentials; }
+
+	/// Ratio max / min average wheel speed of each differential (measured at the clutch).
+	float						GetDifferentialLimitedSlipRatio() const		{ return mDifferentialLimitedSlipRatio; }
+	void						SetDifferentialLimitedSlipRatio(float inV)	{ mDifferentialLimitedSlipRatio = inV; }
+
+	/// Get the average wheel speed of all driven wheels (measured at the clutch)
+	float						GetWheelSpeedAtClutch() const;
+
+	/// Calculate max tire impulses by combining friction, slip, and suspension impulse. Note that the actual applied impulse may be lower (e.g. when the vehicle is stationary on a horizontal surface the actual impulse applied will be 0).
+	using TireMaxImpulseCallback = function<void(uint inWheelIndex, float &outLongitudinalImpulse, float &outLateralImpulse, float inSuspensionImpulse, float inLongitudinalFriction, float inLateralFriction, float inLongitudinalSlip, float inLateralSlip, float inDeltaTime)>;
+	const TireMaxImpulseCallback&GetTireMaxImpulseCallback() const			{ return mTireMaxImpulseCallback; }
+	void						SetTireMaxImpulseCallback(const TireMaxImpulseCallback &inTireMaxImpulseCallback)	{ mTireMaxImpulseCallback = inTireMaxImpulseCallback; }
+
+#ifdef JPH_DEBUG_RENDERER
+	/// Debug drawing of RPM meter
+	void						SetRPMMeter(Vec3Arg inPosition, float inSize) { mRPMMeterPosition = inPosition; mRPMMeterSize = inSize; }
+#endif // JPH_DEBUG_RENDERER
+
+protected:
+	// See: VehicleController
+	virtual Wheel *				ConstructWheel(const WheelSettings &inWheel) const override { JPH_ASSERT(IsKindOf(&inWheel, JPH_RTTI(WheelSettingsWV))); return new WheelWV(static_cast<const WheelSettingsWV &>(inWheel)); }
+	virtual bool				AllowSleep() const override;
+	virtual void				PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override;
+	virtual void				PostCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override;
+	virtual bool				SolveLongitudinalAndLateralConstraints(float inDeltaTime) override;
+	virtual void				SaveState(StateRecorder &inStream) const override;
+	virtual void				RestoreState(StateRecorder &inStream) override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void				Draw(DebugRenderer *inRenderer) const override;
+#endif // JPH_DEBUG_RENDERER
+
+	// Control information
+	float						mForwardInput = 0.0f;						///< Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed
+	float						mRightInput = 0.0f;							///< Value between -1 and 1 indicating desired steering angle
+	float						mBrakeInput = 0.0f;							///< Value between 0 and 1 indicating how strong the brake pedal is pressed
+	float						mHandBrakeInput = 0.0f;						///< Value between 0 and 1 indicating how strong the hand brake is pulled
+
+	// Simulation information
+	VehicleEngine				mEngine;									///< Engine state of the vehicle
+	VehicleTransmission			mTransmission;								///< Transmission state of the vehicle
+	Differentials				mDifferentials;								///< Differential states of the vehicle
+	float						mDifferentialLimitedSlipRatio;				///< Ratio max / min average wheel speed of each differential (measured at the clutch).
+	float						mPreviousDeltaTime = 0.0f;					///< Delta time of the last step
+
+	// Callback that calculates the max impulse that the tire can apply to the ground
+	TireMaxImpulseCallback		mTireMaxImpulseCallback =
+		[](uint, float &outLongitudinalImpulse, float &outLateralImpulse, float inSuspensionImpulse, float inLongitudinalFriction, float inLateralFriction, float, float, float)
+		{
+			outLongitudinalImpulse = inLongitudinalFriction * inSuspensionImpulse;
+			outLateralImpulse = inLateralFriction * inSuspensionImpulse;
+		};
+
+#ifdef JPH_DEBUG_RENDERER
+	// Debug settings
+	Vec3						mRPMMeterPosition { 0, 1, 0 };				///< Position (in local space of the body) of the RPM meter when drawing the constraint
+	float						mRPMMeterSize = 0.5f;						///< Size of the RPM meter when drawing the constraint
+#endif // JPH_DEBUG_RENDERER
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/RegisterTypes.cpp b/thirdparty/jolt_physics/Jolt/RegisterTypes.cpp
new file mode 100644
index 0000000000..5b61a9160a
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/RegisterTypes.cpp
@@ -0,0 +1,192 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/RegisterTypes.h>
+#include <Jolt/Core/Factory.h>
+#include <Jolt/Core/RTTI.h>
+#include <Jolt/Core/TickCounter.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#include <Jolt/Physics/Collision/Shape/TriangleShape.h>
+#include <Jolt/Physics/Collision/Shape/PlaneShape.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/CylinderShape.h>
+#include <Jolt/Physics/Collision/Shape/TaperedCylinderShape.h>
+#include <Jolt/Physics/Collision/Shape/ScaledShape.h>
+#include <Jolt/Physics/Collision/Shape/MeshShape.h>
+#include <Jolt/Physics/Collision/Shape/ConvexHullShape.h>
+#include <Jolt/Physics/Collision/Shape/HeightFieldShape.h>
+#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
+#include <Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h>
+#include <Jolt/Physics/Collision/Shape/MutableCompoundShape.h>
+#include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/Shape/EmptyShape.h>
+#include <Jolt/Physics/Collision/PhysicsMaterialSimple.h>
+#include <Jolt/Physics/SoftBody/SoftBodyShape.h>
+
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, Skeleton)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, SkeletalAnimation)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, RagdollSettings)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PointConstraintSettings)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, SixDOFConstraintSettings)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, SliderConstraintSettings)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, SwingTwistConstraintSettings)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, DistanceConstraintSettings)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, HingeConstraintSettings)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, FixedConstraintSettings)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, ConeConstraintSettings)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PathConstraintSettings)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PathConstraintPath)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PathConstraintPathHermite)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, VehicleConstraintSettings)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, WheeledVehicleControllerSettings)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, RackAndPinionConstraintSettings)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, GearConstraintSettings)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PulleyConstraintSettings)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, MotorSettings)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PhysicsScene)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PhysicsMaterial)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, GroupFilter)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, GroupFilterTable)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, BodyCreationSettings)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, SoftBodyCreationSettings)
+
+JPH_NAMESPACE_BEGIN
+
+bool VerifyJoltVersionIDInternal(uint64 inVersionID)
+{
+	return inVersionID == JPH_VERSION_ID;
+}
+
+void RegisterTypesInternal(uint64 inVersionID)
+{
+	// Version check
+	if (!VerifyJoltVersionIDInternal(inVersionID))
+	{
+		Trace("Version mismatch, make sure you compile the client code with the same Jolt version and compiler definitions!");
+		uint64 mismatch = JPH_VERSION_ID ^ inVersionID;
+		auto check_bit = [mismatch](int inBit, const char *inLabel) { if (mismatch & (uint64(1) << (inBit + 23))) Trace("Mismatching define %s.", inLabel); };
+		check_bit(1, "JPH_DOUBLE_PRECISION");
+		check_bit(2, "JPH_CROSS_PLATFORM_DETERMINISTIC");
+		check_bit(3, "JPH_FLOATING_POINT_EXCEPTIONS_ENABLED");
+		check_bit(4, "JPH_PROFILE_ENABLED");
+		check_bit(5, "JPH_EXTERNAL_PROFILE");
+		check_bit(6, "JPH_DEBUG_RENDERER");
+		check_bit(7, "JPH_DISABLE_TEMP_ALLOCATOR");
+		check_bit(8, "JPH_DISABLE_CUSTOM_ALLOCATOR");
+		check_bit(9, "JPH_OBJECT_LAYER_BITS");
+		check_bit(10, "JPH_ENABLE_ASSERTS");
+		check_bit(11, "JPH_OBJECT_STREAM");
+		std::abort();
+	}
+
+#ifndef JPH_DISABLE_CUSTOM_ALLOCATOR
+	JPH_ASSERT(Allocate != nullptr && Reallocate != nullptr && Free != nullptr && AlignedAllocate != nullptr && AlignedFree != nullptr, "Need to supply an allocator first or call RegisterDefaultAllocator()");
+#endif // !JPH_DISABLE_CUSTOM_ALLOCATOR
+
+	JPH_ASSERT(Factory::sInstance != nullptr, "Need to create a factory first!");
+
+	// Initialize dispatcher
+	CollisionDispatch::sInit();
+
+	// Register base classes first so that we can specialize them later
+	CompoundShape::sRegister();
+	ConvexShape::sRegister();
+
+	// Register compounds before others so that we can specialize them later (register them in reverse order of collision complexity)
+	MutableCompoundShape::sRegister();
+	StaticCompoundShape::sRegister();
+
+	// Leaf classes
+	TriangleShape::sRegister();
+	PlaneShape::sRegister();
+	SphereShape::sRegister();
+	BoxShape::sRegister();
+	CapsuleShape::sRegister();
+	TaperedCapsuleShape::sRegister();
+	CylinderShape::sRegister();
+	TaperedCylinderShape::sRegister();
+	MeshShape::sRegister();
+	ConvexHullShape::sRegister();
+	HeightFieldShape::sRegister();
+	SoftBodyShape::sRegister();
+
+	// Register these last because their collision functions are simple so we want to execute them first (register them in reverse order of collision complexity)
+	RotatedTranslatedShape::sRegister();
+	OffsetCenterOfMassShape::sRegister();
+	ScaledShape::sRegister();
+	EmptyShape::sRegister();
+
+	// Create list of all types
+	const RTTI *types[] = {
+		JPH_RTTI(SkeletalAnimation),
+		JPH_RTTI(Skeleton),
+		JPH_RTTI(CompoundShapeSettings),
+		JPH_RTTI(StaticCompoundShapeSettings),
+		JPH_RTTI(MutableCompoundShapeSettings),
+		JPH_RTTI(TriangleShapeSettings),
+		JPH_RTTI(PlaneShapeSettings),
+		JPH_RTTI(SphereShapeSettings),
+		JPH_RTTI(BoxShapeSettings),
+		JPH_RTTI(CapsuleShapeSettings),
+		JPH_RTTI(TaperedCapsuleShapeSettings),
+		JPH_RTTI(CylinderShapeSettings),
+		JPH_RTTI(TaperedCylinderShapeSettings),
+		JPH_RTTI(ScaledShapeSettings),
+		JPH_RTTI(MeshShapeSettings),
+		JPH_RTTI(ConvexHullShapeSettings),
+		JPH_RTTI(HeightFieldShapeSettings),
+		JPH_RTTI(RotatedTranslatedShapeSettings),
+		JPH_RTTI(OffsetCenterOfMassShapeSettings),
+		JPH_RTTI(EmptyShapeSettings),
+		JPH_RTTI(RagdollSettings),
+		JPH_RTTI(PointConstraintSettings),
+		JPH_RTTI(SixDOFConstraintSettings),
+		JPH_RTTI(SliderConstraintSettings),
+		JPH_RTTI(SwingTwistConstraintSettings),
+		JPH_RTTI(DistanceConstraintSettings),
+		JPH_RTTI(HingeConstraintSettings),
+		JPH_RTTI(FixedConstraintSettings),
+		JPH_RTTI(ConeConstraintSettings),
+		JPH_RTTI(PathConstraintSettings),
+		JPH_RTTI(VehicleConstraintSettings),
+		JPH_RTTI(WheeledVehicleControllerSettings),
+		JPH_RTTI(PathConstraintPath),
+		JPH_RTTI(PathConstraintPathHermite),
+		JPH_RTTI(RackAndPinionConstraintSettings),
+		JPH_RTTI(GearConstraintSettings),
+		JPH_RTTI(PulleyConstraintSettings),
+		JPH_RTTI(MotorSettings),
+		JPH_RTTI(PhysicsScene),
+		JPH_RTTI(PhysicsMaterial),
+		JPH_RTTI(PhysicsMaterialSimple),
+		JPH_RTTI(GroupFilter),
+		JPH_RTTI(GroupFilterTable),
+		JPH_RTTI(BodyCreationSettings),
+		JPH_RTTI(SoftBodyCreationSettings)
+	};
+
+	// Register them all
+	Factory::sInstance->Register(types, (uint)std::size(types));
+
+	// Initialize default physics material
+	if (PhysicsMaterial::sDefault == nullptr)
+		PhysicsMaterial::sDefault = new PhysicsMaterialSimple("Default", Color::sGrey);
+}
+
+void UnregisterTypes()
+{
+	// Unregister all types
+	if (Factory::sInstance != nullptr)
+		Factory::sInstance->Clear();
+
+	// Delete default physics material
+	PhysicsMaterial::sDefault = nullptr;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/RegisterTypes.h b/thirdparty/jolt_physics/Jolt/RegisterTypes.h
new file mode 100644
index 0000000000..372ef7f4f4
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/RegisterTypes.h
@@ -0,0 +1,29 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Internal helper function
+JPH_EXPORT extern bool VerifyJoltVersionIDInternal(uint64 inVersionID);
+
+/// This function can be used to verify the library ABI is compatible with your
+/// application.
+/// Use it in this way: `assert(VerifyJoltVersionID());`.
+/// Returns `false` if the library used is not compatible with your app.
+JPH_INLINE bool VerifyJoltVersionID() { return VerifyJoltVersionIDInternal(JPH_VERSION_ID); }
+
+/// Internal helper function
+JPH_EXPORT extern void RegisterTypesInternal(uint64 inVersionID);
+
+/// Register all physics types with the factory and install their collision handlers with the CollisionDispatch class.
+/// If you have your own custom shape types you probably need to register their handlers with the CollisionDispatch before calling this function.
+/// If you implement your own default material (PhysicsMaterial::sDefault) make sure to initialize it before this function or else this function will create one for you.
+JPH_INLINE void RegisterTypes() { RegisterTypesInternal(JPH_VERSION_ID); }
+
+/// Unregisters all types with the factory and cleans up the default material
+JPH_EXPORT extern void UnregisterTypes();
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Renderer/DebugRenderer.cpp b/thirdparty/jolt_physics/Jolt/Renderer/DebugRenderer.cpp
new file mode 100644
index 0000000000..636c1e2f7a
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Renderer/DebugRenderer.cpp
@@ -0,0 +1,1107 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#ifdef JPH_DEBUG_RENDERER
+
+#include <Jolt/Renderer/DebugRenderer.h>
+#include <Jolt/Core/Profiler.h>
+#include <Jolt/Geometry/OrientedBox.h>
+
+JPH_NAMESPACE_BEGIN
+
+DebugRenderer *DebugRenderer::sInstance = nullptr;
+
+// Number of LOD levels to create
+static const int sMaxLevel = 4;
+
+// Distance for each LOD level, these are tweaked for an object of approx. size 1. Use the lod scale to scale these distances.
+static const float sLODDistanceForLevel[] = { 5.0f, 10.0f, 40.0f, FLT_MAX };
+
+DebugRenderer::Triangle::Triangle(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, ColorArg inColor)
+{
+	// Set position
+	inV1.StoreFloat3(&mV[0].mPosition);
+	inV2.StoreFloat3(&mV[1].mPosition);
+	inV3.StoreFloat3(&mV[2].mPosition);
+
+	// Set color
+	mV[0].mColor = mV[1].mColor = mV[2].mColor = inColor;
+
+	// Calculate normal
+	Vec3 normal = (inV2 - inV1).Cross(inV3 - inV1);
+	float normal_len = normal.Length();
+	if (normal_len > 0.0f)
+		normal /= normal_len;
+	Float3 normal3;
+	normal.StoreFloat3(&normal3);
+	mV[0].mNormal = mV[1].mNormal = mV[2].mNormal = normal3;
+
+	// Reset UV's
+	mV[0].mUV = mV[1].mUV = mV[2].mUV = { 0, 0 };
+}
+
+DebugRenderer::Triangle::Triangle(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, ColorArg inColor, Vec3Arg inUVOrigin, Vec3Arg inUVDirection)
+{
+	// Set position
+	inV1.StoreFloat3(&mV[0].mPosition);
+	inV2.StoreFloat3(&mV[1].mPosition);
+	inV3.StoreFloat3(&mV[2].mPosition);
+
+	// Set color
+	mV[0].mColor = mV[1].mColor = mV[2].mColor = inColor;
+
+	// Calculate normal
+	Vec3 normal = (inV2 - inV1).Cross(inV3 - inV1).Normalized();
+	Float3 normal3;
+	normal.StoreFloat3(&normal3);
+	mV[0].mNormal = mV[1].mNormal = mV[2].mNormal = normal3;
+
+	// Set UV's
+	Vec3 uv1 = inV1 - inUVOrigin;
+	Vec3 uv2 = inV2 - inUVOrigin;
+	Vec3 uv3 = inV3 - inUVOrigin;
+	Vec3 axis2 = normal.Cross(inUVDirection);
+	mV[0].mUV = { inUVDirection.Dot(uv1), axis2.Dot(uv1) };
+	mV[1].mUV = { inUVDirection.Dot(uv2), axis2.Dot(uv2) };
+	mV[2].mUV = { inUVDirection.Dot(uv3), axis2.Dot(uv3) };
+}
+
+DebugRenderer::DebugRenderer()
+{
+	// Store singleton
+	JPH_ASSERT(sInstance == nullptr);
+	sInstance = this;
+}
+
+DebugRenderer::~DebugRenderer()
+{
+	JPH_ASSERT(sInstance == this);
+	sInstance = nullptr;
+}
+
+void DebugRenderer::DrawWireBox(const AABox &inBox, ColorArg inColor)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// 8 vertices
+	RVec3 v1(Real(inBox.mMin.GetX()), Real(inBox.mMin.GetY()), Real(inBox.mMin.GetZ()));
+	RVec3 v2(Real(inBox.mMin.GetX()), Real(inBox.mMin.GetY()), Real(inBox.mMax.GetZ()));
+	RVec3 v3(Real(inBox.mMin.GetX()), Real(inBox.mMax.GetY()), Real(inBox.mMin.GetZ()));
+	RVec3 v4(Real(inBox.mMin.GetX()), Real(inBox.mMax.GetY()), Real(inBox.mMax.GetZ()));
+	RVec3 v5(Real(inBox.mMax.GetX()), Real(inBox.mMin.GetY()), Real(inBox.mMin.GetZ()));
+	RVec3 v6(Real(inBox.mMax.GetX()), Real(inBox.mMin.GetY()), Real(inBox.mMax.GetZ()));
+	RVec3 v7(Real(inBox.mMax.GetX()), Real(inBox.mMax.GetY()), Real(inBox.mMin.GetZ()));
+	RVec3 v8(Real(inBox.mMax.GetX()), Real(inBox.mMax.GetY()), Real(inBox.mMax.GetZ()));
+
+	// 12 edges
+	DrawLine(v1, v2, inColor);
+	DrawLine(v1, v3, inColor);
+	DrawLine(v1, v5, inColor);
+	DrawLine(v2, v4, inColor);
+	DrawLine(v2, v6, inColor);
+	DrawLine(v3, v4, inColor);
+	DrawLine(v3, v7, inColor);
+	DrawLine(v4, v8, inColor);
+	DrawLine(v5, v6, inColor);
+	DrawLine(v5, v7, inColor);
+	DrawLine(v6, v8, inColor);
+	DrawLine(v7, v8, inColor);
+}
+
+void DebugRenderer::DrawWireBox(const OrientedBox &inBox, ColorArg inColor)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// 8 vertices
+	RVec3 v1(inBox.mOrientation * Vec3(-inBox.mHalfExtents.GetX(), -inBox.mHalfExtents.GetY(), -inBox.mHalfExtents.GetZ()));
+	RVec3 v2(inBox.mOrientation * Vec3(-inBox.mHalfExtents.GetX(), -inBox.mHalfExtents.GetY(), inBox.mHalfExtents.GetZ()));
+	RVec3 v3(inBox.mOrientation * Vec3(-inBox.mHalfExtents.GetX(), inBox.mHalfExtents.GetY(), -inBox.mHalfExtents.GetZ()));
+	RVec3 v4(inBox.mOrientation * Vec3(-inBox.mHalfExtents.GetX(), inBox.mHalfExtents.GetY(), inBox.mHalfExtents.GetZ()));
+	RVec3 v5(inBox.mOrientation * Vec3(inBox.mHalfExtents.GetX(), -inBox.mHalfExtents.GetY(), -inBox.mHalfExtents.GetZ()));
+	RVec3 v6(inBox.mOrientation * Vec3(inBox.mHalfExtents.GetX(), -inBox.mHalfExtents.GetY(), inBox.mHalfExtents.GetZ()));
+	RVec3 v7(inBox.mOrientation * Vec3(inBox.mHalfExtents.GetX(), inBox.mHalfExtents.GetY(), -inBox.mHalfExtents.GetZ()));
+	RVec3 v8(inBox.mOrientation * Vec3(inBox.mHalfExtents.GetX(), inBox.mHalfExtents.GetY(), inBox.mHalfExtents.GetZ()));
+
+	// 12 edges
+	DrawLine(v1, v2, inColor);
+	DrawLine(v1, v3, inColor);
+	DrawLine(v1, v5, inColor);
+	DrawLine(v2, v4, inColor);
+	DrawLine(v2, v6, inColor);
+	DrawLine(v3, v4, inColor);
+	DrawLine(v3, v7, inColor);
+	DrawLine(v4, v8, inColor);
+	DrawLine(v5, v6, inColor);
+	DrawLine(v5, v7, inColor);
+	DrawLine(v6, v8, inColor);
+	DrawLine(v7, v8, inColor);
+}
+
+void DebugRenderer::DrawWireBox(RMat44Arg inMatrix, const AABox &inBox, ColorArg inColor)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// 8 vertices
+	RVec3 v1 = inMatrix * Vec3(inBox.mMin.GetX(), inBox.mMin.GetY(), inBox.mMin.GetZ());
+	RVec3 v2 = inMatrix * Vec3(inBox.mMin.GetX(), inBox.mMin.GetY(), inBox.mMax.GetZ());
+	RVec3 v3 = inMatrix * Vec3(inBox.mMin.GetX(), inBox.mMax.GetY(), inBox.mMin.GetZ());
+	RVec3 v4 = inMatrix * Vec3(inBox.mMin.GetX(), inBox.mMax.GetY(), inBox.mMax.GetZ());
+	RVec3 v5 = inMatrix * Vec3(inBox.mMax.GetX(), inBox.mMin.GetY(), inBox.mMin.GetZ());
+	RVec3 v6 = inMatrix * Vec3(inBox.mMax.GetX(), inBox.mMin.GetY(), inBox.mMax.GetZ());
+	RVec3 v7 = inMatrix * Vec3(inBox.mMax.GetX(), inBox.mMax.GetY(), inBox.mMin.GetZ());
+	RVec3 v8 = inMatrix * Vec3(inBox.mMax.GetX(), inBox.mMax.GetY(), inBox.mMax.GetZ());
+
+	// 12 edges
+	DrawLine(v1, v2, inColor);
+	DrawLine(v1, v3, inColor);
+	DrawLine(v1, v5, inColor);
+	DrawLine(v2, v4, inColor);
+	DrawLine(v2, v6, inColor);
+	DrawLine(v3, v4, inColor);
+	DrawLine(v3, v7, inColor);
+	DrawLine(v4, v8, inColor);
+	DrawLine(v5, v6, inColor);
+	DrawLine(v5, v7, inColor);
+	DrawLine(v6, v8, inColor);
+	DrawLine(v7, v8, inColor);
+}
+
+void DebugRenderer::DrawMarker(RVec3Arg inPosition, ColorArg inColor, float inSize)
+{
+	JPH_PROFILE_FUNCTION();
+
+	Vec3 dx(inSize, 0, 0);
+	Vec3 dy(0, inSize, 0);
+	Vec3 dz(0, 0, inSize);
+	DrawLine(inPosition - dy, inPosition + dy, inColor);
+	DrawLine(inPosition - dx, inPosition + dx, inColor);
+	DrawLine(inPosition - dz, inPosition + dz, inColor);
+}
+
+void DebugRenderer::DrawArrow(RVec3Arg inFrom, RVec3Arg inTo, ColorArg inColor, float inSize)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Draw base line
+	DrawLine(inFrom, inTo, inColor);
+
+	if (inSize > 0.0f)
+	{
+		// Draw arrow head
+		Vec3 dir = Vec3(inTo - inFrom);
+		float len = dir.Length();
+		if (len != 0.0f)
+			dir = dir * (inSize / len);
+		else
+			dir = Vec3(inSize, 0, 0);
+		Vec3 perp = inSize * dir.GetNormalizedPerpendicular();
+		DrawLine(inTo - dir + perp, inTo, inColor);
+		DrawLine(inTo - dir - perp, inTo, inColor);
+	}
+}
+
+void DebugRenderer::DrawCoordinateSystem(RMat44Arg inTransform, float inSize)
+{
+	JPH_PROFILE_FUNCTION();
+
+	DrawArrow(inTransform.GetTranslation(), inTransform * Vec3(inSize, 0, 0), Color::sRed, 0.1f * inSize);
+	DrawArrow(inTransform.GetTranslation(), inTransform * Vec3(0, inSize, 0), Color::sGreen, 0.1f * inSize);
+	DrawArrow(inTransform.GetTranslation(), inTransform * Vec3(0, 0, inSize), Color::sBlue, 0.1f * inSize);
+}
+
+void DebugRenderer::DrawPlane(RVec3Arg inPoint, Vec3Arg inNormal, ColorArg inColor, float inSize)
+{
+	// Create orthogonal basis
+	Vec3 perp1 = inNormal.Cross(Vec3::sAxisY()).NormalizedOr(Vec3::sAxisX());
+	Vec3 perp2 = perp1.Cross(inNormal).Normalized();
+	perp1 = inNormal.Cross(perp2);
+
+	// Calculate corners
+	RVec3 corner1 = inPoint + inSize * (perp1 + perp2);
+	RVec3 corner2 = inPoint + inSize * (perp1 - perp2);
+	RVec3 corner3 = inPoint + inSize * (-perp1 - perp2);
+	RVec3 corner4 = inPoint + inSize * (-perp1 + perp2);
+
+	// Draw cross
+	DrawLine(corner1, corner3, inColor);
+	DrawLine(corner2, corner4, inColor);
+
+	// Draw square
+	DrawLine(corner1, corner2, inColor);
+	DrawLine(corner2, corner3, inColor);
+	DrawLine(corner3, corner4, inColor);
+	DrawLine(corner4, corner1, inColor);
+
+	// Draw normal
+	DrawArrow(inPoint, inPoint + inSize * inNormal, inColor, 0.1f * inSize);
+}
+
+void DebugRenderer::DrawWireTriangle(RVec3Arg inV1, RVec3Arg inV2, RVec3Arg inV3, ColorArg inColor)
+{
+	JPH_PROFILE_FUNCTION();
+
+	DrawLine(inV1, inV2, inColor);
+	DrawLine(inV2, inV3, inColor);
+	DrawLine(inV3, inV1, inColor);
+}
+
+void DebugRenderer::DrawWireSphere(RVec3Arg inCenter, float inRadius, ColorArg inColor, int inLevel)
+{
+	RMat44 matrix = RMat44::sTranslation(inCenter) * Mat44::sScale(inRadius);
+
+	DrawWireUnitSphere(matrix, inColor, inLevel);
+}
+
+void DebugRenderer::DrawWireUnitSphere(RMat44Arg inMatrix, ColorArg inColor, int inLevel)
+{
+	JPH_PROFILE_FUNCTION();
+
+	DrawWireUnitSphereRecursive(inMatrix, inColor, Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ(), inLevel);
+	DrawWireUnitSphereRecursive(inMatrix, inColor, -Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ(), inLevel);
+	DrawWireUnitSphereRecursive(inMatrix, inColor, Vec3::sAxisX(), -Vec3::sAxisY(), Vec3::sAxisZ(), inLevel);
+	DrawWireUnitSphereRecursive(inMatrix, inColor, -Vec3::sAxisX(), -Vec3::sAxisY(), Vec3::sAxisZ(), inLevel);
+	DrawWireUnitSphereRecursive(inMatrix, inColor, Vec3::sAxisX(), Vec3::sAxisY(), -Vec3::sAxisZ(), inLevel);
+	DrawWireUnitSphereRecursive(inMatrix, inColor, -Vec3::sAxisX(), Vec3::sAxisY(), -Vec3::sAxisZ(), inLevel);
+	DrawWireUnitSphereRecursive(inMatrix, inColor, Vec3::sAxisX(), -Vec3::sAxisY(), -Vec3::sAxisZ(), inLevel);
+	DrawWireUnitSphereRecursive(inMatrix, inColor, -Vec3::sAxisX(), -Vec3::sAxisY(), -Vec3::sAxisZ(), inLevel);
+}
+
+void DebugRenderer::DrawWireUnitSphereRecursive(RMat44Arg inMatrix, ColorArg inColor, Vec3Arg inDir1, Vec3Arg inDir2, Vec3Arg inDir3, int inLevel)
+{
+	if (inLevel == 0)
+	{
+		RVec3 d1 = inMatrix * inDir1;
+		RVec3 d2 = inMatrix * inDir2;
+		RVec3 d3 = inMatrix * inDir3;
+
+		DrawLine(d1, d2, inColor);
+		DrawLine(d2, d3, inColor);
+		DrawLine(d3, d1, inColor);
+	}
+	else
+	{
+		Vec3 center1 = (inDir1 + inDir2).Normalized();
+		Vec3 center2 = (inDir2 + inDir3).Normalized();
+		Vec3 center3 = (inDir3 + inDir1).Normalized();
+
+		DrawWireUnitSphereRecursive(inMatrix, inColor, inDir1, center1, center3, inLevel - 1);
+		DrawWireUnitSphereRecursive(inMatrix, inColor, center1, center2, center3, inLevel - 1);
+		DrawWireUnitSphereRecursive(inMatrix, inColor, center1, inDir2, center2, inLevel - 1);
+		DrawWireUnitSphereRecursive(inMatrix, inColor, center3, center2, inDir3, inLevel - 1);
+	}
+}
+
+void DebugRenderer::Create8thSphereRecursive(Array<uint32> &ioIndices, Array<Vertex> &ioVertices, Vec3Arg inDir1, uint32 &ioIdx1, Vec3Arg inDir2, uint32 &ioIdx2, Vec3Arg inDir3, uint32 &ioIdx3, const Float2 &inUV, SupportFunction inGetSupport, int inLevel)
+{
+	if (inLevel == 0)
+	{
+		if (ioIdx1 == 0xffffffff)
+		{
+			ioIdx1 = (uint32)ioVertices.size();
+			Float3 position, normal;
+			inGetSupport(inDir1).StoreFloat3(&position);
+			inDir1.StoreFloat3(&normal);
+			ioVertices.push_back({ position, normal, inUV, Color::sWhite });
+		}
+
+		if (ioIdx2 == 0xffffffff)
+		{
+			ioIdx2 = (uint32)ioVertices.size();
+			Float3 position, normal;
+			inGetSupport(inDir2).StoreFloat3(&position);
+			inDir2.StoreFloat3(&normal);
+			ioVertices.push_back({ position, normal, inUV, Color::sWhite });
+		}
+
+		if (ioIdx3 == 0xffffffff)
+		{
+			ioIdx3 = (uint32)ioVertices.size();
+			Float3 position, normal;
+			inGetSupport(inDir3).StoreFloat3(&position);
+			inDir3.StoreFloat3(&normal);
+			ioVertices.push_back({ position, normal, inUV, Color::sWhite });
+		}
+
+		ioIndices.push_back(ioIdx1);
+		ioIndices.push_back(ioIdx2);
+		ioIndices.push_back(ioIdx3);
+	}
+	else
+	{
+		Vec3 center1 = (inDir1 + inDir2).Normalized();
+		Vec3 center2 = (inDir2 + inDir3).Normalized();
+		Vec3 center3 = (inDir3 + inDir1).Normalized();
+
+		uint32 idx1 = 0xffffffff;
+		uint32 idx2 = 0xffffffff;
+		uint32 idx3 = 0xffffffff;
+
+		Create8thSphereRecursive(ioIndices, ioVertices, inDir1,  ioIdx1, center1, idx1,   center3, idx3,   inUV, inGetSupport, inLevel - 1);
+		Create8thSphereRecursive(ioIndices, ioVertices, center1, idx1,	 center2, idx2,   center3, idx3,   inUV, inGetSupport, inLevel - 1);
+		Create8thSphereRecursive(ioIndices, ioVertices, center1, idx1,   inDir2,  ioIdx2, center2, idx2,   inUV, inGetSupport, inLevel - 1);
+		Create8thSphereRecursive(ioIndices, ioVertices, center3, idx3,   center2, idx2,   inDir3,  ioIdx3, inUV, inGetSupport, inLevel - 1);
+	}
+}
+
+void DebugRenderer::Create8thSphere(Array<uint32> &ioIndices, Array<Vertex> &ioVertices, Vec3Arg inDir1, Vec3Arg inDir2, Vec3Arg inDir3, const Float2 &inUV, SupportFunction inGetSupport, int inLevel)
+{
+	uint32 idx1 = 0xffffffff;
+	uint32 idx2 = 0xffffffff;
+	uint32 idx3 = 0xffffffff;
+
+	Create8thSphereRecursive(ioIndices, ioVertices, inDir1, idx1, inDir2, idx2, inDir3, idx3, inUV, inGetSupport, inLevel);
+}
+
+DebugRenderer::Batch DebugRenderer::CreateCylinder(float inTop, float inBottom, float inTopRadius, float inBottomRadius, int inLevel)
+{
+	Array<Vertex> cylinder_vertices;
+	Array<uint32> cylinder_indices;
+
+	for (int q = 0; q < 4; ++q)
+	{
+		Float2 uv = (q & 1) == 0? Float2(0.25f, 0.75f) : Float2(0.25f, 0.25f);
+
+		uint32 center_start_idx = (uint32)cylinder_vertices.size();
+
+		Float3 nt(0.0f, 1.0f, 0.0f);
+		Float3 nb(0.0f, -1.0f, 0.0f);
+		cylinder_vertices.push_back({ Float3(0.0f, inTop, 0.0f), nt, uv, Color::sWhite });
+		cylinder_vertices.push_back({ Float3(0.0f, inBottom, 0.0f), nb, uv, Color::sWhite });
+
+		uint32 vtx_start_idx = (uint32)cylinder_vertices.size();
+
+		int num_parts = 1 << inLevel;
+		for (int i = 0; i <= num_parts; ++i)
+		{
+			// Calculate top and bottom vertex
+			float angle = 0.5f * JPH_PI * (float(q) + float(i) / num_parts);
+			float s = Sin(angle);
+			float c = Cos(angle);
+			Float3 vt(inTopRadius * s, inTop, inTopRadius * c);
+			Float3 vb(inBottomRadius * s, inBottom, inBottomRadius * c);
+
+			// Calculate normal
+			Vec3 edge = Vec3(vt) - Vec3(vb);
+			Float3 n;
+			edge.Cross(Vec3(s, 0, c).Cross(edge)).Normalized().StoreFloat3(&n);
+
+			cylinder_vertices.push_back({ vt, nt, uv, Color::sWhite });
+			cylinder_vertices.push_back({ vb, nb, uv, Color::sWhite });
+			cylinder_vertices.push_back({ vt, n, uv, Color::sWhite });
+			cylinder_vertices.push_back({ vb, n, uv, Color::sWhite });
+		}
+
+		for (int i = 0; i < num_parts; ++i)
+		{
+			uint32 start = vtx_start_idx + 4 * i;
+
+			// Top
+			cylinder_indices.push_back(center_start_idx);
+			cylinder_indices.push_back(start);
+			cylinder_indices.push_back(start + 4);
+
+			// Bottom
+			cylinder_indices.push_back(center_start_idx + 1);
+			cylinder_indices.push_back(start + 5);
+			cylinder_indices.push_back(start + 1);
+
+			// Side
+			cylinder_indices.push_back(start + 2);
+			cylinder_indices.push_back(start + 3);
+			cylinder_indices.push_back(start + 7);
+
+			cylinder_indices.push_back(start + 2);
+			cylinder_indices.push_back(start + 7);
+			cylinder_indices.push_back(start + 6);
+		}
+	}
+
+	return CreateTriangleBatch(cylinder_vertices, cylinder_indices);
+}
+
+void DebugRenderer::CreateQuad(Array<uint32> &ioIndices, Array<Vertex> &ioVertices, Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, Vec3Arg inV4)
+{
+	// Make room
+	uint32 start_idx = uint32(ioVertices.size());
+	ioVertices.resize(start_idx + 4);
+	Vertex *vertices = &ioVertices[start_idx];
+
+	// Set position
+	inV1.StoreFloat3(&vertices[0].mPosition);
+	inV2.StoreFloat3(&vertices[1].mPosition);
+	inV3.StoreFloat3(&vertices[2].mPosition);
+	inV4.StoreFloat3(&vertices[3].mPosition);
+
+	// Set color
+	vertices[0].mColor = vertices[1].mColor = vertices[2].mColor = vertices[3].mColor = Color::sWhite;
+
+	// Calculate normal
+	Vec3 normal = (inV2 - inV1).Cross(inV3 - inV1).Normalized();
+	Float3 normal3;
+	normal.StoreFloat3(&normal3);
+	vertices[0].mNormal = vertices[1].mNormal = vertices[2].mNormal = vertices[3].mNormal = normal3;
+
+	// Set UV's
+	vertices[0].mUV = { 0, 0 };
+	vertices[1].mUV = { 2, 0 };
+	vertices[2].mUV = { 2, 2 };
+	vertices[3].mUV = { 0, 2 };
+
+	// Set indices
+	ioIndices.push_back(start_idx);
+	ioIndices.push_back(start_idx + 1);
+	ioIndices.push_back(start_idx + 2);
+
+	ioIndices.push_back(start_idx);
+	ioIndices.push_back(start_idx + 2);
+	ioIndices.push_back(start_idx + 3);
+}
+
+void DebugRenderer::Initialize()
+{
+	// Box
+	{
+		Array<Vertex> box_vertices;
+		Array<uint32> box_indices;
+
+		// Get corner points
+		Vec3 v0 = Vec3(-1,  1, -1);
+		Vec3 v1 = Vec3( 1,  1, -1);
+		Vec3 v2 = Vec3( 1,  1,  1);
+		Vec3 v3 = Vec3(-1,  1,  1);
+		Vec3 v4 = Vec3(-1, -1, -1);
+		Vec3 v5 = Vec3( 1, -1, -1);
+		Vec3 v6 = Vec3( 1, -1,  1);
+		Vec3 v7 = Vec3(-1, -1,  1);
+
+		// Top
+		CreateQuad(box_indices, box_vertices, v0, v3, v2, v1);
+
+		// Bottom
+		CreateQuad(box_indices, box_vertices, v4, v5, v6, v7);
+
+		// Left
+		CreateQuad(box_indices, box_vertices, v0, v4, v7, v3);
+
+		// Right
+		CreateQuad(box_indices, box_vertices, v2, v6, v5, v1);
+
+		// Front
+		CreateQuad(box_indices, box_vertices, v3, v7, v6, v2);
+
+		// Back
+		CreateQuad(box_indices, box_vertices, v0, v1, v5, v4);
+
+		mBox = new Geometry(CreateTriangleBatch(box_vertices, box_indices), AABox(Vec3(-1, -1, -1), Vec3(1, 1, 1)));
+	}
+
+	// Support function that returns a unit sphere
+	auto sphere_support = [](Vec3Arg inDirection) { return inDirection; };
+
+	// Construct geometries
+	mSphere = new Geometry(AABox(Vec3(-1, -1, -1), Vec3(1, 1, 1)));
+	mCapsuleBottom = new Geometry(AABox(Vec3(-1, -1, -1), Vec3(1, 0, 1)));
+	mCapsuleTop = new Geometry(AABox(Vec3(-1, 0, -1), Vec3(1, 1, 1)));
+	mCapsuleMid = new Geometry(AABox(Vec3(-1, -1, -1), Vec3(1, 1, 1)));
+	mOpenCone = new Geometry(AABox(Vec3(-1, 0, -1), Vec3(1, 1, 1)));
+	mCylinder = new Geometry(AABox(Vec3(-1, -1, -1), Vec3(1, 1, 1)));
+
+	// Iterate over levels
+	for (int level = sMaxLevel; level >= 1; --level)
+	{
+		// Determine at which distance this level should be active
+		float distance = sLODDistanceForLevel[sMaxLevel - level];
+
+		// Sphere
+		mSphere->mLODs.push_back({ CreateTriangleBatchForConvex(sphere_support, level), distance });
+
+		// Capsule bottom half sphere
+		{
+			Array<Vertex> capsule_bottom_vertices;
+			Array<uint32> capsule_bottom_indices;
+			Create8thSphere(capsule_bottom_indices, capsule_bottom_vertices, -Vec3::sAxisX(), -Vec3::sAxisY(),  Vec3::sAxisZ(), Float2(0.25f, 0.25f), sphere_support, level);
+			Create8thSphere(capsule_bottom_indices, capsule_bottom_vertices, -Vec3::sAxisY(),  Vec3::sAxisX(),  Vec3::sAxisZ(), Float2(0.25f, 0.75f), sphere_support, level);
+			Create8thSphere(capsule_bottom_indices, capsule_bottom_vertices,  Vec3::sAxisX(), -Vec3::sAxisY(), -Vec3::sAxisZ(), Float2(0.25f, 0.25f), sphere_support, level);
+			Create8thSphere(capsule_bottom_indices, capsule_bottom_vertices, -Vec3::sAxisY(), -Vec3::sAxisX(), -Vec3::sAxisZ(), Float2(0.25f, 0.75f), sphere_support, level);
+			mCapsuleBottom->mLODs.push_back({ CreateTriangleBatch(capsule_bottom_vertices, capsule_bottom_indices), distance });
+		}
+
+		// Capsule top half sphere
+		{
+			Array<Vertex> capsule_top_vertices;
+			Array<uint32> capsule_top_indices;
+			Create8thSphere(capsule_top_indices, capsule_top_vertices,  Vec3::sAxisX(),  Vec3::sAxisY(),  Vec3::sAxisZ(), Float2(0.25f, 0.75f), sphere_support, level);
+			Create8thSphere(capsule_top_indices, capsule_top_vertices,  Vec3::sAxisY(), -Vec3::sAxisX(),  Vec3::sAxisZ(), Float2(0.25f, 0.25f), sphere_support, level);
+			Create8thSphere(capsule_top_indices, capsule_top_vertices,  Vec3::sAxisY(),  Vec3::sAxisX(), -Vec3::sAxisZ(), Float2(0.25f, 0.25f), sphere_support, level);
+			Create8thSphere(capsule_top_indices, capsule_top_vertices, -Vec3::sAxisX(),  Vec3::sAxisY(), -Vec3::sAxisZ(), Float2(0.25f, 0.75f), sphere_support, level);
+			mCapsuleTop->mLODs.push_back({ CreateTriangleBatch(capsule_top_vertices, capsule_top_indices), distance });
+		}
+
+		// Capsule middle part
+		{
+			Array<Vertex> capsule_mid_vertices;
+			Array<uint32> capsule_mid_indices;
+			for (int q = 0; q < 4; ++q)
+			{
+				Float2 uv = (q & 1) == 0? Float2(0.25f, 0.25f) : Float2(0.25f, 0.75f);
+
+				uint32 start_idx = (uint32)capsule_mid_vertices.size();
+
+				int num_parts = 1 << level;
+				for (int i = 0; i <= num_parts; ++i)
+				{
+					float angle = 0.5f * JPH_PI * (float(q) + float(i) / num_parts);
+					float s = Sin(angle);
+					float c = Cos(angle);
+					Float3 vt(s, 1.0f, c);
+					Float3 vb(s, -1.0f, c);
+					Float3 n(s, 0, c);
+
+					capsule_mid_vertices.push_back({ vt, n, uv, Color::sWhite });
+					capsule_mid_vertices.push_back({ vb, n, uv, Color::sWhite });
+				}
+
+				for (int i = 0; i < num_parts; ++i)
+				{
+					uint32 start = start_idx + 2 * i;
+
+					capsule_mid_indices.push_back(start);
+					capsule_mid_indices.push_back(start + 1);
+					capsule_mid_indices.push_back(start + 3);
+
+					capsule_mid_indices.push_back(start);
+					capsule_mid_indices.push_back(start + 3);
+					capsule_mid_indices.push_back(start + 2);
+				}
+			}
+			mCapsuleMid->mLODs.push_back({ CreateTriangleBatch(capsule_mid_vertices, capsule_mid_indices), distance });
+		}
+
+		// Open cone
+		{
+			Array<Vertex> open_cone_vertices;
+			Array<uint32> open_cone_indices;
+			for (int q = 0; q < 4; ++q)
+			{
+				Float2 uv = (q & 1) == 0? Float2(0.25f, 0.25f) : Float2(0.25f, 0.75f);
+
+				uint32 start_idx = (uint32)open_cone_vertices.size();
+
+				int num_parts = 2 << level;
+				Float3 vt(0, 0, 0);
+				for (int i = 0; i <= num_parts; ++i)
+				{
+					// Calculate bottom vertex
+					float angle = 0.5f * JPH_PI * (float(q) + float(i) / num_parts);
+					float s = Sin(angle);
+					float c = Cos(angle);
+					Float3 vb(s, 1.0f, c);
+
+					// Calculate normal
+					// perpendicular = Y cross vb (perpendicular to the plane in which 0, y and vb exists)
+					// normal = perpendicular cross vb (normal to the edge 0 vb)
+					Vec3 normal = Vec3(s, -Square(s) - Square(c), c).Normalized();
+					Float3 n; normal.StoreFloat3(&n);
+
+					open_cone_vertices.push_back({ vt, n, uv, Color::sWhite });
+					open_cone_vertices.push_back({ vb, n, uv, Color::sWhite });
+				}
+
+				for (int i = 0; i < num_parts; ++i)
+				{
+					uint32 start = start_idx + 2 * i;
+
+					open_cone_indices.push_back(start);
+					open_cone_indices.push_back(start + 1);
+					open_cone_indices.push_back(start + 3);
+				}
+			}
+			mOpenCone->mLODs.push_back({ CreateTriangleBatch(open_cone_vertices, open_cone_indices), distance });
+		}
+
+		// Cylinder
+		mCylinder->mLODs.push_back({ CreateCylinder(1.0f, -1.0f, 1.0f, 1.0f, level), distance });
+	}
+}
+
+AABox DebugRenderer::sCalculateBounds(const Vertex *inVertices, int inVertexCount)
+{
+	AABox bounds;
+	for (const Vertex *v = inVertices, *v_end = inVertices + inVertexCount; v < v_end; ++v)
+		bounds.Encapsulate(Vec3(v->mPosition));
+	return bounds;
+}
+
+DebugRenderer::Batch DebugRenderer::CreateTriangleBatch(const VertexList &inVertices, const IndexedTriangleNoMaterialList &inTriangles)
+{
+	JPH_PROFILE_FUNCTION();
+
+	Array<Vertex> vertices;
+
+	// Create render vertices
+	vertices.resize(inVertices.size());
+	for (size_t v = 0; v < inVertices.size(); ++v)
+	{
+		vertices[v].mPosition = inVertices[v];
+		vertices[v].mNormal = Float3(0, 0, 0);
+		vertices[v].mUV = Float2(0, 0);
+		vertices[v].mColor = Color::sWhite;
+	}
+
+	// Calculate normals
+	for (size_t i = 0; i < inTriangles.size(); ++i)
+	{
+		const IndexedTriangleNoMaterial &tri = inTriangles[i];
+
+		// Calculate normal of face
+		Vec3 vtx[3];
+		for (int j = 0; j < 3; ++j)
+			vtx[j] = Vec3::sLoadFloat3Unsafe(vertices[tri.mIdx[j]].mPosition);
+		Vec3 normal = ((vtx[1] - vtx[0]).Cross(vtx[2] - vtx[0])).Normalized();
+
+		// Add normal to all vertices in face
+		for (int j = 0; j < 3; ++j)
+			(Vec3::sLoadFloat3Unsafe(vertices[tri.mIdx[j]].mNormal) + normal).StoreFloat3(&vertices[tri.mIdx[j]].mNormal);
+	}
+
+	// Renormalize vertex normals
+	for (size_t i = 0; i < vertices.size(); ++i)
+		Vec3::sLoadFloat3Unsafe(vertices[i].mNormal).Normalized().StoreFloat3(&vertices[i].mNormal);
+
+	return CreateTriangleBatch(&vertices[0], (int)vertices.size(), &inTriangles[0].mIdx[0], (int)(3 * inTriangles.size()));
+}
+
+DebugRenderer::Batch DebugRenderer::CreateTriangleBatchForConvex(SupportFunction inGetSupport, int inLevel, AABox *outBounds)
+{
+	JPH_PROFILE_FUNCTION();
+
+	Array<Vertex> vertices;
+	Array<uint32> indices;
+	Create8thSphere(indices, vertices,  Vec3::sAxisX(),  Vec3::sAxisY(),  Vec3::sAxisZ(), Float2(0.25f, 0.25f), inGetSupport, inLevel);
+	Create8thSphere(indices, vertices,  Vec3::sAxisY(), -Vec3::sAxisX(),  Vec3::sAxisZ(), Float2(0.25f, 0.75f), inGetSupport, inLevel);
+	Create8thSphere(indices, vertices, -Vec3::sAxisY(),  Vec3::sAxisX(),  Vec3::sAxisZ(), Float2(0.25f, 0.75f), inGetSupport, inLevel);
+	Create8thSphere(indices, vertices, -Vec3::sAxisX(), -Vec3::sAxisY(),  Vec3::sAxisZ(), Float2(0.25f, 0.25f), inGetSupport, inLevel);
+	Create8thSphere(indices, vertices,  Vec3::sAxisY(),  Vec3::sAxisX(), -Vec3::sAxisZ(), Float2(0.25f, 0.75f), inGetSupport, inLevel);
+	Create8thSphere(indices, vertices, -Vec3::sAxisX(),  Vec3::sAxisY(), -Vec3::sAxisZ(), Float2(0.25f, 0.25f), inGetSupport, inLevel);
+	Create8thSphere(indices, vertices,  Vec3::sAxisX(), -Vec3::sAxisY(), -Vec3::sAxisZ(), Float2(0.25f, 0.25f), inGetSupport, inLevel);
+	Create8thSphere(indices, vertices, -Vec3::sAxisY(), -Vec3::sAxisX(), -Vec3::sAxisZ(), Float2(0.25f, 0.75f), inGetSupport, inLevel);
+
+	if (outBounds != nullptr)
+		*outBounds = sCalculateBounds(&vertices[0], (int)vertices.size());
+
+	return CreateTriangleBatch(vertices, indices);
+}
+
+DebugRenderer::GeometryRef DebugRenderer::CreateTriangleGeometryForConvex(SupportFunction inGetSupport)
+{
+	GeometryRef geometry;
+
+	// Iterate over levels
+	for (int level = sMaxLevel; level >= 1; --level)
+	{
+		// Determine at which distance this level should be active
+		float distance = sLODDistanceForLevel[sMaxLevel - level];
+
+		// Create triangle batch and only calculate bounds for highest LOD level
+		AABox bounds;
+		Batch batch = CreateTriangleBatchForConvex(inGetSupport, level, geometry == nullptr? &bounds : nullptr);
+
+		// Construct geometry in the first iteration
+		if (geometry == nullptr)
+			geometry = new Geometry(bounds);
+
+		// Add the LOD
+		geometry->mLODs.push_back({ batch, distance });
+	}
+
+	return geometry;
+}
+
+void DebugRenderer::DrawBox(const AABox &inBox, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode)
+{
+	JPH_PROFILE_FUNCTION();
+
+	RMat44 m = RMat44::sScale(Vec3::sMax(inBox.GetExtent(), Vec3::sReplicate(1.0e-6f))); // Prevent div by zero when one of the edges has length 0
+	m.SetTranslation(RVec3(inBox.GetCenter()));
+	DrawGeometry(m, inColor, mBox, ECullMode::CullBackFace, inCastShadow, inDrawMode);
+}
+
+void DebugRenderer::DrawBox(RMat44Arg inMatrix, const AABox &inBox, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode)
+{
+	JPH_PROFILE_FUNCTION();
+
+	Mat44 m = Mat44::sScale(Vec3::sMax(inBox.GetExtent(), Vec3::sReplicate(1.0e-6f))); // Prevent div by zero when one of the edges has length 0
+	m.SetTranslation(inBox.GetCenter());
+	DrawGeometry(inMatrix * m, inColor, mBox, ECullMode::CullBackFace, inCastShadow, inDrawMode);
+}
+
+void DebugRenderer::DrawSphere(RVec3Arg inCenter, float inRadius, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode)
+{
+	JPH_PROFILE_FUNCTION();
+
+	RMat44 matrix = RMat44::sTranslation(inCenter) * Mat44::sScale(inRadius);
+
+	DrawUnitSphere(matrix, inColor, inCastShadow, inDrawMode);
+}
+
+void DebugRenderer::DrawUnitSphere(RMat44Arg inMatrix, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode)
+{
+	JPH_PROFILE_FUNCTION();
+
+	DrawGeometry(inMatrix, inColor, mSphere, ECullMode::CullBackFace, inCastShadow, inDrawMode);
+}
+
+void DebugRenderer::DrawCapsule(RMat44Arg inMatrix, float inHalfHeightOfCylinder, float inRadius, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode)
+{
+	JPH_PROFILE_FUNCTION();
+
+	Mat44 scale_matrix = Mat44::sScale(inRadius);
+
+	// Calculate world space bounding box
+	AABox local_bounds(Vec3(-inRadius, -inHalfHeightOfCylinder - inRadius, -inRadius), Vec3(inRadius, inHalfHeightOfCylinder + inRadius, inRadius));
+	AABox world_bounds = local_bounds.Transformed(inMatrix);
+
+	float radius_sq = Square(inRadius);
+
+	// Draw bottom half sphere
+	RMat44 bottom_matrix = inMatrix * Mat44::sTranslation(Vec3(0, -inHalfHeightOfCylinder, 0)) * scale_matrix;
+	DrawGeometry(bottom_matrix, world_bounds, radius_sq, inColor, mCapsuleBottom, ECullMode::CullBackFace, inCastShadow, inDrawMode);
+
+	// Draw top half sphere
+	RMat44 top_matrix = inMatrix * Mat44::sTranslation(Vec3(0, inHalfHeightOfCylinder, 0)) * scale_matrix;
+	DrawGeometry(top_matrix, world_bounds, radius_sq, inColor, mCapsuleTop, ECullMode::CullBackFace, inCastShadow, inDrawMode);
+
+	// Draw middle part
+	DrawGeometry(inMatrix * Mat44::sScale(Vec3(inRadius, inHalfHeightOfCylinder, inRadius)), world_bounds, radius_sq, inColor, mCapsuleMid, ECullMode::CullBackFace, inCastShadow, inDrawMode);
+}
+
+void DebugRenderer::DrawCylinder(RMat44Arg inMatrix, float inHalfHeight, float inRadius, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode)
+{
+	JPH_PROFILE_FUNCTION();
+
+	Mat44 local_transform(Vec4(inRadius, 0, 0, 0), Vec4(0, inHalfHeight, 0, 0), Vec4(0, 0, inRadius, 0), Vec4(0, 0, 0, 1));
+	RMat44 transform = inMatrix * local_transform;
+
+	DrawGeometry(transform, mCylinder->mBounds.Transformed(transform), Square(inRadius), inColor, mCylinder, ECullMode::CullBackFace, inCastShadow, inDrawMode);
+}
+
+void DebugRenderer::DrawOpenCone(RVec3Arg inTop, Vec3Arg inAxis, Vec3Arg inPerpendicular, float inHalfAngle, float inLength, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode)
+{
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(inAxis.IsNormalized(1.0e-4f));
+	JPH_ASSERT(inPerpendicular.IsNormalized(1.0e-4f));
+	JPH_ASSERT(abs(inPerpendicular.Dot(inAxis)) < 1.0e-4f);
+
+	Vec3 axis = Sign(inHalfAngle) * inLength * inAxis;
+	float scale = inLength * Tan(abs(inHalfAngle));
+	if (scale != 0.0f)
+	{
+		Vec3 perp1 = scale * inPerpendicular;
+		Vec3 perp2 = scale * inAxis.Cross(inPerpendicular);
+		RMat44 transform(Vec4(perp1, 0), Vec4(axis, 0), Vec4(perp2, 0), inTop);
+		DrawGeometry(transform, inColor, mOpenCone, ECullMode::Off, inCastShadow, inDrawMode);
+	}
+}
+
+DebugRenderer::Geometry *DebugRenderer::CreateSwingLimitGeometry(int inNumSegments, const Vec3 *inVertices)
+{
+	// Allocate space for vertices
+	int num_vertices = 2 * inNumSegments;
+	Vertex *vertices_start = (Vertex *)JPH_STACK_ALLOC(num_vertices * sizeof(Vertex));
+	Vertex *vertices = vertices_start;
+
+	for (int i = 0; i < inNumSegments; ++i)
+	{
+		// Get output vertices
+		Vertex &top = *(vertices++);
+		Vertex &bottom = *(vertices++);
+
+		// Get local position
+		const Vec3 &pos = inVertices[i];
+
+		// Get local normal
+		const Vec3 &prev_pos = inVertices[(i + inNumSegments - 1) % inNumSegments];
+		const Vec3 &next_pos = inVertices[(i + 1) % inNumSegments];
+		Vec3 normal = 0.5f * (next_pos.Cross(pos).NormalizedOr(Vec3::sZero()) + pos.Cross(prev_pos).NormalizedOr(Vec3::sZero()));
+
+		// Store top vertex
+		top.mPosition = { 0, 0, 0 };
+		normal.StoreFloat3(&top.mNormal);
+		top.mColor = Color::sWhite;
+		top.mUV = { 0, 0 };
+
+		// Store bottom vertex
+		pos.StoreFloat3(&bottom.mPosition);
+		normal.StoreFloat3(&bottom.mNormal);
+		bottom.mColor = Color::sWhite;
+		bottom.mUV = { 0, 0 };
+	}
+
+	// Allocate space for indices
+	int num_indices = 3 * inNumSegments;
+	uint32 *indices_start = (uint32 *)JPH_STACK_ALLOC(num_indices * sizeof(uint32));
+	uint32 *indices = indices_start;
+
+	// Calculate indices
+	for (int i = 0; i < inNumSegments; ++i)
+	{
+		int first = 2 * i;
+		int second = (first + 3) % num_vertices;
+		int third = first + 1;
+
+		// Triangle
+		*indices++ = first;
+		*indices++ = second;
+		*indices++ = third;
+	}
+
+	// Convert to triangle batch
+	return new Geometry(CreateTriangleBatch(vertices_start, num_vertices, indices_start, num_indices), sCalculateBounds(vertices_start, num_vertices));
+}
+
+void DebugRenderer::DrawSwingConeLimits(RMat44Arg inMatrix, float inSwingYHalfAngle, float inSwingZHalfAngle, float inEdgeLength, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Assert sane input
+	JPH_ASSERT(inSwingYHalfAngle >= 0.0f && inSwingYHalfAngle <= JPH_PI);
+	JPH_ASSERT(inSwingZHalfAngle >= 0.0f && inSwingZHalfAngle <= JPH_PI);
+	JPH_ASSERT(inEdgeLength > 0.0f);
+
+	// Check cache
+	SwingConeLimits limits { inSwingYHalfAngle, inSwingZHalfAngle };
+	GeometryRef &geometry = mSwingConeLimits[limits];
+	if (geometry == nullptr)
+	{
+		SwingConeBatches::iterator it = mPrevSwingConeLimits.find(limits);
+		if (it != mPrevSwingConeLimits.end())
+			geometry = it->second;
+	}
+	if (geometry == nullptr)
+	{
+		// Number of segments to draw the cone with
+		const int num_segments = 64;
+		int half_num_segments = num_segments / 2;
+
+		// The y and z values of the quaternion are limited to an ellipse, e1 and e2 are the radii of this ellipse
+		float e1 = Sin(0.5f * inSwingZHalfAngle);
+		float e2 = Sin(0.5f * inSwingYHalfAngle);
+
+		// Check if the limits will draw something
+		if ((e1 <= 0.0f && e2 <= 0.0f) || (e2 >= 1.0f && e1 >= 1.0f))
+			return;
+
+		// Calculate squared values
+		float e1_sq = Square(e1);
+		float e2_sq = Square(e2);
+
+		// Calculate local space vertices for shape
+		Vec3 ls_vertices[num_segments];
+		int tgt_vertex = 0;
+		for (int side_iter = 0; side_iter < 2; ++side_iter)
+			for (int segment_iter = 0; segment_iter < half_num_segments; ++segment_iter)
+			{
+				float y, z;
+				if (e2_sq > e1_sq)
+				{
+					// Trace the y value of the quaternion
+					y = e2 - 2.0f * segment_iter * e2 / half_num_segments;
+
+					// Calculate the corresponding z value of the quaternion
+					float z_sq = e1_sq - e1_sq / e2_sq * Square(y);
+					z = z_sq <= 0.0f? 0.0f : sqrt(z_sq);
+				}
+				else
+				{
+					// Trace the z value of the quaternion
+					z = -e1 + 2.0f * segment_iter * e1 / half_num_segments;
+
+					// Calculate the corresponding y value of the quaternion
+					float y_sq = e2_sq - e2_sq / e1_sq * Square(z);
+					y = y_sq <= 0.0f? 0.0f : sqrt(y_sq);
+				}
+
+				// If we're tracing the opposite side, flip the values
+				if (side_iter == 1)
+				{
+					z = -z;
+					y = -y;
+				}
+
+				// Create quaternion
+				Vec3 q_xyz(0, y, z);
+				float w = sqrt(1.0f - q_xyz.LengthSq());
+				Quat q(Vec4(q_xyz, w));
+
+				// Store vertex
+				ls_vertices[tgt_vertex++] = q.RotateAxisX();
+			}
+
+		geometry = CreateSwingLimitGeometry(num_segments, ls_vertices);
+	}
+
+	DrawGeometry(inMatrix * Mat44::sScale(inEdgeLength), inColor, geometry, ECullMode::Off, inCastShadow, inDrawMode);
+}
+
+void DebugRenderer::DrawSwingPyramidLimits(RMat44Arg inMatrix, float inMinSwingYAngle, float inMaxSwingYAngle, float inMinSwingZAngle, float inMaxSwingZAngle, float inEdgeLength, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode)
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Assert sane input
+	JPH_ASSERT(inMinSwingYAngle <= inMaxSwingYAngle && inMinSwingZAngle <= inMaxSwingZAngle);
+	JPH_ASSERT(inEdgeLength > 0.0f);
+
+	// Check cache
+	SwingPyramidLimits limits { inMinSwingYAngle, inMaxSwingYAngle, inMinSwingZAngle, inMaxSwingZAngle };
+	GeometryRef &geometry = mSwingPyramidLimits[limits];
+	if (geometry == nullptr)
+	{
+		SwingPyramidBatches::iterator it = mPrevSwingPyramidLimits.find(limits);
+		if (it != mPrevSwingPyramidLimits.end())
+			geometry = it->second;
+	}
+	if (geometry == nullptr)
+	{
+		// Number of segments to draw the cone with
+		const int num_segments = 64;
+		int quarter_num_segments = num_segments / 4;
+
+		// Note that this is q = Quat::sRotation(Vec3::sAxisZ(), z) * Quat::sRotation(Vec3::sAxisY(), y) with q.x set to zero so we don't introduce twist
+		// This matches the calculation in SwingTwistConstraintPart::ClampSwingTwist
+		auto get_axis = [](float inY, float inZ) {
+			float hy = 0.5f * inY;
+			float hz = 0.5f * inZ;
+			float cos_hy = Cos(hy);
+			float cos_hz = Cos(hz);
+			return Quat(0, Sin(hy) * cos_hz, cos_hy * Sin(hz), cos_hy * cos_hz).Normalized().RotateAxisX();
+		};
+
+		// Calculate local space vertices for shape
+		Vec3 ls_vertices[num_segments];
+		int tgt_vertex = 0;
+		for (int segment_iter = 0; segment_iter < quarter_num_segments; ++segment_iter)
+			ls_vertices[tgt_vertex++] = get_axis(inMinSwingYAngle, inMaxSwingZAngle - (inMaxSwingZAngle - inMinSwingZAngle) * segment_iter / quarter_num_segments);
+		for (int segment_iter = 0; segment_iter < quarter_num_segments; ++segment_iter)
+			ls_vertices[tgt_vertex++] = get_axis(inMinSwingYAngle + (inMaxSwingYAngle - inMinSwingYAngle) * segment_iter / quarter_num_segments, inMinSwingZAngle);
+		for (int segment_iter = 0; segment_iter < quarter_num_segments; ++segment_iter)
+			ls_vertices[tgt_vertex++] = get_axis(inMaxSwingYAngle, inMinSwingZAngle + (inMaxSwingZAngle - inMinSwingZAngle) * segment_iter / quarter_num_segments);
+		for (int segment_iter = 0; segment_iter < quarter_num_segments; ++segment_iter)
+			ls_vertices[tgt_vertex++] = get_axis(inMaxSwingYAngle - (inMaxSwingYAngle - inMinSwingYAngle) * segment_iter / quarter_num_segments, inMaxSwingZAngle);
+
+		geometry = CreateSwingLimitGeometry(num_segments, ls_vertices);
+	}
+
+	DrawGeometry(inMatrix * Mat44::sScale(inEdgeLength), inColor, geometry, ECullMode::Off, inCastShadow, inDrawMode);
+}
+
+void DebugRenderer::DrawPie(RVec3Arg inCenter, float inRadius, Vec3Arg inNormal, Vec3Arg inAxis, float inMinAngle, float inMaxAngle, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode)
+{
+	if (inMinAngle >= inMaxAngle)
+		return;
+
+	JPH_PROFILE_FUNCTION();
+
+	JPH_ASSERT(inAxis.IsNormalized(1.0e-4f));
+	JPH_ASSERT(inNormal.IsNormalized(1.0e-4f));
+	JPH_ASSERT(abs(inNormal.Dot(inAxis)) < 1.0e-4f);
+
+	// Pies have a unique batch based on the difference between min and max angle
+	float delta_angle = inMaxAngle - inMinAngle;
+	GeometryRef &geometry = mPieLimits[delta_angle];
+	if (geometry == nullptr)
+	{
+		PieBatces::iterator it = mPrevPieLimits.find(delta_angle);
+		if (it != mPrevPieLimits.end())
+			geometry = it->second;
+	}
+	if (geometry == nullptr)
+	{
+		int num_parts = (int)ceil(64.0f * delta_angle / (2.0f * JPH_PI));
+
+		Float3 normal = { 0, 1, 0 };
+		Float3 center = { 0, 0, 0 };
+
+		// Allocate space for vertices
+		int num_vertices = num_parts + 2;
+		Vertex *vertices_start = (Vertex *)JPH_STACK_ALLOC(num_vertices * sizeof(Vertex));
+		Vertex *vertices = vertices_start;
+
+		// Center of circle
+		*vertices++ = { center, normal, { 0, 0 }, Color::sWhite };
+
+		// Outer edge of pie
+		for (int i = 0; i <= num_parts; ++i)
+		{
+			float angle = float(i) / float(num_parts) * delta_angle;
+
+			Float3 pos = { Cos(angle), 0, Sin(angle) };
+			*vertices++ = { pos, normal, { 0, 0 }, Color::sWhite };
+		}
+
+		// Allocate space for indices
+		int num_indices = num_parts * 3;
+		uint32 *indices_start = (uint32 *)JPH_STACK_ALLOC(num_indices * sizeof(uint32));
+		uint32 *indices = indices_start;
+
+		for (int i = 0; i < num_parts; ++i)
+		{
+			*indices++ = 0;
+			*indices++ = i + 1;
+			*indices++ = i + 2;
+		}
+
+		// Convert to triangle batch
+		geometry = new Geometry(CreateTriangleBatch(vertices_start, num_vertices, indices_start, num_indices), sCalculateBounds(vertices_start, num_vertices));
+	}
+
+	// Construct matrix that transforms pie into world space
+	RMat44 matrix = RMat44(Vec4(inRadius * inAxis, 0), Vec4(inRadius * inNormal, 0), Vec4(inRadius * inNormal.Cross(inAxis), 0), inCenter) * Mat44::sRotationY(-inMinAngle);
+
+	DrawGeometry(matrix, inColor, geometry, ECullMode::Off, inCastShadow, inDrawMode);
+}
+
+void DebugRenderer::DrawTaperedCylinder(RMat44Arg inMatrix, float inTop, float inBottom, float inTopRadius, float inBottomRadius, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode)
+{
+	TaperedCylinder tapered_cylinder { inTop, inBottom, inTopRadius, inBottomRadius };
+
+	GeometryRef &geometry = mTaperedCylinders[tapered_cylinder];
+	if (geometry == nullptr)
+	{
+		TaperedCylinderBatces::iterator it = mPrevTaperedCylinders.find(tapered_cylinder);
+		if (it != mPrevTaperedCylinders.end())
+			geometry = it->second;
+	}
+	if (geometry == nullptr)
+	{
+		float max_radius = max(inTopRadius, inBottomRadius);
+		geometry = new Geometry(AABox(Vec3(-max_radius, inBottom, -max_radius), Vec3(max_radius, inTop, max_radius)));
+
+		for (int level = sMaxLevel; level >= 1; --level)
+			geometry->mLODs.push_back({ CreateCylinder(inTop, inBottom, inTopRadius, inBottomRadius, level), sLODDistanceForLevel[sMaxLevel - level] });
+	}
+
+	DrawGeometry(inMatrix, inColor, geometry, ECullMode::CullBackFace, inCastShadow, inDrawMode);
+}
+
+void DebugRenderer::NextFrame()
+{
+	mPrevSwingConeLimits.clear();
+	std::swap(mSwingConeLimits, mPrevSwingConeLimits);
+
+	mPrevSwingPyramidLimits.clear();
+	std::swap(mSwingPyramidLimits, mPrevSwingPyramidLimits);
+
+	mPrevPieLimits.clear();
+	std::swap(mPieLimits, mPrevPieLimits);
+
+	mPrevTaperedCylinders.clear();
+	std::swap(mTaperedCylinders, mPrevTaperedCylinders);
+}
+
+JPH_NAMESPACE_END
+
+#endif // JPH_DEBUG_RENDERER
diff --git a/thirdparty/jolt_physics/Jolt/Renderer/DebugRenderer.h b/thirdparty/jolt_physics/Jolt/Renderer/DebugRenderer.h
new file mode 100644
index 0000000000..7e06ed49a4
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Renderer/DebugRenderer.h
@@ -0,0 +1,383 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#ifndef JPH_DEBUG_RENDERER
+	#error This file should only be included when JPH_DEBUG_RENDERER is defined
+#endif // !JPH_DEBUG_RENDERER
+
+#ifndef JPH_DEBUG_RENDERER_EXPORT
+	// By default export the debug renderer
+	#define JPH_DEBUG_RENDERER_EXPORT JPH_EXPORT
+#endif // !JPH_DEBUG_RENDERER_EXPORT
+
+#include <Jolt/Core/Color.h>
+#include <Jolt/Core/Reference.h>
+#include <Jolt/Core/HashCombine.h>
+#include <Jolt/Core/UnorderedMap.h>
+#include <Jolt/Core/NonCopyable.h>
+#include <Jolt/Math/Float2.h>
+#include <Jolt/Geometry/IndexedTriangle.h>
+#include <Jolt/Geometry/AABox.h>
+
+JPH_NAMESPACE_BEGIN
+
+class OrientedBox;
+
+/// Simple triangle renderer for debugging purposes.
+///
+/// Inherit from this class to provide your own implementation.
+///
+/// Implement the following virtual functions:
+/// - DrawLine
+/// - DrawTriangle
+/// - DrawText3D
+/// - CreateTriangleBatch
+/// - DrawGeometry
+///
+/// Make sure you call Initialize() from the constructor of your implementation.
+///
+/// The CreateTriangleBatch is used to prepare a batch of triangles to be drawn by a single DrawGeometry call,
+/// which means that Jolt can render a complex scene much more efficiently than when each triangle in that scene would have been drawn through DrawTriangle.
+///
+/// Note that an implementation that implements CreateTriangleBatch and DrawGeometry is provided by DebugRendererSimple which can be used to start quickly.
+class JPH_DEBUG_RENDERER_EXPORT DebugRenderer : public NonCopyable
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+							DebugRenderer();
+	virtual					~DebugRenderer();
+
+	/// Call once after frame is complete. Releases unused dynamically generated geometry assets.
+	void					NextFrame();
+
+	/// Draw line
+	virtual void			DrawLine(RVec3Arg inFrom, RVec3Arg inTo, ColorArg inColor) = 0;
+
+	/// Draw wireframe box
+	void					DrawWireBox(const AABox &inBox, ColorArg inColor);
+	void					DrawWireBox(const OrientedBox &inBox, ColorArg inColor);
+	void					DrawWireBox(RMat44Arg inMatrix, const AABox &inBox, ColorArg inColor);
+
+	/// Draw a marker on a position
+	void					DrawMarker(RVec3Arg inPosition, ColorArg inColor, float inSize);
+
+	/// Draw an arrow
+	void					DrawArrow(RVec3Arg inFrom, RVec3Arg inTo, ColorArg inColor, float inSize);
+
+	/// Draw coordinate system (3 arrows, x = red, y = green, z = blue)
+	void					DrawCoordinateSystem(RMat44Arg inTransform, float inSize = 1.0f);
+
+	/// Draw a plane through inPoint with normal inNormal
+	void					DrawPlane(RVec3Arg inPoint, Vec3Arg inNormal, ColorArg inColor, float inSize);
+
+	/// Draw wireframe triangle
+	void					DrawWireTriangle(RVec3Arg inV1, RVec3Arg inV2, RVec3Arg inV3, ColorArg inColor);
+
+	/// Draw a wireframe polygon
+	template <class VERTEX_ARRAY>
+	void					DrawWirePolygon(RMat44Arg inTransform, const VERTEX_ARRAY &inVertices, ColorArg inColor, float inArrowSize = 0.0f) { for (typename VERTEX_ARRAY::size_type i = 0; i < inVertices.size(); ++i) DrawArrow(inTransform * inVertices[i], inTransform * inVertices[(i + 1) % inVertices.size()], inColor, inArrowSize); }
+
+	/// Draw wireframe sphere
+	void					DrawWireSphere(RVec3Arg inCenter, float inRadius, ColorArg inColor, int inLevel = 3);
+	void					DrawWireUnitSphere(RMat44Arg inMatrix, ColorArg inColor, int inLevel = 3);
+
+	/// Enum that determines if a shadow should be cast or not
+	enum class ECastShadow
+	{
+		On,					///< This shape should cast a shadow
+		Off					///< This shape should not cast a shadow
+	};
+
+	/// Determines how triangles are drawn
+	enum class EDrawMode
+	{
+		Solid,				///< Draw as a solid shape
+		Wireframe,			///< Draw as wireframe
+	};
+
+	/// Draw a single back face culled triangle
+	virtual void			DrawTriangle(RVec3Arg inV1, RVec3Arg inV2, RVec3Arg inV3, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::Off) = 0;
+
+	/// Draw a box
+	void					DrawBox(const AABox &inBox, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid);
+	void					DrawBox(RMat44Arg inMatrix, const AABox &inBox, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid);
+
+	/// Draw a sphere
+	void					DrawSphere(RVec3Arg inCenter, float inRadius, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid);
+	void					DrawUnitSphere(RMat44Arg inMatrix, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid);
+
+	/// Draw a capsule with one half sphere at (0, -inHalfHeightOfCylinder, 0) and the other half sphere at (0, inHalfHeightOfCylinder, 0) and radius inRadius.
+	/// The capsule will be transformed by inMatrix.
+	void					DrawCapsule(RMat44Arg inMatrix, float inHalfHeightOfCylinder, float inRadius, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid);
+
+	/// Draw a cylinder with top (0, inHalfHeight, 0) and bottom (0, -inHalfHeight, 0) and radius inRadius.
+	/// The cylinder will be transformed by inMatrix
+	void					DrawCylinder(RMat44Arg inMatrix, float inHalfHeight, float inRadius, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid);
+
+	/// Draw a bottomless cone.
+	/// @param inTop Top of cone, center of base is at inTop + inAxis.
+	/// @param inAxis Height and direction of cone
+	/// @param inPerpendicular Perpendicular vector to inAxis.
+	/// @param inHalfAngle Specifies the cone angle in radians (angle measured between inAxis and cone surface).
+	/// @param inLength The length of the cone.
+	/// @param inColor Color to use for drawing the cone.
+	/// @param inCastShadow determines if this geometry should cast a shadow or not.
+	/// @param inDrawMode determines if we draw the geometry solid or in wireframe.
+	void					DrawOpenCone(RVec3Arg inTop, Vec3Arg inAxis, Vec3Arg inPerpendicular, float inHalfAngle, float inLength, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid);
+
+	/// Draws cone rotation limits as used by the SwingTwistConstraintPart.
+	/// @param inMatrix Matrix that transforms from constraint space to world space
+	/// @param inSwingYHalfAngle See SwingTwistConstraintPart
+	/// @param inSwingZHalfAngle See SwingTwistConstraintPart
+	/// @param inEdgeLength Size of the edge of the cone shape
+	/// @param inColor Color to use for drawing the cone.
+	/// @param inCastShadow determines if this geometry should cast a shadow or not.
+	/// @param inDrawMode determines if we draw the geometry solid or in wireframe.
+	void					DrawSwingConeLimits(RMat44Arg inMatrix, float inSwingYHalfAngle, float inSwingZHalfAngle, float inEdgeLength, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid);
+
+	/// Draws rotation limits as used by the SwingTwistConstraintPart.
+	/// @param inMatrix Matrix that transforms from constraint space to world space
+	/// @param inMinSwingYAngle See SwingTwistConstraintPart
+	/// @param inMaxSwingYAngle See SwingTwistConstraintPart
+	/// @param inMinSwingZAngle See SwingTwistConstraintPart
+	/// @param inMaxSwingZAngle See SwingTwistConstraintPart
+	/// @param inEdgeLength Size of the edge of the cone shape
+	/// @param inColor Color to use for drawing the cone.
+	/// @param inCastShadow determines if this geometry should cast a shadow or not.
+	/// @param inDrawMode determines if we draw the geometry solid or in wireframe.
+	void					DrawSwingPyramidLimits(RMat44Arg inMatrix, float inMinSwingYAngle, float inMaxSwingYAngle, float inMinSwingZAngle, float inMaxSwingZAngle, float inEdgeLength, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid);
+
+	/// Draw a pie (part of a circle).
+	/// @param inCenter The center of the circle.
+	/// @param inRadius Radius of the circle.
+	/// @param inNormal The plane normal in which the pie resides.
+	/// @param inAxis The axis that defines an angle of 0 radians.
+	/// @param inMinAngle The pie will be drawn between [inMinAngle, inMaxAngle] (in radians).
+	/// @param inMaxAngle The pie will be drawn between [inMinAngle, inMaxAngle] (in radians).
+	/// @param inColor Color to use for drawing the pie.
+	/// @param inCastShadow determines if this geometry should cast a shadow or not.
+	/// @param inDrawMode determines if we draw the geometry solid or in wireframe.
+	void					DrawPie(RVec3Arg inCenter, float inRadius, Vec3Arg inNormal, Vec3Arg inAxis, float inMinAngle, float inMaxAngle, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid);
+
+	/// Draw a tapered cylinder
+	/// @param inMatrix Matrix that transforms the cylinder to world space.
+	/// @param inTop Top of cylinder (along Y axis)
+	/// @param inBottom Bottom of cylinder (along Y axis)
+	/// @param inTopRadius Radius at the top
+	/// @param inBottomRadius Radius at the bottom
+	/// @param inColor Color to use for drawing the pie.
+	/// @param inCastShadow determines if this geometry should cast a shadow or not.
+	/// @param inDrawMode determines if we draw the geometry solid or in wireframe.
+	void					DrawTaperedCylinder(RMat44Arg inMatrix, float inTop, float inBottom, float inTopRadius, float inBottomRadius, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid);
+
+	/// Singleton instance
+	static DebugRenderer *	sInstance;
+
+	/// Vertex format used by the triangle renderer
+	class Vertex
+	{
+	public:
+		Float3				mPosition;
+		Float3				mNormal;
+		Float2				mUV;
+		Color				mColor;
+	};
+
+	/// A single triangle
+	class JPH_DEBUG_RENDERER_EXPORT Triangle
+	{
+	public:
+							Triangle() = default;
+							Triangle(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, ColorArg inColor);
+							Triangle(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, ColorArg inColor, Vec3Arg inUVOrigin, Vec3Arg inUVDirection);
+
+		Vertex				mV[3];
+	};
+
+	/// Handle for a batch of triangles
+	using Batch = Ref<RefTargetVirtual>;
+
+	/// A single level of detail
+	class LOD
+	{
+	public:
+		Batch				mTriangleBatch;
+		float				mDistance;
+	};
+
+	/// A geometry primitive containing triangle batches for various lods
+	class Geometry : public RefTarget<Geometry>
+	{
+	public:
+		JPH_OVERRIDE_NEW_DELETE
+
+		/// Constructor
+							Geometry(const AABox &inBounds) : mBounds(inBounds) { }
+							Geometry(const Batch &inBatch, const AABox &inBounds) : mBounds(inBounds) { mLODs.push_back({ inBatch, FLT_MAX }); }
+
+		/// Determine which LOD to render
+		/// @param inCameraPosition Current position of the camera
+		/// @param inWorldSpaceBounds World space bounds for this geometry (transform mBounds by model space matrix)
+		/// @param inLODScaleSq is the squared scale of the model matrix, it is multiplied with the LOD distances in inGeometry to calculate the real LOD distance (so a number > 1 will force a higher LOD).
+		/// @return The selected LOD.
+		const LOD &			GetLOD(Vec3Arg inCameraPosition, const AABox &inWorldSpaceBounds, float inLODScaleSq) const
+		{
+			float dist_sq = inWorldSpaceBounds.GetSqDistanceTo(inCameraPosition);
+			for (const LOD &lod : mLODs)
+				if (dist_sq <= inLODScaleSq * Square(lod.mDistance))
+					return lod;
+
+			return mLODs.back();
+		}
+
+		/// All level of details for this mesh
+		Array<LOD>			mLODs;
+
+		/// Bounding box that encapsulates all LODs
+		AABox				mBounds;
+	};
+
+	/// Handle for a lodded triangle batch
+	using GeometryRef = Ref<Geometry>;
+
+	/// Calculate bounding box for a batch of triangles
+	static AABox			sCalculateBounds(const Vertex *inVertices, int inVertexCount);
+
+	/// Create a batch of triangles that can be drawn efficiently
+	virtual Batch			CreateTriangleBatch(const Triangle *inTriangles, int inTriangleCount) = 0;
+	virtual Batch			CreateTriangleBatch(const Vertex *inVertices, int inVertexCount, const uint32 *inIndices, int inIndexCount) = 0;
+	Batch					CreateTriangleBatch(const Array<Triangle> &inTriangles) { return CreateTriangleBatch(inTriangles.empty()? nullptr : &inTriangles[0], int(inTriangles.size())); }
+	Batch					CreateTriangleBatch(const Array<Vertex> &inVertices, const Array<uint32> &inIndices) { return CreateTriangleBatch(inVertices.empty()? nullptr : &inVertices[0], int(inVertices.size()), inIndices.empty()? nullptr : &inIndices[0], int(inIndices.size())); }
+	Batch					CreateTriangleBatch(const VertexList &inVertices, const IndexedTriangleNoMaterialList &inTriangles);
+
+	/// Create a primitive for a convex shape using its support function
+	using SupportFunction = function<Vec3 (Vec3Arg inDirection)>;
+	Batch					CreateTriangleBatchForConvex(SupportFunction inGetSupport, int inLevel, AABox *outBounds = nullptr);
+	GeometryRef				CreateTriangleGeometryForConvex(SupportFunction inGetSupport);
+
+	/// Determines which polygons are culled
+	enum class ECullMode
+	{
+		CullBackFace,		///< Don't draw backfacing polygons
+		CullFrontFace,		///< Don't draw front facing polygons
+		Off					///< Don't do culling and draw both sides
+	};
+
+	/// Draw some geometry
+	/// @param inModelMatrix is the matrix that transforms the geometry to world space.
+	/// @param inWorldSpaceBounds is the bounding box of the geometry after transforming it into world space.
+	/// @param inLODScaleSq is the squared scale of the model matrix, it is multiplied with the LOD distances in inGeometry to calculate the real LOD distance (so a number > 1 will force a higher LOD).
+	/// @param inModelColor is the color with which to multiply the vertex colors in inGeometry.
+	/// @param inGeometry The geometry to draw.
+	/// @param inCullMode determines which polygons are culled.
+	/// @param inCastShadow determines if this geometry should cast a shadow or not.
+	/// @param inDrawMode determines if we draw the geometry solid or in wireframe.
+	virtual void			DrawGeometry(RMat44Arg inModelMatrix, const AABox &inWorldSpaceBounds, float inLODScaleSq, ColorArg inModelColor, const GeometryRef &inGeometry, ECullMode inCullMode = ECullMode::CullBackFace, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid) = 0;
+	void					DrawGeometry(RMat44Arg inModelMatrix, ColorArg inModelColor, const GeometryRef &inGeometry, ECullMode inCullMode = ECullMode::CullBackFace, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid) { DrawGeometry(inModelMatrix, inGeometry->mBounds.Transformed(inModelMatrix), max(max(inModelMatrix.GetAxisX().LengthSq(), inModelMatrix.GetAxisY().LengthSq()), inModelMatrix.GetAxisZ().LengthSq()), inModelColor, inGeometry, inCullMode, inCastShadow, inDrawMode); }
+
+	/// Draw text
+	virtual void			DrawText3D(RVec3Arg inPosition, const string_view &inString, ColorArg inColor = Color::sWhite, float inHeight = 0.5f) = 0;
+
+protected:
+	/// Initialize the system, must be called from the constructor of the DebugRenderer implementation
+	void					Initialize();
+
+private:
+	/// Recursive helper function for DrawWireUnitSphere
+	void					DrawWireUnitSphereRecursive(RMat44Arg inMatrix, ColorArg inColor, Vec3Arg inDir1, Vec3Arg inDir2, Vec3Arg inDir3, int inLevel);
+
+	/// Helper functions to create a box
+	void					CreateQuad(Array<uint32> &ioIndices, Array<Vertex> &ioVertices, Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, Vec3Arg inV4);
+
+	/// Helper functions to create a vertex and index buffer for a sphere
+	void					Create8thSphereRecursive(Array<uint32> &ioIndices, Array<Vertex> &ioVertices, Vec3Arg inDir1, uint32 &ioIdx1, Vec3Arg inDir2, uint32 &ioIdx2, Vec3Arg inDir3, uint32 &ioIdx3, const Float2 &inUV, SupportFunction inGetSupport, int inLevel);
+	void					Create8thSphere(Array<uint32> &ioIndices, Array<Vertex> &ioVertices, Vec3Arg inDir1, Vec3Arg inDir2, Vec3Arg inDir3, const Float2 &inUV, SupportFunction inGetSupport, int inLevel);
+
+	/// Helper functions to create a vertex and index buffer for a cylinder
+	Batch					CreateCylinder(float inTop, float inBottom, float inTopRadius, float inBottomRadius, int inLevel);
+
+	/// Helper function for DrawSwingConeLimits and DrawSwingPyramidLimits
+	Geometry *				CreateSwingLimitGeometry(int inNumSegments, const Vec3 *inVertices);
+
+	// Predefined shapes
+	GeometryRef				mBox;
+	GeometryRef				mSphere;
+	GeometryRef				mCapsuleTop;
+	GeometryRef				mCapsuleMid;
+	GeometryRef				mCapsuleBottom;
+	GeometryRef				mOpenCone;
+	GeometryRef				mCylinder;
+
+	struct SwingConeLimits
+	{
+		bool				operator == (const SwingConeLimits &inRHS) const
+		{
+			return mSwingYHalfAngle == inRHS.mSwingYHalfAngle
+				&& mSwingZHalfAngle == inRHS.mSwingZHalfAngle;
+		}
+
+		float				mSwingYHalfAngle;
+		float				mSwingZHalfAngle;
+	};
+
+	JPH_MAKE_HASH_STRUCT(SwingConeLimits, SwingConeLimitsHasher, t.mSwingYHalfAngle, t.mSwingZHalfAngle)
+
+	using SwingConeBatches = UnorderedMap<SwingConeLimits, GeometryRef, SwingConeLimitsHasher>;
+	SwingConeBatches		mSwingConeLimits;
+	SwingConeBatches		mPrevSwingConeLimits;
+
+	struct SwingPyramidLimits
+	{
+		bool				operator == (const SwingPyramidLimits &inRHS) const
+		{
+			return mMinSwingYAngle == inRHS.mMinSwingYAngle
+				&& mMaxSwingYAngle == inRHS.mMaxSwingYAngle
+				&& mMinSwingZAngle == inRHS.mMinSwingZAngle
+				&& mMaxSwingZAngle == inRHS.mMaxSwingZAngle;
+		}
+
+		float				mMinSwingYAngle;
+		float				mMaxSwingYAngle;
+		float				mMinSwingZAngle;
+		float				mMaxSwingZAngle;
+	};
+
+	JPH_MAKE_HASH_STRUCT(SwingPyramidLimits, SwingPyramidLimitsHasher, t.mMinSwingYAngle, t.mMaxSwingYAngle, t.mMinSwingZAngle, t.mMaxSwingZAngle)
+
+	using SwingPyramidBatches = UnorderedMap<SwingPyramidLimits, GeometryRef, SwingPyramidLimitsHasher>;
+	SwingPyramidBatches		mSwingPyramidLimits;
+	SwingPyramidBatches		mPrevSwingPyramidLimits;
+
+	using PieBatces = UnorderedMap<float, GeometryRef>;
+	PieBatces				mPieLimits;
+	PieBatces				mPrevPieLimits;
+
+	struct TaperedCylinder
+	{
+		bool				operator == (const TaperedCylinder &inRHS) const
+		{
+			return mTop == inRHS.mTop
+				&& mBottom == inRHS.mBottom
+				&& mTopRadius == inRHS.mTopRadius
+				&& mBottomRadius == inRHS.mBottomRadius;
+		}
+
+		float				mTop;
+		float				mBottom;
+		float				mTopRadius;
+		float				mBottomRadius;
+	};
+
+	JPH_MAKE_HASH_STRUCT(TaperedCylinder, TaperedCylinderHasher, t.mTop, t.mBottom, t.mTopRadius, t.mBottomRadius)
+
+	using TaperedCylinderBatces = UnorderedMap<TaperedCylinder, GeometryRef, TaperedCylinderHasher>;
+	TaperedCylinderBatces	mTaperedCylinders;
+	TaperedCylinderBatces	mPrevTaperedCylinders;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererPlayback.cpp b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererPlayback.cpp
new file mode 100644
index 0000000000..bea7e4e08f
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererPlayback.cpp
@@ -0,0 +1,168 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#ifdef JPH_DEBUG_RENDERER
+
+#include <Jolt/Renderer/DebugRendererPlayback.h>
+
+JPH_NAMESPACE_BEGIN
+
+void DebugRendererPlayback::Parse(StreamIn &inStream)
+{
+	using ECommand = DebugRendererRecorder::ECommand;
+
+	for (;;)
+	{
+		// Read the next command
+		ECommand command;
+		inStream.Read(command);
+
+		if (inStream.IsEOF() || inStream.IsFailed())
+			return;
+
+		if (command == ECommand::CreateBatch)
+		{
+			uint32 id;
+			inStream.Read(id);
+
+			uint32 triangle_count;
+			inStream.Read(triangle_count);
+
+			DebugRenderer::Triangle *triangles = new DebugRenderer::Triangle [triangle_count];
+			inStream.ReadBytes(triangles, triangle_count * sizeof(DebugRenderer::Triangle));
+
+			mBatches.insert({ id, mRenderer.CreateTriangleBatch(triangles, triangle_count) });
+
+			delete [] triangles;
+		}
+		else if (command == ECommand::CreateBatchIndexed)
+		{
+			uint32 id;
+			inStream.Read(id);
+
+			uint32 vertex_count;
+			inStream.Read(vertex_count);
+
+			DebugRenderer::Vertex *vertices = new DebugRenderer::Vertex [vertex_count];
+			inStream.ReadBytes(vertices, vertex_count * sizeof(DebugRenderer::Vertex));
+
+			uint32 index_count;
+			inStream.Read(index_count);
+
+			uint32 *indices = new uint32 [index_count];
+			inStream.ReadBytes(indices, index_count * sizeof(uint32));
+
+			mBatches.insert({ id, mRenderer.CreateTriangleBatch(vertices, vertex_count, indices, index_count) });
+
+			delete [] indices;
+			delete [] vertices;
+		}
+		else if (command == ECommand::CreateGeometry)
+		{
+			uint32 geometry_id;
+			inStream.Read(geometry_id);
+
+			AABox bounds;
+			inStream.Read(bounds.mMin);
+			inStream.Read(bounds.mMax);
+
+			DebugRenderer::GeometryRef geometry = new DebugRenderer::Geometry(bounds);
+			mGeometries[geometry_id] = geometry;
+
+			uint32 num_lods;
+			inStream.Read(num_lods);
+			for (uint32 l = 0; l < num_lods; ++l)
+			{
+				DebugRenderer::LOD lod;
+				inStream.Read(lod.mDistance);
+
+				uint32 batch_id;
+				inStream.Read(batch_id);
+				lod.mTriangleBatch = mBatches.find(batch_id)->second;
+
+				geometry->mLODs.push_back(lod);
+			}
+		}
+		else if (command == ECommand::EndFrame)
+		{
+			mFrames.push_back({});
+			Frame &frame = mFrames.back();
+
+			// Read all lines
+			uint32 num_lines = 0;
+			inStream.Read(num_lines);
+			frame.mLines.resize(num_lines);
+			for (DebugRendererRecorder::LineBlob &line : frame.mLines)
+			{
+				inStream.Read(line.mFrom);
+				inStream.Read(line.mTo);
+				inStream.Read(line.mColor);
+			}
+
+			// Read all triangles
+			uint32 num_triangles = 0;
+			inStream.Read(num_triangles);
+			frame.mTriangles.resize(num_triangles);
+			for (DebugRendererRecorder::TriangleBlob &triangle : frame.mTriangles)
+			{
+				inStream.Read(triangle.mV1);
+				inStream.Read(triangle.mV2);
+				inStream.Read(triangle.mV3);
+				inStream.Read(triangle.mColor);
+				inStream.Read(triangle.mCastShadow);
+			}
+
+			// Read all texts
+			uint32 num_texts = 0;
+			inStream.Read(num_texts);
+			frame.mTexts.resize(num_texts);
+			for (DebugRendererRecorder::TextBlob &text : frame.mTexts)
+			{
+				inStream.Read(text.mPosition);
+				inStream.Read(text.mString);
+				inStream.Read(text.mColor);
+				inStream.Read(text.mHeight);
+			}
+
+			// Read all geometries
+			uint32 num_geometries = 0;
+			inStream.Read(num_geometries);
+			frame.mGeometries.resize(num_geometries);
+			for (DebugRendererRecorder::GeometryBlob &geom : frame.mGeometries)
+			{
+				inStream.Read(geom.mModelMatrix);
+				inStream.Read(geom.mModelColor);
+				inStream.Read(geom.mGeometryID);
+				inStream.Read(geom.mCullMode);
+				inStream.Read(geom.mCastShadow);
+				inStream.Read(geom.mDrawMode);
+			}
+		}
+		else
+			JPH_ASSERT(false);
+	}
+}
+
+void DebugRendererPlayback::DrawFrame(uint inFrameNumber) const
+{
+	const Frame &frame = mFrames[inFrameNumber];
+
+	for (const DebugRendererRecorder::LineBlob &line : frame.mLines)
+		mRenderer.DrawLine(line.mFrom, line.mTo, line.mColor);
+
+	for (const DebugRendererRecorder::TriangleBlob &triangle : frame.mTriangles)
+		mRenderer.DrawTriangle(triangle.mV1, triangle.mV2, triangle.mV3, triangle.mColor, triangle.mCastShadow);
+
+	for (const DebugRendererRecorder::TextBlob &text : frame.mTexts)
+		mRenderer.DrawText3D(text.mPosition, text.mString, text.mColor, text.mHeight);
+
+	for (const DebugRendererRecorder::GeometryBlob &geom : frame.mGeometries)
+		mRenderer.DrawGeometry(geom.mModelMatrix, geom.mModelColor, mGeometries.find(geom.mGeometryID)->second, geom.mCullMode, geom.mCastShadow, geom.mDrawMode);
+}
+
+JPH_NAMESPACE_END
+
+#endif // JPH_DEBUG_RENDERER
diff --git a/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererPlayback.h b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererPlayback.h
new file mode 100644
index 0000000000..23ed454238
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererPlayback.h
@@ -0,0 +1,48 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#ifndef JPH_DEBUG_RENDERER
+	#error This file should only be included when JPH_DEBUG_RENDERER is defined
+#endif // !JPH_DEBUG_RENDERER
+
+#include <Jolt/Renderer/DebugRendererRecorder.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/UnorderedMap.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that can read a recorded stream from DebugRendererRecorder and plays it back trough a DebugRenderer
+class JPH_DEBUG_RENDERER_EXPORT DebugRendererPlayback
+{
+public:
+	/// Constructor
+										DebugRendererPlayback(DebugRenderer &inRenderer) : mRenderer(inRenderer) { }
+
+	/// Parse a stream of frames
+	void								Parse(StreamIn &inStream);
+
+	/// Get the number of parsed frames
+	uint								GetNumFrames() const				{ return (uint)mFrames.size(); }
+
+	/// Draw a frame
+	void								DrawFrame(uint inFrameNumber) const;
+
+private:
+	/// The debug renderer we're using to do the actual rendering
+	DebugRenderer &						mRenderer;
+
+	/// Mapping of ID to batch
+	UnorderedMap<uint32, DebugRenderer::Batch> mBatches;
+
+	/// Mapping of ID to geometry
+	UnorderedMap<uint32, DebugRenderer::GeometryRef> mGeometries;
+
+	/// The list of parsed frames
+	using Frame = DebugRendererRecorder::Frame;
+	Array<Frame>						mFrames;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererRecorder.cpp b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererRecorder.cpp
new file mode 100644
index 0000000000..2e25912957
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererRecorder.cpp
@@ -0,0 +1,158 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#ifdef JPH_DEBUG_RENDERER
+
+#include <Jolt/Renderer/DebugRendererRecorder.h>
+
+JPH_NAMESPACE_BEGIN
+
+void DebugRendererRecorder::DrawLine(RVec3Arg inFrom, RVec3Arg inTo, ColorArg inColor)
+{
+	lock_guard lock(mMutex);
+
+	mCurrentFrame.mLines.push_back({ inFrom, inTo, inColor });
+}
+
+void DebugRendererRecorder::DrawTriangle(RVec3Arg inV1, RVec3Arg inV2, RVec3Arg inV3, ColorArg inColor, ECastShadow inCastShadow)
+{
+	lock_guard lock(mMutex);
+
+	mCurrentFrame.mTriangles.push_back({ inV1, inV2, inV3, inColor, inCastShadow });
+}
+
+DebugRenderer::Batch DebugRendererRecorder::CreateTriangleBatch(const Triangle *inTriangles, int inTriangleCount)
+{
+	if (inTriangles == nullptr || inTriangleCount == 0)
+		return new BatchImpl(0);
+
+	lock_guard lock(mMutex);
+
+	mStream.Write(ECommand::CreateBatch);
+
+	uint32 batch_id = mNextBatchID++;
+	JPH_ASSERT(batch_id != 0);
+	mStream.Write(batch_id);
+	mStream.Write((uint32)inTriangleCount);
+	mStream.WriteBytes(inTriangles, inTriangleCount * sizeof(Triangle));
+
+	return new BatchImpl(batch_id);
+}
+
+DebugRenderer::Batch DebugRendererRecorder::CreateTriangleBatch(const Vertex *inVertices, int inVertexCount, const uint32 *inIndices, int inIndexCount)
+{
+	if (inVertices == nullptr || inVertexCount == 0 || inIndices == nullptr || inIndexCount == 0)
+		return new BatchImpl(0);
+
+	lock_guard lock(mMutex);
+
+	mStream.Write(ECommand::CreateBatchIndexed);
+
+	uint32 batch_id = mNextBatchID++;
+	JPH_ASSERT(batch_id != 0);
+	mStream.Write(batch_id);
+	mStream.Write((uint32)inVertexCount);
+	mStream.WriteBytes(inVertices, inVertexCount * sizeof(Vertex));
+	mStream.Write((uint32)inIndexCount);
+	mStream.WriteBytes(inIndices, inIndexCount * sizeof(uint32));
+
+	return new BatchImpl(batch_id);
+}
+
+void DebugRendererRecorder::DrawGeometry(RMat44Arg inModelMatrix, const AABox &inWorldSpaceBounds, float inLODScaleSq, ColorArg inModelColor, const GeometryRef &inGeometry, ECullMode inCullMode, ECastShadow inCastShadow, EDrawMode inDrawMode)
+{
+	lock_guard lock(mMutex);
+
+	// See if this geometry was used before
+	uint32 &geometry_id = mGeometries[inGeometry];
+	if (geometry_id == 0)
+	{
+		mStream.Write(ECommand::CreateGeometry);
+
+		// Create a new ID
+		geometry_id = mNextGeometryID++;
+		JPH_ASSERT(geometry_id != 0);
+		mStream.Write(geometry_id);
+
+		// Save bounds
+		mStream.Write(inGeometry->mBounds.mMin);
+		mStream.Write(inGeometry->mBounds.mMax);
+
+		// Save the LODs
+		mStream.Write((uint32)inGeometry->mLODs.size());
+		for (const LOD & lod : inGeometry->mLODs)
+		{
+			mStream.Write(lod.mDistance);
+			mStream.Write(static_cast<const BatchImpl *>(lod.mTriangleBatch.GetPtr())->mID);
+		}
+	}
+
+	mCurrentFrame.mGeometries.push_back({ inModelMatrix, inModelColor, geometry_id, inCullMode, inCastShadow, inDrawMode });
+}
+
+void DebugRendererRecorder::DrawText3D(RVec3Arg inPosition, const string_view &inString, ColorArg inColor, float inHeight)
+{
+	lock_guard lock(mMutex);
+
+	mCurrentFrame.mTexts.push_back({ inPosition, inString, inColor, inHeight });
+}
+
+void DebugRendererRecorder::EndFrame()
+{
+	lock_guard lock(mMutex);
+
+	mStream.Write(ECommand::EndFrame);
+
+	// Write all lines
+	mStream.Write((uint32)mCurrentFrame.mLines.size());
+	for (const LineBlob &line : mCurrentFrame.mLines)
+	{
+		mStream.Write(line.mFrom);
+		mStream.Write(line.mTo);
+		mStream.Write(line.mColor);
+	}
+	mCurrentFrame.mLines.clear();
+
+	// Write all triangles
+	mStream.Write((uint32)mCurrentFrame.mTriangles.size());
+	for (const TriangleBlob &triangle : mCurrentFrame.mTriangles)
+	{
+		mStream.Write(triangle.mV1);
+		mStream.Write(triangle.mV2);
+		mStream.Write(triangle.mV3);
+		mStream.Write(triangle.mColor);
+		mStream.Write(triangle.mCastShadow);
+	}
+	mCurrentFrame.mTriangles.clear();
+
+	// Write all texts
+	mStream.Write((uint32)mCurrentFrame.mTexts.size());
+	for (const TextBlob &text : mCurrentFrame.mTexts)
+	{
+		mStream.Write(text.mPosition);
+		mStream.Write(text.mString);
+		mStream.Write(text.mColor);
+		mStream.Write(text.mHeight);
+	}
+	mCurrentFrame.mTexts.clear();
+
+	// Write all geometries
+	mStream.Write((uint32)mCurrentFrame.mGeometries.size());
+	for (const GeometryBlob &geom : mCurrentFrame.mGeometries)
+	{
+		mStream.Write(geom.mModelMatrix);
+		mStream.Write(geom.mModelColor);
+		mStream.Write(geom.mGeometryID);
+		mStream.Write(geom.mCullMode);
+		mStream.Write(geom.mCastShadow);
+		mStream.Write(geom.mDrawMode);
+	}
+	mCurrentFrame.mGeometries.clear();
+}
+
+JPH_NAMESPACE_END
+
+#endif // JPH_DEBUG_RENDERER
diff --git a/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererRecorder.h b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererRecorder.h
new file mode 100644
index 0000000000..9608e03c90
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererRecorder.h
@@ -0,0 +1,130 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#ifndef JPH_DEBUG_RENDERER
+	#error This file should only be included when JPH_DEBUG_RENDERER is defined
+#endif // !JPH_DEBUG_RENDERER
+
+#include <Jolt/Renderer/DebugRenderer.h>
+#include <Jolt/Core/StreamOut.h>
+#include <Jolt/Core/Mutex.h>
+#include <Jolt/Core/UnorderedMap.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Implementation of DebugRenderer that records the API invocations to be played back later
+class JPH_DEBUG_RENDERER_EXPORT DebugRendererRecorder final : public DebugRenderer
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+										DebugRendererRecorder(StreamOut &inStream) : mStream(inStream) { Initialize(); }
+
+	/// Implementation of DebugRenderer interface
+	virtual void						DrawLine(RVec3Arg inFrom, RVec3Arg inTo, ColorArg inColor) override;
+	virtual void						DrawTriangle(RVec3Arg inV1, RVec3Arg inV2, RVec3Arg inV3, ColorArg inColor, ECastShadow inCastShadow) override;
+	virtual Batch						CreateTriangleBatch(const Triangle *inTriangles, int inTriangleCount) override;
+	virtual Batch						CreateTriangleBatch(const Vertex *inVertices, int inVertexCount, const uint32 *inIndices, int inIndexCount) override;
+	virtual void						DrawGeometry(RMat44Arg inModelMatrix, const AABox &inWorldSpaceBounds, float inLODScaleSq, ColorArg inModelColor, const GeometryRef &inGeometry, ECullMode inCullMode, ECastShadow inCastShadow, EDrawMode inDrawMode) override;
+	virtual void						DrawText3D(RVec3Arg inPosition, const string_view &inString, ColorArg inColor, float inHeight) override;
+
+	/// Mark the end of a frame
+	void								EndFrame();
+
+	/// Control commands written into the stream
+	enum class ECommand : uint8
+	{
+		CreateBatch,
+		CreateBatchIndexed,
+		CreateGeometry,
+		EndFrame
+	};
+
+	/// Holds a single line segment
+	struct LineBlob
+	{
+		RVec3							mFrom;
+		RVec3							mTo;
+		Color							mColor;
+	};
+
+	/// Holds a single triangle
+	struct TriangleBlob
+	{
+		RVec3							mV1;
+		RVec3							mV2;
+		RVec3							mV3;
+		Color							mColor;
+		ECastShadow						mCastShadow;
+	};
+
+	/// Holds a single text entry
+	struct TextBlob
+	{
+										TextBlob() = default;
+										TextBlob(RVec3Arg inPosition, const string_view &inString, ColorArg inColor, float inHeight) : mPosition(inPosition), mString(inString), mColor(inColor), mHeight(inHeight) { }
+
+		RVec3							mPosition;
+		String							mString;
+		Color							mColor;
+		float							mHeight;
+	};
+
+	/// Holds a single geometry draw call
+	struct GeometryBlob
+	{
+		RMat44							mModelMatrix;
+		Color							mModelColor;
+		uint32							mGeometryID;
+		ECullMode						mCullMode;
+		ECastShadow						mCastShadow;
+		EDrawMode						mDrawMode;
+	};
+
+	/// All information for a single frame
+	struct Frame
+	{
+		Array<LineBlob>					mLines;
+		Array<TriangleBlob>				mTriangles;
+		Array<TextBlob>					mTexts;
+		Array<GeometryBlob>				mGeometries;
+	};
+
+private:
+	/// Implementation specific batch object
+	class BatchImpl : public RefTargetVirtual
+	{
+	public:
+		JPH_OVERRIDE_NEW_DELETE
+
+										BatchImpl(uint32 inID)		: mID(inID) {  }
+
+		virtual void					AddRef() override			{ ++mRefCount; }
+		virtual void					Release() override			{ if (--mRefCount == 0) delete this; }
+
+		atomic<uint32>					mRefCount = 0;
+		uint32							mID;
+	};
+
+	/// Lock that prevents concurrent access to the internal structures
+	Mutex								mMutex;
+
+	/// Stream that recorded data will be sent to
+	StreamOut &							mStream;
+
+	/// Next available ID
+	uint32								mNextBatchID = 1;
+	uint32								mNextGeometryID = 1;
+
+	/// Cached geometries and their IDs
+	UnorderedMap<GeometryRef, uint32>	mGeometries;
+
+	/// Data that is being accumulated for the current frame
+	Frame								mCurrentFrame;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererSimple.cpp b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererSimple.cpp
new file mode 100644
index 0000000000..a404d95a00
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererSimple.cpp
@@ -0,0 +1,80 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#ifdef JPH_DEBUG_RENDERER
+
+#include <Jolt/Renderer/DebugRendererSimple.h>
+
+JPH_NAMESPACE_BEGIN
+
+DebugRendererSimple::DebugRendererSimple()
+{
+	Initialize();
+}
+
+DebugRenderer::Batch DebugRendererSimple::CreateTriangleBatch(const Triangle *inTriangles, int inTriangleCount)
+{
+	BatchImpl *batch = new BatchImpl;
+	if (inTriangles == nullptr || inTriangleCount == 0)
+		return batch;
+
+	batch->mTriangles.assign(inTriangles, inTriangles + inTriangleCount);
+	return batch;
+}
+
+DebugRenderer::Batch DebugRendererSimple::CreateTriangleBatch(const Vertex *inVertices, int inVertexCount, const uint32 *inIndices, int inIndexCount)
+{
+	BatchImpl *batch = new BatchImpl;
+	if (inVertices == nullptr || inVertexCount == 0 || inIndices == nullptr || inIndexCount == 0)
+		return batch;
+
+	// Convert indexed triangle list to triangle list
+	batch->mTriangles.resize(inIndexCount / 3);
+	for (size_t t = 0; t < batch->mTriangles.size(); ++t)
+	{
+		Triangle &triangle = batch->mTriangles[t];
+		triangle.mV[0] = inVertices[inIndices[t * 3 + 0]];
+		triangle.mV[1] = inVertices[inIndices[t * 3 + 1]];
+		triangle.mV[2] = inVertices[inIndices[t * 3 + 2]];
+	}
+
+	return batch;
+}
+
+void DebugRendererSimple::DrawGeometry(RMat44Arg inModelMatrix, const AABox &inWorldSpaceBounds, float inLODScaleSq, ColorArg inModelColor, const GeometryRef &inGeometry, ECullMode inCullMode, ECastShadow inCastShadow, EDrawMode inDrawMode)
+{
+	// Figure out which LOD to use
+	const LOD *lod = inGeometry->mLODs.data();
+	if (mCameraPosSet)
+		lod = &inGeometry->GetLOD(Vec3(mCameraPos), inWorldSpaceBounds, inLODScaleSq);
+
+	// Draw the batch
+	const BatchImpl *batch = static_cast<const BatchImpl *>(lod->mTriangleBatch.GetPtr());
+	for (const Triangle &triangle : batch->mTriangles)
+	{
+		RVec3 v0 = inModelMatrix * Vec3(triangle.mV[0].mPosition);
+		RVec3 v1 = inModelMatrix * Vec3(triangle.mV[1].mPosition);
+		RVec3 v2 = inModelMatrix * Vec3(triangle.mV[2].mPosition);
+		Color color = inModelColor * triangle.mV[0].mColor;
+
+		switch (inDrawMode)
+		{
+		case EDrawMode::Wireframe:
+			DrawLine(v0, v1, color);
+			DrawLine(v1, v2, color);
+			DrawLine(v2, v0, color);
+			break;
+
+		case EDrawMode::Solid:
+			DrawTriangle(v0, v1, v2, color, inCastShadow);
+			break;
+		}
+	}
+}
+
+JPH_NAMESPACE_END
+
+#endif // JPH_DEBUG_RENDERER
diff --git a/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererSimple.h b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererSimple.h
new file mode 100644
index 0000000000..4a23ab758b
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Renderer/DebugRendererSimple.h
@@ -0,0 +1,88 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#ifndef JPH_DEBUG_RENDERER
+	#error This file should only be included when JPH_DEBUG_RENDERER is defined
+#endif // !JPH_DEBUG_RENDERER
+
+#include <Jolt/Renderer/DebugRenderer.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Inherit from this class to simplify implementing a debug renderer, start with this implementation:
+///
+///		class MyDebugRenderer : public JPH::DebugRendererSimple
+///		{
+///		public:
+///			virtual void DrawLine(JPH::RVec3Arg inFrom, JPH::RVec3Arg inTo, JPH::ColorArg inColor) override
+///			{
+///				// Implement
+///			}
+///
+///			virtual void DrawTriangle(JPH::RVec3Arg inV1, JPH::RVec3Arg inV2, JPH::RVec3Arg inV3, JPH::ColorArg inColor, ECastShadow inCastShadow) override
+///			{
+///				// Implement
+///			}
+///
+///			virtual void DrawText3D(JPH::RVec3Arg inPosition, const string_view &inString, JPH::ColorArg inColor, float inHeight) override
+///			{
+///				// Implement
+///			}
+///		};
+///
+/// Note that this class is meant to be a quick start for implementing a debug renderer, it is not the most efficient way to implement a debug renderer.
+class JPH_DEBUG_RENDERER_EXPORT DebugRendererSimple : public DebugRenderer
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+								DebugRendererSimple();
+
+	/// Should be called every frame by the application to provide the camera position.
+	/// This is used to determine the correct LOD for rendering.
+	void						SetCameraPos(RVec3Arg inCameraPos)
+	{
+		mCameraPos = inCameraPos;
+		mCameraPosSet = true;
+	}
+
+	/// Fallback implementation that uses DrawLine to draw a triangle (override this if you have a version that renders solid triangles)
+	virtual void				DrawTriangle(RVec3Arg inV1, RVec3Arg inV2, RVec3Arg inV3, ColorArg inColor, ECastShadow inCastShadow) override
+	{
+		DrawLine(inV1, inV2, inColor);
+		DrawLine(inV2, inV3, inColor);
+		DrawLine(inV3, inV1, inColor);
+	}
+
+protected:
+	/// Implementation of DebugRenderer interface
+	virtual Batch				CreateTriangleBatch(const Triangle *inTriangles, int inTriangleCount) override;
+	virtual Batch				CreateTriangleBatch(const Vertex *inVertices, int inVertexCount, const uint32 *inIndices, int inIndexCount) override;
+	virtual void				DrawGeometry(RMat44Arg inModelMatrix, const AABox &inWorldSpaceBounds, float inLODScaleSq, ColorArg inModelColor, const GeometryRef &inGeometry, ECullMode inCullMode, ECastShadow inCastShadow, EDrawMode inDrawMode) override;
+
+private:
+	/// Implementation specific batch object
+	class BatchImpl : public RefTargetVirtual
+	{
+	public:
+		JPH_OVERRIDE_NEW_DELETE
+
+		virtual void			AddRef() override			{ ++mRefCount; }
+		virtual void			Release() override			{ if (--mRefCount == 0) delete this; }
+
+		Array<Triangle>			mTriangles;
+
+	private:
+		atomic<uint32>			mRefCount = 0;
+	};
+
+	/// Last provided camera position
+	RVec3						mCameraPos;
+	bool						mCameraPosSet = false;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Skeleton/SkeletalAnimation.cpp b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletalAnimation.cpp
new file mode 100644
index 0000000000..4bf87007e9
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletalAnimation.cpp
@@ -0,0 +1,110 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Skeleton/SkeletalAnimation.h>
+#include <Jolt/Skeleton/SkeletonPose.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SkeletalAnimation::JointState)
+{
+	JPH_ADD_ATTRIBUTE(JointState, mRotation)
+	JPH_ADD_ATTRIBUTE(JointState, mTranslation)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SkeletalAnimation::Keyframe)
+{
+	JPH_ADD_BASE_CLASS(Keyframe, JointState)
+
+	JPH_ADD_ATTRIBUTE(Keyframe, mTime)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SkeletalAnimation::AnimatedJoint)
+{
+	JPH_ADD_ATTRIBUTE(AnimatedJoint, mJointName)
+	JPH_ADD_ATTRIBUTE(AnimatedJoint, mKeyframes)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SkeletalAnimation)
+{
+	JPH_ADD_ATTRIBUTE(SkeletalAnimation, mAnimatedJoints)
+	JPH_ADD_ATTRIBUTE(SkeletalAnimation, mIsLooping)
+}
+
+
+void SkeletalAnimation::JointState::FromMatrix(Mat44Arg inMatrix)
+{
+	mRotation = inMatrix.GetQuaternion();
+	mTranslation = inMatrix.GetTranslation();
+}
+
+float SkeletalAnimation::GetDuration() const
+{
+	if (!mAnimatedJoints.empty() && !mAnimatedJoints[0].mKeyframes.empty())
+		return mAnimatedJoints[0].mKeyframes.back().mTime;
+	else
+		return 0.0f;
+}
+
+void SkeletalAnimation::ScaleJoints(float inScale)
+{
+	for (SkeletalAnimation::AnimatedJoint &j : mAnimatedJoints)
+		for (SkeletalAnimation::Keyframe &k : j.mKeyframes)
+			k.mTranslation *= inScale;
+}
+
+void SkeletalAnimation::Sample(float inTime, SkeletonPose &ioPose) const
+{
+	// Correct time when animation is looping
+	JPH_ASSERT(inTime >= 0.0f);
+	float duration = GetDuration();
+	float time = duration > 0.0f && mIsLooping? fmod(inTime, duration) : inTime;
+
+	for (const AnimatedJoint &aj : mAnimatedJoints)
+	{
+		// Do binary search for keyframe
+		int high = (int)aj.mKeyframes.size(), low = -1;
+		while (high - low > 1)
+		{
+			int probe = (high + low) / 2;
+			if (aj.mKeyframes[probe].mTime < time)
+				low = probe;
+			else
+				high = probe;
+		}
+
+		JointState &state = ioPose.GetJoint(ioPose.GetSkeleton()->GetJointIndex(aj.mJointName));
+
+		if (low == -1)
+		{
+			// Before first key, return first key
+			state = static_cast<const JointState &>(aj.mKeyframes.front());
+		}
+		else if (high == (int)aj.mKeyframes.size())
+		{
+			// Beyond last key, return last key
+			state = static_cast<const JointState &>(aj.mKeyframes.back());
+		}
+		else
+		{
+			// Interpolate
+			const Keyframe &s1 = aj.mKeyframes[low];
+			const Keyframe &s2 = aj.mKeyframes[low + 1];
+
+			float fraction = (time - s1.mTime) / (s2.mTime - s1.mTime);
+			JPH_ASSERT(fraction >= 0.0f && fraction <= 1.0f);
+
+			state.mTranslation = (1.0f - fraction) * s1.mTranslation + fraction * s2.mTranslation;
+			JPH_ASSERT(s1.mRotation.IsNormalized());
+			JPH_ASSERT(s2.mRotation.IsNormalized());
+			state.mRotation = s1.mRotation.SLERP(s2.mRotation, fraction);
+			JPH_ASSERT(state.mRotation.IsNormalized());
+		}
+	}
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Skeleton/SkeletalAnimation.h b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletalAnimation.h
new file mode 100644
index 0000000000..344c6046fa
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletalAnimation.h
@@ -0,0 +1,77 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/Reference.h>
+#include <Jolt/ObjectStream/SerializableObject.h>
+
+JPH_NAMESPACE_BEGIN
+
+class SkeletonPose;
+
+/// Resource for a skinned animation
+class JPH_EXPORT SkeletalAnimation : public RefTarget<SkeletalAnimation>
+{
+	JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SkeletalAnimation)
+
+public:
+	/// Contains the current state of a joint, a local space transformation relative to its parent joint
+	class JointState
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, JointState)
+
+	public:
+		/// Convert from a local space matrix
+		void							FromMatrix(Mat44Arg inMatrix);
+
+		/// Convert to matrix representation
+		inline Mat44					ToMatrix() const									{ return Mat44::sRotationTranslation(mRotation, mTranslation); }
+
+		Quat							mRotation = Quat::sIdentity();						///< Local space rotation of the joint
+		Vec3							mTranslation = Vec3::sZero();						///< Local space translation of the joint
+	};
+
+	/// Contains the state of a single joint at a particular time
+	class Keyframe : public JointState
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Keyframe)
+
+	public:
+		float							mTime = 0.0f;										///< Time of keyframe in seconds
+	};
+
+	using KeyframeVector = Array<Keyframe>;
+
+	/// Contains the animation for a single joint
+	class AnimatedJoint
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, AnimatedJoint)
+
+	public:
+		String							mJointName;											///< Name of the joint
+		KeyframeVector					mKeyframes;											///< List of keyframes over time
+	};
+
+	using AnimatedJointVector = Array<AnimatedJoint>;
+
+	/// Get the length (in seconds) of this animation
+	float								GetDuration() const;
+
+	/// Scale the size of all joints by inScale
+	void								ScaleJoints(float inScale);
+
+	/// Get the (interpolated) joint transforms at time inTime
+	void								Sample(float inTime, SkeletonPose &ioPose) const;
+
+	/// Get joint samples
+	const AnimatedJointVector &			GetAnimatedJoints() const							{ return mAnimatedJoints; }
+	AnimatedJointVector &				GetAnimatedJoints()									{ return mAnimatedJoints; }
+
+private:
+	AnimatedJointVector					mAnimatedJoints;									///< List of joints and keyframes
+	bool								mIsLooping = true;									///< If this animation loops back to start
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Skeleton/Skeleton.cpp b/thirdparty/jolt_physics/Jolt/Skeleton/Skeleton.cpp
new file mode 100644
index 0000000000..5ab7f56e79
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Skeleton/Skeleton.cpp
@@ -0,0 +1,82 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Skeleton/Skeleton.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(Skeleton::Joint)
+{
+	JPH_ADD_ATTRIBUTE(Joint, mName)
+	JPH_ADD_ATTRIBUTE(Joint, mParentName)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(Skeleton)
+{
+	JPH_ADD_ATTRIBUTE(Skeleton, mJoints)
+}
+
+int Skeleton::GetJointIndex(const string_view &inName) const
+{
+	for (int i = 0; i < (int)mJoints.size(); ++i)
+		if (mJoints[i].mName == inName)
+			return i;
+
+	return -1;
+}
+
+void Skeleton::CalculateParentJointIndices()
+{
+	for (Joint &j : mJoints)
+		j.mParentJointIndex = GetJointIndex(j.mParentName);
+}
+
+bool Skeleton::AreJointsCorrectlyOrdered() const
+{
+	for (int i = 0; i < (int)mJoints.size(); ++i)
+		if (mJoints[i].mParentJointIndex >= i)
+			return false;
+
+	return true;
+}
+
+void Skeleton::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write((uint32)mJoints.size());
+	for (const Joint &j : mJoints)
+	{
+		inStream.Write(j.mName);
+		inStream.Write(j.mParentJointIndex);
+		inStream.Write(j.mParentName);
+	}
+}
+
+Skeleton::SkeletonResult Skeleton::sRestoreFromBinaryState(StreamIn &inStream)
+{
+	Ref<Skeleton> skeleton = new Skeleton;
+
+	uint32 len = 0;
+	inStream.Read(len);
+	skeleton->mJoints.resize(len);
+	for (Joint &j : skeleton->mJoints)
+	{
+		inStream.Read(j.mName);
+		inStream.Read(j.mParentJointIndex);
+		inStream.Read(j.mParentName);
+	}
+
+	SkeletonResult result;
+	if (inStream.IsEOF() || inStream.IsFailed())
+		result.SetError("Failed to read skeleton from stream");
+	else
+		result.Set(skeleton);
+	return result;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Skeleton/Skeleton.h b/thirdparty/jolt_physics/Jolt/Skeleton/Skeleton.h
new file mode 100644
index 0000000000..f53df31dfd
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Skeleton/Skeleton.h
@@ -0,0 +1,72 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/Reference.h>
+#include <Jolt/Core/Result.h>
+#include <Jolt/ObjectStream/SerializableObject.h>
+
+JPH_NAMESPACE_BEGIN
+
+class StreamIn;
+class StreamOut;
+
+/// Resource that contains the joint hierarchy for a skeleton
+class JPH_EXPORT Skeleton : public RefTarget<Skeleton>
+{
+	JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Skeleton)
+
+public:
+	using SkeletonResult = Result<Ref<Skeleton>>;
+
+	/// Declare internal structure for a joint
+	class Joint
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Joint)
+
+	public:
+							Joint() = default;
+							Joint(const string_view &inName, const string_view &inParentName, int inParentJointIndex) : mName(inName), mParentName(inParentName), mParentJointIndex(inParentJointIndex) { }
+
+		String				mName;																		///< Name of the joint
+		String				mParentName;																///< Name of parent joint
+		int					mParentJointIndex = -1;														///< Index of parent joint (in mJoints) or -1 if it has no parent
+	};
+
+	using JointVector = Array<Joint>;
+
+	///@name Access to the joints
+	///@{
+	const JointVector &		GetJoints() const															{ return mJoints; }
+	JointVector &			GetJoints()																	{ return mJoints; }
+	int						GetJointCount() const														{ return (int)mJoints.size(); }
+	const Joint &			GetJoint(int inJoint) const													{ return mJoints[inJoint]; }
+	Joint &					GetJoint(int inJoint)														{ return mJoints[inJoint]; }
+	uint					AddJoint(const string_view &inName, const string_view &inParentName = string_view()) { mJoints.emplace_back(inName, inParentName, -1); return (uint)mJoints.size() - 1; }
+	uint					AddJoint(const string_view &inName, int inParentIndex)						{ mJoints.emplace_back(inName, inParentIndex >= 0? mJoints[inParentIndex].mName : String(), inParentIndex); return (uint)mJoints.size() - 1; }
+	///@}
+
+	/// Find joint by name
+	int						GetJointIndex(const string_view &inName) const;
+
+	/// Fill in parent joint indices based on name
+	void					CalculateParentJointIndices();
+
+	/// Many of the algorithms that use the Skeleton class require that parent joints are in the mJoints array before their children.
+	/// This function returns true if this is the case, false if not.
+	bool					AreJointsCorrectlyOrdered() const;
+
+	/// Saves the state of this object in binary form to inStream.
+	void					SaveBinaryState(StreamOut &inStream) const;
+
+	/// Restore the state of this object from inStream.
+	static SkeletonResult	sRestoreFromBinaryState(StreamIn &inStream);
+
+private:
+	/// Joints
+	JointVector				mJoints;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonMapper.cpp b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonMapper.cpp
new file mode 100644
index 0000000000..add2956d54
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonMapper.cpp
@@ -0,0 +1,237 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Skeleton/SkeletonMapper.h>
+
+JPH_NAMESPACE_BEGIN
+
+void SkeletonMapper::Initialize(const Skeleton *inSkeleton1, const Mat44 *inNeutralPose1, const Skeleton *inSkeleton2, const Mat44 *inNeutralPose2, const CanMapJoint &inCanMapJoint)
+{
+	JPH_ASSERT(mMappings.empty() && mChains.empty() && mUnmapped.empty()); // Should not be initialized yet
+
+	// Count joints
+	int n1 = inSkeleton1->GetJointCount();
+	int n2 = inSkeleton2->GetJointCount();
+	JPH_ASSERT(n1 <= n2, "Skeleton 1 should be the low detail skeleton!");
+
+	// Keep track of mapped joints (initialize to false)
+	Array<bool> mapped1(n1, false);
+	Array<bool> mapped2(n2, false);
+
+	// Find joints that can be mapped directly
+	for (int j1 = 0; j1 < n1; ++j1)
+		for (int j2 = 0; j2 < n2; ++j2)
+			if (inCanMapJoint(inSkeleton1, j1, inSkeleton2, j2))
+			{
+				// Calculate the transform that takes this joint from skeleton 1 to 2
+				Mat44 joint_1_to_2 = inNeutralPose1[j1].Inversed() * inNeutralPose2[j2];
+
+				// Ensure bottom right element is 1 (numerical imprecision in the inverse can make this not so)
+				joint_1_to_2(3, 3) = 1.0f;
+
+				mMappings.emplace_back(j1, j2, joint_1_to_2);
+				mapped1[j1] = true;
+				mapped2[j2] = true;
+				break;
+			}
+
+	Array<int> cur_chain; // Taken out of the loop to minimize amount of allocations
+
+	// Find joint chains
+	for (int m1 = 0; m1 < (int)mMappings.size(); ++m1)
+	{
+		Array<int> chain2;
+		int chain2_m = -1;
+
+		for (int m2 = m1 + 1; m2 < (int)mMappings.size(); ++m2)
+		{
+			// Find the chain from back from m2 to m1
+			int start = mMappings[m1].mJointIdx2;
+			int end = mMappings[m2].mJointIdx2;
+			int cur = end;
+			cur_chain.clear(); // Should preserve memory
+			do
+			{
+				cur_chain.push_back(cur);
+				cur = inSkeleton2->GetJoint(cur).mParentJointIndex;
+			}
+			while (cur >= 0 && cur != start && !mapped2[cur]);
+			cur_chain.push_back(start);
+
+			if (cur == start // This should be the correct chain
+				&& cur_chain.size() > 2 // It should have joints between the mapped joints
+				&& cur_chain.size() > chain2.size()) // And it should be the longest so far
+			{
+				chain2.swap(cur_chain);
+				chain2_m = m2;
+			}
+		}
+
+		if (!chain2.empty())
+		{
+			// Get the chain for 1
+			Array<int> chain1;
+			int start = mMappings[m1].mJointIdx1;
+			int cur = mMappings[chain2_m].mJointIdx1;
+			do
+			{
+				chain1.push_back(cur);
+				cur = inSkeleton1->GetJoint(cur).mParentJointIndex;
+			}
+			while (cur >= 0 && cur != start && !mapped1[cur]);
+			chain1.push_back(start);
+
+			// If the chain exists in 1 too
+			if (cur == start)
+			{
+				// Reverse the chains
+				std::reverse(chain1.begin(), chain1.end());
+				std::reverse(chain2.begin(), chain2.end());
+
+				// Mark elements mapped
+				for (int j1 : chain1)
+					mapped1[j1] = true;
+				for (int j2 : chain2)
+					mapped2[j2] = true;
+
+				// Insert the chain
+				mChains.emplace_back(std::move(chain1), std::move(chain2));
+			}
+		}
+	}
+
+	// Collect unmapped joints from 2
+	for (int j2 = 0; j2 < n2; ++j2)
+		if (!mapped2[j2])
+			mUnmapped.emplace_back(j2, inSkeleton2->GetJoint(j2).mParentJointIndex);
+}
+
+void SkeletonMapper::LockTranslations(const Skeleton *inSkeleton2, const bool *inLockedTranslations, const Mat44 *inNeutralPose2)
+{
+	JPH_ASSERT(inSkeleton2->AreJointsCorrectlyOrdered());
+
+	int n = inSkeleton2->GetJointCount();
+
+	// Copy locked joints to array but don't actually include the first joint (this is physics driven)
+	for (int i = 0; i < n; ++i)
+		if (inLockedTranslations[i])
+		{
+			Locked l;
+			l.mJointIdx = i;
+			l.mParentJointIdx = inSkeleton2->GetJoint(i).mParentJointIndex;
+			if (l.mParentJointIdx >= 0)
+				l.mTranslation = inNeutralPose2[l.mParentJointIdx].Inversed() * inNeutralPose2[i].GetTranslation();
+			else
+				l.mTranslation = inNeutralPose2[i].GetTranslation();
+			mLockedTranslations.push_back(l);
+		}
+}
+
+void SkeletonMapper::LockAllTranslations(const Skeleton *inSkeleton2, const Mat44 *inNeutralPose2)
+{
+	JPH_ASSERT(!mMappings.empty(), "Call Initialize first!");
+	JPH_ASSERT(inSkeleton2->AreJointsCorrectlyOrdered());
+
+	// The first mapping is the top most one (remember that joints should be ordered so that parents go before children).
+	// Because we created the mappings from the lowest joint first, this should contain the first mappable joint.
+	int root_idx = mMappings[0].mJointIdx2;
+
+	// Create temp array to hold locked joints
+	int n = inSkeleton2->GetJointCount();
+	bool *locked_translations = (bool *)JPH_STACK_ALLOC(n * sizeof(bool));
+	memset(locked_translations, 0, n * sizeof(bool));
+
+	// Mark root as locked
+	locked_translations[root_idx] = true;
+
+	// Loop over all joints and propagate the locked flag to all children
+	for (int i = root_idx + 1; i < n; ++i)
+	{
+		int parent_idx = inSkeleton2->GetJoint(i).mParentJointIndex;
+		if (parent_idx >= 0)
+			locked_translations[i] = locked_translations[parent_idx];
+	}
+
+	// Unmark root because we don't actually want to include this (this determines the position of the entire ragdoll)
+	locked_translations[root_idx] = false;
+
+	// Call the generic function
+	LockTranslations(inSkeleton2, locked_translations, inNeutralPose2);
+}
+
+void SkeletonMapper::Map(const Mat44 *inPose1ModelSpace, const Mat44 *inPose2LocalSpace, Mat44 *outPose2ModelSpace) const
+{
+	// Apply direct mappings
+	for (const Mapping &m : mMappings)
+		outPose2ModelSpace[m.mJointIdx2] = inPose1ModelSpace[m.mJointIdx1] * m.mJoint1To2;
+
+	// Apply chain mappings
+	for (const Chain &c : mChains)
+	{
+		// Calculate end of chain given local space transforms of the joints of the chain
+		Mat44 &chain_start = outPose2ModelSpace[c.mJointIndices2.front()];
+		Mat44 chain_end = chain_start;
+		for (int j = 1; j < (int)c.mJointIndices2.size(); ++j)
+			chain_end = chain_end * inPose2LocalSpace[c.mJointIndices2[j]];
+
+		// Calculate the direction in world space for skeleton 1 and skeleton 2 and the rotation between them
+		Vec3 actual = chain_end.GetTranslation() - chain_start.GetTranslation();
+		Vec3 desired = inPose1ModelSpace[c.mJointIndices1.back()].GetTranslation() - inPose1ModelSpace[c.mJointIndices1.front()].GetTranslation();
+		Quat rotation = Quat::sFromTo(actual, desired);
+
+		// Rotate the start of the chain
+		chain_start.SetRotation(Mat44::sRotation(rotation) * chain_start.GetRotation());
+
+		// Update all joints but the first and the last joint using their local space transforms
+		for (int j = 1; j < (int)c.mJointIndices2.size() - 1; ++j)
+		{
+			int parent = c.mJointIndices2[j - 1];
+			int child = c.mJointIndices2[j];
+			outPose2ModelSpace[child] = outPose2ModelSpace[parent] * inPose2LocalSpace[child];
+		}
+	}
+
+	// All unmapped joints take the local pose and convert it to model space
+	for (const Unmapped &u : mUnmapped)
+		if (u.mParentJointIdx >= 0)
+		{
+			JPH_ASSERT(u.mParentJointIdx < u.mJointIdx, "Joints must be ordered: parents first");
+			outPose2ModelSpace[u.mJointIdx] = outPose2ModelSpace[u.mParentJointIdx] * inPose2LocalSpace[u.mJointIdx];
+		}
+		else
+			outPose2ModelSpace[u.mJointIdx] = inPose2LocalSpace[u.mJointIdx];
+
+	// Update all locked joint translations
+	for (const Locked &l : mLockedTranslations)
+		outPose2ModelSpace[l.mJointIdx].SetTranslation(outPose2ModelSpace[l.mParentJointIdx] * l.mTranslation);
+}
+
+void SkeletonMapper::MapReverse(const Mat44 *inPose2ModelSpace, Mat44 *outPose1ModelSpace) const
+{
+	// Normally each joint in skeleton 1 should be present in the mapping, so we only need to apply the direct mappings
+	for (const Mapping &m : mMappings)
+		outPose1ModelSpace[m.mJointIdx1] = inPose2ModelSpace[m.mJointIdx2] * m.mJoint2To1;
+}
+
+int SkeletonMapper::GetMappedJointIdx(int inJoint1Idx) const
+{
+	for (const Mapping &m : mMappings)
+		if (m.mJointIdx1 == inJoint1Idx)
+			return m.mJointIdx2;
+
+	return -1;
+}
+
+bool SkeletonMapper::IsJointTranslationLocked(int inJoint2Idx) const
+{
+	for (const Locked &l : mLockedTranslations)
+		if (l.mJointIdx == inJoint2Idx)
+			return true;
+
+	return false;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonMapper.h b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonMapper.h
new file mode 100644
index 0000000000..05cc8866a4
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonMapper.h
@@ -0,0 +1,145 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/Reference.h>
+#include <Jolt/Skeleton/Skeleton.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that is able to map a low detail (ragdoll) skeleton to a high detail (animation) skeleton and vice versa
+class JPH_EXPORT SkeletonMapper : public RefTarget<SkeletonMapper>
+{
+public:
+	/// A joint that maps 1-on-1 to a joint in the other skeleton
+	class Mapping
+	{
+	public:
+							Mapping() = default;
+							Mapping(int inJointIdx1, int inJointIdx2, Mat44Arg inJoint1To2) : mJointIdx1(inJointIdx1), mJointIdx2(inJointIdx2), mJoint1To2(inJoint1To2), mJoint2To1(inJoint1To2.Inversed())
+		{
+			// Ensure bottom right element is 1 (numerical imprecision in the inverse can make this not so)
+			mJoint2To1(3, 3) = 1.0f;
+		}
+
+		int					mJointIdx1;																	///< Index of joint from skeleton 1
+		int					mJointIdx2;																	///< Corresponding index of joint from skeleton 2
+		Mat44				mJoint1To2;																	///< Transforms this joint from skeleton 1 to 2
+		Mat44				mJoint2To1;																	///< Inverse of the transform above
+	};
+
+	/// A joint chain that starts with a 1-on-1 mapped joint and ends with a 1-on-1 mapped joint with intermediate joints that cannot be mapped
+	class Chain
+	{
+	public:
+							Chain() = default;
+							Chain(Array<int> &&inJointIndices1, Array<int> &&inJointIndices2) : mJointIndices1(std::move(inJointIndices1)), mJointIndices2(std::move(inJointIndices2)) { }
+
+		Array<int>			mJointIndices1;																///< Joint chain from skeleton 1
+		Array<int>			mJointIndices2;																///< Corresponding joint chain from skeleton 2
+	};
+
+	/// Joints that could not be mapped from skeleton 1 to 2
+	class Unmapped
+	{
+	public:
+							Unmapped() = default;
+							Unmapped(int inJointIdx, int inParentJointIdx) : mJointIdx(inJointIdx), mParentJointIdx(inParentJointIdx) { }
+
+		int					mJointIdx;																	///< Joint index of unmappable joint
+		int					mParentJointIdx;															///< Parent joint index of unmappable joint
+	};
+
+	/// Joints that should have their translation locked (fixed)
+	class Locked
+	{
+	public:
+		int					mJointIdx;																	///< Joint index of joint with locked translation (in skeleton 2)
+		int					mParentJointIdx;															///< Parent joint index of joint with locked translation (in skeleton 2)
+		Vec3				mTranslation;																///< Translation of neutral pose
+	};
+
+	/// A function that is called to determine if a joint can be mapped from source to target skeleton
+	using CanMapJoint = function<bool (const Skeleton *, int, const Skeleton *, int)>;
+
+	/// Default function that checks if the names of the joints are equal
+	static bool				sDefaultCanMapJoint(const Skeleton *inSkeleton1, int inIndex1, const Skeleton *inSkeleton2, int inIndex2)
+	{
+		return inSkeleton1->GetJoint(inIndex1).mName == inSkeleton2->GetJoint(inIndex2).mName;
+	}
+
+	/// Initialize the skeleton mapper. Skeleton 1 should be the (low detail) ragdoll skeleton and skeleton 2 the (high detail) animation skeleton.
+	/// We assume that each joint in skeleton 1 can be mapped to a joint in skeleton 2 (if not mapping from animation skeleton to ragdoll skeleton will be undefined).
+	/// Skeleton 2 should have the same hierarchy as skeleton 1 but can contain extra joints between those in skeleton 1 and it can have extra joints at the root and leaves of the skeleton.
+	/// @param inSkeleton1 Source skeleton to map from.
+	/// @param inNeutralPose1 Neutral pose of the source skeleton (model space)
+	/// @param inSkeleton2 Target skeleton to map to.
+	/// @param inNeutralPose2 Neutral pose of the target skeleton (model space), inNeutralPose1 and inNeutralPose2 must match as closely as possible, preferably the position of the mappable joints should be identical.
+	/// @param inCanMapJoint Function that checks if joints in skeleton 1 and skeleton 2 are equal.
+	void					Initialize(const Skeleton *inSkeleton1, const Mat44 *inNeutralPose1, const Skeleton *inSkeleton2, const Mat44 *inNeutralPose2, const CanMapJoint &inCanMapJoint = sDefaultCanMapJoint);
+
+	/// This can be called so lock the translation of a specified set of joints in skeleton 2.
+	/// Because constraints are never 100% rigid, there's always a little bit of stretch in the ragdoll when the ragdoll is under stress.
+	/// Locking the translations of the pose will remove the visual stretch from the ragdoll but will introduce a difference between the
+	/// physical simulation and the visual representation.
+	/// @param inSkeleton2 Target skeleton to map to.
+	/// @param inLockedTranslations An array of bools the size of inSkeleton2->GetJointCount(), for each joint indicating if the joint is locked.
+	/// @param inNeutralPose2 Neutral pose to take reference translations from
+	void					LockTranslations(const Skeleton *inSkeleton2, const bool *inLockedTranslations, const Mat44 *inNeutralPose2);
+
+	/// After Initialize(), this can be called to lock the translation of all joints in skeleton 2 below the first mapped joint to those of the neutral pose.
+	/// Because constraints are never 100% rigid, there's always a little bit of stretch in the ragdoll when the ragdoll is under stress.
+	/// Locking the translations of the pose will remove the visual stretch from the ragdoll but will introduce a difference between the
+	/// physical simulation and the visual representation.
+	/// @param inSkeleton2 Target skeleton to map to.
+	/// @param inNeutralPose2 Neutral pose to take reference translations from
+	void					LockAllTranslations(const Skeleton *inSkeleton2, const Mat44 *inNeutralPose2);
+
+	/// Map a pose. Joints that were directly mappable will be copied in model space from pose 1 to pose 2. Any joints that are only present in skeleton 2
+	/// will get their model space transform calculated through the local space transforms of pose 2. Joints that are part of a joint chain between two
+	/// mapped joints will be reoriented towards the next joint in skeleton 1. This means that it is possible for unmapped joints to have some animation,
+	/// but very extreme animation poses will show artifacts.
+	/// @param inPose1ModelSpace Pose on skeleton 1 in model space
+	/// @param inPose2LocalSpace Pose on skeleton 2 in local space (used for the joints that cannot be mapped)
+	/// @param outPose2ModelSpace Model space pose on skeleton 2 (the output of the mapping)
+	void					Map(const Mat44 *inPose1ModelSpace, const Mat44 *inPose2LocalSpace, Mat44 *outPose2ModelSpace) const;
+
+	/// Reverse map a pose, this will only use the mappings and not the chains (it assumes that all joints in skeleton 1 are mapped)
+	/// @param inPose2ModelSpace Model space pose on skeleton 2
+	/// @param outPose1ModelSpace When the function returns this will contain the model space pose for skeleton 1
+	void					MapReverse(const Mat44 *inPose2ModelSpace, Mat44 *outPose1ModelSpace) const;
+
+	/// Search through the directly mapped joints (mMappings) and find inJoint1Idx, returns the corresponding Joint2Idx or -1 if not found.
+	int						GetMappedJointIdx(int inJoint1Idx) const;
+
+	/// Search through the locked translations (mLockedTranslations) and find if joint inJoint2Idx is locked.
+	bool					IsJointTranslationLocked(int inJoint2Idx) const;
+
+	using MappingVector = Array<Mapping>;
+	using ChainVector = Array<Chain>;
+	using UnmappedVector = Array<Unmapped>;
+	using LockedVector = Array<Locked>;
+
+	///@name Access to the mapped joints
+	///@{
+	const MappingVector &	GetMappings() const															{ return mMappings; }
+	MappingVector &			GetMappings()																{ return mMappings; }
+	const ChainVector &		GetChains() const															{ return mChains; }
+	ChainVector &			GetChains()																	{ return mChains; }
+	const UnmappedVector &	GetUnmapped() const															{ return mUnmapped; }
+	UnmappedVector &		GetUnmapped()																{ return mUnmapped; }
+	const LockedVector &	GetLockedTranslations() const												{ return mLockedTranslations; }
+	LockedVector &			GetLockedTranslations()														{ return mLockedTranslations; }
+	///@}
+
+private:
+	/// Joint mappings
+	MappingVector			mMappings;
+	ChainVector				mChains;
+	UnmappedVector			mUnmapped;																	///< Joint indices that could not be mapped from 1 to 2 (these are indices in 2)
+	LockedVector			mLockedTranslations;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonPose.cpp b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonPose.cpp
new file mode 100644
index 0000000000..c64bf04971
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonPose.cpp
@@ -0,0 +1,87 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Skeleton/SkeletonPose.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+void SkeletonPose::SetSkeleton(const Skeleton *inSkeleton)
+{
+	mSkeleton = inSkeleton;
+
+	mJoints.resize(mSkeleton->GetJointCount());
+	mJointMatrices.resize(mSkeleton->GetJointCount());
+}
+
+void SkeletonPose::CalculateJointMatrices()
+{
+	for (int i = 0; i < (int)mJoints.size(); ++i)
+	{
+		mJointMatrices[i] = mJoints[i].ToMatrix();
+
+		int parent = mSkeleton->GetJoint(i).mParentJointIndex;
+		if (parent >= 0)
+		{
+			JPH_ASSERT(parent < i, "Joints must be ordered: parents first");
+			mJointMatrices[i] = mJointMatrices[parent] * mJointMatrices[i];
+		}
+	}
+}
+
+void SkeletonPose::CalculateJointStates()
+{
+	for (int i = 0; i < (int)mJoints.size(); ++i)
+	{
+		Mat44 local_transform;
+		int parent = mSkeleton->GetJoint(i).mParentJointIndex;
+		if (parent >= 0)
+			local_transform = mJointMatrices[parent].Inversed() * mJointMatrices[i];
+		else
+			local_transform = mJointMatrices[i];
+
+		JointState &joint = mJoints[i];
+		joint.mTranslation = local_transform.GetTranslation();
+		joint.mRotation = local_transform.GetQuaternion();
+	}
+}
+
+void SkeletonPose::CalculateLocalSpaceJointMatrices(Mat44 *outMatrices) const
+{
+	for (int i = 0; i < (int)mJoints.size(); ++i)
+		outMatrices[i] = mJoints[i].ToMatrix();
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void SkeletonPose::Draw(const DrawSettings &inDrawSettings, DebugRenderer *inRenderer, RMat44Arg inOffset) const
+{
+	RMat44 offset = inOffset * RMat44::sTranslation(mRootOffset);
+
+	const Skeleton::JointVector &joints = mSkeleton->GetJoints();
+
+	for (int b = 0; b < mSkeleton->GetJointCount(); ++b)
+	{
+		RMat44 joint_transform = offset * mJointMatrices[b];
+
+		if (inDrawSettings.mDrawJoints)
+		{
+			int parent = joints[b].mParentJointIndex;
+			if (parent >= 0)
+				inRenderer->DrawLine(offset * mJointMatrices[parent].GetTranslation(), joint_transform.GetTranslation(), Color::sGreen);
+		}
+
+		if (inDrawSettings.mDrawJointOrientations)
+			inRenderer->DrawCoordinateSystem(joint_transform, 0.05f);
+
+		if (inDrawSettings.mDrawJointNames)
+			inRenderer->DrawText3D(joint_transform.GetTranslation(), joints[b].mName, Color::sWhite, 0.05f);
+	}
+}
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonPose.h b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonPose.h
new file mode 100644
index 0000000000..326227e447
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonPose.h
@@ -0,0 +1,82 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Skeleton/Skeleton.h>
+#include <Jolt/Skeleton/SkeletalAnimation.h>
+
+JPH_NAMESPACE_BEGIN
+
+#ifdef JPH_DEBUG_RENDERER
+class DebugRenderer;
+#endif // JPH_DEBUG_RENDERER
+
+/// Instance of a skeleton, contains the pose the current skeleton is in
+class JPH_EXPORT SkeletonPose
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	using JointState = SkeletalAnimation::JointState;
+	using JointStateVector = Array<JointState>;
+	using Mat44Vector = Array<Mat44>;
+
+	///@name Skeleton
+	///@{
+	void						SetSkeleton(const Skeleton *inSkeleton);
+	const Skeleton *			GetSkeleton() const														{ return mSkeleton; }
+	///@}
+
+	/// Extra offset applied to the root (and therefore also to all of its children)
+	void						SetRootOffset(RVec3Arg inOffset)										{ mRootOffset = inOffset; }
+	RVec3						GetRootOffset() const													{ return mRootOffset; }
+
+	///@name Properties of the joints
+	///@{
+	uint						GetJointCount() const													{ return (uint)mJoints.size(); }
+	const JointStateVector &	GetJoints() const														{ return mJoints; }
+	JointStateVector &			GetJoints()																{ return mJoints; }
+	const JointState &			GetJoint(int inJoint) const												{ return mJoints[inJoint]; }
+	JointState &				GetJoint(int inJoint)													{ return mJoints[inJoint]; }
+	///@}
+
+	///@name Joint matrices
+	///@{
+	const Mat44Vector &			GetJointMatrices() const												{ return mJointMatrices; }
+	Mat44Vector &				GetJointMatrices()														{ return mJointMatrices; }
+	const Mat44 &				GetJointMatrix(int inJoint) const										{ return mJointMatrices[inJoint]; }
+	Mat44 &						GetJointMatrix(int inJoint)												{ return mJointMatrices[inJoint]; }
+	///@}
+
+	/// Convert the joint states to joint matrices
+	void						CalculateJointMatrices();
+
+	/// Convert joint matrices to joint states
+	void						CalculateJointStates();
+
+	/// Outputs the joint matrices in local space (ensure that outMatrices has GetJointCount() elements, assumes that values in GetJoints() is up to date)
+	void						CalculateLocalSpaceJointMatrices(Mat44 *outMatrices) const;
+
+#ifdef JPH_DEBUG_RENDERER
+	/// Draw settings
+	struct DrawSettings
+	{
+		bool					mDrawJoints = true;
+		bool					mDrawJointOrientations = true;
+		bool					mDrawJointNames = false;
+	};
+
+	/// Draw current pose
+	void						Draw(const DrawSettings &inDrawSettings, DebugRenderer *inRenderer, RMat44Arg inOffset = RMat44::sIdentity()) const;
+#endif // JPH_DEBUG_RENDERER
+
+private:
+	RefConst<Skeleton>			mSkeleton;																///< Skeleton definition
+	RVec3						mRootOffset { RVec3::sZero() };											///< Extra offset applied to the root (and therefore also to all of its children)
+	JointStateVector			mJoints;																///< Local joint orientations (local to parent Joint)
+	Mat44Vector					mJointMatrices;															///< Local joint matrices (local to world matrix)
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouper.h b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouper.h
new file mode 100644
index 0000000000..9d75691f68
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouper.h
@@ -0,0 +1,27 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/IndexedTriangle.h>
+#include <Jolt/Core/NonCopyable.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// A class that groups triangles in batches of N (according to closeness)
+class JPH_EXPORT TriangleGrouper : public NonCopyable
+{
+public:
+	/// Virtual destructor
+	virtual					~TriangleGrouper() = default;
+
+	/// Group a batch of indexed triangles
+	/// @param inVertices The list of vertices
+	/// @param inTriangles The list of indexed triangles (indexes into inVertices)
+	/// @param inGroupSize How big each group should be
+	/// @param outGroupedTriangleIndices An ordered list of indices (indexing into inTriangles), contains groups of inGroupSize large worth of indices to triangles that are grouped together. If the triangle count is not an exact multiple of inGroupSize the last batch will be smaller.
+	virtual void			Group(const VertexList &inVertices, const IndexedTriangleList &inTriangles, int inGroupSize, Array<uint> &outGroupedTriangleIndices) = 0;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.cpp b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.cpp
new file mode 100644
index 0000000000..800160818e
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.cpp
@@ -0,0 +1,95 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.h>
+#include <Jolt/Geometry/MortonCode.h>
+#include <Jolt/Core/QuickSort.h>
+
+JPH_NAMESPACE_BEGIN
+
+void TriangleGrouperClosestCentroid::Group(const VertexList &inVertices, const IndexedTriangleList &inTriangles, int inGroupSize, Array<uint> &outGroupedTriangleIndices)
+{
+	const uint triangle_count = (uint)inTriangles.size();
+	const uint num_batches = (triangle_count + inGroupSize - 1) / inGroupSize;
+
+	Array<Vec3> centroids;
+	centroids.resize(triangle_count);
+
+	outGroupedTriangleIndices.resize(triangle_count);
+
+	for (uint t = 0; t < triangle_count; ++t)
+	{
+		// Store centroid
+		centroids[t] = inTriangles[t].GetCentroid(inVertices);
+
+		// Initialize sort table
+		outGroupedTriangleIndices[t] = t;
+	}
+
+	Array<uint>::const_iterator triangles_end = outGroupedTriangleIndices.end();
+
+	// Sort per batch
+	for (uint b = 0; b < num_batches - 1; ++b)
+	{
+		// Get iterators
+		Array<uint>::iterator batch_begin = outGroupedTriangleIndices.begin() + b * inGroupSize;
+		Array<uint>::iterator batch_end = batch_begin + inGroupSize;
+		Array<uint>::iterator batch_begin_plus_1 = batch_begin + 1;
+		Array<uint>::iterator batch_end_minus_1 = batch_end - 1;
+
+		// Find triangle with centroid with lowest X coordinate
+		Array<uint>::iterator lowest_iter = batch_begin;
+		float lowest_val = centroids[*lowest_iter].GetX();
+		for (Array<uint>::iterator other = batch_begin; other != triangles_end; ++other)
+		{
+			float val = centroids[*other].GetX();
+			if (val < lowest_val)
+			{
+				lowest_iter = other;
+				lowest_val = val;
+			}
+		}
+
+		// Make this triangle the first in a new batch
+		std::swap(*batch_begin, *lowest_iter);
+		Vec3 first_centroid = centroids[*batch_begin];
+
+		// Sort remaining triangles in batch on distance to first triangle
+		QuickSort(batch_begin_plus_1, batch_end,
+			[&first_centroid, &centroids](uint inLHS, uint inRHS)
+			{
+				return (centroids[inLHS] - first_centroid).LengthSq() < (centroids[inRHS] - first_centroid).LengthSq();
+			});
+
+		// Loop over remaining triangles
+		float furthest_dist = (centroids[*batch_end_minus_1] - first_centroid).LengthSq();
+		for (Array<uint>::iterator other = batch_end; other != triangles_end; ++other)
+		{
+			// Check if this triangle is closer than the furthest triangle in the batch
+			float dist = (centroids[*other] - first_centroid).LengthSq();
+			if (dist < furthest_dist)
+			{
+				// Replace furthest triangle
+				uint other_val = *other;
+				*other = *batch_end_minus_1;
+
+				// Find first element that is bigger than this one and insert the current item before it
+				Array<uint>::iterator upper = std::upper_bound(batch_begin_plus_1, batch_end, dist,
+					[&first_centroid, &centroids](float inLHS, uint inRHS)
+					{
+						return inLHS < (centroids[inRHS] - first_centroid).LengthSq();
+					});
+				std::copy_backward(upper, batch_end_minus_1, batch_end);
+				*upper = other_val;
+
+				// Calculate new furthest distance
+				furthest_dist = (centroids[*batch_end_minus_1] - first_centroid).LengthSq();
+			}
+		}
+	}
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.h b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.h
new file mode 100644
index 0000000000..5832274164
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.h
@@ -0,0 +1,21 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/TriangleGrouper/TriangleGrouper.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// A class that groups triangles in batches of N.
+/// Starts with centroid with lowest X coordinate and finds N closest centroids, this repeats until all groups have been found.
+/// Time complexity: O(N^2)
+class JPH_EXPORT TriangleGrouperClosestCentroid : public TriangleGrouper
+{
+public:
+	// See: TriangleGrouper::Group
+	virtual void			Group(const VertexList &inVertices, const IndexedTriangleList &inTriangles, int inGroupSize, Array<uint> &outGroupedTriangleIndices) override;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperMorton.cpp b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperMorton.cpp
new file mode 100644
index 0000000000..a6e5a56442
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperMorton.cpp
@@ -0,0 +1,49 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/TriangleGrouper/TriangleGrouperMorton.h>
+#include <Jolt/Geometry/MortonCode.h>
+#include <Jolt/Core/QuickSort.h>
+
+JPH_NAMESPACE_BEGIN
+
+void TriangleGrouperMorton::Group(const VertexList &inVertices, const IndexedTriangleList &inTriangles, int inGroupSize, Array<uint> &outGroupedTriangleIndices)
+{
+	const uint triangle_count = (uint)inTriangles.size();
+
+	Array<Vec3> centroids;
+	centroids.resize(triangle_count);
+
+	outGroupedTriangleIndices.resize(triangle_count);
+
+	for (uint t = 0; t < triangle_count; ++t)
+	{
+		// Store centroid
+		centroids[t] = inTriangles[t].GetCentroid(inVertices);
+
+		// Initialize sort table
+		outGroupedTriangleIndices[t] = t;
+	}
+
+	// Get bounding box of all centroids
+	AABox centroid_bounds;
+	for (uint t = 0; t < triangle_count; ++t)
+		centroid_bounds.Encapsulate(centroids[t]);
+
+	// Make sure box is not degenerate
+	centroid_bounds.EnsureMinimalEdgeLength(1.0e-5f);
+
+	// Calculate morton code for each centroid
+	Array<uint32> morton_codes;
+	morton_codes.resize(triangle_count);
+	for (uint t = 0; t < triangle_count; ++t)
+		morton_codes[t] = MortonCode::sGetMortonCode(centroids[t], centroid_bounds);
+
+	// Sort triangles based on morton code
+	QuickSort(outGroupedTriangleIndices.begin(), outGroupedTriangleIndices.end(), [&morton_codes](uint inLHS, uint inRHS) { return morton_codes[inLHS] < morton_codes[inRHS]; });
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperMorton.h b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperMorton.h
new file mode 100644
index 0000000000..a35f9af004
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/TriangleGrouper/TriangleGrouperMorton.h
@@ -0,0 +1,20 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/TriangleGrouper/TriangleGrouper.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// A class that groups triangles in batches of N according to morton code of centroid.
+/// Time complexity: O(N log(N))
+class JPH_EXPORT TriangleGrouperMorton : public TriangleGrouper
+{
+public:
+	// See: TriangleGrouper::Group
+	virtual void			Group(const VertexList &inVertices, const IndexedTriangleList &inTriangles, int inGroupSize, Array<uint> &outGroupedTriangleIndices) override;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitter.cpp b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitter.cpp
new file mode 100644
index 0000000000..50d15117d3
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitter.cpp
@@ -0,0 +1,67 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/TriangleSplitter/TriangleSplitter.h>
+
+JPH_NAMESPACE_BEGIN
+
+TriangleSplitter::TriangleSplitter(const VertexList &inVertices, const IndexedTriangleList &inTriangles) :
+	mVertices(inVertices),
+	mTriangles(inTriangles)
+{
+	mSortedTriangleIdx.resize(inTriangles.size());
+	mCentroids.resize(inTriangles.size());
+
+	for (uint t = 0; t < inTriangles.size(); ++t)
+	{
+		// Initially triangles start unsorted
+		mSortedTriangleIdx[t] = t;
+
+		// Calculate centroid
+		inTriangles[t].GetCentroid(inVertices).StoreFloat3(&mCentroids[t]);
+	}
+}
+
+bool TriangleSplitter::SplitInternal(const Range &inTriangles, uint inDimension, float inSplit, Range &outLeft, Range &outRight)
+{
+	// Divide triangles
+	uint start = inTriangles.mBegin, end = inTriangles.mEnd;
+	while (start < end)
+	{
+		// Search for first element that is on the right hand side of the split plane
+		while (start < end && mCentroids[mSortedTriangleIdx[start]][inDimension] < inSplit)
+			++start;
+
+		// Search for the first element that is on the left hand side of the split plane
+		while (start < end && mCentroids[mSortedTriangleIdx[end - 1]][inDimension] >= inSplit)
+			--end;
+
+		if (start < end)
+		{
+			// Swap the two elements
+			std::swap(mSortedTriangleIdx[start], mSortedTriangleIdx[end - 1]);
+			++start;
+			--end;
+		}
+	}
+	JPH_ASSERT(start == end);
+
+#ifdef JPH_ENABLE_ASSERTS
+	// Validate division algorithm
+	JPH_ASSERT(inTriangles.mBegin <= start);
+	JPH_ASSERT(start <= inTriangles.mEnd);
+	for (uint i = inTriangles.mBegin; i < start; ++i)
+		JPH_ASSERT(mCentroids[mSortedTriangleIdx[i]][inDimension] < inSplit);
+	for (uint i = start; i < inTriangles.mEnd; ++i)
+		JPH_ASSERT(mCentroids[mSortedTriangleIdx[i]][inDimension] >= inSplit);
+#endif
+
+	outLeft = Range(inTriangles.mBegin, start);
+	outRight = Range(start, inTriangles.mEnd);
+	return outLeft.Count() > 0 && outRight.Count() > 0;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitter.h b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitter.h
new file mode 100644
index 0000000000..a66672238f
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitter.h
@@ -0,0 +1,84 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/IndexedTriangle.h>
+#include <Jolt/Core/NonCopyable.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// A class that splits a triangle list into two parts for building a tree
+class JPH_EXPORT TriangleSplitter : public NonCopyable
+{
+public:
+	/// Constructor
+								TriangleSplitter(const VertexList &inVertices, const IndexedTriangleList &inTriangles);
+
+	/// Virtual destructor
+	virtual						~TriangleSplitter() = default;
+
+	struct Stats
+	{
+		const char *			mSplitterName = nullptr;
+		int						mLeafSize = 0;
+	};
+
+	/// Get stats of splitter
+	virtual void				GetStats(Stats &outStats) const = 0;
+
+	/// Helper struct to indicate triangle range before and after the split
+	struct Range
+	{
+		/// Constructor
+								Range() = default;
+								Range(uint inBegin, uint inEnd) : mBegin(inBegin), mEnd(inEnd) { }
+
+		/// Get number of triangles in range
+		uint					Count() const
+		{
+			return mEnd - mBegin;
+		}
+
+		/// Start and end index (end = 1 beyond end)
+		uint					mBegin;
+		uint					mEnd;
+	};
+
+	/// Range of triangles to start with
+	Range						GetInitialRange() const
+	{
+		return Range(0, (uint)mSortedTriangleIdx.size());
+	}
+
+	/// Split triangles into two groups left and right, returns false if no split could be made
+	/// @param inTriangles The range of triangles (in mSortedTriangleIdx) to process
+	/// @param outLeft On return this will contain the ranges for the left subpart. mSortedTriangleIdx may have been shuffled.
+	/// @param outRight On return this will contain the ranges for the right subpart. mSortedTriangleIdx may have been shuffled.
+	/// @return Returns true when a split was found
+	virtual bool				Split(const Range &inTriangles, Range &outLeft, Range &outRight) = 0;
+
+	/// Get the list of vertices
+	const VertexList &			GetVertices() const
+	{
+		return mVertices;
+	}
+
+	/// Get triangle by index
+	const IndexedTriangle &		GetTriangle(uint inIdx) const
+	{
+		return mTriangles[mSortedTriangleIdx[inIdx]];
+	}
+
+protected:
+	/// Helper function to split triangles based on dimension and split value
+	bool						SplitInternal(const Range &inTriangles, uint inDimension, float inSplit, Range &outLeft, Range &outRight);
+
+	const VertexList &			mVertices;				///< Vertices of the indexed triangles
+	const IndexedTriangleList &	mTriangles;				///< Unsorted triangles
+	Array<Float3>				mCentroids;				///< Unsorted centroids of triangles
+	Array<uint>					mSortedTriangleIdx;		///< Indices to sort triangles
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterBinning.cpp b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterBinning.cpp
new file mode 100644
index 0000000000..cdeb79bf75
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterBinning.cpp
@@ -0,0 +1,136 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/TriangleSplitter/TriangleSplitterBinning.h>
+
+ JPH_NAMESPACE_BEGIN
+
+TriangleSplitterBinning::TriangleSplitterBinning(const VertexList &inVertices, const IndexedTriangleList &inTriangles, uint inMinNumBins, uint inMaxNumBins, uint inNumTrianglesPerBin) :
+	TriangleSplitter(inVertices, inTriangles),
+	mMinNumBins(inMinNumBins),
+	mMaxNumBins(inMaxNumBins),
+	mNumTrianglesPerBin(inNumTrianglesPerBin)
+{
+	mBins.resize(mMaxNumBins * 3); // mMaxNumBins per dimension
+}
+
+bool TriangleSplitterBinning::Split(const Range &inTriangles, Range &outLeft, Range &outRight)
+{
+	// Calculate bounds for this range
+	AABox centroid_bounds;
+	for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; ++t)
+		centroid_bounds.Encapsulate(Vec3(mCentroids[mSortedTriangleIdx[t]]));
+
+	// Convert bounds to min coordinate and size
+	// Prevent division by zero if one of the dimensions is zero
+	constexpr float cMinSize = 1.0e-5f;
+	Vec3 bounds_min = centroid_bounds.mMin;
+	Vec3 bounds_size = Vec3::sMax(centroid_bounds.mMax - bounds_min, Vec3::sReplicate(cMinSize));
+
+	float best_cp = FLT_MAX;
+	uint best_dim = 0xffffffff;
+	float best_split = 0;
+
+	// Bin in all dimensions
+	uint num_bins = Clamp(inTriangles.Count() / mNumTrianglesPerBin, mMinNumBins, mMaxNumBins);
+
+	// Initialize bins
+	for (uint dim = 0; dim < 3; ++dim)
+	{
+		// Get bounding box size for this dimension
+		float bounds_min_dim = bounds_min[dim];
+		float bounds_size_dim = bounds_size[dim];
+
+		// Get the bins for this dimension
+		Bin *bins_dim = &mBins[num_bins * dim];
+
+		for (uint b = 0; b < num_bins; ++b)
+		{
+			Bin &bin = bins_dim[b];
+			bin.mBounds.SetEmpty();
+			bin.mMinCentroid = bounds_min_dim + bounds_size_dim * (b + 1) / num_bins;
+			bin.mNumTriangles = 0;
+		}
+	}
+
+	// Bin all triangles in all dimensions at once
+	for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; ++t)
+	{
+		Vec3 centroid_pos(mCentroids[mSortedTriangleIdx[t]]);
+
+		AABox triangle_bounds = AABox::sFromTriangle(mVertices, GetTriangle(t));
+
+		Vec3 bin_no_f = (centroid_pos - bounds_min) / bounds_size * float(num_bins);
+		UVec4 bin_no = UVec4::sMin(bin_no_f.ToInt(), UVec4::sReplicate(num_bins - 1));
+
+		for (uint dim = 0; dim < 3; ++dim)
+		{
+			// Select bin
+			Bin &bin = mBins[num_bins * dim + bin_no[dim]];
+
+			// Accumulate triangle in bin
+			bin.mBounds.Encapsulate(triangle_bounds);
+			bin.mMinCentroid = min(bin.mMinCentroid, centroid_pos[dim]);
+			bin.mNumTriangles++;
+		}
+	}
+
+	for (uint dim = 0; dim < 3; ++dim)
+	{
+		// Skip axis if too small
+		if (bounds_size[dim] <= cMinSize)
+			continue;
+
+		// Get the bins for this dimension
+		Bin *bins_dim = &mBins[num_bins * dim];
+
+		// Calculate totals left to right
+		AABox prev_bounds;
+		int prev_triangles = 0;
+		for (uint b = 0; b < num_bins; ++b)
+		{
+			Bin &bin = bins_dim[b];
+			bin.mBoundsAccumulatedLeft = prev_bounds; // Don't include this node as we'll take a split on the left side of the bin
+			bin.mNumTrianglesAccumulatedLeft = prev_triangles;
+			prev_bounds.Encapsulate(bin.mBounds);
+			prev_triangles += bin.mNumTriangles;
+		}
+
+		// Calculate totals right to left
+		prev_bounds.SetEmpty();
+		prev_triangles = 0;
+		for (int b = num_bins - 1; b >= 0; --b)
+		{
+			Bin &bin = bins_dim[b];
+			prev_bounds.Encapsulate(bin.mBounds);
+			prev_triangles += bin.mNumTriangles;
+			bin.mBoundsAccumulatedRight = prev_bounds;
+			bin.mNumTrianglesAccumulatedRight = prev_triangles;
+		}
+
+		// Get best splitting plane
+		for (uint b = 1; b < num_bins; ++b) // Start at 1 since selecting bin 0 would result in everything ending up on the right side
+		{
+			// Calculate surface area heuristic and see if it is better than the current best
+			const Bin &bin = bins_dim[b];
+			float cp = bin.mBoundsAccumulatedLeft.GetSurfaceArea() * bin.mNumTrianglesAccumulatedLeft + bin.mBoundsAccumulatedRight.GetSurfaceArea() * bin.mNumTrianglesAccumulatedRight;
+			if (cp < best_cp)
+			{
+				best_cp = cp;
+				best_dim = dim;
+				best_split = bin.mMinCentroid;
+			}
+		}
+	}
+
+	// No split found?
+	if (best_dim == 0xffffffff)
+		return false;
+
+	return SplitInternal(inTriangles, best_dim, best_split, outLeft, outRight);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterBinning.h b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterBinning.h
new file mode 100644
index 0000000000..2cb35c9cc7
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterBinning.h
@@ -0,0 +1,52 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/TriangleSplitter/TriangleSplitter.h>
+#include <Jolt/Geometry/AABox.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Binning splitter approach taken from: Realtime Ray Tracing on GPU with BVH-based Packet Traversal by Johannes Gunther et al.
+class JPH_EXPORT TriangleSplitterBinning : public TriangleSplitter
+{
+public:
+	/// Constructor
+							TriangleSplitterBinning(const VertexList &inVertices, const IndexedTriangleList &inTriangles, uint inMinNumBins = 8, uint inMaxNumBins = 128, uint inNumTrianglesPerBin = 6);
+
+	// See TriangleSplitter::GetStats
+	virtual void			GetStats(Stats &outStats) const override
+	{
+		outStats.mSplitterName = "TriangleSplitterBinning";
+	}
+
+	// See TriangleSplitter::Split
+	virtual bool			Split(const Range &inTriangles, Range &outLeft, Range &outRight) override;
+
+private:
+	// Configuration
+	const uint				mMinNumBins;
+	const uint				mMaxNumBins;
+	const uint				mNumTrianglesPerBin;
+
+	struct Bin
+	{
+		// Properties of this bin
+		AABox				mBounds;
+		float				mMinCentroid;
+		uint				mNumTriangles;
+
+		// Accumulated data from left most / right most bin to current (including this bin)
+		AABox				mBoundsAccumulatedLeft;
+		AABox				mBoundsAccumulatedRight;
+		uint				mNumTrianglesAccumulatedLeft;
+		uint				mNumTrianglesAccumulatedRight;
+	};
+
+	// Scratch area to store the bins
+	Array<Bin>				mBins;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.cpp b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.cpp
new file mode 100644
index 0000000000..333def9889
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.cpp
@@ -0,0 +1,170 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.h>
+#include <Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.h>
+
+JPH_NAMESPACE_BEGIN
+
+TriangleSplitterFixedLeafSize::TriangleSplitterFixedLeafSize(const VertexList &inVertices, const IndexedTriangleList &inTriangles, uint inLeafSize, uint inMinNumBins, uint inMaxNumBins, uint inNumTrianglesPerBin) :
+	TriangleSplitter(inVertices, inTriangles),
+	mLeafSize(inLeafSize),
+	mMinNumBins(inMinNumBins),
+	mMaxNumBins(inMaxNumBins),
+	mNumTrianglesPerBin(inNumTrianglesPerBin)
+{
+	// Group the triangles
+	TriangleGrouperClosestCentroid grouper;
+	grouper.Group(inVertices, inTriangles, mLeafSize, mSortedTriangleIdx);
+
+	// Pad triangles so that we have a multiple of mLeafSize
+	const uint num_triangles = (uint)inTriangles.size();
+	const uint num_groups = (num_triangles + mLeafSize - 1) / mLeafSize;
+	const uint last_triangle_idx = mSortedTriangleIdx.back();
+	for (uint t = num_triangles, t_end = num_groups * mLeafSize; t < t_end; ++t)
+		mSortedTriangleIdx.push_back(last_triangle_idx);
+}
+
+Vec3 TriangleSplitterFixedLeafSize::GetCentroidForGroup(uint inFirstTriangleInGroup)
+{
+	JPH_ASSERT(inFirstTriangleInGroup % mLeafSize == 0);
+	AABox box;
+	for (uint g = 0; g < mLeafSize; ++g)
+		box.Encapsulate(mVertices, GetTriangle(inFirstTriangleInGroup + g));
+	return box.GetCenter();
+}
+
+bool TriangleSplitterFixedLeafSize::Split(const Range &inTriangles, Range &outLeft, Range &outRight)
+{
+	// Cannot split anything smaller than leaf size
+	JPH_ASSERT(inTriangles.Count() > mLeafSize);
+	JPH_ASSERT(inTriangles.Count() % mLeafSize == 0);
+
+	// Calculate bounds for this range
+	AABox centroid_bounds;
+	for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; t += mLeafSize)
+		centroid_bounds.Encapsulate(GetCentroidForGroup(t));
+
+	float best_cp = FLT_MAX;
+	uint best_dim = 0xffffffff;
+	float best_split = 0;
+
+	// Bin in all dimensions
+	uint num_bins = Clamp(inTriangles.Count() / mNumTrianglesPerBin, mMinNumBins, mMaxNumBins);
+	Array<Bin> bins(num_bins);
+	for (uint dim = 0; dim < 3; ++dim)
+	{
+		float bounds_min = centroid_bounds.mMin[dim];
+		float bounds_size = centroid_bounds.mMax[dim] - bounds_min;
+
+		// Skip axis if too small
+		if (bounds_size < 1.0e-5f)
+			continue;
+
+		// Initialize bins
+		for (uint b = 0; b < num_bins; ++b)
+		{
+			Bin &bin = bins[b];
+			bin.mBounds.SetEmpty();
+			bin.mMinCentroid = bounds_min + bounds_size * (b + 1) / num_bins;
+			bin.mNumTriangles = 0;
+		}
+
+		// Bin all triangles
+		for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; t += mLeafSize)
+		{
+			// Calculate average centroid for group
+			float centroid_pos = GetCentroidForGroup(t)[dim];
+
+			// Select bin
+			uint bin_no = min(uint((centroid_pos - bounds_min) / bounds_size * num_bins), num_bins - 1);
+			Bin &bin = bins[bin_no];
+
+			// Put all triangles of group in same bin
+			for (uint g = 0; g < mLeafSize; ++g)
+				bin.mBounds.Encapsulate(mVertices, GetTriangle(t + g));
+			bin.mMinCentroid = min(bin.mMinCentroid, centroid_pos);
+			bin.mNumTriangles += mLeafSize;
+		}
+
+		// Calculate totals left to right
+		AABox prev_bounds;
+		int prev_triangles = 0;
+		for (uint b = 0; b < num_bins; ++b)
+		{
+			Bin &bin = bins[b];
+			bin.mBoundsAccumulatedLeft = prev_bounds; // Don't include this node as we'll take a split on the left side of the bin
+			bin.mNumTrianglesAccumulatedLeft = prev_triangles;
+			prev_bounds.Encapsulate(bin.mBounds);
+			prev_triangles += bin.mNumTriangles;
+		}
+
+		// Calculate totals right to left
+		prev_bounds.SetEmpty();
+		prev_triangles = 0;
+		for (int b = num_bins - 1; b >= 0; --b)
+		{
+			Bin &bin = bins[b];
+			prev_bounds.Encapsulate(bin.mBounds);
+			prev_triangles += bin.mNumTriangles;
+			bin.mBoundsAccumulatedRight = prev_bounds;
+			bin.mNumTrianglesAccumulatedRight = prev_triangles;
+		}
+
+		// Get best splitting plane
+		for (uint b = 1; b < num_bins; ++b) // Start at 1 since selecting bin 0 would result in everything ending up on the right side
+		{
+			// Calculate surface area heuristic and see if it is better than the current best
+			const Bin &bin = bins[b];
+			float cp = bin.mBoundsAccumulatedLeft.GetSurfaceArea() * bin.mNumTrianglesAccumulatedLeft + bin.mBoundsAccumulatedRight.GetSurfaceArea() * bin.mNumTrianglesAccumulatedRight;
+			if (cp < best_cp)
+			{
+				best_cp = cp;
+				best_dim = dim;
+				best_split = bin.mMinCentroid;
+			}
+		}
+	}
+
+	// No split found?
+	if (best_dim == 0xffffffff)
+		return false;
+
+	// Divide triangles
+	uint start = inTriangles.mBegin, end = inTriangles.mEnd;
+	while (start < end)
+	{
+		// Search for first element that is on the right hand side of the split plane
+		while (start < end && GetCentroidForGroup(start)[best_dim] < best_split)
+			start += mLeafSize;
+
+		// Search for the first element that is on the left hand side of the split plane
+		while (start < end && GetCentroidForGroup(end - mLeafSize)[best_dim] >= best_split)
+			end -= mLeafSize;
+
+		if (start < end)
+		{
+			// Swap the two elements
+			for (uint g = 0; g < mLeafSize; ++g)
+				std::swap(mSortedTriangleIdx[start + g], mSortedTriangleIdx[end - mLeafSize + g]);
+			start += mLeafSize;
+			end -= mLeafSize;
+		}
+	}
+	JPH_ASSERT(start == end);
+
+	// No suitable split found, doing random split in half
+	if (start == inTriangles.mBegin || start == inTriangles.mEnd)
+		start = inTriangles.mBegin + (inTriangles.Count() / mLeafSize + 1) / 2 * mLeafSize;
+
+	outLeft = Range(inTriangles.mBegin, start);
+	outRight = Range(start, inTriangles.mEnd);
+	JPH_ASSERT(outLeft.mEnd > outLeft.mBegin && outRight.mEnd > outRight.mBegin);
+	JPH_ASSERT(outLeft.Count() % mLeafSize == 0 && outRight.Count() % mLeafSize == 0);
+	return true;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.h b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.h
new file mode 100644
index 0000000000..029121daea
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.h
@@ -0,0 +1,55 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/TriangleSplitter/TriangleSplitter.h>
+#include <Jolt/Geometry/AABox.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Same as TriangleSplitterBinning, but ensuring that leaves have a fixed amount of triangles
+/// The resulting tree should be suitable for processing on GPU where we want all threads to process an equal amount of triangles
+class JPH_EXPORT TriangleSplitterFixedLeafSize : public TriangleSplitter
+{
+public:
+	/// Constructor
+							TriangleSplitterFixedLeafSize(const VertexList &inVertices, const IndexedTriangleList &inTriangles, uint inLeafSize, uint inMinNumBins = 8, uint inMaxNumBins = 128, uint inNumTrianglesPerBin = 6);
+
+	// See TriangleSplitter::GetStats
+	virtual void			GetStats(Stats &outStats) const override
+	{
+		outStats.mSplitterName = "TriangleSplitterFixedLeafSize";
+		outStats.mLeafSize = mLeafSize;
+	}
+
+	// See TriangleSplitter::Split
+	virtual bool			Split(const Range &inTriangles, Range &outLeft, Range &outRight) override;
+
+private:
+	/// Get centroid for group
+	Vec3					GetCentroidForGroup(uint inFirstTriangleInGroup);
+
+	// Configuration
+	const uint				mLeafSize;
+	const uint				mMinNumBins;
+	const uint				mMaxNumBins;
+	const uint				mNumTrianglesPerBin;
+
+	struct Bin
+	{
+		// Properties of this bin
+		AABox				mBounds;
+		float				mMinCentroid;
+		uint				mNumTriangles;
+
+		// Accumulated data from left most / right most bin to current (including this bin)
+		AABox				mBoundsAccumulatedLeft;
+		AABox				mBoundsAccumulatedRight;
+		uint				mNumTrianglesAccumulatedLeft;
+		uint				mNumTrianglesAccumulatedRight;
+	};
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterLongestAxis.cpp b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterLongestAxis.cpp
new file mode 100644
index 0000000000..f8115ab899
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterLongestAxis.cpp
@@ -0,0 +1,31 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/TriangleSplitter/TriangleSplitterLongestAxis.h>
+#include <Jolt/Geometry/AABox.h>
+
+JPH_NAMESPACE_BEGIN
+
+TriangleSplitterLongestAxis::TriangleSplitterLongestAxis(const VertexList &inVertices, const IndexedTriangleList &inTriangles) :
+	TriangleSplitter(inVertices, inTriangles)
+{
+}
+
+bool TriangleSplitterLongestAxis::Split(const Range &inTriangles, Range &outLeft, Range &outRight)
+{
+	// Calculate bounding box for triangles
+	AABox bounds;
+	for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; ++t)
+		bounds.Encapsulate(mVertices, GetTriangle(t));
+
+	// Calculate split plane
+	uint dimension = bounds.GetExtent().GetHighestComponentIndex();
+	float split = bounds.GetCenter()[dimension];
+
+	return SplitInternal(inTriangles, dimension, split, outLeft, outRight);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterLongestAxis.h b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterLongestAxis.h
new file mode 100644
index 0000000000..daf0d43701
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterLongestAxis.h
@@ -0,0 +1,28 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/TriangleSplitter/TriangleSplitter.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Splitter using center of bounding box with longest axis
+class JPH_EXPORT TriangleSplitterLongestAxis : public TriangleSplitter
+{
+public:
+	/// Constructor
+							TriangleSplitterLongestAxis(const VertexList &inVertices, const IndexedTriangleList &inTriangles);
+
+	// See TriangleSplitter::GetStats
+	virtual void			GetStats(Stats &outStats) const override
+	{
+		outStats.mSplitterName = "TriangleSplitterLongestAxis";
+	}
+
+	// See TriangleSplitter::Split
+	virtual bool			Split(const Range &inTriangles, Range &outLeft, Range &outRight) override;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMean.cpp b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMean.cpp
new file mode 100644
index 0000000000..e884246fea
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMean.cpp
@@ -0,0 +1,40 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/TriangleSplitter/TriangleSplitterMean.h>
+
+JPH_NAMESPACE_BEGIN
+
+TriangleSplitterMean::TriangleSplitterMean(const VertexList &inVertices, const IndexedTriangleList &inTriangles) :
+	TriangleSplitter(inVertices, inTriangles)
+{
+}
+
+bool TriangleSplitterMean::Split(const Range &inTriangles, Range &outLeft, Range &outRight)
+{
+	// Calculate mean value for these triangles
+	Vec3 mean = Vec3::sZero();
+	for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; ++t)
+		mean += Vec3(mCentroids[mSortedTriangleIdx[t]]);
+	mean *= 1.0f / inTriangles.Count();
+
+	// Calculate deviation
+	Vec3 deviation = Vec3::sZero();
+	for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; ++t)
+	{
+		Vec3 delta = Vec3(mCentroids[mSortedTriangleIdx[t]]) - mean;
+		deviation += delta * delta;
+	}
+	deviation *= 1.0f / inTriangles.Count();
+
+	// Calculate split plane
+	uint dimension = deviation.GetHighestComponentIndex();
+	float split = mean[dimension];
+
+	return SplitInternal(inTriangles, dimension, split, outLeft, outRight);
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMean.h b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMean.h
new file mode 100644
index 0000000000..737d76e1c1
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMean.h
@@ -0,0 +1,28 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/TriangleSplitter/TriangleSplitter.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Splitter using mean of axis with biggest centroid deviation
+class JPH_EXPORT TriangleSplitterMean : public TriangleSplitter
+{
+public:
+	/// Constructor
+							TriangleSplitterMean(const VertexList &inVertices, const IndexedTriangleList &inTriangles);
+
+	// See TriangleSplitter::GetStats
+	virtual void			GetStats(Stats &outStats) const override
+	{
+		outStats.mSplitterName = "TriangleSplitterMean";
+	}
+
+	// See TriangleSplitter::Split
+	virtual bool			Split(const Range &inTriangles, Range &outLeft, Range &outRight) override;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMorton.cpp b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMorton.cpp
new file mode 100644
index 0000000000..35b0f4212b
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMorton.cpp
@@ -0,0 +1,63 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/TriangleSplitter/TriangleSplitterMorton.h>
+#include <Jolt/Geometry/MortonCode.h>
+#include <Jolt/Core/QuickSort.h>
+
+JPH_NAMESPACE_BEGIN
+
+TriangleSplitterMorton::TriangleSplitterMorton(const VertexList &inVertices, const IndexedTriangleList &inTriangles) :
+	TriangleSplitter(inVertices, inTriangles)
+{
+	// Calculate bounds of centroids
+	AABox bounds;
+	for (uint t = 0; t < inTriangles.size(); ++t)
+		bounds.Encapsulate(Vec3(mCentroids[t]));
+
+	// Make sure box is not degenerate
+	bounds.EnsureMinimalEdgeLength(1.0e-5f);
+
+	// Calculate morton codes
+	mMortonCodes.resize(inTriangles.size());
+	for (uint t = 0; t < inTriangles.size(); ++t)
+		mMortonCodes[t] = MortonCode::sGetMortonCode(Vec3(mCentroids[t]), bounds);
+
+	// Sort triangles on morton code
+	const Array<uint32> &morton_codes = mMortonCodes;
+	QuickSort(mSortedTriangleIdx.begin(), mSortedTriangleIdx.end(), [&morton_codes](uint inLHS, uint inRHS) { return morton_codes[inLHS] < morton_codes[inRHS]; });
+}
+
+bool TriangleSplitterMorton::Split(const Range &inTriangles, Range &outLeft, Range &outRight)
+{
+	uint32 first_code = mMortonCodes[mSortedTriangleIdx[inTriangles.mBegin]];
+	uint32 last_code = mMortonCodes[mSortedTriangleIdx[inTriangles.mEnd - 1]];
+
+	uint common_prefix = CountLeadingZeros(first_code ^ last_code);
+
+	// Use binary search to find where the next bit differs
+	uint split = inTriangles.mBegin; // Initial guess
+	uint step = inTriangles.Count();
+	do
+	{
+		step = (step + 1) >> 1; // Exponential decrease
+		uint new_split = split + step; // Proposed new position
+		if (new_split < inTriangles.mEnd)
+		{
+			uint32 split_code = mMortonCodes[mSortedTriangleIdx[new_split]];
+			uint split_prefix = CountLeadingZeros(first_code ^ split_code);
+			if (split_prefix > common_prefix)
+				split = new_split; // Accept proposal
+		}
+	}
+	while (step > 1);
+
+	outLeft = Range(inTriangles.mBegin, split + 1);
+	outRight = Range(split + 1, inTriangles.mEnd);
+	return outLeft.Count() > 0 && outRight.Count() > 0;
+}
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMorton.h b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMorton.h
new file mode 100644
index 0000000000..2f48c0ea96
--- /dev/null
+++ b/thirdparty/jolt_physics/Jolt/TriangleSplitter/TriangleSplitterMorton.h
@@ -0,0 +1,32 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/TriangleSplitter/TriangleSplitter.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Splitter using Morton codes, see: http://devblogs.nvidia.com/parallelforall/thinking-parallel-part-iii-tree-construction-gpu/
+class JPH_EXPORT TriangleSplitterMorton : public TriangleSplitter
+{
+public:
+	/// Constructor
+							TriangleSplitterMorton(const VertexList &inVertices, const IndexedTriangleList &inTriangles);
+
+	// See TriangleSplitter::GetStats
+	virtual void			GetStats(Stats &outStats) const override
+	{
+		outStats.mSplitterName = "TriangleSplitterMorton";
+	}
+
+	// See TriangleSplitter::Split
+	virtual bool			Split(const Range &inTriangles, Range &outLeft, Range &outRight) override;
+
+private:
+	// Precalculated Morton codes
+	Array<uint32>			mMortonCodes;
+};
+
+JPH_NAMESPACE_END
diff --git a/thirdparty/jolt_physics/LICENSE b/thirdparty/jolt_physics/LICENSE
new file mode 100644
index 0000000000..4f09768485
--- /dev/null
+++ b/thirdparty/jolt_physics/LICENSE
@@ -0,0 +1,7 @@
+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.

From 11bf2da9de162d8f9372a29be816416c61f6f210 Mon Sep 17 00:00:00 2001
From: Timo Schwarzer <timo@schwarzer.dev>
Date: Wed, 13 Nov 2024 18:52:39 +0100
Subject: [PATCH 075/124] Improve editor file dialog options

---
 editor/gui/editor_file_dialog.cpp | 44 +++++++++++++++++++++----------
 editor/gui/editor_file_dialog.h   |  4 ++-
 2 files changed, 33 insertions(+), 15 deletions(-)

diff --git a/editor/gui/editor_file_dialog.cpp b/editor/gui/editor_file_dialog.cpp
index 7ef9823537..21967f3e08 100644
--- a/editor/gui/editor_file_dialog.cpp
+++ b/editor/gui/editor_file_dialog.cpp
@@ -42,6 +42,7 @@
 #include "editor/themes/editor_scale.h"
 #include "scene/gui/center_container.h"
 #include "scene/gui/check_box.h"
+#include "scene/gui/flow_container.h"
 #include "scene/gui/grid_container.h"
 #include "scene/gui/label.h"
 #include "scene/gui/option_button.h"
@@ -1951,30 +1952,38 @@ void EditorFileDialog::_update_option_controls() {
 	}
 	options_dirty = false;
 
-	while (grid_options->get_child_count() > 0) {
-		Node *child = grid_options->get_child(0);
-		grid_options->remove_child(child);
+	while (flow_checkbox_options->get_child_count() > 0) {
+		Node *child = flow_checkbox_options->get_child(0);
+		flow_checkbox_options->remove_child(child);
 		child->queue_free();
 	}
+	while (grid_select_options->get_child_count() > 0) {
+		Node *child = grid_select_options->get_child(0);
+		grid_select_options->remove_child(child);
+		child->queue_free();
+	}
+
 	selected_options.clear();
 
 	for (const EditorFileDialog::Option &opt : options) {
-		Label *lbl = memnew(Label);
-		lbl->set_text(opt.name);
-		grid_options->add_child(lbl);
 		if (opt.values.is_empty()) {
 			CheckBox *cb = memnew(CheckBox);
 			cb->set_pressed(opt.default_idx);
-			grid_options->add_child(cb);
+			cb->set_text(opt.name);
+			flow_checkbox_options->add_child(cb);
 			cb->connect(SceneStringName(toggled), callable_mp(this, &EditorFileDialog::_option_changed_checkbox_toggled).bind(opt.name));
 			selected_options[opt.name] = (bool)opt.default_idx;
 		} else {
+			Label *lbl = memnew(Label);
+			lbl->set_text(opt.name);
+			grid_select_options->add_child(lbl);
+
 			OptionButton *ob = memnew(OptionButton);
 			for (const String &val : opt.values) {
 				ob->add_item(val);
 			}
 			ob->select(opt.default_idx);
-			grid_options->add_child(ob);
+			grid_select_options->add_child(ob);
 			ob->connect(SceneStringName(item_selected), callable_mp(this, &EditorFileDialog::_option_changed_item_selected).bind(opt.name));
 			selected_options[opt.name] = opt.default_idx;
 		}
@@ -2266,11 +2275,13 @@ void EditorFileDialog::add_side_menu(Control *p_menu, const String &p_title) {
 void EditorFileDialog::_update_side_menu_visibility(bool p_native_dlg) {
 	if (p_native_dlg) {
 		pathhb->set_visible(false);
-		grid_options->set_visible(false);
+		flow_checkbox_options->set_visible(false);
+		grid_select_options->set_visible(false);
 		list_hb->set_visible(false);
 	} else {
 		pathhb->set_visible(true);
-		grid_options->set_visible(true);
+		flow_checkbox_options->set_visible(true);
+		grid_select_options->set_visible(true);
 		list_hb->set_visible(true);
 	}
 }
@@ -2372,10 +2383,15 @@ EditorFileDialog::EditorFileDialog() {
 	body_hsplit->set_v_size_flags(Control::SIZE_EXPAND_FILL);
 	vbc->add_child(body_hsplit);
 
-	grid_options = memnew(GridContainer);
-	grid_options->set_h_size_flags(Control::SIZE_SHRINK_CENTER);
-	grid_options->set_columns(2);
-	vbc->add_child(grid_options);
+	flow_checkbox_options = memnew(HFlowContainer);
+	flow_checkbox_options->set_h_size_flags(Control::SIZE_EXPAND_FILL);
+	flow_checkbox_options->set_alignment(FlowContainer::ALIGNMENT_CENTER);
+	vbc->add_child(flow_checkbox_options);
+
+	grid_select_options = memnew(GridContainer);
+	grid_select_options->set_h_size_flags(Control::SIZE_SHRINK_CENTER);
+	grid_select_options->set_columns(2);
+	vbc->add_child(grid_select_options);
 
 	list_hb = memnew(HSplitContainer);
 	list_hb->set_h_size_flags(Control::SIZE_EXPAND_FILL);
diff --git a/editor/gui/editor_file_dialog.h b/editor/gui/editor_file_dialog.h
index 8a07a20943..d1b37687df 100644
--- a/editor/gui/editor_file_dialog.h
+++ b/editor/gui/editor_file_dialog.h
@@ -39,6 +39,7 @@
 class DependencyRemoveDialog;
 class GridContainer;
 class HSplitContainer;
+class HFlowContainer;
 class ItemList;
 class MenuButton;
 class OptionButton;
@@ -94,7 +95,8 @@ private:
 	Button *makedir = nullptr;
 	Access access = ACCESS_RESOURCES;
 
-	GridContainer *grid_options = nullptr;
+	HFlowContainer *flow_checkbox_options = nullptr;
+	GridContainer *grid_select_options = nullptr;
 	VBoxContainer *vbox = nullptr;
 	FileMode mode = FILE_MODE_SAVE_FILE;
 	bool can_create_dir = false;

From 3dfc832272f8969c72073aecc14293dc0d1dbc1f Mon Sep 17 00:00:00 2001
From: Hugo Locurcio <hugo.locurcio@hugo.pro>
Date: Wed, 11 Dec 2024 15:44:06 +0100
Subject: [PATCH 076/124] Replace `textureSize()` with a uniform in
 BaseMaterial3D for MSDF rendering

This uniform was already defined for other uses previously.

`textureSize()` is known to be slow on mobile platforms due to how
the drivers implement it there, so it's best avoided.
---
 scene/resources/material.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/scene/resources/material.cpp b/scene/resources/material.cpp
index 77ac0569ff..5cdb7602d4 100644
--- a/scene/resources/material.cpp
+++ b/scene/resources/material.cpp
@@ -1515,7 +1515,7 @@ void fragment() {)";
 				vec3(1.0 + 0.055) * pow(albedo_tex.rgb, vec3(1.0 / 2.4)) - vec3(0.055),
 				vec3(12.92) * albedo_tex.rgb,
 				lessThan(albedo_tex.rgb, vec3(0.0031308)));
-		vec2 msdf_size = vec2(msdf_pixel_range) / vec2(textureSize(texture_albedo, 0));
+		vec2 msdf_size = vec2(msdf_pixel_range) / vec2(albedo_texture_size));
 )";
 		if (flags[FLAG_USE_POINT_SIZE]) {
 			code += "		vec2 dest_size = vec2(1.0) / fwidth(POINT_COORD);\n";

From 57073ba14ee5024be0f69cd143ef3a11da03156a Mon Sep 17 00:00:00 2001
From: Lukas Tenbrink <lukas.tenbrink@gmail.com>
Date: Tue, 10 Dec 2024 13:39:42 +0100
Subject: [PATCH 077/124] Add move constructor and move assignment to CowData,
 String, Char16String, CharString and Vector.

---
 core/string/ustring.h    | 11 +++++++++++
 core/templates/cowdata.h | 15 ++++++++++++++-
 core/templates/vector.h  |  8 +++++---
 3 files changed, 30 insertions(+), 4 deletions(-)

diff --git a/core/string/ustring.h b/core/string/ustring.h
index ac4ace2249..594e1d362b 100644
--- a/core/string/ustring.h
+++ b/core/string/ustring.h
@@ -39,6 +39,8 @@
 #include "core/typedefs.h"
 #include "core/variant/array.h"
 
+#include <utility>
+
 /*************************************************************************/
 /*  Utility Functions                                                    */
 /*************************************************************************/
@@ -193,7 +195,10 @@ public:
 
 	_FORCE_INLINE_ Char16String() {}
 	_FORCE_INLINE_ Char16String(const Char16String &p_str) { _cowdata._ref(p_str._cowdata); }
+	_FORCE_INLINE_ Char16String(Char16String &&p_str) :
+			_cowdata(std::move(p_str._cowdata)) {}
 	_FORCE_INLINE_ void operator=(const Char16String &p_str) { _cowdata._ref(p_str._cowdata); }
+	_FORCE_INLINE_ void operator=(Char16String &&p_str) { _cowdata = std::move(p_str._cowdata); }
 	_FORCE_INLINE_ Char16String(const char16_t *p_cstr) { copy_from(p_cstr); }
 
 	void operator=(const char16_t *p_cstr);
@@ -235,7 +240,10 @@ public:
 
 	_FORCE_INLINE_ CharString() {}
 	_FORCE_INLINE_ CharString(const CharString &p_str) { _cowdata._ref(p_str._cowdata); }
+	_FORCE_INLINE_ CharString(CharString &&p_str) :
+			_cowdata(std::move(p_str._cowdata)) {}
 	_FORCE_INLINE_ void operator=(const CharString &p_str) { _cowdata._ref(p_str._cowdata); }
+	_FORCE_INLINE_ void operator=(CharString &&p_str) { _cowdata = std::move(p_str._cowdata); }
 	_FORCE_INLINE_ CharString(const char *p_cstr) { copy_from(p_cstr); }
 
 	void operator=(const char *p_cstr);
@@ -594,7 +602,10 @@ public:
 
 	_FORCE_INLINE_ String() {}
 	_FORCE_INLINE_ String(const String &p_str) { _cowdata._ref(p_str._cowdata); }
+	_FORCE_INLINE_ String(String &&p_str) :
+			_cowdata(std::move(p_str._cowdata)) {}
 	_FORCE_INLINE_ void operator=(const String &p_str) { _cowdata._ref(p_str._cowdata); }
+	_FORCE_INLINE_ void operator=(String &&p_str) { _cowdata = std::move(p_str._cowdata); }
 
 	Vector<uint8_t> to_ascii_buffer() const;
 	Vector<uint8_t> to_utf8_buffer() const;
diff --git a/core/templates/cowdata.h b/core/templates/cowdata.h
index 5f260ee870..f87fa1ad81 100644
--- a/core/templates/cowdata.h
+++ b/core/templates/cowdata.h
@@ -167,6 +167,15 @@ private:
 
 public:
 	void operator=(const CowData<T> &p_from) { _ref(p_from); }
+	void operator=(CowData<T> &&p_from) {
+		if (_ptr == p_from._ptr) {
+			return;
+		}
+
+		_unref();
+		_ptr = p_from._ptr;
+		p_from._ptr = nullptr;
+	}
 
 	_FORCE_INLINE_ T *ptrw() {
 		_copy_on_write();
@@ -241,7 +250,11 @@ public:
 
 	_FORCE_INLINE_ CowData() {}
 	_FORCE_INLINE_ ~CowData();
-	_FORCE_INLINE_ CowData(CowData<T> &p_from) { _ref(p_from); }
+	_FORCE_INLINE_ CowData(const CowData<T> &p_from) { _ref(p_from); }
+	_FORCE_INLINE_ CowData(CowData<T> &&p_from) {
+		_ptr = p_from._ptr;
+		p_from._ptr = nullptr;
+	}
 };
 
 template <typename T>
diff --git a/core/templates/vector.h b/core/templates/vector.h
index 0fcca47bae..32e1339e95 100644
--- a/core/templates/vector.h
+++ b/core/templates/vector.h
@@ -47,6 +47,7 @@
 
 #include <climits>
 #include <initializer_list>
+#include <utility>
 
 template <typename T>
 class VectorWriteProxy {
@@ -147,9 +148,8 @@ public:
 		insert(i, p_val);
 	}
 
-	inline void operator=(const Vector &p_from) {
-		_cowdata._ref(p_from._cowdata);
-	}
+	void operator=(const Vector &p_from) { _cowdata._ref(p_from._cowdata); }
+	void operator=(Vector &&p_from) { _cowdata = std::move(p_from._cowdata); }
 
 	Vector<uint8_t> to_byte_array() const {
 		Vector<uint8_t> ret;
@@ -290,6 +290,8 @@ public:
 		}
 	}
 	_FORCE_INLINE_ Vector(const Vector &p_from) { _cowdata._ref(p_from._cowdata); }
+	_FORCE_INLINE_ Vector(Vector &&p_from) :
+			_cowdata(std::move(p_from._cowdata)) {}
 
 	_FORCE_INLINE_ ~Vector() {}
 };

From a8c8eca74a9c448e2ce95d2a53428234889c1e6c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Pa=CC=84vels=20Nadtoc=CC=8Cajevs?=
 <7645683+bruvzg@users.noreply.github.com>
Date: Wed, 11 Dec 2024 17:28:29 +0200
Subject: [PATCH 078/124] [Windows] Fix loading debug symbol files over 2GB.

---
 .../windows/crash_handler_windows_signal.cpp  |  5 +-
 thirdparty/README.md                          |  4 ++
 .../libbacktrace/patches/patch_big_files.diff | 47 +++++++++++++++++++
 thirdparty/libbacktrace/read.c                | 22 +++++----
 4 files changed, 68 insertions(+), 10 deletions(-)
 create mode 100644 thirdparty/libbacktrace/patches/patch_big_files.diff

diff --git a/platform/windows/crash_handler_windows_signal.cpp b/platform/windows/crash_handler_windows_signal.cpp
index ddfb9b65de..ebc89a8181 100644
--- a/platform/windows/crash_handler_windows_signal.cpp
+++ b/platform/windows/crash_handler_windows_signal.cpp
@@ -167,7 +167,10 @@ extern void CrashHandlerException(int signal) {
 	if (FileAccess::exists(_execpath + ".debugsymbols")) {
 		_execpath = _execpath + ".debugsymbols";
 	}
-	data.state = backtrace_create_state(_execpath.utf8().get_data(), 0, &error_callback, reinterpret_cast<void *>(&data));
+	_execpath = _execpath.replace("/", "\\");
+
+	CharString cs = _execpath.utf8(); // Note: should remain in scope during backtrace_simple call.
+	data.state = backtrace_create_state(cs.get_data(), 0, &error_callback, reinterpret_cast<void *>(&data));
 	if (data.state != nullptr) {
 		data.index = 1;
 		backtrace_simple(data.state, 1, &trace_callback, &error_callback, reinterpret_cast<void *>(&data));
diff --git a/thirdparty/README.md b/thirdparty/README.md
index 669e6829fa..258a12b840 100644
--- a/thirdparty/README.md
+++ b/thirdparty/README.md
@@ -468,6 +468,10 @@ Files extracted from upstream source:
 - `*.{c,h}` files for Windows platform
 - `LICENSE`
 
+Important: Some files have Godot-made changes to load big debug symbol files.
+They are marked with `/* GODOT start */` and `/* GODOT end */`
+comments and a patch is provided in the `patches` folder.
+
 
 ## libktx
 
diff --git a/thirdparty/libbacktrace/patches/patch_big_files.diff b/thirdparty/libbacktrace/patches/patch_big_files.diff
new file mode 100644
index 0000000000..6c3185c8d1
--- /dev/null
+++ b/thirdparty/libbacktrace/patches/patch_big_files.diff
@@ -0,0 +1,47 @@
+diff --git a/thirdparty/libbacktrace/read.c b/thirdparty/libbacktrace/read.c
+index 1811c8d2e0..fda8e222d4 100644
+--- a/thirdparty/libbacktrace/read.c
++++ b/thirdparty/libbacktrace/read.c
+@@ -52,14 +52,9 @@ backtrace_get_view (struct backtrace_state *state, int descriptor,
+ {
+   uint64_t got;
+   ssize_t r;
+-
+-  if ((uint64_t) (size_t) size != size)
+-    {
+-      error_callback (data, "file size too large", 0);
+-      return 0;
+-    }
+-
+-  if (lseek (descriptor, offset, SEEK_SET) < 0)
++/* GODOT start */
++  if (_lseeki64 (descriptor, offset, SEEK_SET) < 0)
++/* GODOT end */
+     {
+       error_callback (data, "lseek", errno);
+       return 0;
+@@ -74,7 +69,13 @@ backtrace_get_view (struct backtrace_state *state, int descriptor,
+   got = 0;
+   while (got < size)
+     {
+-      r = read (descriptor, view->base, size - got);
++/* GODOT start */
++      uint64_t sz = size - got;
++      if (sz > INT_MAX) {
++        sz = INT_MAX;
++      }
++      r = _read (descriptor, view->base, sz);
++/* GODOT end */
+       if (r < 0)
+ 	{
+ 	  error_callback (data, "read", errno);
+@@ -84,6 +85,9 @@ backtrace_get_view (struct backtrace_state *state, int descriptor,
+       if (r == 0)
+ 	break;
+       got += (uint64_t) r;
++/* GODOT start */
++      view->base += r;
++/* GODOT end */
+     }
+ 
+   if (got < size)
diff --git a/thirdparty/libbacktrace/read.c b/thirdparty/libbacktrace/read.c
index 1811c8d2e0..f5e01f01b0 100644
--- a/thirdparty/libbacktrace/read.c
+++ b/thirdparty/libbacktrace/read.c
@@ -52,14 +52,9 @@ backtrace_get_view (struct backtrace_state *state, int descriptor,
 {
   uint64_t got;
   ssize_t r;
-
-  if ((uint64_t) (size_t) size != size)
-    {
-      error_callback (data, "file size too large", 0);
-      return 0;
-    }
-
-  if (lseek (descriptor, offset, SEEK_SET) < 0)
+/* GODOT start */
+  if (_lseeki64 (descriptor, offset, SEEK_SET) < 0)
+/* GODOT end */
     {
       error_callback (data, "lseek", errno);
       return 0;
@@ -74,7 +69,13 @@ backtrace_get_view (struct backtrace_state *state, int descriptor,
   got = 0;
   while (got < size)
     {
-      r = read (descriptor, view->base, size - got);
+/* GODOT start */
+      uint64_t sz = size - got;
+      if (sz > INT_MAX) {
+        sz = INT_MAX;
+      }
+      r = _read (descriptor, view->base, sz);
+/* GODOT end */
       if (r < 0)
 	{
 	  error_callback (data, "read", errno);
@@ -84,6 +85,9 @@ backtrace_get_view (struct backtrace_state *state, int descriptor,
       if (r == 0)
 	break;
       got += (uint64_t) r;
+/* GODOT start */
+      view->base += r;
+/* GODOT end */
     }
 
   if (got < size)

From acf439e96d3950eb166f67fdac98eaacc90bfe70 Mon Sep 17 00:00:00 2001
From: "Matias N. Goldberg" <dark_sylinc@yahoo.com.ar>
Date: Tue, 10 Dec 2024 19:25:46 -0300
Subject: [PATCH 079/124] Keep processing Graphics if there are pending
 operations

Fixes #90017
Fixes #90030
Fixes #98044

This PR makes the following changes:

# Force processing of GPU commands for frame_count frames

The variable `frames_pending_resources_for_processing` is added to track
this.

The ticket #98044 suggested to use `_flush_and_stall_for_all_frames()`
while minimized.

Technically this works and is a viable solution.

However I noticed that this issue was happening because Logic/Physics
continue to work "business as usual" while minimized(\*). Only Graphics
was being deactivated (which caused commands to accumulate until window
is restored).

To continue this behavior of "business as usual", I decided that GPU
work should also "continue as usual" by buffering commands in a double
or triple buffer scheme until all commands are done processing (if they
ever stop coming). This is specially important if the app specifically
intends to keep processing while minimized.

Calling `_flush_and_stall_for_all_frames()` would fix the leak, but it
would make  Godot's behavior different while minimized vs while the
window is presenting.

\* `OS::add_frame_delay` _does_ consider being minimized, but it just
throttles CPU usage. Some platforms such as Android completely disable
processing because the higher level code stops being called when the app
goes into background. But this seems like an implementation-detail that
diverges from the rest of the platforms (e.g. Windows, Linux & macOS
continue to process while minimized).

# Rename p_swap_buffers for p_present

**This is potentially a breaking change** (if it actually breaks
anything, I ignore. But I strongly suspect it doesn't break anything).

"Swap Buffers" is a concept carried from OpenGL, where a frame is "done"
when `glSwapBuffers()` is called, which basically means "present to the
screen".

However it _also_ means that OpenGL internally swaps its internal
buffers in a double/triple buffer scheme (in Vulkan, we do that
ourselves and is tracked by `RenderingDevice::frame`).

Modern APIs like Vulkan differentiate between "submitting GPU work" and
"presenting".

Before this PR, calling `RendererCompositorRD::end_frame(false)` would
literally do nothing. This is often undesired and the cause of the leak.
After this PR, calling `RendererCompositorRD::end_frame(false)` will now
process commands, swap our internal buffers in a double/triple buffer
scheme **but avoid presenting to the screen**.

Hence the rename of the variable from `p_swap_buffers` to `p_present`
(which slightly alters its behavior).
If we want `RendererCompositorRD::end_frame(false)` to do nothing, then
we should not call it at all.

This PR reflects such change: When we're minimized **_and_**
`has_pending_resources_for_processing()` returns false, we don't call
`RendererCompositorRD::end_frame()` at all.

But if `has_pending_resources_for_processing()` returns true, we will
call it, but with `p_present = false` because we're minimized.

There's still the issue that Godot keeps processing work (logic,
scripts, physics) while minimized, which we shouldn't do by default. But
that's work for follow up PR.
---
 main/main.cpp                                       | 13 +++++++++----
 servers/rendering/dummy/rasterizer_dummy.h          |  4 ++--
 servers/rendering/renderer_compositor.h             |  2 +-
 .../renderer_rd/renderer_compositor_rd.cpp          |  8 +++-----
 .../rendering/renderer_rd/renderer_compositor_rd.h  |  2 +-
 servers/rendering/rendering_device.cpp              | 10 ++++++++--
 servers/rendering/rendering_device.h                | 13 ++++++++++++-
 servers/rendering/rendering_server_default.cpp      |  6 +++---
 servers/rendering/rendering_server_default.h        |  2 +-
 9 files changed, 40 insertions(+), 20 deletions(-)

diff --git a/main/main.cpp b/main/main.cpp
index 2c2ef94e0e..51b26a63f2 100644
--- a/main/main.cpp
+++ b/main/main.cpp
@@ -4441,15 +4441,20 @@ bool Main::iteration() {
 
 	RenderingServer::get_singleton()->sync(); //sync if still drawing from previous frames.
 
-	if ((DisplayServer::get_singleton()->can_any_window_draw() || DisplayServer::get_singleton()->has_additional_outputs()) &&
-			RenderingServer::get_singleton()->is_render_loop_enabled()) {
+	const bool has_pending_resources_for_processing = RD::get_singleton() && RD::get_singleton()->has_pending_resources_for_processing();
+	bool wants_present = (DisplayServer::get_singleton()->can_any_window_draw() ||
+								 DisplayServer::get_singleton()->has_additional_outputs()) &&
+			RenderingServer::get_singleton()->is_render_loop_enabled();
+
+	if (wants_present || has_pending_resources_for_processing) {
+		wants_present |= force_redraw_requested;
 		if ((!force_redraw_requested) && OS::get_singleton()->is_in_low_processor_usage_mode()) {
 			if (RenderingServer::get_singleton()->has_changed()) {
-				RenderingServer::get_singleton()->draw(true, scaled_step); // flush visual commands
+				RenderingServer::get_singleton()->draw(wants_present, scaled_step); // flush visual commands
 				Engine::get_singleton()->increment_frames_drawn();
 			}
 		} else {
-			RenderingServer::get_singleton()->draw(true, scaled_step); // flush visual commands
+			RenderingServer::get_singleton()->draw(wants_present, scaled_step); // flush visual commands
 			Engine::get_singleton()->increment_frames_drawn();
 			force_redraw_requested = false;
 		}
diff --git a/servers/rendering/dummy/rasterizer_dummy.h b/servers/rendering/dummy/rasterizer_dummy.h
index 6205193d9a..33b4c38018 100644
--- a/servers/rendering/dummy/rasterizer_dummy.h
+++ b/servers/rendering/dummy/rasterizer_dummy.h
@@ -90,8 +90,8 @@ public:
 
 	void gl_end_frame(bool p_swap_buffers) override {}
 
-	void end_frame(bool p_swap_buffers) override {
-		if (p_swap_buffers) {
+	void end_frame(bool p_present) override {
+		if (p_present) {
 			DisplayServer::get_singleton()->swap_buffers();
 		}
 	}
diff --git a/servers/rendering/renderer_compositor.h b/servers/rendering/renderer_compositor.h
index dbc8f155bf..e52cab740e 100644
--- a/servers/rendering/renderer_compositor.h
+++ b/servers/rendering/renderer_compositor.h
@@ -99,7 +99,7 @@ public:
 	virtual void blit_render_targets_to_screen(DisplayServer::WindowID p_screen, const BlitToScreen *p_render_targets, int p_amount) = 0;
 
 	virtual void gl_end_frame(bool p_swap_buffers) = 0;
-	virtual void end_frame(bool p_swap_buffers) = 0;
+	virtual void end_frame(bool p_present) = 0;
 	virtual void finalize() = 0;
 	virtual uint64_t get_frame_number() const = 0;
 	virtual double get_frame_delta_time() const = 0;
diff --git a/servers/rendering/renderer_rd/renderer_compositor_rd.cpp b/servers/rendering/renderer_rd/renderer_compositor_rd.cpp
index ba47508700..05dda5e74c 100644
--- a/servers/rendering/renderer_rd/renderer_compositor_rd.cpp
+++ b/servers/rendering/renderer_rd/renderer_compositor_rd.cpp
@@ -112,10 +112,8 @@ void RendererCompositorRD::begin_frame(double frame_step) {
 	scene->set_time(time, frame_step);
 }
 
-void RendererCompositorRD::end_frame(bool p_swap_buffers) {
-	if (p_swap_buffers) {
-		RD::get_singleton()->swap_buffers();
-	}
+void RendererCompositorRD::end_frame(bool p_present) {
+	RD::get_singleton()->swap_buffers(p_present);
 }
 
 void RendererCompositorRD::initialize() {
@@ -264,7 +262,7 @@ void RendererCompositorRD::set_boot_image(const Ref<Image> &p_image, const Color
 
 	RD::get_singleton()->draw_list_end();
 
-	RD::get_singleton()->swap_buffers();
+	RD::get_singleton()->swap_buffers(true);
 
 	texture_storage->texture_free(texture);
 	RD::get_singleton()->free(sampler);
diff --git a/servers/rendering/renderer_rd/renderer_compositor_rd.h b/servers/rendering/renderer_rd/renderer_compositor_rd.h
index 6821fa737e..41c11113fe 100644
--- a/servers/rendering/renderer_rd/renderer_compositor_rd.h
+++ b/servers/rendering/renderer_rd/renderer_compositor_rd.h
@@ -128,7 +128,7 @@ public:
 	void blit_render_targets_to_screen(DisplayServer::WindowID p_screen, const BlitToScreen *p_render_targets, int p_amount);
 
 	void gl_end_frame(bool p_swap_buffers) {}
-	void end_frame(bool p_swap_buffers);
+	void end_frame(bool p_present);
 	void finalize();
 
 	_ALWAYS_INLINE_ uint64_t get_frame_number() const { return frame; }
diff --git a/servers/rendering/rendering_device.cpp b/servers/rendering/rendering_device.cpp
index 72984e5abf..85b5759d12 100644
--- a/servers/rendering/rendering_device.cpp
+++ b/servers/rendering/rendering_device.cpp
@@ -5660,6 +5660,8 @@ void RenderingDevice::_free_internal(RID p_id) {
 		ERR_PRINT("Attempted to free invalid ID: " + itos(p_id.get_id()));
 #endif
 	}
+
+	frames_pending_resources_for_processing = uint32_t(frames.size());
 }
 
 // The full list of resources that can be named is in the VkObjectType enum.
@@ -5762,11 +5764,11 @@ String RenderingDevice::get_device_pipeline_cache_uuid() const {
 	return driver->get_pipeline_cache_uuid();
 }
 
-void RenderingDevice::swap_buffers() {
+void RenderingDevice::swap_buffers(bool p_present) {
 	ERR_RENDER_THREAD_GUARD();
 
 	_end_frame();
-	_execute_frame(true);
+	_execute_frame(p_present);
 
 	// Advance to the next frame and begin recording again.
 	frame = (frame + 1) % frames.size();
@@ -5868,6 +5870,10 @@ void RenderingDevice::_free_pending_resources(int p_frame) {
 
 		frames[p_frame].buffers_to_dispose_of.pop_front();
 	}
+
+	if (frames_pending_resources_for_processing > 0u) {
+		--frames_pending_resources_for_processing;
+	}
 }
 
 uint32_t RenderingDevice::get_frame_delay() const {
diff --git a/servers/rendering/rendering_device.h b/servers/rendering/rendering_device.h
index 6b19d05fca..2595aed62d 100644
--- a/servers/rendering/rendering_device.h
+++ b/servers/rendering/rendering_device.h
@@ -1400,6 +1400,17 @@ private:
 	TightLocalVector<Frame> frames;
 	uint64_t frames_drawn = 0;
 
+	// Whenever logic/physics request a graphics operation (not just deleting a resource) that requires
+	// us to flush all graphics commands, we must set frames_pending_resources_for_processing = frames.size().
+	// This is important for when the user requested for the logic loop to still be updated while
+	// graphics should not (e.g. headless Multiplayer servers, minimized windows that need to still
+	// process something on the background).
+	uint32_t frames_pending_resources_for_processing = 0u;
+
+public:
+	bool has_pending_resources_for_processing() const { return frames_pending_resources_for_processing != 0u; }
+
+private:
 	void _free_pending_resources(int p_frame);
 
 	uint64_t texture_memory = 0;
@@ -1444,7 +1455,7 @@ public:
 
 	uint64_t limit_get(Limit p_limit) const;
 
-	void swap_buffers();
+	void swap_buffers(bool p_present);
 
 	uint32_t get_frame_delay() const;
 
diff --git a/servers/rendering/rendering_server_default.cpp b/servers/rendering/rendering_server_default.cpp
index 2ec693cbbf..b5d0d01479 100644
--- a/servers/rendering/rendering_server_default.cpp
+++ b/servers/rendering/rendering_server_default.cpp
@@ -406,15 +406,15 @@ void RenderingServerDefault::sync() {
 	}
 }
 
-void RenderingServerDefault::draw(bool p_swap_buffers, double frame_step) {
+void RenderingServerDefault::draw(bool p_present, double frame_step) {
 	ERR_FAIL_COND_MSG(!Thread::is_main_thread(), "Manually triggering the draw function from the RenderingServer can only be done on the main thread. Call this function from the main thread or use call_deferred().");
 	// Needs to be done before changes is reset to 0, to not force the editor to redraw.
 	RS::get_singleton()->emit_signal(SNAME("frame_pre_draw"));
 	changes = 0;
 	if (create_thread) {
-		command_queue.push(this, &RenderingServerDefault::_draw, p_swap_buffers, frame_step);
+		command_queue.push(this, &RenderingServerDefault::_draw, p_present, frame_step);
 	} else {
-		_draw(p_swap_buffers, frame_step);
+		_draw(p_present, frame_step);
 	}
 }
 
diff --git a/servers/rendering/rendering_server_default.h b/servers/rendering/rendering_server_default.h
index 29b1e163c7..4a0df4a0cf 100644
--- a/servers/rendering/rendering_server_default.h
+++ b/servers/rendering/rendering_server_default.h
@@ -1124,7 +1124,7 @@ public:
 
 	virtual void request_frame_drawn_callback(const Callable &p_callable) override;
 
-	virtual void draw(bool p_swap_buffers, double frame_step) override;
+	virtual void draw(bool p_present, double frame_step) override;
 	virtual void sync() override;
 	virtual bool has_changed() const override;
 	virtual void init() override;

From 9bb747e0e87f846be90a6978063ea8b9127b3080 Mon Sep 17 00:00:00 2001
From: Thaddeus Crews <repiteo@outlook.com>
Date: Tue, 26 Nov 2024 10:35:15 -0600
Subject: [PATCH 080/124] CI: Remove leading-underscore teams from `CODEOWNERS`

---
 .github/CODEOWNERS | 36 ++++++++++++++----------------------
 1 file changed, 14 insertions(+), 22 deletions(-)

diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS
index 713c982123..9de8b01530 100644
--- a/.github/CODEOWNERS
+++ b/.github/CODEOWNERS
@@ -2,10 +2,6 @@
 # Each line is a file pattern followed by one or more owners.
 # Owners can be @users, @org/teams or emails.
 
-# Buildsystem (Before everything to be overwritten)
-
-*                                             @godotengine/buildsystem
-
 # Core
 
 /core/                                        @godotengine/core
@@ -20,8 +16,6 @@
 
 # Drivers
 
-/drivers/                                     @godotengine/_systems
-
 ## Audio
 /drivers/alsa/                                @godotengine/audio
 /drivers/alsamidi/                            @godotengine/audio
@@ -42,7 +36,7 @@
 /drivers/vulkan/                              @godotengine/rendering
 
 ## OS
-/drivers/unix/                                @godotengine/_platforms
+/drivers/unix/                                @godotengine/linux-bsd
 /drivers/windows/                             @godotengine/windows
 
 ## Misc
@@ -50,9 +44,9 @@
 
 # Editor
 
-/editor/                                      @godotengine/_editor
 /editor/**/*2d*                               @godotengine/2d-editor
 /editor/**/*3d*                               @godotengine/3d-editor
+/editor/**/*audio*                            @godotengine/audio
 /editor/**/*code*                             @godotengine/script-editor
 /editor/**/*debugger*                         @godotengine/debugger
 /editor/**/*dock*                             @godotengine/docks
@@ -75,11 +69,6 @@
 
 # Modules
 
-/modules/                                     @godotengine/_engine
-/modules/**/doc_classes/                      @godotengine/_engine @godotengine/documentation
-/modules/**/icons/                            @godotengine/_engine @godotengine/usability
-/modules/**/tests/                            @godotengine/_engine @godotengine/tests
-
 ## Audio (+ video)
 /modules/interactive_music/                   @godotengine/audio
 /modules/interactive_music/doc_classes/       @godotengine/audio @godotengine/documentation
@@ -95,6 +84,7 @@
 ## Import
 /modules/astcenc/                             @godotengine/import
 /modules/basis_universal/                     @godotengine/import
+/modules/bcdec/                               @godotengine/import
 /modules/betsy/                               @godotengine/import
 /modules/bmp/                                 @godotengine/import
 /modules/cvtt/                                @godotengine/import
@@ -132,6 +122,7 @@
 ## Physics
 /modules/godot_physics_2d/                    @godotengine/physics
 /modules/godot_physics_3d/                    @godotengine/physics
+/modules/jolt_physics/                        @godotengine/physics
 
 ## Rendering
 /modules/glslang/                             @godotengine/rendering
@@ -190,7 +181,6 @@
 
 # Platform
 
-/platform/                                    @godotengine/_platforms
 /platform/android/                            @godotengine/android
 /platform/android/doc_classes/                @godotengine/android @godotengine/documentation
 /platform/ios/                                @godotengine/ios
@@ -206,7 +196,6 @@
 
 # Scene
 
-/scene/                                       @godotengine/_systems @godotengine/core
 /scene/2d/                                    @godotengine/2d-nodes
 /scene/2d/physics/                            @godotengine/2d-nodes @godotengine/physics
 /scene/3d/                                    @godotengine/3d-nodes
@@ -216,20 +205,23 @@
 /scene/debugger/                              @godotengine/debugger
 /scene/gui/                                   @godotengine/gui-nodes
 /scene/main/                                  @godotengine/core
-/scene/resources/font.*                       @godotengine/gui-nodes
-/scene/resources/text_line.*                  @godotengine/gui-nodes
-/scene/resources/text_paragraph.*             @godotengine/gui-nodes
-/scene/resources/visual_shader*.*             @godotengine/shaders
+/scene/resources/2d/                          @godotengine/2d-nodes
+/scene/resources/3d/                          @godotengine/3d-nodes
+/scene/resources/animated*                    @godotengine/animation
+/scene/resources/animation*                   @godotengine/animation
+/scene/resources/audio*                       @godotengine/audio
+/scene/resources/font*                        @godotengine/gui-nodes
+/scene/resources/shader*                      @godotengine/shaders
+/scene/resources/text_*                       @godotengine/gui-nodes
+/scene/resources/visual_shader*               @godotengine/shaders
 /scene/theme/                                 @godotengine/gui-nodes
 /scene/theme/icons/                           @godotengine/gui-nodes @godotengine/usability
 
 # Servers
 
-/servers/                                     @godotengine/_systems
 /servers/**/audio_*                           @godotengine/audio
 /servers/**/camera_*                          @godotengine/xr
 /servers/**/debugger_*                        @godotengine/debugger
-/servers/**/display_*                         @godotengine/_platforms
 /servers/**/navigation_*                      @godotengine/navigation
 /servers/**/physics_*                         @godotengine/physics
 /servers/**/rendering_*                       @godotengine/rendering
@@ -238,7 +230,6 @@
 /servers/audio/                               @godotengine/audio
 /servers/camera/                              @godotengine/xr
 /servers/debugger/                            @godotengine/debugger
-/servers/display/                             @godotengine/_platforms
 /servers/navigation/                          @godotengine/navigation
 /servers/rendering/                           @godotengine/rendering
 /servers/text/                                @godotengine/gui-nodes
@@ -254,6 +245,7 @@
 
 # Buildsystem (After everything to catch all)
 
+/*.*                                          @godotengine/buildsystem
 *.py                                          @godotengine/buildsystem
 SConstruct                                    @godotengine/buildsystem
 SCsub                                         @godotengine/buildsystem

From 133471afb0899b96f190a326d0406685fb6f804a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Pa=CC=84vels=20Nadtoc=CC=8Cajevs?=
 <7645683+bruvzg@users.noreply.github.com>
Date: Wed, 11 Dec 2024 21:05:14 +0200
Subject: [PATCH 081/124] [Shader Language] Add missing token name.

---
 servers/rendering/shader_language.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/servers/rendering/shader_language.cpp b/servers/rendering/shader_language.cpp
index 764d420a23..5b0f418ee1 100644
--- a/servers/rendering/shader_language.cpp
+++ b/servers/rendering/shader_language.cpp
@@ -128,6 +128,7 @@ const char *ShaderLanguage::token_names[TK_MAX] = {
 	"TYPE_USAMPLER3D",
 	"TYPE_SAMPLERCUBE",
 	"TYPE_SAMPLERCUBEARRAY",
+	"TYPE_SAMPLEREXT",
 	"INTERPOLATION_FLAT",
 	"INTERPOLATION_SMOOTH",
 	"CONST",

From 054891de044db606ad0491d17c0c3ba6d495bc89 Mon Sep 17 00:00:00 2001
From: Dario <dariosamo@gmail.com>
Date: Fri, 6 Dec 2024 15:10:09 -0300
Subject: [PATCH 082/124] Implement buffer_get_data_async and
 texture_get_data_async.

---
 core/config/project_settings.cpp       |   1 +
 doc/classes/ProjectSettings.xml        |  12 +
 doc/classes/RenderingDevice.xml        |  41 +++
 servers/rendering/rendering_device.cpp | 456 +++++++++++++++++++++----
 servers/rendering/rendering_device.h   |  65 +++-
 5 files changed, 490 insertions(+), 85 deletions(-)

diff --git a/core/config/project_settings.cpp b/core/config/project_settings.cpp
index a8cdb6f737..30cdd789a4 100644
--- a/core/config/project_settings.cpp
+++ b/core/config/project_settings.cpp
@@ -1563,6 +1563,7 @@ ProjectSettings::ProjectSettings() {
 	GLOBAL_DEF(PropertyInfo(Variant::INT, "rendering/rendering_device/staging_buffer/block_size_kb", PROPERTY_HINT_RANGE, "4,2048,1,or_greater"), 256);
 	GLOBAL_DEF(PropertyInfo(Variant::INT, "rendering/rendering_device/staging_buffer/max_size_mb", PROPERTY_HINT_RANGE, "1,1024,1,or_greater"), 128);
 	GLOBAL_DEF(PropertyInfo(Variant::INT, "rendering/rendering_device/staging_buffer/texture_upload_region_size_px", PROPERTY_HINT_RANGE, "1,256,1,or_greater"), 64);
+	GLOBAL_DEF(PropertyInfo(Variant::INT, "rendering/rendering_device/staging_buffer/texture_download_region_size_px", PROPERTY_HINT_RANGE, "1,256,1,or_greater"), 64);
 	GLOBAL_DEF_RST(PropertyInfo(Variant::BOOL, "rendering/rendering_device/pipeline_cache/enable"), true);
 	GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "rendering/rendering_device/pipeline_cache/save_chunk_size_mb", PROPERTY_HINT_RANGE, "0.000001,64.0,0.001,or_greater"), 3.0);
 	GLOBAL_DEF(PropertyInfo(Variant::INT, "rendering/rendering_device/vulkan/max_descriptors_per_pool", PROPERTY_HINT_RANGE, "1,256,1,or_greater"), 64);
diff --git a/doc/classes/ProjectSettings.xml b/doc/classes/ProjectSettings.xml
index b4fb8fe035..e2aa090278 100644
--- a/doc/classes/ProjectSettings.xml
+++ b/doc/classes/ProjectSettings.xml
@@ -2841,10 +2841,22 @@
 			Determines at which interval pipeline cache is saved to disk. The lower the value, the more often it is saved.
 		</member>
 		<member name="rendering/rendering_device/staging_buffer/block_size_kb" type="int" setter="" getter="" default="256">
+			The size of a block allocated in the staging buffers. Staging buffers are the intermediate resources the engine uses to upload or download data to the GPU. This setting determines the max amount of data that can be transferred in a copy operation. Increasing this will result in faster data transfers at the cost of extra memory.
+			[b]Note:[/b] This property is only read when the project starts. There is currently no way to change this value at run-time.
 		</member>
 		<member name="rendering/rendering_device/staging_buffer/max_size_mb" type="int" setter="" getter="" default="128">
+			The maximum amount of memory allowed to be used by staging buffers. If the amount of data being uploaded or downloaded exceeds this amount, the GPU will stall and wait for previous frames to finish.
+			[b]Note:[/b] This property is only read when the project starts. There is currently no way to change this value at run-time.
+		</member>
+		<member name="rendering/rendering_device/staging_buffer/texture_download_region_size_px" type="int" setter="" getter="" default="64">
+			The region size in pixels used to download texture data from the GPU when using methods like [method RenderingDevice.texture_get_data_async].
+			[b]Note:[/b] This property's upper limit is controlled by [member rendering/rendering_device/staging_buffer/block_size_kb] and whether it's possible to allocate a single block of texture data with this region size in the format that is requested.
+			[b]Note:[/b] This property is only read when the project starts. There is currently no way to change this value at run-time.
 		</member>
 		<member name="rendering/rendering_device/staging_buffer/texture_upload_region_size_px" type="int" setter="" getter="" default="64">
+			The region size in pixels used to upload texture data from the GPU when using methods like [method RenderingDevice.texture_update].
+			[b]Note:[/b] This property's upper limit is controlled by [member rendering/rendering_device/staging_buffer/block_size_kb] and whether it's possible to allocate a single block of texture data with this region size in the format that is requested.
+			[b]Note:[/b] This property is only read when the project starts. There is currently no way to change this value at run-time.
 		</member>
 		<member name="rendering/rendering_device/vsync/frame_queue_size" type="int" setter="" getter="" default="2">
 			The number of frames to track on the CPU side before stalling to wait for the GPU.
diff --git a/doc/classes/RenderingDevice.xml b/doc/classes/RenderingDevice.xml
index 59ca06085f..b7f95587cd 100644
--- a/doc/classes/RenderingDevice.xml
+++ b/doc/classes/RenderingDevice.xml
@@ -58,6 +58,27 @@
 			<param index="2" name="size_bytes" type="int" default="0" />
 			<description>
 				Returns a copy of the data of the specified [param buffer], optionally [param offset_bytes] and [param size_bytes] can be set to copy only a portion of the buffer.
+				[b]Note:[/b] This method will block the GPU from working until the data is retrieved. Refer to [method buffer_get_data_async] for an alternative that returns the data in more performant way.
+			</description>
+		</method>
+		<method name="buffer_get_data_async">
+			<return type="int" enum="Error" />
+			<param index="0" name="buffer" type="RID" />
+			<param index="1" name="callback" type="Callable" />
+			<param index="2" name="offset_bytes" type="int" default="0" />
+			<param index="3" name="size_bytes" type="int" default="0" />
+			<description>
+				Asynchronous version of [method buffer_get_data]. RenderingDevice will call [param callback] in a certain amount of frames with the data the buffer had at the time of the request.
+				[b]Note:[/b] At the moment, the delay corresponds to the amount of frames specified by [member ProjectSettings.rendering/rendering_device/vsync/frame_queue_size].
+				[b]Note:[/b] Downloading large buffers can have a prohibitive cost for real-time even when using the asynchronous method due to hardware bandwidth limitations. When dealing with large resources, you can adjust settings such as [member ProjectSettings.rendering/rendering_device/staging_buffer/block_size_kb] to improve the transfer speed at the cost of extra memory.
+				[codeblock]
+				func _buffer_get_data_callback(array):
+				    value = array.decode_u32(0)
+
+				...
+
+				rd.buffer_get_data_async(buffer, _buffer_get_data_callback)
+				[/codeblock]
 			</description>
 		</method>
 		<method name="buffer_update">
@@ -928,6 +949,26 @@
 				Returns the [param texture] data for the specified [param layer] as raw binary data. For 2D textures (which only have one layer), [param layer] must be [code]0[/code].
 				[b]Note:[/b] [param texture] can't be retrieved while a draw list that uses it as part of a framebuffer is being created. Ensure the draw list is finalized (and that the color/depth texture using it is not set to [constant FINAL_ACTION_CONTINUE]) to retrieve this texture. Otherwise, an error is printed and a empty [PackedByteArray] is returned.
 				[b]Note:[/b] [param texture] requires the [constant TEXTURE_USAGE_CAN_COPY_FROM_BIT] to be retrieved. Otherwise, an error is printed and a empty [PackedByteArray] is returned.
+				[b]Note:[/b] This method will block the GPU from working until the data is retrieved. Refer to [method texture_get_data_async] for an alternative that returns the data in more performant way.
+			</description>
+		</method>
+		<method name="texture_get_data_async">
+			<return type="int" enum="Error" />
+			<param index="0" name="texture" type="RID" />
+			<param index="1" name="layer" type="int" />
+			<param index="2" name="callback" type="Callable" />
+			<description>
+				Asynchronous version of [method texture_get_data]. RenderingDevice will call [param callback] in a certain amount of frames with the data the texture had at the time of the request.
+				[b]Note:[/b] At the moment, the delay corresponds to the amount of frames specified by [member ProjectSettings.rendering/rendering_device/vsync/frame_queue_size].
+				[b]Note:[/b] Downloading large textures can have a prohibitive cost for real-time even when using the asynchronous method due to hardware bandwidth limitations. When dealing with large resources, you can adjust settings such as [member ProjectSettings.rendering/rendering_device/staging_buffer/texture_download_region_size_px] and [member ProjectSettings.rendering/rendering_device/staging_buffer/block_size_kb] to improve the transfer speed at the cost of extra memory.
+				[codeblock]
+				func _texture_get_data_callback(array):
+				    value = array.decode_u32(0)
+
+				...
+
+				rd.texture_get_data_async(texture, 0, _texture_get_data_callback)
+				[/codeblock]
 			</description>
 		</method>
 		<method name="texture_get_format">
diff --git a/servers/rendering/rendering_device.cpp b/servers/rendering/rendering_device.cpp
index 2fbafcbda3..78e30f3dea 100644
--- a/servers/rendering/rendering_device.cpp
+++ b/servers/rendering/rendering_device.cpp
@@ -282,20 +282,20 @@ Error RenderingDevice::_buffer_initialize(Buffer *p_buffer, const uint8_t *p_dat
 	return OK;
 }
 
-Error RenderingDevice::_insert_staging_block() {
+Error RenderingDevice::_insert_staging_block(StagingBuffers &p_staging_buffers) {
 	StagingBufferBlock block;
 
-	block.driver_id = driver->buffer_create(staging_buffer_block_size, RDD::BUFFER_USAGE_TRANSFER_FROM_BIT, RDD::MEMORY_ALLOCATION_TYPE_CPU);
+	block.driver_id = driver->buffer_create(p_staging_buffers.block_size, p_staging_buffers.usage_bits, RDD::MEMORY_ALLOCATION_TYPE_CPU);
 	ERR_FAIL_COND_V(!block.driver_id, ERR_CANT_CREATE);
 
 	block.frame_used = 0;
 	block.fill_amount = 0;
 
-	staging_buffer_blocks.insert(staging_buffer_current, block);
+	p_staging_buffers.blocks.insert(p_staging_buffers.current, block);
 	return OK;
 }
 
-Error RenderingDevice::_staging_buffer_allocate(uint32_t p_amount, uint32_t p_required_align, uint32_t &r_alloc_offset, uint32_t &r_alloc_size, StagingRequiredAction &r_required_action, bool p_can_segment) {
+Error RenderingDevice::_staging_buffer_allocate(StagingBuffers &p_staging_buffers, uint32_t p_amount, uint32_t p_required_align, uint32_t &r_alloc_offset, uint32_t &r_alloc_size, StagingRequiredAction &r_required_action, bool p_can_segment) {
 	// Determine a block to use.
 
 	r_alloc_size = p_amount;
@@ -305,10 +305,10 @@ Error RenderingDevice::_staging_buffer_allocate(uint32_t p_amount, uint32_t p_re
 		r_alloc_offset = 0;
 
 		// See if we can use current block.
-		if (staging_buffer_blocks[staging_buffer_current].frame_used == frames_drawn) {
+		if (p_staging_buffers.blocks[p_staging_buffers.current].frame_used == frames_drawn) {
 			// We used this block this frame, let's see if there is still room.
 
-			uint32_t write_from = staging_buffer_blocks[staging_buffer_current].fill_amount;
+			uint32_t write_from = p_staging_buffers.blocks[p_staging_buffers.current].fill_amount;
 
 			{
 				uint32_t align_remainder = write_from % p_required_align;
@@ -317,7 +317,7 @@ Error RenderingDevice::_staging_buffer_allocate(uint32_t p_amount, uint32_t p_re
 				}
 			}
 
-			int32_t available_bytes = int32_t(staging_buffer_block_size) - int32_t(write_from);
+			int32_t available_bytes = int32_t(p_staging_buffers.block_size) - int32_t(write_from);
 
 			if ((int32_t)p_amount < available_bytes) {
 				// All is good, we should be ok, all will fit.
@@ -332,20 +332,20 @@ Error RenderingDevice::_staging_buffer_allocate(uint32_t p_amount, uint32_t p_re
 				// Can't fit it into this buffer.
 				// Will need to try next buffer.
 
-				staging_buffer_current = (staging_buffer_current + 1) % staging_buffer_blocks.size();
+				p_staging_buffers.current = (p_staging_buffers.current + 1) % p_staging_buffers.blocks.size();
 
 				// Before doing anything, though, let's check that we didn't manage to fill all blocks.
 				// Possible in a single frame.
-				if (staging_buffer_blocks[staging_buffer_current].frame_used == frames_drawn) {
+				if (p_staging_buffers.blocks[p_staging_buffers.current].frame_used == frames_drawn) {
 					// Guess we did.. ok, let's see if we can insert a new block.
-					if ((uint64_t)staging_buffer_blocks.size() * staging_buffer_block_size < staging_buffer_max_size) {
+					if ((uint64_t)p_staging_buffers.blocks.size() * p_staging_buffers.block_size < p_staging_buffers.max_size) {
 						// We can, so we are safe.
-						Error err = _insert_staging_block();
+						Error err = _insert_staging_block(p_staging_buffers);
 						if (err) {
 							return err;
 						}
 						// Claim for this frame.
-						staging_buffer_blocks.write[staging_buffer_current].frame_used = frames_drawn;
+						p_staging_buffers.blocks.write[p_staging_buffers.current].frame_used = frames_drawn;
 					} else {
 						// Ok, worst case scenario, all the staging buffers belong to this frame
 						// and this frame is not even done.
@@ -360,20 +360,20 @@ Error RenderingDevice::_staging_buffer_allocate(uint32_t p_amount, uint32_t p_re
 				}
 			}
 
-		} else if (staging_buffer_blocks[staging_buffer_current].frame_used <= frames_drawn - frames.size()) {
+		} else if (p_staging_buffers.blocks[p_staging_buffers.current].frame_used <= frames_drawn - frames.size()) {
 			// This is an old block, which was already processed, let's reuse.
-			staging_buffer_blocks.write[staging_buffer_current].frame_used = frames_drawn;
-			staging_buffer_blocks.write[staging_buffer_current].fill_amount = 0;
+			p_staging_buffers.blocks.write[p_staging_buffers.current].frame_used = frames_drawn;
+			p_staging_buffers.blocks.write[p_staging_buffers.current].fill_amount = 0;
 		} else {
 			// This block may still be in use, let's not touch it unless we have to, so.. can we create a new one?
-			if ((uint64_t)staging_buffer_blocks.size() * staging_buffer_block_size < staging_buffer_max_size) {
+			if ((uint64_t)p_staging_buffers.blocks.size() * p_staging_buffers.block_size < p_staging_buffers.max_size) {
 				// We are still allowed to create a new block, so let's do that and insert it for current pos.
-				Error err = _insert_staging_block();
+				Error err = _insert_staging_block(p_staging_buffers);
 				if (err) {
 					return err;
 				}
 				// Claim for this frame.
-				staging_buffer_blocks.write[staging_buffer_current].frame_used = frames_drawn;
+				p_staging_buffers.blocks.write[p_staging_buffers.current].frame_used = frames_drawn;
 			} else {
 				// Oops, we are out of room and we can't create more.
 				// Let's flush older frames.
@@ -387,12 +387,12 @@ Error RenderingDevice::_staging_buffer_allocate(uint32_t p_amount, uint32_t p_re
 		break;
 	}
 
-	staging_buffer_used = true;
+	p_staging_buffers.used = true;
 
 	return OK;
 }
 
-void RenderingDevice::_staging_buffer_execute_required_action(StagingRequiredAction p_required_action) {
+void RenderingDevice::_staging_buffer_execute_required_action(StagingBuffers &p_staging_buffers, StagingRequiredAction p_required_action) {
 	switch (p_required_action) {
 		case STAGING_REQUIRED_ACTION_NONE: {
 			// Do nothing.
@@ -401,30 +401,30 @@ void RenderingDevice::_staging_buffer_execute_required_action(StagingRequiredAct
 			_flush_and_stall_for_all_frames();
 
 			// Clear the whole staging buffer.
-			for (int i = 0; i < staging_buffer_blocks.size(); i++) {
-				staging_buffer_blocks.write[i].frame_used = 0;
-				staging_buffer_blocks.write[i].fill_amount = 0;
+			for (int i = 0; i < p_staging_buffers.blocks.size(); i++) {
+				p_staging_buffers.blocks.write[i].frame_used = 0;
+				p_staging_buffers.blocks.write[i].fill_amount = 0;
 			}
 
 			// Claim for current frame.
-			staging_buffer_blocks.write[staging_buffer_current].frame_used = frames_drawn;
+			p_staging_buffers.blocks.write[p_staging_buffers.current].frame_used = frames_drawn;
 		} break;
 		case STAGING_REQUIRED_ACTION_STALL_PREVIOUS: {
 			_stall_for_previous_frames();
 
-			for (int i = 0; i < staging_buffer_blocks.size(); i++) {
+			for (int i = 0; i < p_staging_buffers.blocks.size(); i++) {
 				// Clear all blocks but the ones from this frame.
-				int block_idx = (i + staging_buffer_current) % staging_buffer_blocks.size();
-				if (staging_buffer_blocks[block_idx].frame_used == frames_drawn) {
+				int block_idx = (i + p_staging_buffers.current) % p_staging_buffers.blocks.size();
+				if (p_staging_buffers.blocks[block_idx].frame_used == frames_drawn) {
 					break; // Ok, we reached something from this frame, abort.
 				}
 
-				staging_buffer_blocks.write[block_idx].frame_used = 0;
-				staging_buffer_blocks.write[block_idx].fill_amount = 0;
+				p_staging_buffers.blocks.write[block_idx].frame_used = 0;
+				p_staging_buffers.blocks.write[block_idx].fill_amount = 0;
 			}
 
 			// Claim for current frame.
-			staging_buffer_blocks.write[staging_buffer_current].frame_used = frames_drawn;
+			p_staging_buffers.blocks.write[p_staging_buffers.current].frame_used = frames_drawn;
 		} break;
 		default: {
 			DEV_ASSERT(false && "Unknown required action.");
@@ -503,7 +503,7 @@ Error RenderingDevice::buffer_update(RID p_buffer, uint32_t p_offset, uint32_t p
 		uint32_t block_write_amount;
 		StagingRequiredAction required_action;
 
-		Error err = _staging_buffer_allocate(MIN(to_submit, staging_buffer_block_size), required_align, block_write_offset, block_write_amount, required_action);
+		Error err = _staging_buffer_allocate(upload_staging_buffers, MIN(to_submit, upload_staging_buffers.block_size), required_align, block_write_offset, block_write_amount, required_action);
 		if (err) {
 			return err;
 		}
@@ -518,17 +518,17 @@ Error RenderingDevice::buffer_update(RID p_buffer, uint32_t p_offset, uint32_t p
 			command_buffer_copies_vector.clear();
 		}
 
-		_staging_buffer_execute_required_action(required_action);
+		_staging_buffer_execute_required_action(upload_staging_buffers, required_action);
 
 		// Map staging buffer (It's CPU and coherent).
-		uint8_t *data_ptr = driver->buffer_map(staging_buffer_blocks[staging_buffer_current].driver_id);
+		uint8_t *data_ptr = driver->buffer_map(upload_staging_buffers.blocks[upload_staging_buffers.current].driver_id);
 		ERR_FAIL_NULL_V(data_ptr, ERR_CANT_CREATE);
 
 		// Copy to staging buffer.
 		memcpy(data_ptr + block_write_offset, src_data + submit_from, block_write_amount);
 
 		// Unmap.
-		driver->buffer_unmap(staging_buffer_blocks[staging_buffer_current].driver_id);
+		driver->buffer_unmap(upload_staging_buffers.blocks[upload_staging_buffers.current].driver_id);
 
 		// Insert a command to copy this.
 		RDD::BufferCopyRegion region;
@@ -537,11 +537,11 @@ Error RenderingDevice::buffer_update(RID p_buffer, uint32_t p_offset, uint32_t p
 		region.size = block_write_amount;
 
 		RDG::RecordedBufferCopy buffer_copy;
-		buffer_copy.source = staging_buffer_blocks[staging_buffer_current].driver_id;
+		buffer_copy.source = upload_staging_buffers.blocks[upload_staging_buffers.current].driver_id;
 		buffer_copy.region = region;
 		command_buffer_copies_vector.push_back(buffer_copy);
 
-		staging_buffer_blocks.write[staging_buffer_current].fill_amount = block_write_offset + block_write_amount;
+		upload_staging_buffers.blocks.write[upload_staging_buffers.current].fill_amount = block_write_offset + block_write_amount;
 
 		to_submit -= block_write_amount;
 		submit_from += block_write_amount;
@@ -611,7 +611,7 @@ Vector<uint8_t> RenderingDevice::buffer_get_data(RID p_buffer, uint32_t p_offset
 
 	Buffer *buffer = _get_buffer_from_owner(p_buffer);
 	if (!buffer) {
-		ERR_FAIL_V_MSG(Vector<uint8_t>(), "Buffer is either invalid or this type of buffer can't be retrieved. Only Index and Vertex buffers allow retrieving.");
+		ERR_FAIL_V_MSG(Vector<uint8_t>(), "Buffer is either invalid or this type of buffer can't be retrieved.");
 	}
 
 	// Size of buffer to retrieve.
@@ -653,6 +653,89 @@ Vector<uint8_t> RenderingDevice::buffer_get_data(RID p_buffer, uint32_t p_offset
 	return buffer_data;
 }
 
+Error RenderingDevice::buffer_get_data_async(RID p_buffer, const Callable &p_callback, uint32_t p_offset, uint32_t p_size) {
+	ERR_RENDER_THREAD_GUARD_V(ERR_UNAVAILABLE);
+
+	Buffer *buffer = _get_buffer_from_owner(p_buffer);
+	if (buffer == nullptr) {
+		ERR_FAIL_V_MSG(ERR_INVALID_PARAMETER, "Buffer is either invalid or this type of buffer can't be retrieved.");
+	}
+
+	if (p_size == 0) {
+		p_size = buffer->size;
+	}
+
+	ERR_FAIL_COND_V_MSG(p_size + p_offset > buffer->size, ERR_INVALID_PARAMETER, "Size is larger than the buffer.");
+	ERR_FAIL_COND_V_MSG(!p_callback.is_valid(), ERR_INVALID_PARAMETER, "Callback must be valid.");
+
+	_check_transfer_worker_buffer(buffer);
+
+	BufferGetDataRequest get_data_request;
+	uint32_t flushed_copies = 0;
+	get_data_request.callback = p_callback;
+	get_data_request.frame_local_index = frames[frame].download_buffer_copy_regions.size();
+	get_data_request.size = p_size;
+
+	const uint32_t required_align = 32;
+	uint32_t block_write_offset;
+	uint32_t block_write_amount;
+	StagingRequiredAction required_action;
+	uint32_t to_submit = p_size;
+	uint32_t submit_from = 0;
+	while (to_submit > 0) {
+		Error err = _staging_buffer_allocate(download_staging_buffers, MIN(to_submit, download_staging_buffers.block_size), required_align, block_write_offset, block_write_amount, required_action);
+		if (err) {
+			return err;
+		}
+
+		if ((get_data_request.frame_local_count > 0) && required_action == STAGING_REQUIRED_ACTION_FLUSH_AND_STALL_ALL) {
+			if (_buffer_make_mutable(buffer, p_buffer)) {
+				// The buffer must be mutable to be used as a copy source.
+				draw_graph.add_synchronization();
+			}
+
+			for (uint32_t i = flushed_copies; i < get_data_request.frame_local_count; i++) {
+				uint32_t local_index = get_data_request.frame_local_index + i;
+				draw_graph.add_buffer_get_data(buffer->driver_id, buffer->draw_tracker, frames[frame].download_buffer_staging_buffers[local_index], frames[frame].download_buffer_copy_regions[local_index]);
+			}
+
+			flushed_copies = get_data_request.frame_local_count;
+		}
+
+		_staging_buffer_execute_required_action(download_staging_buffers, required_action);
+
+		RDD::BufferCopyRegion region;
+		region.src_offset = submit_from + p_offset;
+		region.dst_offset = block_write_offset;
+		region.size = block_write_amount;
+
+		frames[frame].download_buffer_staging_buffers.push_back(download_staging_buffers.blocks[download_staging_buffers.current].driver_id);
+		frames[frame].download_buffer_copy_regions.push_back(region);
+		get_data_request.frame_local_count++;
+
+		download_staging_buffers.blocks.write[download_staging_buffers.current].fill_amount = block_write_offset + block_write_amount;
+
+		to_submit -= block_write_amount;
+		submit_from += block_write_amount;
+	}
+
+	if (get_data_request.frame_local_count > 0) {
+		if (_buffer_make_mutable(buffer, p_buffer)) {
+			// The buffer must be mutable to be used as a copy source.
+			draw_graph.add_synchronization();
+		}
+
+		for (uint32_t i = flushed_copies; i < get_data_request.frame_local_count; i++) {
+			uint32_t local_index = get_data_request.frame_local_index + i;
+			draw_graph.add_buffer_get_data(buffer->driver_id, buffer->draw_tracker, frames[frame].download_buffer_staging_buffers[local_index], frames[frame].download_buffer_copy_regions[local_index]);
+		}
+
+		frames[frame].download_buffer_get_data_requests.push_back(get_data_request);
+	}
+
+	return OK;
+}
+
 RID RenderingDevice::storage_buffer_create(uint32_t p_size_bytes, const Vector<uint8_t> &p_data, BitField<StorageBufferUsage> p_usage) {
 	ERR_FAIL_COND_V(p_data.size() && (uint32_t)p_data.size() != p_size_bytes, RID());
 
@@ -1461,7 +1544,7 @@ Error RenderingDevice::texture_update(RID p_texture, uint32_t p_layer, const Vec
 					uint32_t to_allocate = region_pitch * region_h;
 					uint32_t alloc_offset = 0, alloc_size = 0;
 					StagingRequiredAction required_action;
-					Error err = _staging_buffer_allocate(to_allocate, required_align, alloc_offset, alloc_size, required_action, false);
+					Error err = _staging_buffer_allocate(upload_staging_buffers, to_allocate, required_align, alloc_offset, alloc_size, required_action, false);
 					ERR_FAIL_COND_V(err, ERR_CANT_CREATE);
 
 					if (!command_buffer_to_texture_copies_vector.is_empty() && required_action == STAGING_REQUIRED_ACTION_FLUSH_AND_STALL_ALL) {
@@ -1475,12 +1558,12 @@ Error RenderingDevice::texture_update(RID p_texture, uint32_t p_layer, const Vec
 						command_buffer_to_texture_copies_vector.clear();
 					}
 
-					_staging_buffer_execute_required_action(required_action);
+					_staging_buffer_execute_required_action(upload_staging_buffers, required_action);
 
 					uint8_t *write_ptr;
 
 					{ // Map.
-						uint8_t *data_ptr = driver->buffer_map(staging_buffer_blocks[staging_buffer_current].driver_id);
+						uint8_t *data_ptr = driver->buffer_map(upload_staging_buffers.blocks[upload_staging_buffers.current].driver_id);
 						ERR_FAIL_NULL_V(data_ptr, ERR_CANT_CREATE);
 						write_ptr = data_ptr;
 						write_ptr += alloc_offset;
@@ -1492,7 +1575,7 @@ Error RenderingDevice::texture_update(RID p_texture, uint32_t p_layer, const Vec
 					_copy_region_block_or_regular(read_ptr_mipmap_layer, write_ptr, x, y, width, region_w, region_h, block_w, block_h, region_pitch, pixel_size, block_size);
 
 					{ // Unmap.
-						driver->buffer_unmap(staging_buffer_blocks[staging_buffer_current].driver_id);
+						driver->buffer_unmap(upload_staging_buffers.blocks[upload_staging_buffers.current].driver_id);
 					}
 
 					RDD::BufferTextureCopyRegion copy_region;
@@ -1505,11 +1588,11 @@ Error RenderingDevice::texture_update(RID p_texture, uint32_t p_layer, const Vec
 					copy_region.texture_region_size = Vector3i(region_logic_w, region_logic_h, 1);
 
 					RDG::RecordedBufferToTextureCopy buffer_to_texture_copy;
-					buffer_to_texture_copy.from_buffer = staging_buffer_blocks[staging_buffer_current].driver_id;
+					buffer_to_texture_copy.from_buffer = upload_staging_buffers.blocks[upload_staging_buffers.current].driver_id;
 					buffer_to_texture_copy.region = copy_region;
 					command_buffer_to_texture_copies_vector.push_back(buffer_to_texture_copy);
 
-					staging_buffer_blocks.write[staging_buffer_current].fill_amount = alloc_offset + alloc_size;
+					upload_staging_buffers.blocks.write[upload_staging_buffers.current].fill_amount = alloc_offset + alloc_size;
 				}
 			}
 		}
@@ -1890,6 +1973,131 @@ Vector<uint8_t> RenderingDevice::texture_get_data(RID p_texture, uint32_t p_laye
 	}
 }
 
+Error RenderingDevice::texture_get_data_async(RID p_texture, uint32_t p_layer, const Callable &p_callback) {
+	ERR_RENDER_THREAD_GUARD_V(ERR_UNAVAILABLE);
+
+	Texture *tex = texture_owner.get_or_null(p_texture);
+	ERR_FAIL_NULL_V(tex, ERR_INVALID_PARAMETER);
+
+	ERR_FAIL_COND_V_MSG(tex->bound, ERR_INVALID_PARAMETER, "Texture can't be retrieved while a draw list that uses it as part of a framebuffer is being created. Ensure the draw list is finalized (and that the color/depth texture using it is not set to `RenderingDevice.FINAL_ACTION_CONTINUE`) to retrieve this texture.");
+	ERR_FAIL_COND_V_MSG(!(tex->usage_flags & TEXTURE_USAGE_CAN_COPY_FROM_BIT), ERR_INVALID_PARAMETER, "Texture requires the `RenderingDevice.TEXTURE_USAGE_CAN_COPY_FROM_BIT` to be set to be retrieved.");
+	ERR_FAIL_COND_V(p_layer >= tex->layers, ERR_INVALID_PARAMETER);
+
+	_check_transfer_worker_texture(tex);
+
+	thread_local LocalVector<RDD::TextureCopyableLayout> mip_layouts;
+	mip_layouts.resize(tex->mipmaps);
+	for (uint32_t i = 0; i < tex->mipmaps; i++) {
+		RDD::TextureSubresource subres;
+		subres.aspect = RDD::TEXTURE_ASPECT_COLOR;
+		subres.layer = p_layer;
+		subres.mipmap = i;
+		driver->texture_get_copyable_layout(tex->driver_id, subres, &mip_layouts[i]);
+
+		// Assuming layers are tightly packed. If this is not true on some driver, we must modify the copy algorithm.
+		DEV_ASSERT(mip_layouts[i].layer_pitch == mip_layouts[i].size / tex->layers);
+	}
+
+	ERR_FAIL_COND_V(mip_layouts.is_empty(), ERR_INVALID_PARAMETER);
+
+	if (_texture_make_mutable(tex, p_texture)) {
+		// The texture must be mutable to be used as a copy source due to layout transitions.
+		draw_graph.add_synchronization();
+	}
+
+	TextureGetDataRequest get_data_request;
+	get_data_request.callback = p_callback;
+	get_data_request.frame_local_index = frames[frame].download_buffer_texture_copy_regions.size();
+	get_data_request.width = tex->width;
+	get_data_request.height = tex->height;
+	get_data_request.depth = tex->depth;
+	get_data_request.format = tex->format;
+	get_data_request.mipmaps = tex->mipmaps;
+
+	uint32_t block_w, block_h;
+	get_compressed_image_format_block_dimensions(tex->format, block_w, block_h);
+
+	uint32_t pixel_size = get_image_format_pixel_size(tex->format);
+	uint32_t pixel_rshift = get_compressed_image_format_pixel_rshift(tex->format);
+
+	uint32_t w, h, d;
+	uint32_t required_align = driver->api_trait_get(RDD::API_TRAIT_TEXTURE_TRANSFER_ALIGNMENT);
+	uint32_t pitch_step = driver->api_trait_get(RDD::API_TRAIT_TEXTURE_DATA_ROW_PITCH_STEP);
+	uint32_t region_size = texture_download_region_size_px;
+	uint32_t logic_w = tex->width;
+	uint32_t logic_h = tex->height;
+	uint32_t mipmap_offset = 0;
+	uint32_t block_write_offset;
+	uint32_t block_write_amount;
+	StagingRequiredAction required_action;
+	uint32_t flushed_copies = 0;
+	for (uint32_t i = 0; i < tex->mipmaps; i++) {
+		uint32_t image_total = get_image_format_required_size(tex->format, tex->width, tex->height, tex->depth, i + 1, &w, &h, &d);
+		uint32_t tight_mip_size = image_total - mipmap_offset;
+		for (uint32_t z = 0; z < d; z++) {
+			for (uint32_t y = 0; y < h; y += region_size) {
+				for (uint32_t x = 0; x < w; x += region_size) {
+					uint32_t region_w = MIN(region_size, w - x);
+					uint32_t region_h = MIN(region_size, h - y);
+					ERR_FAIL_COND_V(region_w % block_w, ERR_BUG);
+					ERR_FAIL_COND_V(region_h % block_h, ERR_BUG);
+
+					uint32_t region_logic_w = MIN(region_size, logic_w - x);
+					uint32_t region_logic_h = MIN(region_size, logic_h - y);
+					uint32_t region_pitch = (region_w * pixel_size * block_w) >> pixel_rshift;
+					region_pitch = STEPIFY(region_pitch, pitch_step);
+
+					uint32_t to_allocate = region_pitch * region_h;
+					Error err = _staging_buffer_allocate(download_staging_buffers, to_allocate, required_align, block_write_offset, block_write_amount, required_action, false);
+					ERR_FAIL_COND_V(err, ERR_CANT_CREATE);
+
+					if ((get_data_request.frame_local_count > 0) && required_action == STAGING_REQUIRED_ACTION_FLUSH_AND_STALL_ALL) {
+						for (uint32_t j = flushed_copies; j < get_data_request.frame_local_count; j++) {
+							uint32_t local_index = get_data_request.frame_local_index + j;
+							draw_graph.add_texture_get_data(tex->driver_id, tex->draw_tracker, frames[frame].download_texture_staging_buffers[local_index], frames[frame].download_buffer_texture_copy_regions[local_index]);
+						}
+
+						flushed_copies = get_data_request.frame_local_count;
+					}
+
+					_staging_buffer_execute_required_action(download_staging_buffers, required_action);
+
+					RDD::BufferTextureCopyRegion copy_region;
+					copy_region.buffer_offset = block_write_offset;
+					copy_region.texture_subresources.aspect = tex->read_aspect_flags;
+					copy_region.texture_subresources.mipmap = i;
+					copy_region.texture_subresources.base_layer = p_layer;
+					copy_region.texture_subresources.layer_count = 1;
+					copy_region.texture_offset = Vector3i(x, y, z);
+					copy_region.texture_region_size = Vector3i(region_logic_w, region_logic_h, 1);
+					frames[frame].download_texture_staging_buffers.push_back(download_staging_buffers.blocks[download_staging_buffers.current].driver_id);
+					frames[frame].download_buffer_texture_copy_regions.push_back(copy_region);
+					frames[frame].download_texture_mipmap_offsets.push_back(mipmap_offset + (tight_mip_size / d) * z);
+					get_data_request.frame_local_count++;
+
+					download_staging_buffers.blocks.write[download_staging_buffers.current].fill_amount = block_write_offset + block_write_amount;
+				}
+			}
+		}
+
+		mipmap_offset = image_total;
+		logic_w = MAX(1u, logic_w >> 1);
+		logic_h = MAX(1u, logic_h >> 1);
+	}
+
+	if (get_data_request.frame_local_count > 0) {
+		for (uint32_t i = flushed_copies; i < get_data_request.frame_local_count; i++) {
+			uint32_t local_index = get_data_request.frame_local_index + i;
+			draw_graph.add_texture_get_data(tex->driver_id, tex->draw_tracker, frames[frame].download_texture_staging_buffers[local_index], frames[frame].download_buffer_texture_copy_regions[local_index]);
+		}
+
+		flushed_copies = get_data_request.frame_local_count;
+		frames[frame].download_texture_get_data_requests.push_back(get_data_request);
+	}
+
+	return OK;
+}
+
 bool RenderingDevice::texture_is_shared(RID p_texture) {
 	ERR_RENDER_THREAD_GUARD_V(false);
 
@@ -6055,11 +6263,8 @@ uint64_t RenderingDevice::get_memory_usage(MemoryType p_type) const {
 }
 
 void RenderingDevice::_begin_frame(bool p_presented) {
-	// Before beginning this frame, wait on the fence if it was signaled to make sure its work is finished.
-	if (frames[frame].fence_signaled) {
-		driver->fence_wait(frames[frame].fence);
-		frames[frame].fence_signaled = false;
-	}
+	// Before writing to this frame, wait for it to be finished.
+	_stall_for_frame(frame);
 
 	if (command_pool_reset_enabled) {
 		bool reset = driver->command_pool_reset(frames[frame].command_pool);
@@ -6081,10 +6286,15 @@ void RenderingDevice::_begin_frame(bool p_presented) {
 	// Erase pending resources.
 	_free_pending_resources(frame);
 
-	// Advance staging buffer if used.
-	if (staging_buffer_used) {
-		staging_buffer_current = (staging_buffer_current + 1) % staging_buffer_blocks.size();
-		staging_buffer_used = false;
+	// Advance staging buffers if used.
+	if (upload_staging_buffers.used) {
+		upload_staging_buffers.current = (upload_staging_buffers.current + 1) % upload_staging_buffers.blocks.size();
+		upload_staging_buffers.used = false;
+	}
+
+	if (download_staging_buffers.used) {
+		download_staging_buffers.current = (download_staging_buffers.current + 1) % download_staging_buffers.blocks.size();
+		download_staging_buffers.used = false;
 	}
 
 	if (frames[frame].timestamp_count) {
@@ -6202,12 +6412,97 @@ void RenderingDevice::_execute_frame(bool p_present) {
 	}
 }
 
+void RenderingDevice::_stall_for_frame(uint32_t p_frame) {
+	thread_local PackedByteArray packed_byte_array;
+
+	if (frames[p_frame].fence_signaled) {
+		driver->fence_wait(frames[p_frame].fence);
+		frames[p_frame].fence_signaled = false;
+
+		// Flush any pending requests for asynchronous buffer downloads.
+		if (!frames[p_frame].download_buffer_get_data_requests.is_empty()) {
+			for (uint32_t i = 0; i < frames[p_frame].download_buffer_get_data_requests.size(); i++) {
+				const BufferGetDataRequest &request = frames[p_frame].download_buffer_get_data_requests[i];
+				packed_byte_array.resize(request.size);
+
+				uint32_t array_offset = 0;
+				for (uint32_t j = 0; j < request.frame_local_count; j++) {
+					uint32_t local_index = request.frame_local_index + j;
+					const RDD::BufferCopyRegion &region = frames[p_frame].download_buffer_copy_regions[local_index];
+					uint8_t *buffer_data = driver->buffer_map(frames[p_frame].download_buffer_staging_buffers[local_index]);
+					memcpy(&packed_byte_array.write[array_offset], &buffer_data[region.dst_offset], region.size);
+					driver->buffer_unmap(frames[p_frame].download_buffer_staging_buffers[local_index]);
+					array_offset += region.size;
+				}
+
+				request.callback.call(packed_byte_array);
+			}
+
+			frames[p_frame].download_buffer_staging_buffers.clear();
+			frames[p_frame].download_buffer_copy_regions.clear();
+			frames[p_frame].download_buffer_get_data_requests.clear();
+		}
+
+		// Flush any pending requests for asynchronous texture downloads.
+		if (!frames[p_frame].download_texture_get_data_requests.is_empty()) {
+			uint32_t pitch_step = driver->api_trait_get(RDD::API_TRAIT_TEXTURE_DATA_ROW_PITCH_STEP);
+			for (uint32_t i = 0; i < frames[p_frame].download_texture_get_data_requests.size(); i++) {
+				const TextureGetDataRequest &request = frames[p_frame].download_texture_get_data_requests[i];
+				uint32_t texture_size = get_image_format_required_size(request.format, request.width, request.height, request.depth, request.mipmaps);
+				packed_byte_array.resize(texture_size);
+
+				// Find the block size of the texture's format.
+				uint32_t block_w = 0;
+				uint32_t block_h = 0;
+				get_compressed_image_format_block_dimensions(request.format, block_w, block_h);
+
+				uint32_t block_size = get_compressed_image_format_block_byte_size(request.format);
+				uint32_t pixel_size = get_image_format_pixel_size(request.format);
+				uint32_t pixel_rshift = get_compressed_image_format_pixel_rshift(request.format);
+				uint32_t region_size = texture_download_region_size_px;
+
+				for (uint32_t j = 0; j < request.frame_local_count; j++) {
+					uint32_t local_index = request.frame_local_index + j;
+					const RDD::BufferTextureCopyRegion &region = frames[p_frame].download_buffer_texture_copy_regions[local_index];
+					uint32_t w = STEPIFY(request.width >> region.texture_subresources.mipmap, block_w);
+					uint32_t h = STEPIFY(request.height >> region.texture_subresources.mipmap, block_h);
+					uint32_t region_w = MIN(region_size, w - region.texture_offset.x);
+					uint32_t region_h = MIN(region_size, h - region.texture_offset.y);
+					uint32_t region_pitch = (region_w * pixel_size * block_w) >> pixel_rshift;
+					region_pitch = STEPIFY(region_pitch, pitch_step);
+
+					uint8_t *buffer_data = driver->buffer_map(frames[p_frame].download_texture_staging_buffers[local_index]);
+					const uint8_t *read_ptr = buffer_data + region.buffer_offset;
+					uint8_t *write_ptr = packed_byte_array.ptrw() + frames[p_frame].download_texture_mipmap_offsets[local_index];
+					uint32_t unit_size = pixel_size;
+					if (block_w != 1 || block_h != 1) {
+						unit_size = block_size;
+					}
+
+					write_ptr += ((region.texture_offset.y / block_h) * (w / block_w) + (region.texture_offset.x / block_w)) * unit_size;
+					for (uint32_t y = region_h / block_h; y > 0; y--) {
+						memcpy(write_ptr, read_ptr, (region_w / block_w) * unit_size);
+						write_ptr += (w / block_w) * unit_size;
+						read_ptr += region_pitch;
+					}
+
+					driver->buffer_unmap(frames[p_frame].download_texture_staging_buffers[local_index]);
+				}
+
+				request.callback.call(packed_byte_array);
+			}
+
+			frames[p_frame].download_texture_staging_buffers.clear();
+			frames[p_frame].download_buffer_texture_copy_regions.clear();
+			frames[p_frame].download_texture_mipmap_offsets.clear();
+			frames[p_frame].download_texture_get_data_requests.clear();
+		}
+	}
+}
+
 void RenderingDevice::_stall_for_previous_frames() {
 	for (uint32_t i = 0; i < frames.size(); i++) {
-		if (frames[i].fence_signaled) {
-			driver->fence_wait(frames[i].fence);
-			frames[i].fence_signaled = false;
-		}
+		_stall_for_frame(i);
 	}
 }
 
@@ -6386,30 +6681,41 @@ Error RenderingDevice::initialize(RenderingContextDriver *p_context, DisplayServ
 	}
 
 	// Convert block size from KB.
-	staging_buffer_block_size = GLOBAL_GET("rendering/rendering_device/staging_buffer/block_size_kb");
-	staging_buffer_block_size = MAX(4u, staging_buffer_block_size);
-	staging_buffer_block_size *= 1024;
+	upload_staging_buffers.block_size = GLOBAL_GET("rendering/rendering_device/staging_buffer/block_size_kb");
+	upload_staging_buffers.block_size = MAX(4u, upload_staging_buffers.block_size);
+	upload_staging_buffers.block_size *= 1024;
 
 	// Convert staging buffer size from MB.
-	staging_buffer_max_size = GLOBAL_GET("rendering/rendering_device/staging_buffer/max_size_mb");
-	staging_buffer_max_size = MAX(1u, staging_buffer_max_size);
-	staging_buffer_max_size *= 1024 * 1024;
+	upload_staging_buffers.max_size = GLOBAL_GET("rendering/rendering_device/staging_buffer/max_size_mb");
+	upload_staging_buffers.max_size = MAX(1u, upload_staging_buffers.max_size);
+	upload_staging_buffers.max_size *= 1024 * 1024;
+	upload_staging_buffers.max_size = MAX(upload_staging_buffers.max_size, upload_staging_buffers.block_size * 4);
 
-	if (staging_buffer_max_size < staging_buffer_block_size * 4) {
-		// Validate enough blocks.
-		staging_buffer_max_size = staging_buffer_block_size * 4;
-	}
+	// Copy the sizes to the download staging buffers.
+	download_staging_buffers.block_size = upload_staging_buffers.block_size;
+	download_staging_buffers.max_size = upload_staging_buffers.max_size;
 
 	texture_upload_region_size_px = GLOBAL_GET("rendering/rendering_device/staging_buffer/texture_upload_region_size_px");
 	texture_upload_region_size_px = nearest_power_of_2_templated(texture_upload_region_size_px);
 
+	texture_download_region_size_px = GLOBAL_GET("rendering/rendering_device/staging_buffer/texture_download_region_size_px");
+	texture_download_region_size_px = nearest_power_of_2_templated(texture_download_region_size_px);
+
 	// Ensure current staging block is valid and at least one per frame exists.
-	staging_buffer_current = 0;
-	staging_buffer_used = false;
+	upload_staging_buffers.current = 0;
+	upload_staging_buffers.used = false;
+	upload_staging_buffers.usage_bits = RDD::BUFFER_USAGE_TRANSFER_FROM_BIT;
+
+	download_staging_buffers.current = 0;
+	download_staging_buffers.used = false;
+	download_staging_buffers.usage_bits = RDD::BUFFER_USAGE_TRANSFER_TO_BIT;
 
 	for (uint32_t i = 0; i < frames.size(); i++) {
-		// Staging was never used, create a block.
-		err = _insert_staging_block();
+		// Staging was never used, create the blocks.
+		err = _insert_staging_block(upload_staging_buffers);
+		ERR_FAIL_COND_V(err, FAILED);
+
+		err = _insert_staging_block(download_staging_buffers);
 		ERR_FAIL_COND_V(err, FAILED);
 	}
 
@@ -6788,8 +7094,12 @@ void RenderingDevice::finalize() {
 
 	frames.clear();
 
-	for (int i = 0; i < staging_buffer_blocks.size(); i++) {
-		driver->buffer_free(staging_buffer_blocks[i].driver_id);
+	for (int i = 0; i < upload_staging_buffers.blocks.size(); i++) {
+		driver->buffer_free(upload_staging_buffers.blocks[i].driver_id);
+	}
+
+	for (int i = 0; i < download_staging_buffers.blocks.size(); i++) {
+		driver->buffer_free(download_staging_buffers.blocks[i].driver_id);
 	}
 
 	while (vertex_formats.size()) {
@@ -6869,6 +7179,7 @@ void RenderingDevice::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("texture_update", "texture", "layer", "data"), &RenderingDevice::texture_update);
 	ClassDB::bind_method(D_METHOD("texture_get_data", "texture", "layer"), &RenderingDevice::texture_get_data);
+	ClassDB::bind_method(D_METHOD("texture_get_data_async", "texture", "layer", "callback"), &RenderingDevice::texture_get_data_async);
 
 	ClassDB::bind_method(D_METHOD("texture_is_format_supported_for_usage", "format", "usage_flags"), &RenderingDevice::texture_is_format_supported_for_usage);
 
@@ -6926,6 +7237,7 @@ void RenderingDevice::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("buffer_update", "buffer", "offset", "size_bytes", "data"), &RenderingDevice::_buffer_update_bind);
 	ClassDB::bind_method(D_METHOD("buffer_clear", "buffer", "offset", "size_bytes"), &RenderingDevice::buffer_clear);
 	ClassDB::bind_method(D_METHOD("buffer_get_data", "buffer", "offset_bytes", "size_bytes"), &RenderingDevice::buffer_get_data, DEFVAL(0), DEFVAL(0));
+	ClassDB::bind_method(D_METHOD("buffer_get_data_async", "buffer", "callback", "offset_bytes", "size_bytes"), &RenderingDevice::buffer_get_data_async, DEFVAL(0), DEFVAL(0));
 
 	ClassDB::bind_method(D_METHOD("render_pipeline_create", "shader", "framebuffer_format", "vertex_format", "primitive", "rasterization_state", "multisample_state", "stencil_state", "color_blend_state", "dynamic_state_flags", "for_render_pass", "specialization_constants"), &RenderingDevice::_render_pipeline_create, DEFVAL(0), DEFVAL(0), DEFVAL(TypedArray<RDPipelineSpecializationConstant>()));
 	ClassDB::bind_method(D_METHOD("render_pipeline_is_valid", "render_pipeline"), &RenderingDevice::render_pipeline_is_valid);
diff --git a/servers/rendering/rendering_device.h b/servers/rendering/rendering_device.h
index 92045bd0d8..98b9cd2180 100644
--- a/servers/rendering/rendering_device.h
+++ b/servers/rendering/rendering_device.h
@@ -156,27 +156,33 @@ private:
 	//
 	// See the comments in the code to understand better how it works.
 
+	enum StagingRequiredAction {
+		STAGING_REQUIRED_ACTION_NONE,
+		STAGING_REQUIRED_ACTION_FLUSH_AND_STALL_ALL,
+		STAGING_REQUIRED_ACTION_STALL_PREVIOUS,
+	};
+
 	struct StagingBufferBlock {
 		RDD::BufferID driver_id;
 		uint64_t frame_used = 0;
 		uint32_t fill_amount = 0;
 	};
 
-	Vector<StagingBufferBlock> staging_buffer_blocks;
-	int staging_buffer_current = 0;
-	uint32_t staging_buffer_block_size = 0;
-	uint64_t staging_buffer_max_size = 0;
-	bool staging_buffer_used = false;
-
-	enum StagingRequiredAction {
-		STAGING_REQUIRED_ACTION_NONE,
-		STAGING_REQUIRED_ACTION_FLUSH_AND_STALL_ALL,
-		STAGING_REQUIRED_ACTION_STALL_PREVIOUS
+	struct StagingBuffers {
+		Vector<StagingBufferBlock> blocks;
+		int current = 0;
+		uint32_t block_size = 0;
+		uint64_t max_size = 0;
+		BitField<RDD::BufferUsageBits> usage_bits;
+		bool used = false;
 	};
 
-	Error _staging_buffer_allocate(uint32_t p_amount, uint32_t p_required_align, uint32_t &r_alloc_offset, uint32_t &r_alloc_size, StagingRequiredAction &r_required_action, bool p_can_segment = true);
-	void _staging_buffer_execute_required_action(StagingRequiredAction p_required_action);
-	Error _insert_staging_block();
+	Error _staging_buffer_allocate(StagingBuffers &p_staging_buffers, uint32_t p_amount, uint32_t p_required_align, uint32_t &r_alloc_offset, uint32_t &r_alloc_size, StagingRequiredAction &r_required_action, bool p_can_segment = true);
+	void _staging_buffer_execute_required_action(StagingBuffers &p_staging_buffers, StagingRequiredAction p_required_action);
+	Error _insert_staging_block(StagingBuffers &p_staging_buffers);
+
+	StagingBuffers upload_staging_buffers;
+	StagingBuffers download_staging_buffers;
 
 	struct Buffer {
 		RDD::BufferID driver_id;
@@ -205,11 +211,19 @@ private:
 	RID_Owner<Buffer, true> storage_buffer_owner;
 	RID_Owner<Buffer, true> texture_buffer_owner;
 
+	struct BufferGetDataRequest {
+		uint32_t frame_local_index = 0;
+		uint32_t frame_local_count = 0;
+		Callable callback;
+		uint32_t size = 0;
+	};
+
 public:
 	Error buffer_copy(RID p_src_buffer, RID p_dst_buffer, uint32_t p_src_offset, uint32_t p_dst_offset, uint32_t p_size);
 	Error buffer_update(RID p_buffer, uint32_t p_offset, uint32_t p_size, const void *p_data);
 	Error buffer_clear(RID p_buffer, uint32_t p_offset, uint32_t p_size);
 	Vector<uint8_t> buffer_get_data(RID p_buffer, uint32_t p_offset = 0, uint32_t p_size = 0); // This causes stall, only use to retrieve large buffers for saving.
+	Error buffer_get_data_async(RID p_buffer, const Callable &p_callback, uint32_t p_offset = 0, uint32_t p_size = 0);
 
 	/*****************/
 	/**** TEXTURE ****/
@@ -300,6 +314,7 @@ public:
 
 	RID_Owner<Texture, true> texture_owner;
 	uint32_t texture_upload_region_size_px = 0;
+	uint32_t texture_download_region_size_px = 0;
 
 	Vector<uint8_t> _texture_get_data(Texture *tex, uint32_t p_layer, bool p_2d = false);
 	uint32_t _texture_layer_count(Texture *p_texture) const;
@@ -311,6 +326,17 @@ public:
 	void _texture_copy_shared(RID p_src_texture_rid, Texture *p_src_texture, RID p_dst_texture_rid, Texture *p_dst_texture);
 	void _texture_create_reinterpret_buffer(Texture *p_texture);
 
+	struct TextureGetDataRequest {
+		uint32_t frame_local_index = 0;
+		uint32_t frame_local_count = 0;
+		Callable callback;
+		uint32_t width = 0;
+		uint32_t height = 0;
+		uint32_t depth = 0;
+		uint32_t mipmaps = 0;
+		RDD::DataFormat format = RDD::DATA_FORMAT_MAX;
+	};
+
 public:
 	struct TextureView {
 		DataFormat format_override = DATA_FORMAT_MAX; // // Means, use same as format.
@@ -342,6 +368,7 @@ public:
 	RID texture_create_shared_from_slice(const TextureView &p_view, RID p_with_texture, uint32_t p_layer, uint32_t p_mipmap, uint32_t p_mipmaps = 1, TextureSliceType p_slice_type = TEXTURE_SLICE_2D, uint32_t p_layers = 0);
 	Error texture_update(RID p_texture, uint32_t p_layer, const Vector<uint8_t> &p_data);
 	Vector<uint8_t> texture_get_data(RID p_texture, uint32_t p_layer); // CPU textures will return immediately, while GPU textures will most likely force a flush
+	Error texture_get_data_async(RID p_texture, uint32_t p_layer, const Callable &p_callback);
 
 	bool texture_is_format_supported_for_usage(DataFormat p_format, BitField<TextureUsageBits> p_usage) const;
 	bool texture_is_shared(RID p_texture);
@@ -1381,6 +1408,17 @@ private:
 		List<RenderPipeline> render_pipelines_to_dispose_of;
 		List<ComputePipeline> compute_pipelines_to_dispose_of;
 
+		// Pending asynchronous data transfer for buffers.
+		LocalVector<RDD::BufferID> download_buffer_staging_buffers;
+		LocalVector<RDD::BufferCopyRegion> download_buffer_copy_regions;
+		LocalVector<BufferGetDataRequest> download_buffer_get_data_requests;
+
+		// Pending asynchronous data transfer for textures.
+		LocalVector<RDD::BufferID> download_texture_staging_buffers;
+		LocalVector<RDD::BufferTextureCopyRegion> download_buffer_texture_copy_regions;
+		LocalVector<uint32_t> download_texture_mipmap_offsets;
+		LocalVector<TextureGetDataRequest> download_texture_get_data_requests;
+
 		// The command pool used by the command buffer.
 		RDD::CommandPoolID command_pool;
 
@@ -1446,6 +1484,7 @@ public:
 	void _begin_frame(bool p_presented = false);
 	void _end_frame();
 	void _execute_frame(bool p_present);
+	void _stall_for_frame(uint32_t p_frame);
 	void _stall_for_previous_frames();
 	void _flush_and_stall_for_all_frames();
 

From 476479419be1acdc272e74255a1586e697fde53b Mon Sep 17 00:00:00 2001
From: smix8 <52464204+smix8@users.noreply.github.com>
Date: Fri, 29 Nov 2024 18:22:26 +0100
Subject: [PATCH 083/124] Despaghettify NavigationServer path queries

Despaghettify NavigationServer path queries.
---
 .../NavigationPathQueryParameters2D.xml       |   3 +
 .../NavigationPathQueryParameters3D.xml       |   3 +
 doc/classes/NavigationServer2D.xml            |   7 +-
 doc/classes/NavigationServer3D.xml            |   7 +-
 doc/classes/ProjectSettings.xml               |   3 +
 .../4.3-stable.expected                       |  13 +
 .../2d/godot_navigation_server_2d.cpp         |  54 +-
 .../2d/godot_navigation_server_2d.h           |   4 +-
 .../3d/godot_navigation_server_3d.cpp         | 196 +----
 .../3d/godot_navigation_server_3d.h           |  10 +-
 modules/navigation/3d/nav_mesh_queries_3d.cpp | 777 +++++++++++-------
 modules/navigation/3d/nav_mesh_queries_3d.h   |  77 +-
 modules/navigation/nav_map.cpp                |  67 +-
 modules/navigation/nav_map.h                  |   9 +-
 modules/navigation/nav_utils.h                |   9 +
 scene/2d/navigation_agent_2d.cpp              |   2 +-
 scene/3d/navigation_agent_3d.cpp              |   2 +-
 .../navigation_path_query_parameters_2d.cpp   |  80 +-
 .../navigation_path_query_parameters_2d.h     |  25 +-
 .../navigation_path_query_parameters_3d.cpp   |  84 +-
 .../navigation_path_query_parameters_3d.h     |  33 +-
 .../navigation_path_query_result_2d.h         |   4 +-
 .../navigation_path_query_result_3d.h         |   4 +-
 servers/navigation/navigation_utilities.h     |  20 +-
 servers/navigation_server_2d.compat.inc       |  46 ++
 servers/navigation_server_2d.cpp              |   3 +-
 servers/navigation_server_2d.h                |  12 +-
 servers/navigation_server_2d_dummy.h          |   4 +-
 servers/navigation_server_3d.compat.inc       |  46 ++
 servers/navigation_server_3d.cpp              |  17 +-
 servers/navigation_server_3d.h                |  13 +-
 servers/navigation_server_3d_dummy.h          |   5 +-
 32 files changed, 984 insertions(+), 655 deletions(-)
 create mode 100644 servers/navigation_server_2d.compat.inc
 create mode 100644 servers/navigation_server_3d.compat.inc

diff --git a/doc/classes/NavigationPathQueryParameters2D.xml b/doc/classes/NavigationPathQueryParameters2D.xml
index ce0a00bc83..1f9c064f93 100644
--- a/doc/classes/NavigationPathQueryParameters2D.xml
+++ b/doc/classes/NavigationPathQueryParameters2D.xml
@@ -49,6 +49,9 @@
 		<constant name="PATH_POSTPROCESSING_EDGECENTERED" value="1" enum="PathPostProcessing">
 			Centers every path position in the middle of the traveled navigation mesh polygon edge. This creates better paths for tile- or gridbased layouts that restrict the movement to the cells center.
 		</constant>
+		<constant name="PATH_POSTPROCESSING_NONE" value="2" enum="PathPostProcessing">
+			Applies no postprocessing and returns the raw path corridor as found by the pathfinding algorithm.
+		</constant>
 		<constant name="PATH_METADATA_INCLUDE_NONE" value="0" enum="PathMetadataFlags" is_bitfield="true">
 			Don't include any additional metadata about the returned path.
 		</constant>
diff --git a/doc/classes/NavigationPathQueryParameters3D.xml b/doc/classes/NavigationPathQueryParameters3D.xml
index fcd913f73b..a4c622d080 100644
--- a/doc/classes/NavigationPathQueryParameters3D.xml
+++ b/doc/classes/NavigationPathQueryParameters3D.xml
@@ -49,6 +49,9 @@
 		<constant name="PATH_POSTPROCESSING_EDGECENTERED" value="1" enum="PathPostProcessing">
 			Centers every path position in the middle of the traveled navigation mesh polygon edge. This creates better paths for tile- or gridbased layouts that restrict the movement to the cells center.
 		</constant>
+		<constant name="PATH_POSTPROCESSING_NONE" value="2" enum="PathPostProcessing">
+			Applies no postprocessing and returns the raw path corridor as found by the pathfinding algorithm.
+		</constant>
 		<constant name="PATH_METADATA_INCLUDE_NONE" value="0" enum="PathMetadataFlags" is_bitfield="true">
 			Don't include any additional metadata about the returned path.
 		</constant>
diff --git a/doc/classes/NavigationServer2D.xml b/doc/classes/NavigationServer2D.xml
index 5c19a6b355..d1fac97b93 100644
--- a/doc/classes/NavigationServer2D.xml
+++ b/doc/classes/NavigationServer2D.xml
@@ -533,7 +533,7 @@
 				Returns all navigation obstacle [RID]s that are currently assigned to the requested navigation [param map].
 			</description>
 		</method>
-		<method name="map_get_path" qualifiers="const">
+		<method name="map_get_path">
 			<return type="PackedVector2Array" />
 			<param index="0" name="map" type="RID" />
 			<param index="1" name="origin" type="Vector2" />
@@ -754,12 +754,13 @@
 				[b]Performance:[/b] While convenient, reading data arrays from [Mesh] resources can affect the frame rate negatively. The data needs to be received from the GPU, stalling the [RenderingServer] in the process. For performance prefer the use of e.g. collision shapes or creating the data arrays entirely in code.
 			</description>
 		</method>
-		<method name="query_path" qualifiers="const">
+		<method name="query_path">
 			<return type="void" />
 			<param index="0" name="parameters" type="NavigationPathQueryParameters2D" />
 			<param index="1" name="result" type="NavigationPathQueryResult2D" />
+			<param index="2" name="callback" type="Callable" default="Callable()" />
 			<description>
-				Queries a path in a given navigation map. Start and target position and other parameters are defined through [NavigationPathQueryParameters2D]. Updates the provided [NavigationPathQueryResult2D] result object with the path among other results requested by the query.
+				Queries a path in a given navigation map. Start and target position and other parameters are defined through [NavigationPathQueryParameters2D]. Updates the provided [NavigationPathQueryResult2D] result object with the path among other results requested by the query. After the process is finished the optional [param callback] will be called.
 			</description>
 		</method>
 		<method name="region_create">
diff --git a/doc/classes/NavigationServer3D.xml b/doc/classes/NavigationServer3D.xml
index 66a286758b..ab65f3f743 100644
--- a/doc/classes/NavigationServer3D.xml
+++ b/doc/classes/NavigationServer3D.xml
@@ -605,7 +605,7 @@
 				Returns all navigation obstacle [RID]s that are currently assigned to the requested navigation [param map].
 			</description>
 		</method>
-		<method name="map_get_path" qualifiers="const">
+		<method name="map_get_path">
 			<return type="PackedVector3Array" />
 			<param index="0" name="map" type="RID" />
 			<param index="1" name="origin" type="Vector3" />
@@ -887,12 +887,13 @@
 				[b]Performance:[/b] While convenient, reading data arrays from [Mesh] resources can affect the frame rate negatively. The data needs to be received from the GPU, stalling the [RenderingServer] in the process. For performance prefer the use of e.g. collision shapes or creating the data arrays entirely in code.
 			</description>
 		</method>
-		<method name="query_path" qualifiers="const">
+		<method name="query_path">
 			<return type="void" />
 			<param index="0" name="parameters" type="NavigationPathQueryParameters3D" />
 			<param index="1" name="result" type="NavigationPathQueryResult3D" />
+			<param index="2" name="callback" type="Callable" default="Callable()" />
 			<description>
-				Queries a path in a given navigation map. Start and target position and other parameters are defined through [NavigationPathQueryParameters3D]. Updates the provided [NavigationPathQueryResult3D] result object with the path among other results requested by the query.
+				Queries a path in a given navigation map. Start and target position and other parameters are defined through [NavigationPathQueryParameters3D]. Updates the provided [NavigationPathQueryResult3D] result object with the path among other results requested by the query. After the process is finished the optional [param callback] will be called.
 			</description>
 		</method>
 		<method name="region_bake_navigation_mesh" deprecated="This method is deprecated due to core threading changes. To upgrade existing code, first create a [NavigationMeshSourceGeometryData3D] resource. Use this resource with [method parse_source_geometry_data] to parse the [SceneTree] for nodes that should contribute to the navigation mesh baking. The [SceneTree] parsing needs to happen on the main thread. After the parsing is finished use the resource with [method bake_from_source_geometry_data] to bake a navigation mesh.">
diff --git a/doc/classes/ProjectSettings.xml b/doc/classes/ProjectSettings.xml
index 0a16b15931..ef3a96cb53 100644
--- a/doc/classes/ProjectSettings.xml
+++ b/doc/classes/ProjectSettings.xml
@@ -2162,6 +2162,9 @@
 		<member name="navigation/baking/use_crash_prevention_checks" type="bool" setter="" getter="" default="true">
 			If enabled, and baking would potentially lead to an engine crash, the baking will be interrupted and an error message with explanation will be raised.
 		</member>
+		<member name="navigation/pathfinding/max_threads" type="int" setter="" getter="" default="4">
+			Maximum number of threads that can run pathfinding queries simultaneously on the same pathfinding graph, for example the same navigation map. Additional threads increase memory consumption and synchronization time due to the need for extra data copies prepared for each thread. A value of [code]-1[/code] means unlimited and the maximum available OS processor count is used. Defaults to [code]1[/code] when the OS does not support threads.
+		</member>
 		<member name="network/limits/debugger/max_chars_per_second" type="int" setter="" getter="" default="32768">
 			Maximum number of characters allowed to send as output from the debugger. Over this value, content is dropped. This helps not to stall the debugger connection.
 		</member>
diff --git a/misc/extension_api_validation/4.3-stable.expected b/misc/extension_api_validation/4.3-stable.expected
index 1bbe720eb9..8cade432e6 100644
--- a/misc/extension_api_validation/4.3-stable.expected
+++ b/misc/extension_api_validation/4.3-stable.expected
@@ -215,3 +215,16 @@ Validate extension JSON: Error: Field 'classes/Control/properties/offset_right':
 Validate extension JSON: Error: Field 'classes/Control/properties/offset_top': type changed value in new API, from "int" to "float".
 
 Property type changed to float to match the actual internal API and documentation.
+
+
+GH-100129
+---------
+Validate extension JSON: Error: Field 'classes/NavigationServer2D/methods/query_path': is_const changed value in new API, from true to false.
+Validate extension JSON: Error: Field 'classes/NavigationServer3D/methods/query_path': is_const changed value in new API, from true to false.
+Validate extension JSON: Error: Field 'classes/NavigationServer2D/methods/query_path/arguments': size changed value in new API, from 2 to 3.
+Validate extension JSON: Error: Field 'classes/NavigationServer3D/methods/query_path/arguments': size changed value in new API, from 2 to 3.
+Validate extension JSON: Error: Field 'classes/NavigationServer2D/methods/map_get_path': is_const changed value in new API, from true to false.
+Validate extension JSON: Error: Field 'classes/NavigationServer3D/methods/map_get_path': is_const changed value in new API, from true to false.
+
+`query_path` and `map_get_path` methods changed to be non const due to internal compatibility and server changes.
+Added optional callback parameters to `query_path` functions. Compatibility methods registered.
diff --git a/modules/navigation/2d/godot_navigation_server_2d.cpp b/modules/navigation/2d/godot_navigation_server_2d.cpp
index 2af125d434..de78ea5826 100644
--- a/modules/navigation/2d/godot_navigation_server_2d.cpp
+++ b/modules/navigation/2d/godot_navigation_server_2d.cpp
@@ -81,12 +81,6 @@
 		return CONV_R(NavigationServer3D::get_singleton()->FUNC_NAME(CONV_0(D_0), CONV_1(D_1))); \
 	}
 
-#define FORWARD_5_R_C(CONV_R, FUNC_NAME, T_0, D_0, T_1, D_1, T_2, D_2, T_3, D_3, T_4, D_4, CONV_0, CONV_1, CONV_2, CONV_3, CONV_4)      \
-	GodotNavigationServer2D::FUNC_NAME(T_0 D_0, T_1 D_1, T_2 D_2, T_3 D_3, T_4 D_4)                                                     \
-			const {                                                                                                                     \
-		return CONV_R(NavigationServer3D::get_singleton()->FUNC_NAME(CONV_0(D_0), CONV_1(D_1), CONV_2(D_2), CONV_3(D_3), CONV_4(D_4))); \
-	}
-
 static RID rid_to_rid(const RID d) {
 	return d;
 }
@@ -277,7 +271,9 @@ real_t FORWARD_1_C(map_get_edge_connection_margin, RID, p_map, rid_to_rid);
 void FORWARD_2(map_set_link_connection_radius, RID, p_map, real_t, p_connection_radius, rid_to_rid, real_to_real);
 real_t FORWARD_1_C(map_get_link_connection_radius, RID, p_map, rid_to_rid);
 
-Vector<Vector2> FORWARD_5_R_C(vector_v3_to_v2, map_get_path, RID, p_map, Vector2, p_origin, Vector2, p_destination, bool, p_optimize, uint32_t, p_layers, rid_to_rid, v2_to_v3, v2_to_v3, bool_to_bool, uint32_to_uint32);
+Vector<Vector2> GodotNavigationServer2D::map_get_path(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize, uint32_t p_navigation_layers) {
+	return vector_v3_to_v2(NavigationServer3D::get_singleton()->map_get_path(p_map, v2_to_v3(p_origin), v2_to_v3(p_destination), p_optimize, p_navigation_layers));
+}
 
 Vector2 FORWARD_2_R_C(v3_to_v2, map_get_closest_point, RID, p_map, const Vector2 &, p_point, rid_to_rid, v2_to_v3);
 RID FORWARD_2_C(map_get_closest_point_owner, RID, p_map, const Vector2 &, p_point, rid_to_rid, v2_to_v3);
@@ -456,16 +452,48 @@ Vector<Vector2> GodotNavigationServer2D::obstacle_get_vertices(RID p_obstacle) c
 	return vector_v3_to_v2(NavigationServer3D::get_singleton()->obstacle_get_vertices(p_obstacle));
 }
 
-void GodotNavigationServer2D::query_path(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result) const {
+void GodotNavigationServer2D::query_path(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result, const Callable &p_callback) {
 	ERR_FAIL_COND(!p_query_parameters.is_valid());
 	ERR_FAIL_COND(!p_query_result.is_valid());
 
-	const NavigationUtilities::PathQueryResult _query_result = NavigationServer3D::get_singleton()->_query_path(p_query_parameters->get_parameters());
+	Ref<NavigationPathQueryParameters3D> query_parameters;
+	query_parameters.instantiate();
 
-	p_query_result->set_path(vector_v3_to_v2(_query_result.path));
-	p_query_result->set_path_types(_query_result.path_types);
-	p_query_result->set_path_rids(_query_result.path_rids);
-	p_query_result->set_path_owner_ids(_query_result.path_owner_ids);
+	query_parameters->set_map(p_query_parameters->get_map());
+	query_parameters->set_start_position(v2_to_v3(p_query_parameters->get_start_position()));
+	query_parameters->set_target_position(v2_to_v3(p_query_parameters->get_target_position()));
+	query_parameters->set_navigation_layers(p_query_parameters->get_navigation_layers());
+	query_parameters->set_pathfinding_algorithm(NavigationPathQueryParameters3D::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR);
+
+	switch (p_query_parameters->get_path_postprocessing()) {
+		case NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL: {
+			query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL);
+		} break;
+		case NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED: {
+			query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED);
+		} break;
+		case NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_NONE: {
+			query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_NONE);
+		} break;
+		default: {
+			WARN_PRINT("No match for used PathPostProcessing - fallback to default");
+			query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL);
+		} break;
+	}
+
+	query_parameters->set_metadata_flags((int64_t)p_query_parameters->get_metadata_flags());
+	query_parameters->set_simplify_path(p_query_parameters->get_simplify_path());
+	query_parameters->set_simplify_epsilon(p_query_parameters->get_simplify_epsilon());
+
+	Ref<NavigationPathQueryResult3D> query_result;
+	query_result.instantiate();
+
+	NavigationServer3D::get_singleton()->query_path(query_parameters, query_result, p_callback);
+
+	p_query_result->set_path(vector_v3_to_v2(query_result->get_path()));
+	p_query_result->set_path_types(query_result->get_path_types());
+	p_query_result->set_path_rids(query_result->get_path_rids());
+	p_query_result->set_path_owner_ids(query_result->get_path_owner_ids());
 }
 
 RID GodotNavigationServer2D::source_geometry_parser_create() {
diff --git a/modules/navigation/2d/godot_navigation_server_2d.h b/modules/navigation/2d/godot_navigation_server_2d.h
index 1579ca2907..80fdea5ba9 100644
--- a/modules/navigation/2d/godot_navigation_server_2d.h
+++ b/modules/navigation/2d/godot_navigation_server_2d.h
@@ -68,7 +68,7 @@ public:
 	virtual real_t map_get_edge_connection_margin(RID p_map) const override;
 	virtual void map_set_link_connection_radius(RID p_map, real_t p_connection_radius) override;
 	virtual real_t map_get_link_connection_radius(RID p_map) const override;
-	virtual Vector<Vector2> map_get_path(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) const override;
+	virtual Vector<Vector2> map_get_path(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) override;
 	virtual Vector2 map_get_closest_point(RID p_map, const Vector2 &p_point) const override;
 	virtual RID map_get_closest_point_owner(RID p_map, const Vector2 &p_point) const override;
 	virtual TypedArray<RID> map_get_links(RID p_map) const override;
@@ -242,7 +242,7 @@ public:
 	virtual void obstacle_set_avoidance_layers(RID p_obstacle, uint32_t p_layers) override;
 	virtual uint32_t obstacle_get_avoidance_layers(RID p_obstacle) const override;
 
-	virtual void query_path(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result) const override;
+	virtual void query_path(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result, const Callable &p_callback) override;
 
 	virtual void init() override;
 	virtual void sync() override;
diff --git a/modules/navigation/3d/godot_navigation_server_3d.cpp b/modules/navigation/3d/godot_navigation_server_3d.cpp
index 5dfc39f6f5..80e87a4b2e 100644
--- a/modules/navigation/3d/godot_navigation_server_3d.cpp
+++ b/modules/navigation/3d/godot_navigation_server_3d.cpp
@@ -237,11 +237,29 @@ real_t GodotNavigationServer3D::map_get_link_connection_radius(RID p_map) const
 	return map->get_link_connection_radius();
 }
 
-Vector<Vector3> GodotNavigationServer3D::map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers) const {
+Vector<Vector3> GodotNavigationServer3D::map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers) {
 	const NavMap *map = map_owner.get_or_null(p_map);
 	ERR_FAIL_NULL_V(map, Vector<Vector3>());
 
-	return map->get_path(p_origin, p_destination, p_optimize, p_navigation_layers, nullptr, nullptr, nullptr);
+	Ref<NavigationPathQueryParameters3D> query_parameters;
+	query_parameters.instantiate();
+
+	query_parameters->set_map(p_map);
+	query_parameters->set_start_position(p_origin);
+	query_parameters->set_target_position(p_destination);
+	query_parameters->set_navigation_layers(p_navigation_layers);
+	query_parameters->set_pathfinding_algorithm(NavigationPathQueryParameters3D::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR);
+	query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL);
+	if (!p_optimize) {
+		query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PATH_POSTPROCESSING_EDGECENTERED);
+	}
+
+	Ref<NavigationPathQueryResult3D> query_result;
+	query_result.instantiate();
+
+	query_path(query_parameters, query_result);
+
+	return query_result->get_path();
 }
 
 Vector3 GodotNavigationServer3D::map_get_closest_point_to_segment(RID p_map, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const {
@@ -1384,86 +1402,14 @@ void GodotNavigationServer3D::finish() {
 #endif // _3D_DISABLED
 }
 
-PathQueryResult GodotNavigationServer3D::_query_path(const PathQueryParameters &p_parameters) const {
-	PathQueryResult r_query_result;
+void GodotNavigationServer3D::query_path(const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result, const Callable &p_callback) {
+	ERR_FAIL_COND(p_query_parameters.is_null());
+	ERR_FAIL_COND(p_query_result.is_null());
 
-	const NavMap *map = map_owner.get_or_null(p_parameters.map);
-	ERR_FAIL_NULL_V(map, r_query_result);
+	NavMap *map = map_owner.get_or_null(p_query_parameters->get_map());
+	ERR_FAIL_NULL(map);
 
-	// run the pathfinding
-
-	if (p_parameters.pathfinding_algorithm == PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR) {
-		// while postprocessing is still part of map.get_path() need to check and route it here for the correct "optimize" post-processing
-		if (p_parameters.path_postprocessing == PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL) {
-			r_query_result.path = map->get_path(
-					p_parameters.start_position,
-					p_parameters.target_position,
-					true,
-					p_parameters.navigation_layers,
-					p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES) ? &r_query_result.path_types : nullptr,
-					p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS) ? &r_query_result.path_rids : nullptr,
-					p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS) ? &r_query_result.path_owner_ids : nullptr);
-		} else if (p_parameters.path_postprocessing == PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED) {
-			r_query_result.path = map->get_path(
-					p_parameters.start_position,
-					p_parameters.target_position,
-					false,
-					p_parameters.navigation_layers,
-					p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES) ? &r_query_result.path_types : nullptr,
-					p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS) ? &r_query_result.path_rids : nullptr,
-					p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS) ? &r_query_result.path_owner_ids : nullptr);
-		}
-	} else {
-		return r_query_result;
-	}
-
-	// add path postprocessing
-
-	if (r_query_result.path.size() > 2 && p_parameters.simplify_path) {
-		const LocalVector<uint32_t> &simplified_path_indices = get_simplified_path_indices(r_query_result.path, p_parameters.simplify_epsilon);
-
-		uint32_t indices_count = simplified_path_indices.size();
-
-		{
-			Vector3 *w = r_query_result.path.ptrw();
-			const Vector3 *r = r_query_result.path.ptr();
-			for (uint32_t i = 0; i < indices_count; i++) {
-				w[i] = r[simplified_path_indices[i]];
-			}
-			r_query_result.path.resize(indices_count);
-		}
-
-		if (p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
-			int32_t *w = r_query_result.path_types.ptrw();
-			const int32_t *r = r_query_result.path_types.ptr();
-			for (uint32_t i = 0; i < indices_count; i++) {
-				w[i] = r[simplified_path_indices[i]];
-			}
-			r_query_result.path_types.resize(indices_count);
-		}
-
-		if (p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) {
-			TypedArray<RID> simplified_path_rids;
-			simplified_path_rids.resize(indices_count);
-			for (uint32_t i = 0; i < indices_count; i++) {
-				simplified_path_rids[i] = r_query_result.path_rids[i];
-			}
-			r_query_result.path_rids = simplified_path_rids;
-		}
-
-		if (p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) {
-			int64_t *w = r_query_result.path_owner_ids.ptrw();
-			const int64_t *r = r_query_result.path_owner_ids.ptr();
-			for (uint32_t i = 0; i < indices_count; i++) {
-				w[i] = r[simplified_path_indices[i]];
-			}
-			r_query_result.path_owner_ids.resize(indices_count);
-		}
-	}
-
-	// add path stats
-
-	return r_query_result;
+	NavMeshQueries3D::map_query_path(map, p_query_parameters, p_query_result, p_callback);
 }
 
 RID GodotNavigationServer3D::source_geometry_parser_create() {
@@ -1490,86 +1436,32 @@ Vector<Vector3> GodotNavigationServer3D::simplify_path(const Vector<Vector3> &p_
 
 	p_epsilon = MAX(0.0, p_epsilon);
 
-	LocalVector<uint32_t> simplified_path_indices = get_simplified_path_indices(p_path, p_epsilon);
+	LocalVector<Vector3> source_path;
+	{
+		source_path.resize(p_path.size());
+		const Vector3 *r = p_path.ptr();
+		for (uint32_t i = 0; i < p_path.size(); i++) {
+			source_path[i] = r[i];
+		}
+	}
 
-	uint32_t indices_count = simplified_path_indices.size();
+	LocalVector<uint32_t> simplified_path_indices = NavMeshQueries3D::get_simplified_path_indices(source_path, p_epsilon);
+
+	uint32_t index_count = simplified_path_indices.size();
 
 	Vector<Vector3> simplified_path;
-	simplified_path.resize(indices_count);
-
-	Vector3 *w = simplified_path.ptrw();
-	const Vector3 *r = p_path.ptr();
-	for (uint32_t i = 0; i < indices_count; i++) {
-		w[i] = r[simplified_path_indices[i]];
+	{
+		simplified_path.resize(index_count);
+		Vector3 *w = simplified_path.ptrw();
+		const Vector3 *r = source_path.ptr();
+		for (uint32_t i = 0; i < index_count; i++) {
+			w[i] = r[simplified_path_indices[i]];
+		}
 	}
 
 	return simplified_path;
 }
 
-LocalVector<uint32_t> GodotNavigationServer3D::get_simplified_path_indices(const Vector<Vector3> &p_path, real_t p_epsilon) {
-	p_epsilon = MAX(0.0, p_epsilon);
-	real_t squared_epsilon = p_epsilon * p_epsilon;
-
-	LocalVector<bool> valid_points;
-	valid_points.resize(p_path.size());
-	for (uint32_t i = 0; i < valid_points.size(); i++) {
-		valid_points[i] = false;
-	}
-
-	simplify_path_segment(0, p_path.size() - 1, p_path, squared_epsilon, valid_points);
-
-	int valid_point_index = 0;
-
-	for (bool valid : valid_points) {
-		if (valid) {
-			valid_point_index += 1;
-		}
-	}
-
-	LocalVector<uint32_t> simplified_path_indices;
-	simplified_path_indices.resize(valid_point_index);
-	valid_point_index = 0;
-
-	for (uint32_t i = 0; i < valid_points.size(); i++) {
-		if (valid_points[i]) {
-			simplified_path_indices[valid_point_index] = i;
-			valid_point_index += 1;
-		}
-	}
-
-	return simplified_path_indices;
-}
-
-void GodotNavigationServer3D::simplify_path_segment(int p_start_inx, int p_end_inx, const Vector<Vector3> &p_points, real_t p_epsilon, LocalVector<bool> &r_valid_points) {
-	r_valid_points[p_start_inx] = true;
-	r_valid_points[p_end_inx] = true;
-
-	const Vector3 &start_point = p_points[p_start_inx];
-	const Vector3 &end_point = p_points[p_end_inx];
-
-	Vector3 path_segment[2] = { start_point, end_point };
-
-	real_t point_max_distance = 0.0;
-	int point_max_index = 0;
-
-	for (int i = p_start_inx; i < p_end_inx; i++) {
-		const Vector3 &checked_point = p_points[i];
-
-		const Vector3 closest_point = Geometry3D::get_closest_point_to_segment(checked_point, path_segment);
-		real_t distance_squared = closest_point.distance_squared_to(checked_point);
-
-		if (distance_squared > point_max_distance) {
-			point_max_index = i;
-			point_max_distance = distance_squared;
-		}
-	}
-
-	if (point_max_distance > p_epsilon) {
-		simplify_path_segment(p_start_inx, point_max_index, p_points, p_epsilon, r_valid_points);
-		simplify_path_segment(point_max_index, p_end_inx, p_points, p_epsilon, r_valid_points);
-	}
-}
-
 int GodotNavigationServer3D::get_process_info(ProcessInfo p_info) const {
 	switch (p_info) {
 		case INFO_ACTIVE_MAPS: {
diff --git a/modules/navigation/3d/godot_navigation_server_3d.h b/modules/navigation/3d/godot_navigation_server_3d.h
index eae6ea2860..ce8d333535 100644
--- a/modules/navigation/3d/godot_navigation_server_3d.h
+++ b/modules/navigation/3d/godot_navigation_server_3d.h
@@ -40,6 +40,8 @@
 #include "core/templates/local_vector.h"
 #include "core/templates/rid.h"
 #include "core/templates/rid_owner.h"
+#include "servers/navigation/navigation_path_query_parameters_3d.h"
+#include "servers/navigation/navigation_path_query_result_3d.h"
 #include "servers/navigation_server_3d.h"
 
 /// The commands are functions executed during the `sync` phase.
@@ -130,7 +132,7 @@ public:
 	COMMAND_2(map_set_link_connection_radius, RID, p_map, real_t, p_connection_radius);
 	virtual real_t map_get_link_connection_radius(RID p_map) const override;
 
-	virtual Vector<Vector3> map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) const override;
+	virtual Vector<Vector3> map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) override;
 
 	virtual Vector3 map_get_closest_point_to_segment(RID p_map, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision = false) const override;
 	virtual Vector3 map_get_closest_point(RID p_map, const Vector3 &p_point) const override;
@@ -273,10 +275,6 @@ public:
 
 	virtual Vector<Vector3> simplify_path(const Vector<Vector3> &p_path, real_t p_epsilon) override;
 
-private:
-	static void simplify_path_segment(int p_start_inx, int p_end_inx, const Vector<Vector3> &p_points, real_t p_epsilon, LocalVector<bool> &r_valid_points);
-	static LocalVector<uint32_t> get_simplified_path_indices(const Vector<Vector3> &p_path, real_t p_epsilon);
-
 public:
 	COMMAND_1(free, RID, p_object);
 
@@ -288,7 +286,7 @@ public:
 	virtual void sync() override;
 	virtual void finish() override;
 
-	virtual NavigationUtilities::PathQueryResult _query_path(const NavigationUtilities::PathQueryParameters &p_parameters) const override;
+	virtual void query_path(const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result, const Callable &p_callback = Callable()) override;
 
 	int get_process_info(ProcessInfo p_info) const override;
 
diff --git a/modules/navigation/3d/nav_mesh_queries_3d.cpp b/modules/navigation/3d/nav_mesh_queries_3d.cpp
index fa7ccbeb70..b60d8d073d 100644
--- a/modules/navigation/3d/nav_mesh_queries_3d.cpp
+++ b/modules/navigation/3d/nav_mesh_queries_3d.cpp
@@ -33,21 +33,22 @@
 #include "nav_mesh_queries_3d.h"
 
 #include "../nav_base.h"
+#include "../nav_map.h"
 
 #include "core/math/geometry_3d.h"
+#include "servers/navigation/navigation_utilities.h"
 
 #define THREE_POINTS_CROSS_PRODUCT(m_a, m_b, m_c) (((m_c) - (m_a)).cross((m_b) - (m_a)))
 
-#define APPEND_METADATA(poly)                                  \
-	if (r_path_types) {                                        \
-		r_path_types->push_back(poly->owner->get_type());      \
-	}                                                          \
-	if (r_path_rids) {                                         \
-		r_path_rids->push_back(poly->owner->get_self());       \
-	}                                                          \
-	if (r_path_owners) {                                       \
-		r_path_owners->push_back(poly->owner->get_owner_id()); \
-	}
+bool NavMeshQueries3D::emit_callback(const Callable &p_callback) {
+	ERR_FAIL_COND_V(!p_callback.is_valid(), false);
+
+	Callable::CallError ce;
+	Variant result;
+	p_callback.callp(nullptr, 0, result, ce);
+
+	return ce.error == Callable::CallError::CALL_OK;
+}
 
 Vector3 NavMeshQueries3D::polygons_get_random_point(const LocalVector<gd::Polygon> &p_polygons, uint32_t p_navigation_layers, bool p_uniformly) {
 	const LocalVector<gd::Polygon> &region_polygons = p_polygons;
@@ -127,87 +128,225 @@ Vector3 NavMeshQueries3D::polygons_get_random_point(const LocalVector<gd::Polygo
 	}
 }
 
-Vector<Vector3> NavMeshQueries3D::polygons_get_path(const LocalVector<gd::Polygon> &p_polygons, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *r_path_owners, const Vector3 &p_map_up, uint32_t p_link_polygons_size) {
-	// Clear metadata outputs.
-	if (r_path_types) {
-		r_path_types->clear();
-	}
-	if (r_path_rids) {
-		r_path_rids->clear();
-	}
-	if (r_path_owners) {
-		r_path_owners->clear();
+void NavMeshQueries3D::_query_task_create_same_polygon_two_point_path(NavMeshPathQueryTask3D &p_query_task, const gd::Polygon *begin_poly, Vector3 begin_point, const gd::Polygon *end_poly, Vector3 end_point) {
+	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
+		p_query_task.path_meta_point_types.resize(2);
+		p_query_task.path_meta_point_types[0] = begin_poly->owner->get_type();
+		p_query_task.path_meta_point_types[1] = end_poly->owner->get_type();
 	}
 
-	// Find the start poly and the end poly on this map.
+	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) {
+		p_query_task.path_meta_point_rids.resize(2);
+		p_query_task.path_meta_point_rids[0] = begin_poly->owner->get_self();
+		p_query_task.path_meta_point_rids[1] = end_poly->owner->get_self();
+	}
+
+	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) {
+		p_query_task.path_meta_point_owners.resize(2);
+		p_query_task.path_meta_point_owners[0] = begin_poly->owner->get_owner_id();
+		p_query_task.path_meta_point_owners[1] = end_poly->owner->get_owner_id();
+	}
+
+	p_query_task.path_points.resize(2);
+	p_query_task.path_points[0] = begin_point;
+	p_query_task.path_points[1] = end_point;
+}
+
+void NavMeshQueries3D::_query_task_push_back_point_with_metadata(NavMeshPathQueryTask3D &p_query_task, Vector3 p_point, const gd::Polygon *p_point_polygon) {
+	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
+		p_query_task.path_meta_point_types.push_back(p_point_polygon->owner->get_type());
+	}
+
+	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) {
+		p_query_task.path_meta_point_rids.push_back(p_point_polygon->owner->get_self());
+	}
+
+	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) {
+		p_query_task.path_meta_point_owners.push_back(p_point_polygon->owner->get_owner_id());
+	}
+
+	p_query_task.path_points.push_back(p_point);
+}
+
+void NavMeshQueries3D::map_query_path(NavMap *map, const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result, const Callable &p_callback) {
+	ERR_FAIL_NULL(map);
+	ERR_FAIL_COND(p_query_parameters.is_null());
+	ERR_FAIL_COND(p_query_result.is_null());
+
+	using namespace NavigationUtilities;
+
+	NavMeshQueries3D::NavMeshPathQueryTask3D query_task;
+	query_task.start_position = p_query_parameters->get_start_position();
+	query_task.target_position = p_query_parameters->get_target_position();
+	query_task.navigation_layers = p_query_parameters->get_navigation_layers();
+	query_task.callback = p_callback;
+
+	switch (p_query_parameters->get_pathfinding_algorithm()) {
+		case NavigationPathQueryParameters3D::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR: {
+			query_task.pathfinding_algorithm = PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR;
+		} break;
+		default: {
+			WARN_PRINT("No match for used PathfindingAlgorithm - fallback to default");
+			query_task.pathfinding_algorithm = PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR;
+		} break;
+	}
+
+	switch (p_query_parameters->get_path_postprocessing()) {
+		case NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL: {
+			query_task.path_postprocessing = PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL;
+		} break;
+		case NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED: {
+			query_task.path_postprocessing = PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED;
+		} break;
+		case NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_NONE: {
+			query_task.path_postprocessing = PathPostProcessing::PATH_POSTPROCESSING_NONE;
+		} break;
+		default: {
+			WARN_PRINT("No match for used PathPostProcessing - fallback to default");
+			query_task.path_postprocessing = PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL;
+		} break;
+	}
+
+	query_task.metadata_flags = (int64_t)p_query_parameters->get_metadata_flags();
+	query_task.simplify_path = p_query_parameters->get_simplify_path();
+	query_task.simplify_epsilon = p_query_parameters->get_simplify_epsilon();
+	query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_STARTED;
+
+	map->query_path(query_task);
+
+	const uint32_t path_point_size = query_task.path_points.size();
+
+	Vector<Vector3> path_points;
+	Vector<int32_t> path_meta_point_types;
+	TypedArray<RID> path_meta_point_rids;
+	Vector<int64_t> path_meta_point_owners;
+
+	{
+		path_points.resize(path_point_size);
+		Vector3 *w = path_points.ptrw();
+		const Vector3 *r = query_task.path_points.ptr();
+		for (uint32_t i = 0; i < path_point_size; i++) {
+			w[i] = r[i];
+		}
+	}
+
+	if (query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
+		path_meta_point_types.resize(path_point_size);
+		int32_t *w = path_meta_point_types.ptrw();
+		const int32_t *r = query_task.path_meta_point_types.ptr();
+		for (uint32_t i = 0; i < path_point_size; i++) {
+			w[i] = r[i];
+		}
+	}
+	if (query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) {
+		path_meta_point_rids.resize(path_point_size);
+		for (uint32_t i = 0; i < path_point_size; i++) {
+			path_meta_point_rids[i] = query_task.path_meta_point_rids[i];
+		}
+	}
+	if (query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) {
+		path_meta_point_owners.resize(path_point_size);
+		int64_t *w = path_meta_point_owners.ptrw();
+		const int64_t *r = query_task.path_meta_point_owners.ptr();
+		for (uint32_t i = 0; i < path_point_size; i++) {
+			w[i] = r[i];
+		}
+	}
+
+	p_query_result->set_path(path_points);
+	p_query_result->set_path_types(path_meta_point_types);
+	p_query_result->set_path_rids(path_meta_point_rids);
+	p_query_result->set_path_owner_ids(path_meta_point_owners);
+
+	if (query_task.callback.is_valid()) {
+		if (emit_callback(query_task.callback)) {
+			query_task.status = NavMeshPathQueryTask3D::TaskStatus::CALLBACK_DISPATCHED;
+		} else {
+			query_task.status = NavMeshPathQueryTask3D::TaskStatus::CALLBACK_FAILED;
+		}
+	}
+}
+
+void NavMeshQueries3D::query_task_polygons_get_path(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_map_up, uint32_t p_link_polygons_size) {
+	p_query_task.path_points.clear();
+	p_query_task.path_meta_point_types.clear();
+	p_query_task.path_meta_point_rids.clear();
+	p_query_task.path_meta_point_owners.clear();
+
+	// Find begin polyon and begin position closest to start position and
+	// end polyon and end position closest to target position on the map.
 	const gd::Polygon *begin_poly = nullptr;
 	const gd::Polygon *end_poly = nullptr;
 	Vector3 begin_point;
 	Vector3 end_point;
-	real_t begin_d = FLT_MAX;
-	real_t end_d = FLT_MAX;
-	// Find the initial poly and the end poly on this map.
-	for (const gd::Polygon &p : p_polygons) {
-		// Only consider the polygon if it in a region with compatible layers.
-		if ((p_navigation_layers & p.owner->get_navigation_layers()) == 0) {
-			continue;
-		}
 
-		// For each face check the distance between the origin/destination
-		for (size_t point_id = 2; point_id < p.points.size(); point_id++) {
-			const Face3 face(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
-
-			Vector3 point = face.get_closest_point_to(p_origin);
-			real_t distance_to_point = point.distance_to(p_origin);
-			if (distance_to_point < begin_d) {
-				begin_d = distance_to_point;
-				begin_poly = &p;
-				begin_point = point;
-			}
-
-			point = face.get_closest_point_to(p_destination);
-			distance_to_point = point.distance_to(p_destination);
-			if (distance_to_point < end_d) {
-				end_d = distance_to_point;
-				end_poly = &p;
-				end_point = point;
-			}
-		}
-	}
+	_query_task_find_start_end_positions(p_query_task, p_polygons, &begin_poly, begin_point, &end_poly, end_point);
 
 	// Check for trivial cases
 	if (!begin_poly || !end_poly) {
-		return Vector<Vector3>();
+		p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FAILED;
+		return;
 	}
+
 	if (begin_poly == end_poly) {
-		if (r_path_types) {
-			r_path_types->resize(2);
-			r_path_types->write[0] = begin_poly->owner->get_type();
-			r_path_types->write[1] = end_poly->owner->get_type();
-		}
-
-		if (r_path_rids) {
-			r_path_rids->resize(2);
-			(*r_path_rids)[0] = begin_poly->owner->get_self();
-			(*r_path_rids)[1] = end_poly->owner->get_self();
-		}
-
-		if (r_path_owners) {
-			r_path_owners->resize(2);
-			r_path_owners->write[0] = begin_poly->owner->get_owner_id();
-			r_path_owners->write[1] = end_poly->owner->get_owner_id();
-		}
-
-		Vector<Vector3> path;
-		path.resize(2);
-		path.write[0] = begin_point;
-		path.write[1] = end_point;
-		return path;
+		_query_task_create_same_polygon_two_point_path(p_query_task, begin_poly, begin_point, end_poly, end_point);
+		return;
 	}
 
+	_query_task_build_path_corridor(p_query_task, p_polygons, p_map_up, p_link_polygons_size, begin_poly, begin_point, end_poly, end_point);
+
+	// Post-Process path.
+	switch (p_query_task.path_postprocessing) {
+		case PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL: {
+			_path_corridor_post_process_corridorfunnel(p_query_task, p_query_task.least_cost_id, begin_poly, begin_point, end_poly, end_point, p_map_up);
+		} break;
+		case PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED: {
+			_path_corridor_post_process_edgecentered(p_query_task, p_query_task.least_cost_id, begin_poly, begin_point, end_poly, end_point);
+		} break;
+		case PathPostProcessing::PATH_POSTPROCESSING_NONE: {
+			_path_corridor_post_process_nopostprocessing(p_query_task, p_query_task.least_cost_id, begin_poly, begin_point, end_poly, end_point);
+		} break;
+		default: {
+			WARN_PRINT("No match for used PathPostProcessing - fallback to default");
+			_path_corridor_post_process_corridorfunnel(p_query_task, p_query_task.least_cost_id, begin_poly, begin_point, end_poly, end_point, p_map_up);
+		} break;
+	}
+
+	p_query_task.path_points.invert();
+	p_query_task.path_meta_point_types.invert();
+	p_query_task.path_meta_point_rids.invert();
+	p_query_task.path_meta_point_owners.invert();
+
+	if (p_query_task.simplify_path) {
+		_query_task_simplified_path_points(p_query_task);
+	}
+
+#ifdef DEBUG_ENABLED
+	// Ensure post conditions as path meta arrays if used MUST match in array size with the path points.
+	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
+		DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_types.size());
+	}
+
+	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) {
+		DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_rids.size());
+	}
+
+	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) {
+		DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_owners.size());
+	}
+#endif // DEBUG_ENABLED
+
+	p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED;
+}
+
+void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_map_up, uint32_t p_link_polygons_size, const gd::Polygon *begin_poly, Vector3 begin_point, const gd::Polygon *end_poly, Vector3 end_point) {
 	// List of all reachable navigation polys.
-	LocalVector<gd::NavigationPoly> navigation_polys;
-	navigation_polys.resize(p_polygons.size() + p_link_polygons_size);
+	LocalVector<gd::NavigationPoly> &navigation_polys = p_query_task.path_query_slot->path_corridor;
+	for (gd::NavigationPoly &polygon : navigation_polys) {
+		polygon.reset();
+	}
+
+	DEV_ASSERT(navigation_polys.size() == p_polygons.size() + p_link_polygons_size);
 
 	// Initialize the matching navigation polygon.
 	gd::NavigationPoly &begin_navigation_poly = navigation_polys[begin_poly->id];
@@ -218,11 +357,12 @@ Vector<Vector3> NavMeshQueries3D::polygons_get_path(const LocalVector<gd::Polygo
 
 	// Heap of polygons to travel next.
 	gd::Heap<gd::NavigationPoly *, gd::NavPolyTravelCostGreaterThan, gd::NavPolyHeapIndexer>
-			traversable_polys;
+			&traversable_polys = p_query_task.path_query_slot->traversable_polys;
+	traversable_polys.clear();
 	traversable_polys.reserve(p_polygons.size() * 0.25);
 
 	// This is an implementation of the A* algorithm.
-	int least_cost_id = begin_poly->id;
+	p_query_task.least_cost_id = begin_poly->id;
 	int prev_least_cost_id = -1;
 	bool found_route = false;
 
@@ -232,24 +372,24 @@ Vector<Vector3> NavMeshQueries3D::polygons_get_path(const LocalVector<gd::Polygo
 
 	while (true) {
 		// Takes the current least_cost_poly neighbors (iterating over its edges) and compute the traveled_distance.
-		for (const gd::Edge &edge : navigation_polys[least_cost_id].poly->edges) {
+		for (const gd::Edge &edge : navigation_polys[p_query_task.least_cost_id].poly->edges) {
 			// Iterate over connections in this edge, then compute the new optimized travel distance assigned to this polygon.
 			for (uint32_t connection_index = 0; connection_index < edge.connections.size(); connection_index++) {
 				const gd::Edge::Connection &connection = edge.connections[connection_index];
 
 				// Only consider the connection to another polygon if this polygon is in a region with compatible layers.
-				if ((p_navigation_layers & connection.polygon->owner->get_navigation_layers()) == 0) {
+				if ((p_query_task.navigation_layers & connection.polygon->owner->get_navigation_layers()) == 0) {
 					continue;
 				}
 
-				const gd::NavigationPoly &least_cost_poly = navigation_polys[least_cost_id];
+				const gd::NavigationPoly &least_cost_poly = navigation_polys[p_query_task.least_cost_id];
 				real_t poly_enter_cost = 0.0;
 				real_t poly_travel_cost = least_cost_poly.poly->owner->get_travel_cost();
 
 				if (prev_least_cost_id != -1 && navigation_polys[prev_least_cost_id].poly->owner->get_self() != least_cost_poly.poly->owner->get_self()) {
 					poly_enter_cost = least_cost_poly.poly->owner->get_enter_cost();
 				}
-				prev_least_cost_id = least_cost_id;
+				prev_least_cost_id = p_query_task.least_cost_id;
 
 				Vector3 pathway[2] = { connection.pathway_start, connection.pathway_end };
 				const Vector3 new_entry = Geometry3D::get_closest_point_to_segment(least_cost_poly.entry, pathway);
@@ -262,7 +402,7 @@ Vector<Vector3> NavMeshQueries3D::polygons_get_path(const LocalVector<gd::Polygo
 					// it is shorter, update the polygon.
 					if (neighbor_poly.traversable_poly_index < traversable_polys.size() &&
 							new_traveled_distance < neighbor_poly.traveled_distance) {
-						neighbor_poly.back_navigation_poly_id = least_cost_id;
+						neighbor_poly.back_navigation_poly_id = p_query_task.least_cost_id;
 						neighbor_poly.back_navigation_edge = connection.edge;
 						neighbor_poly.back_navigation_edge_pathway_start = connection.pathway_start;
 						neighbor_poly.back_navigation_edge_pathway_end = connection.pathway_end;
@@ -278,7 +418,7 @@ Vector<Vector3> NavMeshQueries3D::polygons_get_path(const LocalVector<gd::Polygo
 				} else {
 					// Initialize the matching navigation polygon.
 					neighbor_poly.poly = connection.polygon;
-					neighbor_poly.back_navigation_poly_id = least_cost_id;
+					neighbor_poly.back_navigation_poly_id = p_query_task.least_cost_id;
 					neighbor_poly.back_navigation_edge = connection.edge;
 					neighbor_poly.back_navigation_edge_pathway_start = connection.pathway_start;
 					neighbor_poly.back_navigation_edge_pathway_end = connection.pathway_end;
@@ -307,11 +447,11 @@ Vector<Vector3> NavMeshQueries3D::polygons_get_path(const LocalVector<gd::Polygo
 
 			// Set as end point the furthest reachable point.
 			end_poly = reachable_end;
-			end_d = FLT_MAX;
+			real_t end_d = FLT_MAX;
 			for (size_t point_id = 2; point_id < end_poly->points.size(); point_id++) {
 				Face3 f(end_poly->points[0].pos, end_poly->points[point_id - 1].pos, end_poly->points[point_id].pos);
-				Vector3 spoint = f.get_closest_point_to(p_destination);
-				real_t dpoint = spoint.distance_to(p_destination);
+				Vector3 spoint = f.get_closest_point_to(p_query_task.target_position);
+				real_t dpoint = spoint.distance_to(p_query_task.target_position);
 				if (dpoint < end_d) {
 					end_point = spoint;
 					end_d = dpoint;
@@ -322,8 +462,8 @@ Vector<Vector3> NavMeshQueries3D::polygons_get_path(const LocalVector<gd::Polygo
 			bool closest_point_on_start_poly = false;
 			for (size_t point_id = 2; point_id < begin_poly->points.size(); point_id++) {
 				Face3 f(begin_poly->points[0].pos, begin_poly->points[point_id - 1].pos, begin_poly->points[point_id].pos);
-				Vector3 spoint = f.get_closest_point_to(p_destination);
-				real_t dpoint = spoint.distance_to(p_destination);
+				Vector3 spoint = f.get_closest_point_to(p_query_task.target_position);
+				real_t dpoint = spoint.distance_to(p_query_task.target_position);
 				if (dpoint < end_d) {
 					end_point = spoint;
 					end_d = dpoint;
@@ -332,30 +472,8 @@ Vector<Vector3> NavMeshQueries3D::polygons_get_path(const LocalVector<gd::Polygo
 			}
 
 			if (closest_point_on_start_poly) {
-				// No point to run PostProcessing when start and end convex polygon is the same.
-				if (r_path_types) {
-					r_path_types->resize(2);
-					r_path_types->write[0] = begin_poly->owner->get_type();
-					r_path_types->write[1] = begin_poly->owner->get_type();
-				}
-
-				if (r_path_rids) {
-					r_path_rids->resize(2);
-					(*r_path_rids)[0] = begin_poly->owner->get_self();
-					(*r_path_rids)[1] = begin_poly->owner->get_self();
-				}
-
-				if (r_path_owners) {
-					r_path_owners->resize(2);
-					r_path_owners->write[0] = begin_poly->owner->get_owner_id();
-					r_path_owners->write[1] = begin_poly->owner->get_owner_id();
-				}
-
-				Vector<Vector3> path;
-				path.resize(2);
-				path.write[0] = begin_point;
-				path.write[1] = end_point;
-				return path;
+				_query_task_create_same_polygon_two_point_path(p_query_task, begin_poly, begin_point, end_poly, end_point);
+				return;
 			}
 
 			for (gd::NavigationPoly &nav_poly : navigation_polys) {
@@ -363,7 +481,7 @@ Vector<Vector3> NavMeshQueries3D::polygons_get_path(const LocalVector<gd::Polygo
 			}
 			navigation_polys[begin_poly->id].poly = begin_poly;
 
-			least_cost_id = begin_poly->id;
+			p_query_task.least_cost_id = begin_poly->id;
 			prev_least_cost_id = -1;
 
 			reachable_end = nullptr;
@@ -372,19 +490,19 @@ Vector<Vector3> NavMeshQueries3D::polygons_get_path(const LocalVector<gd::Polygo
 		}
 
 		// Pop the polygon with the lowest travel cost from the heap of traversable polygons.
-		least_cost_id = traversable_polys.pop()->poly->id;
+		p_query_task.least_cost_id = traversable_polys.pop()->poly->id;
 
 		// Store the farthest reachable end polygon in case our goal is not reachable.
 		if (is_reachable) {
-			real_t distance = navigation_polys[least_cost_id].entry.distance_to(p_destination);
+			real_t distance = navigation_polys[p_query_task.least_cost_id].entry.distance_to(p_query_task.target_position);
 			if (distance_to_reachable_end > distance) {
 				distance_to_reachable_end = distance;
-				reachable_end = navigation_polys[least_cost_id].poly;
+				reachable_end = navigation_polys[p_query_task.least_cost_id].poly;
 			}
 		}
 
 		// Check if we reached the end
-		if (navigation_polys[least_cost_id].poly == end_poly) {
+		if (navigation_polys[p_query_task.least_cost_id].poly == end_poly) {
 			found_route = true;
 			break;
 		}
@@ -393,190 +511,227 @@ Vector<Vector3> NavMeshQueries3D::polygons_get_path(const LocalVector<gd::Polygo
 	// We did not find a route but we have both a start polygon and an end polygon at this point.
 	// Usually this happens because there was not a single external or internal connected edge, e.g. our start polygon is an isolated, single convex polygon.
 	if (!found_route) {
-		end_d = FLT_MAX;
+		real_t end_d = FLT_MAX;
 		// Search all faces of the start polygon for the closest point to our target position.
 		for (size_t point_id = 2; point_id < begin_poly->points.size(); point_id++) {
 			Face3 f(begin_poly->points[0].pos, begin_poly->points[point_id - 1].pos, begin_poly->points[point_id].pos);
-			Vector3 spoint = f.get_closest_point_to(p_destination);
-			real_t dpoint = spoint.distance_to(p_destination);
+			Vector3 spoint = f.get_closest_point_to(p_query_task.target_position);
+			real_t dpoint = spoint.distance_to(p_query_task.target_position);
 			if (dpoint < end_d) {
 				end_point = spoint;
 				end_d = dpoint;
 			}
 		}
+		_query_task_create_same_polygon_two_point_path(p_query_task, begin_poly, begin_point, begin_poly, end_point);
+		return;
+	}
+}
 
-		if (r_path_types) {
-			r_path_types->resize(2);
-			r_path_types->write[0] = begin_poly->owner->get_type();
-			r_path_types->write[1] = begin_poly->owner->get_type();
-		}
-
-		if (r_path_rids) {
-			r_path_rids->resize(2);
-			(*r_path_rids)[0] = begin_poly->owner->get_self();
-			(*r_path_rids)[1] = begin_poly->owner->get_self();
-		}
-
-		if (r_path_owners) {
-			r_path_owners->resize(2);
-			r_path_owners->write[0] = begin_poly->owner->get_owner_id();
-			r_path_owners->write[1] = begin_poly->owner->get_owner_id();
-		}
-
-		Vector<Vector3> path;
-		path.resize(2);
-		path.write[0] = begin_point;
-		path.write[1] = end_point;
-		return path;
+void NavMeshQueries3D::_query_task_simplified_path_points(NavMeshPathQueryTask3D &p_query_task) {
+	if (!p_query_task.simplify_path || p_query_task.path_points.size() <= 2) {
+		return;
 	}
 
-	Vector<Vector3> path;
-	// Optimize the path.
-	if (p_optimize) {
-		// Set the apex poly/point to the end point
-		gd::NavigationPoly *apex_poly = &navigation_polys[least_cost_id];
+	const LocalVector<uint32_t> &simplified_path_indices = NavMeshQueries3D::get_simplified_path_indices(p_query_task.path_points, p_query_task.simplify_epsilon);
 
-		Vector3 back_pathway[2] = { apex_poly->back_navigation_edge_pathway_start, apex_poly->back_navigation_edge_pathway_end };
-		const Vector3 back_edge_closest_point = Geometry3D::get_closest_point_to_segment(end_point, back_pathway);
-		if (end_point.is_equal_approx(back_edge_closest_point)) {
-			// The end point is basically on top of the last crossed edge, funneling around the corners would at best do nothing.
-			// At worst it would add an unwanted path point before the last point due to precision issues so skip to the next polygon.
-			if (apex_poly->back_navigation_poly_id != -1) {
-				apex_poly = &navigation_polys[apex_poly->back_navigation_poly_id];
-			}
+	uint32_t index_count = simplified_path_indices.size();
+
+	{
+		Vector3 *points_ptr = p_query_task.path_points.ptr();
+		for (uint32_t i = 0; i < index_count; i++) {
+			points_ptr[i] = points_ptr[simplified_path_indices[i]];
 		}
+		p_query_task.path_points.resize(index_count);
+	}
 
-		Vector3 apex_point = end_point;
-
-		gd::NavigationPoly *left_poly = apex_poly;
-		Vector3 left_portal = apex_point;
-		gd::NavigationPoly *right_poly = apex_poly;
-		Vector3 right_portal = apex_point;
-
-		gd::NavigationPoly *p = apex_poly;
-
-		path.push_back(end_point);
-		APPEND_METADATA(end_poly);
-
-		while (p) {
-			// Set left and right points of the pathway between polygons.
-			Vector3 left = p->back_navigation_edge_pathway_start;
-			Vector3 right = p->back_navigation_edge_pathway_end;
-			if (THREE_POINTS_CROSS_PRODUCT(apex_point, left, right).dot(p_map_up) < 0) {
-				SWAP(left, right);
-			}
-
-			bool skip = false;
-			if (THREE_POINTS_CROSS_PRODUCT(apex_point, left_portal, left).dot(p_map_up) >= 0) {
-				//process
-				if (left_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, left, right_portal).dot(p_map_up) > 0) {
-					left_poly = p;
-					left_portal = left;
-				} else {
-					clip_path(navigation_polys, path, apex_poly, right_portal, right_poly, r_path_types, r_path_rids, r_path_owners, p_map_up);
-
-					apex_point = right_portal;
-					p = right_poly;
-					left_poly = p;
-					apex_poly = p;
-					left_portal = apex_point;
-					right_portal = apex_point;
-
-					path.push_back(apex_point);
-					APPEND_METADATA(apex_poly->poly);
-					skip = true;
-				}
-			}
-
-			if (!skip && THREE_POINTS_CROSS_PRODUCT(apex_point, right_portal, right).dot(p_map_up) <= 0) {
-				//process
-				if (right_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, right, left_portal).dot(p_map_up) < 0) {
-					right_poly = p;
-					right_portal = right;
-				} else {
-					clip_path(navigation_polys, path, apex_poly, left_portal, left_poly, r_path_types, r_path_rids, r_path_owners, p_map_up);
-
-					apex_point = left_portal;
-					p = left_poly;
-					right_poly = p;
-					apex_poly = p;
-					right_portal = apex_point;
-					left_portal = apex_point;
-
-					path.push_back(apex_point);
-					APPEND_METADATA(apex_poly->poly);
-				}
-			}
-
-			// Go to the previous polygon.
-			if (p->back_navigation_poly_id != -1) {
-				p = &navigation_polys[p->back_navigation_poly_id];
-			} else {
-				// The end
-				p = nullptr;
-			}
+	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
+		int32_t *types_ptr = p_query_task.path_meta_point_types.ptr();
+		for (uint32_t i = 0; i < index_count; i++) {
+			types_ptr[i] = types_ptr[simplified_path_indices[i]];
 		}
+		p_query_task.path_meta_point_types.resize(index_count);
+	}
 
-		// If the last point is not the begin point, add it to the list.
-		if (path[path.size() - 1] != begin_point) {
-			path.push_back(begin_point);
-			APPEND_METADATA(begin_poly);
+	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) {
+		RID *rids_ptr = p_query_task.path_meta_point_rids.ptr();
+		for (uint32_t i = 0; i < index_count; i++) {
+			rids_ptr[i] = rids_ptr[simplified_path_indices[i]];
 		}
+		p_query_task.path_meta_point_rids.resize(index_count);
+	}
 
-		path.reverse();
-		if (r_path_types) {
-			r_path_types->reverse();
-		}
-		if (r_path_rids) {
-			r_path_rids->reverse();
-		}
-		if (r_path_owners) {
-			r_path_owners->reverse();
+	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) {
+		int64_t *owners_ptr = p_query_task.path_meta_point_owners.ptr();
+		for (uint32_t i = 0; i < index_count; i++) {
+			owners_ptr[i] = owners_ptr[simplified_path_indices[i]];
 		}
+		p_query_task.path_meta_point_owners.resize(index_count);
+	}
+}
 
-	} else {
-		path.push_back(end_point);
-		APPEND_METADATA(end_poly);
+void NavMeshQueries3D::_path_corridor_post_process_corridorfunnel(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point, const Vector3 &p_map_up) {
+	LocalVector<gd::NavigationPoly> &p_path_corridor = p_query_task.path_query_slot->path_corridor;
 
-		// Add mid points
-		int np_id = least_cost_id;
-		while (np_id != -1 && navigation_polys[np_id].back_navigation_poly_id != -1) {
-			if (navigation_polys[np_id].back_navigation_edge != -1) {
-				int prev = navigation_polys[np_id].back_navigation_edge;
-				int prev_n = (navigation_polys[np_id].back_navigation_edge + 1) % navigation_polys[np_id].poly->points.size();
-				Vector3 point = (navigation_polys[np_id].poly->points[prev].pos + navigation_polys[np_id].poly->points[prev_n].pos) * 0.5;
+	// Set the apex poly/point to the end point
+	gd::NavigationPoly *apex_poly = &p_path_corridor[p_least_cost_id];
 
-				path.push_back(point);
-				APPEND_METADATA(navigation_polys[np_id].poly);
-			} else {
-				path.push_back(navigation_polys[np_id].entry);
-				APPEND_METADATA(navigation_polys[np_id].poly);
-			}
-
-			np_id = navigation_polys[np_id].back_navigation_poly_id;
-		}
-
-		path.push_back(begin_point);
-		APPEND_METADATA(begin_poly);
-
-		path.reverse();
-		if (r_path_types) {
-			r_path_types->reverse();
-		}
-		if (r_path_rids) {
-			r_path_rids->reverse();
-		}
-		if (r_path_owners) {
-			r_path_owners->reverse();
+	Vector3 back_pathway[2] = { apex_poly->back_navigation_edge_pathway_start, apex_poly->back_navigation_edge_pathway_end };
+	const Vector3 back_edge_closest_point = Geometry3D::get_closest_point_to_segment(p_end_point, back_pathway);
+	if (p_end_point.is_equal_approx(back_edge_closest_point)) {
+		// The end point is basically on top of the last crossed edge, funneling around the corners would at best do nothing.
+		// At worst it would add an unwanted path point before the last point due to precision issues so skip to the next polygon.
+		if (apex_poly->back_navigation_poly_id != -1) {
+			apex_poly = &p_path_corridor[apex_poly->back_navigation_poly_id];
 		}
 	}
 
-	// Ensure post conditions (path arrays MUST match in size).
-	CRASH_COND(r_path_types && path.size() != r_path_types->size());
-	CRASH_COND(r_path_rids && path.size() != r_path_rids->size());
-	CRASH_COND(r_path_owners && path.size() != r_path_owners->size());
+	Vector3 apex_point = p_end_point;
 
-	return path;
+	gd::NavigationPoly *left_poly = apex_poly;
+	Vector3 left_portal = apex_point;
+	gd::NavigationPoly *right_poly = apex_poly;
+	Vector3 right_portal = apex_point;
+
+	gd::NavigationPoly *p = apex_poly;
+
+	_query_task_push_back_point_with_metadata(p_query_task, p_end_point, p_end_polygon);
+
+	while (p) {
+		// Set left and right points of the pathway between polygons.
+		Vector3 left = p->back_navigation_edge_pathway_start;
+		Vector3 right = p->back_navigation_edge_pathway_end;
+		if (THREE_POINTS_CROSS_PRODUCT(apex_point, left, right).dot(p_map_up) < 0) {
+			SWAP(left, right);
+		}
+
+		bool skip = false;
+		if (THREE_POINTS_CROSS_PRODUCT(apex_point, left_portal, left).dot(p_map_up) >= 0) {
+			//process
+			if (left_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, left, right_portal).dot(p_map_up) > 0) {
+				left_poly = p;
+				left_portal = left;
+			} else {
+				clip_path(p_query_task, p_path_corridor, apex_poly, right_portal, right_poly, p_map_up);
+
+				apex_point = right_portal;
+				p = right_poly;
+				left_poly = p;
+				apex_poly = p;
+				left_portal = apex_point;
+				right_portal = apex_point;
+
+				_query_task_push_back_point_with_metadata(p_query_task, apex_point, apex_poly->poly);
+
+				skip = true;
+			}
+		}
+
+		if (!skip && THREE_POINTS_CROSS_PRODUCT(apex_point, right_portal, right).dot(p_map_up) <= 0) {
+			//process
+			if (right_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, right, left_portal).dot(p_map_up) < 0) {
+				right_poly = p;
+				right_portal = right;
+			} else {
+				clip_path(p_query_task, p_path_corridor, apex_poly, left_portal, left_poly, p_map_up);
+
+				apex_point = left_portal;
+				p = left_poly;
+				right_poly = p;
+				apex_poly = p;
+				right_portal = apex_point;
+				left_portal = apex_point;
+
+				_query_task_push_back_point_with_metadata(p_query_task, apex_point, apex_poly->poly);
+			}
+		}
+
+		// Go to the previous polygon.
+		if (p->back_navigation_poly_id != -1) {
+			p = &p_path_corridor[p->back_navigation_poly_id];
+		} else {
+			// The end
+			p = nullptr;
+		}
+	}
+
+	// If the last point is not the begin point, add it to the list.
+	if (p_query_task.path_points[p_query_task.path_points.size() - 1] != p_begin_point) {
+		_query_task_push_back_point_with_metadata(p_query_task, p_begin_point, p_begin_poly);
+	}
+}
+
+void NavMeshQueries3D::_path_corridor_post_process_edgecentered(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point) {
+	LocalVector<gd::NavigationPoly> &p_path_corridor = p_query_task.path_query_slot->path_corridor;
+
+	_query_task_push_back_point_with_metadata(p_query_task, p_end_point, p_end_polygon);
+
+	// Add mid points.
+	int np_id = p_least_cost_id;
+	while (np_id != -1 && p_path_corridor[np_id].back_navigation_poly_id != -1) {
+		if (p_path_corridor[np_id].back_navigation_edge != -1) {
+			int prev = p_path_corridor[np_id].back_navigation_edge;
+			int prev_n = (p_path_corridor[np_id].back_navigation_edge + 1) % p_path_corridor[np_id].poly->points.size();
+			Vector3 point = (p_path_corridor[np_id].poly->points[prev].pos + p_path_corridor[np_id].poly->points[prev_n].pos) * 0.5;
+
+			_query_task_push_back_point_with_metadata(p_query_task, point, p_path_corridor[np_id].poly);
+		} else {
+			_query_task_push_back_point_with_metadata(p_query_task, p_path_corridor[np_id].entry, p_path_corridor[np_id].poly);
+		}
+
+		np_id = p_path_corridor[np_id].back_navigation_poly_id;
+	}
+
+	_query_task_push_back_point_with_metadata(p_query_task, p_begin_point, p_begin_poly);
+}
+
+void NavMeshQueries3D::_path_corridor_post_process_nopostprocessing(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point) {
+	LocalVector<gd::NavigationPoly> &p_path_corridor = p_query_task.path_query_slot->path_corridor;
+
+	_query_task_push_back_point_with_metadata(p_query_task, p_end_point, p_end_polygon);
+
+	// Add mid points.
+	int np_id = p_least_cost_id;
+	while (np_id != -1 && p_path_corridor[np_id].back_navigation_poly_id != -1) {
+		_query_task_push_back_point_with_metadata(p_query_task, p_path_corridor[np_id].entry, p_path_corridor[np_id].poly);
+
+		np_id = p_path_corridor[np_id].back_navigation_poly_id;
+	}
+
+	_query_task_push_back_point_with_metadata(p_query_task, p_begin_point, p_begin_poly);
+}
+
+void NavMeshQueries3D::_query_task_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons, const gd::Polygon **r_begin_poly, Vector3 &r_begin_point, const gd::Polygon **r_end_poly, Vector3 &r_end_point) {
+	real_t begin_d = FLT_MAX;
+	real_t end_d = FLT_MAX;
+
+	// Find the initial poly and the end poly on this map.
+	for (const gd::Polygon &p : p_polygons) {
+		// Only consider the polygon if it in a region with compatible layers.
+		if ((p_query_task.navigation_layers & p.owner->get_navigation_layers()) == 0) {
+			continue;
+		}
+
+		// For each face check the distance between the origin/destination.
+		for (size_t point_id = 2; point_id < p.points.size(); point_id++) {
+			const Face3 face(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
+
+			Vector3 point = face.get_closest_point_to(p_query_task.start_position);
+			real_t distance_to_point = point.distance_to(p_query_task.start_position);
+			if (distance_to_point < begin_d) {
+				begin_d = distance_to_point;
+				*r_begin_poly = &p;
+				r_begin_point = point;
+			}
+
+			point = face.get_closest_point_to(p_query_task.target_position);
+			distance_to_point = point.distance_to(p_query_task.target_position);
+			if (distance_to_point < end_d) {
+				end_d = distance_to_point;
+				*r_end_poly = &p;
+				r_end_point = point;
+			}
+		}
+	}
 }
 
 Vector3 NavMeshQueries3D::polygons_get_closest_point_to_segment(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) {
@@ -728,8 +883,8 @@ RID NavMeshQueries3D::polygons_get_closest_point_owner(const LocalVector<gd::Pol
 	return cp.owner;
 }
 
-void NavMeshQueries3D::clip_path(const LocalVector<gd::NavigationPoly> &p_navigation_polys, Vector<Vector3> &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *r_path_owners, const Vector3 &p_map_up) {
-	Vector3 from = path[path.size() - 1];
+void NavMeshQueries3D::clip_path(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::NavigationPoly> &p_navigation_polys, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, const Vector3 &p_map_up) {
+	Vector3 from = p_query_task.path_points[p_query_task.path_points.size() - 1];
 
 	if (from.is_equal_approx(p_to_point)) {
 		return;
@@ -753,13 +908,73 @@ void NavMeshQueries3D::clip_path(const LocalVector<gd::NavigationPoly> &p_naviga
 		if (!pathway_start.is_equal_approx(pathway_end)) {
 			Vector3 inters;
 			if (cut_plane.intersects_segment(pathway_start, pathway_end, &inters)) {
-				if (!inters.is_equal_approx(p_to_point) && !inters.is_equal_approx(path[path.size() - 1])) {
-					path.push_back(inters);
-					APPEND_METADATA(from_poly->poly);
+				if (!inters.is_equal_approx(p_to_point) && !inters.is_equal_approx(p_query_task.path_points[p_query_task.path_points.size() - 1])) {
+					_query_task_push_back_point_with_metadata(p_query_task, inters, from_poly->poly);
 				}
 			}
 		}
 	}
 }
 
+LocalVector<uint32_t> NavMeshQueries3D::get_simplified_path_indices(const LocalVector<Vector3> &p_path, real_t p_epsilon) {
+	p_epsilon = MAX(0.0, p_epsilon);
+	real_t squared_epsilon = p_epsilon * p_epsilon;
+
+	LocalVector<bool> valid_points;
+	valid_points.resize(p_path.size());
+	for (uint32_t i = 0; i < valid_points.size(); i++) {
+		valid_points[i] = false;
+	}
+
+	simplify_path_segment(0, p_path.size() - 1, p_path, squared_epsilon, valid_points);
+
+	int valid_point_index = 0;
+
+	for (bool valid : valid_points) {
+		if (valid) {
+			valid_point_index += 1;
+		}
+	}
+
+	LocalVector<uint32_t> simplified_path_indices;
+	simplified_path_indices.resize(valid_point_index);
+	valid_point_index = 0;
+
+	for (uint32_t i = 0; i < valid_points.size(); i++) {
+		if (valid_points[i]) {
+			simplified_path_indices[valid_point_index] = i;
+			valid_point_index += 1;
+		}
+	}
+
+	return simplified_path_indices;
+}
+
+void NavMeshQueries3D::simplify_path_segment(int p_start_inx, int p_end_inx, const LocalVector<Vector3> &p_points, real_t p_epsilon, LocalVector<bool> &r_valid_points) {
+	r_valid_points[p_start_inx] = true;
+	r_valid_points[p_end_inx] = true;
+
+	Vector3 path_segment[2] = { p_points[p_start_inx], p_points[p_end_inx] };
+
+	real_t point_max_distance = 0.0;
+	int point_max_index = 0;
+
+	for (int i = p_start_inx; i < p_end_inx; i++) {
+		const Vector3 &checked_point = p_points[i];
+
+		const Vector3 closest_point = Geometry3D::get_closest_point_to_segment(checked_point, path_segment);
+		real_t distance_squared = closest_point.distance_squared_to(checked_point);
+
+		if (distance_squared > point_max_distance) {
+			point_max_index = i;
+			point_max_distance = distance_squared;
+		}
+	}
+
+	if (point_max_distance > p_epsilon) {
+		simplify_path_segment(p_start_inx, point_max_index, p_points, p_epsilon, r_valid_points);
+		simplify_path_segment(point_max_index, p_end_inx, p_points, p_epsilon, r_valid_points);
+	}
+}
+
 #endif // _3D_DISABLED
diff --git a/modules/navigation/3d/nav_mesh_queries_3d.h b/modules/navigation/3d/nav_mesh_queries_3d.h
index 109bb2f971..a92520f944 100644
--- a/modules/navigation/3d/nav_mesh_queries_3d.h
+++ b/modules/navigation/3d/nav_mesh_queries_3d.h
@@ -33,20 +33,91 @@
 
 #ifndef _3D_DISABLED
 
-#include "../nav_map.h"
+#include "../nav_utils.h"
+
+#include "servers/navigation/navigation_path_query_parameters_3d.h"
+#include "servers/navigation/navigation_path_query_result_3d.h"
+#include "servers/navigation/navigation_utilities.h"
+
+using namespace NavigationUtilities;
+
+class NavMap;
 
 class NavMeshQueries3D {
 public:
+	struct PathQuerySlot {
+		LocalVector<gd::NavigationPoly> path_corridor;
+		gd::Heap<gd::NavigationPoly *, gd::NavPolyTravelCostGreaterThan, gd::NavPolyHeapIndexer> traversable_polys;
+		bool in_use = false;
+		uint32_t slot_index = 0;
+	};
+
+	struct NavMeshPathQueryTask3D {
+		enum TaskStatus {
+			QUERY_STARTED,
+			QUERY_FINISHED,
+			QUERY_FAILED,
+			CALLBACK_DISPATCHED,
+			CALLBACK_FAILED,
+		};
+
+		// Parameters.
+		Vector3 start_position;
+		Vector3 target_position;
+		uint32_t navigation_layers;
+		BitField<PathMetadataFlags> metadata_flags = PathMetadataFlags::PATH_INCLUDE_ALL;
+		PathfindingAlgorithm pathfinding_algorithm = PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR;
+		PathPostProcessing path_postprocessing = PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL;
+		bool simplify_path = false;
+		real_t simplify_epsilon = 0.0;
+
+		// Path building.
+		Vector3 begin_position;
+		Vector3 end_position;
+		uint32_t least_cost_id = 0;
+		Vector3 map_up;
+		NavMap *map = nullptr;
+		PathQuerySlot *path_query_slot = nullptr;
+
+		// Path points.
+		LocalVector<Vector3> path_points;
+		LocalVector<int32_t> path_meta_point_types;
+		LocalVector<RID> path_meta_point_rids;
+		LocalVector<int64_t> path_meta_point_owners;
+
+		Ref<NavigationPathQueryParameters3D> query_parameters;
+		Ref<NavigationPathQueryResult3D> query_result;
+		Callable callback;
+		NavMeshPathQueryTask3D::TaskStatus status = NavMeshPathQueryTask3D::TaskStatus::QUERY_STARTED;
+	};
+
+	static bool emit_callback(const Callable &p_callback);
+
 	static Vector3 polygons_get_random_point(const LocalVector<gd::Polygon> &p_polygons, uint32_t p_navigation_layers, bool p_uniformly);
 
-	static Vector<Vector3> polygons_get_path(const LocalVector<gd::Polygon> &p_polygons, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *r_path_owners, const Vector3 &p_map_up, uint32_t p_link_polygons_size);
 	static Vector3 polygons_get_closest_point_to_segment(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision);
 	static Vector3 polygons_get_closest_point(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point);
 	static Vector3 polygons_get_closest_point_normal(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point);
 	static gd::ClosestPointQueryResult polygons_get_closest_point_info(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point);
 	static RID polygons_get_closest_point_owner(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point);
 
-	static void clip_path(const LocalVector<gd::NavigationPoly> &p_navigation_polys, Vector<Vector3> &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *r_path_owners, const Vector3 &p_map_up);
+	static void map_query_path(NavMap *map, const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result, const Callable &p_callback);
+
+	static void query_task_polygons_get_path(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_map_up, uint32_t p_link_polygons_size);
+
+	static void _query_task_create_same_polygon_two_point_path(NavMeshPathQueryTask3D &p_query_task, const gd::Polygon *begin_poly, Vector3 begin_point, const gd::Polygon *end_poly, Vector3 end_point);
+	static void _query_task_push_back_point_with_metadata(NavMeshPathQueryTask3D &p_query_task, Vector3 p_point, const gd::Polygon *p_point_polygon);
+	static void _query_task_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons, const gd::Polygon **r_begin_poly, Vector3 &r_begin_point, const gd::Polygon **r_end_poly, Vector3 &r_end_point);
+	static void _query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_map_up, uint32_t p_link_polygons_size, const gd::Polygon *begin_poly, Vector3 begin_point, const gd::Polygon *end_polygon, Vector3 end_point);
+	static void _path_corridor_post_process_corridorfunnel(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point, const Vector3 &p_map_up);
+	static void _path_corridor_post_process_edgecentered(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point);
+	static void _path_corridor_post_process_nopostprocessing(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point);
+
+	static void clip_path(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::NavigationPoly> &p_navigation_polys, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, const Vector3 &p_map_up);
+	static void _query_task_simplified_path_points(NavMeshPathQueryTask3D &p_query_task);
+
+	static void simplify_path_segment(int p_start_inx, int p_end_inx, const LocalVector<Vector3> &p_points, real_t p_epsilon, LocalVector<bool> &r_valid_points);
+	static LocalVector<uint32_t> get_simplified_path_indices(const LocalVector<Vector3> &p_path, real_t p_epsilon);
 };
 
 #endif // _3D_DISABLED
diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp
index 739b031d06..07ee96dc32 100644
--- a/modules/navigation/nav_map.cpp
+++ b/modules/navigation/nav_map.cpp
@@ -35,8 +35,6 @@
 #include "nav_obstacle.h"
 #include "nav_region.h"
 
-#include "3d/nav_mesh_queries_3d.h"
-
 #include "core/config/project_settings.h"
 #include "core/object/worker_thread_pool.h"
 
@@ -123,16 +121,40 @@ gd::PointKey NavMap::get_point_key(const Vector3 &p_pos) const {
 	return p;
 }
 
-Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *r_path_owners) const {
+void NavMap::query_path(NavMeshQueries3D::NavMeshPathQueryTask3D &p_query_task) {
 	RWLockRead read_lock(map_rwlock);
 	if (iteration_id == 0) {
-		NAVMAP_ITERATION_ZERO_ERROR_MSG();
-		return Vector<Vector3>();
+		return;
 	}
 
-	return NavMeshQueries3D::polygons_get_path(
-			polygons, p_origin, p_destination, p_optimize, p_navigation_layers,
-			r_path_types, r_path_rids, r_path_owners, up, link_polygons.size());
+	path_query_slots_semaphore.wait();
+
+	path_query_slots_mutex.lock();
+	for (NavMeshQueries3D::PathQuerySlot &p_path_query_slot : path_query_slots) {
+		if (!p_path_query_slot.in_use) {
+			p_path_query_slot.in_use = true;
+			p_query_task.path_query_slot = &p_path_query_slot;
+			break;
+		}
+	}
+	path_query_slots_mutex.unlock();
+
+	if (p_query_task.path_query_slot == nullptr) {
+		path_query_slots_semaphore.post();
+		ERR_FAIL_NULL_MSG(p_query_task.path_query_slot, "No unused NavMap path query slot found! This should never happen :(.");
+	}
+
+	p_query_task.map_up = get_up();
+
+	NavMeshQueries3D::query_task_polygons_get_path(p_query_task, polygons, up, link_polygons.size());
+
+	path_query_slots_mutex.lock();
+	uint32_t used_slot_index = p_query_task.path_query_slot->slot_index;
+	path_query_slots[used_slot_index].in_use = false;
+	p_query_task.path_query_slot = nullptr;
+	path_query_slots_mutex.unlock();
+
+	path_query_slots_semaphore.post();
 }
 
 Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const {
@@ -639,6 +661,15 @@ void NavMap::sync() {
 
 		// Some code treats 0 as a failure case, so we avoid returning 0 and modulo wrap UINT32_MAX manually.
 		iteration_id = iteration_id % UINT32_MAX + 1;
+
+		path_query_slots_mutex.lock();
+		for (NavMeshQueries3D::PathQuerySlot &p_path_query_slot : path_query_slots) {
+			p_path_query_slot.path_corridor.clear();
+			p_path_query_slot.path_corridor.resize(polygons.size() + link_polygons.size());
+			p_path_query_slot.traversable_polys.clear();
+			p_path_query_slot.traversable_polys.reserve(polygons.size() * 0.25);
+		}
+		path_query_slots_mutex.unlock();
 	}
 
 	map_settings_dirty = false;
@@ -969,6 +1000,26 @@ void NavMap::_sync_dirty_avoidance_update_requests() {
 NavMap::NavMap() {
 	avoidance_use_multiple_threads = GLOBAL_GET("navigation/avoidance/thread_model/avoidance_use_multiple_threads");
 	avoidance_use_high_priority_threads = GLOBAL_GET("navigation/avoidance/thread_model/avoidance_use_high_priority_threads");
+
+	path_query_slots_max = GLOBAL_GET("navigation/pathfinding/max_threads");
+
+	int processor_count = OS::get_singleton()->get_processor_count();
+	if (path_query_slots_max < 0) {
+		path_query_slots_max = processor_count;
+	}
+	if (processor_count < path_query_slots_max) {
+		path_query_slots_max = processor_count;
+	}
+	if (path_query_slots_max < 1) {
+		path_query_slots_max = 1;
+	}
+
+	path_query_slots.resize(path_query_slots_max);
+	for (uint32_t i = 0; i < path_query_slots.size(); i++) {
+		path_query_slots[i].slot_index = i;
+	}
+
+	path_query_slots_semaphore.post(path_query_slots_max);
 }
 
 NavMap::~NavMap() {
diff --git a/modules/navigation/nav_map.h b/modules/navigation/nav_map.h
index feeb2cc548..ce247b83c1 100644
--- a/modules/navigation/nav_map.h
+++ b/modules/navigation/nav_map.h
@@ -31,6 +31,7 @@
 #ifndef NAV_MAP_H
 #define NAV_MAP_H
 
+#include "3d/nav_mesh_queries_3d.h"
 #include "nav_rid.h"
 #include "nav_utils.h"
 
@@ -135,6 +136,11 @@ class NavMap : public NavRid {
 		SelfList<NavObstacle>::List obstacles;
 	} sync_dirty_requests;
 
+	LocalVector<NavMeshQueries3D::PathQuerySlot> path_query_slots;
+	int path_query_slots_max = 4;
+	Mutex path_query_slots_mutex;
+	Semaphore path_query_slots_semaphore;
+
 public:
 	NavMap();
 	~NavMap();
@@ -176,7 +182,8 @@ public:
 
 	gd::PointKey get_point_key(const Vector3 &p_pos) const;
 
-	Vector<Vector3> get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *r_path_owners) const;
+	void query_path(NavMeshQueries3D::NavMeshPathQueryTask3D &p_query_task);
+
 	Vector3 get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const;
 	Vector3 get_closest_point(const Vector3 &p_point) const;
 	Vector3 get_closest_point_normal(const Vector3 &p_point) const;
diff --git a/modules/navigation/nav_utils.h b/modules/navigation/nav_utils.h
index 993a97638d..ea22fb0bab 100644
--- a/modules/navigation/nav_utils.h
+++ b/modules/navigation/nav_utils.h
@@ -145,6 +145,15 @@ struct NavigationPoly {
 	bool operator!=(const NavigationPoly &p_other) const {
 		return !(*this == p_other);
 	}
+
+	void reset() {
+		poly = nullptr;
+		traversable_poly_index = UINT32_MAX;
+		back_navigation_poly_id = -1;
+		back_navigation_edge = -1;
+		traveled_distance = 0.0;
+		distance_to_destination = 0.0;
+	}
 };
 
 struct NavPolyTravelCostGreaterThan {
diff --git a/scene/2d/navigation_agent_2d.cpp b/scene/2d/navigation_agent_2d.cpp
index f030473c4b..786598f01a 100644
--- a/scene/2d/navigation_agent_2d.cpp
+++ b/scene/2d/navigation_agent_2d.cpp
@@ -133,7 +133,7 @@ void NavigationAgent2D::_bind_methods() {
 	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "path_max_distance", PROPERTY_HINT_RANGE, "10,1000,1,or_greater,suffix:px"), "set_path_max_distance", "get_path_max_distance");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "navigation_layers", PROPERTY_HINT_LAYERS_2D_NAVIGATION), "set_navigation_layers", "get_navigation_layers");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "pathfinding_algorithm", PROPERTY_HINT_ENUM, "AStar"), "set_pathfinding_algorithm", "get_pathfinding_algorithm");
-	ADD_PROPERTY(PropertyInfo(Variant::INT, "path_postprocessing", PROPERTY_HINT_ENUM, "Corridorfunnel,Edgecentered"), "set_path_postprocessing", "get_path_postprocessing");
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "path_postprocessing", PROPERTY_HINT_ENUM, "Corridorfunnel,Edgecentered,None"), "set_path_postprocessing", "get_path_postprocessing");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "path_metadata_flags", PROPERTY_HINT_FLAGS, "Include Types,Include RIDs,Include Owners"), "set_path_metadata_flags", "get_path_metadata_flags");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "simplify_path"), "set_simplify_path", "get_simplify_path");
 	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "simplify_epsilon", PROPERTY_HINT_RANGE, "0.0,10.0,0.001,or_greater,suffix:px"), "set_simplify_epsilon", "get_simplify_epsilon");
diff --git a/scene/3d/navigation_agent_3d.cpp b/scene/3d/navigation_agent_3d.cpp
index faf138896a..d5e162d416 100644
--- a/scene/3d/navigation_agent_3d.cpp
+++ b/scene/3d/navigation_agent_3d.cpp
@@ -144,7 +144,7 @@ void NavigationAgent3D::_bind_methods() {
 	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "path_max_distance", PROPERTY_HINT_RANGE, "0.01,100,0.1,or_greater,suffix:m"), "set_path_max_distance", "get_path_max_distance");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "navigation_layers", PROPERTY_HINT_LAYERS_3D_NAVIGATION), "set_navigation_layers", "get_navigation_layers");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "pathfinding_algorithm", PROPERTY_HINT_ENUM, "AStar"), "set_pathfinding_algorithm", "get_pathfinding_algorithm");
-	ADD_PROPERTY(PropertyInfo(Variant::INT, "path_postprocessing", PROPERTY_HINT_ENUM, "Corridorfunnel,Edgecentered"), "set_path_postprocessing", "get_path_postprocessing");
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "path_postprocessing", PROPERTY_HINT_ENUM, "Corridorfunnel,Edgecentered,None"), "set_path_postprocessing", "get_path_postprocessing");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "path_metadata_flags", PROPERTY_HINT_FLAGS, "Include Types,Include RIDs,Include Owners"), "set_path_metadata_flags", "get_path_metadata_flags");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "simplify_path"), "set_simplify_path", "get_simplify_path");
 	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "simplify_epsilon", PROPERTY_HINT_RANGE, "0.0,10.0,0.001,or_greater,suffix:m"), "set_simplify_epsilon", "get_simplify_epsilon");
diff --git a/servers/navigation/navigation_path_query_parameters_2d.cpp b/servers/navigation/navigation_path_query_parameters_2d.cpp
index 6c1f88e349..74aaf64b4e 100644
--- a/servers/navigation/navigation_path_query_parameters_2d.cpp
+++ b/servers/navigation/navigation_path_query_parameters_2d.cpp
@@ -31,108 +31,75 @@
 #include "navigation_path_query_parameters_2d.h"
 
 void NavigationPathQueryParameters2D::set_pathfinding_algorithm(const NavigationPathQueryParameters2D::PathfindingAlgorithm p_pathfinding_algorithm) {
-	switch (p_pathfinding_algorithm) {
-		case PATHFINDING_ALGORITHM_ASTAR: {
-			parameters.pathfinding_algorithm = NavigationUtilities::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR;
-		} break;
-		default: {
-			WARN_PRINT_ONCE("No match for used PathfindingAlgorithm - fallback to default");
-			parameters.pathfinding_algorithm = NavigationUtilities::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR;
-		} break;
-	}
+	pathfinding_algorithm = p_pathfinding_algorithm;
 }
 
 NavigationPathQueryParameters2D::PathfindingAlgorithm NavigationPathQueryParameters2D::get_pathfinding_algorithm() const {
-	switch (parameters.pathfinding_algorithm) {
-		case NavigationUtilities::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR:
-			return PATHFINDING_ALGORITHM_ASTAR;
-		default:
-			WARN_PRINT_ONCE("No match for used PathfindingAlgorithm - fallback to default");
-			return PATHFINDING_ALGORITHM_ASTAR;
-	}
+	return pathfinding_algorithm;
 }
 
 void NavigationPathQueryParameters2D::set_path_postprocessing(const NavigationPathQueryParameters2D::PathPostProcessing p_path_postprocessing) {
-	switch (p_path_postprocessing) {
-		case PATH_POSTPROCESSING_CORRIDORFUNNEL: {
-			parameters.path_postprocessing = NavigationUtilities::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL;
-		} break;
-		case PATH_POSTPROCESSING_EDGECENTERED: {
-			parameters.path_postprocessing = NavigationUtilities::PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED;
-		} break;
-		default: {
-			WARN_PRINT_ONCE("No match for used PathPostProcessing - fallback to default");
-			parameters.path_postprocessing = NavigationUtilities::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL;
-		} break;
-	}
+	path_postprocessing = p_path_postprocessing;
 }
 
 NavigationPathQueryParameters2D::PathPostProcessing NavigationPathQueryParameters2D::get_path_postprocessing() const {
-	switch (parameters.path_postprocessing) {
-		case NavigationUtilities::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL:
-			return PATH_POSTPROCESSING_CORRIDORFUNNEL;
-		case NavigationUtilities::PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED:
-			return PATH_POSTPROCESSING_EDGECENTERED;
-		default:
-			WARN_PRINT_ONCE("No match for used PathPostProcessing - fallback to default");
-			return PATH_POSTPROCESSING_CORRIDORFUNNEL;
-	}
+	return path_postprocessing;
 }
 
-void NavigationPathQueryParameters2D::set_map(const RID &p_map) {
-	parameters.map = p_map;
+void NavigationPathQueryParameters2D::set_map(RID p_map) {
+	map = p_map;
 }
 
-const RID &NavigationPathQueryParameters2D::get_map() const {
-	return parameters.map;
+RID NavigationPathQueryParameters2D::get_map() const {
+	return map;
 }
 
-void NavigationPathQueryParameters2D::set_start_position(const Vector2 p_start_position) {
-	parameters.start_position = Vector3(p_start_position.x, 0.0, p_start_position.y);
+void NavigationPathQueryParameters2D::set_start_position(Vector2 p_start_position) {
+	start_position = p_start_position;
 }
 
 Vector2 NavigationPathQueryParameters2D::get_start_position() const {
-	return Vector2(parameters.start_position.x, parameters.start_position.z);
+	return start_position;
 }
 
-void NavigationPathQueryParameters2D::set_target_position(const Vector2 p_target_position) {
-	parameters.target_position = Vector3(p_target_position.x, 0.0, p_target_position.y);
+void NavigationPathQueryParameters2D::set_target_position(Vector2 p_target_position) {
+	target_position = p_target_position;
 }
 
 Vector2 NavigationPathQueryParameters2D::get_target_position() const {
-	return Vector2(parameters.target_position.x, parameters.target_position.z);
+	return target_position;
 }
 
 void NavigationPathQueryParameters2D::set_navigation_layers(uint32_t p_navigation_layers) {
-	parameters.navigation_layers = p_navigation_layers;
+	navigation_layers = p_navigation_layers;
 }
 
 uint32_t NavigationPathQueryParameters2D::get_navigation_layers() const {
-	return parameters.navigation_layers;
+	return navigation_layers;
 }
 
 void NavigationPathQueryParameters2D::set_metadata_flags(BitField<NavigationPathQueryParameters2D::PathMetadataFlags> p_flags) {
-	parameters.metadata_flags = (int64_t)p_flags;
+	metadata_flags = (int64_t)p_flags;
 }
 
 BitField<NavigationPathQueryParameters2D::PathMetadataFlags> NavigationPathQueryParameters2D::get_metadata_flags() const {
-	return (int64_t)parameters.metadata_flags;
+	return (int64_t)metadata_flags;
 }
 
 void NavigationPathQueryParameters2D::set_simplify_path(bool p_enabled) {
-	parameters.simplify_path = p_enabled;
+	simplify_path = p_enabled;
 }
 
 bool NavigationPathQueryParameters2D::get_simplify_path() const {
-	return parameters.simplify_path;
+	return simplify_path;
 }
 
 void NavigationPathQueryParameters2D::set_simplify_epsilon(real_t p_epsilon) {
-	parameters.simplify_epsilon = MAX(0.0, p_epsilon);
+	simplify_epsilon = MAX(0.0, p_epsilon);
 }
 
 real_t NavigationPathQueryParameters2D::get_simplify_epsilon() const {
-	return parameters.simplify_epsilon;
+	return simplify_epsilon;
 }
 
 void NavigationPathQueryParameters2D::_bind_methods() {
@@ -168,7 +135,7 @@ void NavigationPathQueryParameters2D::_bind_methods() {
 	ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "target_position"), "set_target_position", "get_target_position");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "navigation_layers", PROPERTY_HINT_LAYERS_2D_NAVIGATION), "set_navigation_layers", "get_navigation_layers");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "pathfinding_algorithm", PROPERTY_HINT_ENUM, "AStar"), "set_pathfinding_algorithm", "get_pathfinding_algorithm");
-	ADD_PROPERTY(PropertyInfo(Variant::INT, "path_postprocessing", PROPERTY_HINT_ENUM, "Corridorfunnel,Edgecentered"), "set_path_postprocessing", "get_path_postprocessing");
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "path_postprocessing", PROPERTY_HINT_ENUM, "Corridorfunnel,Edgecentered,None"), "set_path_postprocessing", "get_path_postprocessing");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "metadata_flags", PROPERTY_HINT_FLAGS, "Include Types,Include RIDs,Include Owners"), "set_metadata_flags", "get_metadata_flags");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "simplify_path"), "set_simplify_path", "get_simplify_path");
 	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "simplify_epsilon"), "set_simplify_epsilon", "get_simplify_epsilon");
@@ -177,6 +144,7 @@ void NavigationPathQueryParameters2D::_bind_methods() {
 
 	BIND_ENUM_CONSTANT(PATH_POSTPROCESSING_CORRIDORFUNNEL);
 	BIND_ENUM_CONSTANT(PATH_POSTPROCESSING_EDGECENTERED);
+	BIND_ENUM_CONSTANT(PATH_POSTPROCESSING_NONE);
 
 	BIND_BITFIELD_FLAG(PATH_METADATA_INCLUDE_NONE);
 	BIND_BITFIELD_FLAG(PATH_METADATA_INCLUDE_TYPES);
diff --git a/servers/navigation/navigation_path_query_parameters_2d.h b/servers/navigation/navigation_path_query_parameters_2d.h
index a1d5f2d109..91031bfc18 100644
--- a/servers/navigation/navigation_path_query_parameters_2d.h
+++ b/servers/navigation/navigation_path_query_parameters_2d.h
@@ -37,19 +37,18 @@
 class NavigationPathQueryParameters2D : public RefCounted {
 	GDCLASS(NavigationPathQueryParameters2D, RefCounted);
 
-	NavigationUtilities::PathQueryParameters parameters;
-
 protected:
 	static void _bind_methods();
 
 public:
 	enum PathfindingAlgorithm {
-		PATHFINDING_ALGORITHM_ASTAR = 0,
+		PATHFINDING_ALGORITHM_ASTAR = NavigationUtilities::PATHFINDING_ALGORITHM_ASTAR,
 	};
 
 	enum PathPostProcessing {
-		PATH_POSTPROCESSING_CORRIDORFUNNEL = 0,
-		PATH_POSTPROCESSING_EDGECENTERED,
+		PATH_POSTPROCESSING_CORRIDORFUNNEL = NavigationUtilities::PATH_POSTPROCESSING_CORRIDORFUNNEL,
+		PATH_POSTPROCESSING_EDGECENTERED = NavigationUtilities::PATH_POSTPROCESSING_EDGECENTERED,
+		PATH_POSTPROCESSING_NONE = NavigationUtilities::PATH_POSTPROCESSING_NONE,
 	};
 
 	enum PathMetadataFlags {
@@ -60,16 +59,26 @@ public:
 		PATH_METADATA_INCLUDE_ALL = NavigationUtilities::PathMetadataFlags::PATH_INCLUDE_ALL
 	};
 
-	const NavigationUtilities::PathQueryParameters &get_parameters() const { return parameters; }
+private:
+	PathfindingAlgorithm pathfinding_algorithm = PATHFINDING_ALGORITHM_ASTAR;
+	PathPostProcessing path_postprocessing = PATH_POSTPROCESSING_CORRIDORFUNNEL;
+	RID map;
+	Vector2 start_position;
+	Vector2 target_position;
+	uint32_t navigation_layers = 1;
+	BitField<PathMetadataFlags> metadata_flags = PATH_METADATA_INCLUDE_ALL;
+	bool simplify_path = false;
+	real_t simplify_epsilon = 0.0;
 
+public:
 	void set_pathfinding_algorithm(const PathfindingAlgorithm p_pathfinding_algorithm);
 	PathfindingAlgorithm get_pathfinding_algorithm() const;
 
 	void set_path_postprocessing(const PathPostProcessing p_path_postprocessing);
 	PathPostProcessing get_path_postprocessing() const;
 
-	void set_map(const RID &p_map);
-	const RID &get_map() const;
+	void set_map(RID p_map);
+	RID get_map() const;
 
 	void set_start_position(const Vector2 p_start_position);
 	Vector2 get_start_position() const;
diff --git a/servers/navigation/navigation_path_query_parameters_3d.cpp b/servers/navigation/navigation_path_query_parameters_3d.cpp
index b0a5b0ad82..99c5318bed 100644
--- a/servers/navigation/navigation_path_query_parameters_3d.cpp
+++ b/servers/navigation/navigation_path_query_parameters_3d.cpp
@@ -31,108 +31,75 @@
 #include "navigation_path_query_parameters_3d.h"
 
 void NavigationPathQueryParameters3D::set_pathfinding_algorithm(const NavigationPathQueryParameters3D::PathfindingAlgorithm p_pathfinding_algorithm) {
-	switch (p_pathfinding_algorithm) {
-		case PATHFINDING_ALGORITHM_ASTAR: {
-			parameters.pathfinding_algorithm = NavigationUtilities::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR;
-		} break;
-		default: {
-			WARN_PRINT_ONCE("No match for used PathfindingAlgorithm - fallback to default");
-			parameters.pathfinding_algorithm = NavigationUtilities::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR;
-		} break;
-	}
+	pathfinding_algorithm = p_pathfinding_algorithm;
 }
 
 NavigationPathQueryParameters3D::PathfindingAlgorithm NavigationPathQueryParameters3D::get_pathfinding_algorithm() const {
-	switch (parameters.pathfinding_algorithm) {
-		case NavigationUtilities::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR:
-			return PATHFINDING_ALGORITHM_ASTAR;
-		default:
-			WARN_PRINT_ONCE("No match for used PathfindingAlgorithm - fallback to default");
-			return PATHFINDING_ALGORITHM_ASTAR;
-	}
+	return pathfinding_algorithm;
 }
 
 void NavigationPathQueryParameters3D::set_path_postprocessing(const NavigationPathQueryParameters3D::PathPostProcessing p_path_postprocessing) {
-	switch (p_path_postprocessing) {
-		case PATH_POSTPROCESSING_CORRIDORFUNNEL: {
-			parameters.path_postprocessing = NavigationUtilities::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL;
-		} break;
-		case PATH_POSTPROCESSING_EDGECENTERED: {
-			parameters.path_postprocessing = NavigationUtilities::PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED;
-		} break;
-		default: {
-			WARN_PRINT_ONCE("No match for used PathPostProcessing - fallback to default");
-			parameters.path_postprocessing = NavigationUtilities::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL;
-		} break;
-	}
+	path_postprocessing = p_path_postprocessing;
 }
 
 NavigationPathQueryParameters3D::PathPostProcessing NavigationPathQueryParameters3D::get_path_postprocessing() const {
-	switch (parameters.path_postprocessing) {
-		case NavigationUtilities::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL:
-			return PATH_POSTPROCESSING_CORRIDORFUNNEL;
-		case NavigationUtilities::PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED:
-			return PATH_POSTPROCESSING_EDGECENTERED;
-		default:
-			WARN_PRINT_ONCE("No match for used PathPostProcessing - fallback to default");
-			return PATH_POSTPROCESSING_CORRIDORFUNNEL;
-	}
+	return path_postprocessing;
 }
 
-void NavigationPathQueryParameters3D::set_map(const RID &p_map) {
-	parameters.map = p_map;
+void NavigationPathQueryParameters3D::set_map(RID p_map) {
+	map = p_map;
 }
 
-const RID &NavigationPathQueryParameters3D::get_map() const {
-	return parameters.map;
+RID NavigationPathQueryParameters3D::get_map() const {
+	return map;
 }
 
-void NavigationPathQueryParameters3D::set_start_position(const Vector3 &p_start_position) {
-	parameters.start_position = p_start_position;
+void NavigationPathQueryParameters3D::set_start_position(Vector3 p_start_position) {
+	start_position = p_start_position;
 }
 
-const Vector3 &NavigationPathQueryParameters3D::get_start_position() const {
-	return parameters.start_position;
+Vector3 NavigationPathQueryParameters3D::get_start_position() const {
+	return start_position;
 }
 
-void NavigationPathQueryParameters3D::set_target_position(const Vector3 &p_target_position) {
-	parameters.target_position = p_target_position;
+void NavigationPathQueryParameters3D::set_target_position(Vector3 p_target_position) {
+	target_position = p_target_position;
 }
 
-const Vector3 &NavigationPathQueryParameters3D::get_target_position() const {
-	return parameters.target_position;
+Vector3 NavigationPathQueryParameters3D::get_target_position() const {
+	return target_position;
 }
 
 void NavigationPathQueryParameters3D::set_navigation_layers(uint32_t p_navigation_layers) {
-	parameters.navigation_layers = p_navigation_layers;
+	navigation_layers = p_navigation_layers;
 }
 
 uint32_t NavigationPathQueryParameters3D::get_navigation_layers() const {
-	return parameters.navigation_layers;
+	return navigation_layers;
 }
 
 void NavigationPathQueryParameters3D::set_metadata_flags(BitField<NavigationPathQueryParameters3D::PathMetadataFlags> p_flags) {
-	parameters.metadata_flags = (int64_t)p_flags;
+	metadata_flags = (int64_t)p_flags;
 }
 
 BitField<NavigationPathQueryParameters3D::PathMetadataFlags> NavigationPathQueryParameters3D::get_metadata_flags() const {
-	return (int64_t)parameters.metadata_flags;
+	return (int64_t)metadata_flags;
 }
 
 void NavigationPathQueryParameters3D::set_simplify_path(bool p_enabled) {
-	parameters.simplify_path = p_enabled;
+	simplify_path = p_enabled;
 }
 
 bool NavigationPathQueryParameters3D::get_simplify_path() const {
-	return parameters.simplify_path;
+	return simplify_path;
 }
 
 void NavigationPathQueryParameters3D::set_simplify_epsilon(real_t p_epsilon) {
-	parameters.simplify_epsilon = MAX(0.0, p_epsilon);
+	simplify_epsilon = MAX(0.0, p_epsilon);
 }
 
 real_t NavigationPathQueryParameters3D::get_simplify_epsilon() const {
-	return parameters.simplify_epsilon;
+	return simplify_epsilon;
 }
 
 void NavigationPathQueryParameters3D::_bind_methods() {
@@ -168,7 +135,7 @@ void NavigationPathQueryParameters3D::_bind_methods() {
 	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "target_position"), "set_target_position", "get_target_position");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "navigation_layers", PROPERTY_HINT_LAYERS_3D_NAVIGATION), "set_navigation_layers", "get_navigation_layers");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "pathfinding_algorithm", PROPERTY_HINT_ENUM, "AStar"), "set_pathfinding_algorithm", "get_pathfinding_algorithm");
-	ADD_PROPERTY(PropertyInfo(Variant::INT, "path_postprocessing", PROPERTY_HINT_ENUM, "Corridorfunnel,Edgecentered"), "set_path_postprocessing", "get_path_postprocessing");
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "path_postprocessing", PROPERTY_HINT_ENUM, "Corridorfunnel,Edgecentered,None"), "set_path_postprocessing", "get_path_postprocessing");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "metadata_flags", PROPERTY_HINT_FLAGS, "Include Types,Include RIDs,Include Owners"), "set_metadata_flags", "get_metadata_flags");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "simplify_path"), "set_simplify_path", "get_simplify_path");
 	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "simplify_epsilon"), "set_simplify_epsilon", "get_simplify_epsilon");
@@ -177,6 +144,7 @@ void NavigationPathQueryParameters3D::_bind_methods() {
 
 	BIND_ENUM_CONSTANT(PATH_POSTPROCESSING_CORRIDORFUNNEL);
 	BIND_ENUM_CONSTANT(PATH_POSTPROCESSING_EDGECENTERED);
+	BIND_ENUM_CONSTANT(PATH_POSTPROCESSING_NONE);
 
 	BIND_BITFIELD_FLAG(PATH_METADATA_INCLUDE_NONE);
 	BIND_BITFIELD_FLAG(PATH_METADATA_INCLUDE_TYPES);
diff --git a/servers/navigation/navigation_path_query_parameters_3d.h b/servers/navigation/navigation_path_query_parameters_3d.h
index 2eb85db787..66cad8dcd5 100644
--- a/servers/navigation/navigation_path_query_parameters_3d.h
+++ b/servers/navigation/navigation_path_query_parameters_3d.h
@@ -37,19 +37,18 @@
 class NavigationPathQueryParameters3D : public RefCounted {
 	GDCLASS(NavigationPathQueryParameters3D, RefCounted);
 
-	NavigationUtilities::PathQueryParameters parameters;
-
 protected:
 	static void _bind_methods();
 
 public:
 	enum PathfindingAlgorithm {
-		PATHFINDING_ALGORITHM_ASTAR = 0,
+		PATHFINDING_ALGORITHM_ASTAR = NavigationUtilities::PATHFINDING_ALGORITHM_ASTAR,
 	};
 
 	enum PathPostProcessing {
-		PATH_POSTPROCESSING_CORRIDORFUNNEL = 0,
-		PATH_POSTPROCESSING_EDGECENTERED,
+		PATH_POSTPROCESSING_CORRIDORFUNNEL = NavigationUtilities::PATH_POSTPROCESSING_CORRIDORFUNNEL,
+		PATH_POSTPROCESSING_EDGECENTERED = NavigationUtilities::PATH_POSTPROCESSING_EDGECENTERED,
+		PATH_POSTPROCESSING_NONE = NavigationUtilities::PATH_POSTPROCESSING_NONE,
 	};
 
 	enum PathMetadataFlags {
@@ -60,22 +59,32 @@ public:
 		PATH_METADATA_INCLUDE_ALL = NavigationUtilities::PathMetadataFlags::PATH_INCLUDE_ALL
 	};
 
-	const NavigationUtilities::PathQueryParameters &get_parameters() const { return parameters; }
+private:
+	PathfindingAlgorithm pathfinding_algorithm = PATHFINDING_ALGORITHM_ASTAR;
+	PathPostProcessing path_postprocessing = PATH_POSTPROCESSING_CORRIDORFUNNEL;
+	RID map;
+	Vector3 start_position;
+	Vector3 target_position;
+	uint32_t navigation_layers = 1;
+	BitField<PathMetadataFlags> metadata_flags = PATH_METADATA_INCLUDE_ALL;
+	bool simplify_path = false;
+	real_t simplify_epsilon = 0.0;
 
+public:
 	void set_pathfinding_algorithm(const PathfindingAlgorithm p_pathfinding_algorithm);
 	PathfindingAlgorithm get_pathfinding_algorithm() const;
 
 	void set_path_postprocessing(const PathPostProcessing p_path_postprocessing);
 	PathPostProcessing get_path_postprocessing() const;
 
-	void set_map(const RID &p_map);
-	const RID &get_map() const;
+	void set_map(RID p_map);
+	RID get_map() const;
 
-	void set_start_position(const Vector3 &p_start_position);
-	const Vector3 &get_start_position() const;
+	void set_start_position(Vector3 p_start_position);
+	Vector3 get_start_position() const;
 
-	void set_target_position(const Vector3 &p_target_position);
-	const Vector3 &get_target_position() const;
+	void set_target_position(Vector3 p_target_position);
+	Vector3 get_target_position() const;
 
 	void set_navigation_layers(uint32_t p_navigation_layers);
 	uint32_t get_navigation_layers() const;
diff --git a/servers/navigation/navigation_path_query_result_2d.h b/servers/navigation/navigation_path_query_result_2d.h
index 856219f998..1320d15ea2 100644
--- a/servers/navigation/navigation_path_query_result_2d.h
+++ b/servers/navigation/navigation_path_query_result_2d.h
@@ -47,8 +47,8 @@ protected:
 
 public:
 	enum PathSegmentType {
-		PATH_SEGMENT_TYPE_REGION = 0,
-		PATH_SEGMENT_TYPE_LINK = 1,
+		PATH_SEGMENT_TYPE_REGION = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_REGION,
+		PATH_SEGMENT_TYPE_LINK = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_LINK,
 	};
 
 	void set_path(const Vector<Vector2> &p_path);
diff --git a/servers/navigation/navigation_path_query_result_3d.h b/servers/navigation/navigation_path_query_result_3d.h
index fd8545a0c9..8c4e89b9b0 100644
--- a/servers/navigation/navigation_path_query_result_3d.h
+++ b/servers/navigation/navigation_path_query_result_3d.h
@@ -48,8 +48,8 @@ protected:
 
 public:
 	enum PathSegmentType {
-		PATH_SEGMENT_TYPE_REGION = 0,
-		PATH_SEGMENT_TYPE_LINK = 1,
+		PATH_SEGMENT_TYPE_REGION = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_REGION,
+		PATH_SEGMENT_TYPE_LINK = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_LINK,
 	};
 
 	void set_path(const Vector<Vector3> &p_path);
diff --git a/servers/navigation/navigation_utilities.h b/servers/navigation/navigation_utilities.h
index 7ae22b1d3a..36192c140f 100644
--- a/servers/navigation/navigation_utilities.h
+++ b/servers/navigation/navigation_utilities.h
@@ -43,6 +43,7 @@ enum PathfindingAlgorithm {
 enum PathPostProcessing {
 	PATH_POSTPROCESSING_CORRIDORFUNNEL = 0,
 	PATH_POSTPROCESSING_EDGECENTERED,
+	PATH_POSTPROCESSING_NONE,
 };
 
 enum PathSegmentType {
@@ -58,25 +59,6 @@ enum PathMetadataFlags {
 	PATH_INCLUDE_ALL = PATH_INCLUDE_TYPES | PATH_INCLUDE_RIDS | PATH_INCLUDE_OWNERS
 };
 
-struct PathQueryParameters {
-	PathfindingAlgorithm pathfinding_algorithm = PATHFINDING_ALGORITHM_ASTAR;
-	PathPostProcessing path_postprocessing = PATH_POSTPROCESSING_CORRIDORFUNNEL;
-	RID map;
-	Vector3 start_position;
-	Vector3 target_position;
-	uint32_t navigation_layers = 1;
-	BitField<PathMetadataFlags> metadata_flags = PATH_INCLUDE_ALL;
-	bool simplify_path = false;
-	real_t simplify_epsilon = 0.0;
-};
-
-struct PathQueryResult {
-	PackedVector3Array path;
-	PackedInt32Array path_types;
-	TypedArray<RID> path_rids;
-	PackedInt64Array path_owner_ids;
-};
-
 } //namespace NavigationUtilities
 
 #endif // NAVIGATION_UTILITIES_H
diff --git a/servers/navigation_server_2d.compat.inc b/servers/navigation_server_2d.compat.inc
new file mode 100644
index 0000000000..14205ef536
--- /dev/null
+++ b/servers/navigation_server_2d.compat.inc
@@ -0,0 +1,46 @@
+/**************************************************************************/
+/*  navigation_server_2d.compat.inc                                       */
+/**************************************************************************/
+/*                         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 DISABLE_DEPRECATED
+
+Vector<Vector2> NavigationServer2D::_map_get_path_bind_compat_100129(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize, uint32_t p_navigation_layers) const {
+	return const_cast<NavigationServer2D *>(this)->map_get_path(p_map, p_origin, p_destination, p_optimize, p_navigation_layers);
+}
+
+void NavigationServer2D::_query_path_bind_compat_100129(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result) const {
+	return const_cast<NavigationServer2D *>(this)->query_path(p_query_parameters, p_query_result, Callable());
+}
+
+void NavigationServer2D::_bind_compatibility_methods() {
+	ClassDB::bind_compatibility_method(D_METHOD("map_get_path", "map", "origin", "destination", "optimize", "navigation_layers"), &NavigationServer2D::_map_get_path_bind_compat_100129, DEFVAL(1));
+	ClassDB::bind_compatibility_method(D_METHOD("query_path", "parameters", "result"), &NavigationServer2D::_query_path_bind_compat_100129);
+}
+
+#endif // DISABLE_DEPRECATED
diff --git a/servers/navigation_server_2d.cpp b/servers/navigation_server_2d.cpp
index ceb5e909a3..cd0dd7e3a0 100644
--- a/servers/navigation_server_2d.cpp
+++ b/servers/navigation_server_2d.cpp
@@ -29,6 +29,7 @@
 /**************************************************************************/
 
 #include "navigation_server_2d.h"
+#include "navigation_server_2d.compat.inc"
 
 #include "servers/navigation_server_3d.h"
 
@@ -62,7 +63,7 @@ void NavigationServer2D::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("map_get_random_point", "map", "navigation_layers", "uniformly"), &NavigationServer2D::map_get_random_point);
 
-	ClassDB::bind_method(D_METHOD("query_path", "parameters", "result"), &NavigationServer2D::query_path);
+	ClassDB::bind_method(D_METHOD("query_path", "parameters", "result", "callback"), &NavigationServer2D::query_path, DEFVAL(Callable()));
 
 	ClassDB::bind_method(D_METHOD("region_create"), &NavigationServer2D::region_create);
 	ClassDB::bind_method(D_METHOD("region_set_enabled", "region", "enabled"), &NavigationServer2D::region_set_enabled);
diff --git a/servers/navigation_server_2d.h b/servers/navigation_server_2d.h
index 250183300f..e7c7cf0653 100644
--- a/servers/navigation_server_2d.h
+++ b/servers/navigation_server_2d.h
@@ -91,7 +91,7 @@ public:
 	virtual real_t map_get_link_connection_radius(RID p_map) const = 0;
 
 	/// Returns the navigation path to reach the destination from the origin.
-	virtual Vector<Vector2> map_get_path(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) const = 0;
+	virtual Vector<Vector2> map_get_path(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) = 0;
 
 	virtual Vector2 map_get_closest_point(RID p_map, const Vector2 &p_point) const = 0;
 	virtual RID map_get_closest_point_owner(RID p_map, const Vector2 &p_point) const = 0;
@@ -293,7 +293,7 @@ public:
 	virtual uint32_t obstacle_get_avoidance_layers(RID p_obstacle) const = 0;
 
 	/// Returns a customized navigation path using a query parameters object
-	virtual void query_path(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result) const = 0;
+	virtual void query_path(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result, const Callable &p_callback = Callable()) = 0;
 
 	virtual void init() = 0;
 	virtual void sync() = 0;
@@ -318,6 +318,14 @@ public:
 	void set_debug_enabled(bool p_enabled);
 	bool get_debug_enabled() const;
 
+protected:
+#ifndef DISABLE_DEPRECATED
+	Vector<Vector2> _map_get_path_bind_compat_100129(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) const;
+	void _query_path_bind_compat_100129(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result) const;
+	static void _bind_compatibility_methods();
+#endif
+
+public:
 #ifdef DEBUG_ENABLED
 	void set_debug_navigation_enabled(bool p_enabled);
 	bool get_debug_navigation_enabled() const;
diff --git a/servers/navigation_server_2d_dummy.h b/servers/navigation_server_2d_dummy.h
index 5bc91830e6..c54e62f80e 100644
--- a/servers/navigation_server_2d_dummy.h
+++ b/servers/navigation_server_2d_dummy.h
@@ -50,7 +50,7 @@ public:
 	real_t map_get_edge_connection_margin(RID p_map) const override { return 0; }
 	void map_set_link_connection_radius(RID p_map, real_t p_connection_radius) override {}
 	real_t map_get_link_connection_radius(RID p_map) const override { return 0; }
-	Vector<Vector2> map_get_path(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) const override { return Vector<Vector2>(); }
+	Vector<Vector2> map_get_path(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) override { return Vector<Vector2>(); }
 	Vector2 map_get_closest_point(RID p_map, const Vector2 &p_point) const override { return Vector2(); }
 	RID map_get_closest_point_owner(RID p_map, const Vector2 &p_point) const override { return RID(); }
 	TypedArray<RID> map_get_links(RID p_map) const override { return TypedArray<RID>(); }
@@ -158,7 +158,7 @@ public:
 	void obstacle_set_avoidance_layers(RID p_obstacle, uint32_t p_layers) override {}
 	uint32_t obstacle_get_avoidance_layers(RID p_agent) const override { return 0; }
 
-	void query_path(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result) const override {}
+	void query_path(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result, const Callable &p_callback = Callable()) override {}
 
 	void init() override {}
 	void sync() override {}
diff --git a/servers/navigation_server_3d.compat.inc b/servers/navigation_server_3d.compat.inc
new file mode 100644
index 0000000000..633e4d5ddf
--- /dev/null
+++ b/servers/navigation_server_3d.compat.inc
@@ -0,0 +1,46 @@
+/**************************************************************************/
+/*  navigation_server_3d.compat.inc                                       */
+/**************************************************************************/
+/*                         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 DISABLE_DEPRECATED
+
+Vector<Vector3> NavigationServer3D::_map_get_path_bind_compat_100129(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers) const {
+	return const_cast<NavigationServer3D *>(this)->map_get_path(p_map, p_origin, p_destination, p_optimize, p_navigation_layers);
+}
+
+void NavigationServer3D::_query_path_bind_compat_100129(const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result) const {
+	return const_cast<NavigationServer3D *>(this)->query_path(p_query_parameters, p_query_result, Callable());
+}
+
+void NavigationServer3D::_bind_compatibility_methods() {
+	ClassDB::bind_compatibility_method(D_METHOD("map_get_path", "map", "origin", "destination", "optimize", "navigation_layers"), &NavigationServer3D::_map_get_path_bind_compat_100129, DEFVAL(1));
+	ClassDB::bind_compatibility_method(D_METHOD("query_path", "parameters", "result"), &NavigationServer3D::_query_path_bind_compat_100129);
+}
+
+#endif // DISABLE_DEPRECATED
diff --git a/servers/navigation_server_3d.cpp b/servers/navigation_server_3d.cpp
index 572309c429..4c36df83f3 100644
--- a/servers/navigation_server_3d.cpp
+++ b/servers/navigation_server_3d.cpp
@@ -29,6 +29,7 @@
 /**************************************************************************/
 
 #include "navigation_server_3d.h"
+#include "navigation_server_3d.compat.inc"
 
 #include "core/config/project_settings.h"
 #include "scene/main/node.h"
@@ -72,7 +73,7 @@ void NavigationServer3D::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("map_get_random_point", "map", "navigation_layers", "uniformly"), &NavigationServer3D::map_get_random_point);
 
-	ClassDB::bind_method(D_METHOD("query_path", "parameters", "result"), &NavigationServer3D::query_path);
+	ClassDB::bind_method(D_METHOD("query_path", "parameters", "result", "callback"), &NavigationServer3D::query_path, DEFVAL(Callable()));
 
 	ClassDB::bind_method(D_METHOD("region_create"), &NavigationServer3D::region_create);
 	ClassDB::bind_method(D_METHOD("region_set_enabled", "region", "enabled"), &NavigationServer3D::region_set_enabled);
@@ -247,6 +248,8 @@ NavigationServer3D::NavigationServer3D() {
 	GLOBAL_DEF("navigation/avoidance/thread_model/avoidance_use_multiple_threads", true);
 	GLOBAL_DEF("navigation/avoidance/thread_model/avoidance_use_high_priority_threads", true);
 
+	GLOBAL_DEF("navigation/pathfinding/max_threads", 4);
+
 	GLOBAL_DEF("navigation/baking/use_crash_prevention_checks", true);
 	GLOBAL_DEF("navigation/baking/thread_model/baking_use_multiple_threads", true);
 	GLOBAL_DEF("navigation/baking/thread_model/baking_use_high_priority_threads", true);
@@ -935,18 +938,6 @@ bool NavigationServer3D::get_debug_avoidance_enabled() const {
 
 #endif // DEBUG_ENABLED
 
-void NavigationServer3D::query_path(const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result) const {
-	ERR_FAIL_COND(!p_query_parameters.is_valid());
-	ERR_FAIL_COND(!p_query_result.is_valid());
-
-	const NavigationUtilities::PathQueryResult _query_result = _query_path(p_query_parameters->get_parameters());
-
-	p_query_result->set_path(_query_result.path);
-	p_query_result->set_path_types(_query_result.path_types);
-	p_query_result->set_path_rids(_query_result.path_rids);
-	p_query_result->set_path_owner_ids(_query_result.path_owner_ids);
-}
-
 ///////////////////////////////////////////////////////
 
 NavigationServer3DCallback NavigationServer3DManager::create_callback = nullptr;
diff --git a/servers/navigation_server_3d.h b/servers/navigation_server_3d.h
index 6dbbd35648..df5f7658bb 100644
--- a/servers/navigation_server_3d.h
+++ b/servers/navigation_server_3d.h
@@ -103,7 +103,7 @@ public:
 	virtual real_t map_get_link_connection_radius(RID p_map) const = 0;
 
 	/// Returns the navigation path to reach the destination from the origin.
-	virtual Vector<Vector3> map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) const = 0;
+	virtual Vector<Vector3> map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) = 0;
 
 	virtual Vector3 map_get_closest_point_to_segment(RID p_map, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision = false) const = 0;
 	virtual Vector3 map_get_closest_point(RID p_map, const Vector3 &p_point) const = 0;
@@ -343,9 +343,7 @@ public:
 	virtual void finish() = 0;
 
 	/// Returns a customized navigation path using a query parameters object
-	virtual void query_path(const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result) const;
-
-	virtual NavigationUtilities::PathQueryResult _query_path(const NavigationUtilities::PathQueryParameters &p_parameters) const = 0;
+	virtual void query_path(const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result, const Callable &p_callback = Callable()) = 0;
 
 #ifndef _3D_DISABLED
 	virtual void parse_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data, Node *p_root_node, const Callable &p_callback = Callable()) = 0;
@@ -380,6 +378,13 @@ public:
 	void set_debug_enabled(bool p_enabled);
 	bool get_debug_enabled() const;
 
+protected:
+#ifndef DISABLE_DEPRECATED
+	Vector<Vector3> _map_get_path_bind_compat_100129(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) const;
+	void _query_path_bind_compat_100129(const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result) const;
+	static void _bind_compatibility_methods();
+#endif
+
 private:
 	bool debug_enabled = false;
 
diff --git a/servers/navigation_server_3d_dummy.h b/servers/navigation_server_3d_dummy.h
index 210c404365..418f7b373d 100644
--- a/servers/navigation_server_3d_dummy.h
+++ b/servers/navigation_server_3d_dummy.h
@@ -55,7 +55,7 @@ public:
 	real_t map_get_edge_connection_margin(RID p_map) const override { return 0; }
 	void map_set_link_connection_radius(RID p_map, real_t p_connection_radius) override {}
 	real_t map_get_link_connection_radius(RID p_map) const override { return 0; }
-	Vector<Vector3> map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers) const override { return Vector<Vector3>(); }
+	Vector<Vector3> map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers) override { return Vector<Vector3>(); }
 	Vector3 map_get_closest_point_to_segment(RID p_map, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const override { return Vector3(); }
 	Vector3 map_get_closest_point(RID p_map, const Vector3 &p_point) const override { return Vector3(); }
 	Vector3 map_get_closest_point_normal(RID p_map, const Vector3 &p_point) const override { return Vector3(); }
@@ -178,6 +178,8 @@ public:
 	void obstacle_set_avoidance_layers(RID p_obstacle, uint32_t p_layers) override {}
 	uint32_t obstacle_get_avoidance_layers(RID p_obstacle) const override { return 0; }
 
+	virtual void query_path(const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result, const Callable &p_callback = Callable()) override {}
+
 #ifndef _3D_DISABLED
 	void parse_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data, Node *p_root_node, const Callable &p_callback = Callable()) override {}
 	void bake_from_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data, const Callable &p_callback = Callable()) override {}
@@ -197,7 +199,6 @@ public:
 	void sync() override {}
 	void finish() override {}
 
-	NavigationUtilities::PathQueryResult _query_path(const NavigationUtilities::PathQueryParameters &p_parameters) const override { return NavigationUtilities::PathQueryResult(); }
 	int get_process_info(ProcessInfo p_info) const override { return 0; }
 
 	void set_debug_enabled(bool p_enabled) {}

From 0a61ebdceac8086ed82804575361388c022faa77 Mon Sep 17 00:00:00 2001
From: Bastiaan Olij <mux213@gmail.com>
Date: Wed, 18 Sep 2024 10:14:21 +1000
Subject: [PATCH 084/124] OpenXR: Add support for binding modifiers

---
 doc/classes/ProjectSettings.xml               |   6 +
 editor/editor_inspector.cpp                   |   1 +
 editor/icons/Modifiers.svg                    |   1 +
 main/main.cpp                                 |   6 +-
 .../openxr/action_map/openxr_action_map.cpp   |  48 ++-
 modules/openxr/action_map/openxr_action_map.h |   1 +
 .../openxr/action_map/openxr_action_set.cpp   |   2 +-
 .../action_map/openxr_binding_modifier.cpp    |  52 +++
 .../action_map/openxr_binding_modifier.h      |  81 +++++
 .../action_map/openxr_haptic_feedback.cpp     |  93 ++++++
 .../action_map/openxr_haptic_feedback.h       |  72 +++++
 .../action_map/openxr_interaction_profile.cpp | 169 +++++++++-
 .../action_map/openxr_interaction_profile.h   |  53 ++-
 .../openxr_interaction_profile_metadata.cpp   | 301 +++++++++---------
 modules/openxr/config.py                      |  10 +
 .../OpenXRActionBindingModifier.xml           |  11 +
 .../OpenXRAnalogThresholdModifier.xml         |  26 ++
 .../doc_classes/OpenXRBindingModifier.xml     |  26 ++
 .../OpenXRBindingModifierEditor.xml           |  38 +++
 .../doc_classes/OpenXRDpadBindingModifier.xml |  43 +++
 .../openxr/doc_classes/OpenXRHapticBase.xml   |  11 +
 .../doc_classes/OpenXRHapticVibration.xml     |  22 ++
 .../openxr/doc_classes/OpenXRIPBinding.xml    |  16 +
 .../doc_classes/OpenXRIPBindingModifier.xml   |  11 +
 .../doc_classes/OpenXRInteractionProfile.xml  |  16 +
 .../OpenXRInteractionProfileEditor.xml        |  11 +
 .../OpenXRInteractionProfileEditorBase.xml    |  25 ++
 modules/openxr/editor/SCsub                   |   7 +-
 .../editor/openxr_action_map_editor.cpp       |  56 +++-
 .../openxr/editor/openxr_action_map_editor.h  |   8 +
 .../editor/openxr_binding_modifier_editor.cpp | 289 +++++++++++++++++
 .../editor/openxr_binding_modifier_editor.h   | 113 +++++++
 .../openxr_binding_modifiers_dialog.cpp       | 258 +++++++++++++++
 .../editor/openxr_binding_modifiers_dialog.h  |  81 +++++
 .../openxr/editor/openxr_editor_plugin.cpp    |   3 +
 modules/openxr/editor/openxr_editor_plugin.h  |   2 +
 .../openxr_interaction_profile_editor.cpp     | 135 ++++++--
 .../openxr_interaction_profile_editor.h       |  42 ++-
 ...enxr_select_interaction_profile_dialog.cpp |  16 +-
 .../openxr/editor/openxr_select_runtime.cpp   |   6 +-
 modules/openxr/editor/openxr_select_runtime.h |   2 +-
 .../openxr_dpad_binding_extension.cpp         | 274 ++++++++++++++++
 .../openxr_dpad_binding_extension.h           | 109 +++++++
 .../openxr_htc_controller_extension.cpp       | 140 ++++----
 .../openxr_htc_vive_tracker_extension.cpp     | 161 +++-------
 .../openxr_huawei_controller_extension.cpp    |  52 ++-
 .../openxr_meta_controller_extension.cpp      | 197 +++++-------
 .../openxr_ml2_controller_extension.cpp       |   7 +-
 .../openxr_pico_controller_extension.cpp      | 130 ++++----
 ...penxr_valve_analog_threshold_extension.cpp | 193 +++++++++++
 .../openxr_valve_analog_threshold_extension.h |  88 +++++
 .../openxr_wmr_controller_extension.cpp       | 135 ++++----
 modules/openxr/openxr_api.cpp                 | 170 ++++++++--
 modules/openxr/openxr_api.h                   |  12 +-
 modules/openxr/openxr_interface.cpp           |  24 +-
 modules/openxr/register_types.cpp             |  28 ++
 56 files changed, 3127 insertions(+), 763 deletions(-)
 create mode 100644 editor/icons/Modifiers.svg
 create mode 100644 modules/openxr/action_map/openxr_binding_modifier.cpp
 create mode 100644 modules/openxr/action_map/openxr_binding_modifier.h
 create mode 100644 modules/openxr/action_map/openxr_haptic_feedback.cpp
 create mode 100644 modules/openxr/action_map/openxr_haptic_feedback.h
 create mode 100644 modules/openxr/doc_classes/OpenXRActionBindingModifier.xml
 create mode 100644 modules/openxr/doc_classes/OpenXRAnalogThresholdModifier.xml
 create mode 100644 modules/openxr/doc_classes/OpenXRBindingModifier.xml
 create mode 100644 modules/openxr/doc_classes/OpenXRBindingModifierEditor.xml
 create mode 100644 modules/openxr/doc_classes/OpenXRDpadBindingModifier.xml
 create mode 100644 modules/openxr/doc_classes/OpenXRHapticBase.xml
 create mode 100644 modules/openxr/doc_classes/OpenXRHapticVibration.xml
 create mode 100644 modules/openxr/doc_classes/OpenXRIPBindingModifier.xml
 create mode 100644 modules/openxr/doc_classes/OpenXRInteractionProfileEditor.xml
 create mode 100644 modules/openxr/doc_classes/OpenXRInteractionProfileEditorBase.xml
 create mode 100644 modules/openxr/editor/openxr_binding_modifier_editor.cpp
 create mode 100644 modules/openxr/editor/openxr_binding_modifier_editor.h
 create mode 100644 modules/openxr/editor/openxr_binding_modifiers_dialog.cpp
 create mode 100644 modules/openxr/editor/openxr_binding_modifiers_dialog.h
 create mode 100644 modules/openxr/extensions/openxr_dpad_binding_extension.cpp
 create mode 100644 modules/openxr/extensions/openxr_dpad_binding_extension.h
 create mode 100644 modules/openxr/extensions/openxr_valve_analog_threshold_extension.cpp
 create mode 100644 modules/openxr/extensions/openxr_valve_analog_threshold_extension.h

diff --git a/doc/classes/ProjectSettings.xml b/doc/classes/ProjectSettings.xml
index 0a16b15931..0e54294fb6 100644
--- a/doc/classes/ProjectSettings.xml
+++ b/doc/classes/ProjectSettings.xml
@@ -3114,6 +3114,12 @@
 		<member name="threading/worker_pool/max_threads" type="int" setter="" getter="" default="-1">
 			Maximum number of threads to be used by [WorkerThreadPool]. Value of [code]-1[/code] means no limit.
 		</member>
+		<member name="xr/openxr/binding_modifiers/analog_threshold" type="bool" setter="" getter="" default="false">
+			If [code]true[/code], enables the analog threshold binding modifier if supported by the XR runtime.
+		</member>
+		<member name="xr/openxr/binding_modifiers/dpad_binding" type="bool" setter="" getter="" default="false">
+			If [code]true[/code], enables the D-pad binding modifier if supported by the XR runtime.
+		</member>
 		<member name="xr/openxr/default_action_map" type="String" setter="" getter="" default="&quot;res://openxr_action_map.tres&quot;">
 			Action map configuration to load by default.
 		</member>
diff --git a/editor/editor_inspector.cpp b/editor/editor_inspector.cpp
index f5d1c11154..e709646d73 100644
--- a/editor/editor_inspector.cpp
+++ b/editor/editor_inspector.cpp
@@ -4459,6 +4459,7 @@ void EditorInspector::_notification(int p_what) {
 		} break;
 
 		case NOTIFICATION_READY: {
+			ERR_FAIL_NULL(EditorFeatureProfileManager::get_singleton());
 			EditorFeatureProfileManager::get_singleton()->connect("current_feature_profile_changed", callable_mp(this, &EditorInspector::_feature_profile_changed));
 			set_process(is_visible_in_tree());
 			add_theme_style_override(SceneStringName(panel), get_theme_stylebox(SceneStringName(panel), SNAME("Tree")));
diff --git a/editor/icons/Modifiers.svg b/editor/icons/Modifiers.svg
new file mode 100644
index 0000000000..1b25d62027
--- /dev/null
+++ b/editor/icons/Modifiers.svg
@@ -0,0 +1 @@
+<svg xmlns="http://www.w3.org/2000/svg" width="16" height="16"><path fill="#e0e0e0" d="M4 1 3 3l1 2v4H2v3.5a2.5 2.5 0 0 0 5 0V9H5V5l1-2-1-2zm6 .174a3 3 0 0 0 0 5.652V14a1 1 0 0 0 2 0V6.824a3 3 0 0 0 0-5.648V4a1 1 0 0 1-2 0z"/></svg>
\ No newline at end of file
diff --git a/main/main.cpp b/main/main.cpp
index 0b3dcc3f5b..d9524e0ad1 100644
--- a/main/main.cpp
+++ b/main/main.cpp
@@ -2630,7 +2630,11 @@ Error Main::setup(const char *execpath, int argc, char *argv[], bool p_second_ph
 	GLOBAL_DEF_BASIC("xr/openxr/extensions/hand_tracking_unobstructed_data_source", false); // XR_HAND_TRACKING_DATA_SOURCE_UNOBSTRUCTED_EXT
 	GLOBAL_DEF_BASIC("xr/openxr/extensions/hand_tracking_controller_data_source", false); // XR_HAND_TRACKING_DATA_SOURCE_CONTROLLER_EXT
 	GLOBAL_DEF_RST_BASIC("xr/openxr/extensions/hand_interaction_profile", false);
-	GLOBAL_DEF_BASIC("xr/openxr/extensions/eye_gaze_interaction", false);
+	GLOBAL_DEF_RST_BASIC("xr/openxr/extensions/eye_gaze_interaction", false);
+
+	// OpenXR Binding modifier settings
+	GLOBAL_DEF_BASIC("xr/openxr/binding_modifiers/analog_threshold", false);
+	GLOBAL_DEF_RST_BASIC("xr/openxr/binding_modifiers/dpad_binding", false);
 
 #ifdef TOOLS_ENABLED
 	// Disabled for now, using XR inside of the editor we'll be working on during the coming months.
diff --git a/modules/openxr/action_map/openxr_action_map.cpp b/modules/openxr/action_map/openxr_action_map.cpp
index f924386ecf..858833dd4d 100644
--- a/modules/openxr/action_map/openxr_action_map.cpp
+++ b/modules/openxr/action_map/openxr_action_map.cpp
@@ -107,14 +107,25 @@ void OpenXRActionMap::remove_action_set(Ref<OpenXRActionSet> p_action_set) {
 	}
 }
 
-void OpenXRActionMap::set_interaction_profiles(Array p_interaction_profiles) {
-	interaction_profiles.clear();
+void OpenXRActionMap::clear_interaction_profiles() {
+	if (interaction_profiles.is_empty()) {
+		return;
+	}
 
-	for (int i = 0; i < p_interaction_profiles.size(); i++) {
-		Ref<OpenXRInteractionProfile> interaction_profile = p_interaction_profiles[i];
-		if (interaction_profile.is_valid() && !interaction_profiles.has(interaction_profile)) {
-			interaction_profiles.push_back(interaction_profile);
-		}
+	// Interaction profiles held within our action map set should be released and destroyed but just in case they are still used some where else.
+	for (Ref<OpenXRInteractionProfile> interaction_profile : interaction_profiles) {
+		interaction_profile->action_map = nullptr;
+	}
+	interaction_profiles.clear();
+	emit_changed();
+}
+
+void OpenXRActionMap::set_interaction_profiles(Array p_interaction_profiles) {
+	clear_interaction_profiles();
+
+	for (const Variant &interaction_profile : p_interaction_profiles) {
+		// Add them anew so we verify our interaction profile pointer.
+		add_interaction_profile(interaction_profile);
 	}
 }
 
@@ -127,8 +138,7 @@ int OpenXRActionMap::get_interaction_profile_count() const {
 }
 
 Ref<OpenXRInteractionProfile> OpenXRActionMap::find_interaction_profile(String p_path) const {
-	for (int i = 0; i < interaction_profiles.size(); i++) {
-		Ref<OpenXRInteractionProfile> interaction_profile = interaction_profiles[i];
+	for (Ref<OpenXRInteractionProfile> interaction_profile : interaction_profiles) {
 		if (interaction_profile->get_interaction_profile_path() == p_path) {
 			return interaction_profile;
 		}
@@ -147,6 +157,13 @@ void OpenXRActionMap::add_interaction_profile(Ref<OpenXRInteractionProfile> p_in
 	ERR_FAIL_COND(p_interaction_profile.is_null());
 
 	if (!interaction_profiles.has(p_interaction_profile)) {
+		if (p_interaction_profile->action_map && p_interaction_profile->action_map != this) {
+			// Interaction profiles should only relate to our action map.
+			p_interaction_profile->action_map->remove_interaction_profile(p_interaction_profile);
+		}
+
+		p_interaction_profile->action_map = this;
+
 		interaction_profiles.push_back(p_interaction_profile);
 		emit_changed();
 	}
@@ -156,6 +173,10 @@ void OpenXRActionMap::remove_interaction_profile(Ref<OpenXRInteractionProfile> p
 	int idx = interaction_profiles.find(p_interaction_profile);
 	if (idx != -1) {
 		interaction_profiles.remove_at(idx);
+
+		ERR_FAIL_COND_MSG(p_interaction_profile->action_map != this, "Removing interaction profile that belongs to this action map but had incorrect action map pointer."); // This should never happen!
+		p_interaction_profile->action_map = nullptr;
+
 		emit_changed();
 	}
 }
@@ -549,9 +570,7 @@ Ref<OpenXRAction> OpenXRActionMap::get_action(const String p_path) const {
 void OpenXRActionMap::remove_action(const String p_path, bool p_remove_interaction_profiles) {
 	Ref<OpenXRAction> action = get_action(p_path);
 	if (action.is_valid()) {
-		for (int i = 0; i < interaction_profiles.size(); i++) {
-			Ref<OpenXRInteractionProfile> interaction_profile = interaction_profiles[i];
-
+		for (Ref<OpenXRInteractionProfile> interaction_profile : interaction_profiles) {
 			if (p_remove_interaction_profiles) {
 				// Remove any bindings for this action
 				interaction_profile->remove_binding_for_action(action);
@@ -571,8 +590,7 @@ void OpenXRActionMap::remove_action(const String p_path, bool p_remove_interacti
 PackedStringArray OpenXRActionMap::get_top_level_paths(const Ref<OpenXRAction> p_action) {
 	PackedStringArray arr;
 
-	for (int i = 0; i < interaction_profiles.size(); i++) {
-		Ref<OpenXRInteractionProfile> ip = interaction_profiles[i];
+	for (Ref<OpenXRInteractionProfile> ip : interaction_profiles) {
 		const OpenXRInteractionProfileMetadata::InteractionProfile *profile = OpenXRInteractionProfileMetadata::get_singleton()->get_profile(ip->get_interaction_profile_path());
 
 		if (profile != nullptr) {
@@ -598,5 +616,5 @@ PackedStringArray OpenXRActionMap::get_top_level_paths(const Ref<OpenXRAction> p
 
 OpenXRActionMap::~OpenXRActionMap() {
 	action_sets.clear();
-	interaction_profiles.clear();
+	clear_interaction_profiles();
 }
diff --git a/modules/openxr/action_map/openxr_action_map.h b/modules/openxr/action_map/openxr_action_map.h
index 678b3d7fbc..7239a8534c 100644
--- a/modules/openxr/action_map/openxr_action_map.h
+++ b/modules/openxr/action_map/openxr_action_map.h
@@ -57,6 +57,7 @@ public:
 	void add_action_set(Ref<OpenXRActionSet> p_action_set); // Add an action set to our action map
 	void remove_action_set(Ref<OpenXRActionSet> p_action_set); // Remove an action set from our action map
 
+	void clear_interaction_profiles(); // Remove all our interaction profiles
 	void set_interaction_profiles(Array p_interaction_profiles); // Set our interaction profiles by providing an array (for loading from resource)
 	Array get_interaction_profiles() const; // Get our interaction profiles as an array (for saving to resource)
 
diff --git a/modules/openxr/action_map/openxr_action_set.cpp b/modules/openxr/action_map/openxr_action_set.cpp
index d583af2b2f..08e46b65ce 100644
--- a/modules/openxr/action_map/openxr_action_set.cpp
+++ b/modules/openxr/action_map/openxr_action_set.cpp
@@ -141,7 +141,7 @@ void OpenXRActionSet::remove_action(Ref<OpenXRAction> p_action) {
 	if (idx != -1) {
 		actions.remove_at(idx);
 
-		ERR_FAIL_COND_MSG(p_action->action_set != this, "Removing action that belongs to this action set but had incorrect action set pointer."); // this should never happen!
+		ERR_FAIL_COND_MSG(p_action->action_set != this, "Removing action that belongs to this action set but had incorrect action set pointer."); // This should never happen!
 		p_action->action_set = nullptr;
 
 		emit_changed();
diff --git a/modules/openxr/action_map/openxr_binding_modifier.cpp b/modules/openxr/action_map/openxr_binding_modifier.cpp
new file mode 100644
index 0000000000..3a1c577615
--- /dev/null
+++ b/modules/openxr/action_map/openxr_binding_modifier.cpp
@@ -0,0 +1,52 @@
+/**************************************************************************/
+/*  openxr_binding_modifier.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 "openxr_binding_modifier.h"
+
+void OpenXRBindingModifier::_bind_methods() {
+	GDVIRTUAL_BIND(_get_description);
+	GDVIRTUAL_BIND(_get_ip_modification);
+}
+
+String OpenXRBindingModifier::get_description() const {
+	String desc;
+	if (GDVIRTUAL_CALL(_get_description, desc)) {
+		return desc;
+	}
+	return "";
+}
+
+PackedByteArray OpenXRBindingModifier::get_ip_modification() {
+	PackedByteArray data;
+	if (GDVIRTUAL_CALL(_get_ip_modification, data)) {
+		return data;
+	}
+	return PackedByteArray();
+}
diff --git a/modules/openxr/action_map/openxr_binding_modifier.h b/modules/openxr/action_map/openxr_binding_modifier.h
new file mode 100644
index 0000000000..51f3b52b61
--- /dev/null
+++ b/modules/openxr/action_map/openxr_binding_modifier.h
@@ -0,0 +1,81 @@
+/**************************************************************************/
+/*  openxr_binding_modifier.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 OPENXR_BINDING_MODIFIER_H
+#define OPENXR_BINDING_MODIFIER_H
+
+#include "../action_map/openxr_action.h"
+#include "core/io/resource.h"
+
+// Part of implementation for:
+// https://registry.khronos.org/OpenXR/specs/1.1/html/xrspec.html#XR_KHR_binding_modification
+
+class OpenXRInteractionProfile;
+class OpenXRIPBinding;
+
+class OpenXRBindingModifier : public Resource {
+	GDCLASS(OpenXRBindingModifier, Resource);
+
+protected:
+	static void _bind_methods();
+
+	GDVIRTUAL0RC_REQUIRED(String, _get_description)
+	GDVIRTUAL0R_REQUIRED(PackedByteArray, _get_ip_modification)
+
+public:
+	virtual String get_description() const; // Returns the description shown in the editor
+	virtual PackedByteArray get_ip_modification(); // Return the XrBindingModificationsKHR binding modifier struct data used when calling xrSuggestInteractionProfileBindings
+};
+
+class OpenXRIPBindingModifier : public OpenXRBindingModifier {
+	GDCLASS(OpenXRIPBindingModifier, OpenXRBindingModifier);
+
+protected:
+	friend class OpenXRInteractionProfile;
+
+	OpenXRInteractionProfile *interaction_profile = nullptr; // action belongs to this interaction profile
+
+public:
+	OpenXRInteractionProfile *get_interaction_profile() const { return interaction_profile; }
+};
+
+class OpenXRActionBindingModifier : public OpenXRBindingModifier {
+	GDCLASS(OpenXRActionBindingModifier, OpenXRBindingModifier);
+
+protected:
+	friend class OpenXRIPBinding;
+
+	OpenXRIPBinding *ip_binding = nullptr; // action belongs to this binding
+
+public:
+	OpenXRIPBinding *get_ip_binding() const { return ip_binding; }
+};
+
+#endif // OPENXR_BINDING_MODIFIER_H
diff --git a/modules/openxr/action_map/openxr_haptic_feedback.cpp b/modules/openxr/action_map/openxr_haptic_feedback.cpp
new file mode 100644
index 0000000000..583e4d4fe3
--- /dev/null
+++ b/modules/openxr/action_map/openxr_haptic_feedback.cpp
@@ -0,0 +1,93 @@
+/**************************************************************************/
+/*  openxr_haptic_feedback.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 "openxr_haptic_feedback.h"
+
+////////////////////////////////////////////////////////////////////////////
+// OpenXRHapticBase
+
+void OpenXRHapticBase::_bind_methods() {
+}
+
+////////////////////////////////////////////////////////////////////////////
+// OpenXRHapticVibration
+
+void OpenXRHapticVibration::_bind_methods() {
+	ClassDB::bind_method(D_METHOD("set_duration", "duration"), &OpenXRHapticVibration::set_duration);
+	ClassDB::bind_method(D_METHOD("get_duration"), &OpenXRHapticVibration::get_duration);
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "duration"), "set_duration", "get_duration");
+
+	ClassDB::bind_method(D_METHOD("set_frequency", "frequency"), &OpenXRHapticVibration::set_frequency);
+	ClassDB::bind_method(D_METHOD("get_frequency"), &OpenXRHapticVibration::get_frequency);
+	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "frequency"), "set_frequency", "get_frequency");
+
+	ClassDB::bind_method(D_METHOD("set_amplitude", "amplitude"), &OpenXRHapticVibration::set_amplitude);
+	ClassDB::bind_method(D_METHOD("get_amplitude"), &OpenXRHapticVibration::get_amplitude);
+	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "amplitude", PROPERTY_HINT_RANGE, "0.0,1.0,0.01"), "set_amplitude", "get_amplitude");
+}
+
+void OpenXRHapticVibration::set_duration(int64_t p_duration) {
+	haptic_vibration.duration = p_duration;
+	emit_changed();
+}
+
+int64_t OpenXRHapticVibration::get_duration() const {
+	return haptic_vibration.duration;
+}
+
+void OpenXRHapticVibration::set_frequency(float p_frequency) {
+	haptic_vibration.frequency = p_frequency;
+	emit_changed();
+}
+
+float OpenXRHapticVibration::get_frequency() const {
+	return haptic_vibration.frequency;
+}
+
+void OpenXRHapticVibration::set_amplitude(float p_amplitude) {
+	haptic_vibration.amplitude = p_amplitude;
+	emit_changed();
+}
+
+float OpenXRHapticVibration::get_amplitude() const {
+	return haptic_vibration.amplitude;
+}
+
+const XrHapticBaseHeader *OpenXRHapticVibration::get_xr_structure() {
+	return (XrHapticBaseHeader *)&haptic_vibration;
+}
+
+OpenXRHapticVibration::OpenXRHapticVibration() {
+	haptic_vibration.type = XR_TYPE_HAPTIC_VIBRATION;
+	haptic_vibration.next = nullptr;
+	haptic_vibration.duration = -1;
+	haptic_vibration.frequency = 0.0;
+	haptic_vibration.amplitude = 1.0;
+}
diff --git a/modules/openxr/action_map/openxr_haptic_feedback.h b/modules/openxr/action_map/openxr_haptic_feedback.h
new file mode 100644
index 0000000000..d16f1ab169
--- /dev/null
+++ b/modules/openxr/action_map/openxr_haptic_feedback.h
@@ -0,0 +1,72 @@
+/**************************************************************************/
+/*  openxr_haptic_feedback.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 OPENXR_HAPTIC_FEEDBACK_H
+#define OPENXR_HAPTIC_FEEDBACK_H
+
+#include "core/io/resource.h"
+#include <openxr/openxr.h>
+
+class OpenXRHapticBase : public Resource {
+	GDCLASS(OpenXRHapticBase, Resource);
+
+private:
+protected:
+	static void _bind_methods();
+
+public:
+	virtual const XrHapticBaseHeader *get_xr_structure() = 0;
+};
+
+class OpenXRHapticVibration : public OpenXRHapticBase {
+	GDCLASS(OpenXRHapticVibration, OpenXRHapticBase);
+
+private:
+	XrHapticVibration haptic_vibration;
+
+protected:
+	static void _bind_methods();
+
+public:
+	void set_duration(int64_t p_duration);
+	int64_t get_duration() const;
+
+	void set_frequency(float p_frequency);
+	float get_frequency() const;
+
+	void set_amplitude(float p_amplitude);
+	float get_amplitude() const;
+
+	virtual const XrHapticBaseHeader *get_xr_structure() override;
+
+	OpenXRHapticVibration();
+};
+
+#endif // OPENXR_HAPTIC_FEEDBACK_H
diff --git a/modules/openxr/action_map/openxr_interaction_profile.cpp b/modules/openxr/action_map/openxr_interaction_profile.cpp
index 2aab55f6ec..b9aa72bfc7 100644
--- a/modules/openxr/action_map/openxr_interaction_profile.cpp
+++ b/modules/openxr/action_map/openxr_interaction_profile.cpp
@@ -39,6 +39,12 @@ void OpenXRIPBinding::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("get_binding_path"), &OpenXRIPBinding::get_binding_path);
 	ADD_PROPERTY(PropertyInfo(Variant::STRING, "binding_path"), "set_binding_path", "get_binding_path");
 
+	ClassDB::bind_method(D_METHOD("get_binding_modifier_count"), &OpenXRIPBinding::get_binding_modifier_count);
+	ClassDB::bind_method(D_METHOD("get_binding_modifier", "index"), &OpenXRIPBinding::get_binding_modifier);
+	ClassDB::bind_method(D_METHOD("set_binding_modifiers", "binding_modifiers"), &OpenXRIPBinding::set_binding_modifiers);
+	ClassDB::bind_method(D_METHOD("get_binding_modifiers"), &OpenXRIPBinding::get_binding_modifiers);
+	ADD_PROPERTY(PropertyInfo(Variant::ARRAY, "binding_modifiers", PROPERTY_HINT_RESOURCE_TYPE, "OpenXRActionBindingModifier", PROPERTY_USAGE_NO_EDITOR), "set_binding_modifiers", "get_binding_modifiers");
+
 	// Deprecated
 #ifndef DISABLE_DEPRECATED
 	ClassDB::bind_method(D_METHOD("set_paths", "paths"), &OpenXRIPBinding::set_paths);
@@ -63,7 +69,7 @@ Ref<OpenXRIPBinding> OpenXRIPBinding::new_binding(const Ref<OpenXRAction> p_acti
 	return binding;
 }
 
-void OpenXRIPBinding::set_action(const Ref<OpenXRAction> p_action) {
+void OpenXRIPBinding::set_action(const Ref<OpenXRAction> &p_action) {
 	action = p_action;
 	emit_changed();
 }
@@ -81,6 +87,75 @@ String OpenXRIPBinding::get_binding_path() const {
 	return binding_path;
 }
 
+int OpenXRIPBinding::get_binding_modifier_count() const {
+	return binding_modifiers.size();
+}
+
+Ref<OpenXRActionBindingModifier> OpenXRIPBinding::get_binding_modifier(int p_index) const {
+	ERR_FAIL_INDEX_V(p_index, binding_modifiers.size(), nullptr);
+
+	return binding_modifiers[p_index];
+}
+
+void OpenXRIPBinding::clear_binding_modifiers() {
+	// Binding modifiers held within our interaction profile set should be released and destroyed but just in case they are still used some where else.
+	if (binding_modifiers.is_empty()) {
+		return;
+	}
+
+	for (int i = 0; i < binding_modifiers.size(); i++) {
+		Ref<OpenXRActionBindingModifier> binding_modifier = binding_modifiers[i];
+		binding_modifier->ip_binding = nullptr;
+	}
+	binding_modifiers.clear();
+	emit_changed();
+}
+
+void OpenXRIPBinding::set_binding_modifiers(const Array &p_binding_modifiers) {
+	clear_binding_modifiers();
+
+	// Any binding modifier not retained in p_binding_modifiers should be freed automatically, those held within our Array will have be relinked to our interaction profile.
+	for (int i = 0; i < p_binding_modifiers.size(); i++) {
+		// Add them anew so we verify our binding modifier pointer.
+		add_binding_modifier(p_binding_modifiers[i]);
+	}
+}
+
+Array OpenXRIPBinding::get_binding_modifiers() const {
+	Array ret;
+	for (const Ref<OpenXRActionBindingModifier> &binding_modifier : binding_modifiers) {
+		ret.push_back(binding_modifier);
+	}
+	return ret;
+}
+
+void OpenXRIPBinding::add_binding_modifier(const Ref<OpenXRActionBindingModifier> &p_binding_modifier) {
+	ERR_FAIL_COND(p_binding_modifier.is_null());
+
+	if (!binding_modifiers.has(p_binding_modifier)) {
+		if (p_binding_modifier->ip_binding && p_binding_modifier->ip_binding != this) {
+			// Binding modifier should only relate to our binding.
+			p_binding_modifier->ip_binding->remove_binding_modifier(p_binding_modifier);
+		}
+
+		p_binding_modifier->ip_binding = this;
+		binding_modifiers.push_back(p_binding_modifier);
+		emit_changed();
+	}
+}
+
+void OpenXRIPBinding::remove_binding_modifier(const Ref<OpenXRActionBindingModifier> &p_binding_modifier) {
+	int idx = binding_modifiers.find(p_binding_modifier);
+	if (idx != -1) {
+		binding_modifiers.remove_at(idx);
+
+		ERR_FAIL_COND_MSG(p_binding_modifier->ip_binding != this, "Removing binding modifier that belongs to this binding but had incorrect binding pointer."); // This should never happen!
+		p_binding_modifier->ip_binding = nullptr;
+
+		emit_changed();
+	}
+}
+
 #ifndef DISABLE_DEPRECATED
 
 void OpenXRIPBinding::set_paths(const PackedStringArray p_paths) { // Deprecated, but needed for loading old action maps.
@@ -148,6 +223,12 @@ void OpenXRInteractionProfile::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_bindings", "bindings"), &OpenXRInteractionProfile::set_bindings);
 	ClassDB::bind_method(D_METHOD("get_bindings"), &OpenXRInteractionProfile::get_bindings);
 	ADD_PROPERTY(PropertyInfo(Variant::ARRAY, "bindings", PROPERTY_HINT_RESOURCE_TYPE, "OpenXRIPBinding", PROPERTY_USAGE_NO_EDITOR), "set_bindings", "get_bindings");
+
+	ClassDB::bind_method(D_METHOD("get_binding_modifier_count"), &OpenXRInteractionProfile::get_binding_modifier_count);
+	ClassDB::bind_method(D_METHOD("get_binding_modifier", "index"), &OpenXRInteractionProfile::get_binding_modifier);
+	ClassDB::bind_method(D_METHOD("set_binding_modifiers", "binding_modifiers"), &OpenXRInteractionProfile::set_binding_modifiers);
+	ClassDB::bind_method(D_METHOD("get_binding_modifiers"), &OpenXRInteractionProfile::get_binding_modifiers);
+	ADD_PROPERTY(PropertyInfo(Variant::ARRAY, "binding_modifiers", PROPERTY_HINT_RESOURCE_TYPE, "OpenXRIPBindingModifier", PROPERTY_USAGE_NO_EDITOR), "set_binding_modifiers", "get_binding_modifiers");
 }
 
 Ref<OpenXRInteractionProfile> OpenXRInteractionProfile::new_profile(const char *p_input_profile_path) {
@@ -183,7 +264,7 @@ Ref<OpenXRIPBinding> OpenXRInteractionProfile::get_binding(int p_index) const {
 	return bindings[p_index];
 }
 
-void OpenXRInteractionProfile::set_bindings(Array p_bindings) {
+void OpenXRInteractionProfile::set_bindings(const Array &p_bindings) {
 	bindings.clear();
 
 	for (Ref<OpenXRIPBinding> binding : p_bindings) {
@@ -203,7 +284,7 @@ Array OpenXRInteractionProfile::get_bindings() const {
 	return bindings;
 }
 
-Ref<OpenXRIPBinding> OpenXRInteractionProfile::find_binding(const Ref<OpenXRAction> p_action, const String &p_binding_path) const {
+Ref<OpenXRIPBinding> OpenXRInteractionProfile::find_binding(const Ref<OpenXRAction> &p_action, const String &p_binding_path) const {
 	for (Ref<OpenXRIPBinding> binding : bindings) {
 		if (binding->get_action() == p_action && binding->get_binding_path() == p_binding_path) {
 			return binding;
@@ -213,7 +294,7 @@ Ref<OpenXRIPBinding> OpenXRInteractionProfile::find_binding(const Ref<OpenXRActi
 	return Ref<OpenXRIPBinding>();
 }
 
-Vector<Ref<OpenXRIPBinding>> OpenXRInteractionProfile::get_bindings_for_action(const Ref<OpenXRAction> p_action) const {
+Vector<Ref<OpenXRIPBinding>> OpenXRInteractionProfile::get_bindings_for_action(const Ref<OpenXRAction> &p_action) const {
 	Vector<Ref<OpenXRIPBinding>> ret_bindings;
 
 	for (Ref<OpenXRIPBinding> binding : bindings) {
@@ -225,7 +306,7 @@ Vector<Ref<OpenXRIPBinding>> OpenXRInteractionProfile::get_bindings_for_action(c
 	return ret_bindings;
 }
 
-void OpenXRInteractionProfile::add_binding(Ref<OpenXRIPBinding> p_binding) {
+void OpenXRInteractionProfile::add_binding(const Ref<OpenXRIPBinding> &p_binding) {
 	ERR_FAIL_COND(p_binding.is_null());
 
 	if (!bindings.has(p_binding)) {
@@ -236,7 +317,7 @@ void OpenXRInteractionProfile::add_binding(Ref<OpenXRIPBinding> p_binding) {
 	}
 }
 
-void OpenXRInteractionProfile::remove_binding(Ref<OpenXRIPBinding> p_binding) {
+void OpenXRInteractionProfile::remove_binding(const Ref<OpenXRIPBinding> &p_binding) {
 	int idx = bindings.find(p_binding);
 	if (idx != -1) {
 		bindings.remove_at(idx);
@@ -244,7 +325,7 @@ void OpenXRInteractionProfile::remove_binding(Ref<OpenXRIPBinding> p_binding) {
 	}
 }
 
-void OpenXRInteractionProfile::add_new_binding(const Ref<OpenXRAction> p_action, const String &p_paths) {
+void OpenXRInteractionProfile::add_new_binding(const Ref<OpenXRAction> &p_action, const String &p_paths) {
 	// This is a helper function to help build our default action sets
 
 	PackedStringArray paths = p_paths.split(",", false);
@@ -255,7 +336,7 @@ void OpenXRInteractionProfile::add_new_binding(const Ref<OpenXRAction> p_action,
 	}
 }
 
-void OpenXRInteractionProfile::remove_binding_for_action(const Ref<OpenXRAction> p_action) {
+void OpenXRInteractionProfile::remove_binding_for_action(const Ref<OpenXRAction> &p_action) {
 	for (int i = bindings.size() - 1; i >= 0; i--) {
 		Ref<OpenXRIPBinding> binding = bindings[i];
 		if (binding->get_action() == p_action) {
@@ -264,7 +345,7 @@ void OpenXRInteractionProfile::remove_binding_for_action(const Ref<OpenXRAction>
 	}
 }
 
-bool OpenXRInteractionProfile::has_binding_for_action(const Ref<OpenXRAction> p_action) {
+bool OpenXRInteractionProfile::has_binding_for_action(const Ref<OpenXRAction> &p_action) {
 	for (int i = bindings.size() - 1; i >= 0; i--) {
 		Ref<OpenXRIPBinding> binding = bindings[i];
 		if (binding->get_action() == p_action) {
@@ -275,6 +356,76 @@ bool OpenXRInteractionProfile::has_binding_for_action(const Ref<OpenXRAction> p_
 	return false;
 }
 
+int OpenXRInteractionProfile::get_binding_modifier_count() const {
+	return binding_modifiers.size();
+}
+
+Ref<OpenXRIPBindingModifier> OpenXRInteractionProfile::get_binding_modifier(int p_index) const {
+	ERR_FAIL_INDEX_V(p_index, binding_modifiers.size(), nullptr);
+
+	return binding_modifiers[p_index];
+}
+
+void OpenXRInteractionProfile::clear_binding_modifiers() {
+	// Binding modifiers held within our interaction profile set should be released and destroyed but just in case they are still used some where else.
+	if (binding_modifiers.is_empty()) {
+		return;
+	}
+
+	for (int i = 0; i < binding_modifiers.size(); i++) {
+		Ref<OpenXRIPBindingModifier> binding_modifier = binding_modifiers[i];
+		binding_modifier->interaction_profile = nullptr;
+	}
+	binding_modifiers.clear();
+	emit_changed();
+}
+
+void OpenXRInteractionProfile::set_binding_modifiers(const Array &p_binding_modifiers) {
+	clear_binding_modifiers();
+
+	// Any binding modifier not retained in p_binding_modifiers should be freed automatically, those held within our Array will have be relinked to our interaction profile.
+	for (int i = 0; i < p_binding_modifiers.size(); i++) {
+		// Add them anew so we verify our binding modifier pointer.
+		add_binding_modifier(p_binding_modifiers[i]);
+	}
+}
+
+Array OpenXRInteractionProfile::get_binding_modifiers() const {
+	Array ret;
+	for (const Ref<OpenXRIPBindingModifier> &binding_modifier : binding_modifiers) {
+		ret.push_back(binding_modifier);
+	}
+	return ret;
+}
+
+void OpenXRInteractionProfile::add_binding_modifier(const Ref<OpenXRIPBindingModifier> &p_binding_modifier) {
+	ERR_FAIL_COND(p_binding_modifier.is_null());
+
+	if (!binding_modifiers.has(p_binding_modifier)) {
+		if (p_binding_modifier->interaction_profile && p_binding_modifier->interaction_profile != this) {
+			// Binding modifier should only relate to our interaction profile.
+			p_binding_modifier->interaction_profile->remove_binding_modifier(p_binding_modifier);
+		}
+
+		p_binding_modifier->interaction_profile = this;
+		binding_modifiers.push_back(p_binding_modifier);
+		emit_changed();
+	}
+}
+
+void OpenXRInteractionProfile::remove_binding_modifier(const Ref<OpenXRIPBindingModifier> &p_binding_modifier) {
+	int idx = binding_modifiers.find(p_binding_modifier);
+	if (idx != -1) {
+		binding_modifiers.remove_at(idx);
+
+		ERR_FAIL_COND_MSG(p_binding_modifier->interaction_profile != this, "Removing binding modifier that belongs to this interaction profile but had incorrect interaction profile pointer."); // This should never happen!
+		p_binding_modifier->interaction_profile = nullptr;
+
+		emit_changed();
+	}
+}
+
 OpenXRInteractionProfile::~OpenXRInteractionProfile() {
 	bindings.clear();
+	clear_binding_modifiers();
 }
diff --git a/modules/openxr/action_map/openxr_interaction_profile.h b/modules/openxr/action_map/openxr_interaction_profile.h
index 952f87a09d..846c9290ba 100644
--- a/modules/openxr/action_map/openxr_interaction_profile.h
+++ b/modules/openxr/action_map/openxr_interaction_profile.h
@@ -32,29 +32,48 @@
 #define OPENXR_INTERACTION_PROFILE_H
 
 #include "openxr_action.h"
+#include "openxr_binding_modifier.h"
 #include "openxr_interaction_profile_metadata.h"
 
 #include "core/io/resource.h"
 
+class OpenXRActionMap;
+
 class OpenXRIPBinding : public Resource {
 	GDCLASS(OpenXRIPBinding, Resource);
 
 private:
 	Ref<OpenXRAction> action;
 	String binding_path;
+	Vector<Ref<OpenXRActionBindingModifier>> binding_modifiers;
 
 protected:
+	friend class OpenXRActionMap;
+
+	OpenXRActionMap *action_map = nullptr;
+
 	static void _bind_methods();
 
 public:
 	static Ref<OpenXRIPBinding> new_binding(const Ref<OpenXRAction> p_action, const String &p_binding_path); // Helper function for adding a new binding.
 
-	void set_action(const Ref<OpenXRAction> p_action); // Set the action for this binding.
+	OpenXRActionMap *get_action_map() { return action_map; } // Return the action map we're a part of.
+
+	void set_action(const Ref<OpenXRAction> &p_action); // Set the action for this binding.
 	Ref<OpenXRAction> get_action() const; // Get the action for this binding.
 
 	void set_binding_path(const String &path);
 	String get_binding_path() const;
 
+	int get_binding_modifier_count() const; // Retrieve the number of binding modifiers in this profile path
+	Ref<OpenXRActionBindingModifier> get_binding_modifier(int p_index) const;
+	void clear_binding_modifiers(); // Remove all binding modifiers
+	void set_binding_modifiers(const Array &p_bindings); // Set the binding modifiers (for loading from a resource)
+	Array get_binding_modifiers() const; // Get the binding modifiers (for saving to a resource)
+
+	void add_binding_modifier(const Ref<OpenXRActionBindingModifier> &p_binding_modifier); // Add a binding modifier object
+	void remove_binding_modifier(const Ref<OpenXRActionBindingModifier> &p_binding_modifier); // Remove a binding modifier object
+
 	// Deprecated.
 #ifndef DISABLE_DEPRECATED
 	void set_paths(const PackedStringArray p_paths); // Set our paths (for loading from resource), needed for loading old action maps.
@@ -76,29 +95,45 @@ class OpenXRInteractionProfile : public Resource {
 private:
 	String interaction_profile_path;
 	Array bindings;
+	Vector<Ref<OpenXRIPBindingModifier>> binding_modifiers;
 
 protected:
+	friend class OpenXRActionMap;
+
+	OpenXRActionMap *action_map = nullptr;
+
 	static void _bind_methods();
 
 public:
 	static Ref<OpenXRInteractionProfile> new_profile(const char *p_input_profile_path); // Helper function to create a new interaction profile
 
+	OpenXRActionMap *get_action_map() { return action_map; }
+
 	void set_interaction_profile_path(const String p_input_profile_path); // Set our input profile path
 	String get_interaction_profile_path() const; // get our input profile path
 
 	int get_binding_count() const; // Retrieve the number of bindings in this profile path
 	Ref<OpenXRIPBinding> get_binding(int p_index) const;
-	void set_bindings(Array p_bindings); // Set the bindings (for loading from a resource)
+	void set_bindings(const Array &p_bindings); // Set the bindings (for loading from a resource)
 	Array get_bindings() const; // Get the bindings (for saving to a resource)
 
-	Ref<OpenXRIPBinding> find_binding(const Ref<OpenXRAction> p_action, const String &p_binding_path) const; // Get our binding record
-	Vector<Ref<OpenXRIPBinding>> get_bindings_for_action(const Ref<OpenXRAction> p_action) const; // Get our binding record for a given action
-	void add_binding(Ref<OpenXRIPBinding> p_binding); // Add a binding object
-	void remove_binding(Ref<OpenXRIPBinding> p_binding); // Remove a binding object
+	Ref<OpenXRIPBinding> find_binding(const Ref<OpenXRAction> &p_action, const String &p_binding_path) const; // Get our binding record
+	Vector<Ref<OpenXRIPBinding>> get_bindings_for_action(const Ref<OpenXRAction> &p_action) const; // Get our binding record for a given action
+	void add_binding(const Ref<OpenXRIPBinding> &p_binding); // Add a binding object
+	void remove_binding(const Ref<OpenXRIPBinding> &p_binding); // Remove a binding object
 
-	void add_new_binding(const Ref<OpenXRAction> p_action, const String &p_paths); // Create a new binding for this profile
-	void remove_binding_for_action(const Ref<OpenXRAction> p_action); // Remove all bindings for this action
-	bool has_binding_for_action(const Ref<OpenXRAction> p_action); // Returns true if we have a binding for this action
+	void add_new_binding(const Ref<OpenXRAction> &p_action, const String &p_paths); // Create a new binding for this profile
+	void remove_binding_for_action(const Ref<OpenXRAction> &p_action); // Remove all bindings for this action
+	bool has_binding_for_action(const Ref<OpenXRAction> &p_action); // Returns true if we have a binding for this action
+
+	int get_binding_modifier_count() const; // Retrieve the number of binding modifiers in this profile path
+	Ref<OpenXRIPBindingModifier> get_binding_modifier(int p_index) const;
+	void clear_binding_modifiers(); // Remove all binding modifiers
+	void set_binding_modifiers(const Array &p_bindings); // Set the binding modifiers (for loading from a resource)
+	Array get_binding_modifiers() const; // Get the binding modifiers (for saving to a resource)
+
+	void add_binding_modifier(const Ref<OpenXRIPBindingModifier> &p_binding_modifier); // Add a binding modifier object
+	void remove_binding_modifier(const Ref<OpenXRIPBindingModifier> &p_binding_modifier); // Remove a binding modifier object
 
 	~OpenXRInteractionProfile();
 };
diff --git a/modules/openxr/action_map/openxr_interaction_profile_metadata.cpp b/modules/openxr/action_map/openxr_interaction_profile_metadata.cpp
index 6315c95a03..d5f6f67214 100644
--- a/modules/openxr/action_map/openxr_interaction_profile_metadata.cpp
+++ b/modules/openxr/action_map/openxr_interaction_profile_metadata.cpp
@@ -93,9 +93,9 @@ void OpenXRInteractionProfileMetadata::register_io_path(const String &p_interact
 	ERR_FAIL_COND_MSG(!has_interaction_profile(p_interaction_profile), "Unknown interaction profile " + p_interaction_profile);
 	ERR_FAIL_COND_MSG(!has_top_level_path(p_toplevel_path), "Unknown top level path " + p_toplevel_path);
 
-	for (int i = 0; i < interaction_profiles.size(); i++) {
-		if (interaction_profiles[i].openxr_path == p_interaction_profile) {
-			ERR_FAIL_COND_MSG(interaction_profiles[i].has_io_path(p_openxr_path), p_interaction_profile + " already has io path " + p_openxr_path + " registered!");
+	for (InteractionProfile &interaction_profile : interaction_profiles) {
+		if (interaction_profile.openxr_path == p_interaction_profile) {
+			ERR_FAIL_COND_MSG(interaction_profile.has_io_path(p_openxr_path), p_interaction_profile + " already has io path " + p_openxr_path + " registered!");
 
 			IOPath new_io_path = {
 				p_display_name,
@@ -105,7 +105,7 @@ void OpenXRInteractionProfileMetadata::register_io_path(const String &p_interact
 				p_action_type
 			};
 
-			interaction_profiles.ptrw()[i].io_paths.push_back(new_io_path);
+			interaction_profile.io_paths.push_back(new_io_path);
 		}
 	}
 }
@@ -141,8 +141,8 @@ String OpenXRInteractionProfileMetadata::get_top_level_extension(const String p_
 }
 
 bool OpenXRInteractionProfileMetadata::has_interaction_profile(const String p_openxr_path) const {
-	for (int i = 0; i < interaction_profiles.size(); i++) {
-		if (interaction_profiles[i].openxr_path == p_openxr_path) {
+	for (const InteractionProfile &interaction_profile : interaction_profiles) {
+		if (interaction_profile.openxr_path == p_openxr_path) {
 			return true;
 		}
 	}
@@ -151,9 +151,9 @@ bool OpenXRInteractionProfileMetadata::has_interaction_profile(const String p_op
 }
 
 String OpenXRInteractionProfileMetadata::get_interaction_profile_extension(const String p_openxr_path) const {
-	for (int i = 0; i < interaction_profiles.size(); i++) {
-		if (interaction_profiles[i].openxr_path == p_openxr_path) {
-			return interaction_profiles[i].openxr_extension_name;
+	for (const InteractionProfile &interaction_profile : interaction_profiles) {
+		if (interaction_profile.openxr_path == p_openxr_path) {
+			return interaction_profile.openxr_extension_name;
 		}
 	}
 
@@ -161,9 +161,9 @@ String OpenXRInteractionProfileMetadata::get_interaction_profile_extension(const
 }
 
 const OpenXRInteractionProfileMetadata::InteractionProfile *OpenXRInteractionProfileMetadata::get_profile(const String p_openxr_path) const {
-	for (int i = 0; i < interaction_profiles.size(); i++) {
-		if (interaction_profiles[i].openxr_path == p_openxr_path) {
-			return &interaction_profiles[i];
+	for (const InteractionProfile &interaction_profile : interaction_profiles) {
+		if (interaction_profile.openxr_path == p_openxr_path) {
+			return &interaction_profile;
 		}
 	}
 
@@ -202,8 +202,8 @@ const OpenXRInteractionProfileMetadata::IOPath *OpenXRInteractionProfileMetadata
 PackedStringArray OpenXRInteractionProfileMetadata::get_interaction_profile_paths() const {
 	PackedStringArray arr;
 
-	for (int i = 0; i < interaction_profiles.size(); i++) {
-		arr.push_back(interaction_profiles[i].openxr_path);
+	for (const InteractionProfile &interaction_profile : interaction_profiles) {
+		arr.push_back(interaction_profile.openxr_path);
 	}
 
 	return arr;
@@ -225,179 +225,164 @@ void OpenXRInteractionProfileMetadata::_register_core_metadata() {
 	register_top_level_path("Gamepad", "/user/gamepad", "");
 	register_top_level_path("Treadmill", "/user/treadmill", "");
 
-	// Fallback Khronos simple controller
-	register_interaction_profile("Simple controller", "/interaction_profiles/khr/simple_controller", "");
-	register_io_path("/interaction_profiles/khr/simple_controller", "Grip pose", "/user/hand/left", "/user/hand/left/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	register_io_path("/interaction_profiles/khr/simple_controller", "Grip pose", "/user/hand/right", "/user/hand/right/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	register_io_path("/interaction_profiles/khr/simple_controller", "Aim pose", "/user/hand/left", "/user/hand/left/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	register_io_path("/interaction_profiles/khr/simple_controller", "Aim pose", "/user/hand/right", "/user/hand/right/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	register_io_path("/interaction_profiles/khr/simple_controller", "Palm pose", "/user/hand/left", "/user/hand/left/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
-	register_io_path("/interaction_profiles/khr/simple_controller", "Palm pose", "/user/hand/right", "/user/hand/right/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
+	{ // Fallback Khronos simple controller
+		const String profile_path = "/interaction_profiles/khr/simple_controller";
+		register_interaction_profile("Simple controller", profile_path, "");
+		for (const String user_path : { "/user/hand/left", "/user/hand/right" }) {
+			register_io_path(profile_path, "Grip pose", user_path, user_path + "/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			register_io_path(profile_path, "Aim pose", user_path, user_path + "/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			register_io_path(profile_path, "Palm pose", user_path, user_path + "/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
 
-	register_io_path("/interaction_profiles/khr/simple_controller", "Menu click", "/user/hand/left", "/user/hand/left/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/khr/simple_controller", "Menu click", "/user/hand/right", "/user/hand/right/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/khr/simple_controller", "Select click", "/user/hand/left", "/user/hand/left/input/select/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/khr/simple_controller", "Select click", "/user/hand/right", "/user/hand/right/input/select/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Menu click", user_path, user_path + "/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Select click", user_path, user_path + "/input/select/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	register_io_path("/interaction_profiles/khr/simple_controller", "Haptic output", "/user/hand/left", "/user/hand/left/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	register_io_path("/interaction_profiles/khr/simple_controller", "Haptic output", "/user/hand/right", "/user/hand/right/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+			register_io_path(profile_path, "Haptic output", user_path, user_path + "/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+		}
+	}
 
-	// Original HTC Vive wands
-	register_interaction_profile("HTC Vive wand", "/interaction_profiles/htc/vive_controller", "");
-	register_io_path("/interaction_profiles/htc/vive_controller", "Grip pose", "/user/hand/left", "/user/hand/left/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	register_io_path("/interaction_profiles/htc/vive_controller", "Grip pose", "/user/hand/right", "/user/hand/right/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	register_io_path("/interaction_profiles/htc/vive_controller", "Aim pose", "/user/hand/left", "/user/hand/left/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	register_io_path("/interaction_profiles/htc/vive_controller", "Aim pose", "/user/hand/right", "/user/hand/right/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	register_io_path("/interaction_profiles/htc/vive_controller", "Palm pose", "/user/hand/left", "/user/hand/left/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
-	register_io_path("/interaction_profiles/htc/vive_controller", "Palm pose", "/user/hand/right", "/user/hand/right/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
+	{ // Original HTC Vive wands
+		const String profile_path = "/interaction_profiles/htc/vive_controller";
+		register_interaction_profile("HTC Vive wand", profile_path, "");
+		for (const String user_path : { "/user/hand/left", "/user/hand/right" }) {
+			register_io_path(profile_path, "Grip pose", user_path, user_path + "/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			register_io_path(profile_path, "Aim pose", user_path, user_path + "/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			register_io_path(profile_path, "Palm pose", user_path, user_path + "/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
 
-	register_io_path("/interaction_profiles/htc/vive_controller", "Menu click", "/user/hand/left", "/user/hand/left/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/htc/vive_controller", "Menu click", "/user/hand/right", "/user/hand/right/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/htc/vive_controller", "System click", "/user/hand/left", "/user/hand/left/input/system/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/htc/vive_controller", "System click", "/user/hand/right", "/user/hand/right/input/system/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Menu click", user_path, user_path + "/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "System click", user_path, user_path + "/input/system/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	register_io_path("/interaction_profiles/htc/vive_controller", "Trigger", "/user/hand/left", "/user/hand/left/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	register_io_path("/interaction_profiles/htc/vive_controller", "Trigger click", "/user/hand/left", "/user/hand/left/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/htc/vive_controller", "Trigger", "/user/hand/right", "/user/hand/right/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	register_io_path("/interaction_profiles/htc/vive_controller", "Trigger click", "/user/hand/right", "/user/hand/right/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Trigger", user_path, user_path + "/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			register_io_path(profile_path, "Trigger click", user_path, user_path + "/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	register_io_path("/interaction_profiles/htc/vive_controller", "Squeeze click", "/user/hand/left", "/user/hand/left/input/squeeze/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/htc/vive_controller", "Squeeze click", "/user/hand/right", "/user/hand/right/input/squeeze/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Squeeze click", user_path, user_path + "/input/squeeze/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	register_io_path("/interaction_profiles/htc/vive_controller", "Trackpad", "/user/hand/left", "/user/hand/left/input/trackpad", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	register_io_path("/interaction_profiles/htc/vive_controller", "Trackpad click", "/user/hand/left", "/user/hand/left/input/trackpad/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/htc/vive_controller", "Trackpad touch", "/user/hand/left", "/user/hand/left/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/htc/vive_controller", "Trackpad", "/user/hand/right", "/user/hand/right/input/trackpad", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	register_io_path("/interaction_profiles/htc/vive_controller", "Trackpad click", "/user/hand/right", "/user/hand/right/input/trackpad/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/htc/vive_controller", "Trackpad touch", "/user/hand/right", "/user/hand/right/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Trackpad", user_path, user_path + "/input/trackpad", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
+			register_io_path(profile_path, "Trackpad click", user_path, user_path + "/input/trackpad/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Trackpad touch", user_path, user_path + "/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Trackpad Dpad Up", user_path, user_path + "/input/trackpad/dpad_up", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Trackpad Dpad Down", user_path, user_path + "/input/trackpad/dpad_down", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Trackpad Dpad Left", user_path, user_path + "/input/trackpad/dpad_left", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Trackpad Dpad Right", user_path, user_path + "/input/trackpad/dpad_right", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Trackpad Dpad Center", user_path, user_path + "/input/trackpad/dpad_center", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	register_io_path("/interaction_profiles/htc/vive_controller", "Haptic output", "/user/hand/left", "/user/hand/left/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	register_io_path("/interaction_profiles/htc/vive_controller", "Haptic output", "/user/hand/right", "/user/hand/right/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+			register_io_path(profile_path, "Haptic output", user_path, user_path + "/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+		}
+	}
 
-	// Microsoft motion controller (original WMR controllers)
-	register_interaction_profile("MS Motion controller", "/interaction_profiles/microsoft/motion_controller", "");
-	register_io_path("/interaction_profiles/microsoft/motion_controller", "Grip pose", "/user/hand/left", "/user/hand/left/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	register_io_path("/interaction_profiles/microsoft/motion_controller", "Grip pose", "/user/hand/right", "/user/hand/right/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	register_io_path("/interaction_profiles/microsoft/motion_controller", "Aim pose", "/user/hand/left", "/user/hand/left/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	register_io_path("/interaction_profiles/microsoft/motion_controller", "Aim pose", "/user/hand/right", "/user/hand/right/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	register_io_path("/interaction_profiles/microsoft/motion_controller", "Palm pose", "/user/hand/left", "/user/hand/left/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
-	register_io_path("/interaction_profiles/microsoft/motion_controller", "Palm pose", "/user/hand/right", "/user/hand/right/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
+	{ // Microsoft motion controller (original WMR controllers)
+		const String profile_path = "/interaction_profiles/microsoft/motion_controller";
+		register_interaction_profile("MS Motion controller", profile_path, "");
+		for (const String user_path : { "/user/hand/left", "/user/hand/right" }) {
+			register_io_path(profile_path, "Grip pose", user_path, user_path + "/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			register_io_path(profile_path, "Aim pose", user_path, user_path + "/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			register_io_path(profile_path, "Palm pose", user_path, user_path + "/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
 
-	register_io_path("/interaction_profiles/microsoft/motion_controller", "Menu click", "/user/hand/left", "/user/hand/left/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/microsoft/motion_controller", "Menu click", "/user/hand/right", "/user/hand/right/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Menu click", user_path, user_path + "/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	register_io_path("/interaction_profiles/microsoft/motion_controller", "Trigger", "/user/hand/left", "/user/hand/left/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	register_io_path("/interaction_profiles/microsoft/motion_controller", "Trigger click", "/user/hand/left", "/user/hand/left/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/microsoft/motion_controller", "Trigger", "/user/hand/right", "/user/hand/right/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	register_io_path("/interaction_profiles/microsoft/motion_controller", "Trigger click", "/user/hand/right", "/user/hand/right/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Trigger", user_path, user_path + "/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			register_io_path(profile_path, "Trigger click", user_path, user_path + "/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	register_io_path("/interaction_profiles/microsoft/motion_controller", "Squeeze click", "/user/hand/left", "/user/hand/left/input/squeeze/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/microsoft/motion_controller", "Squeeze click", "/user/hand/right", "/user/hand/right/input/squeeze/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Squeeze click", user_path, user_path + "/input/squeeze/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	register_io_path("/interaction_profiles/microsoft/motion_controller", "Thumbstick", "/user/hand/left", "/user/hand/left/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	register_io_path("/interaction_profiles/microsoft/motion_controller", "Thumbstick click", "/user/hand/left", "/user/hand/left/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/microsoft/motion_controller", "Thumbstick", "/user/hand/right", "/user/hand/right/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	register_io_path("/interaction_profiles/microsoft/motion_controller", "Thumbstick click", "/user/hand/right", "/user/hand/right/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Thumbstick", user_path, user_path + "/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
+			register_io_path(profile_path, "Thumbstick click", user_path, user_path + "/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Thumbstick Dpad Up", user_path, user_path + "/input/thumbstick/dpad_up", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Thumbstick Dpad Down", user_path, user_path + "/input/thumbstick/dpad_down", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Thumbstick Dpad Left", user_path, user_path + "/input/thumbstick/dpad_left", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Thumbstick Dpad Right", user_path, user_path + "/input/thumbstick/dpad_right", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	register_io_path("/interaction_profiles/microsoft/motion_controller", "Trackpad", "/user/hand/left", "/user/hand/left/input/trackpad", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	register_io_path("/interaction_profiles/microsoft/motion_controller", "Trackpad click", "/user/hand/left", "/user/hand/left/input/trackpad/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/microsoft/motion_controller", "Trackpad touch", "/user/hand/left", "/user/hand/left/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/microsoft/motion_controller", "Trackpad", "/user/hand/right", "/user/hand/right/input/trackpad", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	register_io_path("/interaction_profiles/microsoft/motion_controller", "Trackpad click", "/user/hand/right", "/user/hand/right/input/trackpad/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/microsoft/motion_controller", "Trackpad touch", "/user/hand/right", "/user/hand/right/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Trackpad", user_path, user_path + "/input/trackpad", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
+			register_io_path(profile_path, "Trackpad click", user_path, user_path + "/input/trackpad/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Trackpad touch", user_path, user_path + "/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Trackpad Dpad Up", user_path, user_path + "/input/trackpad/dpad_up", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Trackpad Dpad Down", user_path, user_path + "/input/trackpad/dpad_down", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Trackpad Dpad Left", user_path, user_path + "/input/trackpad/dpad_left", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Trackpad Dpad Right", user_path, user_path + "/input/trackpad/dpad_right", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Trackpad Dpad Center", user_path, user_path + "/input/trackpad/dpad_center", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	register_io_path("/interaction_profiles/microsoft/motion_controller", "Haptic output", "/user/hand/left", "/user/hand/left/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	register_io_path("/interaction_profiles/microsoft/motion_controller", "Haptic output", "/user/hand/right", "/user/hand/right/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+			register_io_path(profile_path, "Haptic output", user_path, user_path + "/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+		}
+	}
 
-	// Meta touch controller (original touch controllers, Quest 1 and Quest 2 controllers)
-	register_interaction_profile("Touch controller", "/interaction_profiles/oculus/touch_controller", "");
-	register_io_path("/interaction_profiles/oculus/touch_controller", "Grip pose", "/user/hand/left", "/user/hand/left/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	register_io_path("/interaction_profiles/oculus/touch_controller", "Grip pose", "/user/hand/right", "/user/hand/right/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	register_io_path("/interaction_profiles/oculus/touch_controller", "Aim pose", "/user/hand/left", "/user/hand/left/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	register_io_path("/interaction_profiles/oculus/touch_controller", "Aim pose", "/user/hand/right", "/user/hand/right/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	register_io_path("/interaction_profiles/oculus/touch_controller", "Palm pose", "/user/hand/left", "/user/hand/left/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
-	register_io_path("/interaction_profiles/oculus/touch_controller", "Palm pose", "/user/hand/right", "/user/hand/right/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
+	{ // Meta touch controller (original touch controllers, Quest 1 and Quest 2 controllers)
+		const String profile_path = "/interaction_profiles/oculus/touch_controller";
+		register_interaction_profile("Touch controller", profile_path, "");
+		for (const String user_path : { "/user/hand/left", "/user/hand/right" }) {
+			register_io_path(profile_path, "Grip pose", user_path, user_path + "/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			register_io_path(profile_path, "Aim pose", user_path, user_path + "/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			register_io_path(profile_path, "Palm pose", user_path, user_path + "/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
 
-	register_io_path("/interaction_profiles/oculus/touch_controller", "Menu click", "/user/hand/left", "/user/hand/left/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/oculus/touch_controller", "System click", "/user/hand/right", "/user/hand/right/input/system/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Trigger", user_path, user_path + "/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			register_io_path(profile_path, "Trigger touch", user_path, user_path + "/input/trigger/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	register_io_path("/interaction_profiles/oculus/touch_controller", "X click", "/user/hand/left", "/user/hand/left/input/x/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/oculus/touch_controller", "X touch", "/user/hand/left", "/user/hand/left/input/x/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/oculus/touch_controller", "Y click", "/user/hand/left", "/user/hand/left/input/y/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/oculus/touch_controller", "Y touch", "/user/hand/left", "/user/hand/left/input/y/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/oculus/touch_controller", "A click", "/user/hand/right", "/user/hand/right/input/a/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/oculus/touch_controller", "A touch", "/user/hand/right", "/user/hand/right/input/a/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/oculus/touch_controller", "B click", "/user/hand/right", "/user/hand/right/input/b/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/oculus/touch_controller", "B touch", "/user/hand/right", "/user/hand/right/input/b/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Squeeze", user_path, user_path + "/input/squeeze/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
 
-	register_io_path("/interaction_profiles/oculus/touch_controller", "Trigger", "/user/hand/left", "/user/hand/left/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	register_io_path("/interaction_profiles/oculus/touch_controller", "Trigger touch", "/user/hand/left", "/user/hand/left/input/trigger/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/oculus/touch_controller", "Trigger", "/user/hand/right", "/user/hand/right/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	register_io_path("/interaction_profiles/oculus/touch_controller", "Trigger touch", "/user/hand/right", "/user/hand/right/input/trigger/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Thumbstick", user_path, user_path + "/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
+			register_io_path(profile_path, "Thumbstick click", user_path, user_path + "/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Thumbstick touch", user_path, user_path + "/input/thumbstick/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Thumbstick Dpad Up", user_path, user_path + "/input/thumbstick/dpad_up", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Thumbstick Dpad Down", user_path, user_path + "/input/thumbstick/dpad_down", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Thumbstick Dpad Left", user_path, user_path + "/input/thumbstick/dpad_left", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Thumbstick Dpad Right", user_path, user_path + "/input/thumbstick/dpad_right", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	register_io_path("/interaction_profiles/oculus/touch_controller", "Squeeze", "/user/hand/left", "/user/hand/left/input/squeeze/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	register_io_path("/interaction_profiles/oculus/touch_controller", "Squeeze", "/user/hand/right", "/user/hand/right/input/squeeze/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			register_io_path(profile_path, "Thumbrest touch", user_path, user_path + "/input/thumbrest/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	register_io_path("/interaction_profiles/oculus/touch_controller", "Thumbstick", "/user/hand/left", "/user/hand/left/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	register_io_path("/interaction_profiles/oculus/touch_controller", "Thumbstick click", "/user/hand/left", "/user/hand/left/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/oculus/touch_controller", "Thumbstick touch", "/user/hand/left", "/user/hand/left/input/thumbstick/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/oculus/touch_controller", "Thumbstick", "/user/hand/right", "/user/hand/right/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	register_io_path("/interaction_profiles/oculus/touch_controller", "Thumbstick click", "/user/hand/right", "/user/hand/right/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/oculus/touch_controller", "Thumbstick touch", "/user/hand/right", "/user/hand/right/input/thumbstick/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Haptic output", user_path, user_path + "/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+		}
 
-	register_io_path("/interaction_profiles/oculus/touch_controller", "Thumbrest touch", "/user/hand/left", "/user/hand/left/input/thumbrest/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/oculus/touch_controller", "Thumbrest touch", "/user/hand/right", "/user/hand/right/input/thumbrest/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		register_io_path(profile_path, "Menu click", "/user/hand/left", "/user/hand/left/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		register_io_path(profile_path, "System click", "/user/hand/right", "/user/hand/right/input/system/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	register_io_path("/interaction_profiles/oculus/touch_controller", "Haptic output", "/user/hand/left", "/user/hand/left/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	register_io_path("/interaction_profiles/oculus/touch_controller", "Haptic output", "/user/hand/right", "/user/hand/right/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+		register_io_path(profile_path, "X click", "/user/hand/left", "/user/hand/left/input/x/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		register_io_path(profile_path, "X touch", "/user/hand/left", "/user/hand/left/input/x/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		register_io_path(profile_path, "Y click", "/user/hand/left", "/user/hand/left/input/y/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		register_io_path(profile_path, "Y touch", "/user/hand/left", "/user/hand/left/input/y/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		register_io_path(profile_path, "A click", "/user/hand/right", "/user/hand/right/input/a/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		register_io_path(profile_path, "A touch", "/user/hand/right", "/user/hand/right/input/a/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		register_io_path(profile_path, "B click", "/user/hand/right", "/user/hand/right/input/b/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		register_io_path(profile_path, "B touch", "/user/hand/right", "/user/hand/right/input/b/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+	}
 
-	// Valve Index controller
-	register_interaction_profile("Index controller", "/interaction_profiles/valve/index_controller", "");
-	register_io_path("/interaction_profiles/valve/index_controller", "Grip pose", "/user/hand/left", "/user/hand/left/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	register_io_path("/interaction_profiles/valve/index_controller", "Grip pose", "/user/hand/right", "/user/hand/right/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	register_io_path("/interaction_profiles/valve/index_controller", "Aim pose", "/user/hand/left", "/user/hand/left/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	register_io_path("/interaction_profiles/valve/index_controller", "Aim pose", "/user/hand/right", "/user/hand/right/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	register_io_path("/interaction_profiles/valve/index_controller", "Palm pose", "/user/hand/left", "/user/hand/left/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
-	register_io_path("/interaction_profiles/valve/index_controller", "Palm pose", "/user/hand/right", "/user/hand/right/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
+	{ // Valve Index controller
+		const String profile_path = "/interaction_profiles/valve/index_controller";
+		register_interaction_profile("Index controller", profile_path, "");
+		for (const String user_path : { "/user/hand/left", "/user/hand/right" }) {
+			register_io_path(profile_path, "Grip pose", user_path, user_path + "/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			register_io_path(profile_path, "Aim pose", user_path, user_path + "/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			register_io_path(profile_path, "Palm pose", user_path, user_path + "/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
 
-	register_io_path("/interaction_profiles/valve/index_controller", "System click", "/user/hand/left", "/user/hand/left/input/system/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/valve/index_controller", "System click", "/user/hand/right", "/user/hand/right/input/system/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "System click", user_path, user_path + "/input/system/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	register_io_path("/interaction_profiles/valve/index_controller", "A click", "/user/hand/left", "/user/hand/left/input/a/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/valve/index_controller", "A touch", "/user/hand/left", "/user/hand/left/input/a/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/valve/index_controller", "A click", "/user/hand/right", "/user/hand/right/input/a/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/valve/index_controller", "A touch", "/user/hand/right", "/user/hand/right/input/a/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/valve/index_controller", "B click", "/user/hand/left", "/user/hand/left/input/b/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/valve/index_controller", "B touch", "/user/hand/left", "/user/hand/left/input/b/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/valve/index_controller", "B click", "/user/hand/right", "/user/hand/right/input/b/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/valve/index_controller", "B touch", "/user/hand/right", "/user/hand/right/input/b/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "A click", user_path, user_path + "/input/a/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "A touch", user_path, user_path + "/input/a/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "B click", user_path, user_path + "/input/b/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "B touch", user_path, user_path + "/input/b/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	register_io_path("/interaction_profiles/valve/index_controller", "Trigger", "/user/hand/left", "/user/hand/left/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	register_io_path("/interaction_profiles/valve/index_controller", "Trigger click", "/user/hand/left", "/user/hand/left/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/valve/index_controller", "Trigger touch", "/user/hand/left", "/user/hand/left/input/trigger/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/valve/index_controller", "Trigger", "/user/hand/right", "/user/hand/right/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	register_io_path("/interaction_profiles/valve/index_controller", "Trigger click", "/user/hand/right", "/user/hand/right/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/valve/index_controller", "Trigger touch", "/user/hand/right", "/user/hand/right/input/trigger/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Trigger", user_path, user_path + "/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			register_io_path(profile_path, "Trigger click", user_path, user_path + "/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Trigger touch", user_path, user_path + "/input/trigger/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	register_io_path("/interaction_profiles/valve/index_controller", "Squeeze", "/user/hand/left", "/user/hand/left/input/squeeze/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	register_io_path("/interaction_profiles/valve/index_controller", "Squeeze force", "/user/hand/left", "/user/hand/left/input/squeeze/force", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	register_io_path("/interaction_profiles/valve/index_controller", "Squeeze", "/user/hand/right", "/user/hand/right/input/squeeze/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	register_io_path("/interaction_profiles/valve/index_controller", "Squeeze force", "/user/hand/right", "/user/hand/right/input/squeeze/force", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			register_io_path(profile_path, "Squeeze", user_path, user_path + "/input/squeeze/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			register_io_path(profile_path, "Squeeze force", user_path, user_path + "/input/squeeze/force", "", OpenXRAction::OPENXR_ACTION_FLOAT);
 
-	register_io_path("/interaction_profiles/valve/index_controller", "Thumbstick", "/user/hand/left", "/user/hand/left/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	register_io_path("/interaction_profiles/valve/index_controller", "Thumbstick click", "/user/hand/left", "/user/hand/left/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/valve/index_controller", "Thumbstick touch", "/user/hand/left", "/user/hand/left/input/thumbstick/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/valve/index_controller", "Thumbstick", "/user/hand/right", "/user/hand/right/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	register_io_path("/interaction_profiles/valve/index_controller", "Thumbstick click", "/user/hand/right", "/user/hand/right/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/valve/index_controller", "Thumbstick touch", "/user/hand/right", "/user/hand/right/input/thumbstick/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Thumbstick", user_path, user_path + "/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
+			register_io_path(profile_path, "Thumbstick click", user_path, user_path + "/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Thumbstick touch", user_path, user_path + "/input/thumbstick/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Thumbstick Dpad Up", user_path, user_path + "/input/thumbstick/dpad_up", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Thumbstick Dpad Down", user_path, user_path + "/input/thumbstick/dpad_down", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Thumbstick Dpad Left", user_path, user_path + "/input/thumbstick/dpad_left", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Thumbstick Dpad Right", user_path, user_path + "/input/thumbstick/dpad_right", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	register_io_path("/interaction_profiles/valve/index_controller", "Trackpad", "/user/hand/left", "/user/hand/left/input/trackpad", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	register_io_path("/interaction_profiles/valve/index_controller", "Trackpad force", "/user/hand/left", "/user/hand/left/input/trackpad/force", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	register_io_path("/interaction_profiles/valve/index_controller", "Trackpad touch", "/user/hand/left", "/user/hand/left/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	register_io_path("/interaction_profiles/valve/index_controller", "Trackpad", "/user/hand/right", "/user/hand/right/input/trackpad", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	register_io_path("/interaction_profiles/valve/index_controller", "Trackpad force", "/user/hand/right", "/user/hand/right/input/trackpad/force", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	register_io_path("/interaction_profiles/valve/index_controller", "Trackpad touch", "/user/hand/right", "/user/hand/right/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Trackpad", user_path, user_path + "/input/trackpad", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
+			register_io_path(profile_path, "Trackpad force", user_path, user_path + "/input/trackpad/force", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			register_io_path(profile_path, "Trackpad touch", user_path, user_path + "/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Trackpad Dpad Up", user_path, user_path + "/input/trackpad/dpad_up", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Trackpad Dpad Down", user_path, user_path + "/input/trackpad/dpad_down", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Trackpad Dpad Left", user_path, user_path + "/input/trackpad/dpad_left", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Trackpad Dpad Right", user_path, user_path + "/input/trackpad/dpad_right", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			register_io_path(profile_path, "Trackpad Dpad Center", user_path, user_path + "/input/trackpad/dpad_center", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	register_io_path("/interaction_profiles/valve/index_controller", "Haptic output", "/user/hand/left", "/user/hand/left/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	register_io_path("/interaction_profiles/valve/index_controller", "Haptic output", "/user/hand/right", "/user/hand/right/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+			register_io_path(profile_path, "Haptic output", user_path, user_path + "/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+		}
+	}
 }
diff --git a/modules/openxr/config.py b/modules/openxr/config.py
index 559aa2acf6..ca9e5ff95c 100644
--- a/modules/openxr/config.py
+++ b/modules/openxr/config.py
@@ -27,6 +27,16 @@ def get_doc_classes():
         "OpenXRCompositionLayerQuad",
         "OpenXRCompositionLayerCylinder",
         "OpenXRCompositionLayerEquirect",
+        "OpenXRBindingModifier",
+        "OpenXRIPBindingModifier",
+        "OpenXRActionBindingModifier",
+        "OpenXRAnalogThresholdModifier",
+        "OpenXRDpadBindingModifier",
+        "OpenXRInteractionProfileEditorBase",
+        "OpenXRInteractionProfileEditor",
+        "OpenXRBindingModifierEditor",
+        "OpenXRHapticBase",
+        "OpenXRHapticVibration",
     ]
 
 
diff --git a/modules/openxr/doc_classes/OpenXRActionBindingModifier.xml b/modules/openxr/doc_classes/OpenXRActionBindingModifier.xml
new file mode 100644
index 0000000000..817c6a93b9
--- /dev/null
+++ b/modules/openxr/doc_classes/OpenXRActionBindingModifier.xml
@@ -0,0 +1,11 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<class name="OpenXRActionBindingModifier" inherits="OpenXRBindingModifier" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="../../../doc/class.xsd">
+	<brief_description>
+		Binding modifier that applies on individual actions related to an interaction profile.
+	</brief_description>
+	<description>
+		Binding modifier that applies on individual actions related to an interaction profile.
+	</description>
+	<tutorials>
+	</tutorials>
+</class>
diff --git a/modules/openxr/doc_classes/OpenXRAnalogThresholdModifier.xml b/modules/openxr/doc_classes/OpenXRAnalogThresholdModifier.xml
new file mode 100644
index 0000000000..8f19c640da
--- /dev/null
+++ b/modules/openxr/doc_classes/OpenXRAnalogThresholdModifier.xml
@@ -0,0 +1,26 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<class name="OpenXRAnalogThresholdModifier" inherits="OpenXRActionBindingModifier" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="../../../doc/class.xsd">
+	<brief_description>
+		The analog threshold binding modifier can modify a float input to a boolean input with specified thresholds.
+	</brief_description>
+	<description>
+		The analog threshold binding modifier can modify a float input to a boolean input with specified thresholds.
+		See [url=https://registry.khronos.org/OpenXR/specs/1.1/html/xrspec.html#XR_VALVE_analog_threshold]XR_VALVE_analog_threshold[/url] for in-depth details.
+	</description>
+	<tutorials>
+	</tutorials>
+	<members>
+		<member name="off_haptic" type="OpenXRHapticBase" setter="set_off_haptic" getter="get_off_haptic">
+			Haptic pulse to emit when the user releases the input.
+		</member>
+		<member name="off_threshold" type="float" setter="set_off_threshold" getter="get_off_threshold" default="0.4">
+			When our input value falls below this, our output becomes false.
+		</member>
+		<member name="on_haptic" type="OpenXRHapticBase" setter="set_on_haptic" getter="get_on_haptic">
+			Haptic pulse to emit when the user presses the input.
+		</member>
+		<member name="on_threshold" type="float" setter="set_on_threshold" getter="get_on_threshold" default="0.6">
+			When our input value is equal or larger than this value, our output becomes true. It stays true until it falls under the [member off_threshold] value.
+		</member>
+	</members>
+</class>
diff --git a/modules/openxr/doc_classes/OpenXRBindingModifier.xml b/modules/openxr/doc_classes/OpenXRBindingModifier.xml
new file mode 100644
index 0000000000..2553f8d5ba
--- /dev/null
+++ b/modules/openxr/doc_classes/OpenXRBindingModifier.xml
@@ -0,0 +1,26 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<class name="OpenXRBindingModifier" inherits="Resource" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="../../../doc/class.xsd">
+	<brief_description>
+		Binding modifier base class.
+	</brief_description>
+	<description>
+		Binding modifier base class. Subclasses implement various modifiers that alter how an OpenXR runtime processes inputs.
+	</description>
+	<tutorials>
+	</tutorials>
+	<methods>
+		<method name="_get_description" qualifiers="virtual const">
+			<return type="String" />
+			<description>
+				Return the description of this class that is used for the title bar of the binding modifier editor.
+			</description>
+		</method>
+		<method name="_get_ip_modification" qualifiers="virtual">
+			<return type="PackedByteArray" />
+			<description>
+				Returns the data that is sent to OpenXR when submitting the suggested interacting bindings this modifier is a part of.
+				[b]Note:[/b] This must be data compatible with a [code]XrBindingModificationBaseHeaderKHR[/code] structure.
+			</description>
+		</method>
+	</methods>
+</class>
diff --git a/modules/openxr/doc_classes/OpenXRBindingModifierEditor.xml b/modules/openxr/doc_classes/OpenXRBindingModifierEditor.xml
new file mode 100644
index 0000000000..4997e3f71e
--- /dev/null
+++ b/modules/openxr/doc_classes/OpenXRBindingModifierEditor.xml
@@ -0,0 +1,38 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<class name="OpenXRBindingModifierEditor" inherits="PanelContainer" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="../../../doc/class.xsd">
+	<brief_description>
+		Binding modifier editor.
+	</brief_description>
+	<description>
+		This is the default binding modifier editor used in the OpenXR action map.
+	</description>
+	<tutorials>
+	</tutorials>
+	<methods>
+		<method name="get_binding_modifier" qualifiers="const">
+			<return type="OpenXRBindingModifier" />
+			<description>
+				Returns the [OpenXRBindingModifier] currently being edited.
+			</description>
+		</method>
+		<method name="setup">
+			<return type="void" />
+			<param index="0" name="action_map" type="OpenXRActionMap" />
+			<param index="1" name="binding_modifier" type="OpenXRBindingModifier" />
+			<description>
+				Setup this editor for the provided [param action_map] and [param binding_modifier].
+			</description>
+		</method>
+	</methods>
+	<members>
+		<member name="size_flags_horizontal" type="int" setter="set_h_size_flags" getter="get_h_size_flags" overrides="Control" enum="Control.SizeFlags" is_bitfield="true" default="3" />
+	</members>
+	<signals>
+		<signal name="binding_modifier_removed">
+			<param index="0" name="binding_modifier_editor" type="Object" />
+			<description>
+				Signal emitted when the user presses the delete binding modifier button for this modifier.
+			</description>
+		</signal>
+	</signals>
+</class>
diff --git a/modules/openxr/doc_classes/OpenXRDpadBindingModifier.xml b/modules/openxr/doc_classes/OpenXRDpadBindingModifier.xml
new file mode 100644
index 0000000000..a65bca7997
--- /dev/null
+++ b/modules/openxr/doc_classes/OpenXRDpadBindingModifier.xml
@@ -0,0 +1,43 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<class name="OpenXRDpadBindingModifier" inherits="OpenXRIPBindingModifier" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="../../../doc/class.xsd">
+	<brief_description>
+		The DPad binding modifier converts an axis input to a dpad output.
+	</brief_description>
+	<description>
+		The DPad binding modifier converts an axis input to a dpad output, emulating a DPad. New input paths for each dpad direction will be added to the interaction profile. When bound to actions the DPad emulation will be activated. You should [b]not[/b] combine dpad inputs with normal inputs in the same action set for the same control, this will result in an error being returned when suggested bindings are submitted to OpenXR.
+		See [url=https://registry.khronos.org/OpenXR/specs/1.1/html/xrspec.html#XR_EXT_dpad_binding]XR_EXT_dpad_binding[/url] for in-depth details.
+		[b]Note:[/b] If the DPad binding modifier extension is enabled, all dpad binding paths will be available in the action map. Adding the modifier to an interaction profile allows you to further customize the behavior.
+	</description>
+	<tutorials>
+	</tutorials>
+	<members>
+		<member name="action_set" type="OpenXRActionSet" setter="set_action_set" getter="get_action_set">
+			Action set for which this dpad binding modifier is active.
+		</member>
+		<member name="center_region" type="float" setter="set_center_region" getter="get_center_region" default="0.1">
+			Center region in which our center position of our dpad return [code]true[/code].
+		</member>
+		<member name="input_path" type="String" setter="set_input_path" getter="get_input_path" default="&quot;&quot;">
+			Input path for this dpad binding modifier.
+		</member>
+		<member name="is_sticky" type="bool" setter="set_is_sticky" getter="get_is_sticky" default="false">
+			If [code]false[/code], when the joystick enters a new dpad zone this becomes true.
+			If [code]true[/code], when the joystick remains in active dpad zone, this remains true even if we overlap with another zone.
+		</member>
+		<member name="off_haptic" type="OpenXRHapticBase" setter="set_off_haptic" getter="get_off_haptic">
+			Haptic pulse to emit when the user releases the input.
+		</member>
+		<member name="on_haptic" type="OpenXRHapticBase" setter="set_on_haptic" getter="get_on_haptic">
+			Haptic pulse to emit when the user presses the input.
+		</member>
+		<member name="threshold" type="float" setter="set_threshold" getter="get_threshold" default="0.6">
+			When our input value is equal or larger than this value, our dpad in that direction becomes true. It stays true until it falls under the [member threshold_released] value.
+		</member>
+		<member name="threshold_released" type="float" setter="set_threshold_released" getter="get_threshold_released" default="0.4">
+			When our input value falls below this, our output becomes false.
+		</member>
+		<member name="wedge_angle" type="float" setter="set_wedge_angle" getter="get_wedge_angle" default="1.5708">
+			The angle of each wedge that identifies the 4 directions of the emulated dpad.
+		</member>
+	</members>
+</class>
diff --git a/modules/openxr/doc_classes/OpenXRHapticBase.xml b/modules/openxr/doc_classes/OpenXRHapticBase.xml
new file mode 100644
index 0000000000..c23d86587c
--- /dev/null
+++ b/modules/openxr/doc_classes/OpenXRHapticBase.xml
@@ -0,0 +1,11 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<class name="OpenXRHapticBase" inherits="Resource" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="../../../doc/class.xsd">
+	<brief_description>
+		OpenXR Haptic feedback base class.
+	</brief_description>
+	<description>
+		This is a base class for haptic feedback resources.
+	</description>
+	<tutorials>
+	</tutorials>
+</class>
diff --git a/modules/openxr/doc_classes/OpenXRHapticVibration.xml b/modules/openxr/doc_classes/OpenXRHapticVibration.xml
new file mode 100644
index 0000000000..f147abb102
--- /dev/null
+++ b/modules/openxr/doc_classes/OpenXRHapticVibration.xml
@@ -0,0 +1,22 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<class name="OpenXRHapticVibration" inherits="OpenXRHapticBase" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="../../../doc/class.xsd">
+	<brief_description>
+		Vibration haptic feedback.
+	</brief_description>
+	<description>
+		This haptic feedback resource makes it possible to define a vibration based haptic feedback pulse that can be triggered through actions in the OpenXR action map.
+	</description>
+	<tutorials>
+	</tutorials>
+	<members>
+		<member name="amplitude" type="float" setter="set_amplitude" getter="get_amplitude" default="1.0">
+			The amplitude of the pulse between [code]0.0[/code] and [code]1.0[/code].
+		</member>
+		<member name="duration" type="int" setter="set_duration" getter="get_duration" default="-1">
+			The duration of the pulse in nanoseconds. Use [code]-1[/code] for a minimum duration pulse for the current XR runtime.
+		</member>
+		<member name="frequency" type="float" setter="set_frequency" getter="get_frequency" default="0.0">
+			The frequency of the pulse in Hz. [code]0.0[/code] will let the XR runtime chose an optimal frequency for the device used.
+		</member>
+	</members>
+</class>
diff --git a/modules/openxr/doc_classes/OpenXRIPBinding.xml b/modules/openxr/doc_classes/OpenXRIPBinding.xml
index ddd6fbe268..5b027acae4 100644
--- a/modules/openxr/doc_classes/OpenXRIPBinding.xml
+++ b/modules/openxr/doc_classes/OpenXRIPBinding.xml
@@ -16,6 +16,19 @@
 				Add an input/output path to this binding.
 			</description>
 		</method>
+		<method name="get_binding_modifier" qualifiers="const">
+			<return type="OpenXRActionBindingModifier" />
+			<param index="0" name="index" type="int" />
+			<description>
+				Get the [OpenXRBindingModifier] at this index.
+			</description>
+		</method>
+		<method name="get_binding_modifier_count" qualifiers="const">
+			<return type="int" />
+			<description>
+				Get the number of binding modifiers for this binding.
+			</description>
+		</method>
 		<method name="get_path_count" qualifiers="const" deprecated="Binding is for a single path.">
 			<return type="int" />
 			<description>
@@ -41,6 +54,9 @@
 		<member name="action" type="OpenXRAction" setter="set_action" getter="get_action">
 			[OpenXRAction] that is bound to [member binding_path].
 		</member>
+		<member name="binding_modifiers" type="Array" setter="set_binding_modifiers" getter="get_binding_modifiers" default="[]">
+			Binding modifiers for this binding.
+		</member>
 		<member name="binding_path" type="String" setter="set_binding_path" getter="get_binding_path" default="&quot;&quot;">
 			Binding path that defines the input or output bound to [member action].
 			[b]Note:[/b] Binding paths are suggestions, an XR runtime may choose to bind the action to a different input or output emulating this input or output.
diff --git a/modules/openxr/doc_classes/OpenXRIPBindingModifier.xml b/modules/openxr/doc_classes/OpenXRIPBindingModifier.xml
new file mode 100644
index 0000000000..8ec5f0a4e4
--- /dev/null
+++ b/modules/openxr/doc_classes/OpenXRIPBindingModifier.xml
@@ -0,0 +1,11 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<class name="OpenXRIPBindingModifier" inherits="OpenXRBindingModifier" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="../../../doc/class.xsd">
+	<brief_description>
+		Binding modifier that applies directly on an interaction profile.
+	</brief_description>
+	<description>
+		Binding modifier that applies directly on an interaction profile.
+	</description>
+	<tutorials>
+	</tutorials>
+</class>
diff --git a/modules/openxr/doc_classes/OpenXRInteractionProfile.xml b/modules/openxr/doc_classes/OpenXRInteractionProfile.xml
index ed5113f83c..d786739bda 100644
--- a/modules/openxr/doc_classes/OpenXRInteractionProfile.xml
+++ b/modules/openxr/doc_classes/OpenXRInteractionProfile.xml
@@ -23,8 +23,24 @@
 				Get the number of bindings in this interaction profile.
 			</description>
 		</method>
+		<method name="get_binding_modifier" qualifiers="const">
+			<return type="OpenXRIPBindingModifier" />
+			<param index="0" name="index" type="int" />
+			<description>
+				Get the [OpenXRBindingModifier] at this index.
+			</description>
+		</method>
+		<method name="get_binding_modifier_count" qualifiers="const">
+			<return type="int" />
+			<description>
+				Get the number of binding modifiers in this interaction profile.
+			</description>
+		</method>
 	</methods>
 	<members>
+		<member name="binding_modifiers" type="Array" setter="set_binding_modifiers" getter="get_binding_modifiers" default="[]">
+			Binding modifiers for this interaction profile.
+		</member>
 		<member name="bindings" type="Array" setter="set_bindings" getter="get_bindings" default="[]">
 			Action bindings for this interaction profile.
 		</member>
diff --git a/modules/openxr/doc_classes/OpenXRInteractionProfileEditor.xml b/modules/openxr/doc_classes/OpenXRInteractionProfileEditor.xml
new file mode 100644
index 0000000000..0155d74bc8
--- /dev/null
+++ b/modules/openxr/doc_classes/OpenXRInteractionProfileEditor.xml
@@ -0,0 +1,11 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<class name="OpenXRInteractionProfileEditor" inherits="OpenXRInteractionProfileEditorBase" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="../../../doc/class.xsd">
+	<brief_description>
+		Default OpenXR interaction profile editor.
+	</brief_description>
+	<description>
+		This is the default OpenXR interaction profile editor that provides a generic interface for editing any interaction profile for which no custom editor has been defined.
+	</description>
+	<tutorials>
+	</tutorials>
+</class>
diff --git a/modules/openxr/doc_classes/OpenXRInteractionProfileEditorBase.xml b/modules/openxr/doc_classes/OpenXRInteractionProfileEditorBase.xml
new file mode 100644
index 0000000000..49298c68b8
--- /dev/null
+++ b/modules/openxr/doc_classes/OpenXRInteractionProfileEditorBase.xml
@@ -0,0 +1,25 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<class name="OpenXRInteractionProfileEditorBase" inherits="HBoxContainer" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="../../../doc/class.xsd">
+	<brief_description>
+		Base class for editing interaction profiles.
+	</brief_description>
+	<description>
+		This is a base class for interaction profile editors used by the OpenXR action map editor. It can be used to create bespoke editors for specific interaction profiles.
+	</description>
+	<tutorials>
+	</tutorials>
+	<methods>
+		<method name="setup">
+			<return type="void" />
+			<param index="0" name="action_map" type="OpenXRActionMap" />
+			<param index="1" name="interaction_profile" type="OpenXRInteractionProfile" />
+			<description>
+				Setup this editor for the provided [param action_map] and [param interaction_profile].
+			</description>
+		</method>
+	</methods>
+	<members>
+		<member name="size_flags_horizontal" type="int" setter="set_h_size_flags" getter="get_h_size_flags" overrides="Control" enum="Control.SizeFlags" is_bitfield="true" default="3" />
+		<member name="size_flags_vertical" type="int" setter="set_v_size_flags" getter="get_v_size_flags" overrides="Control" enum="Control.SizeFlags" is_bitfield="true" default="3" />
+	</members>
+</class>
diff --git a/modules/openxr/editor/SCsub b/modules/openxr/editor/SCsub
index 39eb469978..d659be1d99 100644
--- a/modules/openxr/editor/SCsub
+++ b/modules/openxr/editor/SCsub
@@ -2,5 +2,10 @@
 from misc.utility.scons_hints import *
 
 Import("env")
+Import("env_openxr")
 
-env.add_source_files(env.modules_sources, "*.cpp")
+module_obj = []
+
+env_openxr.add_source_files(module_obj, "*.cpp")
+
+env.modules_sources += module_obj
diff --git a/modules/openxr/editor/openxr_action_map_editor.cpp b/modules/openxr/editor/openxr_action_map_editor.cpp
index a353073f21..23d793aefe 100644
--- a/modules/openxr/editor/openxr_action_map_editor.cpp
+++ b/modules/openxr/editor/openxr_action_map_editor.cpp
@@ -37,7 +37,8 @@
 #include "editor/gui/editor_file_dialog.h"
 #include "editor/themes/editor_scale.h"
 
-// TODO implement redo/undo system
+HashMap<String, String> OpenXRActionMapEditor::interaction_profile_editors;
+HashMap<String, String> OpenXRActionMapEditor::binding_modifier_editors;
 
 void OpenXRActionMapEditor::_bind_methods() {
 	ClassDB::bind_method("_add_action_set_editor", &OpenXRActionMapEditor::_add_action_set_editor);
@@ -50,6 +51,9 @@ void OpenXRActionMapEditor::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("_do_remove_action_set_editor", "action_set_editor"), &OpenXRActionMapEditor::_do_remove_action_set_editor);
 	ClassDB::bind_method(D_METHOD("_do_add_interaction_profile_editor", "interaction_profile_editor"), &OpenXRActionMapEditor::_do_add_interaction_profile_editor);
 	ClassDB::bind_method(D_METHOD("_do_remove_interaction_profile_editor", "interaction_profile_editor"), &OpenXRActionMapEditor::_do_remove_interaction_profile_editor);
+
+	ClassDB::bind_static_method("OpenXRActionMapEditor", D_METHOD("register_interaction_profile_editor", "interaction_profile_path", "editor_class"), &OpenXRActionMapEditor::register_interaction_profile_editor);
+	ClassDB::bind_static_method("OpenXRActionMapEditor", D_METHOD("register_binding_modifier_editor", "binding_modifier_class", "editor_class"), &OpenXRActionMapEditor::register_binding_modifier_editor);
 }
 
 void OpenXRActionMapEditor::_notification(int p_what) {
@@ -100,19 +104,32 @@ OpenXRInteractionProfileEditorBase *OpenXRActionMapEditor::_add_interaction_prof
 
 	// need to instance the correct editor for our profile
 	OpenXRInteractionProfileEditorBase *new_profile_editor = nullptr;
-	if (profile_path == "placeholder_text") {
-		// instance specific editor for this type
-	} else {
+	if (interaction_profile_editors.has(profile_path)) {
+		Object *new_editor = ClassDB::instantiate(interaction_profile_editors[profile_path]);
+		if (new_editor) {
+			new_profile_editor = Object::cast_to<OpenXRInteractionProfileEditorBase>(new_editor);
+			if (!new_profile_editor) {
+				WARN_PRINT("Interaction profile editor type mismatch for " + profile_path);
+				memfree(new_editor);
+			}
+		}
+	}
+	if (!new_profile_editor) {
 		// instance generic editor
-		new_profile_editor = memnew(OpenXRInteractionProfileEditor(action_map, p_interaction_profile));
+		new_profile_editor = memnew(OpenXRInteractionProfileEditor);
 	}
 
 	// now add it in..
 	ERR_FAIL_NULL_V(new_profile_editor, nullptr);
+	new_profile_editor->setup(action_map, p_interaction_profile);
 	tabs->add_child(new_profile_editor);
 	new_profile_editor->add_theme_style_override(SceneStringName(panel), get_theme_stylebox(SceneStringName(panel), SNAME("Tree")));
 	tabs->set_tab_button_icon(tabs->get_tab_count() - 1, get_theme_icon(SNAME("close"), SNAME("TabBar")));
 
+	if (!new_profile_editor->tooltip.is_empty()) {
+		tabs->set_tab_tooltip(tabs->get_tab_count() - 1, new_profile_editor->tooltip);
+	}
+
 	return new_profile_editor;
 }
 
@@ -195,8 +212,19 @@ void OpenXRActionMapEditor::_on_remove_action_set(Object *p_action_set_editor) {
 	Ref<OpenXRActionSet> action_set = action_set_editor->get_action_set();
 	ERR_FAIL_COND(action_set.is_null());
 
+	// Remove all actions first.
 	action_set_editor->remove_all_actions();
 
+	// Make sure we update our interaction profiles.
+	for (int i = 0; i < tabs->get_tab_count(); i++) {
+		// First tab won't be an interaction profile editor, but being thorough..
+		OpenXRInteractionProfileEditorBase *interaction_profile_editor = Object::cast_to<OpenXRInteractionProfileEditorBase>(tabs->get_tab_control(i));
+		if (interaction_profile_editor) {
+			interaction_profile_editor->remove_all_for_action_set(action_set);
+		}
+	}
+
+	// And now we can remove our action set.
 	undo_redo->create_action(TTR("Remove action set"));
 	undo_redo->add_do_method(this, "_do_remove_action_set_editor", action_set_editor);
 	undo_redo->add_undo_method(this, "_do_add_action_set_editor", action_set_editor);
@@ -210,7 +238,7 @@ void OpenXRActionMapEditor::_on_action_removed(Ref<OpenXRAction> p_action) {
 		// First tab won't be an interaction profile editor, but being thorough..
 		OpenXRInteractionProfileEditorBase *interaction_profile_editor = Object::cast_to<OpenXRInteractionProfileEditorBase>(tabs->get_tab_control(i));
 		if (interaction_profile_editor) {
-			interaction_profile_editor->remove_all_bindings_for_action(p_action);
+			interaction_profile_editor->remove_all_for_action(p_action);
 		}
 	}
 }
@@ -387,6 +415,22 @@ void OpenXRActionMapEditor::_clear_action_map() {
 	}
 }
 
+void OpenXRActionMapEditor::register_interaction_profile_editor(const String &p_for_path, const String &p_editor_class) {
+	interaction_profile_editors[p_for_path] = p_editor_class;
+}
+
+void OpenXRActionMapEditor::register_binding_modifier_editor(const String &p_binding_modifier_class, const String &p_editor_class) {
+	binding_modifier_editors[p_binding_modifier_class] = p_editor_class;
+}
+
+String OpenXRActionMapEditor::get_binding_modifier_editor_class(const String &p_binding_modifier_class) {
+	if (binding_modifier_editors.has(p_binding_modifier_class)) {
+		return binding_modifier_editors[p_binding_modifier_class];
+	}
+
+	return OpenXRBindingModifierEditor::get_class_static();
+}
+
 OpenXRActionMapEditor::OpenXRActionMapEditor() {
 	undo_redo = EditorUndoRedoManager::get_singleton();
 	set_custom_minimum_size(Size2(0.0, 300.0));
diff --git a/modules/openxr/editor/openxr_action_map_editor.h b/modules/openxr/editor/openxr_action_map_editor.h
index cfe5fed095..088bf0c613 100644
--- a/modules/openxr/editor/openxr_action_map_editor.h
+++ b/modules/openxr/editor/openxr_action_map_editor.h
@@ -36,6 +36,7 @@
 #include "openxr_interaction_profile_editor.h"
 #include "openxr_select_interaction_profile_dialog.h"
 
+#include "core/templates/hash_map.h"
 #include "editor/editor_undo_redo_manager.h"
 #include "editor/plugins/editor_plugin.h"
 #include "scene/gui/box_container.h"
@@ -48,6 +49,9 @@ class OpenXRActionMapEditor : public VBoxContainer {
 	GDCLASS(OpenXRActionMapEditor, VBoxContainer);
 
 private:
+	static HashMap<String, String> interaction_profile_editors; // interaction profile path, interaction profile editor
+	static HashMap<String, String> binding_modifier_editors; // binding modifier class, binding modifiers editor
+
 	EditorUndoRedoManager *undo_redo;
 	String edited_path;
 	Ref<OpenXRActionMap> action_map;
@@ -100,6 +104,10 @@ protected:
 	void _do_remove_interaction_profile_editor(OpenXRInteractionProfileEditorBase *p_interaction_profile_editor);
 
 public:
+	static void register_interaction_profile_editor(const String &p_for_path, const String &p_editor_class);
+	static void register_binding_modifier_editor(const String &p_binding_modifier_class, const String &p_editor_class);
+	static String get_binding_modifier_editor_class(const String &p_binding_modifier_class);
+
 	void open_action_map(String p_path);
 
 	OpenXRActionMapEditor();
diff --git a/modules/openxr/editor/openxr_binding_modifier_editor.cpp b/modules/openxr/editor/openxr_binding_modifier_editor.cpp
new file mode 100644
index 0000000000..b8770852af
--- /dev/null
+++ b/modules/openxr/editor/openxr_binding_modifier_editor.cpp
@@ -0,0 +1,289 @@
+/**************************************************************************/
+/*  openxr_binding_modifier_editor.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 "openxr_binding_modifier_editor.h"
+
+#include "editor/editor_string_names.h"
+
+///////////////////////////////////////////////////////////////////////////////////////////////////
+// EditorPropertyActionSet
+
+void EditorPropertyActionSet::_set_read_only(bool p_read_only) {
+	options->set_disabled(p_read_only);
+}
+
+void EditorPropertyActionSet::_option_selected(int p_which) {
+	Ref<OpenXRActionSet> val = options->get_item_metadata(p_which);
+	emit_changed(get_edited_property(), val);
+}
+
+void EditorPropertyActionSet::update_property() {
+	Variant current = get_edited_property_value();
+	if (current.get_type() == Variant::NIL) {
+		options->select(-1);
+		return;
+	}
+
+	Ref<OpenXRActionSet> which = current;
+	for (int i = 0; i < options->get_item_count(); i++) {
+		if (which == (Ref<OpenXRActionSet>)options->get_item_metadata(i)) {
+			options->select(i);
+			return;
+		}
+	}
+
+	// Couldn't find it? deselect..
+	options->select(-1);
+}
+
+void EditorPropertyActionSet::setup(const Ref<OpenXRActionMap> &p_action_map) {
+	options->clear();
+
+	Array action_sets = p_action_map->get_action_sets();
+	for (Ref<OpenXRActionSet> action_set : action_sets) {
+		options->add_item(action_set->get_localized_name());
+		options->set_item_metadata(-1, action_set);
+	}
+}
+
+void EditorPropertyActionSet::set_option_button_clip(bool p_enable) {
+	options->set_clip_text(p_enable);
+}
+
+EditorPropertyActionSet::EditorPropertyActionSet() {
+	options = memnew(OptionButton);
+	options->set_clip_text(true);
+	options->set_flat(true);
+	options->set_auto_translate_mode(AUTO_TRANSLATE_MODE_DISABLED);
+	add_child(options);
+	add_focusable(options);
+	options->connect(SceneStringName(item_selected), callable_mp(this, &EditorPropertyActionSet::_option_selected));
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////
+// EditorPropertyBindingPath
+
+void EditorPropertyBindingPath::_set_read_only(bool p_read_only) {
+	options->set_disabled(p_read_only);
+}
+
+void EditorPropertyBindingPath::_option_selected(int p_which) {
+	String val = options->get_item_metadata(p_which);
+	emit_changed(get_edited_property(), val);
+}
+
+void EditorPropertyBindingPath::update_property() {
+	Variant current = get_edited_property_value();
+	if (current.get_type() == Variant::NIL) {
+		options->select(-1);
+		return;
+	}
+
+	String which = current;
+	for (int i = 0; i < options->get_item_count(); i++) {
+		if (which == (String)options->get_item_metadata(i)) {
+			options->select(i);
+			return;
+		}
+	}
+
+	// Couldn't find it? deselect..
+	options->select(-1);
+}
+
+void EditorPropertyBindingPath::setup(const String &p_interaction_profile_path, Vector<OpenXRAction::ActionType> p_include_action_types) {
+	options->clear();
+
+	const OpenXRInteractionProfileMetadata::InteractionProfile *profile_def = OpenXRInteractionProfileMetadata::get_singleton()->get_profile(p_interaction_profile_path);
+
+	// Determine toplevel paths
+	Vector<String> top_level_paths;
+	for (const OpenXRInteractionProfileMetadata::IOPath &io_path : profile_def->io_paths) {
+		if (!top_level_paths.has(io_path.top_level_path)) {
+			top_level_paths.push_back(io_path.top_level_path);
+		}
+	}
+
+	for (const String &top_level_path : top_level_paths) {
+		String top_level_name = OpenXRInteractionProfileMetadata::get_singleton()->get_top_level_name(top_level_path);
+
+		for (const OpenXRInteractionProfileMetadata::IOPath &io_path : profile_def->io_paths) {
+			if (io_path.top_level_path == top_level_path && p_include_action_types.has(io_path.action_type)) {
+				options->add_item(top_level_name + "/" + io_path.display_name);
+				options->set_item_metadata(-1, io_path.openxr_path);
+			}
+		}
+	}
+}
+
+void EditorPropertyBindingPath::set_option_button_clip(bool p_enable) {
+	options->set_clip_text(p_enable);
+}
+
+EditorPropertyBindingPath::EditorPropertyBindingPath() {
+	options = memnew(OptionButton);
+	options->set_clip_text(true);
+	options->set_flat(true);
+	options->set_auto_translate_mode(AUTO_TRANSLATE_MODE_DISABLED);
+	add_child(options);
+	add_focusable(options);
+	options->connect(SceneStringName(item_selected), callable_mp(this, &EditorPropertyBindingPath::_option_selected));
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////
+// OpenXRBindingModifierEditor
+
+bool EditorInspectorPluginBindingModifier::can_handle(Object *p_object) {
+	Ref<OpenXRBindingModifier> binding_modifier(Object::cast_to<OpenXRBindingModifier>(p_object));
+	return binding_modifier.is_valid();
+}
+
+bool EditorInspectorPluginBindingModifier::parse_property(Object *p_object, const Variant::Type p_type, const String &p_path, const PropertyHint p_hint, const String &p_hint_text, const BitField<PropertyUsageFlags> p_usage, const bool p_wide) {
+	Ref<OpenXRActionBindingModifier> action_binding_modifier(Object::cast_to<OpenXRActionBindingModifier>(p_object));
+	if (action_binding_modifier.is_valid()) {
+		if (p_type == Variant::OBJECT && p_hint == PROPERTY_HINT_RESOURCE_TYPE && p_hint_text == OpenXRActionSet::get_class_static()) {
+			OpenXRIPBinding *ip_binding = action_binding_modifier->get_ip_binding();
+			ERR_FAIL_NULL_V(ip_binding, false);
+
+			OpenXRActionMap *action_map = ip_binding->get_action_map();
+			ERR_FAIL_NULL_V(action_map, false);
+
+			EditorPropertyActionSet *action_set_property = memnew(EditorPropertyActionSet);
+			action_set_property->setup(action_map);
+			add_property_editor(p_path, action_set_property);
+			return true;
+		}
+
+		return false;
+	}
+
+	Ref<OpenXRIPBindingModifier> ip_binding_modifier(Object::cast_to<OpenXRIPBindingModifier>(p_object));
+	if (ip_binding_modifier.is_valid()) {
+		if (p_type == Variant::OBJECT && p_hint == PROPERTY_HINT_RESOURCE_TYPE && p_hint_text == OpenXRActionSet::get_class_static()) {
+			OpenXRInteractionProfile *interaction_profile = ip_binding_modifier->get_interaction_profile();
+			ERR_FAIL_NULL_V(interaction_profile, false);
+
+			OpenXRActionMap *action_map = interaction_profile->get_action_map();
+			ERR_FAIL_NULL_V(action_map, false);
+
+			EditorPropertyActionSet *action_set_property = memnew(EditorPropertyActionSet);
+			action_set_property->setup(action_map);
+			add_property_editor(p_path, action_set_property);
+			return true;
+		}
+
+		if (p_type == Variant::STRING && p_hint == PROPERTY_HINT_TYPE_STRING && p_hint_text == "binding_path") {
+			EditorPropertyBindingPath *binding_path_property = memnew(EditorPropertyBindingPath);
+
+			OpenXRInteractionProfile *interaction_profile = ip_binding_modifier->get_interaction_profile();
+			ERR_FAIL_NULL_V(interaction_profile, false);
+
+			Vector<OpenXRAction::ActionType> action_types;
+			action_types.push_back(OpenXRAction::OPENXR_ACTION_VECTOR2);
+			binding_path_property->setup(interaction_profile->get_interaction_profile_path(), action_types);
+
+			add_property_editor(p_path, binding_path_property);
+			return true;
+		}
+
+		return false;
+	}
+
+	return false;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////
+// OpenXRBindingModifierEditor
+
+void OpenXRBindingModifierEditor::_bind_methods() {
+	ClassDB::bind_method(D_METHOD("get_binding_modifier"), &OpenXRBindingModifierEditor::get_binding_modifier);
+	ClassDB::bind_method(D_METHOD("setup", "action_map", "binding_modifier"), &OpenXRBindingModifierEditor::setup);
+
+	ADD_SIGNAL(MethodInfo("binding_modifier_removed", PropertyInfo(Variant::OBJECT, "binding_modifier_editor")));
+}
+
+void OpenXRBindingModifierEditor::_notification(int p_what) {
+	switch (p_what) {
+		case NOTIFICATION_ENTER_TREE:
+		case NOTIFICATION_THEME_CHANGED: {
+			rem_binding_modifier_btn->set_button_icon(get_theme_icon(SNAME("Remove"), EditorStringName(EditorIcons)));
+		} break;
+	}
+}
+
+void OpenXRBindingModifierEditor::_on_remove_binding_modifier() {
+	// Tell parent to remove us
+	emit_signal("binding_modifier_removed", this);
+}
+
+OpenXRBindingModifierEditor::OpenXRBindingModifierEditor() {
+	undo_redo = EditorUndoRedoManager::get_singleton();
+
+	set_h_size_flags(Control::SIZE_EXPAND_FILL);
+
+	main_vb = memnew(VBoxContainer);
+	main_vb->set_h_size_flags(Control::SIZE_EXPAND_FILL);
+	add_child(main_vb);
+
+	header_hb = memnew(HBoxContainer);
+	header_hb->set_h_size_flags(Control::SIZE_EXPAND_FILL);
+	main_vb->add_child(header_hb);
+
+	binding_modifier_title = memnew(Label);
+	binding_modifier_title->set_h_size_flags(Control::SIZE_EXPAND_FILL);
+	header_hb->add_child(binding_modifier_title);
+
+	rem_binding_modifier_btn = memnew(Button);
+	rem_binding_modifier_btn->set_tooltip_text(TTR("Remove binding modifier."));
+	rem_binding_modifier_btn->connect(SceneStringName(pressed), callable_mp(this, &OpenXRBindingModifierEditor::_on_remove_binding_modifier));
+	rem_binding_modifier_btn->set_flat(true);
+	header_hb->add_child(rem_binding_modifier_btn);
+
+	editor_inspector = memnew(EditorInspector);
+	editor_inspector->set_horizontal_scroll_mode(ScrollContainer::SCROLL_MODE_DISABLED);
+	editor_inspector->set_vertical_scroll_mode(ScrollContainer::SCROLL_MODE_DISABLED);
+	editor_inspector->set_h_size_flags(Control::SIZE_EXPAND_FILL);
+	main_vb->add_child(editor_inspector);
+}
+
+void OpenXRBindingModifierEditor::setup(Ref<OpenXRActionMap> p_action_map, Ref<OpenXRBindingModifier> p_binding_modifier) {
+	ERR_FAIL_NULL(binding_modifier_title);
+	ERR_FAIL_NULL(editor_inspector);
+
+	action_map = p_action_map;
+	binding_modifier = p_binding_modifier;
+
+	if (p_binding_modifier.is_valid()) {
+		binding_modifier_title->set_text(p_binding_modifier->get_description());
+
+		editor_inspector->set_object_class(p_binding_modifier->get_class());
+		editor_inspector->edit(p_binding_modifier.ptr());
+	}
+}
diff --git a/modules/openxr/editor/openxr_binding_modifier_editor.h b/modules/openxr/editor/openxr_binding_modifier_editor.h
new file mode 100644
index 0000000000..5392299699
--- /dev/null
+++ b/modules/openxr/editor/openxr_binding_modifier_editor.h
@@ -0,0 +1,113 @@
+/**************************************************************************/
+/*  openxr_binding_modifier_editor.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 OPENXR_BINDING_MODIFIER_EDITOR_H
+#define OPENXR_BINDING_MODIFIER_EDITOR_H
+
+#include "../action_map/openxr_action_map.h"
+#include "../action_map/openxr_action_set.h"
+#include "../action_map/openxr_binding_modifier.h"
+#include "editor/editor_inspector.h"
+#include "editor/editor_undo_redo_manager.h"
+#include "scene/gui/box_container.h"
+#include "scene/gui/button.h"
+#include "scene/gui/label.h"
+#include "scene/gui/panel_container.h"
+
+class EditorPropertyActionSet : public EditorProperty {
+	GDCLASS(EditorPropertyActionSet, EditorProperty);
+	OptionButton *options = nullptr;
+
+	void _option_selected(int p_which);
+
+protected:
+	virtual void _set_read_only(bool p_read_only) override;
+
+public:
+	void setup(const Ref<OpenXRActionMap> &p_action_map);
+	virtual void update_property() override;
+	void set_option_button_clip(bool p_enable);
+	EditorPropertyActionSet();
+};
+
+class EditorPropertyBindingPath : public EditorProperty {
+	GDCLASS(EditorPropertyBindingPath, EditorProperty);
+	OptionButton *options = nullptr;
+
+	void _option_selected(int p_which);
+
+protected:
+	virtual void _set_read_only(bool p_read_only) override;
+
+public:
+	void setup(const String &p_interaction_profile_path, Vector<OpenXRAction::ActionType> p_include_action_types);
+	virtual void update_property() override;
+	void set_option_button_clip(bool p_enable);
+	EditorPropertyBindingPath();
+};
+
+class EditorInspectorPluginBindingModifier : public EditorInspectorPlugin {
+	GDCLASS(EditorInspectorPluginBindingModifier, EditorInspectorPlugin);
+
+public:
+	virtual bool can_handle(Object *p_object) override;
+	virtual bool parse_property(Object *p_object, const Variant::Type p_type, const String &p_path, const PropertyHint p_hint, const String &p_hint_text, const BitField<PropertyUsageFlags> p_usage, const bool p_wide) override;
+};
+
+class OpenXRBindingModifierEditor : public PanelContainer {
+	GDCLASS(OpenXRBindingModifierEditor, PanelContainer);
+
+private:
+	HBoxContainer *header_hb = nullptr;
+	Label *binding_modifier_title = nullptr;
+	Button *rem_binding_modifier_btn = nullptr;
+	EditorInspector *editor_inspector = nullptr;
+
+protected:
+	VBoxContainer *main_vb = nullptr;
+
+	EditorUndoRedoManager *undo_redo;
+	Ref<OpenXRBindingModifier> binding_modifier;
+	Ref<OpenXRActionMap> action_map;
+
+	static void _bind_methods();
+	void _notification(int p_what);
+
+	void _on_remove_binding_modifier();
+
+public:
+	Ref<OpenXRBindingModifier> get_binding_modifier() const { return binding_modifier; }
+
+	virtual void setup(Ref<OpenXRActionMap> p_action_map, Ref<OpenXRBindingModifier> p_binding_modifier);
+
+	OpenXRBindingModifierEditor();
+};
+
+#endif // OPENXR_BINDING_MODIFIER_EDITOR_H
diff --git a/modules/openxr/editor/openxr_binding_modifiers_dialog.cpp b/modules/openxr/editor/openxr_binding_modifiers_dialog.cpp
new file mode 100644
index 0000000000..f12d465b46
--- /dev/null
+++ b/modules/openxr/editor/openxr_binding_modifiers_dialog.cpp
@@ -0,0 +1,258 @@
+/**************************************************************************/
+/*  openxr_binding_modifiers_dialog.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 "openxr_binding_modifiers_dialog.h"
+#include "../action_map/openxr_interaction_profile_metadata.h"
+#include "editor/editor_string_names.h"
+#include "openxr_action_map_editor.h"
+
+void OpenXRBindingModifiersDialog::_bind_methods() {
+	ClassDB::bind_method(D_METHOD("_do_add_binding_modifier_editor", "binding_modifier_editor"), &OpenXRBindingModifiersDialog::_do_add_binding_modifier_editor);
+	ClassDB::bind_method(D_METHOD("_do_remove_binding_modifier_editor", "binding_modifier_editor"), &OpenXRBindingModifiersDialog::_do_remove_binding_modifier_editor);
+}
+
+void OpenXRBindingModifiersDialog::_notification(int p_what) {
+	switch (p_what) {
+		case NOTIFICATION_READY: {
+			_create_binding_modifiers();
+		} break;
+
+		case NOTIFICATION_THEME_CHANGED: {
+			if (binding_modifier_sc) {
+				binding_modifier_sc->add_theme_style_override(SceneStringName(panel), get_theme_stylebox(SceneStringName(panel), SNAME("Tree")));
+			}
+		} break;
+	}
+}
+
+OpenXRBindingModifierEditor *OpenXRBindingModifiersDialog::_add_binding_modifier_editor(Ref<OpenXRBindingModifier> p_binding_modifier) {
+	ERR_FAIL_COND_V(p_binding_modifier.is_null(), nullptr);
+
+	String class_name = p_binding_modifier->get_class();
+	ERR_FAIL_COND_V(class_name.is_empty(), nullptr);
+	String editor_class = OpenXRActionMapEditor::get_binding_modifier_editor_class(class_name);
+	ERR_FAIL_COND_V(editor_class.is_empty(), nullptr);
+
+	OpenXRBindingModifierEditor *new_editor = nullptr;
+
+	Object *obj = ClassDB::instantiate(editor_class);
+	if (obj) {
+		new_editor = Object::cast_to<OpenXRBindingModifierEditor>(obj);
+		if (!new_editor) {
+			// Not of correct type?? Free it.
+			memfree(obj);
+		}
+	}
+	ERR_FAIL_NULL_V(new_editor, nullptr);
+
+	new_editor->setup(action_map, p_binding_modifier);
+	new_editor->connect("binding_modifier_removed", callable_mp(this, &OpenXRBindingModifiersDialog::_on_remove_binding_modifier));
+
+	binding_modifiers_vb->add_child(new_editor);
+	new_editor->add_theme_style_override(SceneStringName(panel), get_theme_stylebox(SceneStringName(panel), SNAME("Tree")));
+
+	return new_editor;
+}
+
+void OpenXRBindingModifiersDialog::_create_binding_modifiers() {
+	Array new_binding_modifiers;
+
+	if (ip_binding.is_valid()) {
+		new_binding_modifiers = ip_binding->get_binding_modifiers();
+	} else if (interaction_profile.is_valid()) {
+		new_binding_modifiers = interaction_profile->get_binding_modifiers();
+	} else {
+		ERR_FAIL_MSG("No binding nor interaction profile specified.");
+	}
+
+	for (int i = 0; i < new_binding_modifiers.size(); i++) {
+		Ref<OpenXRBindingModifier> binding_modifier = new_binding_modifiers[i];
+		_add_binding_modifier_editor(binding_modifier);
+	}
+}
+
+void OpenXRBindingModifiersDialog::_on_add_binding_modifier() {
+	create_dialog->popup_create(false);
+}
+
+void OpenXRBindingModifiersDialog::_on_remove_binding_modifier(Object *p_binding_modifier_editor) {
+	if (ip_binding.is_valid()) {
+		ip_binding->set_edited(true);
+	} else if (interaction_profile.is_valid()) {
+		interaction_profile->set_edited(true);
+	} else {
+		ERR_FAIL_MSG("No binding nor interaction profile specified.");
+	}
+
+	OpenXRBindingModifierEditor *binding_modifier_editor = Object::cast_to<OpenXRBindingModifierEditor>(p_binding_modifier_editor);
+	ERR_FAIL_NULL(binding_modifier_editor);
+	ERR_FAIL_COND(binding_modifier_editor->get_parent() != binding_modifiers_vb);
+
+	undo_redo->create_action(TTR("Remove binding modifier"));
+	undo_redo->add_do_method(this, "_do_remove_binding_modifier_editor", binding_modifier_editor);
+	undo_redo->add_undo_method(this, "_do_add_binding_modifier_editor", binding_modifier_editor);
+	undo_redo->commit_action(true);
+}
+
+void OpenXRBindingModifiersDialog::_on_dialog_created() {
+	// Instance new binding modifier object
+	Variant obj = create_dialog->instantiate_selected();
+	ERR_FAIL_COND(obj.get_type() != Variant::OBJECT);
+
+	Ref<OpenXRBindingModifier> new_binding_modifier = obj;
+	ERR_FAIL_COND(new_binding_modifier.is_null());
+
+	if (ip_binding.is_valid()) {
+		// Add it to our binding.
+		ip_binding->add_binding_modifier(new_binding_modifier);
+		ip_binding->set_edited(true);
+	} else if (interaction_profile.is_valid()) {
+		// Add it to our interaction profile.
+		interaction_profile->add_binding_modifier(new_binding_modifier);
+		interaction_profile->set_edited(true);
+	} else {
+		ERR_FAIL_MSG("No binding nor interaction profile specified.");
+	}
+
+	// Create our editor for this.
+	OpenXRBindingModifierEditor *binding_modifier_editor = _add_binding_modifier_editor(new_binding_modifier);
+	ERR_FAIL_NULL(binding_modifier_editor);
+
+	// Add undo/redo.
+	undo_redo->create_action(TTR("Add binding modifier"));
+	undo_redo->add_do_method(this, "_do_add_binding_modifier_editor", binding_modifier_editor);
+	undo_redo->add_undo_method(this, "_do_remove_binding_modifier_editor", binding_modifier_editor);
+	undo_redo->commit_action(false);
+}
+
+void OpenXRBindingModifiersDialog::_do_add_binding_modifier_editor(OpenXRBindingModifierEditor *p_binding_modifier_editor) {
+	Ref<OpenXRBindingModifier> binding_modifier = p_binding_modifier_editor->get_binding_modifier();
+	ERR_FAIL_COND(binding_modifier.is_null());
+
+	if (ip_binding.is_valid()) {
+		// Add it to our binding
+		ip_binding->add_binding_modifier(binding_modifier);
+	} else if (interaction_profile.is_valid()) {
+		// Add it to our interaction profile
+		interaction_profile->add_binding_modifier(binding_modifier);
+	} else {
+		ERR_FAIL_MSG("No binding nor interaction profile specified.");
+	}
+
+	binding_modifiers_vb->add_child(p_binding_modifier_editor);
+}
+
+void OpenXRBindingModifiersDialog::_do_remove_binding_modifier_editor(OpenXRBindingModifierEditor *p_binding_modifier_editor) {
+	Ref<OpenXRBindingModifier> binding_modifier = p_binding_modifier_editor->get_binding_modifier();
+	ERR_FAIL_COND(binding_modifier.is_null());
+
+	if (ip_binding.is_valid()) {
+		// Remove it from our binding.
+		ip_binding->remove_binding_modifier(binding_modifier);
+	} else if (interaction_profile.is_valid()) {
+		// Removed it to from interaction profile.
+		interaction_profile->remove_binding_modifier(binding_modifier);
+	} else {
+		ERR_FAIL_MSG("No binding nor interaction profile specified.");
+	}
+
+	binding_modifiers_vb->remove_child(p_binding_modifier_editor);
+}
+
+OpenXRBindingModifiersDialog::OpenXRBindingModifiersDialog() {
+	undo_redo = EditorUndoRedoManager::get_singleton();
+
+	set_transient(true);
+
+	binding_modifier_sc = memnew(ScrollContainer);
+	binding_modifier_sc->set_custom_minimum_size(Size2(350.0, 0.0));
+	binding_modifier_sc->set_h_size_flags(Control::SIZE_EXPAND_FILL);
+	binding_modifier_sc->set_v_size_flags(Control::SIZE_EXPAND_FILL);
+	binding_modifier_sc->set_horizontal_scroll_mode(ScrollContainer::SCROLL_MODE_DISABLED);
+	add_child(binding_modifier_sc);
+
+	binding_modifiers_vb = memnew(VBoxContainer);
+	binding_modifiers_vb->set_h_size_flags(Control::SIZE_EXPAND_FILL);
+	binding_modifier_sc->add_child(binding_modifiers_vb);
+
+	binding_warning_label = memnew(Label);
+	binding_warning_label->set_autowrap_mode(TextServer::AUTOWRAP_WORD);
+	binding_warning_label->set_text(TTR("Note: modifiers will only be applied if supported on the host system."));
+	binding_modifiers_vb->add_child(binding_warning_label);
+
+	add_binding_modifier_btn = memnew(Button);
+	add_binding_modifier_btn->set_text(TTR("Add binding modifier"));
+	add_binding_modifier_btn->connect("pressed", callable_mp(this, &OpenXRBindingModifiersDialog::_on_add_binding_modifier));
+	binding_modifiers_vb->add_child(add_binding_modifier_btn);
+
+	// TODO may need to create our own dialog for this that can filter on binding modifiers recorded on interaction profiles or on individual bindings.
+
+	create_dialog = memnew(CreateDialog);
+	create_dialog->set_transient(true);
+	create_dialog->connect("create", callable_mp(this, &OpenXRBindingModifiersDialog::_on_dialog_created));
+	add_child(create_dialog);
+}
+
+void OpenXRBindingModifiersDialog::setup(Ref<OpenXRActionMap> p_action_map, Ref<OpenXRInteractionProfile> p_interaction_profile, Ref<OpenXRIPBinding> p_ip_binding) {
+	OpenXRInteractionProfileMetadata *meta_data = OpenXRInteractionProfileMetadata::get_singleton();
+	action_map = p_action_map;
+	interaction_profile = p_interaction_profile;
+	ip_binding = p_ip_binding;
+
+	String profile_path = interaction_profile->get_interaction_profile_path();
+
+	if (ip_binding.is_valid()) {
+		String action_name = "unset";
+		String path_name = "unset";
+
+		Ref<OpenXRAction> action = p_ip_binding->get_action();
+		if (action.is_valid()) {
+			action_name = action->get_name_with_set();
+		}
+
+		const OpenXRInteractionProfileMetadata::IOPath *io_path = meta_data->get_io_path(profile_path, p_ip_binding->get_binding_path());
+		if (io_path != nullptr) {
+			path_name = io_path->display_name;
+		}
+
+		create_dialog->set_base_type("OpenXRActionBindingModifier");
+		set_title(TTR("Binding modifiers for:") + " " + action_name + ": " + path_name);
+	} else if (interaction_profile.is_valid()) {
+		String profile_name = profile_path;
+
+		const OpenXRInteractionProfileMetadata::InteractionProfile *profile_def = meta_data->get_profile(profile_path);
+		if (profile_def != nullptr) {
+			profile_name = profile_def->display_name;
+		}
+
+		create_dialog->set_base_type("OpenXRIPBindingModifier");
+		set_title(TTR("Binding modifiers for:") + " " + profile_name);
+	}
+}
diff --git a/modules/openxr/editor/openxr_binding_modifiers_dialog.h b/modules/openxr/editor/openxr_binding_modifiers_dialog.h
new file mode 100644
index 0000000000..c60a3760b3
--- /dev/null
+++ b/modules/openxr/editor/openxr_binding_modifiers_dialog.h
@@ -0,0 +1,81 @@
+/**************************************************************************/
+/*  openxr_binding_modifiers_dialog.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 OPENXR_BINDING_MODIFIERS_DIALOG_H
+#define OPENXR_BINDING_MODIFIERS_DIALOG_H
+
+#include "../action_map/openxr_action_map.h"
+#include "../action_map/openxr_interaction_profile.h"
+#include "../editor/openxr_binding_modifier_editor.h"
+#include "editor/create_dialog.h"
+#include "editor/editor_undo_redo_manager.h"
+#include "scene/gui/box_container.h"
+#include "scene/gui/button.h"
+#include "scene/gui/dialogs.h"
+#include "scene/gui/label.h"
+#include "scene/gui/scroll_container.h"
+
+class OpenXRBindingModifiersDialog : public AcceptDialog {
+	GDCLASS(OpenXRBindingModifiersDialog, AcceptDialog);
+
+private:
+	ScrollContainer *binding_modifier_sc = nullptr;
+	VBoxContainer *binding_modifiers_vb = nullptr;
+	Label *binding_warning_label = nullptr;
+	Button *add_binding_modifier_btn = nullptr;
+	CreateDialog *create_dialog = nullptr;
+
+	OpenXRBindingModifierEditor *_add_binding_modifier_editor(Ref<OpenXRBindingModifier> p_binding_modifier);
+	void _create_binding_modifiers();
+
+	void _on_add_binding_modifier();
+	void _on_remove_binding_modifier(Object *p_binding_modifier_editor);
+	void _on_dialog_created();
+
+protected:
+	EditorUndoRedoManager *undo_redo;
+	Ref<OpenXRActionMap> action_map;
+	Ref<OpenXRInteractionProfile> interaction_profile;
+	Ref<OpenXRIPBinding> ip_binding;
+
+	static void _bind_methods();
+	void _notification(int p_what);
+
+	// used for undo/redo
+	void _do_add_binding_modifier_editor(OpenXRBindingModifierEditor *p_binding_modifier_editor);
+	void _do_remove_binding_modifier_editor(OpenXRBindingModifierEditor *p_binding_modifier_editor);
+
+public:
+	OpenXRBindingModifiersDialog();
+
+	void setup(Ref<OpenXRActionMap> p_action_map, Ref<OpenXRInteractionProfile> p_interaction_profile, Ref<OpenXRIPBinding> p_ip_binding = Ref<OpenXRIPBinding>());
+};
+
+#endif // OPENXR_BINDING_MODIFIERS_DIALOG_H
diff --git a/modules/openxr/editor/openxr_editor_plugin.cpp b/modules/openxr/editor/openxr_editor_plugin.cpp
index f6b7f2dd0c..ecd1d8451f 100644
--- a/modules/openxr/editor/openxr_editor_plugin.cpp
+++ b/modules/openxr/editor/openxr_editor_plugin.cpp
@@ -56,6 +56,9 @@ OpenXREditorPlugin::OpenXREditorPlugin() {
 	action_map_editor = memnew(OpenXRActionMapEditor);
 	EditorNode::get_bottom_panel()->add_item(TTR("OpenXR Action Map"), action_map_editor, ED_SHORTCUT_AND_COMMAND("bottom_panels/toggle_openxr_action_map_bottom_panel", TTR("Toggle OpenXR Action Map Bottom Panel")));
 
+	binding_modifier_inspector_plugin = Ref<EditorInspectorPluginBindingModifier>(memnew(EditorInspectorPluginBindingModifier));
+	EditorInspector::add_inspector_plugin(binding_modifier_inspector_plugin);
+
 #ifndef ANDROID_ENABLED
 	select_runtime = memnew(OpenXRSelectRuntime);
 	add_control_to_container(CONTAINER_TOOLBAR, select_runtime);
diff --git a/modules/openxr/editor/openxr_editor_plugin.h b/modules/openxr/editor/openxr_editor_plugin.h
index 672df0de28..18c82b24ac 100644
--- a/modules/openxr/editor/openxr_editor_plugin.h
+++ b/modules/openxr/editor/openxr_editor_plugin.h
@@ -32,6 +32,7 @@
 #define OPENXR_EDITOR_PLUGIN_H
 
 #include "openxr_action_map_editor.h"
+#include "openxr_binding_modifier_editor.h"
 #include "openxr_select_runtime.h"
 
 #include "editor/plugins/editor_plugin.h"
@@ -40,6 +41,7 @@ class OpenXREditorPlugin : public EditorPlugin {
 	GDCLASS(OpenXREditorPlugin, EditorPlugin);
 
 	OpenXRActionMapEditor *action_map_editor = nullptr;
+	Ref<EditorInspectorPluginBindingModifier> binding_modifier_inspector_plugin = nullptr;
 #ifndef ANDROID_ENABLED
 	OpenXRSelectRuntime *select_runtime = nullptr;
 #endif
diff --git a/modules/openxr/editor/openxr_interaction_profile_editor.cpp b/modules/openxr/editor/openxr_interaction_profile_editor.cpp
index a390919856..e1a6be1562 100644
--- a/modules/openxr/editor/openxr_interaction_profile_editor.cpp
+++ b/modules/openxr/editor/openxr_interaction_profile_editor.cpp
@@ -29,20 +29,16 @@
 /**************************************************************************/
 
 #include "openxr_interaction_profile_editor.h"
-
+#include "../openxr_api.h"
 #include "editor/editor_string_names.h"
-#include "scene/gui/box_container.h"
-#include "scene/gui/button.h"
-#include "scene/gui/label.h"
-#include "scene/gui/line_edit.h"
-#include "scene/gui/panel_container.h"
-#include "scene/gui/separator.h"
-#include "scene/gui/text_edit.h"
+#include "openxr_action_map_editor.h"
 
 ///////////////////////////////////////////////////////////////////////////
 // Interaction profile editor base
 
 void OpenXRInteractionProfileEditorBase::_bind_methods() {
+	ClassDB::bind_method(D_METHOD("setup", "action_map", "interaction_profile"), &OpenXRInteractionProfileEditorBase::setup);
+
 	ClassDB::bind_method(D_METHOD("_add_binding", "action", "path"), &OpenXRInteractionProfileEditorBase::_add_binding);
 	ClassDB::bind_method(D_METHOD("_remove_binding", "action", "path"), &OpenXRInteractionProfileEditorBase::_remove_binding);
 }
@@ -112,12 +108,36 @@ void OpenXRInteractionProfileEditorBase::_remove_binding(const String p_action,
 	}
 }
 
-void OpenXRInteractionProfileEditorBase::remove_all_bindings_for_action(Ref<OpenXRAction> p_action) {
+void OpenXRInteractionProfileEditorBase::_update_interaction_profile() {
+	if (!is_dirty) {
+		// no need to update
+		return;
+	}
+
+	// Nothing to do here for now..
+
+	// and we've updated it...
+	is_dirty = false;
+}
+
+void OpenXRInteractionProfileEditorBase::_theme_changed() {
+	if (binding_modifiers_btn) {
+		binding_modifiers_btn->set_button_icon(get_theme_icon(SNAME("Modifiers"), EditorStringName(EditorIcons)));
+	}
+}
+
+void OpenXRInteractionProfileEditorBase::remove_all_for_action_set(Ref<OpenXRActionSet> p_action_set) {
+	// Note, don't need to remove bindings themselves as remove_all_for_action will be called for each before this is called.
+
+	// TODO update binding modifiers
+}
+
+void OpenXRInteractionProfileEditorBase::remove_all_for_action(Ref<OpenXRAction> p_action) {
 	Vector<Ref<OpenXRIPBinding>> bindings = interaction_profile->get_bindings_for_action(p_action);
 	if (bindings.size() > 0) {
 		String action_name = p_action->get_name_with_set();
 
-		// for our undo/redo we process all paths
+		// For our undo/redo we process all paths
 		undo_redo->create_action(TTR("Remove action from interaction profile"));
 		for (const Ref<OpenXRIPBinding> &binding : bindings) {
 			undo_redo->add_do_method(this, "_remove_binding", action_name, binding->get_binding_path());
@@ -125,7 +145,7 @@ void OpenXRInteractionProfileEditorBase::remove_all_bindings_for_action(Ref<Open
 		}
 		undo_redo->commit_action(false);
 
-		// but we take a shortcut here :)
+		// But remove them all in one go so we're more efficient in updating our UI.
 		for (const Ref<OpenXRIPBinding> &binding : bindings) {
 			interaction_profile->remove_binding(binding);
 		}
@@ -137,10 +157,39 @@ void OpenXRInteractionProfileEditorBase::remove_all_bindings_for_action(Ref<Open
 		_do_update_interaction_profile();
 	}
 }
+void OpenXRInteractionProfileEditorBase::_on_open_binding_modifiers() {
+	binding_modifiers_dialog->popup_centered(Size2i(500, 400));
+}
 
-OpenXRInteractionProfileEditorBase::OpenXRInteractionProfileEditorBase(Ref<OpenXRActionMap> p_action_map, Ref<OpenXRInteractionProfile> p_interaction_profile) {
+OpenXRInteractionProfileEditorBase::OpenXRInteractionProfileEditorBase() {
 	undo_redo = EditorUndoRedoManager::get_singleton();
 
+	set_h_size_flags(SIZE_EXPAND_FILL);
+	set_v_size_flags(SIZE_EXPAND_FILL);
+
+	interaction_profile_sc = memnew(ScrollContainer);
+	interaction_profile_sc->set_h_size_flags(SIZE_EXPAND_FILL);
+	interaction_profile_sc->set_v_size_flags(SIZE_EXPAND_FILL);
+	add_child(interaction_profile_sc);
+
+	binding_modifiers_dialog = memnew(OpenXRBindingModifiersDialog);
+	add_child(binding_modifiers_dialog);
+
+	toolbar_vb = memnew(VBoxContainer);
+	toolbar_vb->set_v_size_flags(SIZE_EXPAND_FILL);
+	add_child(toolbar_vb);
+
+	binding_modifiers_btn = memnew(Button);
+	binding_modifiers_btn->set_tooltip_text(TTR("Edit binding modifiers"));
+	binding_modifiers_btn->connect("pressed", callable_mp(this, &OpenXRInteractionProfileEditorBase::_on_open_binding_modifiers));
+	// TODO show visual difference if there are binding modifiers for this interaction profile
+	toolbar_vb->add_child(binding_modifiers_btn);
+}
+
+void OpenXRInteractionProfileEditorBase::setup(Ref<OpenXRActionMap> p_action_map, Ref<OpenXRInteractionProfile> p_interaction_profile) {
+	ERR_FAIL_NULL(binding_modifiers_dialog);
+	binding_modifiers_dialog->setup(p_action_map, p_interaction_profile);
+
 	action_map = p_action_map;
 	interaction_profile = p_interaction_profile;
 	String profile_path = interaction_profile->get_interaction_profile_path();
@@ -149,11 +198,15 @@ OpenXRInteractionProfileEditorBase::OpenXRInteractionProfileEditorBase(Ref<OpenX
 	profile_def = OpenXRInteractionProfileMetadata::get_singleton()->get_profile(profile_path);
 	if (profile_def != nullptr) {
 		profile_name = profile_def->display_name;
+
+		if (!profile_def->openxr_extension_name.is_empty()) {
+			profile_name += "*";
+
+			tooltip = vformat(TTR("Note: This interaction profile requires extension %s support."), profile_def->openxr_extension_name);
+		}
 	}
 
 	set_name(profile_name);
-	set_h_size_flags(SIZE_EXPAND_FILL);
-	set_v_size_flags(SIZE_EXPAND_FILL);
 
 	// Make sure it is updated when it enters the tree...
 	is_dirty = true;
@@ -167,7 +220,7 @@ void OpenXRInteractionProfileEditor::select_action_for(const String p_io_path) {
 	select_action_dialog->open();
 }
 
-void OpenXRInteractionProfileEditor::action_selected(const String p_action) {
+void OpenXRInteractionProfileEditor::_on_action_selected(const String p_action) {
 	undo_redo->create_action(TTR("Add binding"));
 	undo_redo->add_do_method(this, "_add_binding", p_action, selecting_for_io_path);
 	undo_redo->add_undo_method(this, "_remove_binding", p_action, selecting_for_io_path);
@@ -189,7 +242,12 @@ void OpenXRInteractionProfileEditor::_add_io_path(VBoxContainer *p_container, co
 	p_container->add_child(path_hb);
 
 	Label *path_label = memnew(Label);
-	path_label->set_text(p_io_path->display_name);
+	if (p_io_path->openxr_extension_name.is_empty()) {
+		path_label->set_text(p_io_path->display_name);
+	} else {
+		path_label->set_text(p_io_path->display_name + "*");
+		p_container->set_tooltip_text(vformat(TTR("Note: This binding path requires extension %s support."), p_io_path->openxr_extension_name));
+	}
 	path_label->set_h_size_flags(Control::SIZE_EXPAND_FILL);
 	path_hb->add_child(path_label);
 
@@ -243,6 +301,17 @@ void OpenXRInteractionProfileEditor::_add_io_path(VBoxContainer *p_container, co
 				action_label->set_h_size_flags(Control::SIZE_EXPAND_FILL);
 				action_hb->add_child(action_label);
 
+				OpenXRBindingModifiersDialog *action_binding_modifiers_dialog = memnew(OpenXRBindingModifiersDialog);
+				action_binding_modifiers_dialog->setup(action_map, interaction_profile, binding);
+				action_hb->add_child(action_binding_modifiers_dialog);
+
+				Button *action_binding_modifiers_btn = memnew(Button);
+				action_binding_modifiers_btn->set_flat(true);
+				action_binding_modifiers_btn->set_button_icon(get_theme_icon(SNAME("Modifiers"), EditorStringName(EditorIcons)));
+				action_binding_modifiers_btn->connect(SceneStringName(pressed), callable_mp((Window *)action_binding_modifiers_dialog, &Window::popup_centered).bind(Size2i(500, 400)));
+				// TODO change style of button if there are binding modifiers
+				action_hb->add_child(action_binding_modifiers_btn);
+
 				Button *action_rem = memnew(Button);
 				action_rem->set_flat(true);
 				action_rem->set_button_icon(get_theme_icon(SNAME("Remove"), EditorStringName(EditorIcons)));
@@ -261,9 +330,11 @@ void OpenXRInteractionProfileEditor::_update_interaction_profile() {
 		return;
 	}
 
+	PackedStringArray requested_extensions = OpenXRAPI::get_all_requested_extensions();
+
 	// out with the old...
-	while (main_hb->get_child_count() > 0) {
-		memdelete(main_hb->get_child(0));
+	while (interaction_profile_hb->get_child_count() > 0) {
+		memdelete(interaction_profile_hb->get_child(0));
 	}
 
 	// in with the new...
@@ -281,7 +352,7 @@ void OpenXRInteractionProfileEditor::_update_interaction_profile() {
 	for (int i = 0; i < top_level_paths.size(); i++) {
 		PanelContainer *panel = memnew(PanelContainer);
 		panel->set_v_size_flags(Control::SIZE_EXPAND_FILL);
-		main_hb->add_child(panel);
+		interaction_profile_hb->add_child(panel);
 		panel->add_theme_style_override(SceneStringName(panel), get_theme_stylebox(SceneStringName(panel), SNAME("TabContainer")));
 
 		VBoxContainer *container = memnew(VBoxContainer);
@@ -293,31 +364,37 @@ void OpenXRInteractionProfileEditor::_update_interaction_profile() {
 
 		for (int j = 0; j < profile_def->io_paths.size(); j++) {
 			const OpenXRInteractionProfileMetadata::IOPath *io_path = &profile_def->io_paths[j];
-			if (io_path->top_level_path == top_level_paths[i]) {
+			if (io_path->top_level_path == top_level_paths[i] && (io_path->openxr_extension_name.is_empty() || requested_extensions.has(io_path->openxr_extension_name))) {
 				_add_io_path(container, io_path);
 			}
 		}
 	}
 
-	// and we've updated it...
-	is_dirty = false;
+	OpenXRInteractionProfileEditorBase::_update_interaction_profile();
 }
 
 void OpenXRInteractionProfileEditor::_theme_changed() {
-	for (int i = 0; i < main_hb->get_child_count(); i++) {
-		Control *panel = Object::cast_to<Control>(main_hb->get_child(i));
+	OpenXRInteractionProfileEditorBase::_theme_changed();
+
+	interaction_profile_sc->add_theme_style_override(SceneStringName(panel), get_theme_stylebox(SceneStringName(panel), SNAME("Tree")));
+
+	for (int i = 0; i < interaction_profile_hb->get_child_count(); i++) {
+		Control *panel = Object::cast_to<Control>(interaction_profile_hb->get_child(i));
 		if (panel) {
 			panel->add_theme_style_override(SceneStringName(panel), get_theme_stylebox(SceneStringName(panel), SNAME("TabContainer")));
 		}
 	}
 }
 
-OpenXRInteractionProfileEditor::OpenXRInteractionProfileEditor(Ref<OpenXRActionMap> p_action_map, Ref<OpenXRInteractionProfile> p_interaction_profile) :
-		OpenXRInteractionProfileEditorBase(p_action_map, p_interaction_profile) {
-	main_hb = memnew(HBoxContainer);
-	add_child(main_hb);
+OpenXRInteractionProfileEditor::OpenXRInteractionProfileEditor() {
+	interaction_profile_hb = memnew(HBoxContainer);
+	interaction_profile_sc->add_child(interaction_profile_hb);
+}
+
+void OpenXRInteractionProfileEditor::setup(Ref<OpenXRActionMap> p_action_map, Ref<OpenXRInteractionProfile> p_interaction_profile) {
+	OpenXRInteractionProfileEditorBase::setup(p_action_map, p_interaction_profile);
 
 	select_action_dialog = memnew(OpenXRSelectActionDialog(p_action_map));
-	select_action_dialog->connect("action_selected", callable_mp(this, &OpenXRInteractionProfileEditor::action_selected));
+	select_action_dialog->connect("action_selected", callable_mp(this, &OpenXRInteractionProfileEditor::_on_action_selected));
 	add_child(select_action_dialog);
 }
diff --git a/modules/openxr/editor/openxr_interaction_profile_editor.h b/modules/openxr/editor/openxr_interaction_profile_editor.h
index 2ec72127cf..51615af9ad 100644
--- a/modules/openxr/editor/openxr_interaction_profile_editor.h
+++ b/modules/openxr/editor/openxr_interaction_profile_editor.h
@@ -34,19 +34,29 @@
 #include "../action_map/openxr_action_map.h"
 #include "../action_map/openxr_interaction_profile.h"
 #include "../action_map/openxr_interaction_profile_metadata.h"
-#include "openxr_select_action_dialog.h"
-
+#include "../editor/openxr_binding_modifiers_dialog.h"
 #include "editor/editor_undo_redo_manager.h"
-#include "scene/gui/scroll_container.h"
+#include "openxr_select_action_dialog.h"
+#include "scene/gui/box_container.h"
+#include "scene/gui/button.h"
 
-class OpenXRInteractionProfileEditorBase : public ScrollContainer {
-	GDCLASS(OpenXRInteractionProfileEditorBase, ScrollContainer);
+class OpenXRInteractionProfileEditorBase : public HBoxContainer {
+	GDCLASS(OpenXRInteractionProfileEditorBase, HBoxContainer);
+
+private:
+	OpenXRBindingModifiersDialog *binding_modifiers_dialog = nullptr;
+	VBoxContainer *toolbar_vb = nullptr;
+	Button *binding_modifiers_btn = nullptr;
+
+	void _on_open_binding_modifiers();
 
 protected:
 	EditorUndoRedoManager *undo_redo;
 	Ref<OpenXRInteractionProfile> interaction_profile;
 	Ref<OpenXRActionMap> action_map;
 
+	ScrollContainer *interaction_profile_sc = nullptr;
+
 	bool is_dirty = false;
 
 	static void _bind_methods();
@@ -55,18 +65,23 @@ protected:
 	const OpenXRInteractionProfileMetadata::InteractionProfile *profile_def = nullptr;
 
 public:
+	String tooltip; // Tooltip text to show on tab
+
 	Ref<OpenXRInteractionProfile> get_interaction_profile() { return interaction_profile; }
 
-	virtual void _update_interaction_profile() {}
-	virtual void _theme_changed() {}
+	virtual void _update_interaction_profile();
+	virtual void _theme_changed();
 
 	void _do_update_interaction_profile();
 	void _add_binding(const String p_action, const String p_path);
 	void _remove_binding(const String p_action, const String p_path);
 
-	void remove_all_bindings_for_action(Ref<OpenXRAction> p_action);
+	void remove_all_for_action_set(Ref<OpenXRActionSet> p_action_set);
+	void remove_all_for_action(Ref<OpenXRAction> p_action);
 
-	OpenXRInteractionProfileEditorBase(Ref<OpenXRActionMap> p_action_map, Ref<OpenXRInteractionProfile> p_interaction_profile);
+	virtual void setup(Ref<OpenXRActionMap> p_action_map, Ref<OpenXRInteractionProfile> p_interaction_profile);
+
+	OpenXRInteractionProfileEditorBase();
 };
 
 class OpenXRInteractionProfileEditor : public OpenXRInteractionProfileEditorBase {
@@ -74,19 +89,22 @@ class OpenXRInteractionProfileEditor : public OpenXRInteractionProfileEditorBase
 
 private:
 	String selecting_for_io_path;
-	HBoxContainer *main_hb = nullptr;
+	HBoxContainer *interaction_profile_hb = nullptr;
+
 	OpenXRSelectActionDialog *select_action_dialog = nullptr;
 
 	void _add_io_path(VBoxContainer *p_container, const OpenXRInteractionProfileMetadata::IOPath *p_io_path);
 
 public:
 	void select_action_for(const String p_io_path);
-	void action_selected(const String p_action);
+	void _on_action_selected(const String p_action);
 	void _on_remove_pressed(const String p_action, const String p_for_io_path);
 
 	virtual void _update_interaction_profile() override;
 	virtual void _theme_changed() override;
-	OpenXRInteractionProfileEditor(Ref<OpenXRActionMap> p_action_map, Ref<OpenXRInteractionProfile> p_interaction_profile);
+	virtual void setup(Ref<OpenXRActionMap> p_action_map, Ref<OpenXRInteractionProfile> p_interaction_profile) override;
+
+	OpenXRInteractionProfileEditor();
 };
 
 #endif // OPENXR_INTERACTION_PROFILE_EDITOR_H
diff --git a/modules/openxr/editor/openxr_select_interaction_profile_dialog.cpp b/modules/openxr/editor/openxr_select_interaction_profile_dialog.cpp
index ee8940f30b..ffb7fb6464 100644
--- a/modules/openxr/editor/openxr_select_interaction_profile_dialog.cpp
+++ b/modules/openxr/editor/openxr_select_interaction_profile_dialog.cpp
@@ -29,6 +29,7 @@
 /**************************************************************************/
 
 #include "openxr_select_interaction_profile_dialog.h"
+#include "../openxr_api.h"
 
 void OpenXRSelectInteractionProfileDialog::_bind_methods() {
 	ADD_SIGNAL(MethodInfo("interaction_profile_selected", PropertyInfo(Variant::STRING, "interaction_profile")));
@@ -66,22 +67,27 @@ void OpenXRSelectInteractionProfileDialog::_on_select_interaction_profile(const
 void OpenXRSelectInteractionProfileDialog::open(PackedStringArray p_do_not_include) {
 	int available_count = 0;
 
+	OpenXRInteractionProfileMetadata *meta_data = OpenXRInteractionProfileMetadata::get_singleton();
+	ERR_FAIL_NULL(meta_data);
+
 	// Out with the old.
 	while (main_vb->get_child_count() > 1) {
 		memdelete(main_vb->get_child(1));
 	}
 
+	PackedStringArray requested_extensions = OpenXRAPI::get_all_requested_extensions();
+
 	selected_interaction_profile = "";
 	ip_buttons.clear();
 
 	// In with the new.
-	PackedStringArray interaction_profiles = OpenXRInteractionProfileMetadata::get_singleton()->get_interaction_profile_paths();
-	for (int i = 0; i < interaction_profiles.size(); i++) {
-		const String &path = interaction_profiles[i];
-		if (!p_do_not_include.has(path)) {
+	PackedStringArray interaction_profiles = meta_data->get_interaction_profile_paths();
+	for (const String &path : interaction_profiles) {
+		const String extension = meta_data->get_interaction_profile_extension(path);
+		if (!p_do_not_include.has(path) && (extension.is_empty() || requested_extensions.has(extension))) {
 			Button *ip_button = memnew(Button);
 			ip_button->set_flat(true);
-			ip_button->set_text(OpenXRInteractionProfileMetadata::get_singleton()->get_profile(path)->display_name);
+			ip_button->set_text(meta_data->get_profile(path)->display_name);
 			ip_button->set_text_alignment(HORIZONTAL_ALIGNMENT_LEFT);
 			ip_button->connect(SceneStringName(pressed), callable_mp(this, &OpenXRSelectInteractionProfileDialog::_on_select_interaction_profile).bind(path));
 			main_vb->add_child(ip_button);
diff --git a/modules/openxr/editor/openxr_select_runtime.cpp b/modules/openxr/editor/openxr_select_runtime.cpp
index 4a2a87cb88..5be2383d9d 100644
--- a/modules/openxr/editor/openxr_select_runtime.cpp
+++ b/modules/openxr/editor/openxr_select_runtime.cpp
@@ -75,7 +75,7 @@ void OpenXRSelectRuntime::_update_items() {
 	select(current_runtime);
 }
 
-void OpenXRSelectRuntime::_item_selected(int p_which) {
+void OpenXRSelectRuntime::_on_item_selected(int p_which) {
 	OS *os = OS::get_singleton();
 
 	if (p_which == 0) {
@@ -95,11 +95,11 @@ void OpenXRSelectRuntime::_notification(int p_notification) {
 			_update_items();
 
 			// Connect signal
-			connect(SceneStringName(item_selected), callable_mp(this, &OpenXRSelectRuntime::_item_selected));
+			connect(SceneStringName(item_selected), callable_mp(this, &OpenXRSelectRuntime::_on_item_selected));
 		} break;
 		case NOTIFICATION_EXIT_TREE: {
 			// Disconnect signal
-			disconnect(SceneStringName(item_selected), callable_mp(this, &OpenXRSelectRuntime::_item_selected));
+			disconnect(SceneStringName(item_selected), callable_mp(this, &OpenXRSelectRuntime::_on_item_selected));
 		} break;
 	}
 }
diff --git a/modules/openxr/editor/openxr_select_runtime.h b/modules/openxr/editor/openxr_select_runtime.h
index 9a3487439c..f5b2f1da60 100644
--- a/modules/openxr/editor/openxr_select_runtime.h
+++ b/modules/openxr/editor/openxr_select_runtime.h
@@ -44,7 +44,7 @@ protected:
 
 private:
 	void _update_items();
-	void _item_selected(int p_which);
+	void _on_item_selected(int p_which);
 };
 
 #endif // OPENXR_SELECT_RUNTIME_H
diff --git a/modules/openxr/extensions/openxr_dpad_binding_extension.cpp b/modules/openxr/extensions/openxr_dpad_binding_extension.cpp
new file mode 100644
index 0000000000..3cca29d4d6
--- /dev/null
+++ b/modules/openxr/extensions/openxr_dpad_binding_extension.cpp
@@ -0,0 +1,274 @@
+/**************************************************************************/
+/*  openxr_dpad_binding_extension.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 "openxr_dpad_binding_extension.h"
+#include "../action_map/openxr_interaction_profile_metadata.h"
+#include "../openxr_api.h"
+#include "core/math/math_funcs.h"
+
+// Implementation for:
+// https://registry.khronos.org/OpenXR/specs/1.1/html/xrspec.html#XR_EXT_dpad_binding
+
+///////////////////////////////////////////////////////////////////////////////////////////////////
+// OpenXRDPadBindingExtension
+
+OpenXRDPadBindingExtension *OpenXRDPadBindingExtension::singleton = nullptr;
+
+OpenXRDPadBindingExtension *OpenXRDPadBindingExtension::get_singleton() {
+	return singleton;
+}
+
+OpenXRDPadBindingExtension::OpenXRDPadBindingExtension() {
+	singleton = this;
+}
+
+OpenXRDPadBindingExtension::~OpenXRDPadBindingExtension() {
+	singleton = nullptr;
+}
+
+HashMap<String, bool *> OpenXRDPadBindingExtension::get_requested_extensions() {
+	HashMap<String, bool *> request_extensions;
+
+	// Note, we're dependent on the binding modifier extension, this may be requested by multiple extension wrappers.
+	request_extensions[XR_KHR_BINDING_MODIFICATION_EXTENSION_NAME] = &binding_modifier_ext;
+	request_extensions[XR_EXT_DPAD_BINDING_EXTENSION_NAME] = &dpad_binding_ext;
+
+	return request_extensions;
+}
+
+bool OpenXRDPadBindingExtension::is_available() {
+	return binding_modifier_ext && dpad_binding_ext;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////
+// OpenXRDpadBindingModifier
+
+void OpenXRDpadBindingModifier::_bind_methods() {
+	ClassDB::bind_method(D_METHOD("set_action_set", "action_set"), &OpenXRDpadBindingModifier::set_action_set);
+	ClassDB::bind_method(D_METHOD("get_action_set"), &OpenXRDpadBindingModifier::get_action_set);
+	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "action_set", PROPERTY_HINT_RESOURCE_TYPE, "OpenXRActionSet"), "set_action_set", "get_action_set");
+
+	ClassDB::bind_method(D_METHOD("set_input_path", "input_path"), &OpenXRDpadBindingModifier::set_input_path);
+	ClassDB::bind_method(D_METHOD("get_input_path"), &OpenXRDpadBindingModifier::get_input_path);
+	ADD_PROPERTY(PropertyInfo(Variant::STRING, "input_path", PROPERTY_HINT_TYPE_STRING, "binding_path"), "set_input_path", "get_input_path");
+
+	ClassDB::bind_method(D_METHOD("set_threshold", "threshold"), &OpenXRDpadBindingModifier::set_threshold);
+	ClassDB::bind_method(D_METHOD("get_threshold"), &OpenXRDpadBindingModifier::get_threshold);
+	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "threshold", PROPERTY_HINT_RANGE, "0.0,1.0,0.01"), "set_threshold", "get_threshold");
+
+	ClassDB::bind_method(D_METHOD("set_threshold_released", "threshold_released"), &OpenXRDpadBindingModifier::set_threshold_released);
+	ClassDB::bind_method(D_METHOD("get_threshold_released"), &OpenXRDpadBindingModifier::get_threshold_released);
+	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "threshold_released", PROPERTY_HINT_RANGE, "0.0,1.0,0.01"), "set_threshold_released", "get_threshold_released");
+
+	ClassDB::bind_method(D_METHOD("set_center_region", "center_region"), &OpenXRDpadBindingModifier::set_center_region);
+	ClassDB::bind_method(D_METHOD("get_center_region"), &OpenXRDpadBindingModifier::get_center_region);
+	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "center_region", PROPERTY_HINT_RANGE, "0.0,1.0,0.01"), "set_center_region", "get_center_region");
+
+	ClassDB::bind_method(D_METHOD("set_wedge_angle", "wedge_angle"), &OpenXRDpadBindingModifier::set_wedge_angle);
+	ClassDB::bind_method(D_METHOD("get_wedge_angle"), &OpenXRDpadBindingModifier::get_wedge_angle);
+	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "wedge_angle"), "set_wedge_angle", "get_wedge_angle");
+
+	ClassDB::bind_method(D_METHOD("set_is_sticky", "is_sticky"), &OpenXRDpadBindingModifier::set_is_sticky);
+	ClassDB::bind_method(D_METHOD("get_is_sticky"), &OpenXRDpadBindingModifier::get_is_sticky);
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "is_sticky"), "set_is_sticky", "get_is_sticky");
+
+	ClassDB::bind_method(D_METHOD("set_on_haptic", "haptic"), &OpenXRDpadBindingModifier::set_on_haptic);
+	ClassDB::bind_method(D_METHOD("get_on_haptic"), &OpenXRDpadBindingModifier::get_on_haptic);
+	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "on_haptic", PROPERTY_HINT_RESOURCE_TYPE, "OpenXRHapticBase"), "set_on_haptic", "get_on_haptic");
+
+	ClassDB::bind_method(D_METHOD("set_off_haptic", "haptic"), &OpenXRDpadBindingModifier::set_off_haptic);
+	ClassDB::bind_method(D_METHOD("get_off_haptic"), &OpenXRDpadBindingModifier::get_off_haptic);
+	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "off_haptic", PROPERTY_HINT_RESOURCE_TYPE, "OpenXRHapticBase"), "set_off_haptic", "get_off_haptic");
+}
+
+OpenXRDpadBindingModifier::OpenXRDpadBindingModifier() {
+	ERR_FAIL_COND(dpad_bindings_data.resize_zeroed(sizeof(XrInteractionProfileDpadBindingEXT)) != OK);
+	dpad_bindings = (XrInteractionProfileDpadBindingEXT *)dpad_bindings_data.ptrw();
+
+	dpad_bindings->type = XR_TYPE_INTERACTION_PROFILE_DPAD_BINDING_EXT;
+	dpad_bindings->next = nullptr;
+
+	dpad_bindings->forceThreshold = 0.6;
+	dpad_bindings->forceThresholdReleased = 0.4;
+	dpad_bindings->centerRegion = 0.1;
+	dpad_bindings->wedgeAngle = Math::deg_to_rad(90.0);
+	dpad_bindings->isSticky = false;
+}
+
+void OpenXRDpadBindingModifier::set_action_set(const Ref<OpenXRActionSet> p_action_set) {
+	action_set = p_action_set;
+}
+
+Ref<OpenXRActionSet> OpenXRDpadBindingModifier::get_action_set() const {
+	return action_set;
+}
+
+void OpenXRDpadBindingModifier::set_input_path(const String &p_input_path) {
+	input_path = p_input_path;
+	emit_changed();
+}
+
+String OpenXRDpadBindingModifier::get_input_path() const {
+	return input_path;
+}
+
+void OpenXRDpadBindingModifier::set_threshold(float p_threshold) {
+	ERR_FAIL_NULL(dpad_bindings);
+	ERR_FAIL_COND(p_threshold < 0.0 || p_threshold > 1.0);
+
+	dpad_bindings->forceThreshold = p_threshold;
+	emit_changed();
+}
+
+float OpenXRDpadBindingModifier::get_threshold() const {
+	ERR_FAIL_NULL_V(dpad_bindings, 0.0);
+	return dpad_bindings->forceThreshold;
+}
+
+void OpenXRDpadBindingModifier::set_threshold_released(float p_threshold) {
+	ERR_FAIL_NULL(dpad_bindings);
+	ERR_FAIL_COND(p_threshold < 0.0 || p_threshold > 1.0);
+
+	dpad_bindings->forceThresholdReleased = p_threshold;
+	emit_changed();
+}
+
+float OpenXRDpadBindingModifier::get_threshold_released() const {
+	ERR_FAIL_NULL_V(dpad_bindings, 0.0);
+	return dpad_bindings->forceThresholdReleased;
+}
+
+void OpenXRDpadBindingModifier::set_center_region(float p_center_region) {
+	ERR_FAIL_NULL(dpad_bindings);
+	ERR_FAIL_COND(p_center_region < 0.0 || p_center_region > 1.0);
+
+	dpad_bindings->centerRegion = p_center_region;
+	emit_changed();
+}
+
+float OpenXRDpadBindingModifier::get_center_region() const {
+	ERR_FAIL_NULL_V(dpad_bindings, 0.0);
+	return dpad_bindings->centerRegion;
+}
+
+void OpenXRDpadBindingModifier::set_wedge_angle(float p_wedge_angle) {
+	ERR_FAIL_NULL(dpad_bindings);
+	dpad_bindings->wedgeAngle = p_wedge_angle;
+	emit_changed();
+}
+
+float OpenXRDpadBindingModifier::get_wedge_angle() const {
+	ERR_FAIL_NULL_V(dpad_bindings, 0.0);
+	return dpad_bindings->wedgeAngle;
+}
+
+void OpenXRDpadBindingModifier::set_wedge_angle_deg(float p_wedge_angle) {
+	ERR_FAIL_NULL(dpad_bindings);
+	dpad_bindings->wedgeAngle = Math::deg_to_rad(p_wedge_angle);
+	emit_changed();
+}
+
+float OpenXRDpadBindingModifier::get_wedge_angle_deg() const {
+	ERR_FAIL_NULL_V(dpad_bindings, 0.0);
+	return Math::rad_to_deg(dpad_bindings->wedgeAngle);
+}
+
+void OpenXRDpadBindingModifier::set_is_sticky(bool p_sticky) {
+	ERR_FAIL_NULL(dpad_bindings);
+	dpad_bindings->isSticky = p_sticky;
+	emit_changed();
+}
+
+bool OpenXRDpadBindingModifier::get_is_sticky() const {
+	ERR_FAIL_NULL_V(dpad_bindings, false);
+	return dpad_bindings->isSticky;
+}
+
+void OpenXRDpadBindingModifier::set_on_haptic(const Ref<OpenXRHapticBase> &p_haptic) {
+	on_haptic = p_haptic;
+	emit_changed();
+}
+
+Ref<OpenXRHapticBase> OpenXRDpadBindingModifier::get_on_haptic() const {
+	return on_haptic;
+}
+
+void OpenXRDpadBindingModifier::set_off_haptic(const Ref<OpenXRHapticBase> &p_haptic) {
+	off_haptic = p_haptic;
+	emit_changed();
+}
+
+Ref<OpenXRHapticBase> OpenXRDpadBindingModifier::get_off_haptic() const {
+	return off_haptic;
+}
+
+PackedByteArray OpenXRDpadBindingModifier::get_ip_modification() {
+	ERR_FAIL_NULL_V(dpad_bindings, PackedByteArray());
+
+	OpenXRAPI *openxr_api = OpenXRAPI::get_singleton();
+	ERR_FAIL_NULL_V(openxr_api, PackedByteArray());
+
+	OpenXRDPadBindingExtension *dpad_binding_ext = OpenXRDPadBindingExtension::get_singleton();
+	if (!dpad_binding_ext || !dpad_binding_ext->is_available()) {
+		// Extension not enabled!
+		WARN_PRINT("DPad binding extension is not enabled or available.");
+		return PackedByteArray();
+	}
+
+	dpad_bindings->binding = openxr_api->get_xr_path(input_path);
+	ERR_FAIL_COND_V(dpad_bindings->binding == XR_NULL_PATH, PackedByteArray());
+
+	// Get our action set
+	ERR_FAIL_COND_V(!action_set.is_valid(), PackedByteArray());
+	RID action_set_rid = openxr_api->find_action_set(action_set->get_name());
+	ERR_FAIL_COND_V(!action_set_rid.is_valid(), PackedByteArray());
+	dpad_bindings->actionSet = openxr_api->action_set_get_handle(action_set_rid);
+
+	// These are set already:
+	// - forceThreshold
+	// - forceThresholdReleased
+	// - centerRegion
+	// - wedgeAngle
+	// - isSticky
+
+	if (on_haptic.is_valid()) {
+		dpad_bindings->onHaptic = on_haptic->get_xr_structure();
+	} else {
+		dpad_bindings->onHaptic = nullptr;
+	}
+
+	if (off_haptic.is_valid()) {
+		dpad_bindings->offHaptic = off_haptic->get_xr_structure();
+	} else {
+		dpad_bindings->offHaptic = nullptr;
+	}
+
+	return dpad_bindings_data;
+}
diff --git a/modules/openxr/extensions/openxr_dpad_binding_extension.h b/modules/openxr/extensions/openxr_dpad_binding_extension.h
new file mode 100644
index 0000000000..e0c8c3e26a
--- /dev/null
+++ b/modules/openxr/extensions/openxr_dpad_binding_extension.h
@@ -0,0 +1,109 @@
+/**************************************************************************/
+/*  openxr_dpad_binding_extension.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 OPENXR_DPAD_BINDING_EXTENSION_H
+#define OPENXR_DPAD_BINDING_EXTENSION_H
+
+#include "../action_map/openxr_action_set.h"
+#include "../action_map/openxr_binding_modifier.h"
+#include "../action_map/openxr_haptic_feedback.h"
+#include "../util.h"
+#include "openxr_extension_wrapper.h"
+
+class OpenXRDPadBindingExtension : public OpenXRExtensionWrapper {
+public:
+	static OpenXRDPadBindingExtension *get_singleton();
+
+	OpenXRDPadBindingExtension();
+	virtual ~OpenXRDPadBindingExtension() override;
+
+	virtual HashMap<String, bool *> get_requested_extensions() override;
+
+	bool is_available();
+
+private:
+	static OpenXRDPadBindingExtension *singleton;
+
+	bool binding_modifier_ext = false;
+	bool dpad_binding_ext = false;
+};
+
+class OpenXRDpadBindingModifier : public OpenXRIPBindingModifier {
+	GDCLASS(OpenXRDpadBindingModifier, OpenXRIPBindingModifier);
+
+private:
+	PackedByteArray dpad_bindings_data;
+	XrInteractionProfileDpadBindingEXT *dpad_bindings = nullptr;
+	String input_path;
+	Ref<OpenXRActionSet> action_set;
+	Ref<OpenXRHapticBase> on_haptic;
+	Ref<OpenXRHapticBase> off_haptic;
+
+protected:
+	static void _bind_methods();
+
+public:
+	OpenXRDpadBindingModifier();
+
+	void set_action_set(const Ref<OpenXRActionSet> p_action_set);
+	Ref<OpenXRActionSet> get_action_set() const;
+
+	void set_input_path(const String &p_input_path);
+	String get_input_path() const;
+
+	void set_threshold(float p_threshold);
+	float get_threshold() const;
+
+	void set_threshold_released(float p_threshold);
+	float get_threshold_released() const;
+
+	void set_center_region(float p_center_region);
+	float get_center_region() const;
+
+	void set_wedge_angle(float p_wedge_angle);
+	float get_wedge_angle() const;
+
+	void set_wedge_angle_deg(float p_wedge_angle);
+	float get_wedge_angle_deg() const;
+
+	void set_is_sticky(bool p_sticky);
+	bool get_is_sticky() const;
+
+	void set_on_haptic(const Ref<OpenXRHapticBase> &p_haptic);
+	Ref<OpenXRHapticBase> get_on_haptic() const;
+
+	void set_off_haptic(const Ref<OpenXRHapticBase> &p_haptic);
+	Ref<OpenXRHapticBase> get_off_haptic() const;
+
+	virtual String get_description() const override { return "DPad modifier"; }
+	virtual PackedByteArray get_ip_modification() override;
+};
+
+#endif // OPENXR_DPAD_BINDING_EXTENSION_H
diff --git a/modules/openxr/extensions/openxr_htc_controller_extension.cpp b/modules/openxr/extensions/openxr_htc_controller_extension.cpp
index 416934fa47..36894cb0ce 100644
--- a/modules/openxr/extensions/openxr_htc_controller_extension.cpp
+++ b/modules/openxr/extensions/openxr_htc_controller_extension.cpp
@@ -61,96 +61,88 @@ void OpenXRHTCControllerExtension::on_register_metadata() {
 	metadata->register_top_level_path("HTC left hand tracker", "/user/hand_htc/left", XR_HTC_HAND_INTERACTION_EXTENSION_NAME);
 	metadata->register_top_level_path("HTC right hand tracker", "/user/hand_htc/right", XR_HTC_HAND_INTERACTION_EXTENSION_NAME);
 
-	// HTC Vive Cosmos controller
-	metadata->register_interaction_profile("Vive Cosmos controller", "/interaction_profiles/htc/vive_cosmos_controller", XR_HTC_VIVE_COSMOS_CONTROLLER_INTERACTION_EXTENSION_NAME);
-	metadata->register_io_path("/interaction_profiles/htc/vive_cosmos_controller", "Grip pose", "/user/hand/left", "/user/hand/left/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/htc/vive_cosmos_controller", "Grip pose", "/user/hand/right", "/user/hand/right/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/htc/vive_cosmos_controller", "Aim pose", "/user/hand/left", "/user/hand/left/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/htc/vive_cosmos_controller", "Aim pose", "/user/hand/right", "/user/hand/right/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/htc/vive_cosmos_controller", "Palm pose", "/user/hand/left", "/user/hand/left/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/htc/vive_cosmos_controller", "Palm pose", "/user/hand/right", "/user/hand/right/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
+	{ // HTC Vive Cosmos controller
+		const String profile_path = "/interaction_profiles/htc/vive_cosmos_controller";
+		metadata->register_interaction_profile("Vive Cosmos controller", profile_path, XR_HTC_VIVE_COSMOS_CONTROLLER_INTERACTION_EXTENSION_NAME);
+		for (const String user_path : { "/user/hand/left", "/user/hand/right" }) {
+			metadata->register_io_path(profile_path, "Grip pose", user_path, user_path + "/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			metadata->register_io_path(profile_path, "Aim pose", user_path, user_path + "/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			metadata->register_io_path(profile_path, "Palm pose", user_path, user_path + "/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
 
-	metadata->register_io_path("/interaction_profiles/htc/vive_cosmos_controller", "Menu click", "/user/hand/left", "/user/hand/left/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_cosmos_controller", "System click", "/user/hand/right", "/user/hand/right/input/system/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Shoulder click", user_path, user_path + "/input/shoulder/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/htc/vive_cosmos_controller", "X click", "/user/hand/left", "/user/hand/left/input/x/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_cosmos_controller", "Y click", "/user/hand/left", "/user/hand/left/input/y/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_cosmos_controller", "A click", "/user/hand/right", "/user/hand/right/input/a/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_cosmos_controller", "B click", "/user/hand/right", "/user/hand/right/input/b/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trigger", user_path, user_path + "/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Trigger click", user_path, user_path + "/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/htc/vive_cosmos_controller", "Shoulder click", "/user/hand/left", "/user/hand/left/input/shoulder/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_cosmos_controller", "Shoulder click", "/user/hand/right", "/user/hand/right/input/shoulder/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Squeeze click", user_path, user_path + "/input/squeeze/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/htc/vive_cosmos_controller", "Trigger", "/user/hand/left", "/user/hand/left/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/htc/vive_cosmos_controller", "Trigger click", "/user/hand/left", "/user/hand/left/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_cosmos_controller", "Trigger", "/user/hand/right", "/user/hand/right/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/htc/vive_cosmos_controller", "Trigger click", "/user/hand/right", "/user/hand/right/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick", user_path, user_path + "/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
+			metadata->register_io_path(profile_path, "Thumbstick click", user_path, user_path + "/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick touch", user_path, user_path + "/input/thumbstick/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Up", user_path, user_path + "/input/thumbstick/dpad_up", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Down", user_path, user_path + "/input/thumbstick/dpad_down", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Left", user_path, user_path + "/input/thumbstick/dpad_left", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Right", user_path, user_path + "/input/thumbstick/dpad_right", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/htc/vive_cosmos_controller", "Squeeze click", "/user/hand/left", "/user/hand/left/input/squeeze/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_cosmos_controller", "Squeeze click", "/user/hand/right", "/user/hand/right/input/squeeze/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Haptic output", user_path, user_path + "/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+		}
 
-	metadata->register_io_path("/interaction_profiles/htc/vive_cosmos_controller", "Thumbstick", "/user/hand/left", "/user/hand/left/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/htc/vive_cosmos_controller", "Thumbstick click", "/user/hand/left", "/user/hand/left/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_cosmos_controller", "Thumbstick touch", "/user/hand/left", "/user/hand/left/input/thumbstick/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_cosmos_controller", "Thumbstick", "/user/hand/right", "/user/hand/right/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/htc/vive_cosmos_controller", "Thumbstick click", "/user/hand/right", "/user/hand/right/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_cosmos_controller", "Thumbstick touch", "/user/hand/right", "/user/hand/right/input/thumbstick/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "Menu click", "/user/hand/left", "/user/hand/left/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "System click", "/user/hand/right", "/user/hand/right/input/system/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/htc/vive_cosmos_controller", "Haptic output", "/user/hand/left", "/user/hand/left/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	metadata->register_io_path("/interaction_profiles/htc/vive_cosmos_controller", "Haptic output", "/user/hand/right", "/user/hand/right/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+		metadata->register_io_path(profile_path, "X click", "/user/hand/left", "/user/hand/left/input/x/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "Y click", "/user/hand/left", "/user/hand/left/input/y/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "A click", "/user/hand/right", "/user/hand/right/input/a/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "B click", "/user/hand/right", "/user/hand/right/input/b/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+	}
 
-	// HTC Vive Focus 3 controller
-	metadata->register_interaction_profile("Vive Focus 3 controller", "/interaction_profiles/htc/vive_focus3_controller", XR_HTC_VIVE_FOCUS3_CONTROLLER_INTERACTION_EXTENSION_NAME);
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "Grip pose", "/user/hand/left", "/user/hand/left/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "Grip pose", "/user/hand/right", "/user/hand/right/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "Aim pose", "/user/hand/left", "/user/hand/left/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "Aim pose", "/user/hand/right", "/user/hand/right/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "Palm pose", "/user/hand/left", "/user/hand/left/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "Palm pose", "/user/hand/right", "/user/hand/right/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
+	{ // HTC Vive Focus 3 controller
+		const String profile_path = "/interaction_profiles/htc/vive_focus3_controller";
+		metadata->register_interaction_profile("Vive Focus 3 controller", profile_path, XR_HTC_VIVE_FOCUS3_CONTROLLER_INTERACTION_EXTENSION_NAME);
+		for (const String user_path : { "/user/hand/left", "/user/hand/right" }) {
+			metadata->register_io_path(profile_path, "Grip pose", user_path, user_path + "/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			metadata->register_io_path(profile_path, "Aim pose", user_path, user_path + "/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			metadata->register_io_path(profile_path, "Palm pose", user_path, user_path + "/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
 
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "Menu click", "/user/hand/left", "/user/hand/left/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "System click", "/user/hand/right", "/user/hand/right/input/system/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trigger", user_path, user_path + "/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Trigger click", user_path, user_path + "/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trigger touch", user_path, user_path + "/input/trigger/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "X click", "/user/hand/left", "/user/hand/left/input/x/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "Y click", "/user/hand/left", "/user/hand/left/input/y/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "A click", "/user/hand/right", "/user/hand/right/input/a/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "B click", "/user/hand/right", "/user/hand/right/input/b/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Squeeze click", user_path, user_path + "/input/squeeze/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Squeeze touch", user_path, user_path + "/input/squeeze/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "Trigger", "/user/hand/left", "/user/hand/left/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "Trigger click", "/user/hand/left", "/user/hand/left/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "Trigger touch", "/user/hand/left", "/user/hand/left/input/trigger/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "Trigger", "/user/hand/right", "/user/hand/right/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "Trigger click", "/user/hand/right", "/user/hand/right/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "Trigger touch", "/user/hand/right", "/user/hand/right/input/trigger/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick", user_path, user_path + "/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
+			metadata->register_io_path(profile_path, "Thumbstick click", user_path, user_path + "/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick touch", user_path, user_path + "/input/thumbstick/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Up", user_path, user_path + "/input/thumbstick/dpad_up", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Down", user_path, user_path + "/input/thumbstick/dpad_down", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Left", user_path, user_path + "/input/thumbstick/dpad_left", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Right", user_path, user_path + "/input/thumbstick/dpad_right", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "Squeeze click", "/user/hand/left", "/user/hand/left/input/squeeze/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "Squeeze touch", "/user/hand/left", "/user/hand/left/input/squeeze/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "Squeeze click", "/user/hand/right", "/user/hand/right/input/squeeze/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "Squeeze touch", "/user/hand/right", "/user/hand/right/input/squeeze/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbrest touch", user_path, user_path + "/input/thumbrest/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "Thumbstick", "/user/hand/left", "/user/hand/left/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "Thumbstick click", "/user/hand/left", "/user/hand/left/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "Thumbstick touch", "/user/hand/left", "/user/hand/left/input/thumbstick/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "Thumbstick", "/user/hand/right", "/user/hand/right/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "Thumbstick click", "/user/hand/right", "/user/hand/right/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "Thumbstick touch", "/user/hand/right", "/user/hand/right/input/thumbstick/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Haptic output", user_path, user_path + "/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+		}
 
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "Thumbrest touch", "/user/hand/left", "/user/hand/left/input/thumbrest/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "Thumbrest touch", "/user/hand/right", "/user/hand/right/input/thumbrest/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "Menu click", "/user/hand/left", "/user/hand/left/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "System click", "/user/hand/right", "/user/hand/right/input/system/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "Haptic output", "/user/hand/left", "/user/hand/left/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	metadata->register_io_path("/interaction_profiles/htc/vive_focus3_controller", "Haptic output", "/user/hand/right", "/user/hand/right/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+		metadata->register_io_path(profile_path, "X click", "/user/hand/left", "/user/hand/left/input/x/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "Y click", "/user/hand/left", "/user/hand/left/input/y/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "A click", "/user/hand/right", "/user/hand/right/input/a/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "B click", "/user/hand/right", "/user/hand/right/input/b/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+	}
 
-	// HTC Hand interaction profile
-	metadata->register_interaction_profile("HTC Hand interaction", "/interaction_profiles/htc/hand_interaction", XR_HTC_HAND_INTERACTION_EXTENSION_NAME);
-	metadata->register_io_path("/interaction_profiles/htc/hand_interaction", "Grip pose", "/user/hand_htc/left", "/user/hand_htc/left/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/htc/hand_interaction", "Grip pose", "/user/hand_htc/right", "/user/hand_htc/right/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/htc/hand_interaction", "Aim pose", "/user/hand_htc/left", "/user/hand_htc/left/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/htc/hand_interaction", "Aim pose", "/user/hand_htc/right", "/user/hand_htc/right/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+	{ // HTC Hand interaction profile
+		const String profile_path = "/interaction_profiles/htc/hand_interaction";
+		metadata->register_interaction_profile("HTC Hand interaction", profile_path, XR_HTC_HAND_INTERACTION_EXTENSION_NAME);
+		for (const String user_path : { "/user/hand_htc/left", "/user/hand_htc/right" }) {
+			metadata->register_io_path(profile_path, "Grip pose", user_path, user_path + "/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			metadata->register_io_path(profile_path, "Aim pose", user_path, user_path + "/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
 
-	metadata->register_io_path("/interaction_profiles/htc/hand_interaction", "Select (pinch)", "/user/hand_htc/left", "/user/hand_htc/left/input/select/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/htc/hand_interaction", "Select (pinch)", "/user/hand_htc/right", "/user/hand_htc/right/input/select/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Select (pinch)", user_path, user_path + "/input/select/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
 
-	metadata->register_io_path("/interaction_profiles/htc/hand_interaction", "Squeeze (grab)", "/user/hand_htc/left", "/user/hand_htc/left/input/squeeze/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/htc/hand_interaction", "Squeeze (grab)", "/user/hand_htc/right", "/user/hand_htc/right/input/squeeze/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Squeeze (grab)", user_path, user_path + "/input/squeeze/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+		}
+	}
 }
diff --git a/modules/openxr/extensions/openxr_htc_vive_tracker_extension.cpp b/modules/openxr/extensions/openxr_htc_vive_tracker_extension.cpp
index b2ddf71f5e..20c07cd254 100644
--- a/modules/openxr/extensions/openxr_htc_vive_tracker_extension.cpp
+++ b/modules/openxr/extensions/openxr_htc_vive_tracker_extension.cpp
@@ -83,134 +83,45 @@ void OpenXRHTCViveTrackerExtension::on_register_metadata() {
 	metadata->register_top_level_path("Camera tracker", "/user/vive_tracker_htcx/role/camera", XR_HTCX_VIVE_TRACKER_INTERACTION_EXTENSION_NAME);
 	metadata->register_top_level_path("Keyboard tracker", "/user/vive_tracker_htcx/role/keyboard", XR_HTCX_VIVE_TRACKER_INTERACTION_EXTENSION_NAME);
 
-	// HTC Vive tracker
-	// Interestingly enough trackers don't have buttons or inputs, yet these are defined in the spec.
-	// I think this can be supported through attachments on the trackers.
-	metadata->register_interaction_profile("HTC Vive tracker", "/interaction_profiles/htc/vive_tracker_htcx", XR_HTCX_VIVE_TRACKER_INTERACTION_EXTENSION_NAME);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Menu click", "/user/vive_tracker_htcx/role/left_foot", "/user/vive_tracker_htcx/role/left_foot/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Menu click", "/user/vive_tracker_htcx/role/right_foot", "/user/vive_tracker_htcx/role/right_foot/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Menu click", "/user/vive_tracker_htcx/role/left_shoulder", "/user/vive_tracker_htcx/role/left_shoulder/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Menu click", "/user/vive_tracker_htcx/role/right_shoulder", "/user/vive_tracker_htcx/role/right_shoulder/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Menu click", "/user/vive_tracker_htcx/role/left_elbow", "/user/vive_tracker_htcx/role/left_elbow/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Menu click", "/user/vive_tracker_htcx/role/right_elbow", "/user/vive_tracker_htcx/role/right_elbow/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Menu click", "/user/vive_tracker_htcx/role/left_knee", "/user/vive_tracker_htcx/role/left_knee/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Menu click", "/user/vive_tracker_htcx/role/right_knee", "/user/vive_tracker_htcx/role/right_knee/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Menu click", "/user/vive_tracker_htcx/role/waist", "/user/vive_tracker_htcx/role/waist/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Menu click", "/user/vive_tracker_htcx/role/chest", "/user/vive_tracker_htcx/role/chest/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Menu click", "/user/vive_tracker_htcx/role/camera", "/user/vive_tracker_htcx/role/camera/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Menu click", "/user/vive_tracker_htcx/role/keyboard", "/user/vive_tracker_htcx/role/keyboard/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+	{ // HTC Vive tracker
+		// Interestingly enough trackers don't have buttons or inputs, yet these are defined in the spec.
+		// I think this can be supported through attachments on the trackers.
+		const String profile_path = "/interaction_profiles/htc/vive_tracker_htcx";
+		metadata->register_interaction_profile("HTC Vive tracker", profile_path, XR_HTCX_VIVE_TRACKER_INTERACTION_EXTENSION_NAME);
+		for (const String user_path : {
+					 /* "/user/vive_tracker_htcx/role/handheld_object", */
+					 "/user/vive_tracker_htcx/role/left_foot",
+					 "/user/vive_tracker_htcx/role/right_foot",
+					 "/user/vive_tracker_htcx/role/left_shoulder",
+					 "/user/vive_tracker_htcx/role/right_shoulder",
+					 "/user/vive_tracker_htcx/role/left_elbow",
+					 "/user/vive_tracker_htcx/role/right_elbow",
+					 "/user/vive_tracker_htcx/role/left_knee",
+					 "/user/vive_tracker_htcx/role/right_knee",
+					 "/user/vive_tracker_htcx/role/waist",
+					 "/user/vive_tracker_htcx/role/chest",
+					 "/user/vive_tracker_htcx/role/camera",
+					 "/user/vive_tracker_htcx/role/keyboard",
+			 }) {
+			metadata->register_io_path(profile_path, "Grip pose", user_path, user_path + "/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
 
-	// metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trigger", "/user/vive_tracker_htcx/role/handheld_object", "/user/vive_tracker_htcx/role/handheld_object/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trigger", "/user/vive_tracker_htcx/role/left_foot", "/user/vive_tracker_htcx/role/left_foot/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trigger", "/user/vive_tracker_htcx/role/right_foot", "/user/vive_tracker_htcx/role/right_foot/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trigger", "/user/vive_tracker_htcx/role/left_shoulder", "/user/vive_tracker_htcx/role/left_shoulder/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trigger", "/user/vive_tracker_htcx/role/right_shoulder", "/user/vive_tracker_htcx/role/right_shoulder/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trigger", "/user/vive_tracker_htcx/role/left_elbow", "/user/vive_tracker_htcx/role/left_elbow/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trigger", "/user/vive_tracker_htcx/role/right_elbow", "/user/vive_tracker_htcx/role/right_elbow/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trigger", "/user/vive_tracker_htcx/role/left_knee", "/user/vive_tracker_htcx/role/left_knee/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trigger", "/user/vive_tracker_htcx/role/right_knee", "/user/vive_tracker_htcx/role/right_knee/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trigger", "/user/vive_tracker_htcx/role/waist", "/user/vive_tracker_htcx/role/waist/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trigger", "/user/vive_tracker_htcx/role/chest", "/user/vive_tracker_htcx/role/chest/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trigger", "/user/vive_tracker_htcx/role/camera", "/user/vive_tracker_htcx/role/camera/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trigger", "/user/vive_tracker_htcx/role/keyboard", "/user/vive_tracker_htcx/role/keyboard/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Menu click", user_path, user_path + "/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trigger", user_path, user_path + "/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Trigger click", user_path, user_path + "/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Squeeze click", user_path, user_path + "/input/squeeze/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	// metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trigger click", "/user/vive_tracker_htcx/role/handheld_object", "/user/vive_tracker_htcx/role/handheld_object/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trigger click", "/user/vive_tracker_htcx/role/left_foot", "/user/vive_tracker_htcx/role/left_foot/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trigger click", "/user/vive_tracker_htcx/role/right_foot", "/user/vive_tracker_htcx/role/right_foot/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trigger click", "/user/vive_tracker_htcx/role/left_shoulder", "/user/vive_tracker_htcx/role/left_shoulder/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trigger click", "/user/vive_tracker_htcx/role/right_shoulder", "/user/vive_tracker_htcx/role/right_shoulder/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trigger click", "/user/vive_tracker_htcx/role/left_elbow", "/user/vive_tracker_htcx/role/left_elbow/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trigger click", "/user/vive_tracker_htcx/role/right_elbow", "/user/vive_tracker_htcx/role/right_elbow/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trigger click", "/user/vive_tracker_htcx/role/left_knee", "/user/vive_tracker_htcx/role/left_knee/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trigger click", "/user/vive_tracker_htcx/role/right_knee", "/user/vive_tracker_htcx/role/right_knee/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trigger click", "/user/vive_tracker_htcx/role/waist", "/user/vive_tracker_htcx/role/waist/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trigger click", "/user/vive_tracker_htcx/role/chest", "/user/vive_tracker_htcx/role/chest/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trigger click", "/user/vive_tracker_htcx/role/camera", "/user/vive_tracker_htcx/role/camera/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trigger click", "/user/vive_tracker_htcx/role/keyboard", "/user/vive_tracker_htcx/role/keyboard/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trackpad", user_path, user_path + "/input/trackpad", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
+			metadata->register_io_path(profile_path, "Trackpad click", user_path, user_path + "/input/trackpad/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trackpad touch", user_path, user_path + "/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trackpad Dpad Up", user_path, user_path + "/input/trackpad/dpad_up", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trackpad Dpad Down", user_path, user_path + "/input/trackpad/dpad_down", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trackpad Dpad Left", user_path, user_path + "/input/trackpad/dpad_left", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trackpad Dpad Right", user_path, user_path + "/input/trackpad/dpad_right", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trackpad Dpad Center", user_path, user_path + "/input/trackpad/dpad_center", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	// metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Squeeze click", "/user/vive_tracker_htcx/role/handheld_object", "/user/vive_tracker_htcx/role/handheld_object/input/squeeze/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Squeeze click", "/user/vive_tracker_htcx/role/left_foot", "/user/vive_tracker_htcx/role/left_foot/input/squeeze/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Squeeze click", "/user/vive_tracker_htcx/role/right_foot", "/user/vive_tracker_htcx/role/right_foot/input/squeeze/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Squeeze click", "/user/vive_tracker_htcx/role/left_shoulder", "/user/vive_tracker_htcx/role/left_shoulder/input/squeeze/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Squeeze click", "/user/vive_tracker_htcx/role/right_shoulder", "/user/vive_tracker_htcx/role/right_shoulder/input/squeeze/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Squeeze click", "/user/vive_tracker_htcx/role/left_elbow", "/user/vive_tracker_htcx/role/left_elbow/input/squeeze/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Squeeze click", "/user/vive_tracker_htcx/role/right_elbow", "/user/vive_tracker_htcx/role/right_elbow/input/squeeze/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Squeeze click", "/user/vive_tracker_htcx/role/left_knee", "/user/vive_tracker_htcx/role/left_knee/input/squeeze/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Squeeze click", "/user/vive_tracker_htcx/role/right_knee", "/user/vive_tracker_htcx/role/right_knee/input/squeeze/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Squeeze click", "/user/vive_tracker_htcx/role/waist", "/user/vive_tracker_htcx/role/waist/input/squeeze/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Squeeze click", "/user/vive_tracker_htcx/role/chest", "/user/vive_tracker_htcx/role/chest/input/squeeze/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Squeeze click", "/user/vive_tracker_htcx/role/camera", "/user/vive_tracker_htcx/role/camera/input/squeeze/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Squeeze click", "/user/vive_tracker_htcx/role/keyboard", "/user/vive_tracker_htcx/role/keyboard/input/squeeze/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-
-	// metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad", "/user/vive_tracker_htcx/role/handheld_object", "/user/vive_tracker_htcx/role/handheld_object/input/trackpad", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad", "/user/vive_tracker_htcx/role/left_foot", "/user/vive_tracker_htcx/role/left_foot/input/trackpad", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad", "/user/vive_tracker_htcx/role/right_foot", "/user/vive_tracker_htcx/role/right_foot/input/trackpad", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad", "/user/vive_tracker_htcx/role/left_shoulder", "/user/vive_tracker_htcx/role/left_shoulder/input/trackpad", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad", "/user/vive_tracker_htcx/role/right_shoulder", "/user/vive_tracker_htcx/role/right_shoulder/input/trackpad", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad", "/user/vive_tracker_htcx/role/left_elbow", "/user/vive_tracker_htcx/role/left_elbow/input/trackpad", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad", "/user/vive_tracker_htcx/role/right_elbow", "/user/vive_tracker_htcx/role/right_elbow/input/trackpad", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad", "/user/vive_tracker_htcx/role/left_knee", "/user/vive_tracker_htcx/role/left_knee/input/trackpad", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad", "/user/vive_tracker_htcx/role/right_knee", "/user/vive_tracker_htcx/role/right_knee/input/trackpad", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad", "/user/vive_tracker_htcx/role/waist", "/user/vive_tracker_htcx/role/waist/input/trackpad", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad", "/user/vive_tracker_htcx/role/chest", "/user/vive_tracker_htcx/role/chest/input/trackpad", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad", "/user/vive_tracker_htcx/role/camera", "/user/vive_tracker_htcx/role/camera/input/trackpad", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad", "/user/vive_tracker_htcx/role/keyboard", "/user/vive_tracker_htcx/role/keyboard/input/trackpad", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-
-	// metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad click", "/user/vive_tracker_htcx/role/handheld_object", "/user/vive_tracker_htcx/role/handheld_object/input/trackpad/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad click", "/user/vive_tracker_htcx/role/left_foot", "/user/vive_tracker_htcx/role/left_foot/input/trackpad/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad click", "/user/vive_tracker_htcx/role/right_foot", "/user/vive_tracker_htcx/role/right_foot/input/trackpad/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad click", "/user/vive_tracker_htcx/role/left_shoulder", "/user/vive_tracker_htcx/role/left_shoulder/input/trackpad/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad click", "/user/vive_tracker_htcx/role/right_shoulder", "/user/vive_tracker_htcx/role/right_shoulder/input/trackpad/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad click", "/user/vive_tracker_htcx/role/left_elbow", "/user/vive_tracker_htcx/role/left_elbow/input/trackpad/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad click", "/user/vive_tracker_htcx/role/right_elbow", "/user/vive_tracker_htcx/role/right_elbow/input/trackpad/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad click", "/user/vive_tracker_htcx/role/left_knee", "/user/vive_tracker_htcx/role/left_knee/input/trackpad/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad click", "/user/vive_tracker_htcx/role/right_knee", "/user/vive_tracker_htcx/role/right_knee/input/trackpad/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad click", "/user/vive_tracker_htcx/role/waist", "/user/vive_tracker_htcx/role/waist/input/trackpad/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad click", "/user/vive_tracker_htcx/role/chest", "/user/vive_tracker_htcx/role/chest/input/trackpad/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad click", "/user/vive_tracker_htcx/role/camera", "/user/vive_tracker_htcx/role/camera/input/trackpad/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad click", "/user/vive_tracker_htcx/role/keyboard", "/user/vive_tracker_htcx/role/keyboard/input/trackpad/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-
-	// metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad touch", "/user/vive_tracker_htcx/role/handheld_object", "/user/vive_tracker_htcx/role/handheld_object/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad touch", "/user/vive_tracker_htcx/role/left_foot", "/user/vive_tracker_htcx/role/left_foot/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad touch", "/user/vive_tracker_htcx/role/right_foot", "/user/vive_tracker_htcx/role/right_foot/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad touch", "/user/vive_tracker_htcx/role/left_shoulder", "/user/vive_tracker_htcx/role/left_shoulder/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad touch", "/user/vive_tracker_htcx/role/right_shoulder", "/user/vive_tracker_htcx/role/right_shoulder/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad touch", "/user/vive_tracker_htcx/role/left_elbow", "/user/vive_tracker_htcx/role/left_elbow/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad touch", "/user/vive_tracker_htcx/role/right_elbow", "/user/vive_tracker_htcx/role/right_elbow/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad touch", "/user/vive_tracker_htcx/role/left_knee", "/user/vive_tracker_htcx/role/left_knee/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad touch", "/user/vive_tracker_htcx/role/right_knee", "/user/vive_tracker_htcx/role/right_knee/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad touch", "/user/vive_tracker_htcx/role/waist", "/user/vive_tracker_htcx/role/waist/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad touch", "/user/vive_tracker_htcx/role/chest", "/user/vive_tracker_htcx/role/chest/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad touch", "/user/vive_tracker_htcx/role/camera", "/user/vive_tracker_htcx/role/camera/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Trackpad touch", "/user/vive_tracker_htcx/role/keyboard", "/user/vive_tracker_htcx/role/keyboard/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-
-	// register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Grip pose", "/user/vive_tracker_htcx/role/handheld_object", "/user/vive_tracker_htcx/role/handheld_object/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Grip pose", "/user/vive_tracker_htcx/role/left_foot", "/user/vive_tracker_htcx/role/left_foot/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Grip pose", "/user/vive_tracker_htcx/role/right_foot", "/user/vive_tracker_htcx/role/right_foot/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Grip pose", "/user/vive_tracker_htcx/role/left_shoulder", "/user/vive_tracker_htcx/role/left_shoulder/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Grip pose", "/user/vive_tracker_htcx/role/right_shoulder", "/user/vive_tracker_htcx/role/right_shoulder/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Grip pose", "/user/vive_tracker_htcx/role/left_elbow", "/user/vive_tracker_htcx/role/left_elbow/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Grip pose", "/user/vive_tracker_htcx/role/right_elbow", "/user/vive_tracker_htcx/role/right_elbow/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Grip pose", "/user/vive_tracker_htcx/role/left_knee", "/user/vive_tracker_htcx/role/left_knee/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Grip pose", "/user/vive_tracker_htcx/role/right_knee", "/user/vive_tracker_htcx/role/right_knee/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Grip pose", "/user/vive_tracker_htcx/role/waist", "/user/vive_tracker_htcx/role/waist/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Grip pose", "/user/vive_tracker_htcx/role/chest", "/user/vive_tracker_htcx/role/chest/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Grip pose", "/user/vive_tracker_htcx/role/camera", "/user/vive_tracker_htcx/role/camera/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Grip pose", "/user/vive_tracker_htcx/role/keyboard", "/user/vive_tracker_htcx/role/keyboard/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-
-	// metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Haptic output", "/user/vive_tracker_htcx/role/handheld_object", "/user/vive_tracker_htcx/role/handheld_object/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Haptic output", "/user/vive_tracker_htcx/role/left_foot", "/user/vive_tracker_htcx/role/left_foot/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Haptic output", "/user/vive_tracker_htcx/role/right_foot", "/user/vive_tracker_htcx/role/right_foot/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Haptic output", "/user/vive_tracker_htcx/role/left_shoulder", "/user/vive_tracker_htcx/role/left_shoulder/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Haptic output", "/user/vive_tracker_htcx/role/right_shoulder", "/user/vive_tracker_htcx/role/right_shoulder/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Haptic output", "/user/vive_tracker_htcx/role/left_elbow", "/user/vive_tracker_htcx/role/left_elbow/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Haptic output", "/user/vive_tracker_htcx/role/right_elbow", "/user/vive_tracker_htcx/role/right_elbow/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Haptic output", "/user/vive_tracker_htcx/role/left_knee", "/user/vive_tracker_htcx/role/left_knee/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Haptic output", "/user/vive_tracker_htcx/role/right_knee", "/user/vive_tracker_htcx/role/right_knee/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Haptic output", "/user/vive_tracker_htcx/role/waist", "/user/vive_tracker_htcx/role/waist/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Haptic output", "/user/vive_tracker_htcx/role/chest", "/user/vive_tracker_htcx/role/chest/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Haptic output", "/user/vive_tracker_htcx/role/camera", "/user/vive_tracker_htcx/role/camera/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	metadata->register_io_path("/interaction_profiles/htc/vive_tracker_htcx", "Haptic output", "/user/vive_tracker_htcx/role/keyboard", "/user/vive_tracker_htcx/role/keyboard/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+			metadata->register_io_path(profile_path, "Haptic output", user_path, user_path + "/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+		}
+	}
 }
 
 bool OpenXRHTCViveTrackerExtension::on_event_polled(const XrEventDataBuffer &event) {
diff --git a/modules/openxr/extensions/openxr_huawei_controller_extension.cpp b/modules/openxr/extensions/openxr_huawei_controller_extension.cpp
index baa6d6ded8..844ce22d20 100644
--- a/modules/openxr/extensions/openxr_huawei_controller_extension.cpp
+++ b/modules/openxr/extensions/openxr_huawei_controller_extension.cpp
@@ -48,37 +48,33 @@ void OpenXRHuaweiControllerExtension::on_register_metadata() {
 	OpenXRInteractionProfileMetadata *metadata = OpenXRInteractionProfileMetadata::get_singleton();
 	ERR_FAIL_NULL(metadata);
 
-	// Huawei controller
-	metadata->register_interaction_profile("Huawei controller", "/interaction_profiles/huawei/controller", XR_HUAWEI_CONTROLLER_INTERACTION_EXTENSION_NAME);
-	metadata->register_io_path("/interaction_profiles/huawei/controller", "Grip pose", "/user/hand/left", "/user/hand/left/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/huawei/controller", "Grip pose", "/user/hand/right", "/user/hand/right/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/huawei/controller", "Aim pose", "/user/hand/left", "/user/hand/left/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/huawei/controller", "Aim pose", "/user/hand/right", "/user/hand/right/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/huawei/controller", "Palm pose", "/user/hand/left", "/user/hand/left/input/palm_ext/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/huawei/controller", "Palm pose", "/user/hand/right", "/user/hand/right/input/palm_ext/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+	{ // Huawei controller
+		const String profile_path = "/interaction_profiles/huawei/controller";
+		metadata->register_interaction_profile("Huawei controller", profile_path, XR_HUAWEI_CONTROLLER_INTERACTION_EXTENSION_NAME);
+		for (const String user_path : { "/user/hand/left", "/user/hand/right" }) {
+			metadata->register_io_path(profile_path, "Grip pose", user_path, user_path + "/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			metadata->register_io_path(profile_path, "Aim pose", user_path, user_path + "/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			metadata->register_io_path(profile_path, "Palm pose", user_path, user_path + "/input/palm_ext/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
 
-	metadata->register_io_path("/interaction_profiles/huawei/controller", "Home click", "/user/hand/left", "/user/hand/left/input/home/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/huawei/controller", "Home click", "/user/hand/right", "/user/hand/right/input/home/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/huawei/controller", "Back click", "/user/hand/left", "/user/hand/left/input/back/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/huawei/controller", "Back click", "/user/hand/right", "/user/hand/right/input/back/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Home click", user_path, user_path + "/input/home/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Back click", user_path, user_path + "/input/back/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/huawei/controller", "Volume up click", "/user/hand/left", "/user/hand/left/input/volume_up/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/huawei/controller", "Volume up click", "/user/hand/right", "/user/hand/right/input/volume_up/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/huawei/controller", "Volume down click", "/user/hand/left", "/user/hand/left/input/volume_down/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/huawei/controller", "Volume down click", "/user/hand/right", "/user/hand/right/input/volume_down/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Volume up click", user_path, user_path + "/input/volume_up/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Volume down click", user_path, user_path + "/input/volume_down/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/huawei/controller", "Trigger", "/user/hand/left", "/user/hand/left/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/huawei/controller", "Trigger click", "/user/hand/left", "/user/hand/left/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/huawei/controller", "Trigger", "/user/hand/right", "/user/hand/right/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/huawei/controller", "Trigger click", "/user/hand/right", "/user/hand/right/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trigger", user_path, user_path + "/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Trigger click", user_path, user_path + "/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/huawei/controller", "Trackpad", "/user/hand/left", "/user/hand/left/input/trackpad", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/huawei/controller", "Trackpad click", "/user/hand/left", "/user/hand/left/input/trackpad/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/huawei/controller", "Trackpad touch", "/user/hand/left", "/user/hand/left/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/huawei/controller", "Trackpad", "/user/hand/right", "/user/hand/right/input/trackpad", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/huawei/controller", "Trackpad click", "/user/hand/right", "/user/hand/right/input/trackpad/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/huawei/controller", "Trackpad touch", "/user/hand/right", "/user/hand/right/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trackpad", user_path, user_path + "/input/trackpad", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
+			metadata->register_io_path(profile_path, "Trackpad click", user_path, user_path + "/input/trackpad/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trackpad touch", user_path, user_path + "/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trackpad Dpad Up", user_path, user_path + "/input/trackpad/dpad_up", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trackpad Dpad Down", user_path, user_path + "/input/trackpad/dpad_down", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trackpad Dpad Left", user_path, user_path + "/input/trackpad/dpad_left", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trackpad Dpad Right", user_path, user_path + "/input/trackpad/dpad_right", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trackpad Dpad Center", user_path, user_path + "/input/trackpad/dpad_center", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/huawei/controller", "Haptic output", "/user/hand/left", "/user/hand/left/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	metadata->register_io_path("/interaction_profiles/huawei/controller", "Haptic output", "/user/hand/right", "/user/hand/right/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+			metadata->register_io_path(profile_path, "Haptic output", user_path, user_path + "/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+		}
+	}
 }
diff --git a/modules/openxr/extensions/openxr_meta_controller_extension.cpp b/modules/openxr/extensions/openxr_meta_controller_extension.cpp
index 72dc74bf64..ad62f584c1 100644
--- a/modules/openxr/extensions/openxr_meta_controller_extension.cpp
+++ b/modules/openxr/extensions/openxr_meta_controller_extension.cpp
@@ -52,133 +52,110 @@ void OpenXRMetaControllerExtension::on_register_metadata() {
 
 	// Note, we register controllers regardless if they are supported on the current hardware.
 
-	// Normal touch controller is part of the core spec, but we do have some extensions.
-	metadata->register_io_path("/interaction_profiles/oculus/touch_controller", "Trigger proximity", "/user/hand/left", "/user/hand/left/input/trigger/proximity_fb ", "XR_FB_touch_controller_proximity", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/oculus/touch_controller", "Trigger proximity", "/user/hand/right", "/user/hand/right/input/trigger/proximity_fb ", "XR_FB_touch_controller_proximity", OpenXRAction::OPENXR_ACTION_BOOL);
+	{ // Normal touch controller is part of the core spec, but we do have some extensions.
+		const String profile_path = "/interaction_profiles/oculus/touch_controller";
+		for (const String user_path : { "/user/hand/left", "/user/hand/right" }) {
+			metadata->register_io_path(profile_path, "Trigger proximity", user_path, user_path + "/input/trigger/proximity_fb ", "XR_FB_touch_controller_proximity", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumb proximity", user_path, user_path + "/input/thumb_fb/proximity_fb ", "XR_FB_touch_controller_proximity", OpenXRAction::OPENXR_ACTION_BOOL);
+		}
+	}
 
-	metadata->register_io_path("/interaction_profiles/oculus/touch_controller", "Thumb proximity", "/user/hand/left", "/user/hand/left/input/thumb_fb/proximity_fb ", "XR_FB_touch_controller_proximity", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/oculus/touch_controller", "Thumb proximity", "/user/hand/right", "/user/hand/right/input/thumb_fb/proximity_fb ", "XR_FB_touch_controller_proximity", OpenXRAction::OPENXR_ACTION_BOOL);
+	{ // Touch controller pro (Quest Pro)
+		const String profile_path = "/interaction_profiles/facebook/touch_controller_pro";
+		metadata->register_interaction_profile("Touch controller pro", profile_path, "XR_FB_touch_controller_pro");
+		for (const String user_path : { "/user/hand/left", "/user/hand/right" }) {
+			metadata->register_io_path(profile_path, "Grip pose", user_path, user_path + "/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			metadata->register_io_path(profile_path, "Aim pose", user_path, user_path + "/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			metadata->register_io_path(profile_path, "Palm pose", user_path, user_path + "/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
 
-	// Touch controller pro (Quest Pro)
-	metadata->register_interaction_profile("Touch controller pro", "/interaction_profiles/facebook/touch_controller_pro", "XR_FB_touch_controller_pro");
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Grip pose", "/user/hand/left", "/user/hand/left/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Grip pose", "/user/hand/right", "/user/hand/right/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Aim pose", "/user/hand/left", "/user/hand/left/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Aim pose", "/user/hand/right", "/user/hand/right/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Palm pose", "/user/hand/left", "/user/hand/left/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Palm pose", "/user/hand/right", "/user/hand/right/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
+			metadata->register_io_path(profile_path, "Trigger", user_path, user_path + "/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Trigger touch", user_path, user_path + "/input/trigger/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trigger proximity", user_path, user_path + "/input/trigger/proximity_fb", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trigger curl", user_path, user_path + "/input/trigger/curl_fb", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Trigger slide", user_path, user_path + "/input/trigger/slide_fb", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Trigger force", user_path, user_path + "/input/trigger/force", "", OpenXRAction::OPENXR_ACTION_FLOAT);
 
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Menu click", "/user/hand/left", "/user/hand/left/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "System click", "/user/hand/right", "/user/hand/right/input/system/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Squeeze", user_path, user_path + "/input/squeeze/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
 
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "X click", "/user/hand/left", "/user/hand/left/input/x/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "X touch", "/user/hand/left", "/user/hand/left/input/x/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Y click", "/user/hand/left", "/user/hand/left/input/y/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Y touch", "/user/hand/left", "/user/hand/left/input/y/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "A click", "/user/hand/right", "/user/hand/right/input/a/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "A touch", "/user/hand/right", "/user/hand/right/input/a/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "B click", "/user/hand/right", "/user/hand/right/input/b/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "B touch", "/user/hand/right", "/user/hand/right/input/b/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumb proximity", user_path, user_path + "/input/thumb_fb/proximity_fb", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Trigger", "/user/hand/left", "/user/hand/left/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Trigger touch", "/user/hand/left", "/user/hand/left/input/trigger/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Trigger proximity", "/user/hand/left", "/user/hand/left/input/trigger/proximity_fb", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Trigger curl", "/user/hand/left", "/user/hand/left/input/trigger/curl_fb", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Trigger slide", "/user/hand/left", "/user/hand/left/input/trigger/slide_fb", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Trigger force", "/user/hand/left", "/user/hand/left/input/trigger/force", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Trigger", "/user/hand/right", "/user/hand/right/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Trigger touch", "/user/hand/right", "/user/hand/right/input/trigger/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Trigger proximity", "/user/hand/right", "/user/hand/right/input/trigger/proximity_fb", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Trigger curl", "/user/hand/right", "/user/hand/right/input/trigger/curl_fb", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Trigger slide", "/user/hand/right", "/user/hand/right/input/trigger/slide_fb", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Trigger force", "/user/hand/right", "/user/hand/right/input/trigger/force", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Thumbstick", user_path, user_path + "/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
+			metadata->register_io_path(profile_path, "Thumbstick X", user_path, user_path + "/input/thumbstick/x", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Thumbstick Y", user_path, user_path + "/input/thumbstick/y", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Thumbstick click", user_path, user_path + "/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick touch", user_path, user_path + "/input/thumbstick/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Up", user_path, user_path + "/input/thumbstick/dpad_up", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Down", user_path, user_path + "/input/thumbstick/dpad_down", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Left", user_path, user_path + "/input/thumbstick/dpad_left", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Right", user_path, user_path + "/input/thumbstick/dpad_right", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Squeeze", "/user/hand/left", "/user/hand/left/input/squeeze/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Squeeze", "/user/hand/right", "/user/hand/right/input/squeeze/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Thumbrest touch", user_path, user_path + "/input/thumbrest/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbrest force", user_path, user_path + "/input/thumbrest/force", "", OpenXRAction::OPENXR_ACTION_FLOAT);
 
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Thumb proximity", "/user/hand/left", "/user/hand/left/input/thumb_fb/proximity_fb", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Thumb proximity", "/user/hand/right", "/user/hand/right/input/thumb_fb/proximity_fb", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Stylus force", user_path, user_path + "/input/stylus_fb/force", "", OpenXRAction::OPENXR_ACTION_FLOAT);
 
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Thumbstick", "/user/hand/left", "/user/hand/left/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Thumbstick X", "/user/hand/left", "/user/hand/left/input/thumbstick/x", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Thumbstick Y", "/user/hand/left", "/user/hand/left/input/thumbstick/y", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Thumbstick click", "/user/hand/left", "/user/hand/left/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Thumbstick touch", "/user/hand/left", "/user/hand/left/input/thumbstick/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Thumbstick", "/user/hand/right", "/user/hand/right/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Thumbstick X", "/user/hand/right", "/user/hand/right/input/thumbstick/x", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Thumbstick Y", "/user/hand/right", "/user/hand/right/input/thumbstick/y", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Thumbstick click", "/user/hand/right", "/user/hand/right/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Thumbstick touch", "/user/hand/right", "/user/hand/right/input/thumbstick/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Haptic output", user_path, user_path + "/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+			metadata->register_io_path(profile_path, "Haptic trigger output", user_path, user_path + "/output/haptic_trigger_fb", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+			metadata->register_io_path(profile_path, "Haptic thumb output", user_path, user_path + "/output/haptic_thumb_fb", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+		}
 
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Thumbrest touch", "/user/hand/left", "/user/hand/left/input/thumbrest/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Thumbrest force", "/user/hand/left", "/user/hand/left/input/thumbrest/force", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Thumbrest touch", "/user/hand/right", "/user/hand/right/input/thumbrest/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Thumbrest force", "/user/hand/right", "/user/hand/right/input/thumbrest/force", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+		metadata->register_io_path(profile_path, "Menu click", "/user/hand/left", "/user/hand/left/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "System click", "/user/hand/right", "/user/hand/right/input/system/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Stylus force", "/user/hand/left", "/user/hand/left/input/stylus_fb/force", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Stylus force", "/user/hand/right", "/user/hand/right/input/stylus_fb/force", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+		metadata->register_io_path(profile_path, "X click", "/user/hand/left", "/user/hand/left/input/x/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "X touch", "/user/hand/left", "/user/hand/left/input/x/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "Y click", "/user/hand/left", "/user/hand/left/input/y/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "Y touch", "/user/hand/left", "/user/hand/left/input/y/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "A click", "/user/hand/right", "/user/hand/right/input/a/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "A touch", "/user/hand/right", "/user/hand/right/input/a/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "B click", "/user/hand/right", "/user/hand/right/input/b/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "B touch", "/user/hand/right", "/user/hand/right/input/b/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+	}
 
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Haptic output", "/user/hand/left", "/user/hand/left/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Haptic trigger output", "/user/hand/left", "/user/hand/left/output/haptic_trigger_fb", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Haptic thumb output", "/user/hand/left", "/user/hand/left/output/haptic_thumb_fb", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Haptic output", "/user/hand/right", "/user/hand/right/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Haptic trigger output", "/user/hand/right", "/user/hand/right/output/haptic_trigger_fb", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	metadata->register_io_path("/interaction_profiles/facebook/touch_controller_pro", "Haptic thumb output", "/user/hand/right", "/user/hand/right/output/haptic_thumb_fb", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+	{ // Touch controller plus (Quest 3)
+		const String profile_path = "/interaction_profiles/meta/touch_controller_plus";
+		metadata->register_interaction_profile("Touch controller plus", profile_path, "XR_META_touch_controller_plus");
+		for (const String user_path : { "/user/hand/left", "/user/hand/right" }) {
+			metadata->register_io_path(profile_path, "Grip pose", user_path, user_path + "/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			metadata->register_io_path(profile_path, "Aim pose", user_path, user_path + "/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			metadata->register_io_path(profile_path, "Palm pose", user_path, user_path + "/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
 
-	// Touch controller plus (Quest 3)
-	metadata->register_interaction_profile("Touch controller plus", "/interaction_profiles/meta/touch_controller_plus", "XR_META_touch_controller_plus");
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Grip pose", "/user/hand/left", "/user/hand/left/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Grip pose", "/user/hand/right", "/user/hand/right/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Aim pose", "/user/hand/left", "/user/hand/left/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Aim pose", "/user/hand/right", "/user/hand/right/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Palm pose", "/user/hand/left", "/user/hand/left/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Palm pose", "/user/hand/right", "/user/hand/right/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
+			metadata->register_io_path(profile_path, "Trigger", user_path, user_path + "/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Trigger touch", user_path, user_path + "/input/trigger/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trigger proximity", user_path, user_path + "/input/trigger/proximity_meta", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trigger curl", user_path, user_path + "/input/trigger/curl_meta", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Trigger slide", user_path, user_path + "/input/trigger/slide_meta", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Trigger force", user_path, user_path + "/input/trigger/force", "", OpenXRAction::OPENXR_ACTION_FLOAT);
 
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Menu click", "/user/hand/left", "/user/hand/left/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "System click", "/user/hand/right", "/user/hand/right/input/system/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Squeeze", user_path, user_path + "/input/squeeze/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
 
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "X click", "/user/hand/left", "/user/hand/left/input/x/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "X touch", "/user/hand/left", "/user/hand/left/input/x/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Y click", "/user/hand/left", "/user/hand/left/input/y/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Y touch", "/user/hand/left", "/user/hand/left/input/y/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "A click", "/user/hand/right", "/user/hand/right/input/a/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "A touch", "/user/hand/right", "/user/hand/right/input/a/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "B click", "/user/hand/right", "/user/hand/right/input/b/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "B touch", "/user/hand/right", "/user/hand/right/input/b/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumb proximity", user_path, user_path + "/input/thumb_meta/proximity_meta", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Trigger", "/user/hand/left", "/user/hand/left/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Trigger touch", "/user/hand/left", "/user/hand/left/input/trigger/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Trigger proximity", "/user/hand/left", "/user/hand/left/input/trigger/proximity_meta", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Trigger curl", "/user/hand/left", "/user/hand/left/input/trigger/curl_meta", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Trigger slide", "/user/hand/left", "/user/hand/left/input/trigger/slide_meta", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Trigger force", "/user/hand/left", "/user/hand/left/input/trigger/force", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Trigger", "/user/hand/right", "/user/hand/right/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Trigger touch", "/user/hand/right", "/user/hand/right/input/trigger/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Trigger proximity", "/user/hand/right", "/user/hand/right/input/trigger/proximity_meta", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Trigger curl", "/user/hand/right", "/user/hand/right/input/trigger/curl_meta", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Trigger slide", "/user/hand/right", "/user/hand/right/input/trigger/slide_meta", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Trigger force", "/user/hand/right", "/user/hand/right/input/trigger/force", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Thumbstick", user_path, user_path + "/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
+			metadata->register_io_path(profile_path, "Thumbstick X", user_path, user_path + "/input/thumbstick/x", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Thumbstick Y", user_path, user_path + "/input/thumbstick/y", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Thumbstick click", user_path, user_path + "/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick touch", user_path, user_path + "/input/thumbstick/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Up", user_path, user_path + "/input/thumbstick/dpad_up", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Down", user_path, user_path + "/input/thumbstick/dpad_down", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Left", user_path, user_path + "/input/thumbstick/dpad_left", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Right", user_path, user_path + "/input/thumbstick/dpad_right", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Squeeze", "/user/hand/left", "/user/hand/left/input/squeeze/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Squeeze", "/user/hand/right", "/user/hand/right/input/squeeze/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Thumbrest touch", user_path, user_path + "/input/thumbrest/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Thumb proximity", "/user/hand/left", "/user/hand/left/input/thumb_meta/proximity_meta", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Thumb proximity", "/user/hand/right", "/user/hand/right/input/thumb_meta/proximity_meta", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Haptic output", user_path, user_path + "/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+		}
 
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Thumbstick", "/user/hand/left", "/user/hand/left/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Thumbstick X", "/user/hand/left", "/user/hand/left/input/thumbstick/x", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Thumbstick Y", "/user/hand/left", "/user/hand/left/input/thumbstick/y", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Thumbstick click", "/user/hand/left", "/user/hand/left/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Thumbstick touch", "/user/hand/left", "/user/hand/left/input/thumbstick/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Thumbstick", "/user/hand/right", "/user/hand/right/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Thumbstick X", "/user/hand/right", "/user/hand/right/input/thumbstick/x", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Thumbstick Y", "/user/hand/right", "/user/hand/right/input/thumbstick/y", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Thumbstick click", "/user/hand/right", "/user/hand/right/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Thumbstick touch", "/user/hand/right", "/user/hand/right/input/thumbstick/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "Menu click", "/user/hand/left", "/user/hand/left/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "System click", "/user/hand/right", "/user/hand/right/input/system/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Thumbrest touch", "/user/hand/left", "/user/hand/left/input/thumbrest/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Thumbrest touch", "/user/hand/right", "/user/hand/right/input/thumbrest/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Haptic output", "/user/hand/left", "/user/hand/left/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	metadata->register_io_path("/interaction_profiles/meta/touch_controller_plus", "Haptic output", "/user/hand/right", "/user/hand/right/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+		metadata->register_io_path(profile_path, "X click", "/user/hand/left", "/user/hand/left/input/x/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "X touch", "/user/hand/left", "/user/hand/left/input/x/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "Y click", "/user/hand/left", "/user/hand/left/input/y/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "Y touch", "/user/hand/left", "/user/hand/left/input/y/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "A click", "/user/hand/right", "/user/hand/right/input/a/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "A touch", "/user/hand/right", "/user/hand/right/input/a/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "B click", "/user/hand/right", "/user/hand/right/input/b/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "B touch", "/user/hand/right", "/user/hand/right/input/b/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+	}
 }
diff --git a/modules/openxr/extensions/openxr_ml2_controller_extension.cpp b/modules/openxr/extensions/openxr_ml2_controller_extension.cpp
index 979ac22d08..f36273658d 100644
--- a/modules/openxr/extensions/openxr_ml2_controller_extension.cpp
+++ b/modules/openxr/extensions/openxr_ml2_controller_extension.cpp
@@ -50,7 +50,7 @@ void OpenXRML2ControllerExtension::on_register_metadata() {
 
 	// Magic Leap 2 Controller
 	const String profile_path = "/interaction_profiles/ml/ml2_controller";
-	metadata->register_interaction_profile("Magic Leap 2 controller", "/interaction_profiles/ml/ml2_controller", XR_ML_ML2_CONTROLLER_INTERACTION_EXTENSION_NAME);
+	metadata->register_interaction_profile("Magic Leap 2 controller", profile_path, XR_ML_ML2_CONTROLLER_INTERACTION_EXTENSION_NAME);
 	for (const String user_path : { "/user/hand/left", "/user/hand/right" }) {
 		metadata->register_io_path(profile_path, "Grip pose", user_path, user_path + "/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
 		metadata->register_io_path(profile_path, "Aim pose", user_path, user_path + "/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
@@ -66,6 +66,11 @@ void OpenXRML2ControllerExtension::on_register_metadata() {
 		metadata->register_io_path(profile_path, "Trackpad X", user_path, user_path + "/input/trackpad/x", "", OpenXRAction::OPENXR_ACTION_FLOAT);
 		metadata->register_io_path(profile_path, "Trackpad Y", user_path, user_path + "/input/trackpad/y", "", OpenXRAction::OPENXR_ACTION_FLOAT);
 		metadata->register_io_path(profile_path, "Trackpad touch", user_path, user_path + "/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
+		metadata->register_io_path(profile_path, "Trackpad Dpad Up", user_path, user_path + "/input/trackpad/dpad_up", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "Trackpad Dpad Down", user_path, user_path + "/input/trackpad/dpad_down", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "Trackpad Dpad Left", user_path, user_path + "/input/trackpad/dpad_left", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "Trackpad Dpad Right", user_path, user_path + "/input/trackpad/dpad_right", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "Trackpad Dpad Center", user_path, user_path + "/input/trackpad/dpad_center", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
 
 		metadata->register_io_path(profile_path, "Haptic output", user_path, user_path + "/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
 	}
diff --git a/modules/openxr/extensions/openxr_pico_controller_extension.cpp b/modules/openxr/extensions/openxr_pico_controller_extension.cpp
index f2901d49f7..b4419d103d 100644
--- a/modules/openxr/extensions/openxr_pico_controller_extension.cpp
+++ b/modules/openxr/extensions/openxr_pico_controller_extension.cpp
@@ -55,85 +55,79 @@ void OpenXRPicoControllerExtension::on_register_metadata() {
 	// Make sure we switch to our new name.
 	metadata->register_profile_rename("/interaction_profiles/pico/neo3_controller", "/interaction_profiles/bytedance/pico_neo3_controller");
 
-	// Pico neo 3 controller.
-	metadata->register_interaction_profile("Pico Neo3 controller", "/interaction_profiles/bytedance/pico_neo3_controller", XR_BD_CONTROLLER_INTERACTION_EXTENSION_NAME);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "Grip pose", "/user/hand/left", "/user/hand/left/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "Grip pose", "/user/hand/right", "/user/hand/right/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "Aim pose", "/user/hand/left", "/user/hand/left/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "Aim pose", "/user/hand/right", "/user/hand/right/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "Palm pose", "/user/hand/left", "/user/hand/left/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "Palm pose", "/user/hand/right", "/user/hand/right/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
+	{ // Pico neo 3 controller.
+		const String profile_path = "/interaction_profiles/bytedance/pico_neo3_controller";
+		metadata->register_interaction_profile("Pico Neo3 controller", profile_path, XR_BD_CONTROLLER_INTERACTION_EXTENSION_NAME);
+		for (const String user_path : { "/user/hand/left", "/user/hand/right" }) {
+			metadata->register_io_path(profile_path, "Grip pose", user_path, user_path + "/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			metadata->register_io_path(profile_path, "Aim pose", user_path, user_path + "/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			metadata->register_io_path(profile_path, "Palm pose", user_path, user_path + "/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
 
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "Menu click", "/user/hand/left", "/user/hand/left/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "Menu click", "/user/hand/right", "/user/hand/right/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "System click", "/user/hand/left", "/user/hand/left/input/system/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "System click", "/user/hand/right", "/user/hand/right/input/system/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Menu click", user_path, user_path + "/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "System click", user_path, user_path + "/input/system/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "X click", "/user/hand/left", "/user/hand/left/input/x/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "X touch", "/user/hand/left", "/user/hand/left/input/x/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "Y click", "/user/hand/left", "/user/hand/left/input/y/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "Y touch", "/user/hand/left", "/user/hand/left/input/y/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "A click", "/user/hand/right", "/user/hand/right/input/a/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "A touch", "/user/hand/right", "/user/hand/right/input/a/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "B click", "/user/hand/right", "/user/hand/right/input/b/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "B touch", "/user/hand/right", "/user/hand/right/input/b/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trigger", user_path, user_path + "/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Trigger touch", user_path, user_path + "/input/trigger/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "Trigger", "/user/hand/left", "/user/hand/left/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "Trigger touch", "/user/hand/left", "/user/hand/left/input/trigger/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "Trigger", "/user/hand/right", "/user/hand/right/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "Trigger touch", "/user/hand/right", "/user/hand/right/input/trigger/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Squeeze", user_path, user_path + "/input/squeeze/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
 
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "Squeeze", "/user/hand/left", "/user/hand/left/input/squeeze/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "Squeeze", "/user/hand/right", "/user/hand/right/input/squeeze/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Thumbstick", user_path, user_path + "/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
+			metadata->register_io_path(profile_path, "Thumbstick click", user_path, user_path + "/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick touch", user_path, user_path + "/input/thumbstick/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Up", user_path, user_path + "/input/thumbstick/dpad_up", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Down", user_path, user_path + "/input/thumbstick/dpad_down", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Left", user_path, user_path + "/input/thumbstick/dpad_left", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Right", user_path, user_path + "/input/thumbstick/dpad_right", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "Thumbstick", "/user/hand/left", "/user/hand/left/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "Thumbstick click", "/user/hand/left", "/user/hand/left/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "Thumbstick touch", "/user/hand/left", "/user/hand/left/input/thumbstick/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "Thumbstick", "/user/hand/right", "/user/hand/right/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "Thumbstick click", "/user/hand/right", "/user/hand/right/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "Thumbstick touch", "/user/hand/right", "/user/hand/right/input/thumbstick/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Haptic output", user_path, user_path + "/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+		}
 
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "Haptic output", "/user/hand/left", "/user/hand/left/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico_neo3_controller", "Haptic output", "/user/hand/right", "/user/hand/right/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+		metadata->register_io_path(profile_path, "X click", "/user/hand/left", "/user/hand/left/input/x/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "X touch", "/user/hand/left", "/user/hand/left/input/x/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "Y click", "/user/hand/left", "/user/hand/left/input/y/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "Y touch", "/user/hand/left", "/user/hand/left/input/y/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "A click", "/user/hand/right", "/user/hand/right/input/a/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "A touch", "/user/hand/right", "/user/hand/right/input/a/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "B click", "/user/hand/right", "/user/hand/right/input/b/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "B touch", "/user/hand/right", "/user/hand/right/input/b/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+	}
 
-	// Pico 4 controller.
-	metadata->register_interaction_profile("Pico 4 controller", "/interaction_profiles/bytedance/pico4_controller", XR_BD_CONTROLLER_INTERACTION_EXTENSION_NAME);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "Grip pose", "/user/hand/left", "/user/hand/left/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "Grip pose", "/user/hand/right", "/user/hand/right/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "Aim pose", "/user/hand/left", "/user/hand/left/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "Aim pose", "/user/hand/right", "/user/hand/right/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "Palm pose", "/user/hand/left", "/user/hand/left/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "Palm pose", "/user/hand/right", "/user/hand/right/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
+	{ // Pico 4 controller.
+		const String profile_path = "/interaction_profiles/bytedance/pico4_controller";
+		metadata->register_interaction_profile("Pico 4 controller", profile_path, XR_BD_CONTROLLER_INTERACTION_EXTENSION_NAME);
+		for (const String user_path : { "/user/hand/left", "/user/hand/right" }) {
+			metadata->register_io_path(profile_path, "Grip pose", user_path, user_path + "/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			metadata->register_io_path(profile_path, "Aim pose", user_path, user_path + "/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			metadata->register_io_path(profile_path, "Palm pose", user_path, user_path + "/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
 
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "Menu click", "/user/hand/left", "/user/hand/left/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	// Note, no menu on right controller!
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "System click", "/user/hand/left", "/user/hand/left/input/system/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "System click", "/user/hand/right", "/user/hand/right/input/system/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "System click", user_path, user_path + "/input/system/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "X click", "/user/hand/left", "/user/hand/left/input/x/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "X touch", "/user/hand/left", "/user/hand/left/input/x/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "Y click", "/user/hand/left", "/user/hand/left/input/y/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "Y touch", "/user/hand/left", "/user/hand/left/input/y/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "A click", "/user/hand/right", "/user/hand/right/input/a/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "A touch", "/user/hand/right", "/user/hand/right/input/a/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "B click", "/user/hand/right", "/user/hand/right/input/b/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "B touch", "/user/hand/right", "/user/hand/right/input/b/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trigger", user_path, user_path + "/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Trigger touch", user_path, user_path + "/input/trigger/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "Trigger", "/user/hand/left", "/user/hand/left/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "Trigger touch", "/user/hand/left", "/user/hand/left/input/trigger/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "Trigger", "/user/hand/right", "/user/hand/right/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "Trigger touch", "/user/hand/right", "/user/hand/right/input/trigger/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Squeeze", user_path, user_path + "/input/squeeze/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
 
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "Squeeze", "/user/hand/left", "/user/hand/left/input/squeeze/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "Squeeze", "/user/hand/right", "/user/hand/right/input/squeeze/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Thumbstick", user_path, user_path + "/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
+			metadata->register_io_path(profile_path, "Thumbstick click", user_path, user_path + "/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick touch", user_path, user_path + "/input/thumbstick/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Up", user_path, user_path + "/input/thumbstick/dpad_up", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Down", user_path, user_path + "/input/thumbstick/dpad_down", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Left", user_path, user_path + "/input/thumbstick/dpad_left", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Right", user_path, user_path + "/input/thumbstick/dpad_right", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "Thumbstick", "/user/hand/left", "/user/hand/left/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "Thumbstick click", "/user/hand/left", "/user/hand/left/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "Thumbstick touch", "/user/hand/left", "/user/hand/left/input/thumbstick/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "Thumbstick", "/user/hand/right", "/user/hand/right/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "Thumbstick click", "/user/hand/right", "/user/hand/right/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "Thumbstick touch", "/user/hand/right", "/user/hand/right/input/thumbstick/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Haptic output", user_path, user_path + "/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+		}
 
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "Haptic output", "/user/hand/left", "/user/hand/left/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	metadata->register_io_path("/interaction_profiles/bytedance/pico4_controller", "Haptic output", "/user/hand/right", "/user/hand/right/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+		metadata->register_io_path(profile_path, "Menu click", "/user/hand/left", "/user/hand/left/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		// Note, no menu on right controller!
+
+		metadata->register_io_path(profile_path, "X click", "/user/hand/left", "/user/hand/left/input/x/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "X touch", "/user/hand/left", "/user/hand/left/input/x/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "Y click", "/user/hand/left", "/user/hand/left/input/y/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "Y touch", "/user/hand/left", "/user/hand/left/input/y/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "A click", "/user/hand/right", "/user/hand/right/input/a/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "A touch", "/user/hand/right", "/user/hand/right/input/a/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "B click", "/user/hand/right", "/user/hand/right/input/b/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "B touch", "/user/hand/right", "/user/hand/right/input/b/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+	}
 }
diff --git a/modules/openxr/extensions/openxr_valve_analog_threshold_extension.cpp b/modules/openxr/extensions/openxr_valve_analog_threshold_extension.cpp
new file mode 100644
index 0000000000..b00c3db9d9
--- /dev/null
+++ b/modules/openxr/extensions/openxr_valve_analog_threshold_extension.cpp
@@ -0,0 +1,193 @@
+/**************************************************************************/
+/*  openxr_valve_analog_threshold_extension.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 "openxr_valve_analog_threshold_extension.h"
+#include "../action_map/openxr_action_set.h"
+#include "../action_map/openxr_interaction_profile.h"
+#include "../openxr_api.h"
+
+// Implementation for:
+// https://registry.khronos.org/OpenXR/specs/1.1/html/xrspec.html#XR_VALVE_analog_threshold
+
+///////////////////////////////////////////////////////////////////////////////////////////////////
+// OpenXRValveAnalogThresholdExtension
+
+OpenXRValveAnalogThresholdExtension *OpenXRValveAnalogThresholdExtension::singleton = nullptr;
+
+OpenXRValveAnalogThresholdExtension *OpenXRValveAnalogThresholdExtension::get_singleton() {
+	return singleton;
+}
+
+OpenXRValveAnalogThresholdExtension::OpenXRValveAnalogThresholdExtension() {
+	singleton = this;
+}
+
+OpenXRValveAnalogThresholdExtension::~OpenXRValveAnalogThresholdExtension() {
+	singleton = nullptr;
+}
+
+HashMap<String, bool *> OpenXRValveAnalogThresholdExtension::get_requested_extensions() {
+	HashMap<String, bool *> request_extensions;
+
+	// Note, we're dependent on the binding modifier extension, this may be requested by multiple extension wrappers.
+	request_extensions[XR_KHR_BINDING_MODIFICATION_EXTENSION_NAME] = &binding_modifier_ext;
+	request_extensions[XR_VALVE_ANALOG_THRESHOLD_EXTENSION_NAME] = &threshold_ext;
+
+	return request_extensions;
+}
+
+bool OpenXRValveAnalogThresholdExtension::is_available() {
+	return binding_modifier_ext && threshold_ext;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////
+// OpenXRAnalogThresholdModifier
+
+void OpenXRAnalogThresholdModifier::_bind_methods() {
+	ClassDB::bind_method(D_METHOD("set_on_threshold", "on_threshold"), &OpenXRAnalogThresholdModifier::set_on_threshold);
+	ClassDB::bind_method(D_METHOD("get_on_threshold"), &OpenXRAnalogThresholdModifier::get_on_threshold);
+	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "on_threshold", PROPERTY_HINT_RANGE, "0.0,1.0,0.01"), "set_on_threshold", "get_on_threshold");
+
+	ClassDB::bind_method(D_METHOD("set_off_threshold", "off_threshold"), &OpenXRAnalogThresholdModifier::set_off_threshold);
+	ClassDB::bind_method(D_METHOD("get_off_threshold"), &OpenXRAnalogThresholdModifier::get_off_threshold);
+	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "off_threshold", PROPERTY_HINT_RANGE, "0.0,1.0,0.01"), "set_off_threshold", "get_off_threshold");
+
+	ClassDB::bind_method(D_METHOD("set_on_haptic", "haptic"), &OpenXRAnalogThresholdModifier::set_on_haptic);
+	ClassDB::bind_method(D_METHOD("get_on_haptic"), &OpenXRAnalogThresholdModifier::get_on_haptic);
+	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "on_haptic", PROPERTY_HINT_RESOURCE_TYPE, "OpenXRHapticBase"), "set_on_haptic", "get_on_haptic");
+
+	ClassDB::bind_method(D_METHOD("set_off_haptic", "haptic"), &OpenXRAnalogThresholdModifier::set_off_haptic);
+	ClassDB::bind_method(D_METHOD("get_off_haptic"), &OpenXRAnalogThresholdModifier::get_off_haptic);
+	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "off_haptic", PROPERTY_HINT_RESOURCE_TYPE, "OpenXRHapticBase"), "set_off_haptic", "get_off_haptic");
+}
+
+OpenXRAnalogThresholdModifier::OpenXRAnalogThresholdModifier() {
+	analog_threshold.type = XR_TYPE_INTERACTION_PROFILE_ANALOG_THRESHOLD_VALVE;
+	analog_threshold.next = nullptr;
+
+	analog_threshold.onThreshold = 0.6;
+	analog_threshold.offThreshold = 0.4;
+}
+
+void OpenXRAnalogThresholdModifier::set_on_threshold(float p_threshold) {
+	ERR_FAIL_COND(p_threshold < 0.0 || p_threshold > 1.0);
+
+	analog_threshold.onThreshold = p_threshold;
+	emit_changed();
+}
+
+float OpenXRAnalogThresholdModifier::get_on_threshold() const {
+	return analog_threshold.onThreshold;
+}
+
+void OpenXRAnalogThresholdModifier::set_off_threshold(float p_threshold) {
+	ERR_FAIL_COND(p_threshold < 0.0 || p_threshold > 1.0);
+
+	analog_threshold.offThreshold = p_threshold;
+	emit_changed();
+}
+
+float OpenXRAnalogThresholdModifier::get_off_threshold() const {
+	return analog_threshold.offThreshold;
+}
+
+void OpenXRAnalogThresholdModifier::set_on_haptic(const Ref<OpenXRHapticBase> &p_haptic) {
+	on_haptic = p_haptic;
+	emit_changed();
+}
+
+Ref<OpenXRHapticBase> OpenXRAnalogThresholdModifier::get_on_haptic() const {
+	return on_haptic;
+}
+
+void OpenXRAnalogThresholdModifier::set_off_haptic(const Ref<OpenXRHapticBase> &p_haptic) {
+	off_haptic = p_haptic;
+	emit_changed();
+}
+
+Ref<OpenXRHapticBase> OpenXRAnalogThresholdModifier::get_off_haptic() const {
+	return off_haptic;
+}
+
+PackedByteArray OpenXRAnalogThresholdModifier::get_ip_modification() {
+	PackedByteArray ret;
+
+	OpenXRAPI *openxr_api = OpenXRAPI::get_singleton();
+	ERR_FAIL_NULL_V(openxr_api, ret);
+
+	OpenXRValveAnalogThresholdExtension *analog_threshold_ext = OpenXRValveAnalogThresholdExtension::get_singleton();
+	if (!analog_threshold_ext || !analog_threshold_ext->is_available()) {
+		// Extension not enabled!
+		WARN_PRINT("Analog threshold extension is not enabled or available.");
+		return ret;
+	}
+
+	ERR_FAIL_NULL_V(ip_binding, ret);
+
+	Ref<OpenXRAction> action = ip_binding->get_action();
+	ERR_FAIL_COND_V(!action.is_valid(), ret);
+
+	// Get our action set
+	Ref<OpenXRActionSet> action_set = action->get_action_set();
+	ERR_FAIL_COND_V(!action_set.is_valid(), ret);
+	RID action_set_rid = openxr_api->find_action_set(action_set->get_name());
+	ERR_FAIL_COND_V(!action_set_rid.is_valid(), ret);
+
+	// Get our action
+	RID action_rid = openxr_api->find_action(action->get_name(), action_set_rid);
+	ERR_FAIL_COND_V(!action_rid.is_valid(), ret);
+
+	analog_threshold.action = openxr_api->action_get_handle(action_rid);
+
+	analog_threshold.binding = openxr_api->get_xr_path(ip_binding->get_binding_path());
+	ERR_FAIL_COND_V(analog_threshold.binding == XR_NULL_PATH, ret);
+
+	// These are set already:
+	// - analog_threshold.onThreshold
+	// - analog_threshold.offThreshold
+
+	if (on_haptic.is_valid()) {
+		analog_threshold.onHaptic = on_haptic->get_xr_structure();
+	} else {
+		analog_threshold.onHaptic = nullptr;
+	}
+
+	if (off_haptic.is_valid()) {
+		analog_threshold.offHaptic = off_haptic->get_xr_structure();
+	} else {
+		analog_threshold.offHaptic = nullptr;
+	}
+
+	// Copy into byte array so we can return it.
+	ERR_FAIL_COND_V(ret.resize(sizeof(XrInteractionProfileAnalogThresholdVALVE)) != OK, ret);
+	memcpy(ret.ptrw(), &analog_threshold, sizeof(XrInteractionProfileAnalogThresholdVALVE));
+
+	return ret;
+}
diff --git a/modules/openxr/extensions/openxr_valve_analog_threshold_extension.h b/modules/openxr/extensions/openxr_valve_analog_threshold_extension.h
new file mode 100644
index 0000000000..d42eea8784
--- /dev/null
+++ b/modules/openxr/extensions/openxr_valve_analog_threshold_extension.h
@@ -0,0 +1,88 @@
+/**************************************************************************/
+/*  openxr_valve_analog_threshold_extension.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 OPENXR_VALVE_ANALOG_THRESHOLD_EXTENSION_H
+#define OPENXR_VALVE_ANALOG_THRESHOLD_EXTENSION_H
+
+#include "../action_map/openxr_binding_modifier.h"
+#include "../action_map/openxr_haptic_feedback.h"
+#include "../util.h"
+#include "core/io/resource.h"
+#include "openxr_extension_wrapper.h"
+
+class OpenXRValveAnalogThresholdExtension : public OpenXRExtensionWrapper {
+public:
+	static OpenXRValveAnalogThresholdExtension *get_singleton();
+
+	OpenXRValveAnalogThresholdExtension();
+	virtual ~OpenXRValveAnalogThresholdExtension() override;
+
+	virtual HashMap<String, bool *> get_requested_extensions() override;
+
+	bool is_available();
+
+private:
+	static OpenXRValveAnalogThresholdExtension *singleton;
+
+	bool binding_modifier_ext = false;
+	bool threshold_ext = false;
+};
+
+class OpenXRAnalogThresholdModifier : public OpenXRActionBindingModifier {
+	GDCLASS(OpenXRAnalogThresholdModifier, OpenXRActionBindingModifier);
+
+private:
+	XrInteractionProfileAnalogThresholdVALVE analog_threshold;
+	Ref<OpenXRHapticBase> on_haptic;
+	Ref<OpenXRHapticBase> off_haptic;
+
+protected:
+	static void _bind_methods();
+
+public:
+	OpenXRAnalogThresholdModifier();
+
+	void set_on_threshold(float p_threshold);
+	float get_on_threshold() const;
+
+	void set_off_threshold(float p_threshold);
+	float get_off_threshold() const;
+
+	void set_on_haptic(const Ref<OpenXRHapticBase> &p_haptic);
+	Ref<OpenXRHapticBase> get_on_haptic() const;
+
+	void set_off_haptic(const Ref<OpenXRHapticBase> &p_haptic);
+	Ref<OpenXRHapticBase> get_off_haptic() const;
+
+	virtual String get_description() const override { return "Analog threshold modifier"; }
+	virtual PackedByteArray get_ip_modification() override;
+};
+
+#endif // OPENXR_VALVE_ANALOG_THRESHOLD_EXTENSION_H
diff --git a/modules/openxr/extensions/openxr_wmr_controller_extension.cpp b/modules/openxr/extensions/openxr_wmr_controller_extension.cpp
index 3437320452..6b0ccca3ea 100644
--- a/modules/openxr/extensions/openxr_wmr_controller_extension.cpp
+++ b/modules/openxr/extensions/openxr_wmr_controller_extension.cpp
@@ -51,90 +51,85 @@ void OpenXRWMRControllerExtension::on_register_metadata() {
 	OpenXRInteractionProfileMetadata *metadata = OpenXRInteractionProfileMetadata::get_singleton();
 	ERR_FAIL_NULL(metadata);
 
-	// HP MR controller (newer G2 controllers)
-	metadata->register_interaction_profile("HPMR controller", "/interaction_profiles/hp/mixed_reality_controller", XR_EXT_HP_MIXED_REALITY_CONTROLLER_EXTENSION_NAME);
-	metadata->register_io_path("/interaction_profiles/hp/mixed_reality_controller", "Grip pose", "/user/hand/left", "/user/hand/left/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/hp/mixed_reality_controller", "Grip pose", "/user/hand/right", "/user/hand/right/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/hp/mixed_reality_controller", "Aim pose", "/user/hand/left", "/user/hand/left/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/hp/mixed_reality_controller", "Aim pose", "/user/hand/right", "/user/hand/right/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/hp/mixed_reality_controller", "Palm pose", "/user/hand/left", "/user/hand/left/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/hp/mixed_reality_controller", "Palm pose", "/user/hand/right", "/user/hand/right/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
+	{ // HP MR controller (newer G2 controllers)
+		const String profile_path = "/interaction_profiles/hp/mixed_reality_controller";
+		metadata->register_interaction_profile("HPMR controller", profile_path, XR_EXT_HP_MIXED_REALITY_CONTROLLER_EXTENSION_NAME);
+		for (const String user_path : { "/user/hand/left", "/user/hand/right" }) {
+			metadata->register_io_path(profile_path, "Grip pose", user_path, user_path + "/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			metadata->register_io_path(profile_path, "Aim pose", user_path, user_path + "/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			metadata->register_io_path(profile_path, "Palm pose", user_path, user_path + "/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
 
-	metadata->register_io_path("/interaction_profiles/hp/mixed_reality_controller", "Menu click", "/user/hand/left", "/user/hand/left/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/hp/mixed_reality_controller", "Menu click", "/user/hand/right", "/user/hand/right/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Menu click", user_path, user_path + "/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/hp/mixed_reality_controller", "X click", "/user/hand/left", "/user/hand/left/input/x/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/hp/mixed_reality_controller", "Y click", "/user/hand/left", "/user/hand/left/input/y/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/hp/mixed_reality_controller", "A click", "/user/hand/right", "/user/hand/right/input/a/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/hp/mixed_reality_controller", "B click", "/user/hand/right", "/user/hand/right/input/b/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trigger", user_path, user_path + "/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Trigger click", user_path, user_path + "/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/hp/mixed_reality_controller", "Trigger", "/user/hand/left", "/user/hand/left/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/hp/mixed_reality_controller", "Trigger click", "/user/hand/left", "/user/hand/left/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/hp/mixed_reality_controller", "Trigger", "/user/hand/right", "/user/hand/right/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/hp/mixed_reality_controller", "Trigger click", "/user/hand/right", "/user/hand/right/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Squeeze", user_path, user_path + "/input/squeeze/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
 
-	metadata->register_io_path("/interaction_profiles/hp/mixed_reality_controller", "Squeeze", "/user/hand/left", "/user/hand/left/input/squeeze/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/hp/mixed_reality_controller", "Squeeze", "/user/hand/right", "/user/hand/right/input/squeeze/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Thumbstick", user_path, user_path + "/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
+			metadata->register_io_path(profile_path, "Thumbstick click", user_path, user_path + "/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Up", user_path, user_path + "/input/thumbstick/dpad_up", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Down", user_path, user_path + "/input/thumbstick/dpad_down", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Left", user_path, user_path + "/input/thumbstick/dpad_left", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Right", user_path, user_path + "/input/thumbstick/dpad_right", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/hp/mixed_reality_controller", "Thumbstick", "/user/hand/left", "/user/hand/left/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/hp/mixed_reality_controller", "Thumbstick click", "/user/hand/left", "/user/hand/left/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/hp/mixed_reality_controller", "Thumbstick", "/user/hand/right", "/user/hand/right/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/hp/mixed_reality_controller", "Thumbstick click", "/user/hand/right", "/user/hand/right/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Haptic output", user_path, user_path + "/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+		}
 
-	metadata->register_io_path("/interaction_profiles/hp/mixed_reality_controller", "Haptic output", "/user/hand/left", "/user/hand/left/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	metadata->register_io_path("/interaction_profiles/hp/mixed_reality_controller", "Haptic output", "/user/hand/right", "/user/hand/right/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+		metadata->register_io_path(profile_path, "X click", "/user/hand/left", "/user/hand/left/input/x/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "Y click", "/user/hand/left", "/user/hand/left/input/y/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "A click", "/user/hand/right", "/user/hand/right/input/a/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+		metadata->register_io_path(profile_path, "B click", "/user/hand/right", "/user/hand/right/input/b/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+	}
 
-	// Samsung Odyssey controller
-	metadata->register_interaction_profile("Samsung Odyssey controller", "/interaction_profiles/samsung/odyssey_controller", XR_EXT_SAMSUNG_ODYSSEY_CONTROLLER_EXTENSION_NAME);
-	metadata->register_io_path("/interaction_profiles/samsung/odyssey_controller", "Grip pose", "/user/hand/left", "/user/hand/left/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/samsung/odyssey_controller", "Grip pose", "/user/hand/right", "/user/hand/right/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/samsung/odyssey_controller", "Aim pose", "/user/hand/left", "/user/hand/left/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/samsung/odyssey_controller", "Aim pose", "/user/hand/right", "/user/hand/right/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/samsung/odyssey_controller", "Palm pose", "/user/hand/left", "/user/hand/left/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/samsung/odyssey_controller", "Palm pose", "/user/hand/right", "/user/hand/right/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
+	{ // Samsung Odyssey controller
+		const String profile_path = "/interaction_profiles/samsung/odyssey_controller";
+		metadata->register_interaction_profile("Samsung Odyssey controller", profile_path, XR_EXT_SAMSUNG_ODYSSEY_CONTROLLER_EXTENSION_NAME);
+		for (const String user_path : { "/user/hand/left", "/user/hand/right" }) {
+			metadata->register_io_path(profile_path, "Grip pose", user_path, user_path + "/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			metadata->register_io_path(profile_path, "Aim pose", user_path, user_path + "/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			metadata->register_io_path(profile_path, "Palm pose", user_path, user_path + "/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
 
-	metadata->register_io_path("/interaction_profiles/samsung/odyssey_controller", "Menu click", "/user/hand/left", "/user/hand/left/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/samsung/odyssey_controller", "Menu click", "/user/hand/right", "/user/hand/right/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Menu click", user_path, user_path + "/input/menu/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/samsung/odyssey_controller", "Trigger", "/user/hand/left", "/user/hand/left/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/samsung/odyssey_controller", "Trigger click", "/user/hand/left", "/user/hand/left/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/samsung/odyssey_controller", "Trigger", "/user/hand/right", "/user/hand/right/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/samsung/odyssey_controller", "Trigger click", "/user/hand/right", "/user/hand/right/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trigger", user_path, user_path + "/input/trigger/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Trigger click", user_path, user_path + "/input/trigger/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/samsung/odyssey_controller", "Squeeze click", "/user/hand/left", "/user/hand/left/input/squeeze/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/samsung/odyssey_controller", "Squeeze click", "/user/hand/right", "/user/hand/right/input/squeeze/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Squeeze click", user_path, user_path + "/input/squeeze/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/samsung/odyssey_controller", "Thumbstick", "/user/hand/left", "/user/hand/left/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/samsung/odyssey_controller", "Thumbstick click", "/user/hand/left", "/user/hand/left/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/samsung/odyssey_controller", "Thumbstick", "/user/hand/right", "/user/hand/right/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/samsung/odyssey_controller", "Thumbstick click", "/user/hand/right", "/user/hand/right/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick", user_path, user_path + "/input/thumbstick", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
+			metadata->register_io_path(profile_path, "Thumbstick click", user_path, user_path + "/input/thumbstick/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Up", user_path, user_path + "/input/thumbstick/dpad_up", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Down", user_path, user_path + "/input/thumbstick/dpad_down", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Left", user_path, user_path + "/input/thumbstick/dpad_left", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Thumbstick Dpad Right", user_path, user_path + "/input/thumbstick/dpad_right", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/samsung/odyssey_controller", "Trackpad", "/user/hand/left", "/user/hand/left/input/trackpad", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/samsung/odyssey_controller", "Trackpad click", "/user/hand/left", "/user/hand/left/input/trackpad/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/samsung/odyssey_controller", "Trackpad touch", "/user/hand/left", "/user/hand/left/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/samsung/odyssey_controller", "Trackpad", "/user/hand/right", "/user/hand/right/input/trackpad", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
-	metadata->register_io_path("/interaction_profiles/samsung/odyssey_controller", "Trackpad click", "/user/hand/right", "/user/hand/right/input/trackpad/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
-	metadata->register_io_path("/interaction_profiles/samsung/odyssey_controller", "Trackpad touch", "/user/hand/right", "/user/hand/right/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trackpad", user_path, user_path + "/input/trackpad", "", OpenXRAction::OPENXR_ACTION_VECTOR2);
+			metadata->register_io_path(profile_path, "Trackpad click", user_path, user_path + "/input/trackpad/click", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trackpad touch", user_path, user_path + "/input/trackpad/touch", "", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trackpad Dpad Up", user_path, user_path + "/input/trackpad/dpad_up", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trackpad Dpad Down", user_path, user_path + "/input/trackpad/dpad_down", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trackpad Dpad Left", user_path, user_path + "/input/trackpad/dpad_left", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trackpad Dpad Right", user_path, user_path + "/input/trackpad/dpad_right", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
+			metadata->register_io_path(profile_path, "Trackpad Dpad Center", user_path, user_path + "/input/trackpad/dpad_center", "XR_EXT_dpad_binding", OpenXRAction::OPENXR_ACTION_BOOL);
 
-	metadata->register_io_path("/interaction_profiles/samsung/odyssey_controller", "Haptic output", "/user/hand/left", "/user/hand/left/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
-	metadata->register_io_path("/interaction_profiles/samsung/odyssey_controller", "Haptic output", "/user/hand/right", "/user/hand/right/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+			metadata->register_io_path(profile_path, "Haptic output", user_path, user_path + "/output/haptic", "", OpenXRAction::OPENXR_ACTION_HAPTIC);
+		}
+	}
 
-	// MSFT Hand interaction profile, also supported by other headsets
-	metadata->register_interaction_profile("MSFT Hand interaction", "/interaction_profiles/microsoft/hand_interaction", XR_MSFT_HAND_INTERACTION_EXTENSION_NAME);
-	metadata->register_io_path("/interaction_profiles/microsoft/hand_interaction", "Grip pose", "/user/hand/left", "/user/hand/left/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/microsoft/hand_interaction", "Grip pose", "/user/hand/right", "/user/hand/right/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/microsoft/hand_interaction", "Aim pose", "/user/hand/left", "/user/hand/left/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/microsoft/hand_interaction", "Aim pose", "/user/hand/right", "/user/hand/right/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/microsoft/hand_interaction", "Pinch pose", "/user/hand/left", "/user/hand/left/input/pinch_ext/pose", XR_EXT_HAND_INTERACTION_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/microsoft/hand_interaction", "Pinch pose", "/user/hand/right", "/user/hand/right/input/pinch_ext/pose", XR_EXT_HAND_INTERACTION_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/microsoft/hand_interaction", "Poke pose", "/user/hand/left", "/user/hand/left/input/poke_ext/pose", XR_EXT_HAND_INTERACTION_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/microsoft/hand_interaction", "Poke pose", "/user/hand/right", "/user/hand/right/input/poke_ext/pose", XR_EXT_HAND_INTERACTION_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/microsoft/hand_interaction", "Palm pose", "/user/hand/left", "/user/hand/left/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
-	metadata->register_io_path("/interaction_profiles/microsoft/hand_interaction", "Palm pose", "/user/hand/right", "/user/hand/right/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
+	{ // MSFT Hand interaction profile, also supported by other headsets
+		const String profile_path = "/interaction_profiles/microsoft/hand_interaction";
+		metadata->register_interaction_profile("MSFT Hand interaction", profile_path, XR_MSFT_HAND_INTERACTION_EXTENSION_NAME);
+		for (const String user_path : { "/user/hand/left", "/user/hand/right" }) {
+			metadata->register_io_path(profile_path, "Grip pose", user_path, user_path + "/input/grip/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			metadata->register_io_path(profile_path, "Aim pose", user_path, user_path + "/input/aim/pose", "", OpenXRAction::OPENXR_ACTION_POSE);
+			metadata->register_io_path(profile_path, "Pinch pose", user_path, user_path + "/input/pinch_ext/pose", XR_EXT_HAND_INTERACTION_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
+			metadata->register_io_path(profile_path, "Poke pose", user_path, user_path + "/input/poke_ext/pose", XR_EXT_HAND_INTERACTION_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
+			metadata->register_io_path(profile_path, "Palm pose", user_path, user_path + "/input/palm_ext/pose", XR_EXT_PALM_POSE_EXTENSION_NAME, OpenXRAction::OPENXR_ACTION_POSE);
 
-	metadata->register_io_path("/interaction_profiles/microsoft/hand_interaction", "Select (pinch)", "/user/hand/left", "/user/hand/left/input/select/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/microsoft/hand_interaction", "Select (pinch)", "/user/hand/right", "/user/hand/right/input/select/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Select (pinch)", user_path, user_path + "/input/select/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
 
-	metadata->register_io_path("/interaction_profiles/microsoft/hand_interaction", "Squeeze (grab)", "/user/hand/left", "/user/hand/left/input/squeeze/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
-	metadata->register_io_path("/interaction_profiles/microsoft/hand_interaction", "Squeeze (grab)", "/user/hand/right", "/user/hand/right/input/squeeze/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+			metadata->register_io_path(profile_path, "Squeeze (grab)", user_path, user_path + "/input/squeeze/value", "", OpenXRAction::OPENXR_ACTION_FLOAT);
+		}
+	}
 }
diff --git a/modules/openxr/openxr_api.cpp b/modules/openxr/openxr_api.cpp
index 1775541757..b024e150d5 100644
--- a/modules/openxr/openxr_api.cpp
+++ b/modules/openxr/openxr_api.cpp
@@ -513,40 +513,66 @@ void OpenXRAPI::copy_string_to_char_buffer(const String p_string, char *p_buffer
 	}
 }
 
-bool OpenXRAPI::create_instance() {
-	// Create our OpenXR instance, this will query any registered extension wrappers for extensions we need to enable.
+PackedStringArray OpenXRAPI::get_all_requested_extensions() {
+	// This returns all extensions we will request regardless of whether they are available.
+	// This is mostly used by the editor to filter features not enabled through project settings.
 
-	// Append the extensions requested by the registered extension wrappers.
-	HashMap<String, bool *> requested_extensions;
+	PackedStringArray requested_extensions;
 	for (OpenXRExtensionWrapper *wrapper : registered_extension_wrappers) {
 		const HashMap<String, bool *> &wrapper_request_extensions = wrapper->get_requested_extensions();
 
 		for (const KeyValue<String, bool *> &requested_extension : wrapper_request_extensions) {
-			requested_extensions[requested_extension.key] = requested_extension.value;
+			if (!requested_extensions.has(requested_extension.key)) {
+				requested_extensions.push_back(requested_extension.key);
+			}
+		}
+	}
+
+	return requested_extensions;
+}
+
+bool OpenXRAPI::create_instance() {
+	// Create our OpenXR instance, this will query any registered extension wrappers for extensions we need to enable.
+
+	// We can request an extension multiple times if there are dependencies
+	struct RequestExtension {
+		String name;
+		bool *enabled;
+	};
+
+	// Find all extensions we wish to enable.
+	Vector<RequestExtension> requested_extensions;
+	for (OpenXRExtensionWrapper *wrapper : registered_extension_wrappers) {
+		const HashMap<String, bool *> &wrapper_request_extensions = wrapper->get_requested_extensions();
+
+		for (const KeyValue<String, bool *> &requested_extension : wrapper_request_extensions) {
+			requested_extensions.push_back({ requested_extension.key, requested_extension.value });
 		}
 	}
 
 	// Check which extensions are supported.
 	enabled_extensions.clear();
 
-	for (KeyValue<String, bool *> &requested_extension : requested_extensions) {
-		if (!is_extension_supported(requested_extension.key)) {
-			if (requested_extension.value == nullptr) {
+	for (RequestExtension &requested_extension : requested_extensions) {
+		if (!is_extension_supported(requested_extension.name)) {
+			if (requested_extension.enabled == nullptr) {
 				// Null means this is a mandatory extension so we fail.
-				ERR_FAIL_V_MSG(false, String("OpenXR: OpenXR Runtime does not support ") + requested_extension.key + String(" extension!"));
+				ERR_FAIL_V_MSG(false, String("OpenXR: OpenXR Runtime does not support ") + requested_extension.name + String(" extension!"));
 			} else {
 				// Set this extension as not supported.
-				*requested_extension.value = false;
+				*requested_extension.enabled = false;
 			}
-		} else if (requested_extension.value != nullptr) {
-			// Set this extension as supported.
-			*requested_extension.value = true;
-
-			// And record that we want to enable it.
-			enabled_extensions.push_back(requested_extension.key.ascii());
 		} else {
-			// Record that we want to enable this.
-			enabled_extensions.push_back(requested_extension.key.ascii());
+			if (requested_extension.enabled != nullptr) {
+				// Set this extension as supported.
+				*requested_extension.enabled = true;
+			}
+
+			// And record that we want to enable it (dependent extensions may be requested multiple times).
+			CharString ext_name = requested_extension.name.ascii();
+			if (!enabled_extensions.has(ext_name)) {
+				enabled_extensions.push_back(ext_name);
+			}
 		}
 	}
 
@@ -2765,6 +2791,25 @@ bool OpenXRAPI::xr_result(XrResult result, const char *format, Array args) const
 	return false;
 }
 
+XrPath OpenXRAPI::get_xr_path(const String &p_path) {
+	ERR_FAIL_COND_V(instance == XR_NULL_HANDLE, XR_NULL_PATH);
+
+	if (p_path.is_empty()) {
+		// This isn't necesairily an issue, so silently return a null path.
+		return XR_NULL_PATH;
+	}
+
+	XrPath path = XR_NULL_PATH;
+
+	XrResult result = xrStringToPath(instance, p_path.utf8().get_data(), &path);
+	if (XR_FAILED(result)) {
+		print_line("OpenXR: failed to get path for ", p_path, "! [", get_error_string(result), "]");
+		return XR_NULL_PATH;
+	}
+
+	return path;
+}
+
 RID OpenXRAPI::get_tracker_rid(XrPath p_path) {
 	List<RID> current;
 	tracker_owner.get_owned_list(&current);
@@ -2799,11 +2844,8 @@ RID OpenXRAPI::tracker_create(const String p_name) {
 	new_tracker.toplevel_path = XR_NULL_PATH;
 	new_tracker.active_profile_rid = RID();
 
-	XrResult result = xrStringToPath(instance, p_name.utf8().get_data(), &new_tracker.toplevel_path);
-	if (XR_FAILED(result)) {
-		print_line("OpenXR: failed to get path for ", p_name, "! [", get_error_string(result), "]");
-		return RID();
-	}
+	new_tracker.toplevel_path = get_xr_path(p_name);
+	ERR_FAIL_COND_V(new_tracker.toplevel_path == XR_NULL_PATH, RID());
 
 	return tracker_owner.make_rid(new_tracker);
 }
@@ -2894,6 +2936,19 @@ RID OpenXRAPI::action_set_create(const String p_name, const String p_localized_n
 	return action_set_owner.make_rid(action_set);
 }
 
+RID OpenXRAPI::find_action_set(const String p_name) {
+	List<RID> current;
+	action_set_owner.get_owned_list(&current);
+	for (const RID &E : current) {
+		ActionSet *action_set = action_set_owner.get_or_null(E);
+		if (action_set && action_set->name == p_name) {
+			return E;
+		}
+	}
+
+	return RID();
+}
+
 String OpenXRAPI::action_set_get_name(RID p_action_set) {
 	if (p_action_set.is_null()) {
 		return String("None");
@@ -2905,6 +2960,17 @@ String OpenXRAPI::action_set_get_name(RID p_action_set) {
 	return action_set->name;
 }
 
+XrActionSet OpenXRAPI::action_set_get_handle(RID p_action_set) {
+	if (p_action_set.is_null()) {
+		return XR_NULL_HANDLE;
+	}
+
+	ActionSet *action_set = action_set_owner.get_or_null(p_action_set);
+	ERR_FAIL_NULL_V(action_set, XR_NULL_HANDLE);
+
+	return action_set->handle;
+}
+
 bool OpenXRAPI::attach_action_sets(const Vector<RID> &p_action_sets) {
 	ERR_FAIL_COND_V(session == XR_NULL_HANDLE, false);
 
@@ -2987,12 +3053,12 @@ RID OpenXRAPI::get_action_rid(XrAction p_action) {
 	return RID();
 }
 
-RID OpenXRAPI::find_action(const String &p_name) {
+RID OpenXRAPI::find_action(const String &p_name, const RID &p_action_set) {
 	List<RID> current;
 	action_owner.get_owned_list(&current);
 	for (const RID &E : current) {
 		Action *action = action_owner.get_or_null(E);
-		if (action && action->name == p_name) {
+		if (action && action->name == p_name && (p_action_set.is_null() || action->action_set_rid == p_action_set)) {
 			return E;
 		}
 	}
@@ -3082,6 +3148,17 @@ String OpenXRAPI::action_get_name(RID p_action) {
 	return action->name;
 }
 
+XrAction OpenXRAPI::action_get_handle(RID p_action) {
+	if (p_action.is_null()) {
+		return XR_NULL_HANDLE;
+	}
+
+	Action *action = action_owner.get_or_null(p_action);
+	ERR_FAIL_NULL_V(action, XR_NULL_HANDLE);
+
+	return action->handle;
+}
+
 void OpenXRAPI::action_free(RID p_action) {
 	Action *action = action_owner.get_or_null(p_action);
 	ERR_FAIL_NULL(action);
@@ -3158,29 +3235,41 @@ void OpenXRAPI::interaction_profile_clear_bindings(RID p_interaction_profile) {
 	ip->bindings.clear();
 }
 
-bool OpenXRAPI::interaction_profile_add_binding(RID p_interaction_profile, RID p_action, const String p_path) {
+int OpenXRAPI::interaction_profile_add_binding(RID p_interaction_profile, RID p_action, const String p_path) {
 	InteractionProfile *ip = interaction_profile_owner.get_or_null(p_interaction_profile);
-	ERR_FAIL_NULL_V(ip, false);
+	ERR_FAIL_NULL_V(ip, -1);
 
 	if (!interaction_profile_supports_io_path(ip->name, p_path)) {
-		return false;
+		return -1;
 	}
 
 	XrActionSuggestedBinding binding;
 
 	Action *action = action_owner.get_or_null(p_action);
-	ERR_FAIL_COND_V(action == nullptr || action->handle == XR_NULL_HANDLE, false);
+	ERR_FAIL_COND_V(action == nullptr || action->handle == XR_NULL_HANDLE, -1);
 
 	binding.action = action->handle;
 
 	XrResult result = xrStringToPath(instance, p_path.utf8().get_data(), &binding.binding);
 	if (XR_FAILED(result)) {
 		print_line("OpenXR: failed to get path for ", p_path, "! [", get_error_string(result), "]");
-		return false;
+		return -1;
 	}
 
 	ip->bindings.push_back(binding);
 
+	return ip->bindings.size() - 1;
+}
+
+bool OpenXRAPI::interaction_profile_add_modifier(RID p_interaction_profile, const PackedByteArray &p_modifier) {
+	InteractionProfile *ip = interaction_profile_owner.get_or_null(p_interaction_profile);
+	ERR_FAIL_NULL_V(ip, false);
+
+	if (!p_modifier.is_empty()) {
+		// Add it to our stack.
+		ip->modifiers.push_back(p_modifier);
+	}
+
 	return true;
 }
 
@@ -3190,9 +3279,27 @@ bool OpenXRAPI::interaction_profile_suggest_bindings(RID p_interaction_profile)
 	InteractionProfile *ip = interaction_profile_owner.get_or_null(p_interaction_profile);
 	ERR_FAIL_NULL_V(ip, false);
 
+	void *next = nullptr;
+
+	// Note, extensions should only add binding modifiers if they are supported, else this may fail.
+	XrBindingModificationsKHR binding_modifiers;
+	Vector<const XrBindingModificationBaseHeaderKHR *> modifiers;
+	if (!ip->modifiers.is_empty()) {
+		for (const PackedByteArray &modifier : ip->modifiers) {
+			const XrBindingModificationBaseHeaderKHR *ptr = (const XrBindingModificationBaseHeaderKHR *)modifier.ptr();
+			modifiers.push_back(ptr);
+		}
+
+		binding_modifiers.type = XR_TYPE_BINDING_MODIFICATIONS_KHR;
+		binding_modifiers.next = next;
+		binding_modifiers.bindingModificationCount = modifiers.size();
+		binding_modifiers.bindingModifications = modifiers.ptr();
+		next = &binding_modifiers;
+	}
+
 	const XrInteractionProfileSuggestedBinding suggested_bindings = {
 		XR_TYPE_INTERACTION_PROFILE_SUGGESTED_BINDING, // type
-		nullptr, // next
+		next, // next
 		ip->path, // interactionProfile
 		uint32_t(ip->bindings.size()), // countSuggestedBindings
 		ip->bindings.ptr() // suggestedBindings
@@ -3231,6 +3338,7 @@ void OpenXRAPI::interaction_profile_free(RID p_interaction_profile) {
 	ERR_FAIL_NULL(ip);
 
 	ip->bindings.clear();
+	ip->modifiers.clear();
 
 	interaction_profile_owner.free(p_interaction_profile);
 }
diff --git a/modules/openxr/openxr_api.h b/modules/openxr/openxr_api.h
index ecffce1816..486f605f76 100644
--- a/modules/openxr/openxr_api.h
+++ b/modules/openxr/openxr_api.h
@@ -300,6 +300,7 @@ private:
 		String name; // Name of the interaction profile (i.e. "/interaction_profiles/valve/index_controller")
 		XrPath path; // OpenXR path for this profile
 		Vector<XrActionSuggestedBinding> bindings; // OpenXR action bindings
+		Vector<PackedByteArray> modifiers; // Array of modifiers we'll add into XrBindingModificationsKHR
 	};
 	RID_Owner<InteractionProfile, true> interaction_profile_owner;
 	RID get_interaction_profile_rid(XrPath p_path);
@@ -410,8 +411,8 @@ public:
 	XRPose::TrackingConfidence transform_from_location(const XrSpaceLocation &p_location, Transform3D &r_transform);
 	XRPose::TrackingConfidence transform_from_location(const XrHandJointLocationEXT &p_location, Transform3D &r_transform);
 	void parse_velocities(const XrSpaceVelocity &p_velocity, Vector3 &r_linear_velocity, Vector3 &r_angular_velocity);
-
 	bool xr_result(XrResult result, const char *format, Array args = Array()) const;
+	XrPath get_xr_path(const String &p_path);
 	bool is_top_level_path_supported(const String &p_toplevel_path);
 	bool is_interaction_profile_supported(const String &p_ip_path);
 	bool interaction_profile_supports_io_path(const String &p_ip_path, const String &p_io_path);
@@ -435,6 +436,7 @@ public:
 	static const Vector<OpenXRExtensionWrapper *> &get_registered_extension_wrappers();
 	static void register_extension_metadata();
 	static void cleanup_extension_wrappers();
+	static PackedStringArray get_all_requested_extensions();
 
 	void set_form_factor(XrFormFactor p_form_factor);
 	XrFormFactor get_form_factor() const { return form_factor; }
@@ -515,22 +517,26 @@ public:
 
 	RID action_set_create(const String p_name, const String p_localized_name, const int p_priority);
 	String action_set_get_name(RID p_action_set);
+	XrActionSet action_set_get_handle(RID p_action_set);
 	bool attach_action_sets(const Vector<RID> &p_action_sets);
 	void action_set_free(RID p_action_set);
 
 	RID action_create(RID p_action_set, const String p_name, const String p_localized_name, OpenXRAction::ActionType p_action_type, const Vector<RID> &p_trackers);
 	String action_get_name(RID p_action);
+	XrAction action_get_handle(RID p_action);
 	void action_free(RID p_action);
 
 	RID interaction_profile_create(const String p_name);
 	String interaction_profile_get_name(RID p_interaction_profile);
 	void interaction_profile_clear_bindings(RID p_interaction_profile);
-	bool interaction_profile_add_binding(RID p_interaction_profile, RID p_action, const String p_path);
+	int interaction_profile_add_binding(RID p_interaction_profile, RID p_action, const String p_path);
+	bool interaction_profile_add_modifier(RID p_interaction_profile, const PackedByteArray &p_modifier);
 	bool interaction_profile_suggest_bindings(RID p_interaction_profile);
 	void interaction_profile_free(RID p_interaction_profile);
 
 	RID find_tracker(const String &p_name);
-	RID find_action(const String &p_name);
+	RID find_action_set(const String p_name);
+	RID find_action(const String &p_name, const RID &p_action_set = RID());
 
 	bool sync_action_sets(const Vector<RID> p_active_sets);
 	bool get_action_bool(RID p_action, RID p_tracker);
diff --git a/modules/openxr/openxr_interface.cpp b/modules/openxr/openxr_interface.cpp
index 68e04694e3..6a1486d764 100644
--- a/modules/openxr/openxr_interface.cpp
+++ b/modules/openxr/openxr_interface.cpp
@@ -287,6 +287,13 @@ void OpenXRInterface::_load_action_map() {
 			if (ip.is_valid()) {
 				openxr_api->interaction_profile_clear_bindings(ip);
 
+				for (Ref<OpenXRBindingModifier> xr_binding_modifier : xr_interaction_profile->get_binding_modifiers()) {
+					PackedByteArray bm = xr_binding_modifier->get_ip_modification();
+					if (!bm.is_empty()) {
+						openxr_api->interaction_profile_add_modifier(ip, bm);
+					}
+				}
+
 				Array xr_bindings = xr_interaction_profile->get_bindings();
 				for (int j = 0; j < xr_bindings.size(); j++) {
 					Ref<OpenXRIPBinding> xr_binding = xr_bindings[j];
@@ -300,7 +307,18 @@ void OpenXRInterface::_load_action_map() {
 						continue;
 					}
 
-					openxr_api->interaction_profile_add_binding(ip, action->action_rid, xr_binding->get_binding_path());
+					int binding_no = openxr_api->interaction_profile_add_binding(ip, action->action_rid, xr_binding->get_binding_path());
+					if (binding_no >= 0) {
+						for (Ref<OpenXRBindingModifier> xr_binding_modifier : xr_binding->get_binding_modifiers()) {
+							// Binding modifiers on bindings can be added to the interaction profile.
+							PackedByteArray bm = xr_binding_modifier->get_ip_modification();
+							if (!bm.is_empty()) {
+								openxr_api->interaction_profile_add_modifier(ip, bm);
+							}
+
+							// And possibly in the future on the binding itself, we're just preparing for that eventuality.
+						}
+					}
 				}
 
 				// Now submit our suggestions
@@ -593,8 +611,8 @@ void OpenXRInterface::free_trackers() {
 void OpenXRInterface::free_interaction_profiles() {
 	ERR_FAIL_NULL(openxr_api);
 
-	for (int i = 0; i < interaction_profiles.size(); i++) {
-		openxr_api->interaction_profile_free(interaction_profiles[i]);
+	for (const RID &interaction_profile : interaction_profiles) {
+		openxr_api->interaction_profile_free(interaction_profile);
 	}
 	interaction_profiles.clear();
 }
diff --git a/modules/openxr/register_types.cpp b/modules/openxr/register_types.cpp
index f3fda2517c..69c119d069 100644
--- a/modules/openxr/register_types.cpp
+++ b/modules/openxr/register_types.cpp
@@ -33,6 +33,7 @@
 #include "action_map/openxr_action.h"
 #include "action_map/openxr_action_map.h"
 #include "action_map/openxr_action_set.h"
+#include "action_map/openxr_haptic_feedback.h"
 #include "action_map/openxr_interaction_profile.h"
 #include "action_map/openxr_interaction_profile_metadata.h"
 #include "openxr_interface.h"
@@ -49,6 +50,7 @@
 #include "extensions/openxr_composition_layer_depth_extension.h"
 #include "extensions/openxr_composition_layer_extension.h"
 #include "extensions/openxr_debug_utils_extension.h"
+#include "extensions/openxr_dpad_binding_extension.h"
 #include "extensions/openxr_eye_gaze_interaction.h"
 #include "extensions/openxr_fb_display_refresh_rate_extension.h"
 #include "extensions/openxr_hand_interaction_extension.h"
@@ -62,6 +64,7 @@
 #include "extensions/openxr_mxink_extension.h"
 #include "extensions/openxr_palm_pose_extension.h"
 #include "extensions/openxr_pico_controller_extension.h"
+#include "extensions/openxr_valve_analog_threshold_extension.h"
 #include "extensions/openxr_visibility_mask_extension.h"
 #include "extensions/openxr_wmr_controller_extension.h"
 
@@ -78,6 +81,10 @@
 
 #ifdef TOOLS_ENABLED
 #include "editor/editor_node.h"
+
+#include "editor/openxr_binding_modifier_editor.h"
+#include "editor/openxr_interaction_profile_editor.h"
+
 #endif
 
 static OpenXRAPI *openxr_api = nullptr;
@@ -140,6 +147,14 @@ void initialize_openxr_module(ModuleInitializationLevel p_level) {
 			if (GLOBAL_GET("xr/openxr/extensions/hand_tracking")) {
 				OpenXRAPI::register_extension_wrapper(memnew(OpenXRHandTrackingExtension));
 			}
+
+			// register gated binding modifiers
+			if (GLOBAL_GET("xr/openxr/binding_modifiers/analog_threshold")) {
+				OpenXRAPI::register_extension_wrapper(memnew(OpenXRValveAnalogThresholdExtension));
+			}
+			if (GLOBAL_GET("xr/openxr/binding_modifiers/dpad_binding")) {
+				OpenXRAPI::register_extension_wrapper(memnew(OpenXRDPadBindingExtension));
+			}
 		}
 
 		if (OpenXRAPI::openxr_is_enabled()) {
@@ -181,6 +196,15 @@ void initialize_openxr_module(ModuleInitializationLevel p_level) {
 		GDREGISTER_CLASS(OpenXRIPBinding);
 		GDREGISTER_CLASS(OpenXRInteractionProfile);
 
+		GDREGISTER_ABSTRACT_CLASS(OpenXRBindingModifier);
+		GDREGISTER_VIRTUAL_CLASS(OpenXRIPBindingModifier);
+		GDREGISTER_VIRTUAL_CLASS(OpenXRActionBindingModifier);
+		GDREGISTER_CLASS(OpenXRAnalogThresholdModifier);
+		GDREGISTER_CLASS(OpenXRDpadBindingModifier);
+
+		GDREGISTER_ABSTRACT_CLASS(OpenXRHapticBase);
+		GDREGISTER_CLASS(OpenXRHapticVibration);
+
 		GDREGISTER_ABSTRACT_CLASS(OpenXRCompositionLayer);
 		GDREGISTER_CLASS(OpenXRCompositionLayerEquirect);
 		GDREGISTER_CLASS(OpenXRCompositionLayerCylinder);
@@ -201,6 +225,10 @@ void initialize_openxr_module(ModuleInitializationLevel p_level) {
 		}
 
 #ifdef TOOLS_ENABLED
+		GDREGISTER_ABSTRACT_CLASS(OpenXRInteractionProfileEditorBase);
+		GDREGISTER_CLASS(OpenXRInteractionProfileEditor);
+		GDREGISTER_CLASS(OpenXRBindingModifierEditor);
+
 		EditorNode::add_init_callback(_editor_init);
 #endif
 	}

From ef3eecd34ec5b58f140a77e5c2bb336bec83dfdc Mon Sep 17 00:00:00 2001
From: Lukas Tenbrink <lukas.tenbrink@gmail.com>
Date: Thu, 12 Dec 2024 00:28:13 +0100
Subject: [PATCH 085/124] Optimize `String.count` and `String.countn` by
 avoiding repeated reallocations.

---
 core/string/ustring.cpp | 28 ++++++++++++----------------
 1 file changed, 12 insertions(+), 16 deletions(-)

diff --git a/core/string/ustring.cpp b/core/string/ustring.cpp
index 561498ed02..e1588d3752 100644
--- a/core/string/ustring.cpp
+++ b/core/string/ustring.cpp
@@ -3735,14 +3735,12 @@ int String::_count(const String &p_string, int p_from, int p_to, bool p_case_ins
 		return 0;
 	}
 	int c = 0;
-	int idx = -1;
-	do {
-		idx = p_case_insensitive ? str.findn(p_string) : str.find(p_string);
-		if (idx != -1) {
-			str = str.substr(idx + slen, str.length() - slen);
-			++c;
-		}
-	} while (idx != -1);
+	int idx = 0;
+	while ((idx = p_case_insensitive ? str.findn(p_string, idx) : str.find(p_string, idx)) != -1) {
+		// Skip the occurrence itself.
+		idx += slen;
+		++c;
+	}
 	return c;
 }
 
@@ -3774,14 +3772,12 @@ int String::_count(const char *p_string, int p_from, int p_to, bool p_case_insen
 		return 0;
 	}
 	int c = 0;
-	int idx = -1;
-	do {
-		idx = p_case_insensitive ? str.findn(p_string) : str.find(p_string);
-		if (idx != -1) {
-			str = str.substr(idx + substring_length, str.length() - substring_length);
-			++c;
-		}
-	} while (idx != -1);
+	int idx = 0;
+	while ((idx = p_case_insensitive ? str.findn(p_string, idx) : str.find(p_string, idx)) != -1) {
+		// Skip the occurrence itself.
+		idx += substring_length;
+		++c;
+	}
 	return c;
 }
 

From 76af9537ed05fe60fe6282a8872d22f2383c96ab Mon Sep 17 00:00:00 2001
From: Lukas Tenbrink <lukas.tenbrink@gmail.com>
Date: Thu, 12 Dec 2024 01:17:02 +0100
Subject: [PATCH 086/124] Optimize `StringBuilder.as_string` by constructing
 the string in-place and skipping unnecessary checks.

Co-authored-by: YYF233333 <nbyyf2002@mail.ustc.edu.cn>
---
 core/string/string_builder.cpp | 11 +++++------
 1 file changed, 5 insertions(+), 6 deletions(-)

diff --git a/core/string/string_builder.cpp b/core/string/string_builder.cpp
index 7f0a6abc04..91ac8a09bb 100644
--- a/core/string/string_builder.cpp
+++ b/core/string/string_builder.cpp
@@ -61,7 +61,9 @@ String StringBuilder::as_string() const {
 		return "";
 	}
 
-	char32_t *buffer = memnew_arr(char32_t, string_length);
+	String string;
+	string.resize(string_length + 1);
+	char32_t *buffer = string.ptrw();
 
 	int current_position = 0;
 
@@ -92,10 +94,7 @@ String StringBuilder::as_string() const {
 			c_string_elem++;
 		}
 	}
+	buffer[current_position] = 0;
 
-	String final_string = String(buffer, string_length);
-
-	memdelete_arr(buffer);
-
-	return final_string;
+	return string;
 }

From f8827271d734cc5a19bd5294be36b9628c480dce Mon Sep 17 00:00:00 2001
From: clayjohn <claynjohn@gmail.com>
Date: Thu, 12 Dec 2024 00:19:57 -0800
Subject: [PATCH 087/124] Remove positional light mask from directional lights
 in Canvas Item shaders.

---
 drivers/gles3/shaders/canvas.glsl                 | 2 +-
 servers/rendering/renderer_rd/shaders/canvas.glsl | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/drivers/gles3/shaders/canvas.glsl b/drivers/gles3/shaders/canvas.glsl
index 3857aa8841..5ed4dbcc85 100644
--- a/drivers/gles3/shaders/canvas.glsl
+++ b/drivers/gles3/shaders/canvas.glsl
@@ -731,7 +731,7 @@ void main() {
 		}
 #endif
 
-		if (bool(light_array[light_base].flags & LIGHT_FLAGS_HAS_SHADOW) && bool(read_draw_data_flags & uint(INSTANCE_FLAGS_SHADOW_MASKED << i))) {
+		if (bool(light_array[light_base].flags & LIGHT_FLAGS_HAS_SHADOW)) {
 			vec2 shadow_pos = (vec4(shadow_vertex, 0.0, 1.0) * mat4(light_array[light_base].shadow_matrix[0], light_array[light_base].shadow_matrix[1], vec4(0.0, 0.0, 1.0, 0.0), vec4(0.0, 0.0, 0.0, 1.0))).xy; //multiply inverse given its transposed. Optimizer removes useless operations.
 
 			vec4 shadow_uv = vec4(shadow_pos.x, light_array[light_base].shadow_y_ofs, shadow_pos.y * light_array[light_base].shadow_zfar_inv, 1.0);
diff --git a/servers/rendering/renderer_rd/shaders/canvas.glsl b/servers/rendering/renderer_rd/shaders/canvas.glsl
index b66aa71f6b..c1510a11ca 100644
--- a/servers/rendering/renderer_rd/shaders/canvas.glsl
+++ b/servers/rendering/renderer_rd/shaders/canvas.glsl
@@ -630,7 +630,7 @@ void main() {
 			}
 #endif
 
-			if (bool(light_array.data[light_base].flags & LIGHT_FLAGS_HAS_SHADOW) && bool(draw_data.flags & (INSTANCE_FLAGS_SHADOW_MASKED << i))) {
+			if (bool(light_array.data[light_base].flags & LIGHT_FLAGS_HAS_SHADOW)) {
 				vec2 shadow_pos = (vec4(shadow_vertex, 0.0, 1.0) * mat4(light_array.data[light_base].shadow_matrix[0], light_array.data[light_base].shadow_matrix[1], vec4(0.0, 0.0, 1.0, 0.0), vec4(0.0, 0.0, 0.0, 1.0))).xy; //multiply inverse given its transposed. Optimizer removes useless operations.
 
 				vec4 shadow_uv = vec4(shadow_pos.x, light_array.data[light_base].shadow_y_ofs, shadow_pos.y * light_array.data[light_base].shadow_zfar_inv, 1.0);

From 63b91381abd1d698884d1ae7a94c4819b0b05514 Mon Sep 17 00:00:00 2001
From: Uumutunal <unal.umut98@gmail.com>
Date: Thu, 12 Dec 2024 11:37:24 +0300
Subject: [PATCH 088/124] Fix 3D editor snap setting values not being displayed
 correctly

---
 editor/plugins/node_3d_editor_plugin.cpp | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/editor/plugins/node_3d_editor_plugin.cpp b/editor/plugins/node_3d_editor_plugin.cpp
index f000840695..037c165d43 100644
--- a/editor/plugins/node_3d_editor_plugin.cpp
+++ b/editor/plugins/node_3d_editor_plugin.cpp
@@ -6629,9 +6629,12 @@ void Node3DEditor::_snap_changed() {
 }
 
 void Node3DEditor::_snap_update() {
-	snap_translate->set_text(String::num(snap_translate_value));
-	snap_rotate->set_text(String::num(snap_rotate_value));
-	snap_scale->set_text(String::num(snap_scale_value));
+	double snap = EDITOR_GET("interface/inspector/default_float_step");
+	int snap_step_decimals = Math::range_step_decimals(snap);
+
+	snap_translate->set_text(String::num(snap_translate_value, snap_step_decimals));
+	snap_rotate->set_text(String::num(snap_rotate_value, snap_step_decimals));
+	snap_scale->set_text(String::num(snap_scale_value, snap_step_decimals));
 }
 
 void Node3DEditor::_xform_dialog_action() {

From 189c8eb67107b90f5635d825d9c6fcb2ae5c69c3 Mon Sep 17 00:00:00 2001
From: BlueCube3310 <53150244+BlueCube3310@users.noreply.github.com>
Date: Thu, 30 Nov 2023 17:14:45 +0100
Subject: [PATCH 089/124] Implement shadowmasks for LightmapGI

Co-authored-by: dearthdev <nathandearthdev@gmail.com>
---
 doc/classes/LightmapGI.xml                    |   5 +
 doc/classes/LightmapGIData.xml                |  14 +
 drivers/gles3/rasterizer_scene_gles3.cpp      |  58 ++-
 drivers/gles3/shaders/scene.glsl              | 223 ++++++----
 drivers/gles3/storage/light_storage.cpp       |  27 ++
 drivers/gles3/storage/light_storage.h         |   8 +-
 modules/lightmapper_rd/lightmapper_rd.cpp     | 173 ++++++--
 modules/lightmapper_rd/lightmapper_rd.h       |  23 +-
 modules/lightmapper_rd/lm_common_inc.glsl     |   3 +
 modules/lightmapper_rd/lm_compute.glsl        |  31 +-
 scene/3d/lightmap_gi.cpp                      | 132 +++++-
 scene/3d/lightmap_gi.h                        |  27 +-
 scene/3d/lightmapper.h                        |  11 +-
 .../rendering/dummy/storage/light_storage.h   |   4 +
 .../render_forward_clustered.cpp              |  33 +-
 .../render_forward_clustered.h                |   2 +-
 .../forward_mobile/render_forward_mobile.cpp  |  31 +-
 .../forward_mobile/render_forward_mobile.h    |   2 +-
 .../scene_forward_clustered.glsl              | 404 ++++++++++--------
 .../scene_forward_clustered_inc.glsl          |   9 +-
 .../forward_mobile/scene_forward_mobile.glsl  | 218 ++++++----
 .../scene_forward_mobile_inc.glsl             |   9 +-
 .../renderer_rd/storage_rd/light_storage.cpp  |  68 ++-
 .../renderer_rd/storage_rd/light_storage.h    |  14 +
 servers/rendering/rendering_server_default.h  |   4 +
 servers/rendering/storage/light_storage.h     |   4 +
 servers/rendering_server.h                    |  12 +-
 27 files changed, 1098 insertions(+), 451 deletions(-)

diff --git a/doc/classes/LightmapGI.xml b/doc/classes/LightmapGI.xml
index 837da1794f..a2491dac47 100644
--- a/doc/classes/LightmapGI.xml
+++ b/doc/classes/LightmapGI.xml
@@ -69,6 +69,11 @@
 			The quality preset to use when baking lightmaps. This affects bake times, but output file sizes remain mostly identical across quality levels.
 			To further speed up bake times, decrease [member bounces], disable [member use_denoiser] and increase the lightmap texel size on 3D scenes in the Import dock.
 		</member>
+		<member name="shadowmask_mode" type="int" setter="set_shadowmask_mode" getter="get_shadowmask_mode" enum="LightmapGIData.ShadowmaskMode" default="0" experimental="">
+			The shadowmasking policy to use for directional shadows on static objects that are baked with this [LightmapGI] instance.
+			Shadowmasking allows [DirectionalLight3D] nodes to cast shadows even outside the range defined by their [member DirectionalLight3D.directional_shadow_max_distance] property. This is done by baking a texture that contains a shadowmap for the directional light, then using this texture according to the current shadowmask mode.
+			[b]Note:[/b] The shadowmask texture is only created if [member shadowmask_mode] is not [constant LightmapGIData.SHADOWMASK_MODE_NONE]. To see a difference, you need to bake lightmaps again after switching from [constant LightmapGIData.SHADOWMASK_MODE_NONE] to any other mode.
+		</member>
 		<member name="texel_scale" type="float" setter="set_texel_scale" getter="get_texel_scale" default="1.0">
 			Scales the lightmap texel density of all meshes for the current bake. This is a multiplier that builds upon the existing lightmap texel size defined in each imported 3D scene, along with the per-mesh density multiplier (which is designed to be used when the same mesh is used at different scales). Lower values will result in faster bake times.
 			For example, doubling [member texel_scale] doubles the lightmap texture resolution for all objects [i]on each axis[/i], so it will [i]quadruple[/i] the texel count.
diff --git a/doc/classes/LightmapGIData.xml b/doc/classes/LightmapGIData.xml
index 76824f84a0..e7454f4ea8 100644
--- a/doc/classes/LightmapGIData.xml
+++ b/doc/classes/LightmapGIData.xml
@@ -60,5 +60,19 @@
 		<member name="lightmap_textures" type="TextureLayered[]" setter="set_lightmap_textures" getter="get_lightmap_textures" default="[]">
 			The lightmap atlas textures generated by the lightmapper.
 		</member>
+		<member name="shadowmask_textures" type="TextureLayered[]" setter="set_shadowmask_textures" getter="get_shadowmask_textures" default="[]">
+			The shadowmask atlas textures generated by the lightmapper.
+		</member>
 	</members>
+	<constants>
+		<constant name="SHADOWMASK_MODE_NONE" value="0" enum="ShadowmaskMode">
+			Shadowmasking is disabled. No shadowmask texture will be created when baking lightmaps. Existing shadowmask textures will be removed during baking.
+		</constant>
+		<constant name="SHADOWMASK_MODE_REPLACE" value="1" enum="ShadowmaskMode">
+			Shadowmasking is enabled. Directional shadows that are outside the [member DirectionalLight3D.directional_shadow_max_distance] will be rendered using the shadowmask texture. Shadows that are inside the range will be rendered using real-time shadows exclusively. This mode allows for more precise real-time shadows up close, without the potential "smearing" effect that can occur when using lightmaps with a high texel size. The downside is that when the camera moves fast, the transition between the real-time light and shadowmask can be obvious. Also, objects that only have shadows baked in the shadowmask (and no real-time shadows) won't display any shadows up close.
+		</constant>
+		<constant name="SHADOWMASK_MODE_OVERLAY" value="2" enum="ShadowmaskMode">
+			Shadowmasking is enabled. Directional shadows will be rendered with real-time shadows overlaid on top of the shadowmask texture. This mode makes for smoother shadow transitions when the camera moves fast, at the cost of a potential smearing effect for directional shadows that are up close (due to the real-time shadow being mixed with a low-resolution shadowmask). Objects that only have shadows baked in the shadowmask (and no real-time shadows) will keep their shadows up close.
+		</constant>
+	</constants>
 </class>
diff --git a/drivers/gles3/rasterizer_scene_gles3.cpp b/drivers/gles3/rasterizer_scene_gles3.cpp
index cd4c7dd82f..907e57823b 100644
--- a/drivers/gles3/rasterizer_scene_gles3.cpp
+++ b/drivers/gles3/rasterizer_scene_gles3.cpp
@@ -2660,14 +2660,14 @@ void RasterizerSceneGLES3::render_scene(const Ref<RenderSceneBuffers> &p_render_
 				glBlitFramebuffer(0, 0, size.x, size.y,
 						0, 0, size.x, size.y,
 						GL_COLOR_BUFFER_BIT, GL_NEAREST);
-				glActiveTexture(GL_TEXTURE0 + config->max_texture_image_units - 5);
+				glActiveTexture(GL_TEXTURE0 + config->max_texture_image_units - 6);
 				glBindTexture(GL_TEXTURE_2D, backbuffer);
 			}
 			if (scene_state.used_depth_texture) {
 				glBlitFramebuffer(0, 0, size.x, size.y,
 						0, 0, size.x, size.y,
 						GL_DEPTH_BUFFER_BIT, GL_NEAREST);
-				glActiveTexture(GL_TEXTURE0 + config->max_texture_image_units - 6);
+				glActiveTexture(GL_TEXTURE0 + config->max_texture_image_units - 7);
 				glBindTexture(GL_TEXTURE_2D, backbuffer_depth);
 			}
 		}
@@ -3245,8 +3245,28 @@ void RasterizerSceneGLES3::_render_list_template(RenderListParameters *p_params,
 					spec_constants |= SceneShaderGLES3::DISABLE_LIGHT_OMNI;
 					spec_constants |= SceneShaderGLES3::DISABLE_LIGHT_SPOT;
 					spec_constants |= SceneShaderGLES3::DISABLE_LIGHT_DIRECTIONAL;
-					spec_constants |= SceneShaderGLES3::DISABLE_LIGHTMAP;
 					spec_constants |= SceneShaderGLES3::DISABLE_REFLECTION_PROBE;
+
+					bool disable_lightmaps = true;
+
+					// Additive directional passes may use shadowmasks, so enable lightmaps for them.
+					if (pass >= int32_t(inst->light_passes.size()) && inst->lightmap_instance.is_valid()) {
+						GLES3::LightmapInstance *li = GLES3::LightStorage::get_singleton()->get_lightmap_instance(inst->lightmap_instance);
+						GLES3::Lightmap *lm = GLES3::LightStorage::get_singleton()->get_lightmap(li->lightmap);
+
+						if (lm->shadowmask_mode != RS::SHADOWMASK_MODE_NONE) {
+							spec_constants |= SceneShaderGLES3::USE_LIGHTMAP;
+							disable_lightmaps = false;
+
+							if (lightmap_bicubic_upscale) {
+								spec_constants |= SceneShaderGLES3::LIGHTMAP_BICUBIC_FILTER;
+							}
+						}
+					}
+
+					if (disable_lightmaps) {
+						spec_constants |= SceneShaderGLES3::DISABLE_LIGHTMAP;
+					}
 				}
 
 				if (uses_additive_lighting) {
@@ -3341,6 +3361,33 @@ void RasterizerSceneGLES3::_render_list_template(RenderListParameters *p_params,
 						GLuint tex = GLES3::LightStorage::get_singleton()->directional_shadow_get_texture();
 						glActiveTexture(GL_TEXTURE0 + config->max_texture_image_units - 3);
 						glBindTexture(GL_TEXTURE_2D, tex);
+
+						if (inst->lightmap_instance.is_valid()) {
+							// Use shadowmasks for directional light passes.
+							GLES3::LightmapInstance *li = GLES3::LightStorage::get_singleton()->get_lightmap_instance(inst->lightmap_instance);
+							GLES3::Lightmap *lm = GLES3::LightStorage::get_singleton()->get_lightmap(li->lightmap);
+
+							material_storage->shaders.scene_shader.version_set_uniform(SceneShaderGLES3::LIGHTMAP_SLICE, inst->lightmap_slice_index, shader->version, instance_variant, spec_constants);
+
+							Vector4 uv_scale(inst->lightmap_uv_scale.position.x, inst->lightmap_uv_scale.position.y, inst->lightmap_uv_scale.size.x, inst->lightmap_uv_scale.size.y);
+							material_storage->shaders.scene_shader.version_set_uniform(SceneShaderGLES3::LIGHTMAP_UV_SCALE, uv_scale, shader->version, instance_variant, spec_constants);
+
+							if (lightmap_bicubic_upscale) {
+								Vector2 light_texture_size(lm->light_texture_size.x, lm->light_texture_size.y);
+								material_storage->shaders.scene_shader.version_set_uniform(SceneShaderGLES3::LIGHTMAP_TEXTURE_SIZE, light_texture_size, shader->version, instance_variant, spec_constants);
+							}
+
+							material_storage->shaders.scene_shader.version_set_uniform(SceneShaderGLES3::LIGHTMAP_SHADOWMASK_MODE, (uint32_t)lm->shadowmask_mode, shader->version, instance_variant, spec_constants);
+
+							if (lm->shadow_texture.is_valid()) {
+								tex = GLES3::TextureStorage::get_singleton()->texture_get_texid(lm->shadow_texture);
+							} else {
+								tex = GLES3::TextureStorage::get_singleton()->texture_get_texid(GLES3::TextureStorage::get_singleton()->texture_gl_get_default(GLES3::DEFAULT_GL_TEXTURE_2D_ARRAY_WHITE));
+							}
+
+							glActiveTexture(GL_TEXTURE0 + config->max_texture_image_units - 5);
+							glBindTexture(GL_TEXTURE_2D_ARRAY, tex);
+						}
 					}
 				}
 
@@ -3399,6 +3446,7 @@ void RasterizerSceneGLES3::_render_list_template(RenderListParameters *p_params,
 							};
 							glUniformMatrix3fv(material_storage->shaders.scene_shader.version_get_uniform(SceneShaderGLES3::LIGHTMAP_NORMAL_XFORM, shader->version, instance_variant, spec_constants), 1, GL_FALSE, matrix);
 						}
+
 					} else if (inst->lightmap_sh) {
 						glUniform4fv(material_storage->shaders.scene_shader.version_get_uniform(SceneShaderGLES3::LIGHTMAP_CAPTURES, shader->version, instance_variant, spec_constants), 9, reinterpret_cast<const GLfloat *>(inst->lightmap_sh->sh));
 					}
@@ -3430,7 +3478,7 @@ void RasterizerSceneGLES3::_render_list_template(RenderListParameters *p_params,
 						material_storage->shaders.scene_shader.version_set_uniform(SceneShaderGLES3::REFPROBE1_AMBIENT_COLOR, probe->ambient_color * probe->ambient_color_energy, shader->version, instance_variant, spec_constants);
 						material_storage->shaders.scene_shader.version_set_uniform(SceneShaderGLES3::REFPROBE1_LOCAL_MATRIX, inst->reflection_probes_local_transform_cache[0], shader->version, instance_variant, spec_constants);
 
-						glActiveTexture(GL_TEXTURE0 + config->max_texture_image_units - 7);
+						glActiveTexture(GL_TEXTURE0 + config->max_texture_image_units - 8);
 						glBindTexture(GL_TEXTURE_CUBE_MAP, light_storage->reflection_probe_instance_get_texture(inst->reflection_probe_rid_cache[0]));
 					}
 
@@ -3448,7 +3496,7 @@ void RasterizerSceneGLES3::_render_list_template(RenderListParameters *p_params,
 						material_storage->shaders.scene_shader.version_set_uniform(SceneShaderGLES3::REFPROBE2_AMBIENT_COLOR, probe->ambient_color * probe->ambient_color_energy, shader->version, instance_variant, spec_constants);
 						material_storage->shaders.scene_shader.version_set_uniform(SceneShaderGLES3::REFPROBE2_LOCAL_MATRIX, inst->reflection_probes_local_transform_cache[1], shader->version, instance_variant, spec_constants);
 
-						glActiveTexture(GL_TEXTURE0 + config->max_texture_image_units - 8);
+						glActiveTexture(GL_TEXTURE0 + config->max_texture_image_units - 9);
 						glBindTexture(GL_TEXTURE_CUBE_MAP, light_storage->reflection_probe_instance_get_texture(inst->reflection_probe_rid_cache[1]));
 
 						spec_constants |= SceneShaderGLES3::SECOND_REFLECTION_PROBE;
diff --git a/drivers/gles3/shaders/scene.glsl b/drivers/gles3/shaders/scene.glsl
index fd195fe7c4..f8b495b7fb 100644
--- a/drivers/gles3/shaders/scene.glsl
+++ b/drivers/gles3/shaders/scene.glsl
@@ -809,10 +809,11 @@ void main() {
 2-radiance
 3-shadow
 4-lightmap textures
-5-screen
-6-depth
-7-reflection probe 1
-8-reflection probe 2
+5-shadowmask textures
+6-screen
+7-depth
+8-reflection probe 1
+9-reflection probe 2
 
 */
 
@@ -887,7 +888,7 @@ uniform float refprobe1_intensity;
 uniform int refprobe1_ambient_mode;
 uniform vec4 refprobe1_ambient_color;
 
-uniform samplerCube refprobe1_texture; // texunit:-7
+uniform samplerCube refprobe1_texture; // texunit:-8
 
 #ifdef SECOND_REFLECTION_PROBE
 
@@ -900,7 +901,7 @@ uniform float refprobe2_intensity;
 uniform int refprobe2_ambient_mode;
 uniform vec4 refprobe2_ambient_color;
 
-uniform samplerCube refprobe2_texture; // texunit:-8
+uniform samplerCube refprobe2_texture; // texunit:-9
 
 #endif // SECOND_REFLECTION_PROBE
 
@@ -1170,9 +1171,16 @@ float sample_shadow(highp sampler2DShadow shadow, float shadow_pixel_size, vec4
 #ifndef DISABLE_LIGHTMAP
 #ifdef USE_LIGHTMAP
 uniform mediump sampler2DArray lightmap_textures; //texunit:-4
+uniform lowp sampler2DArray shadowmask_textures; //texunit:-5
 uniform lowp uint lightmap_slice;
 uniform highp vec4 lightmap_uv_scale;
 uniform float lightmap_exposure_normalization;
+uniform uint lightmap_shadowmask_mode;
+
+#define SHADOWMASK_MODE_NONE uint(0)
+#define SHADOWMASK_MODE_REPLACE uint(1)
+#define SHADOWMASK_MODE_OVERLAY uint(2)
+#define SHADOWMASK_MODE_ONLY uint(3)
 
 #ifdef LIGHTMAP_BICUBIC_FILTER
 uniform highp vec2 lightmap_texture_size;
@@ -1189,8 +1197,8 @@ uniform mediump vec4[9] lightmap_captures;
 #endif // !DISABLE_LIGHTMAP
 
 #ifdef USE_MULTIVIEW
-uniform highp sampler2DArray depth_buffer; // texunit:-6
-uniform highp sampler2DArray color_buffer; // texunit:-5
+uniform highp sampler2DArray depth_buffer; // texunit:-7
+uniform highp sampler2DArray color_buffer; // texunit:-6
 vec3 multiview_uv(vec2 uv) {
 	return vec3(uv, ViewIndex);
 }
@@ -1198,8 +1206,8 @@ ivec3 multiview_uv(ivec2 uv) {
 	return ivec3(uv, int(ViewIndex));
 }
 #else
-uniform highp sampler2D depth_buffer; // texunit:-6
-uniform highp sampler2D color_buffer; // texunit:-5
+uniform highp sampler2D depth_buffer; // texunit:-7
+uniform highp sampler2D color_buffer; // texunit:-6
 vec2 multiview_uv(vec2 uv) {
 	return uv;
 }
@@ -2278,111 +2286,146 @@ void main() {
 #if !defined(ADDITIVE_OMNI) && !defined(ADDITIVE_SPOT)
 
 #ifndef SHADOWS_DISABLED
+// Baked shadowmasks
+#ifdef USE_LIGHTMAP
+	float shadowmask = 1.0f;
+
+	if (lightmap_shadowmask_mode != SHADOWMASK_MODE_NONE) {
+		vec3 uvw;
+		uvw.xy = uv2 * lightmap_uv_scale.zw + lightmap_uv_scale.xy;
+		uvw.z = float(lightmap_slice);
+
+#ifdef LIGHTMAP_BICUBIC_FILTER
+		shadowmask = textureArray_bicubic(shadowmask_textures, uvw, lightmap_texture_size).x;
+#else
+		shadowmask = textureLod(shadowmask_textures, uvw, 0.0).x;
+#endif
+	}
+#endif //USE_LIGHTMAP
+
+	float directional_shadow = 1.0;
+
+#ifdef USE_LIGHTMAP
+	if (lightmap_shadowmask_mode != SHADOWMASK_MODE_ONLY) {
+#endif
 
 // Orthogonal shadows
 #if !defined(LIGHT_USE_PSSM2) && !defined(LIGHT_USE_PSSM4)
-	float directional_shadow = sample_shadow(directional_shadow_atlas, directional_shadows[directional_shadow_index].shadow_atlas_pixel_size, shadow_coord);
+		directional_shadow = sample_shadow(directional_shadow_atlas, directional_shadows[directional_shadow_index].shadow_atlas_pixel_size, shadow_coord);
 #endif // !defined(LIGHT_USE_PSSM2) && !defined(LIGHT_USE_PSSM4)
 
 // PSSM2 shadows
 #ifdef LIGHT_USE_PSSM2
-	float depth_z = -vertex.z;
-	vec4 light_split_offsets = directional_shadows[directional_shadow_index].shadow_split_offsets;
-	//take advantage of prefetch
-	float shadow1 = sample_shadow(directional_shadow_atlas, directional_shadows[directional_shadow_index].shadow_atlas_pixel_size, shadow_coord);
-	float shadow2 = sample_shadow(directional_shadow_atlas, directional_shadows[directional_shadow_index].shadow_atlas_pixel_size, shadow_coord2);
-	float directional_shadow = 1.0;
+		float depth_z = -vertex.z;
+		vec4 light_split_offsets = directional_shadows[directional_shadow_index].shadow_split_offsets;
+		//take advantage of prefetch
+		float shadow1 = sample_shadow(directional_shadow_atlas, directional_shadows[directional_shadow_index].shadow_atlas_pixel_size, shadow_coord);
+		float shadow2 = sample_shadow(directional_shadow_atlas, directional_shadows[directional_shadow_index].shadow_atlas_pixel_size, shadow_coord2);
 
-	if (depth_z < light_split_offsets.y) {
-
-#ifdef LIGHT_USE_PSSM_BLEND
-		float directional_shadow2 = 1.0;
-		float pssm_blend = 0.0;
-		bool use_blend = true;
-#endif
-		if (depth_z < light_split_offsets.x) {
-			directional_shadow = shadow1;
-
-#ifdef LIGHT_USE_PSSM_BLEND
-			directional_shadow2 = shadow2;
-			pssm_blend = smoothstep(0.0, light_split_offsets.x, depth_z);
-#endif
-		} else {
-			directional_shadow = shadow2;
-#ifdef LIGHT_USE_PSSM_BLEND
-			use_blend = false;
-#endif
-		}
-#ifdef LIGHT_USE_PSSM_BLEND
-		if (use_blend) {
-			directional_shadow = mix(directional_shadow, directional_shadow2, pssm_blend);
-		}
-#endif
-	}
-
-#endif //LIGHT_USE_PSSM2
-// PSSM4 shadows
-#ifdef LIGHT_USE_PSSM4
-	float depth_z = -vertex.z;
-	vec4 light_split_offsets = directional_shadows[directional_shadow_index].shadow_split_offsets;
-
-	float shadow1 = sample_shadow(directional_shadow_atlas, directional_shadows[directional_shadow_index].shadow_atlas_pixel_size, shadow_coord);
-	float shadow2 = sample_shadow(directional_shadow_atlas, directional_shadows[directional_shadow_index].shadow_atlas_pixel_size, shadow_coord2);
-	float shadow3 = sample_shadow(directional_shadow_atlas, directional_shadows[directional_shadow_index].shadow_atlas_pixel_size, shadow_coord3);
-	float shadow4 = sample_shadow(directional_shadow_atlas, directional_shadows[directional_shadow_index].shadow_atlas_pixel_size, shadow_coord4);
-	float directional_shadow = 1.0;
-
-	if (depth_z < light_split_offsets.w) {
-
-#ifdef LIGHT_USE_PSSM_BLEND
-		float directional_shadow2 = 1.0;
-		float pssm_blend = 0.0;
-		bool use_blend = true;
-#endif
 		if (depth_z < light_split_offsets.y) {
+
+#ifdef LIGHT_USE_PSSM_BLEND
+			float directional_shadow2 = 1.0;
+			float pssm_blend = 0.0;
+			bool use_blend = true;
+#endif
 			if (depth_z < light_split_offsets.x) {
 				directional_shadow = shadow1;
 
 #ifdef LIGHT_USE_PSSM_BLEND
 				directional_shadow2 = shadow2;
-
 				pssm_blend = smoothstep(0.0, light_split_offsets.x, depth_z);
 #endif
 			} else {
 				directional_shadow = shadow2;
-
 #ifdef LIGHT_USE_PSSM_BLEND
-				directional_shadow2 = shadow3;
-
-				pssm_blend = smoothstep(light_split_offsets.x, light_split_offsets.y, depth_z);
-#endif
-			}
-		} else {
-			if (depth_z < light_split_offsets.z) {
-				directional_shadow = shadow3;
-
-#if defined(LIGHT_USE_PSSM_BLEND)
-				directional_shadow2 = shadow4;
-				pssm_blend = smoothstep(light_split_offsets.y, light_split_offsets.z, depth_z);
-#endif
-
-			} else {
-				directional_shadow = shadow4;
-
-#if defined(LIGHT_USE_PSSM_BLEND)
 				use_blend = false;
 #endif
 			}
-		}
-#if defined(LIGHT_USE_PSSM_BLEND)
-		if (use_blend) {
-			directional_shadow = mix(directional_shadow, directional_shadow2, pssm_blend);
-		}
+#ifdef LIGHT_USE_PSSM_BLEND
+			if (use_blend) {
+				directional_shadow = mix(directional_shadow, directional_shadow2, pssm_blend);
+			}
 #endif
-	}
+		}
+
+#endif //LIGHT_USE_PSSM2
+// PSSM4 shadows
+#ifdef LIGHT_USE_PSSM4
+		float depth_z = -vertex.z;
+		vec4 light_split_offsets = directional_shadows[directional_shadow_index].shadow_split_offsets;
+
+		float shadow1 = sample_shadow(directional_shadow_atlas, directional_shadows[directional_shadow_index].shadow_atlas_pixel_size, shadow_coord);
+		float shadow2 = sample_shadow(directional_shadow_atlas, directional_shadows[directional_shadow_index].shadow_atlas_pixel_size, shadow_coord2);
+		float shadow3 = sample_shadow(directional_shadow_atlas, directional_shadows[directional_shadow_index].shadow_atlas_pixel_size, shadow_coord3);
+		float shadow4 = sample_shadow(directional_shadow_atlas, directional_shadows[directional_shadow_index].shadow_atlas_pixel_size, shadow_coord4);
+
+		if (depth_z < light_split_offsets.w) {
+#ifdef LIGHT_USE_PSSM_BLEND
+			float directional_shadow2 = 1.0;
+			float pssm_blend = 0.0;
+			bool use_blend = true;
+#endif
+			if (depth_z < light_split_offsets.y) {
+				if (depth_z < light_split_offsets.x) {
+					directional_shadow = shadow1;
+
+#ifdef LIGHT_USE_PSSM_BLEND
+					directional_shadow2 = shadow2;
+
+					pssm_blend = smoothstep(0.0, light_split_offsets.x, depth_z);
+#endif
+				} else {
+					directional_shadow = shadow2;
+
+#ifdef LIGHT_USE_PSSM_BLEND
+					directional_shadow2 = shadow3;
+
+					pssm_blend = smoothstep(light_split_offsets.x, light_split_offsets.y, depth_z);
+#endif
+				}
+			} else {
+				if (depth_z < light_split_offsets.z) {
+					directional_shadow = shadow3;
+
+#if defined(LIGHT_USE_PSSM_BLEND)
+					directional_shadow2 = shadow4;
+					pssm_blend = smoothstep(light_split_offsets.y, light_split_offsets.z, depth_z);
+#endif
+
+				} else {
+					directional_shadow = shadow4;
+
+#if defined(LIGHT_USE_PSSM_BLEND)
+					use_blend = false;
+#endif
+				}
+			}
+#if defined(LIGHT_USE_PSSM_BLEND)
+			if (use_blend) {
+				directional_shadow = mix(directional_shadow, directional_shadow2, pssm_blend);
+			}
+#endif
+		}
 
 #endif //LIGHT_USE_PSSM4
-	directional_shadow = mix(directional_shadow, 1.0, smoothstep(directional_shadows[directional_shadow_index].fade_from, directional_shadows[directional_shadow_index].fade_to, vertex.z));
+
+#ifdef USE_LIGHTMAP
+		if (lightmap_shadowmask_mode == SHADOWMASK_MODE_REPLACE) {
+			directional_shadow = mix(directional_shadow, shadowmask, smoothstep(directional_shadows[directional_shadow_index].fade_from, directional_shadows[directional_shadow_index].fade_to, vertex.z));
+		} else if (lightmap_shadowmask_mode == SHADOWMASK_MODE_OVERLAY) {
+			directional_shadow = shadowmask * mix(directional_shadow, 1.0, smoothstep(directional_shadows[directional_shadow_index].fade_from, directional_shadows[directional_shadow_index].fade_to, vertex.z));
+		} else {
+#endif
+			directional_shadow = mix(directional_shadow, 1.0, smoothstep(directional_shadows[directional_shadow_index].fade_from, directional_shadows[directional_shadow_index].fade_to, vertex.z));
+#ifdef USE_LIGHTMAP
+		}
+
+	} else { // lightmap_shadowmask_mode == SHADOWMASK_MODE_ONLY
+		directional_shadow = shadowmask;
+	}
+#endif
+
 	directional_shadow = mix(1.0, directional_shadow, directional_lights[directional_shadow_index].shadow_opacity);
 
 #else
diff --git a/drivers/gles3/storage/light_storage.cpp b/drivers/gles3/storage/light_storage.cpp
index 886918a2f7..4ec37fe4ab 100644
--- a/drivers/gles3/storage/light_storage.cpp
+++ b/drivers/gles3/storage/light_storage.cpp
@@ -1204,6 +1204,33 @@ float LightStorage::lightmap_get_probe_capture_update_speed() const {
 	return lightmap_probe_capture_update_speed;
 }
 
+void LightStorage::lightmap_set_shadowmask_textures(RID p_lightmap, RID p_shadow) {
+	Lightmap *lightmap = lightmap_owner.get_or_null(p_lightmap);
+	ERR_FAIL_NULL(lightmap);
+	lightmap->shadow_texture = p_shadow;
+
+	GLuint tex = GLES3::TextureStorage::get_singleton()->texture_get_texid(lightmap->shadow_texture);
+	glBindTexture(GL_TEXTURE_2D_ARRAY, tex);
+	glTexParameteri(GL_TEXTURE_2D_ARRAY, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+	glTexParameteri(GL_TEXTURE_2D_ARRAY, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+	glTexParameteri(GL_TEXTURE_2D_ARRAY, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+	glTexParameteri(GL_TEXTURE_2D_ARRAY, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+	glBindTexture(GL_TEXTURE_2D_ARRAY, 0);
+}
+
+RS::ShadowmaskMode LightStorage::lightmap_get_shadowmask_mode(RID p_lightmap) {
+	Lightmap *lightmap = lightmap_owner.get_or_null(p_lightmap);
+	ERR_FAIL_NULL_V(lightmap, RS::SHADOWMASK_MODE_NONE);
+
+	return lightmap->shadowmask_mode;
+}
+
+void LightStorage::lightmap_set_shadowmask_mode(RID p_lightmap, RS::ShadowmaskMode p_mode) {
+	Lightmap *lightmap = lightmap_owner.get_or_null(p_lightmap);
+	ERR_FAIL_NULL(lightmap);
+	lightmap->shadowmask_mode = p_mode;
+}
+
 /* LIGHTMAP INSTANCE */
 
 RID LightStorage::lightmap_instance_create(RID p_lightmap) {
diff --git a/drivers/gles3/storage/light_storage.h b/drivers/gles3/storage/light_storage.h
index 0695102640..0bf410fac6 100644
--- a/drivers/gles3/storage/light_storage.h
+++ b/drivers/gles3/storage/light_storage.h
@@ -177,12 +177,14 @@ struct ReflectionProbeInstance {
 
 struct Lightmap {
 	RID light_texture;
+	RID shadow_texture;
 	bool uses_spherical_harmonics = false;
 	bool interior = false;
 	AABB bounds = AABB(Vector3(), Vector3(1, 1, 1));
 	float baked_exposure = 1.0;
 	Vector2i light_texture_size;
 	int32_t array_index = -1; //unassigned
+	RS::ShadowmaskMode shadowmask_mode = RS::SHADOWMASK_MODE_NONE;
 	PackedVector3Array points;
 	PackedColorArray point_sh;
 	PackedInt32Array tetrahedra;
@@ -231,8 +233,6 @@ private:
 	mutable RID_Owner<ReflectionProbeInstance> reflection_probe_instance_owner;
 
 	/* LIGHTMAP */
-
-	Vector<RID> lightmap_textures;
 	float lightmap_probe_capture_update_speed = 4;
 
 	mutable RID_Owner<Lightmap, true> lightmap_owner;
@@ -737,6 +737,10 @@ public:
 	virtual void lightmap_set_probe_capture_update_speed(float p_speed) override;
 	virtual float lightmap_get_probe_capture_update_speed() const override;
 
+	virtual void lightmap_set_shadowmask_textures(RID p_lightmap, RID p_shadow) override;
+	virtual RS::ShadowmaskMode lightmap_get_shadowmask_mode(RID p_lightmap) override;
+	virtual void lightmap_set_shadowmask_mode(RID p_lightmap, RS::ShadowmaskMode p_mode) override;
+
 	/* LIGHTMAP INSTANCE */
 
 	LightmapInstance *get_lightmap_instance(RID p_rid) { return lightmap_instance_owner.get_or_null(p_rid); }
diff --git a/modules/lightmapper_rd/lightmapper_rd.cpp b/modules/lightmapper_rd/lightmapper_rd.cpp
index 9cfeff4f69..a6074012de 100644
--- a/modules/lightmapper_rd/lightmapper_rd.cpp
+++ b/modules/lightmapper_rd/lightmapper_rd.cpp
@@ -62,7 +62,7 @@ void LightmapperRD::add_mesh(const MeshData &p_mesh) {
 	mesh_instances.push_back(mi);
 }
 
-void LightmapperRD::add_directional_light(bool p_static, const Vector3 &p_direction, const Color &p_color, float p_energy, float p_indirect_energy, float p_angular_distance, float p_shadow_blur) {
+void LightmapperRD::add_directional_light(const String &p_name, bool p_static, const Vector3 &p_direction, const Color &p_color, float p_energy, float p_indirect_energy, float p_angular_distance, float p_shadow_blur) {
 	Light l;
 	l.type = LIGHT_TYPE_DIRECTIONAL;
 	l.direction[0] = p_direction.x;
@@ -77,9 +77,10 @@ void LightmapperRD::add_directional_light(bool p_static, const Vector3 &p_direct
 	l.size = Math::tan(Math::deg_to_rad(p_angular_distance));
 	l.shadow_blur = p_shadow_blur;
 	lights.push_back(l);
+	light_names.push_back(p_name);
 }
 
-void LightmapperRD::add_omni_light(bool p_static, const Vector3 &p_position, const Color &p_color, float p_energy, float p_indirect_energy, float p_range, float p_attenuation, float p_size, float p_shadow_blur) {
+void LightmapperRD::add_omni_light(const String &p_name, bool p_static, const Vector3 &p_position, const Color &p_color, float p_energy, float p_indirect_energy, float p_range, float p_attenuation, float p_size, float p_shadow_blur) {
 	Light l;
 	l.type = LIGHT_TYPE_OMNI;
 	l.position[0] = p_position.x;
@@ -96,9 +97,10 @@ void LightmapperRD::add_omni_light(bool p_static, const Vector3 &p_position, con
 	l.size = p_size;
 	l.shadow_blur = p_shadow_blur;
 	lights.push_back(l);
+	light_names.push_back(p_name);
 }
 
-void LightmapperRD::add_spot_light(bool p_static, const Vector3 &p_position, const Vector3 p_direction, const Color &p_color, float p_energy, float p_indirect_energy, float p_range, float p_attenuation, float p_spot_angle, float p_spot_attenuation, float p_size, float p_shadow_blur) {
+void LightmapperRD::add_spot_light(const String &p_name, bool p_static, const Vector3 &p_position, const Vector3 p_direction, const Color &p_color, float p_energy, float p_indirect_energy, float p_range, float p_attenuation, float p_spot_angle, float p_spot_attenuation, float p_size, float p_shadow_blur) {
 	Light l;
 	l.type = LIGHT_TYPE_SPOT;
 	l.position[0] = p_position.x;
@@ -120,6 +122,7 @@ void LightmapperRD::add_spot_light(bool p_static, const Vector3 &p_position, con
 	l.size = p_size;
 	l.shadow_blur = p_shadow_blur;
 	lights.push_back(l);
+	light_names.push_back(p_name);
 }
 
 void LightmapperRD::add_probe(const Vector3 &p_position) {
@@ -826,9 +829,9 @@ LightmapperRD::BakeError LightmapperRD::_pack_l1(RenderingDevice *rd, Ref<RDShad
 	return BAKE_OK;
 }
 
-Error LightmapperRD::_store_pfm(RenderingDevice *p_rd, RID p_atlas_tex, int p_index, const Size2i &p_atlas_size, const String &p_name) {
+Error LightmapperRD::_store_pfm(RenderingDevice *p_rd, RID p_atlas_tex, int p_index, const Size2i &p_atlas_size, const String &p_name, bool p_shadowmask) {
 	Vector<uint8_t> data = p_rd->texture_get_data(p_atlas_tex, p_index);
-	Ref<Image> img = Image::create_from_data(p_atlas_size.width, p_atlas_size.height, false, Image::FORMAT_RGBAH, data);
+	Ref<Image> img = Image::create_from_data(p_atlas_size.width, p_atlas_size.height, false, p_shadowmask ? Image::FORMAT_RGBA8 : Image::FORMAT_RGBAH, data);
 	img->convert(Image::FORMAT_RGBF);
 	Vector<uint8_t> data_float = img->get_data();
 
@@ -848,7 +851,7 @@ Error LightmapperRD::_store_pfm(RenderingDevice *p_rd, RID p_atlas_tex, int p_in
 	return OK;
 }
 
-Ref<Image> LightmapperRD::_read_pfm(const String &p_name) {
+Ref<Image> LightmapperRD::_read_pfm(const String &p_name, bool p_shadowmask) {
 	Error err = OK;
 	Ref<FileAccess> file = FileAccess::open(p_name, FileAccess::READ, &err);
 	ERR_FAIL_COND_V_MSG(err, Ref<Image>(), vformat("Can't load PFM at path: '%s'.", p_name));
@@ -881,23 +884,23 @@ Ref<Image> LightmapperRD::_read_pfm(const String &p_name) {
 	}
 #endif
 	Ref<Image> img = Image::create_from_data(new_width, new_height, false, Image::FORMAT_RGBF, new_data);
-	img->convert(Image::FORMAT_RGBAH);
+	img->convert(p_shadowmask ? Image::FORMAT_RGBA8 : Image::FORMAT_RGBAH);
 	return img;
 }
 
-LightmapperRD::BakeError LightmapperRD::_denoise_oidn(RenderingDevice *p_rd, RID p_source_light_tex, RID p_source_normal_tex, RID p_dest_light_tex, const Size2i &p_atlas_size, int p_atlas_slices, bool p_bake_sh, const String &p_exe) {
+LightmapperRD::BakeError LightmapperRD::_denoise_oidn(RenderingDevice *p_rd, RID p_source_light_tex, RID p_source_normal_tex, RID p_dest_light_tex, const Size2i &p_atlas_size, int p_atlas_slices, bool p_bake_sh, bool p_shadowmask, const String &p_exe) {
 	Ref<DirAccess> da = DirAccess::create(DirAccess::ACCESS_FILESYSTEM);
 
 	for (int i = 0; i < p_atlas_slices; i++) {
 		String fname_norm_in = EditorPaths::get_singleton()->get_cache_dir().path_join(vformat("temp_norm_%d.pfm", i));
-		_store_pfm(p_rd, p_source_normal_tex, i, p_atlas_size, fname_norm_in);
+		_store_pfm(p_rd, p_source_normal_tex, i, p_atlas_size, fname_norm_in, false);
 
 		for (int j = 0; j < (p_bake_sh ? 4 : 1); j++) {
 			int index = i * (p_bake_sh ? 4 : 1) + j;
 			String fname_light_in = EditorPaths::get_singleton()->get_cache_dir().path_join(vformat("temp_light_%d.pfm", index));
 			String fname_out = EditorPaths::get_singleton()->get_cache_dir().path_join(vformat("temp_denoised_%d.pfm", index));
 
-			_store_pfm(p_rd, p_source_light_tex, index, p_atlas_size, fname_light_in);
+			_store_pfm(p_rd, p_source_light_tex, index, p_atlas_size, fname_light_in, p_shadowmask);
 
 			List<String> args;
 			args.push_back("--device");
@@ -906,7 +909,7 @@ LightmapperRD::BakeError LightmapperRD::_denoise_oidn(RenderingDevice *p_rd, RID
 			args.push_back("--filter");
 			args.push_back("RTLightmap");
 
-			args.push_back("--hdr");
+			args.push_back(p_shadowmask ? "--ldr" : "--hdr");
 			args.push_back(fname_light_in);
 
 			args.push_back("--nrm");
@@ -928,7 +931,7 @@ LightmapperRD::BakeError LightmapperRD::_denoise_oidn(RenderingDevice *p_rd, RID
 				ERR_FAIL_V_MSG(BAKE_ERROR_LIGHTMAP_CANT_PRE_BAKE_MESHES, vformat("OIDN denoiser failed, return code: %d", exitcode));
 			}
 
-			Ref<Image> img = _read_pfm(fname_out);
+			Ref<Image> img = _read_pfm(fname_out, p_shadowmask);
 			da->remove(fname_out);
 
 			ERR_FAIL_COND_V(img.is_null(), BAKE_ERROR_LIGHTMAP_CANT_PRE_BAKE_MESHES);
@@ -1029,7 +1032,7 @@ LightmapperRD::BakeError LightmapperRD::_denoise(RenderingDevice *p_rd, Ref<RDSh
 	return BAKE_OK;
 }
 
-LightmapperRD::BakeError LightmapperRD::bake(BakeQuality p_quality, bool p_use_denoiser, float p_denoiser_strength, int p_denoiser_range, int p_bounces, float p_bounce_indirect_energy, float p_bias, int p_max_texture_size, bool p_bake_sh, bool p_texture_for_bounces, GenerateProbes p_generate_probes, const Ref<Image> &p_environment_panorama, const Basis &p_environment_transform, BakeStepFunc p_step_function, void *p_bake_userdata, float p_exposure_normalization) {
+LightmapperRD::BakeError LightmapperRD::bake(BakeQuality p_quality, bool p_use_denoiser, float p_denoiser_strength, int p_denoiser_range, int p_bounces, float p_bounce_indirect_energy, float p_bias, int p_max_texture_size, bool p_bake_sh, bool p_bake_shadowmask, bool p_texture_for_bounces, GenerateProbes p_generate_probes, const Ref<Image> &p_environment_panorama, const Basis &p_environment_transform, BakeStepFunc p_step_function, void *p_bake_userdata, float p_exposure_normalization) {
 	int denoiser = GLOBAL_GET("rendering/lightmapping/denoising/denoiser");
 	String oidn_path = EDITOR_GET("filesystem/tools/oidn/oidn_denoise_path");
 
@@ -1050,7 +1053,8 @@ LightmapperRD::BakeError LightmapperRD::bake(BakeQuality p_quality, bool p_use_d
 	if (p_step_function) {
 		p_step_function(0.0, RTR("Begin Bake"), p_bake_userdata, true);
 	}
-	bake_textures.clear();
+	lightmap_textures.clear();
+	shadowmask_textures.clear();
 	int grid_size = 128;
 
 	/* STEP 1: Fetch material textures and compute the bounds */
@@ -1066,6 +1070,35 @@ LightmapperRD::BakeError LightmapperRD::bake(BakeQuality p_quality, bool p_use_d
 		return bake_error;
 	}
 
+	// The index of the directional light used for shadowmasking.
+	int shadowmask_light_idx = -1;
+
+	// If there are no valid directional lights for shadowmasking, the entire
+	// scene would be shadowed and this saves baking time.
+	if (p_bake_shadowmask) {
+		int shadowmask_lights_count = 0;
+
+		for (int i = 0; i < lights.size(); i++) {
+			if (lights[i].type == LightType::LIGHT_TYPE_DIRECTIONAL && !lights[i].static_bake) {
+				if (shadowmask_light_idx < 0) {
+					shadowmask_light_idx = i;
+				}
+
+				shadowmask_lights_count += 1;
+			}
+		}
+
+		if (shadowmask_light_idx < 0) {
+			p_bake_shadowmask = false;
+			WARN_PRINT("Shadowmask disabled: no directional light with their bake mode set to dynamic exists.");
+
+		} else if (shadowmask_lights_count > 1) {
+			WARN_PRINT(
+					vformat("%d directional lights detected for shadowmask baking. Only %s will be used.",
+							shadowmask_lights_count, light_names[shadowmask_light_idx]));
+		}
+	}
+
 #ifdef DEBUG_TEXTURES
 	for (int i = 0; i < atlas_slices; i++) {
 		albedo_images[i]->save_png("res://0_albedo_" + itos(i) + ".png");
@@ -1119,17 +1152,23 @@ LightmapperRD::BakeError LightmapperRD::bake(BakeQuality p_quality, bool p_use_d
 	RID light_accum_tex;
 	RID light_accum_tex2;
 	RID light_environment_tex;
+	RID shadowmask_tex;
+	RID shadowmask_tex2;
 
-#define FREE_TEXTURES             \
-	rd->free(albedo_array_tex);   \
-	rd->free(emission_array_tex); \
-	rd->free(normal_tex);         \
-	rd->free(position_tex);       \
-	rd->free(unocclude_tex);      \
-	rd->free(light_source_tex);   \
-	rd->free(light_accum_tex2);   \
-	rd->free(light_accum_tex);    \
-	rd->free(light_environment_tex);
+#define FREE_TEXTURES                \
+	rd->free(albedo_array_tex);      \
+	rd->free(emission_array_tex);    \
+	rd->free(normal_tex);            \
+	rd->free(position_tex);          \
+	rd->free(unocclude_tex);         \
+	rd->free(light_source_tex);      \
+	rd->free(light_accum_tex2);      \
+	rd->free(light_accum_tex);       \
+	rd->free(light_environment_tex); \
+	if (p_bake_shadowmask) {         \
+		rd->free(shadowmask_tex);    \
+		rd->free(shadowmask_tex2);   \
+	}
 
 	{ // create all textures
 
@@ -1161,9 +1200,22 @@ LightmapperRD::BakeError LightmapperRD::bake(BakeQuality p_quality, bool p_use_d
 		position_tex = rd->texture_create(tf, RD::TextureView());
 		unocclude_tex = rd->texture_create(tf, RD::TextureView());
 
-		tf.format = RD::DATA_FORMAT_R16G16B16A16_SFLOAT;
 		tf.usage_bits = RD::TEXTURE_USAGE_COLOR_ATTACHMENT_BIT | RD::TEXTURE_USAGE_SAMPLING_BIT | RD::TEXTURE_USAGE_STORAGE_BIT | RD::TEXTURE_USAGE_CAN_COPY_FROM_BIT | RD::TEXTURE_USAGE_CAN_COPY_TO_BIT | RD::TEXTURE_USAGE_CAN_UPDATE_BIT;
 
+		// shadowmask
+		if (p_bake_shadowmask) {
+			tf.format = RD::DATA_FORMAT_R8G8B8A8_UNORM;
+
+			shadowmask_tex = rd->texture_create(tf, RD::TextureView());
+			rd->texture_clear(shadowmask_tex, Color(0, 0, 0, 0), 0, 1, 0, atlas_slices);
+
+			shadowmask_tex2 = rd->texture_create(tf, RD::TextureView());
+			rd->texture_clear(shadowmask_tex2, Color(0, 0, 0, 0), 0, 1, 0, atlas_slices);
+		}
+
+		// lightmap
+		tf.format = RD::DATA_FORMAT_R16G16B16A16_SFLOAT;
+
 		light_source_tex = rd->texture_create(tf, RD::TextureView());
 		rd->texture_clear(light_source_tex, Color(0, 0, 0, 0), 0, 1, 0, atlas_slices);
 
@@ -1266,6 +1318,7 @@ LightmapperRD::BakeError LightmapperRD::bake(BakeQuality p_quality, bool p_use_d
 	bake_parameters.exposure_normalization = p_exposure_normalization;
 	bake_parameters.bounces = p_bounces;
 	bake_parameters.bounce_indirect_energy = p_bounce_indirect_energy;
+	bake_parameters.shadowmask_light_idx = shadowmask_light_idx;
 
 	bake_parameters_buffer = rd->uniform_buffer_create(sizeof(BakeParameters));
 	rd->buffer_update(bake_parameters_buffer, 0, sizeof(BakeParameters), &bake_parameters);
@@ -1463,6 +1516,10 @@ LightmapperRD::BakeError LightmapperRD::bake(BakeQuality p_quality, bool p_use_d
 		defines += "\n#define USE_LIGHT_TEXTURE_FOR_BOUNCES\n";
 	}
 
+	if (p_bake_shadowmask) {
+		defines += "\n#define USE_SHADOWMASK\n";
+	}
+
 	compute_shader.instantiate();
 	err = compute_shader->parse_versions_from_text(lm_compute_shader_glsl, defines);
 	if (err != OK) {
@@ -1634,6 +1691,14 @@ LightmapperRD::BakeError LightmapperRD::bake(BakeQuality p_quality, bool p_use_d
 				u.append_id(light_accum_tex);
 				uniforms.push_back(u);
 			}
+
+			if (p_bake_shadowmask) {
+				RD::Uniform u;
+				u.uniform_type = RD::UNIFORM_TYPE_IMAGE;
+				u.binding = 5;
+				u.append_id(shadowmask_tex);
+				uniforms.push_back(u);
+			}
 		}
 
 		RID light_uniform_set = rd->uniform_set_create(uniforms, compute_shader_primary, 1);
@@ -1945,7 +2010,7 @@ LightmapperRD::BakeError LightmapperRD::bake(BakeQuality p_quality, bool p_use_d
 			BakeError error;
 			if (denoiser == 1) {
 				// OIDN (external).
-				error = _denoise_oidn(rd, light_accum_tex, normal_tex, light_accum_tex, atlas_size, atlas_slices, p_bake_sh, oidn_path);
+				error = _denoise_oidn(rd, light_accum_tex, normal_tex, light_accum_tex, atlas_size, atlas_slices, p_bake_sh, false, oidn_path);
 			} else {
 				// JNLM (built-in).
 				SWAP(light_accum_tex, light_accum_tex2);
@@ -1955,14 +2020,39 @@ LightmapperRD::BakeError LightmapperRD::bake(BakeQuality p_quality, bool p_use_d
 				return error;
 			}
 		}
+
+		if (p_bake_shadowmask) {
+			BakeError error;
+			if (denoiser == 1) {
+				// OIDN (external).
+				error = _denoise_oidn(rd, shadowmask_tex, normal_tex, shadowmask_tex, atlas_size, atlas_slices, false, true, oidn_path);
+			} else {
+				// JNLM (built-in).
+				SWAP(shadowmask_tex, shadowmask_tex2);
+				error = _denoise(rd, compute_shader, compute_base_uniform_set, push_constant, shadowmask_tex2, normal_tex, shadowmask_tex, p_denoiser_strength, p_denoiser_range, atlas_size, atlas_slices, false, p_step_function, p_bake_userdata);
+			}
+			if (unlikely(error != BAKE_OK)) {
+				return error;
+			}
+		}
 	}
 
+	/* DILATE */
+
 	{
 		SWAP(light_accum_tex, light_accum_tex2);
 		BakeError error = _dilate(rd, compute_shader, compute_base_uniform_set, push_constant, light_accum_tex2, light_accum_tex, atlas_size, atlas_slices * (p_bake_sh ? 4 : 1));
 		if (unlikely(error != BAKE_OK)) {
 			return error;
 		}
+
+		if (p_bake_shadowmask) {
+			SWAP(shadowmask_tex, shadowmask_tex2);
+			error = _dilate(rd, compute_shader, compute_base_uniform_set, push_constant, shadowmask_tex2, shadowmask_tex, atlas_size, atlas_slices);
+			if (unlikely(error != BAKE_OK)) {
+				return error;
+			}
+		}
 	}
 
 #ifdef DEBUG_TEXTURES
@@ -2139,6 +2229,7 @@ LightmapperRD::BakeError LightmapperRD::bake(BakeQuality p_quality, bool p_use_d
 		img->save_exr("res://5_blendseams" + itos(i) + ".exr", false);
 	}
 #endif
+
 	if (p_step_function) {
 		p_step_function(0.9, RTR("Retrieving textures"), p_bake_userdata, true);
 	}
@@ -2147,7 +2238,16 @@ LightmapperRD::BakeError LightmapperRD::bake(BakeQuality p_quality, bool p_use_d
 		Vector<uint8_t> s = rd->texture_get_data(light_accum_tex, i);
 		Ref<Image> img = Image::create_from_data(atlas_size.width, atlas_size.height, false, Image::FORMAT_RGBAH, s);
 		img->convert(Image::FORMAT_RGBH); //remove alpha
-		bake_textures.push_back(img);
+		lightmap_textures.push_back(img);
+	}
+
+	if (p_bake_shadowmask) {
+		for (int i = 0; i < atlas_slices; i++) {
+			Vector<uint8_t> s = rd->texture_get_data(shadowmask_tex, i);
+			Ref<Image> img = Image::create_from_data(atlas_size.width, atlas_size.height, false, Image::FORMAT_RGBA8, s);
+			img->convert(Image::FORMAT_R8);
+			shadowmask_textures.push_back(img);
+		}
 	}
 
 	if (probe_positions.size() > 0) {
@@ -2180,12 +2280,21 @@ LightmapperRD::BakeError LightmapperRD::bake(BakeQuality p_quality, bool p_use_d
 }
 
 int LightmapperRD::get_bake_texture_count() const {
-	return bake_textures.size();
+	return lightmap_textures.size();
 }
 
 Ref<Image> LightmapperRD::get_bake_texture(int p_index) const {
-	ERR_FAIL_INDEX_V(p_index, bake_textures.size(), Ref<Image>());
-	return bake_textures[p_index];
+	ERR_FAIL_INDEX_V(p_index, lightmap_textures.size(), Ref<Image>());
+	return lightmap_textures[p_index];
+}
+
+int LightmapperRD::get_shadowmask_texture_count() const {
+	return shadowmask_textures.size();
+}
+
+Ref<Image> LightmapperRD::get_shadowmask_texture(int p_index) const {
+	ERR_FAIL_INDEX_V(p_index, shadowmask_textures.size(), Ref<Image>());
+	return shadowmask_textures[p_index];
 }
 
 int LightmapperRD::get_bake_mesh_count() const {
@@ -2198,9 +2307,9 @@ Variant LightmapperRD::get_bake_mesh_userdata(int p_index) const {
 }
 
 Rect2 LightmapperRD::get_bake_mesh_uv_scale(int p_index) const {
-	ERR_FAIL_COND_V(bake_textures.is_empty(), Rect2());
+	ERR_FAIL_COND_V(lightmap_textures.is_empty(), Rect2());
 	Rect2 uv_ofs;
-	Vector2 atlas_size = Vector2(bake_textures[0]->get_width(), bake_textures[0]->get_height());
+	Vector2 atlas_size = Vector2(lightmap_textures[0]->get_width(), lightmap_textures[0]->get_height());
 	uv_ofs.position = Vector2(mesh_instances[p_index].offset) / atlas_size;
 	uv_ofs.size = Vector2(mesh_instances[p_index].data.albedo_on_uv2->get_width(), mesh_instances[p_index].data.albedo_on_uv2->get_height()) / atlas_size;
 	return uv_ofs;
diff --git a/modules/lightmapper_rd/lightmapper_rd.h b/modules/lightmapper_rd/lightmapper_rd.h
index f43da39670..7e3efa71cc 100644
--- a/modules/lightmapper_rd/lightmapper_rd.h
+++ b/modules/lightmapper_rd/lightmapper_rd.h
@@ -57,7 +57,8 @@ class LightmapperRD : public Lightmapper {
 		uint32_t bounces = 0;
 
 		float bounce_indirect_energy = 0.0f;
-		uint32_t pad[3] = {};
+		int shadowmask_light_idx = 0;
+		uint32_t pad[2] = {};
 	};
 
 	struct MeshInstance {
@@ -202,6 +203,7 @@ class LightmapperRD : public Lightmapper {
 	Vector<MeshInstance> mesh_instances;
 
 	Vector<Light> lights;
+	Vector<String> light_names;
 
 	struct TriangleSort {
 		uint32_t cell_index = 0;
@@ -253,7 +255,8 @@ class LightmapperRD : public Lightmapper {
 		uint32_t pad = 0;
 	};
 
-	Vector<Ref<Image>> bake_textures;
+	Vector<Ref<Image>> lightmap_textures;
+	Vector<Ref<Image>> shadowmask_textures;
 	Vector<Color> probe_values;
 
 	struct DenoiseParams {
@@ -275,20 +278,22 @@ class LightmapperRD : public Lightmapper {
 	BakeError _denoise(RenderingDevice *p_rd, Ref<RDShaderFile> &p_compute_shader, const RID &p_compute_base_uniform_set, PushConstant &p_push_constant, RID p_source_light_tex, RID p_source_normal_tex, RID p_dest_light_tex, float p_denoiser_strength, int p_denoiser_range, const Size2i &p_atlas_size, int p_atlas_slices, bool p_bake_sh, BakeStepFunc p_step_function, void *p_bake_userdata);
 	BakeError _pack_l1(RenderingDevice *rd, Ref<RDShaderFile> &compute_shader, RID &compute_base_uniform_set, PushConstant &push_constant, RID &source_light_tex, RID &dest_light_tex, const Size2i &atlas_size, int atlas_slices);
 
-	Error _store_pfm(RenderingDevice *p_rd, RID p_atlas_tex, int p_index, const Size2i &p_atlas_size, const String &p_name);
-	Ref<Image> _read_pfm(const String &p_name);
-	BakeError _denoise_oidn(RenderingDevice *p_rd, RID p_source_light_tex, RID p_source_normal_tex, RID p_dest_light_tex, const Size2i &p_atlas_size, int p_atlas_slices, bool p_bake_sh, const String &p_exe);
+	Error _store_pfm(RenderingDevice *p_rd, RID p_atlas_tex, int p_index, const Size2i &p_atlas_size, const String &p_name, bool p_shadowmask);
+	Ref<Image> _read_pfm(const String &p_name, bool p_shadowmask);
+	BakeError _denoise_oidn(RenderingDevice *p_rd, RID p_source_light_tex, RID p_source_normal_tex, RID p_dest_light_tex, const Size2i &p_atlas_size, int p_atlas_slices, bool p_bake_sh, bool p_shadowmask, const String &p_exe);
 
 public:
 	virtual void add_mesh(const MeshData &p_mesh) override;
-	virtual void add_directional_light(bool p_static, const Vector3 &p_direction, const Color &p_color, float p_energy, float p_indirect_energy, float p_angular_distance, float p_shadow_blur) override;
-	virtual void add_omni_light(bool p_static, const Vector3 &p_position, const Color &p_color, float p_energy, float p_indirect_energy, float p_range, float p_attenuation, float p_size, float p_shadow_blur) override;
-	virtual void add_spot_light(bool p_static, const Vector3 &p_position, const Vector3 p_direction, const Color &p_color, float p_energy, float p_indirect_energy, float p_range, float p_attenuation, float p_spot_angle, float p_spot_attenuation, float p_size, float p_shadow_blur) override;
+	virtual void add_directional_light(const String &p_name, bool p_static, const Vector3 &p_direction, const Color &p_color, float p_energy, float p_indirect_energy, float p_angular_distance, float p_shadow_blur) override;
+	virtual void add_omni_light(const String &p_name, bool p_static, const Vector3 &p_position, const Color &p_color, float p_energy, float p_indirect_energy, float p_range, float p_attenuation, float p_size, float p_shadow_blur) override;
+	virtual void add_spot_light(const String &p_name, bool p_static, const Vector3 &p_position, const Vector3 p_direction, const Color &p_color, float p_energy, float p_indirect_energy, float p_range, float p_attenuation, float p_spot_angle, float p_spot_attenuation, float p_size, float p_shadow_blur) override;
 	virtual void add_probe(const Vector3 &p_position) override;
-	virtual BakeError bake(BakeQuality p_quality, bool p_use_denoiser, float p_denoiser_strength, int p_denoiser_range, int p_bounces, float p_bounce_indirect_energy, float p_bias, int p_max_texture_size, bool p_bake_sh, bool p_texture_for_bounces, GenerateProbes p_generate_probes, const Ref<Image> &p_environment_panorama, const Basis &p_environment_transform, BakeStepFunc p_step_function = nullptr, void *p_bake_userdata = nullptr, float p_exposure_normalization = 1.0) override;
+	virtual BakeError bake(BakeQuality p_quality, bool p_use_denoiser, float p_denoiser_strength, int p_denoiser_range, int p_bounces, float p_bounce_indirect_energy, float p_bias, int p_max_texture_size, bool p_bake_sh, bool p_bake_shadowmask, bool p_texture_for_bounces, GenerateProbes p_generate_probes, const Ref<Image> &p_environment_panorama, const Basis &p_environment_transform, BakeStepFunc p_step_function = nullptr, void *p_bake_userdata = nullptr, float p_exposure_normalization = 1.0) override;
 
 	int get_bake_texture_count() const override;
 	Ref<Image> get_bake_texture(int p_index) const override;
+	int get_shadowmask_texture_count() const override;
+	Ref<Image> get_shadowmask_texture(int p_index) const override;
 	int get_bake_mesh_count() const override;
 	Variant get_bake_mesh_userdata(int p_index) const override;
 	Rect2 get_bake_mesh_uv_scale(int p_index) const override;
diff --git a/modules/lightmapper_rd/lm_common_inc.glsl b/modules/lightmapper_rd/lm_common_inc.glsl
index 962e444911..954440dfe5 100644
--- a/modules/lightmapper_rd/lm_common_inc.glsl
+++ b/modules/lightmapper_rd/lm_common_inc.glsl
@@ -17,6 +17,9 @@ layout(set = 0, binding = 0) uniform BakeParameters {
 	uint bounces;
 
 	float bounce_indirect_energy;
+	int shadowmask_light_idx;
+	uint pad0;
+	uint pad1;
 }
 bake_params;
 
diff --git a/modules/lightmapper_rd/lm_compute.glsl b/modules/lightmapper_rd/lm_compute.glsl
index 31b721bb20..fd18d879c9 100644
--- a/modules/lightmapper_rd/lm_compute.glsl
+++ b/modules/lightmapper_rd/lm_compute.glsl
@@ -60,7 +60,9 @@ layout(rgba16f, set = 1, binding = 4) uniform restrict image2DArray accum_light;
 
 #endif
 
-#ifdef MODE_BOUNCE_LIGHT
+#if defined(MODE_DIRECT_LIGHT) && defined(USE_SHADOWMASK)
+layout(rgba8, set = 1, binding = 5) uniform restrict writeonly image2DArray shadowmask;
+#elif defined(MODE_BOUNCE_LIGHT)
 layout(set = 1, binding = 5) uniform texture2D environment;
 #endif
 
@@ -389,8 +391,9 @@ vec2 get_vogel_disk(float p_i, float p_rotation, float p_sample_count_sqrt) {
 	return vec2(cos(theta), sin(theta)) * r;
 }
 
-void trace_direct_light(vec3 p_position, vec3 p_normal, uint p_light_index, bool p_soft_shadowing, out vec3 r_light, out vec3 r_light_dir, inout uint r_noise, float p_texel_size) {
+void trace_direct_light(vec3 p_position, vec3 p_normal, uint p_light_index, bool p_soft_shadowing, out vec3 r_light, out vec3 r_light_dir, inout uint r_noise, float p_texel_size, out float r_shadow) {
 	r_light = vec3(0.0f);
+	r_shadow = 0.0f;
 
 	vec3 light_pos;
 	float dist;
@@ -507,6 +510,7 @@ void trace_direct_light(vec3 p_position, vec3 p_normal, uint p_light_index, bool
 		}
 	}
 
+	r_shadow = penumbra;
 	r_light = light_data.color * light_data.energy * attenuation * penumbra;
 }
 
@@ -556,7 +560,8 @@ vec3 trace_indirect_light(vec3 p_position, vec3 p_ray_dir, inout uint r_noise, f
 			for (uint i = 0; i < bake_params.light_count; i++) {
 				vec3 light;
 				vec3 light_dir;
-				trace_direct_light(position, normal, i, false, light, light_dir, r_noise, p_texel_size);
+				float shadow;
+				trace_direct_light(position, normal, i, false, light, light_dir, r_noise, p_texel_size, shadow);
 				direct_light += light * lights.data[i].indirect_energy;
 			}
 
@@ -614,7 +619,6 @@ void main() {
 #endif
 
 #ifdef MODE_DIRECT_LIGHT
-
 	vec3 normal = texelFetch(sampler2DArray(source_normal, linear_sampler), ivec3(atlas_pos, params.atlas_slice), 0).xyz;
 	if (length(normal) < 0.5) {
 		return; //empty texel, no process
@@ -631,6 +635,10 @@ void main() {
 	vec3 light_for_texture = vec3(0.0);
 	vec3 light_for_bounces = vec3(0.0);
 
+#ifdef USE_SHADOWMASK
+	float shadowmask_value = 0.0f;
+#endif
+
 #ifdef USE_SH_LIGHTMAPS
 	vec4 sh_accum[4] = vec4[](
 			vec4(0.0, 0.0, 0.0, 1.0),
@@ -644,7 +652,8 @@ void main() {
 	for (uint i = 0; i < bake_params.light_count; i++) {
 		vec3 light;
 		vec3 light_dir;
-		trace_direct_light(position, normal, i, true, light, light_dir, noise, texel_size_world_space);
+		float shadow;
+		trace_direct_light(position, normal, i, true, light, light_dir, noise, texel_size_world_space, shadow);
 
 		if (lights.data[i].static_bake) {
 			light_for_texture += light;
@@ -669,6 +678,12 @@ void main() {
 		}
 
 		light_for_bounces += light * lights.data[i].indirect_energy;
+
+#ifdef USE_SHADOWMASK
+		if (lights.data[i].type == LIGHT_TYPE_DIRECTIONAL && i == bake_params.shadowmask_light_idx) {
+			shadowmask_value = max(shadowmask_value, shadow);
+		}
+#endif
 	}
 
 	light_for_bounces *= bake_params.exposure_normalization;
@@ -685,6 +700,10 @@ void main() {
 	imageStore(accum_light, ivec3(atlas_pos, params.atlas_slice), vec4(light_for_texture, 1.0));
 #endif
 
+#ifdef USE_SHADOWMASK
+	imageStore(shadowmask, ivec3(atlas_pos, params.atlas_slice), vec4(shadowmask_value, shadowmask_value, shadowmask_value, 1.0));
+#endif
+
 #endif
 
 #ifdef MODE_BOUNCE_LIGHT
@@ -850,7 +869,7 @@ void main() {
 
 #endif
 
-#ifdef MODE_DILATE
+#if defined(MODE_DILATE)
 
 	vec4 c = texelFetch(sampler2DArray(source_light, linear_sampler), ivec3(atlas_pos, params.atlas_slice), 0);
 	//sides first, as they are closer
diff --git a/scene/3d/lightmap_gi.cpp b/scene/3d/lightmap_gi.cpp
index aa4445a7ba..4c3d1a8218 100644
--- a/scene/3d/lightmap_gi.cpp
+++ b/scene/3d/lightmap_gi.cpp
@@ -130,6 +130,52 @@ TypedArray<TextureLayered> LightmapGIData::get_lightmap_textures() const {
 	return storage_light_textures;
 }
 
+void LightmapGIData::set_shadowmask_textures(const TypedArray<TextureLayered> &p_data) {
+	storage_shadowmask_textures = p_data;
+
+	if (p_data.is_empty()) {
+		combined_shadowmask_texture = Ref<TextureLayered>();
+		_reset_shadowmask_textures();
+		return;
+	}
+
+	if (p_data.size() == 1) {
+		combined_shadowmask_texture = p_data[0];
+
+	} else {
+		Vector<Ref<Image>> images;
+		for (int i = 0; i < p_data.size(); i++) {
+			Ref<TextureLayered> texture = p_data[i];
+			ERR_FAIL_COND_MSG(texture.is_null(), vformat("Invalid TextureLayered at index %d.", i));
+			for (int j = 0; j < texture->get_layers(); j++) {
+				images.push_back(texture->get_layer_data(j));
+			}
+		}
+
+		Ref<Texture2DArray> combined_texture;
+		combined_texture.instantiate();
+
+		combined_texture->create_from_images(images);
+		combined_shadowmask_texture = combined_texture;
+	}
+
+	_reset_shadowmask_textures();
+}
+
+TypedArray<TextureLayered> LightmapGIData::get_shadowmask_textures() const {
+	return storage_shadowmask_textures;
+}
+
+void LightmapGIData::clear_shadowmask_textures() {
+	RS::get_singleton()->lightmap_set_shadowmask_textures(lightmap, RID());
+	storage_shadowmask_textures.clear();
+	combined_shadowmask_texture.unref();
+}
+
+bool LightmapGIData::has_shadowmask_textures() {
+	return !storage_shadowmask_textures.is_empty() && combined_shadowmask_texture.is_valid();
+}
+
 RID LightmapGIData::get_rid() const {
 	return lightmap;
 }
@@ -142,6 +188,10 @@ void LightmapGIData::_reset_lightmap_textures() {
 	RS::get_singleton()->lightmap_set_textures(lightmap, combined_light_texture.is_valid() ? combined_light_texture->get_rid() : RID(), uses_spherical_harmonics);
 }
 
+void LightmapGIData::_reset_shadowmask_textures() {
+	RS::get_singleton()->lightmap_set_shadowmask_textures(lightmap, combined_shadowmask_texture.is_valid() ? combined_shadowmask_texture->get_rid() : RID());
+}
+
 void LightmapGIData::set_uses_spherical_harmonics(bool p_enable) {
 	uses_spherical_harmonics = p_enable;
 	_reset_lightmap_textures();
@@ -159,6 +209,14 @@ bool LightmapGIData::_is_using_packed_directional() const {
 	return _uses_packed_directional;
 }
 
+void LightmapGIData::update_shadowmask_mode(ShadowmaskMode p_mode) {
+	RS::get_singleton()->lightmap_set_shadowmask_mode(lightmap, (RS::ShadowmaskMode)p_mode);
+}
+
+LightmapGIData::ShadowmaskMode LightmapGIData::get_shadowmask_mode() const {
+	return (ShadowmaskMode)RS::get_singleton()->lightmap_get_shadowmask_mode(lightmap);
+}
+
 void LightmapGIData::set_capture_data(const AABB &p_bounds, bool p_interior, const PackedVector3Array &p_points, const PackedColorArray &p_point_sh, const PackedInt32Array &p_tetrahedra, const PackedInt32Array &p_bsp_tree, float p_baked_exposure) {
 	if (p_points.size()) {
 		int pc = p_points.size();
@@ -260,6 +318,9 @@ void LightmapGIData::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_lightmap_textures", "light_textures"), &LightmapGIData::set_lightmap_textures);
 	ClassDB::bind_method(D_METHOD("get_lightmap_textures"), &LightmapGIData::get_lightmap_textures);
 
+	ClassDB::bind_method(D_METHOD("set_shadowmask_textures", "shadowmask_textures"), &LightmapGIData::set_shadowmask_textures);
+	ClassDB::bind_method(D_METHOD("get_shadowmask_textures"), &LightmapGIData::get_shadowmask_textures);
+
 	ClassDB::bind_method(D_METHOD("set_uses_spherical_harmonics", "uses_spherical_harmonics"), &LightmapGIData::set_uses_spherical_harmonics);
 	ClassDB::bind_method(D_METHOD("is_using_spherical_harmonics"), &LightmapGIData::is_using_spherical_harmonics);
 
@@ -275,6 +336,7 @@ void LightmapGIData::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("_get_probe_data"), &LightmapGIData::_get_probe_data);
 
 	ADD_PROPERTY(PropertyInfo(Variant::ARRAY, "lightmap_textures", PROPERTY_HINT_ARRAY_TYPE, "TextureLayered", PROPERTY_USAGE_DEFAULT | PROPERTY_USAGE_READ_ONLY), "set_lightmap_textures", "get_lightmap_textures");
+	ADD_PROPERTY(PropertyInfo(Variant::ARRAY, "shadowmask_textures", PROPERTY_HINT_ARRAY_TYPE, "TextureLayered", PROPERTY_USAGE_DEFAULT | PROPERTY_USAGE_READ_ONLY), "set_shadowmask_textures", "get_shadowmask_textures");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "uses_spherical_harmonics", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR | PROPERTY_USAGE_INTERNAL), "set_uses_spherical_harmonics", "is_using_spherical_harmonics");
 	ADD_PROPERTY(PropertyInfo(Variant::ARRAY, "user_data", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR | PROPERTY_USAGE_INTERNAL), "_set_user_data", "_get_user_data");
 	ADD_PROPERTY(PropertyInfo(Variant::DICTIONARY, "probe_data", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR | PROPERTY_USAGE_INTERNAL), "_set_probe_data", "_get_probe_data");
@@ -290,6 +352,10 @@ void LightmapGIData::_bind_methods() {
 	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "light_texture", PROPERTY_HINT_RESOURCE_TYPE, "TextureLayered", PROPERTY_USAGE_NONE), "set_light_texture", "get_light_texture");
 	ADD_PROPERTY(PropertyInfo(Variant::ARRAY, "light_textures", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_INTERNAL), "_set_light_textures_data", "_get_light_textures_data");
 #endif
+
+	BIND_ENUM_CONSTANT(SHADOWMASK_MODE_NONE);
+	BIND_ENUM_CONSTANT(SHADOWMASK_MODE_REPLACE);
+	BIND_ENUM_CONSTANT(SHADOWMASK_MODE_OVERLAY);
 }
 
 LightmapGIData::LightmapGIData() {
@@ -738,12 +804,12 @@ void LightmapGI::_gen_new_positions_from_octree(const GenProbesOctree *p_cell, f
 	}
 }
 
-LightmapGI::BakeError LightmapGI::_save_and_reimport_atlas_textures(const Ref<Lightmapper> p_lightmapper, const String &p_base_name, TypedArray<TextureLayered> &r_textures, bool p_compress) const {
+LightmapGI::BakeError LightmapGI::_save_and_reimport_atlas_textures(const Ref<Lightmapper> p_lightmapper, const String &p_base_name, TypedArray<TextureLayered> &r_textures, bool p_is_shadowmask, bool p_compress) const {
 	Vector<Ref<Image>> images;
-	images.resize(p_lightmapper->get_bake_texture_count());
+	images.resize(p_is_shadowmask ? p_lightmapper->get_shadowmask_texture_count() : p_lightmapper->get_bake_texture_count());
 
 	for (int i = 0; i < images.size(); i++) {
-		images.set(i, p_lightmapper->get_bake_texture(i));
+		images.set(i, p_is_shadowmask ? p_lightmapper->get_shadowmask_texture(i) : p_lightmapper->get_bake_texture(i));
 	}
 
 	const int slice_count = images.size();
@@ -765,7 +831,7 @@ LightmapGI::BakeError LightmapGI::_save_and_reimport_atlas_textures(const Ref<Li
 			texture_image->blit_rect(images[i * slices_per_texture + j], Rect2i(0, 0, slice_width, slice_height), Point2i(0, slice_height * j));
 		}
 
-		const String atlas_path = (texture_count > 1 ? p_base_name + "_" + itos(i) : p_base_name) + ".exr";
+		const String atlas_path = (texture_count > 1 ? p_base_name + "_" + itos(i) : p_base_name) + (p_is_shadowmask ? ".png" : ".exr");
 		const String config_path = atlas_path + ".import";
 
 		Ref<ConfigFile> config;
@@ -790,7 +856,12 @@ LightmapGI::BakeError LightmapGI::_save_and_reimport_atlas_textures(const Ref<Li
 		config->save(config_path);
 
 		// Save the file.
-		Error save_err = texture_image->save_exr(atlas_path, false);
+		Error save_err;
+		if (p_is_shadowmask) {
+			save_err = texture_image->save_png(atlas_path);
+		} else {
+			save_err = texture_image->save_exr(atlas_path, false);
+		}
 
 		ERR_FAIL_COND_V(save_err, LightmapGI::BAKE_ERROR_CANT_CREATE_IMAGE);
 
@@ -1104,20 +1175,20 @@ LightmapGI::BakeError LightmapGI::bake(Node *p_from_node, String p_image_data_pa
 			if (Object::cast_to<DirectionalLight3D>(light)) {
 				DirectionalLight3D *l = Object::cast_to<DirectionalLight3D>(light);
 				if (l->get_sky_mode() != DirectionalLight3D::SKY_MODE_SKY_ONLY) {
-					lightmapper->add_directional_light(light->get_bake_mode() == Light3D::BAKE_STATIC, -xf.basis.get_column(Vector3::AXIS_Z).normalized(), linear_color, energy, indirect_energy, l->get_param(Light3D::PARAM_SIZE), l->get_param(Light3D::PARAM_SHADOW_BLUR));
+					lightmapper->add_directional_light(light->get_name(), light->get_bake_mode() == Light3D::BAKE_STATIC, -xf.basis.get_column(Vector3::AXIS_Z).normalized(), linear_color, energy, indirect_energy, l->get_param(Light3D::PARAM_SIZE), l->get_param(Light3D::PARAM_SHADOW_BLUR));
 				}
 			} else if (Object::cast_to<OmniLight3D>(light)) {
 				OmniLight3D *l = Object::cast_to<OmniLight3D>(light);
 				if (use_physical_light_units) {
 					energy *= (1.0 / (Math_PI * 4.0));
 				}
-				lightmapper->add_omni_light(light->get_bake_mode() == Light3D::BAKE_STATIC, xf.origin, linear_color, energy, indirect_energy, l->get_param(Light3D::PARAM_RANGE), l->get_param(Light3D::PARAM_ATTENUATION), l->get_param(Light3D::PARAM_SIZE), l->get_param(Light3D::PARAM_SHADOW_BLUR));
+				lightmapper->add_omni_light(light->get_name(), light->get_bake_mode() == Light3D::BAKE_STATIC, xf.origin, linear_color, energy, indirect_energy, l->get_param(Light3D::PARAM_RANGE), l->get_param(Light3D::PARAM_ATTENUATION), l->get_param(Light3D::PARAM_SIZE), l->get_param(Light3D::PARAM_SHADOW_BLUR));
 			} else if (Object::cast_to<SpotLight3D>(light)) {
 				SpotLight3D *l = Object::cast_to<SpotLight3D>(light);
 				if (use_physical_light_units) {
 					energy *= (1.0 / Math_PI);
 				}
-				lightmapper->add_spot_light(light->get_bake_mode() == Light3D::BAKE_STATIC, xf.origin, -xf.basis.get_column(Vector3::AXIS_Z).normalized(), linear_color, energy, indirect_energy, l->get_param(Light3D::PARAM_RANGE), l->get_param(Light3D::PARAM_ATTENUATION), l->get_param(Light3D::PARAM_SPOT_ANGLE), l->get_param(Light3D::PARAM_SPOT_ATTENUATION), l->get_param(Light3D::PARAM_SIZE), l->get_param(Light3D::PARAM_SHADOW_BLUR));
+				lightmapper->add_spot_light(light->get_name(), light->get_bake_mode() == Light3D::BAKE_STATIC, xf.origin, -xf.basis.get_column(Vector3::AXIS_Z).normalized(), linear_color, energy, indirect_energy, l->get_param(Light3D::PARAM_RANGE), l->get_param(Light3D::PARAM_ATTENUATION), l->get_param(Light3D::PARAM_SPOT_ANGLE), l->get_param(Light3D::PARAM_SPOT_ATTENUATION), l->get_param(Light3D::PARAM_SIZE), l->get_param(Light3D::PARAM_SHADOW_BLUR));
 			}
 		}
 		for (int i = 0; i < probes_found.size(); i++) {
@@ -1181,7 +1252,7 @@ LightmapGI::BakeError LightmapGI::bake(Node *p_from_node, String p_image_data_pa
 		}
 	}
 
-	Lightmapper::BakeError bake_err = lightmapper->bake(Lightmapper::BakeQuality(bake_quality), use_denoiser, denoiser_strength, denoiser_range, bounces, bounce_indirect_energy, bias, max_texture_size, directional, use_texture_for_bounces, Lightmapper::GenerateProbes(gen_probes), environment_image, environment_transform, _lightmap_bake_step_function, &bsud, exposure_normalization);
+	Lightmapper::BakeError bake_err = lightmapper->bake(Lightmapper::BakeQuality(bake_quality), use_denoiser, denoiser_strength, denoiser_range, bounces, bounce_indirect_energy, bias, max_texture_size, directional, shadowmask_mode != LightmapGIData::SHADOWMASK_MODE_NONE, use_texture_for_bounces, Lightmapper::GenerateProbes(gen_probes), environment_image, environment_transform, _lightmap_bake_step_function, &bsud, exposure_normalization);
 
 	if (bake_err == Lightmapper::BAKE_ERROR_TEXTURE_EXCEEDS_MAX_SIZE) {
 		return BAKE_ERROR_TEXTURE_SIZE_TOO_SMALL;
@@ -1196,15 +1267,23 @@ LightmapGI::BakeError LightmapGI::bake(Node *p_from_node, String p_image_data_pa
 	// POSTBAKE: Save Textures.
 
 	TypedArray<TextureLayered> lightmap_textures;
+	TypedArray<TextureLayered> shadowmask_textures;
 
 	const String texture_filename = p_image_data_path.get_basename();
+	const int shadowmask_texture_count = lightmapper->get_shadowmask_texture_count();
+	const bool save_shadowmask = shadowmask_mode != LightmapGIData::SHADOWMASK_MODE_NONE && shadowmask_texture_count > 0;
 
 	// Save the lightmap atlases.
-	BakeError save_err = _save_and_reimport_atlas_textures(lightmapper, texture_filename, lightmap_textures, false);
+	BakeError save_err = _save_and_reimport_atlas_textures(lightmapper, texture_filename, lightmap_textures, false, false);
 	ERR_FAIL_COND_V(save_err != BAKE_ERROR_OK, save_err);
 
-	// POSTBAKE: Save Light Data.
+	if (save_shadowmask) {
+		// Save the shadowmask atlases.
+		save_err = _save_and_reimport_atlas_textures(lightmapper, texture_filename + "_shadow", shadowmask_textures, true, true);
+		ERR_FAIL_COND_V(save_err != BAKE_ERROR_OK, save_err);
+	}
 
+	/* POSTBAKE: Save Light Data. */
 	Ref<LightmapGIData> gi_data;
 
 	if (get_light_data().is_valid()) {
@@ -1217,6 +1296,13 @@ LightmapGI::BakeError LightmapGI::bake(Node *p_from_node, String p_image_data_pa
 	}
 
 	gi_data->set_lightmap_textures(lightmap_textures);
+
+	if (save_shadowmask) {
+		gi_data->set_shadowmask_textures(shadowmask_textures);
+	} else {
+		gi_data->clear_shadowmask_textures();
+	}
+
 	gi_data->set_uses_spherical_harmonics(directional);
 	gi_data->_set_uses_packed_directional(directional); // New SH lightmaps are packed automatically.
 
@@ -1375,6 +1461,7 @@ LightmapGI::BakeError LightmapGI::bake(Node *p_from_node, String p_image_data_pa
 	}
 
 	set_light_data(gi_data);
+	update_configuration_warnings();
 
 	return BAKE_ERROR_OK;
 }
@@ -1452,6 +1539,7 @@ void LightmapGI::set_light_data(const Ref<LightmapGIData> &p_data) {
 		if (is_inside_tree()) {
 			_assign_lightmaps();
 		}
+		light_data->update_shadowmask_mode(shadowmask_mode);
 	}
 
 	update_gizmos();
@@ -1506,6 +1594,19 @@ bool LightmapGI::is_directional() const {
 	return directional;
 }
 
+void LightmapGI::set_shadowmask_mode(LightmapGIData::ShadowmaskMode p_mode) {
+	shadowmask_mode = p_mode;
+	if (light_data.is_valid()) {
+		light_data->update_shadowmask_mode(p_mode);
+	}
+
+	update_configuration_warnings();
+}
+
+LightmapGIData::ShadowmaskMode LightmapGI::get_shadowmask_mode() const {
+	return shadowmask_mode;
+}
+
 void LightmapGI::set_use_texture_for_bounces(bool p_enable) {
 	use_texture_for_bounces = p_enable;
 }
@@ -1625,6 +1726,11 @@ PackedStringArray LightmapGI::get_configuration_warnings() const {
 		warnings.push_back(vformat(RTR("Lightmaps can only be baked from a GPU that supports the RenderingDevice backends.\nYour GPU (%s) does not support RenderingDevice, as it does not support Vulkan, Direct3D 12, or Metal.\nLightmap baking will not be available on this device, although rendering existing baked lightmaps will work."), RenderingServer::get_singleton()->get_video_adapter_name()));
 		return warnings;
 	}
+
+	if (shadowmask_mode != LightmapGIData::SHADOWMASK_MODE_NONE && light_data.is_valid() && !light_data->has_shadowmask_textures()) {
+		warnings.push_back(RTR("The lightmap has no baked shadowmask textures. Please rebake with the Shadowmask Mode set to anything other than None."));
+	}
+
 #elif defined(ANDROID_ENABLED) || defined(IOS_ENABLED)
 	warnings.push_back(vformat(RTR("Lightmaps cannot be baked on %s. Rendering existing baked lightmaps will still work."), OS::get_singleton()->get_name()));
 #else
@@ -1704,6 +1810,9 @@ void LightmapGI::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_directional", "directional"), &LightmapGI::set_directional);
 	ClassDB::bind_method(D_METHOD("is_directional"), &LightmapGI::is_directional);
 
+	ClassDB::bind_method(D_METHOD("set_shadowmask_mode", "mode"), &LightmapGI::set_shadowmask_mode);
+	ClassDB::bind_method(D_METHOD("get_shadowmask_mode"), &LightmapGI::get_shadowmask_mode);
+
 	ClassDB::bind_method(D_METHOD("set_use_texture_for_bounces", "use_texture_for_bounces"), &LightmapGI::set_use_texture_for_bounces);
 	ClassDB::bind_method(D_METHOD("is_using_texture_for_bounces"), &LightmapGI::is_using_texture_for_bounces);
 
@@ -1717,6 +1826,7 @@ void LightmapGI::_bind_methods() {
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "bounces", PROPERTY_HINT_RANGE, "0,6,1,or_greater"), "set_bounces", "get_bounces");
 	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "bounce_indirect_energy", PROPERTY_HINT_RANGE, "0,2,0.01"), "set_bounce_indirect_energy", "get_bounce_indirect_energy");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "directional"), "set_directional", "is_directional");
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "shadowmask_mode", PROPERTY_HINT_ENUM, "None,Replace,Overlay"), "set_shadowmask_mode", "get_shadowmask_mode");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "use_texture_for_bounces"), "set_use_texture_for_bounces", "is_using_texture_for_bounces");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "interior"), "set_interior", "is_interior");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "use_denoiser"), "set_use_denoiser", "is_using_denoiser");
diff --git a/scene/3d/lightmap_gi.h b/scene/3d/lightmap_gi.h
index faa8b84fa1..6afd4ea591 100644
--- a/scene/3d/lightmap_gi.h
+++ b/scene/3d/lightmap_gi.h
@@ -43,12 +43,23 @@ class LightmapGIData : public Resource {
 	GDCLASS(LightmapGIData, Resource);
 	RES_BASE_EXTENSION("lmbake")
 
+public:
+	enum ShadowmaskMode {
+		SHADOWMASK_MODE_NONE,
+		SHADOWMASK_MODE_REPLACE,
+		SHADOWMASK_MODE_OVERLAY,
+		SHADOWMASK_MODE_ONLY,
+	};
+
+private:
 	// The 'merged' texture atlases actually used by the renderer.
 	Ref<TextureLayered> combined_light_texture;
+	Ref<TextureLayered> combined_shadowmask_texture;
 
 	// The temporary texture atlas arrays which are used for storage.
 	// If a single atlas is too large, it's split and recombined during loading.
 	TypedArray<TextureLayered> storage_light_textures;
+	TypedArray<TextureLayered> storage_shadowmask_textures;
 
 	bool uses_spherical_harmonics = false;
 	bool interior = false;
@@ -74,6 +85,7 @@ class LightmapGIData : public Resource {
 	Dictionary _get_probe_data() const;
 
 	void _reset_lightmap_textures();
+	void _reset_shadowmask_textures();
 
 protected:
 	static void _bind_methods();
@@ -101,6 +113,9 @@ public:
 	void _set_uses_packed_directional(bool p_enable);
 	bool _is_using_packed_directional() const;
 
+	void update_shadowmask_mode(ShadowmaskMode p_mode);
+	ShadowmaskMode get_shadowmask_mode() const;
+
 	bool is_interior() const;
 	float get_baked_exposure() const;
 
@@ -116,6 +131,11 @@ public:
 	void set_lightmap_textures(const TypedArray<TextureLayered> &p_data);
 	TypedArray<TextureLayered> get_lightmap_textures() const;
 
+	void set_shadowmask_textures(const TypedArray<TextureLayered> &p_data);
+	TypedArray<TextureLayered> get_shadowmask_textures() const;
+	void clear_shadowmask_textures();
+	bool has_shadowmask_textures();
+
 	virtual RID get_rid() const override;
 	LightmapGIData();
 	~LightmapGIData();
@@ -179,6 +199,7 @@ private:
 	float environment_custom_energy = 1.0;
 	bool directional = false;
 	bool use_texture_for_bounces = true;
+	LightmapGIData::ShadowmaskMode shadowmask_mode = LightmapGIData::SHADOWMASK_MODE_NONE;
 	GenerateProbes gen_probes = GENERATE_PROBES_SUBDIV_8;
 	Ref<CameraAttributes> camera_attributes;
 
@@ -249,7 +270,7 @@ private:
 	void _plot_triangle_into_octree(GenProbesOctree *p_cell, float p_cell_size, const Vector3 *p_triangle);
 	void _gen_new_positions_from_octree(const GenProbesOctree *p_cell, float p_cell_size, const Vector<Vector3> &probe_positions, LocalVector<Vector3> &new_probe_positions, HashMap<Vector3i, bool> &positions_used, const AABB &p_bounds);
 
-	BakeError _save_and_reimport_atlas_textures(const Ref<Lightmapper> p_lightmapper, const String &p_base_name, TypedArray<TextureLayered> &r_textures, bool p_compress = false) const;
+	BakeError _save_and_reimport_atlas_textures(const Ref<Lightmapper> p_lightmapper, const String &p_base_name, TypedArray<TextureLayered> &r_textures, bool p_is_shadowmask = false, bool p_compress = false) const;
 
 protected:
 	void _validate_property(PropertyInfo &p_property) const;
@@ -275,6 +296,9 @@ public:
 	void set_directional(bool p_enable);
 	bool is_directional() const;
 
+	void set_shadowmask_mode(LightmapGIData::ShadowmaskMode p_mode);
+	LightmapGIData::ShadowmaskMode get_shadowmask_mode() const;
+
 	void set_use_texture_for_bounces(bool p_enable);
 	bool is_using_texture_for_bounces() const;
 
@@ -323,6 +347,7 @@ public:
 	LightmapGI();
 };
 
+VARIANT_ENUM_CAST(LightmapGIData::ShadowmaskMode);
 VARIANT_ENUM_CAST(LightmapGI::BakeQuality);
 VARIANT_ENUM_CAST(LightmapGI::GenerateProbes);
 VARIANT_ENUM_CAST(LightmapGI::BakeError);
diff --git a/scene/3d/lightmapper.h b/scene/3d/lightmapper.h
index 1228c63edc..03ef3f3806 100644
--- a/scene/3d/lightmapper.h
+++ b/scene/3d/lightmapper.h
@@ -133,7 +133,6 @@ public:
 		GENERATE_PROBES_SUBDIV_8,
 		GENERATE_PROBES_SUBDIV_16,
 		GENERATE_PROBES_SUBDIV_32,
-
 	};
 
 	enum LightType {
@@ -178,14 +177,16 @@ public:
 	};
 
 	virtual void add_mesh(const MeshData &p_mesh) = 0;
-	virtual void add_directional_light(bool p_static, const Vector3 &p_direction, const Color &p_color, float p_energy, float p_indirect_energy, float p_angular_distance, float p_shadow_blur) = 0;
-	virtual void add_omni_light(bool p_static, const Vector3 &p_position, const Color &p_color, float p_energy, float p_indirect_energy, float p_range, float p_attenuation, float p_size, float p_shadow_blur) = 0;
-	virtual void add_spot_light(bool p_static, const Vector3 &p_position, const Vector3 p_direction, const Color &p_color, float p_energy, float p_indirect_energy, float p_range, float p_attenuation, float p_spot_angle, float p_spot_attenuation, float p_size, float p_shadow_blur) = 0;
+	virtual void add_directional_light(const String &p_name, bool p_static, const Vector3 &p_direction, const Color &p_color, float p_energy, float p_indirect_energy, float p_angular_distance, float p_shadow_blur) = 0;
+	virtual void add_omni_light(const String &p_name, bool p_static, const Vector3 &p_position, const Color &p_color, float p_energy, float p_indirect_energy, float p_range, float p_attenuation, float p_size, float p_shadow_blur) = 0;
+	virtual void add_spot_light(const String &p_name, bool p_static, const Vector3 &p_position, const Vector3 p_direction, const Color &p_color, float p_energy, float p_indirect_energy, float p_range, float p_attenuation, float p_spot_angle, float p_spot_attenuation, float p_size, float p_shadow_blur) = 0;
 	virtual void add_probe(const Vector3 &p_position) = 0;
-	virtual BakeError bake(BakeQuality p_quality, bool p_use_denoiser, float p_denoiser_strength, int p_denoiser_range, int p_bounces, float p_bounce_indirect_energy, float p_bias, int p_max_texture_size, bool p_bake_sh, bool p_texture_for_bounces, GenerateProbes p_generate_probes, const Ref<Image> &p_environment_panorama, const Basis &p_environment_transform, BakeStepFunc p_step_function = nullptr, void *p_step_userdata = nullptr, float p_exposure_normalization = 1.0) = 0;
+	virtual BakeError bake(BakeQuality p_quality, bool p_use_denoiser, float p_denoiser_strength, int p_denoiser_range, int p_bounces, float p_bounce_indirect_energy, float p_bias, int p_max_texture_size, bool p_bake_sh, bool p_bake_shadowmask, bool p_texture_for_bounces, GenerateProbes p_generate_probes, const Ref<Image> &p_environment_panorama, const Basis &p_environment_transform, BakeStepFunc p_step_function = nullptr, void *p_step_userdata = nullptr, float p_exposure_normalization = 1.0) = 0;
 
 	virtual int get_bake_texture_count() const = 0;
 	virtual Ref<Image> get_bake_texture(int p_index) const = 0;
+	virtual int get_shadowmask_texture_count() const = 0;
+	virtual Ref<Image> get_shadowmask_texture(int p_index) const = 0;
 	virtual int get_bake_mesh_count() const = 0;
 	virtual Variant get_bake_mesh_userdata(int p_index) const = 0;
 	virtual Rect2 get_bake_mesh_uv_scale(int p_index) const = 0;
diff --git a/servers/rendering/dummy/storage/light_storage.h b/servers/rendering/dummy/storage/light_storage.h
index d25523753c..44cdffaedc 100644
--- a/servers/rendering/dummy/storage/light_storage.h
+++ b/servers/rendering/dummy/storage/light_storage.h
@@ -191,6 +191,10 @@ public:
 	virtual void lightmap_set_probe_capture_update_speed(float p_speed) override {}
 	virtual float lightmap_get_probe_capture_update_speed() const override { return 0; }
 
+	virtual void lightmap_set_shadowmask_textures(RID p_lightmap, RID p_shadow) override {}
+	virtual RS::ShadowmaskMode lightmap_get_shadowmask_mode(RID p_lightmap) override { return RS::SHADOWMASK_MODE_NONE; }
+	virtual void lightmap_set_shadowmask_mode(RID p_lightmap, RS::ShadowmaskMode p_mode) override {}
+
 	/* LIGHTMAP INSTANCE */
 
 	bool owns_lightmap_instance(RID p_rid) { return lightmap_instance_owner.owns(p_rid); }
diff --git a/servers/rendering/renderer_rd/forward_clustered/render_forward_clustered.cpp b/servers/rendering/renderer_rd/forward_clustered/render_forward_clustered.cpp
index 43a50e97c4..4084465033 100644
--- a/servers/rendering/renderer_rd/forward_clustered/render_forward_clustered.cpp
+++ b/servers/rendering/renderer_rd/forward_clustered/render_forward_clustered.cpp
@@ -1167,6 +1167,7 @@ void RenderForwardClustered::_setup_lightmaps(const RenderDataRD *p_render_data,
 
 		// Exposure.
 		scene_state.lightmaps[i].exposure_normalization = 1.0;
+		scene_state.lightmaps[i].flags = light_storage->lightmap_get_shadowmask_mode(lightmap);
 		if (p_render_data->camera_attributes.is_valid()) {
 			float baked_exposure = light_storage->lightmap_get_baked_exposure_normalization(lightmap);
 			float enf = RSG::camera_attributes->camera_attributes_get_exposure_normalization_factor(p_render_data->camera_attributes);
@@ -3223,15 +3224,29 @@ RID RenderForwardClustered::_setup_render_pass_uniform_set(RenderListType p_rend
 		u.uniform_type = RD::UNIFORM_TYPE_TEXTURE;
 
 		RID default_tex = texture_storage->texture_rd_get_default(RendererRD::TextureStorage::DEFAULT_RD_TEXTURE_2D_ARRAY_WHITE);
-		for (uint32_t i = 0; i < scene_state.max_lightmaps; i++) {
-			if (p_render_data && i < p_render_data->lightmaps->size()) {
-				RID base = light_storage->lightmap_instance_get_lightmap((*p_render_data->lightmaps)[i]);
-				RID texture = light_storage->lightmap_get_texture(base);
-				RID rd_texture = texture_storage->texture_get_rd_texture(texture);
-				u.append_id(rd_texture);
-			} else {
-				u.append_id(default_tex);
+		for (uint32_t i = 0; i < scene_state.max_lightmaps * 2; i++) {
+			uint32_t current_lightmap_index = i < scene_state.max_lightmaps ? i : i - scene_state.max_lightmaps;
+
+			if (p_render_data && current_lightmap_index < p_render_data->lightmaps->size()) {
+				RID base = light_storage->lightmap_instance_get_lightmap((*p_render_data->lightmaps)[current_lightmap_index]);
+				RID texture;
+
+				if (i < scene_state.max_lightmaps) {
+					// Lightmap
+					texture = light_storage->lightmap_get_texture(base);
+				} else {
+					// Shadowmask
+					texture = light_storage->shadowmask_get_texture(base);
+				}
+
+				if (texture.is_valid()) {
+					RID rd_texture = texture_storage->texture_get_rd_texture(texture);
+					u.append_id(rd_texture);
+					continue;
+				}
 			}
+
+			u.append_id(default_tex);
 		}
 
 		uniforms.push_back(u);
@@ -3535,7 +3550,7 @@ RID RenderForwardClustered::_setup_sdfgi_render_pass_uniform_set(RID p_albedo_te
 		u.uniform_type = RD::UNIFORM_TYPE_TEXTURE;
 
 		RID default_tex = texture_storage->texture_rd_get_default(RendererRD::TextureStorage::DEFAULT_RD_TEXTURE_2D_ARRAY_WHITE);
-		for (uint32_t i = 0; i < scene_state.max_lightmaps; i++) {
+		for (uint32_t i = 0; i < scene_state.max_lightmaps * 2; i++) {
 			u.append_id(default_tex);
 		}
 
diff --git a/servers/rendering/renderer_rd/forward_clustered/render_forward_clustered.h b/servers/rendering/renderer_rd/forward_clustered/render_forward_clustered.h
index 728164c38e..ac4b66e1dc 100644
--- a/servers/rendering/renderer_rd/forward_clustered/render_forward_clustered.h
+++ b/servers/rendering/renderer_rd/forward_clustered/render_forward_clustered.h
@@ -237,7 +237,7 @@ private:
 		float normal_xform[12];
 		float texture_size[2];
 		float exposure_normalization;
-		float pad;
+		uint32_t flags;
 	};
 
 	struct LightmapCaptureData {
diff --git a/servers/rendering/renderer_rd/forward_mobile/render_forward_mobile.cpp b/servers/rendering/renderer_rd/forward_mobile/render_forward_mobile.cpp
index 92d4fe499b..20f8ef02c0 100644
--- a/servers/rendering/renderer_rd/forward_mobile/render_forward_mobile.cpp
+++ b/servers/rendering/renderer_rd/forward_mobile/render_forward_mobile.cpp
@@ -477,15 +477,29 @@ RID RenderForwardMobile::_setup_render_pass_uniform_set(RenderListType p_render_
 		u.uniform_type = RD::UNIFORM_TYPE_TEXTURE;
 
 		RID default_tex = texture_storage->texture_rd_get_default(RendererRD::TextureStorage::DEFAULT_RD_TEXTURE_2D_ARRAY_WHITE);
-		for (uint32_t i = 0; i < scene_state.max_lightmaps; i++) {
-			if (p_render_data && i < p_render_data->lightmaps->size()) {
-				RID base = light_storage->lightmap_instance_get_lightmap((*p_render_data->lightmaps)[i]);
-				RID texture = light_storage->lightmap_get_texture(base);
-				RID rd_texture = texture_storage->texture_get_rd_texture(texture);
-				u.append_id(rd_texture);
-			} else {
-				u.append_id(default_tex);
+		for (uint32_t i = 0; i < scene_state.max_lightmaps * 2; i++) {
+			uint32_t current_lightmap_index = i < scene_state.max_lightmaps ? i : i - scene_state.max_lightmaps;
+
+			if (p_render_data && current_lightmap_index < p_render_data->lightmaps->size()) {
+				RID base = light_storage->lightmap_instance_get_lightmap((*p_render_data->lightmaps)[current_lightmap_index]);
+				RID texture;
+
+				if (i < scene_state.max_lightmaps) {
+					// Lightmap
+					texture = light_storage->lightmap_get_texture(base);
+				} else {
+					// Shadowmask
+					texture = light_storage->shadowmask_get_texture(base);
+				}
+
+				if (texture.is_valid()) {
+					RID rd_texture = texture_storage->texture_get_rd_texture(texture);
+					u.append_id(rd_texture);
+					continue;
+				}
 			}
+
+			u.append_id(default_tex);
 		}
 
 		uniforms.push_back(u);
@@ -642,6 +656,7 @@ void RenderForwardMobile::_setup_lightmaps(const RenderDataRD *p_render_data, co
 
 		// Exposure.
 		scene_state.lightmaps[i].exposure_normalization = 1.0;
+		scene_state.lightmaps[i].flags = light_storage->lightmap_get_shadowmask_mode(lightmap);
 		if (p_render_data->camera_attributes.is_valid()) {
 			float baked_exposure = light_storage->lightmap_get_baked_exposure_normalization(lightmap);
 			float enf = RSG::camera_attributes->camera_attributes_get_exposure_normalization_factor(p_render_data->camera_attributes);
diff --git a/servers/rendering/renderer_rd/forward_mobile/render_forward_mobile.h b/servers/rendering/renderer_rd/forward_mobile/render_forward_mobile.h
index 5a4c114041..0969dd0b50 100644
--- a/servers/rendering/renderer_rd/forward_mobile/render_forward_mobile.h
+++ b/servers/rendering/renderer_rd/forward_mobile/render_forward_mobile.h
@@ -184,7 +184,7 @@ private:
 		float normal_xform[12];
 		float texture_size[2];
 		float exposure_normalization;
-		float pad;
+		uint32_t flags;
 	};
 
 	struct LightmapCaptureData {
diff --git a/servers/rendering/renderer_rd/shaders/forward_clustered/scene_forward_clustered.glsl b/servers/rendering/renderer_rd/shaders/forward_clustered/scene_forward_clustered.glsl
index 5b4d746b52..c14ea34c8f 100644
--- a/servers/rendering/renderer_rd/shaders/forward_clustered/scene_forward_clustered.glsl
+++ b/servers/rendering/renderer_rd/shaders/forward_clustered/scene_forward_clustered.glsl
@@ -1978,9 +1978,34 @@ void fragment_shader(in SceneData scene_data) {
 		uint shadow0 = 0;
 		uint shadow1 = 0;
 
+		float shadowmask = 1.0;
+
+#ifdef USE_LIGHTMAP
+		uint shadowmask_mode = LIGHTMAP_SHADOWMASK_MODE_NONE;
+
+		if (bool(instances.data[instance_index].flags & INSTANCE_FLAGS_USE_LIGHTMAP)) {
+			const uint ofs = instances.data[instance_index].gi_offset & 0xFFFF;
+			shadowmask_mode = lightmaps.data[ofs].flags;
+
+			if (shadowmask_mode != LIGHTMAP_SHADOWMASK_MODE_NONE) {
+				const uint slice = instances.data[instance_index].gi_offset >> 16;
+				const vec2 scaled_uv = uv2 * instances.data[instance_index].lightmap_uv_scale.zw + instances.data[instance_index].lightmap_uv_scale.xy;
+				const vec3 uvw = vec3(scaled_uv, float(slice));
+
+				if (sc_use_lightmap_bicubic_filter()) {
+					shadowmask = textureArray_bicubic(lightmap_textures[MAX_LIGHTMAP_TEXTURES + ofs], uvw, lightmaps.data[ofs].light_texture_size).x;
+				} else {
+					shadowmask = textureLod(sampler2DArray(lightmap_textures[MAX_LIGHTMAP_TEXTURES + ofs], SAMPLER_LINEAR_CLAMP), uvw, 0.0).x;
+				}
+			}
+		}
+
+		if (shadowmask_mode != LIGHTMAP_SHADOWMASK_MODE_ONLY) {
+#endif // USE_LIGHTMAP
+
 #ifdef USE_VERTEX_LIGHTING
-		// Only process the first light's shadow for vertex lighting.
-		for (uint i = 0; i < 1; i++) {
+			// Only process the first light's shadow for vertex lighting.
+			for (uint i = 0; i < 1; i++) {
 #else
 		for (uint i = 0; i < 8; i++) {
 			if (i >= scene_data.directional_light_count) {
@@ -1988,20 +2013,20 @@ void fragment_shader(in SceneData scene_data) {
 			}
 #endif
 
-			if (!bool(directional_lights.data[i].mask & instances.data[instance_index].layer_mask)) {
-				continue; //not masked
-			}
+				if (!bool(directional_lights.data[i].mask & instances.data[instance_index].layer_mask)) {
+					continue; //not masked
+				}
 
-			if (directional_lights.data[i].bake_mode == LIGHT_BAKE_STATIC && bool(instances.data[instance_index].flags & INSTANCE_FLAGS_USE_LIGHTMAP)) {
-				continue; // Statically baked light and object uses lightmap, skip
-			}
+				if (directional_lights.data[i].bake_mode == LIGHT_BAKE_STATIC && bool(instances.data[instance_index].flags & INSTANCE_FLAGS_USE_LIGHTMAP)) {
+					continue; // Statically baked light and object uses lightmap, skip
+				}
 
-			float shadow = 1.0;
+				float shadow = 1.0;
 
-			if (directional_lights.data[i].shadow_opacity > 0.001) {
-				float depth_z = -vertex.z;
-				vec3 light_dir = directional_lights.data[i].direction;
-				vec3 base_normal_bias = normalize(normal_interp) * (1.0 - max(0.0, dot(light_dir, -normalize(normal_interp))));
+				if (directional_lights.data[i].shadow_opacity > 0.001) {
+					float depth_z = -vertex.z;
+					vec3 light_dir = directional_lights.data[i].direction;
+					vec3 base_normal_bias = normalize(normal_interp) * (1.0 - max(0.0, dot(light_dir, -normalize(normal_interp))));
 
 #define BIAS_FUNC(m_var, m_idx)                                                                 \
 	m_var.xyz += light_dir * directional_lights.data[i].shadow_bias[m_idx];                     \
@@ -2009,195 +2034,218 @@ void fragment_shader(in SceneData scene_data) {
 	normal_bias -= light_dir * dot(light_dir, normal_bias);                                     \
 	m_var.xyz += normal_bias;
 
-				//version with soft shadows, more expensive
-				if (sc_use_directional_soft_shadows() && directional_lights.data[i].softshadow_angle > 0) {
-					uint blend_count = 0;
-					const uint blend_max = directional_lights.data[i].blend_splits ? 2 : 1;
-
-					if (depth_z < directional_lights.data[i].shadow_split_offsets.x) {
-						vec4 v = vec4(vertex, 1.0);
-
-						BIAS_FUNC(v, 0)
-
-						vec4 pssm_coord = (directional_lights.data[i].shadow_matrix1 * v);
-						pssm_coord /= pssm_coord.w;
-
-						float range_pos = dot(directional_lights.data[i].direction, v.xyz);
-						float range_begin = directional_lights.data[i].shadow_range_begin.x;
-						float test_radius = (range_pos - range_begin) * directional_lights.data[i].softshadow_angle;
-						vec2 tex_scale = directional_lights.data[i].uv_scale1 * test_radius;
-						shadow = sample_directional_soft_shadow(directional_shadow_atlas, pssm_coord.xyz, tex_scale * directional_lights.data[i].soft_shadow_scale, scene_data.taa_frame_count);
-						blend_count++;
-					}
-
-					if (blend_count < blend_max && depth_z < directional_lights.data[i].shadow_split_offsets.y) {
-						vec4 v = vec4(vertex, 1.0);
-
-						BIAS_FUNC(v, 1)
-
-						vec4 pssm_coord = (directional_lights.data[i].shadow_matrix2 * v);
-						pssm_coord /= pssm_coord.w;
-
-						float range_pos = dot(directional_lights.data[i].direction, v.xyz);
-						float range_begin = directional_lights.data[i].shadow_range_begin.y;
-						float test_radius = (range_pos - range_begin) * directional_lights.data[i].softshadow_angle;
-						vec2 tex_scale = directional_lights.data[i].uv_scale2 * test_radius;
-						float s = sample_directional_soft_shadow(directional_shadow_atlas, pssm_coord.xyz, tex_scale * directional_lights.data[i].soft_shadow_scale, scene_data.taa_frame_count);
-
-						if (blend_count == 0) {
-							shadow = s;
-						} else {
-							//blend
-							float blend = smoothstep(0.0, directional_lights.data[i].shadow_split_offsets.x, depth_z);
-							shadow = mix(shadow, s, blend);
-						}
-
-						blend_count++;
-					}
-
-					if (blend_count < blend_max && depth_z < directional_lights.data[i].shadow_split_offsets.z) {
-						vec4 v = vec4(vertex, 1.0);
-
-						BIAS_FUNC(v, 2)
-
-						vec4 pssm_coord = (directional_lights.data[i].shadow_matrix3 * v);
-						pssm_coord /= pssm_coord.w;
-
-						float range_pos = dot(directional_lights.data[i].direction, v.xyz);
-						float range_begin = directional_lights.data[i].shadow_range_begin.z;
-						float test_radius = (range_pos - range_begin) * directional_lights.data[i].softshadow_angle;
-						vec2 tex_scale = directional_lights.data[i].uv_scale3 * test_radius;
-						float s = sample_directional_soft_shadow(directional_shadow_atlas, pssm_coord.xyz, tex_scale * directional_lights.data[i].soft_shadow_scale, scene_data.taa_frame_count);
-
-						if (blend_count == 0) {
-							shadow = s;
-						} else {
-							//blend
-							float blend = smoothstep(directional_lights.data[i].shadow_split_offsets.x, directional_lights.data[i].shadow_split_offsets.y, depth_z);
-							shadow = mix(shadow, s, blend);
-						}
-
-						blend_count++;
-					}
-
-					if (blend_count < blend_max) {
-						vec4 v = vec4(vertex, 1.0);
-
-						BIAS_FUNC(v, 3)
-
-						vec4 pssm_coord = (directional_lights.data[i].shadow_matrix4 * v);
-						pssm_coord /= pssm_coord.w;
-
-						float range_pos = dot(directional_lights.data[i].direction, v.xyz);
-						float range_begin = directional_lights.data[i].shadow_range_begin.w;
-						float test_radius = (range_pos - range_begin) * directional_lights.data[i].softshadow_angle;
-						vec2 tex_scale = directional_lights.data[i].uv_scale4 * test_radius;
-						float s = sample_directional_soft_shadow(directional_shadow_atlas, pssm_coord.xyz, tex_scale * directional_lights.data[i].soft_shadow_scale, scene_data.taa_frame_count);
-
-						if (blend_count == 0) {
-							shadow = s;
-						} else {
-							//blend
-							float blend = smoothstep(directional_lights.data[i].shadow_split_offsets.y, directional_lights.data[i].shadow_split_offsets.z, depth_z);
-							shadow = mix(shadow, s, blend);
-						}
-					}
-
-				} else { //no soft shadows
-
-					vec4 pssm_coord;
-					float blur_factor;
-
-					if (depth_z < directional_lights.data[i].shadow_split_offsets.x) {
-						vec4 v = vec4(vertex, 1.0);
-
-						BIAS_FUNC(v, 0)
-
-						pssm_coord = (directional_lights.data[i].shadow_matrix1 * v);
-						blur_factor = 1.0;
-					} else if (depth_z < directional_lights.data[i].shadow_split_offsets.y) {
-						vec4 v = vec4(vertex, 1.0);
-
-						BIAS_FUNC(v, 1)
-
-						pssm_coord = (directional_lights.data[i].shadow_matrix2 * v);
-						// Adjust shadow blur with reference to the first split to reduce discrepancy between shadow splits.
-						blur_factor = directional_lights.data[i].shadow_split_offsets.x / directional_lights.data[i].shadow_split_offsets.y;
-					} else if (depth_z < directional_lights.data[i].shadow_split_offsets.z) {
-						vec4 v = vec4(vertex, 1.0);
-
-						BIAS_FUNC(v, 2)
-
-						pssm_coord = (directional_lights.data[i].shadow_matrix3 * v);
-						// Adjust shadow blur with reference to the first split to reduce discrepancy between shadow splits.
-						blur_factor = directional_lights.data[i].shadow_split_offsets.x / directional_lights.data[i].shadow_split_offsets.z;
-					} else {
-						vec4 v = vec4(vertex, 1.0);
-
-						BIAS_FUNC(v, 3)
-
-						pssm_coord = (directional_lights.data[i].shadow_matrix4 * v);
-						// Adjust shadow blur with reference to the first split to reduce discrepancy between shadow splits.
-						blur_factor = directional_lights.data[i].shadow_split_offsets.x / directional_lights.data[i].shadow_split_offsets.w;
-					}
-
-					pssm_coord /= pssm_coord.w;
-
-					shadow = sample_directional_pcf_shadow(directional_shadow_atlas, scene_data.directional_shadow_pixel_size * directional_lights.data[i].soft_shadow_scale * (blur_factor + (1.0 - blur_factor) * float(directional_lights.data[i].blend_splits)), pssm_coord, scene_data.taa_frame_count);
-
-					if (directional_lights.data[i].blend_splits) {
-						float pssm_blend;
-						float blur_factor2;
+					//version with soft shadows, more expensive
+					if (sc_use_directional_soft_shadows() && directional_lights.data[i].softshadow_angle > 0) {
+						uint blend_count = 0;
+						const uint blend_max = directional_lights.data[i].blend_splits ? 2 : 1;
 
 						if (depth_z < directional_lights.data[i].shadow_split_offsets.x) {
 							vec4 v = vec4(vertex, 1.0);
+
+							BIAS_FUNC(v, 0)
+
+							vec4 pssm_coord = (directional_lights.data[i].shadow_matrix1 * v);
+							pssm_coord /= pssm_coord.w;
+
+							float range_pos = dot(directional_lights.data[i].direction, v.xyz);
+							float range_begin = directional_lights.data[i].shadow_range_begin.x;
+							float test_radius = (range_pos - range_begin) * directional_lights.data[i].softshadow_angle;
+							vec2 tex_scale = directional_lights.data[i].uv_scale1 * test_radius;
+							shadow = sample_directional_soft_shadow(directional_shadow_atlas, pssm_coord.xyz, tex_scale * directional_lights.data[i].soft_shadow_scale, scene_data.taa_frame_count);
+							blend_count++;
+						}
+
+						if (blend_count < blend_max && depth_z < directional_lights.data[i].shadow_split_offsets.y) {
+							vec4 v = vec4(vertex, 1.0);
+
 							BIAS_FUNC(v, 1)
-							pssm_coord = (directional_lights.data[i].shadow_matrix2 * v);
-							pssm_blend = smoothstep(directional_lights.data[i].shadow_split_offsets.x - directional_lights.data[i].shadow_split_offsets.x * 0.1, directional_lights.data[i].shadow_split_offsets.x, depth_z);
-							// Adjust shadow blur with reference to the first split to reduce discrepancy between shadow splits.
-							blur_factor2 = directional_lights.data[i].shadow_split_offsets.x / directional_lights.data[i].shadow_split_offsets.y;
+
+							vec4 pssm_coord = (directional_lights.data[i].shadow_matrix2 * v);
+							pssm_coord /= pssm_coord.w;
+
+							float range_pos = dot(directional_lights.data[i].direction, v.xyz);
+							float range_begin = directional_lights.data[i].shadow_range_begin.y;
+							float test_radius = (range_pos - range_begin) * directional_lights.data[i].softshadow_angle;
+							vec2 tex_scale = directional_lights.data[i].uv_scale2 * test_radius;
+							float s = sample_directional_soft_shadow(directional_shadow_atlas, pssm_coord.xyz, tex_scale * directional_lights.data[i].soft_shadow_scale, scene_data.taa_frame_count);
+
+							if (blend_count == 0) {
+								shadow = s;
+							} else {
+								//blend
+								float blend = smoothstep(0.0, directional_lights.data[i].shadow_split_offsets.x, depth_z);
+								shadow = mix(shadow, s, blend);
+							}
+
+							blend_count++;
+						}
+
+						if (blend_count < blend_max && depth_z < directional_lights.data[i].shadow_split_offsets.z) {
+							vec4 v = vec4(vertex, 1.0);
+
+							BIAS_FUNC(v, 2)
+
+							vec4 pssm_coord = (directional_lights.data[i].shadow_matrix3 * v);
+							pssm_coord /= pssm_coord.w;
+
+							float range_pos = dot(directional_lights.data[i].direction, v.xyz);
+							float range_begin = directional_lights.data[i].shadow_range_begin.z;
+							float test_radius = (range_pos - range_begin) * directional_lights.data[i].softshadow_angle;
+							vec2 tex_scale = directional_lights.data[i].uv_scale3 * test_radius;
+							float s = sample_directional_soft_shadow(directional_shadow_atlas, pssm_coord.xyz, tex_scale * directional_lights.data[i].soft_shadow_scale, scene_data.taa_frame_count);
+
+							if (blend_count == 0) {
+								shadow = s;
+							} else {
+								//blend
+								float blend = smoothstep(directional_lights.data[i].shadow_split_offsets.x, directional_lights.data[i].shadow_split_offsets.y, depth_z);
+								shadow = mix(shadow, s, blend);
+							}
+
+							blend_count++;
+						}
+
+						if (blend_count < blend_max) {
+							vec4 v = vec4(vertex, 1.0);
+
+							BIAS_FUNC(v, 3)
+
+							vec4 pssm_coord = (directional_lights.data[i].shadow_matrix4 * v);
+							pssm_coord /= pssm_coord.w;
+
+							float range_pos = dot(directional_lights.data[i].direction, v.xyz);
+							float range_begin = directional_lights.data[i].shadow_range_begin.w;
+							float test_radius = (range_pos - range_begin) * directional_lights.data[i].softshadow_angle;
+							vec2 tex_scale = directional_lights.data[i].uv_scale4 * test_radius;
+							float s = sample_directional_soft_shadow(directional_shadow_atlas, pssm_coord.xyz, tex_scale * directional_lights.data[i].soft_shadow_scale, scene_data.taa_frame_count);
+
+							if (blend_count == 0) {
+								shadow = s;
+							} else {
+								//blend
+								float blend = smoothstep(directional_lights.data[i].shadow_split_offsets.y, directional_lights.data[i].shadow_split_offsets.z, depth_z);
+								shadow = mix(shadow, s, blend);
+							}
+						}
+
+					} else { //no soft shadows
+
+						vec4 pssm_coord;
+						float blur_factor;
+
+						if (depth_z < directional_lights.data[i].shadow_split_offsets.x) {
+							vec4 v = vec4(vertex, 1.0);
+
+							BIAS_FUNC(v, 0)
+
+							pssm_coord = (directional_lights.data[i].shadow_matrix1 * v);
+							blur_factor = 1.0;
 						} else if (depth_z < directional_lights.data[i].shadow_split_offsets.y) {
 							vec4 v = vec4(vertex, 1.0);
-							BIAS_FUNC(v, 2)
-							pssm_coord = (directional_lights.data[i].shadow_matrix3 * v);
-							pssm_blend = smoothstep(directional_lights.data[i].shadow_split_offsets.y - directional_lights.data[i].shadow_split_offsets.y * 0.1, directional_lights.data[i].shadow_split_offsets.y, depth_z);
+
+							BIAS_FUNC(v, 1)
+
+							pssm_coord = (directional_lights.data[i].shadow_matrix2 * v);
 							// Adjust shadow blur with reference to the first split to reduce discrepancy between shadow splits.
-							blur_factor2 = directional_lights.data[i].shadow_split_offsets.x / directional_lights.data[i].shadow_split_offsets.z;
+							blur_factor = directional_lights.data[i].shadow_split_offsets.x / directional_lights.data[i].shadow_split_offsets.y;
 						} else if (depth_z < directional_lights.data[i].shadow_split_offsets.z) {
 							vec4 v = vec4(vertex, 1.0);
-							BIAS_FUNC(v, 3)
-							pssm_coord = (directional_lights.data[i].shadow_matrix4 * v);
-							pssm_blend = smoothstep(directional_lights.data[i].shadow_split_offsets.z - directional_lights.data[i].shadow_split_offsets.z * 0.1, directional_lights.data[i].shadow_split_offsets.z, depth_z);
+
+							BIAS_FUNC(v, 2)
+
+							pssm_coord = (directional_lights.data[i].shadow_matrix3 * v);
 							// Adjust shadow blur with reference to the first split to reduce discrepancy between shadow splits.
-							blur_factor2 = directional_lights.data[i].shadow_split_offsets.x / directional_lights.data[i].shadow_split_offsets.w;
+							blur_factor = directional_lights.data[i].shadow_split_offsets.x / directional_lights.data[i].shadow_split_offsets.z;
 						} else {
-							pssm_blend = 0.0; //if no blend, same coord will be used (divide by z will result in same value, and already cached)
-							blur_factor2 = 1.0;
+							vec4 v = vec4(vertex, 1.0);
+
+							BIAS_FUNC(v, 3)
+
+							pssm_coord = (directional_lights.data[i].shadow_matrix4 * v);
+							// Adjust shadow blur with reference to the first split to reduce discrepancy between shadow splits.
+							blur_factor = directional_lights.data[i].shadow_split_offsets.x / directional_lights.data[i].shadow_split_offsets.w;
 						}
 
 						pssm_coord /= pssm_coord.w;
 
-						float shadow2 = sample_directional_pcf_shadow(directional_shadow_atlas, scene_data.directional_shadow_pixel_size * directional_lights.data[i].soft_shadow_scale * (blur_factor2 + (1.0 - blur_factor2) * float(directional_lights.data[i].blend_splits)), pssm_coord, scene_data.taa_frame_count);
-						shadow = mix(shadow, shadow2, pssm_blend);
-					}
-				}
+						shadow = sample_directional_pcf_shadow(directional_shadow_atlas, scene_data.directional_shadow_pixel_size * directional_lights.data[i].soft_shadow_scale * (blur_factor + (1.0 - blur_factor) * float(directional_lights.data[i].blend_splits)), pssm_coord, scene_data.taa_frame_count);
 
-				shadow = mix(shadow, 1.0, smoothstep(directional_lights.data[i].fade_from, directional_lights.data[i].fade_to, vertex.z)); //done with negative values for performance
+						if (directional_lights.data[i].blend_splits) {
+							float pssm_blend;
+							float blur_factor2;
+
+							if (depth_z < directional_lights.data[i].shadow_split_offsets.x) {
+								vec4 v = vec4(vertex, 1.0);
+								BIAS_FUNC(v, 1)
+								pssm_coord = (directional_lights.data[i].shadow_matrix2 * v);
+								pssm_blend = smoothstep(directional_lights.data[i].shadow_split_offsets.x - directional_lights.data[i].shadow_split_offsets.x * 0.1, directional_lights.data[i].shadow_split_offsets.x, depth_z);
+								// Adjust shadow blur with reference to the first split to reduce discrepancy between shadow splits.
+								blur_factor2 = directional_lights.data[i].shadow_split_offsets.x / directional_lights.data[i].shadow_split_offsets.y;
+							} else if (depth_z < directional_lights.data[i].shadow_split_offsets.y) {
+								vec4 v = vec4(vertex, 1.0);
+								BIAS_FUNC(v, 2)
+								pssm_coord = (directional_lights.data[i].shadow_matrix3 * v);
+								pssm_blend = smoothstep(directional_lights.data[i].shadow_split_offsets.y - directional_lights.data[i].shadow_split_offsets.y * 0.1, directional_lights.data[i].shadow_split_offsets.y, depth_z);
+								// Adjust shadow blur with reference to the first split to reduce discrepancy between shadow splits.
+								blur_factor2 = directional_lights.data[i].shadow_split_offsets.x / directional_lights.data[i].shadow_split_offsets.z;
+							} else if (depth_z < directional_lights.data[i].shadow_split_offsets.z) {
+								vec4 v = vec4(vertex, 1.0);
+								BIAS_FUNC(v, 3)
+								pssm_coord = (directional_lights.data[i].shadow_matrix4 * v);
+								pssm_blend = smoothstep(directional_lights.data[i].shadow_split_offsets.z - directional_lights.data[i].shadow_split_offsets.z * 0.1, directional_lights.data[i].shadow_split_offsets.z, depth_z);
+								// Adjust shadow blur with reference to the first split to reduce discrepancy between shadow splits.
+								blur_factor2 = directional_lights.data[i].shadow_split_offsets.x / directional_lights.data[i].shadow_split_offsets.w;
+							} else {
+								pssm_blend = 0.0; //if no blend, same coord will be used (divide by z will result in same value, and already cached)
+								blur_factor2 = 1.0;
+							}
+
+							pssm_coord /= pssm_coord.w;
+
+							float shadow2 = sample_directional_pcf_shadow(directional_shadow_atlas, scene_data.directional_shadow_pixel_size * directional_lights.data[i].soft_shadow_scale * (blur_factor2 + (1.0 - blur_factor2) * float(directional_lights.data[i].blend_splits)), pssm_coord, scene_data.taa_frame_count);
+							shadow = mix(shadow, shadow2, pssm_blend);
+						}
+					}
+
+#ifdef USE_LIGHTMAP
+					if (shadowmask_mode == LIGHTMAP_SHADOWMASK_MODE_REPLACE) {
+						shadow = mix(shadow, shadowmask, smoothstep(directional_lights.data[i].fade_from, directional_lights.data[i].fade_to, vertex.z)); //done with negative values for performance
+					} else if (shadowmask_mode == LIGHTMAP_SHADOWMASK_MODE_OVERLAY) {
+						shadow = shadowmask * mix(shadow, 1.0, smoothstep(directional_lights.data[i].fade_from, directional_lights.data[i].fade_to, vertex.z)); //done with negative values for performance
+					} else {
+#endif
+						shadow = mix(shadow, 1.0, smoothstep(directional_lights.data[i].fade_from, directional_lights.data[i].fade_to, vertex.z)); //done with negative values for performance
+#ifdef USE_LIGHTMAP
+					}
+#endif
 
 #ifdef USE_VERTEX_LIGHTING
-				diffuse_light *= mix(1.0, shadow, diffuse_light_interp.a);
-				specular_light *= mix(1.0, shadow, specular_light_interp.a);
+					diffuse_light *= mix(1.0, shadow, diffuse_light_interp.a);
+					specular_light *= mix(1.0, shadow, specular_light_interp.a);
 #endif
 
 #undef BIAS_FUNC
-			} // shadows
+				} // shadows
 
-			if (i < 4) {
-				shadow0 |= uint(clamp(shadow * 255.0, 0.0, 255.0)) << (i * 8);
-			} else {
-				shadow1 |= uint(clamp(shadow * 255.0, 0.0, 255.0)) << ((i - 4) * 8);
+				if (i < 4) {
+					shadow0 |= uint(clamp(shadow * 255.0, 0.0, 255.0)) << (i * 8);
+				} else {
+					shadow1 |= uint(clamp(shadow * 255.0, 0.0, 255.0)) << ((i - 4) * 8);
+				}
 			}
+
+#ifdef USE_LIGHTMAP
+		} else { // shadowmask_mode == LIGHTMAP_SHADOWMASK_MODE_ONLY
+
+#ifdef USE_VERTEX_LIGHTING
+			diffuse_light *= mix(1.0, shadowmask, diffuse_light_interp.a);
+			specular_light *= mix(1.0, shadowmask, specular_light_interp.a);
+#endif
+
+			shadow0 |= uint(clamp(shadowmask * 255.0, 0.0, 255.0));
 		}
+#endif // USE_LIGHTMAP
+
 #endif // SHADOWS_DISABLED
 
 #ifndef USE_VERTEX_LIGHTING
diff --git a/servers/rendering/renderer_rd/shaders/forward_clustered/scene_forward_clustered_inc.glsl b/servers/rendering/renderer_rd/shaders/forward_clustered/scene_forward_clustered_inc.glsl
index 8f153f7ed5..3257a0d056 100644
--- a/servers/rendering/renderer_rd/shaders/forward_clustered/scene_forward_clustered_inc.glsl
+++ b/servers/rendering/renderer_rd/shaders/forward_clustered/scene_forward_clustered_inc.glsl
@@ -200,11 +200,16 @@ directional_lights;
 #define LIGHTMAP_FLAG_USE_DIRECTION 1
 #define LIGHTMAP_FLAG_USE_SPECULAR_DIRECTION 2
 
+#define LIGHTMAP_SHADOWMASK_MODE_NONE 0
+#define LIGHTMAP_SHADOWMASK_MODE_REPLACE 1
+#define LIGHTMAP_SHADOWMASK_MODE_OVERLAY 2
+#define LIGHTMAP_SHADOWMASK_MODE_ONLY 3
+
 struct Lightmap {
 	mat3 normal_xform;
 	vec2 light_texture_size;
 	float exposure_normalization;
-	float pad;
+	uint flags;
 };
 
 layout(set = 0, binding = 7, std140) restrict readonly buffer Lightmaps {
@@ -349,7 +354,7 @@ layout(set = 1, binding = 5) uniform texture2D shadow_atlas;
 
 layout(set = 1, binding = 6) uniform texture2D directional_shadow_atlas;
 
-layout(set = 1, binding = 7) uniform texture2DArray lightmap_textures[MAX_LIGHTMAP_TEXTURES];
+layout(set = 1, binding = 7) uniform texture2DArray lightmap_textures[MAX_LIGHTMAP_TEXTURES * 2];
 
 layout(set = 1, binding = 8) uniform texture3D voxel_gi_textures[MAX_VOXEL_GI_INSTANCES];
 
diff --git a/servers/rendering/renderer_rd/shaders/forward_mobile/scene_forward_mobile.glsl b/servers/rendering/renderer_rd/shaders/forward_mobile/scene_forward_mobile.glsl
index dbca038d3a..a3c264b823 100644
--- a/servers/rendering/renderer_rd/shaders/forward_mobile/scene_forward_mobile.glsl
+++ b/servers/rendering/renderer_rd/shaders/forward_mobile/scene_forward_mobile.glsl
@@ -1434,30 +1434,54 @@ void main() {
 		uint shadow0 = 0;
 		uint shadow1 = 0;
 
+		float shadowmask = 1.0;
+
+#ifdef USE_LIGHTMAP
+		uint shadowmask_mode = LIGHTMAP_SHADOWMASK_MODE_NONE;
+
+		if (bool(instances.data[draw_call.instance_index].flags & INSTANCE_FLAGS_USE_LIGHTMAP)) {
+			const uint ofs = instances.data[draw_call.instance_index].gi_offset & 0xFFFF;
+			shadowmask_mode = lightmaps.data[ofs].flags;
+
+			if (shadowmask_mode != LIGHTMAP_SHADOWMASK_MODE_NONE) {
+				const uint slice = instances.data[draw_call.instance_index].gi_offset >> 16;
+				const vec2 scaled_uv = uv2 * instances.data[draw_call.instance_index].lightmap_uv_scale.zw + instances.data[draw_call.instance_index].lightmap_uv_scale.xy;
+				const vec3 uvw = vec3(scaled_uv, float(slice));
+
+				if (sc_use_lightmap_bicubic_filter()) {
+					shadowmask = textureArray_bicubic(lightmap_textures[MAX_LIGHTMAP_TEXTURES + ofs], uvw, lightmaps.data[ofs].light_texture_size).x;
+				} else {
+					shadowmask = textureLod(sampler2DArray(lightmap_textures[MAX_LIGHTMAP_TEXTURES + ofs], SAMPLER_LINEAR_CLAMP), uvw, 0.0).x;
+				}
+			}
+		}
+
+		if (shadowmask_mode != LIGHTMAP_SHADOWMASK_MODE_ONLY) {
+#endif // USE_LIGHTMAP
+
 #ifdef USE_VERTEX_LIGHTING
-		// Only process the first light's shadow for vertex lighting.
-		for (uint i = 0; i < 1; i++) {
+			// Only process the first light's shadow for vertex lighting.
+			for (uint i = 0; i < 1; i++) {
 #else
 		for (uint i = 0; i < sc_directional_lights(); i++) {
 #endif
+				if (!bool(directional_lights.data[i].mask & instances.data[draw_call.instance_index].layer_mask)) {
+					continue; //not masked
+				}
 
-			if (!bool(directional_lights.data[i].mask & instances.data[draw_call.instance_index].layer_mask)) {
-				continue; //not masked
-			}
+				if (directional_lights.data[i].bake_mode == LIGHT_BAKE_STATIC && bool(instances.data[draw_call.instance_index].flags & INSTANCE_FLAGS_USE_LIGHTMAP)) {
+					continue; // Statically baked light and object uses lightmap, skip.
+				}
 
-			if (directional_lights.data[i].bake_mode == LIGHT_BAKE_STATIC && bool(instances.data[draw_call.instance_index].flags & INSTANCE_FLAGS_USE_LIGHTMAP)) {
-				continue; // Statically baked light and object uses lightmap, skip.
-			}
+				float shadow = 1.0;
 
-			float shadow = 1.0;
+				if (directional_lights.data[i].shadow_opacity > 0.001) {
+					float depth_z = -vertex.z;
 
-			if (directional_lights.data[i].shadow_opacity > 0.001) {
-				float depth_z = -vertex.z;
-
-				vec4 pssm_coord;
-				float blur_factor;
-				vec3 light_dir = directional_lights.data[i].direction;
-				vec3 base_normal_bias = normalize(normal_interp) * (1.0 - max(0.0, dot(light_dir, -normalize(normal_interp))));
+					vec4 pssm_coord;
+					float blur_factor;
+					vec3 light_dir = directional_lights.data[i].direction;
+					vec3 base_normal_bias = normalize(normal_interp) * (1.0 - max(0.0, dot(light_dir, -normalize(normal_interp))));
 
 #define BIAS_FUNC(m_var, m_idx)                                                                 \
 	m_var.xyz += light_dir * directional_lights.data[i].shadow_bias[m_idx];                     \
@@ -1465,97 +1489,119 @@ void main() {
 	normal_bias -= light_dir * dot(light_dir, normal_bias);                                     \
 	m_var.xyz += normal_bias;
 
-				if (depth_z < directional_lights.data[i].shadow_split_offsets.x) {
-					vec4 v = vec4(vertex, 1.0);
-
-					BIAS_FUNC(v, 0)
-
-					pssm_coord = (directional_lights.data[i].shadow_matrix1 * v);
-					blur_factor = 1.0;
-				} else if (depth_z < directional_lights.data[i].shadow_split_offsets.y) {
-					vec4 v = vec4(vertex, 1.0);
-
-					BIAS_FUNC(v, 1)
-
-					pssm_coord = (directional_lights.data[i].shadow_matrix2 * v);
-					// Adjust shadow blur with reference to the first split to reduce discrepancy between shadow splits.
-					blur_factor = directional_lights.data[i].shadow_split_offsets.x / directional_lights.data[i].shadow_split_offsets.y;
-					;
-				} else if (depth_z < directional_lights.data[i].shadow_split_offsets.z) {
-					vec4 v = vec4(vertex, 1.0);
-
-					BIAS_FUNC(v, 2)
-
-					pssm_coord = (directional_lights.data[i].shadow_matrix3 * v);
-					// Adjust shadow blur with reference to the first split to reduce discrepancy between shadow splits.
-					blur_factor = directional_lights.data[i].shadow_split_offsets.x / directional_lights.data[i].shadow_split_offsets.z;
-				} else {
-					vec4 v = vec4(vertex, 1.0);
-
-					BIAS_FUNC(v, 3)
-
-					pssm_coord = (directional_lights.data[i].shadow_matrix4 * v);
-					// Adjust shadow blur with reference to the first split to reduce discrepancy between shadow splits.
-					blur_factor = directional_lights.data[i].shadow_split_offsets.x / directional_lights.data[i].shadow_split_offsets.w;
-				}
-
-				pssm_coord /= pssm_coord.w;
-
-				bool blend_split = sc_directional_light_blend_split(i);
-				float blend_split_weight = blend_split ? 1.0f : 0.0f;
-				shadow = sample_directional_pcf_shadow(directional_shadow_atlas, scene_data.directional_shadow_pixel_size * directional_lights.data[i].soft_shadow_scale * (blur_factor + (1.0 - blur_factor) * blend_split_weight), pssm_coord, scene_data.taa_frame_count);
-
-				if (blend_split) {
-					float pssm_blend;
-					float blur_factor2;
-
 					if (depth_z < directional_lights.data[i].shadow_split_offsets.x) {
 						vec4 v = vec4(vertex, 1.0);
-						BIAS_FUNC(v, 1)
-						pssm_coord = (directional_lights.data[i].shadow_matrix2 * v);
-						pssm_blend = smoothstep(directional_lights.data[i].shadow_split_offsets.x - directional_lights.data[i].shadow_split_offsets.x * 0.1, directional_lights.data[i].shadow_split_offsets.x, depth_z);
-						// Adjust shadow blur with reference to the first split to reduce discrepancy between shadow splits.
-						blur_factor2 = directional_lights.data[i].shadow_split_offsets.x / directional_lights.data[i].shadow_split_offsets.y;
+
+						BIAS_FUNC(v, 0)
+
+						pssm_coord = (directional_lights.data[i].shadow_matrix1 * v);
+						blur_factor = 1.0;
 					} else if (depth_z < directional_lights.data[i].shadow_split_offsets.y) {
 						vec4 v = vec4(vertex, 1.0);
-						BIAS_FUNC(v, 2)
-						pssm_coord = (directional_lights.data[i].shadow_matrix3 * v);
-						pssm_blend = smoothstep(directional_lights.data[i].shadow_split_offsets.y - directional_lights.data[i].shadow_split_offsets.y * 0.1, directional_lights.data[i].shadow_split_offsets.y, depth_z);
+
+						BIAS_FUNC(v, 1)
+
+						pssm_coord = (directional_lights.data[i].shadow_matrix2 * v);
 						// Adjust shadow blur with reference to the first split to reduce discrepancy between shadow splits.
-						blur_factor2 = directional_lights.data[i].shadow_split_offsets.x / directional_lights.data[i].shadow_split_offsets.z;
+						blur_factor = directional_lights.data[i].shadow_split_offsets.x / directional_lights.data[i].shadow_split_offsets.y;
 					} else if (depth_z < directional_lights.data[i].shadow_split_offsets.z) {
 						vec4 v = vec4(vertex, 1.0);
-						BIAS_FUNC(v, 3)
-						pssm_coord = (directional_lights.data[i].shadow_matrix4 * v);
-						pssm_blend = smoothstep(directional_lights.data[i].shadow_split_offsets.z - directional_lights.data[i].shadow_split_offsets.z * 0.1, directional_lights.data[i].shadow_split_offsets.z, depth_z);
+
+						BIAS_FUNC(v, 2)
+
+						pssm_coord = (directional_lights.data[i].shadow_matrix3 * v);
 						// Adjust shadow blur with reference to the first split to reduce discrepancy between shadow splits.
-						blur_factor2 = directional_lights.data[i].shadow_split_offsets.x / directional_lights.data[i].shadow_split_offsets.w;
+						blur_factor = directional_lights.data[i].shadow_split_offsets.x / directional_lights.data[i].shadow_split_offsets.z;
 					} else {
-						pssm_blend = 0.0; //if no blend, same coord will be used (divide by z will result in same value, and already cached)
-						blur_factor2 = 1.0;
+						vec4 v = vec4(vertex, 1.0);
+
+						BIAS_FUNC(v, 3)
+
+						pssm_coord = (directional_lights.data[i].shadow_matrix4 * v);
+						// Adjust shadow blur with reference to the first split to reduce discrepancy between shadow splits.
+						blur_factor = directional_lights.data[i].shadow_split_offsets.x / directional_lights.data[i].shadow_split_offsets.w;
 					}
 
 					pssm_coord /= pssm_coord.w;
 
-					float shadow2 = sample_directional_pcf_shadow(directional_shadow_atlas, scene_data.directional_shadow_pixel_size * directional_lights.data[i].soft_shadow_scale * (blur_factor2 + (1.0 - blur_factor2) * blend_split_weight), pssm_coord, scene_data.taa_frame_count);
-					shadow = mix(shadow, shadow2, pssm_blend);
-				}
+					bool blend_split = sc_directional_light_blend_split(i);
+					float blend_split_weight = blend_split ? 1.0f : 0.0f;
+					shadow = sample_directional_pcf_shadow(directional_shadow_atlas, scene_data.directional_shadow_pixel_size * directional_lights.data[i].soft_shadow_scale * (blur_factor + (1.0 - blur_factor) * blend_split_weight), pssm_coord, scene_data.taa_frame_count);
 
-				shadow = mix(shadow, 1.0, smoothstep(directional_lights.data[i].fade_from, directional_lights.data[i].fade_to, vertex.z)); //done with negative values for performance
+					if (blend_split) {
+						float pssm_blend;
+						float blur_factor2;
+
+						if (depth_z < directional_lights.data[i].shadow_split_offsets.x) {
+							vec4 v = vec4(vertex, 1.0);
+							BIAS_FUNC(v, 1)
+							pssm_coord = (directional_lights.data[i].shadow_matrix2 * v);
+							pssm_blend = smoothstep(directional_lights.data[i].shadow_split_offsets.x - directional_lights.data[i].shadow_split_offsets.x * 0.1, directional_lights.data[i].shadow_split_offsets.x, depth_z);
+							// Adjust shadow blur with reference to the first split to reduce discrepancy between shadow splits.
+							blur_factor2 = directional_lights.data[i].shadow_split_offsets.x / directional_lights.data[i].shadow_split_offsets.y;
+						} else if (depth_z < directional_lights.data[i].shadow_split_offsets.y) {
+							vec4 v = vec4(vertex, 1.0);
+							BIAS_FUNC(v, 2)
+							pssm_coord = (directional_lights.data[i].shadow_matrix3 * v);
+							pssm_blend = smoothstep(directional_lights.data[i].shadow_split_offsets.y - directional_lights.data[i].shadow_split_offsets.y * 0.1, directional_lights.data[i].shadow_split_offsets.y, depth_z);
+							// Adjust shadow blur with reference to the first split to reduce discrepancy between shadow splits.
+							blur_factor2 = directional_lights.data[i].shadow_split_offsets.x / directional_lights.data[i].shadow_split_offsets.z;
+						} else if (depth_z < directional_lights.data[i].shadow_split_offsets.z) {
+							vec4 v = vec4(vertex, 1.0);
+							BIAS_FUNC(v, 3)
+							pssm_coord = (directional_lights.data[i].shadow_matrix4 * v);
+							pssm_blend = smoothstep(directional_lights.data[i].shadow_split_offsets.z - directional_lights.data[i].shadow_split_offsets.z * 0.1, directional_lights.data[i].shadow_split_offsets.z, depth_z);
+							// Adjust shadow blur with reference to the first split to reduce discrepancy between shadow splits.
+							blur_factor2 = directional_lights.data[i].shadow_split_offsets.x / directional_lights.data[i].shadow_split_offsets.w;
+						} else {
+							pssm_blend = 0.0; //if no blend, same coord will be used (divide by z will result in same value, and already cached)
+							blur_factor2 = 1.0;
+						}
+
+						pssm_coord /= pssm_coord.w;
+
+						float shadow2 = sample_directional_pcf_shadow(directional_shadow_atlas, scene_data.directional_shadow_pixel_size * directional_lights.data[i].soft_shadow_scale * (blur_factor2 + (1.0 - blur_factor2) * blend_split_weight), pssm_coord, scene_data.taa_frame_count);
+						shadow = mix(shadow, shadow2, pssm_blend);
+					}
+
+#ifdef USE_LIGHTMAP
+					if (shadowmask_mode == LIGHTMAP_SHADOWMASK_MODE_REPLACE) {
+						shadow = mix(shadow, shadowmask, smoothstep(directional_lights.data[i].fade_from, directional_lights.data[i].fade_to, vertex.z)); //done with negative values for performance
+					} else if (shadowmask_mode == LIGHTMAP_SHADOWMASK_MODE_OVERLAY) {
+						shadow = shadowmask * mix(shadow, 1.0, smoothstep(directional_lights.data[i].fade_from, directional_lights.data[i].fade_to, vertex.z)); //done with negative values for performance
+					} else {
+#endif
+						shadow = mix(shadow, 1.0, smoothstep(directional_lights.data[i].fade_from, directional_lights.data[i].fade_to, vertex.z)); //done with negative values for performance
+#ifdef USE_LIGHTMAP
+					}
+#endif
 
 #ifdef USE_VERTEX_LIGHTING
-				diffuse_light *= mix(1.0, shadow, diffuse_light_interp.a);
-				specular_light *= mix(1.0, shadow, specular_light_interp.a);
+					diffuse_light *= mix(1.0, shadow, diffuse_light_interp.a);
+					specular_light *= mix(1.0, shadow, specular_light_interp.a);
 #endif
 #undef BIAS_FUNC
+				}
+
+				if (i < 4) {
+					shadow0 |= uint(clamp(shadow * 255.0, 0.0, 255.0)) << (i * 8);
+				} else {
+					shadow1 |= uint(clamp(shadow * 255.0, 0.0, 255.0)) << ((i - 4) * 8);
+				}
 			}
 
-			if (i < 4) {
-				shadow0 |= uint(clamp(shadow * 255.0, 0.0, 255.0)) << (i * 8);
-			} else {
-				shadow1 |= uint(clamp(shadow * 255.0, 0.0, 255.0)) << ((i - 4) * 8);
-			}
+#ifdef USE_LIGHTMAP
+		} else { // shadowmask_mode == LIGHTMAP_SHADOWMASK_MODE_ONLY
+
+#ifdef USE_VERTEX_LIGHTING
+			diffuse_light *= mix(1.0, shadowmask, diffuse_light_interp.a);
+			specular_light *= mix(1.0, shadowmask, specular_light_interp.a);
+#endif
+
+			shadow0 |= uint(clamp(shadowmask * 255.0, 0.0, 255.0));
 		}
+#endif // USE_LIGHTMAP
+
 #endif // SHADOWS_DISABLED
 
 #ifndef USE_VERTEX_LIGHTING
diff --git a/servers/rendering/renderer_rd/shaders/forward_mobile/scene_forward_mobile_inc.glsl b/servers/rendering/renderer_rd/shaders/forward_mobile/scene_forward_mobile_inc.glsl
index 5e4719c498..49c8905dbf 100644
--- a/servers/rendering/renderer_rd/shaders/forward_mobile/scene_forward_mobile_inc.glsl
+++ b/servers/rendering/renderer_rd/shaders/forward_mobile/scene_forward_mobile_inc.glsl
@@ -246,11 +246,16 @@ directional_lights;
 #define LIGHTMAP_FLAG_USE_DIRECTION 1
 #define LIGHTMAP_FLAG_USE_SPECULAR_DIRECTION 2
 
+#define LIGHTMAP_SHADOWMASK_MODE_NONE 0
+#define LIGHTMAP_SHADOWMASK_MODE_REPLACE 1
+#define LIGHTMAP_SHADOWMASK_MODE_OVERLAY 2
+#define LIGHTMAP_SHADOWMASK_MODE_ONLY 3
+
 struct Lightmap {
 	mediump mat3 normal_xform;
 	vec2 light_texture_size;
 	float exposure_normalization;
-	float pad;
+	uint flags;
 };
 
 layout(set = 0, binding = 7, std140) restrict readonly buffer Lightmaps {
@@ -330,7 +335,7 @@ layout(set = 1, binding = 4) uniform highp texture2D shadow_atlas;
 layout(set = 1, binding = 5) uniform highp texture2D directional_shadow_atlas;
 
 // this needs to change to providing just the lightmap we're using..
-layout(set = 1, binding = 6) uniform texture2DArray lightmap_textures[MAX_LIGHTMAP_TEXTURES];
+layout(set = 1, binding = 6) uniform texture2DArray lightmap_textures[MAX_LIGHTMAP_TEXTURES * 2];
 
 #ifdef USE_MULTIVIEW
 layout(set = 1, binding = 9) uniform highp texture2DArray depth_buffer;
diff --git a/servers/rendering/renderer_rd/storage_rd/light_storage.cpp b/servers/rendering/renderer_rd/storage_rd/light_storage.cpp
index a812c894b9..5c1283c762 100644
--- a/servers/rendering/renderer_rd/storage_rd/light_storage.cpp
+++ b/servers/rendering/renderer_rd/storage_rd/light_storage.cpp
@@ -55,12 +55,18 @@ LightStorage::LightStorage() {
 
 		if (textures_per_stage <= 256) {
 			lightmap_textures.resize(32);
+			shadowmask_textures.resize(32);
 		} else {
 			lightmap_textures.resize(1024);
+			shadowmask_textures.resize(1024);
 		}
 
-		for (int i = 0; i < lightmap_textures.size(); i++) {
-			lightmap_textures.write[i] = texture_storage->texture_rd_get_default(TextureStorage::DEFAULT_RD_TEXTURE_2D_ARRAY_WHITE);
+		for (RID &lightmap_texture : lightmap_textures) {
+			lightmap_texture = texture_storage->texture_rd_get_default(TextureStorage::DEFAULT_RD_TEXTURE_2D_ARRAY_WHITE);
+		}
+
+		for (RID &shadowmask_texture : shadowmask_textures) {
+			shadowmask_texture = texture_storage->texture_rd_get_default(TextureStorage::DEFAULT_RD_TEXTURE_2D_ARRAY_WHITE);
 		}
 	}
 
@@ -2003,6 +2009,64 @@ AABB LightStorage::lightmap_get_aabb(RID p_lightmap) const {
 	return lm->bounds;
 }
 
+void LightStorage::lightmap_set_shadowmask_textures(RID p_lightmap, RID p_shadow) {
+	TextureStorage *texture_storage = TextureStorage::get_singleton();
+
+	Lightmap *lm = lightmap_owner.get_or_null(p_lightmap);
+	ERR_FAIL_NULL(lm);
+
+	// Erase lightmap users from shadow texture.
+	if (lm->shadow_texture.is_valid()) {
+		TextureStorage::Texture *t = texture_storage->get_texture(lm->shadow_texture);
+		if (t) {
+			t->lightmap_users.erase(p_lightmap);
+		}
+	}
+
+	TextureStorage::Texture *t = texture_storage->get_texture(p_shadow);
+	lm->shadow_texture = p_shadow;
+
+	RID default_2d_array = texture_storage->texture_rd_get_default(TextureStorage::DEFAULT_RD_TEXTURE_2D_ARRAY_WHITE);
+	if (!t) {
+		if (lm->array_index >= 0) {
+			shadowmask_textures.write[lm->array_index] = default_2d_array;
+			lm->array_index = -1;
+		}
+
+		return;
+	}
+
+	t->lightmap_users.insert(p_lightmap);
+
+	if (lm->array_index < 0) {
+		// Not in array, try to put in array.
+		for (int i = 0; i < shadowmask_textures.size(); i++) {
+			if (shadowmask_textures[i] == default_2d_array) {
+				lm->array_index = i;
+				break;
+			}
+		}
+	}
+
+	ERR_FAIL_COND_MSG(lm->array_index < 0, vformat("Maximum amount of shadowmasks in use (%d) has been exceeded, shadowmask will not display properly.", shadowmask_textures.size()));
+
+	shadowmask_textures.write[lm->array_index] = t->rd_texture;
+}
+
+RS::ShadowmaskMode LightStorage::lightmap_get_shadowmask_mode(RID p_lightmap) {
+	Lightmap *lm = lightmap_owner.get_or_null(p_lightmap);
+	ERR_FAIL_NULL_V(lm, RS::SHADOWMASK_MODE_NONE);
+
+	return lm->shadowmask_mode;
+}
+
+void LightStorage::lightmap_set_shadowmask_mode(RID p_lightmap, RS::ShadowmaskMode p_mode) {
+	Lightmap *lm = lightmap_owner.get_or_null(p_lightmap);
+	ERR_FAIL_NULL(lm);
+
+	lm->shadowmask_mode = p_mode;
+}
+
 /* LIGHTMAP INSTANCE */
 
 RID LightStorage::lightmap_instance_create(RID p_lightmap) {
diff --git a/servers/rendering/renderer_rd/storage_rd/light_storage.h b/servers/rendering/renderer_rd/storage_rd/light_storage.h
index b5f1523123..ccba9e4aa5 100644
--- a/servers/rendering/renderer_rd/storage_rd/light_storage.h
+++ b/servers/rendering/renderer_rd/storage_rd/light_storage.h
@@ -329,6 +329,8 @@ private:
 
 	struct Lightmap {
 		RID light_texture;
+		RID shadow_texture;
+		RS::ShadowmaskMode shadowmask_mode = RS::SHADOWMASK_MODE_NONE;
 		bool uses_spherical_harmonics = false;
 		bool interior = false;
 		AABB bounds = AABB(Vector3(), Vector3(1, 1, 1));
@@ -356,6 +358,8 @@ private:
 
 	mutable RID_Owner<Lightmap, true> lightmap_owner;
 
+	Vector<RID> shadowmask_textures;
+
 	/* LIGHTMAP INSTANCE */
 
 	struct LightmapInstance {
@@ -985,6 +989,10 @@ public:
 
 	Dependency *lightmap_get_dependency(RID p_lightmap) const;
 
+	virtual void lightmap_set_shadowmask_textures(RID p_lightmap, RID p_shadow) override;
+	virtual RS::ShadowmaskMode lightmap_get_shadowmask_mode(RID p_lightmap) override;
+	virtual void lightmap_set_shadowmask_mode(RID p_lightmap, RS::ShadowmaskMode p_mode) override;
+
 	virtual float lightmap_get_probe_capture_update_speed() const override {
 		return lightmap_probe_capture_update_speed;
 	}
@@ -1027,6 +1035,12 @@ public:
 		return lightmap_textures;
 	}
 
+	_FORCE_INLINE_ RID shadowmask_get_texture(RID p_lightmap) const {
+		const Lightmap *lm = lightmap_owner.get_or_null(p_lightmap);
+		ERR_FAIL_NULL_V(lm, RID());
+		return lm->shadow_texture;
+	}
+
 	/* LIGHTMAP INSTANCE */
 
 	bool owns_lightmap_instance(RID p_rid) { return lightmap_instance_owner.owns(p_rid); }
diff --git a/servers/rendering/rendering_server_default.h b/servers/rendering/rendering_server_default.h
index 29b1e163c7..f0e6ef2011 100644
--- a/servers/rendering/rendering_server_default.h
+++ b/servers/rendering/rendering_server_default.h
@@ -489,6 +489,10 @@ public:
 	FUNC1RC(PackedInt32Array, lightmap_get_probe_capture_bsp_tree, RID)
 	FUNC1(lightmap_set_probe_capture_update_speed, float)
 
+	FUNC2(lightmap_set_shadowmask_textures, RID, RID)
+	FUNC1R(ShadowmaskMode, lightmap_get_shadowmask_mode, RID)
+	FUNC2(lightmap_set_shadowmask_mode, RID, ShadowmaskMode)
+
 	/* Shadow Atlas */
 	FUNC0R(RID, shadow_atlas_create)
 	FUNC3(shadow_atlas_set_size, RID, int, bool)
diff --git a/servers/rendering/storage/light_storage.h b/servers/rendering/storage/light_storage.h
index 1e149e3a97..7d6cfd9494 100644
--- a/servers/rendering/storage/light_storage.h
+++ b/servers/rendering/storage/light_storage.h
@@ -175,6 +175,10 @@ public:
 	virtual void lightmap_set_probe_capture_update_speed(float p_speed) = 0;
 	virtual float lightmap_get_probe_capture_update_speed() const = 0;
 
+	virtual void lightmap_set_shadowmask_textures(RID p_lightmap, RID p_shadow) = 0;
+	virtual RS::ShadowmaskMode lightmap_get_shadowmask_mode(RID p_lightmap) = 0;
+	virtual void lightmap_set_shadowmask_mode(RID p_lightmap, RS::ShadowmaskMode p_mode) = 0;
+
 	/* LIGHTMAP INSTANCE */
 
 	virtual RID lightmap_instance_create(RID p_lightmap) = 0;
diff --git a/servers/rendering_server.h b/servers/rendering_server.h
index 0917af73c6..49990d72cd 100644
--- a/servers/rendering_server.h
+++ b/servers/rendering_server.h
@@ -709,6 +709,13 @@ public:
 
 	/* LIGHTMAP */
 
+	enum ShadowmaskMode {
+		SHADOWMASK_MODE_NONE,
+		SHADOWMASK_MODE_REPLACE,
+		SHADOWMASK_MODE_OVERLAY,
+		SHADOWMASK_MODE_ONLY,
+	};
+
 	virtual RID lightmap_create() = 0;
 
 	virtual void lightmap_set_textures(RID p_lightmap, RID p_light, bool p_uses_spherical_haromics) = 0;
@@ -722,9 +729,12 @@ public:
 	virtual PackedInt32Array lightmap_get_probe_capture_bsp_tree(RID p_lightmap) const = 0;
 
 	virtual void lightmap_set_probe_capture_update_speed(float p_speed) = 0;
-
 	virtual void lightmaps_set_bicubic_filter(bool p_enable) = 0;
 
+	virtual void lightmap_set_shadowmask_textures(RID p_lightmap, RID p_shadow) = 0;
+	virtual ShadowmaskMode lightmap_get_shadowmask_mode(RID p_lightmap) = 0;
+	virtual void lightmap_set_shadowmask_mode(RID p_lightmap, ShadowmaskMode p_mode) = 0;
+
 	/* PARTICLES API */
 
 	virtual RID particles_create() = 0;

From 9c34ad17916292d3c2aaea838a5574158cb67c17 Mon Sep 17 00:00:00 2001
From: Lazy-Rabbit-2001 <2733679597@qq.com>
Date: Thu, 12 Dec 2024 18:13:31 +0800
Subject: [PATCH 090/124] Add popup_create_dialog() for EditorInterface to
 create custom create dialog

---
 doc/classes/EditorInterface.xml | 29 ++++++++++++++++++++
 editor/create_dialog.cpp        | 29 +++++++++++++++++---
 editor/create_dialog.h          |  6 ++++-
 editor/editor_interface.cpp     | 48 +++++++++++++++++++++++++++++++++
 editor/editor_interface.h       |  4 +++
 5 files changed, 111 insertions(+), 5 deletions(-)

diff --git a/doc/classes/EditorInterface.xml b/doc/classes/EditorInterface.xml
index 624e828520..d291a9e355 100644
--- a/doc/classes/EditorInterface.xml
+++ b/doc/classes/EditorInterface.xml
@@ -270,6 +270,35 @@
 				Plays the main scene.
 			</description>
 		</method>
+		<method name="popup_create_dialog">
+			<return type="void" />
+			<param index="0" name="callback" type="Callable" />
+			<param index="1" name="base_type" type="StringName" default="&quot;&quot;" />
+			<param index="2" name="current_type" type="String" default="&quot;&quot;" />
+			<param index="3" name="dialog_title" type="String" default="&quot;&quot;" />
+			<param index="4" name="type_blocklist" type="StringName[]" default="[]" />
+			<param index="5" name="type_suffixes" type="Dictionary" default="{}" />
+			<description>
+				Pops up an editor dialog for creating an object.
+				The [param callback] must take a single argument of type [StringName] which will contain the type name of the selected object or be empty if no item is selected.
+				The [param base_type] specifies the base type of objects to display. For example, if you set this to "Resource", all types derived from [Resource] will display in the create dialog.
+				The [param current_type] will be passed in the search box of the create dialog, and the specified type can be immediately selected when the dialog pops up. If the [param current_type] is not derived from [param base_type], there will be no result of the type in the dialog.
+				The [param dialog_title] allows you to define a custom title for the dialog. This is useful if you want to accurately hint the usage of the dialog. If the [param dialog_title] is an empty string, the dialog will use "Create New 'Base Type'" as the default title.
+				The [param type_blocklist] contains a list of type names, and the types in the blocklist will be hidden from the create dialog.
+				The [param type_suffixes] is a dictionary, with keys being [StringName]s and values being [String]s. Custom suffixes override the default suffixes which are file names of their scripts. For example, if you set a custom suffix as "Custom Suffix" for a global script type,
+				[codeblock lang=text]
+				Node
+				|- MyCustomNode (my_custom_node.gd)
+				[/codeblock]
+				will be
+				[codeblock lang=text]
+				Node
+				|- MyCustomNode (Custom Suffix)
+				[/codeblock]
+				Bear in mind that when a built-in type does not have any custom suffix, its suffix will be removed. The suffix of a type created from a script will fall back to its script file name. For global types by scripts, if you customize their suffixes to an empty string, their suffixes will be removed.
+				[b]Note:[/b] Trying to list the base type in the [param type_blocklist] will hide all types derived from the base type from the create dialog.
+			</description>
+		</method>
 		<method name="popup_dialog">
 			<return type="void" />
 			<param index="0" name="dialog" type="Window" />
diff --git a/editor/create_dialog.cpp b/editor/create_dialog.cpp
index c66adb63e8..9c2de726e8 100644
--- a/editor/create_dialog.cpp
+++ b/editor/create_dialog.cpp
@@ -145,6 +145,11 @@ bool CreateDialog::_should_hide_type(const StringName &p_type) const {
 				return true; // Parent type is blacklisted.
 			}
 		}
+		for (const StringName &F : custom_type_blocklist) {
+			if (ClassDB::is_parent_class(p_type, F)) {
+				return true; // Parent type is excluded in custom type blocklist.
+			}
+		}
 	} else {
 		if (!ScriptServer::is_global_class(p_type)) {
 			return true;
@@ -154,8 +159,12 @@ bool CreateDialog::_should_hide_type(const StringName &p_type) const {
 		}
 
 		StringName native_type = ScriptServer::get_global_class_native_base(p_type);
-		if (ClassDB::class_exists(native_type) && !ClassDB::can_instantiate(native_type)) {
-			return true;
+		if (ClassDB::class_exists(native_type)) {
+			if (!ClassDB::can_instantiate(native_type)) {
+				return true;
+			} else if (custom_type_blocklist.has(p_type) || custom_type_blocklist.has(native_type)) {
+				return true;
+			}
 		}
 
 		String script_path = ScriptServer::get_global_class_path(p_type);
@@ -283,15 +292,27 @@ void CreateDialog::_configure_search_option_item(TreeItem *r_item, const StringN
 	bool is_abstract = false;
 	if (p_type_category == TypeCategory::CPP_TYPE) {
 		r_item->set_text(0, p_type);
+		if (custom_type_suffixes.has(p_type)) {
+			String suffix = custom_type_suffixes.get(p_type);
+			if (!suffix.is_empty()) {
+				r_item->set_suffix(0, "(" + suffix + ")");
+			}
+		}
 	} else if (p_type_category == TypeCategory::PATH_TYPE) {
 		r_item->set_text(0, "\"" + p_type + "\"");
 	} else if (script_type) {
 		r_item->set_metadata(0, p_type);
 		r_item->set_text(0, p_type);
 		String script_path = ScriptServer::get_global_class_path(p_type);
-		r_item->set_suffix(0, "(" + script_path.get_file() + ")");
-
 		Ref<Script> scr = ResourceLoader::load(script_path, "Script");
+		String suffix = script_path.get_file();
+		if (scr.is_valid() && custom_type_suffixes.has(p_type)) {
+			suffix = custom_type_suffixes.get(p_type);
+		}
+		if (!suffix.is_empty()) {
+			r_item->set_suffix(0, "(" + suffix + ")");
+		}
+
 		ERR_FAIL_COND(!scr.is_valid());
 		is_abstract = scr->is_abstract();
 	} else {
diff --git a/editor/create_dialog.h b/editor/create_dialog.h
index e2b1b7b34b..7ee74bf05d 100644
--- a/editor/create_dialog.h
+++ b/editor/create_dialog.h
@@ -66,6 +66,8 @@ class CreateDialog : public ConfirmationDialog {
 	HashMap<String, int> custom_type_indices;
 	List<StringName> type_list;
 	HashSet<StringName> type_blacklist;
+	HashSet<StringName> custom_type_blocklist;
+	HashMap<StringName, String> custom_type_suffixes;
 
 	void _update_search();
 	bool _should_hide_type(const StringName &p_type) const;
@@ -115,8 +117,10 @@ public:
 	String get_base_type() const { return base_type; }
 	void select_base();
 
+	void set_type_blocklist(const HashSet<StringName> &p_blocklist) { custom_type_blocklist = p_blocklist; }
+	void set_type_suffixes(const HashMap<StringName, String> &p_suffixes) { custom_type_suffixes = p_suffixes; }
+
 	void set_preferred_search_result_type(const String &p_preferred_type) { preferred_search_result_type = p_preferred_type; }
-	String get_preferred_search_result_type() { return preferred_search_result_type; }
 
 	void popup_create(bool p_dont_clear, bool p_replace_mode = false, const String &p_current_type = "", const String &p_current_name = "");
 
diff --git a/editor/editor_interface.cpp b/editor/editor_interface.cpp
index 158d1bd4c4..37b4e1291b 100644
--- a/editor/editor_interface.cpp
+++ b/editor/editor_interface.cpp
@@ -32,6 +32,7 @@
 #include "editor_interface.compat.inc"
 
 #include "core/config/project_settings.h"
+#include "editor/create_dialog.h"
 #include "editor/editor_command_palette.h"
 #include "editor/editor_feature_profile.h"
 #include "editor/editor_main_screen.h"
@@ -512,6 +513,45 @@ void EditorInterface::popup_quick_open(const Callable &p_callback, const TypedAr
 	quick_open->popup_dialog(base_types, callable_mp(this, &EditorInterface::_quick_open).bind(p_callback));
 }
 
+void EditorInterface::popup_create_dialog(const Callable &p_callback, const StringName &p_base_type, const String &p_current_type, const String &p_dialog_title, const TypedArray<StringName> &p_custom_type_blocklist, const Dictionary &p_custom_suffix) {
+	if (!create_dialog) {
+		create_dialog = memnew(CreateDialog);
+		get_base_control()->add_child(create_dialog);
+	}
+
+	HashSet<StringName> blocklist;
+	for (const Variant &E : p_custom_type_blocklist) {
+		blocklist.insert(E);
+	}
+	create_dialog->set_type_blocklist(blocklist);
+
+	HashMap<StringName, String> suffix_map;
+	List<Variant> keys;
+	p_custom_suffix.get_key_list(&keys);
+	for (Variant &k : keys) {
+		const StringName key = k;
+		if (key.is_empty()) {
+			continue;
+		}
+		suffix_map.insert(key, p_custom_suffix[key]);
+	}
+	create_dialog->set_type_suffixes(suffix_map);
+
+	String safe_base_type = p_base_type;
+	if (p_base_type.is_empty() || (!ClassDB::class_exists(p_base_type) && !ScriptServer::is_global_class(p_base_type))) {
+		ERR_PRINT(vformat("Invalid base type '%s'. The base type has fallen back to 'Object'.", p_base_type));
+		safe_base_type = "Object";
+	}
+
+	create_dialog->set_base_type(safe_base_type);
+	create_dialog->popup_create(false, true, p_current_type, "");
+	create_dialog->set_title(p_dialog_title.is_empty() ? vformat(TTR("Create New %s"), p_base_type) : p_dialog_title);
+
+	const Callable callback = callable_mp(this, &EditorInterface::_create_dialog_item_selected);
+	create_dialog->connect(SNAME("create"), callback.bind(false, p_callback), CONNECT_DEFERRED);
+	create_dialog->connect(SNAME("canceled"), callback.bind(true, p_callback), CONNECT_DEFERRED);
+}
+
 void EditorInterface::_node_selected(const NodePath &p_node_path, const Callable &p_callback) {
 	const Callable callback = callable_mp(this, &EditorInterface::_node_selected);
 	node_selector->disconnect(SNAME("selected"), callback);
@@ -555,6 +595,13 @@ void EditorInterface::_quick_open(const String &p_file_path, const Callable &p_c
 	_call_dialog_callback(p_callback, p_file_path, "quick open");
 }
 
+void EditorInterface::_create_dialog_item_selected(bool p_is_canceled, const Callable &p_callback) {
+	const Callable callback = callable_mp(this, &EditorInterface::_create_dialog_item_selected);
+	create_dialog->disconnect(SNAME("create"), callback);
+	create_dialog->disconnect(SNAME("canceled"), callback);
+	_call_dialog_callback(p_callback, p_is_canceled ? "" : create_dialog->get_selected_type(), "create dialog");
+}
+
 void EditorInterface::_call_dialog_callback(const Callable &p_callback, const Variant &p_selected, const String &p_context) {
 	Callable::CallError ce;
 	Variant ret;
@@ -773,6 +820,7 @@ void EditorInterface::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("popup_property_selector", "object", "callback", "type_filter", "current_value"), &EditorInterface::popup_property_selector, DEFVAL(PackedInt32Array()), DEFVAL(String()));
 	ClassDB::bind_method(D_METHOD("popup_method_selector", "object", "callback", "current_value"), &EditorInterface::popup_method_selector, DEFVAL(String()));
 	ClassDB::bind_method(D_METHOD("popup_quick_open", "callback", "base_types"), &EditorInterface::popup_quick_open, DEFVAL(TypedArray<StringName>()));
+	ClassDB::bind_method(D_METHOD("popup_create_dialog", "callback", "base_type", "current_type", "dialog_title", "type_blocklist", "type_suffixes"), &EditorInterface::popup_create_dialog, DEFVAL(""), DEFVAL(""), DEFVAL(""), DEFVAL(TypedArray<StringName>()), DEFVAL(Dictionary()));
 
 	// Editor docks.
 
diff --git a/editor/editor_interface.h b/editor/editor_interface.h
index 92d9812003..ca5306fd6d 100644
--- a/editor/editor_interface.h
+++ b/editor/editor_interface.h
@@ -37,6 +37,7 @@
 #include "core/object/script_language.h"
 
 class Control;
+class CreateDialog;
 class EditorCommandPalette;
 class EditorFileSystem;
 class EditorInspector;
@@ -69,11 +70,13 @@ class EditorInterface : public Object {
 	PropertySelector *property_selector = nullptr;
 	PropertySelector *method_selector = nullptr;
 	SceneTreeDialog *node_selector = nullptr;
+	CreateDialog *create_dialog = nullptr;
 
 	void _node_selected(const NodePath &p_node_paths, const Callable &p_callback);
 	void _property_selected(const String &p_property_name, const Callable &p_callback);
 	void _method_selected(const String &p_property_name, const Callable &p_callback);
 	void _quick_open(const String &p_file_path, const Callable &p_callback);
+	void _create_dialog_item_selected(bool p_is_canceled, const Callable &p_callback);
 	void _call_dialog_callback(const Callable &p_callback, const Variant &p_selected, const String &p_context);
 
 	// Editor tools.
@@ -145,6 +148,7 @@ public:
 	void popup_property_selector(Object *p_object, const Callable &p_callback, const PackedInt32Array &p_type_filter = PackedInt32Array(), const String &p_current_value = String());
 	void popup_method_selector(Object *p_object, const Callable &p_callback, const String &p_current_value = String());
 	void popup_quick_open(const Callable &p_callback, const TypedArray<StringName> &p_base_types = TypedArray<StringName>());
+	void popup_create_dialog(const Callable &p_callback, const StringName &p_base_type = "", const String &p_current_type = "", const String &p_dialog_title = "", const TypedArray<StringName> &p_custom_type_blocklist = TypedArray<String>(), const Dictionary &p_custom_suffix = Dictionary());
 
 	// Editor docks.
 

From 0400cbce1e68902ebda4f5775017d008e5aaa480 Mon Sep 17 00:00:00 2001
From: BlueCube3310 <53150244+BlueCube3310@users.noreply.github.com>
Date: Wed, 9 Oct 2024 12:30:24 +0200
Subject: [PATCH 091/124] Clean up ResourceImporterTexture

---
 editor/import/resource_importer_texture.cpp | 415 +++++++++++---------
 editor/import/resource_importer_texture.h   |   4 +
 2 files changed, 223 insertions(+), 196 deletions(-)

diff --git a/editor/import/resource_importer_texture.cpp b/editor/import/resource_importer_texture.cpp
index d72c15bc2a..4dd0173cbf 100644
--- a/editor/import/resource_importer_texture.cpp
+++ b/editor/import/resource_importer_texture.cpp
@@ -46,7 +46,6 @@ void ResourceImporterTexture::_texture_reimport_roughness(const Ref<CompressedTe
 	ERR_FAIL_COND(p_tex.is_null());
 
 	MutexLock lock(singleton->mutex);
-
 	StringName path = p_tex->get_path();
 
 	if (!singleton->make_flags.has(path)) {
@@ -62,7 +61,6 @@ void ResourceImporterTexture::_texture_reimport_3d(const Ref<CompressedTexture2D
 	ERR_FAIL_COND(p_tex.is_null());
 
 	MutexLock lock(singleton->mutex);
-
 	StringName path = p_tex->get_path();
 
 	if (!singleton->make_flags.has(path)) {
@@ -76,7 +74,6 @@ void ResourceImporterTexture::_texture_reimport_normal(const Ref<CompressedTextu
 	ERR_FAIL_COND(p_tex.is_null());
 
 	MutexLock lock(singleton->mutex);
-
 	StringName path = p_tex->get_path();
 
 	if (!singleton->make_flags.has(path)) {
@@ -86,79 +83,87 @@ void ResourceImporterTexture::_texture_reimport_normal(const Ref<CompressedTextu
 	singleton->make_flags[path].flags |= MAKE_NORMAL_FLAG;
 }
 
+inline void ResourceImporterTexture::_print_callback_message(const String &p_message) {
+#ifdef TOOLS_ENABLED
+	EditorToaster::get_singleton()->popup_str(p_message);
+#endif
+	print_line(p_message);
+}
+
 void ResourceImporterTexture::update_imports() {
 	if (EditorFileSystem::get_singleton()->is_scanning() || EditorFileSystem::get_singleton()->is_importing()) {
-		return; // do nothing for now
+		return; // Don't update when EditorFileSystem is doing something else.
 	}
 
 	MutexLock lock(mutex);
 	Vector<String> to_reimport;
-	{
-		if (make_flags.is_empty()) {
-			return;
-		}
 
-		for (const KeyValue<StringName, MakeInfo> &E : make_flags) {
-			Ref<ConfigFile> cf;
-			cf.instantiate();
-			String src_path = String(E.key) + ".import";
-
-			Error err = cf->load(src_path);
-			ERR_CONTINUE(err != OK);
-
-			bool changed = false;
-
-			if (E.value.flags & MAKE_NORMAL_FLAG && int(cf->get_value("params", "compress/normal_map")) == 0) {
-				String message = vformat(TTR("%s: Texture detected as used as a normal map in 3D. Enabling red-green texture compression to reduce memory usage (blue channel is discarded)."), String(E.key));
-#ifdef TOOLS_ENABLED
-				EditorToaster::get_singleton()->popup_str(message);
-#endif
-				print_line(message);
-				cf->set_value("params", "compress/normal_map", 1);
-				changed = true;
-			}
-
-			if (E.value.flags & MAKE_ROUGHNESS_FLAG && int(cf->get_value("params", "roughness/mode")) == 0) {
-				String message = vformat(TTR("%s: Texture detected as used as a roughness map in 3D. Enabling roughness limiter based on the detected associated normal map at %s."), String(E.key), E.value.normal_path_for_roughness);
-#ifdef TOOLS_ENABLED
-				EditorToaster::get_singleton()->popup_str(message);
-#endif
-				print_line(message);
-				cf->set_value("params", "roughness/mode", E.value.channel_for_roughness + 2);
-				cf->set_value("params", "roughness/src_normal", E.value.normal_path_for_roughness);
-				changed = true;
-			}
-
-			if (E.value.flags & MAKE_3D_FLAG && bool(cf->get_value("params", "detect_3d/compress_to"))) {
-				const int compress_to = cf->get_value("params", "detect_3d/compress_to");
-				String compress_string;
-				cf->set_value("params", "detect_3d/compress_to", 0);
-				if (compress_to == 1) {
-					cf->set_value("params", "compress/mode", COMPRESS_VRAM_COMPRESSED);
-					compress_string = "VRAM Compressed (S3TC/ETC/BPTC)";
-				} else if (compress_to == 2) {
-					cf->set_value("params", "compress/mode", COMPRESS_BASIS_UNIVERSAL);
-					compress_string = "Basis Universal";
-				}
-				String message = vformat(TTR("%s: Texture detected as used in 3D. Enabling mipmap generation and setting the texture compression mode to %s."), String(E.key), compress_string);
-#ifdef TOOLS_ENABLED
-				EditorToaster::get_singleton()->popup_str(message);
-#endif
-				print_line(message);
-				cf->set_value("params", "mipmaps/generate", true);
-				changed = true;
-			}
-
-			if (changed) {
-				cf->save(src_path);
-				to_reimport.push_back(E.key);
-			}
-		}
-
-		make_flags.clear();
+	if (make_flags.is_empty()) {
+		return;
 	}
 
-	if (to_reimport.size()) {
+	for (const KeyValue<StringName, MakeInfo> &E : make_flags) {
+		Ref<ConfigFile> cf;
+		cf.instantiate();
+		String src_path = String(E.key) + ".import";
+
+		Error err = cf->load(src_path);
+		ERR_CONTINUE(err != OK);
+
+		bool changed = false;
+
+		if (E.value.flags & MAKE_NORMAL_FLAG && int(cf->get_value("params", "compress/normal_map")) == 0) {
+			_print_callback_message(
+					vformat(TTR("%s: Texture detected as used as a normal map in 3D. Enabling red-green texture compression to reduce memory usage (blue channel is discarded)."),
+							String(E.key)));
+
+			cf->set_value("params", "compress/normal_map", 1);
+			changed = true;
+		}
+
+		if (E.value.flags & MAKE_ROUGHNESS_FLAG && int(cf->get_value("params", "roughness/mode")) == 0) {
+			_print_callback_message(
+					vformat(TTR("%s: Texture detected as used as a roughness map in 3D. Enabling roughness limiter based on the detected associated normal map at %s."),
+							String(E.key), E.value.normal_path_for_roughness));
+
+			cf->set_value("params", "roughness/mode", E.value.channel_for_roughness + 2);
+			cf->set_value("params", "roughness/src_normal", E.value.normal_path_for_roughness);
+			changed = true;
+		}
+
+		if (E.value.flags & MAKE_3D_FLAG && bool(cf->get_value("params", "detect_3d/compress_to"))) {
+			const int compress_to = cf->get_value("params", "detect_3d/compress_to");
+
+			// 3D detected, disable the callback.
+			cf->set_value("params", "detect_3d/compress_to", 0);
+
+			String compress_string;
+			if (compress_to == 1) {
+				cf->set_value("params", "compress/mode", COMPRESS_VRAM_COMPRESSED);
+				compress_string = "VRAM Compressed (S3TC/ETC/BPTC)";
+
+			} else if (compress_to == 2) {
+				cf->set_value("params", "compress/mode", COMPRESS_BASIS_UNIVERSAL);
+				compress_string = "Basis Universal";
+			}
+
+			_print_callback_message(
+					vformat(TTR("%s: Texture detected as used in 3D. Enabling mipmap generation and setting the texture compression mode to %s."),
+							String(E.key), compress_string));
+
+			cf->set_value("params", "mipmaps/generate", true);
+			changed = true;
+		}
+
+		if (changed) {
+			cf->save(src_path);
+			to_reimport.push_back(E.key);
+		}
+	}
+
+	make_flags.clear();
+
+	if (!to_reimport.is_empty()) {
 		EditorFileSystem::get_singleton()->reimport_files(to_reimport);
 	}
 }
@@ -189,21 +194,25 @@ bool ResourceImporterTexture::get_option_visibility(const String &p_path, const
 		if (compress_mode != COMPRESS_VRAM_COMPRESSED) {
 			return false;
 		}
+
 	} else if (p_option == "compress/lossy_quality") {
 		int compress_mode = int(p_options["compress/mode"]);
 		if (compress_mode != COMPRESS_LOSSY) {
 			return false;
 		}
+
 	} else if (p_option == "compress/hdr_mode") {
 		int compress_mode = int(p_options["compress/mode"]);
 		if (compress_mode < COMPRESS_VRAM_COMPRESSED) {
 			return false;
 		}
+
 	} else if (p_option == "compress/normal_map") {
 		int compress_mode = int(p_options["compress/mode"]);
 		if (compress_mode == COMPRESS_LOSSLESS) {
 			return false;
 		}
+
 	} else if (p_option == "mipmaps/limit") {
 		return p_options["mipmaps/generate"];
 	}
@@ -260,9 +269,9 @@ void ResourceImporterTexture::get_import_options(const String &p_path, List<Impo
 void ResourceImporterTexture::save_to_ctex_format(Ref<FileAccess> f, const Ref<Image> &p_image, CompressMode p_compress_mode, Image::UsedChannels p_channels, Image::CompressMode p_compress_format, float p_lossy_quality) {
 	switch (p_compress_mode) {
 		case COMPRESS_LOSSLESS: {
-			bool lossless_force_png = GLOBAL_GET("rendering/textures/lossless_compression/force_png") ||
-					!Image::_webp_mem_loader_func; // WebP module disabled.
-			bool use_webp = !lossless_force_png && p_image->get_width() <= 16383 && p_image->get_height() <= 16383; // WebP has a size limit
+			bool lossless_force_png = GLOBAL_GET("rendering/textures/lossless_compression/force_png") || !Image::_webp_mem_loader_func; // WebP module disabled or png is forced.
+			bool use_webp = !lossless_force_png && p_image->get_width() <= 16383 && p_image->get_height() <= 16383; // WebP has a size limit.
+
 			f->store_32(use_webp ? CompressedTexture2D::DATA_FORMAT_WEBP : CompressedTexture2D::DATA_FORMAT_PNG);
 			f->store_16(p_image->get_width());
 			f->store_16(p_image->get_height());
@@ -276,11 +285,11 @@ void ResourceImporterTexture::save_to_ctex_format(Ref<FileAccess> f, const Ref<I
 				} else {
 					data = Image::png_packer(i ? p_image->get_image_from_mipmap(i) : p_image);
 				}
-				int data_len = data.size();
-				f->store_32(data_len);
 
-				const uint8_t *r = data.ptr();
-				f->store_buffer(r, data_len);
+				const uint64_t data_size = data.size();
+
+				f->store_32(data_size);
+				f->store_buffer(data.ptr(), data_size);
 			}
 
 		} break;
@@ -293,16 +302,15 @@ void ResourceImporterTexture::save_to_ctex_format(Ref<FileAccess> f, const Ref<I
 
 			for (int i = 0; i < p_image->get_mipmap_count() + 1; i++) {
 				Vector<uint8_t> data = Image::webp_lossy_packer(i ? p_image->get_image_from_mipmap(i) : p_image, p_lossy_quality);
-				int data_len = data.size();
-				f->store_32(data_len);
+				const uint64_t data_size = data.size();
 
-				const uint8_t *r = data.ptr();
-				f->store_buffer(r, data_len);
+				f->store_32(data_size);
+				f->store_buffer(data.ptr(), data_size);
 			}
+
 		} break;
 		case COMPRESS_VRAM_COMPRESSED: {
 			Ref<Image> image = p_image->duplicate();
-
 			image->compress_from_channels(p_compress_format, p_channels);
 
 			f->store_32(CompressedTexture2D::DATA_FORMAT_IMAGE);
@@ -310,11 +318,8 @@ void ResourceImporterTexture::save_to_ctex_format(Ref<FileAccess> f, const Ref<I
 			f->store_16(image->get_height());
 			f->store_32(image->get_mipmap_count());
 			f->store_32(image->get_format());
+			f->store_buffer(image->get_data());
 
-			Vector<uint8_t> data = image->get_data();
-			int dl = data.size();
-			const uint8_t *r = data.ptr();
-			f->store_buffer(r, dl);
 		} break;
 		case COMPRESS_VRAM_UNCOMPRESSED: {
 			f->store_32(CompressedTexture2D::DATA_FORMAT_IMAGE);
@@ -322,12 +327,7 @@ void ResourceImporterTexture::save_to_ctex_format(Ref<FileAccess> f, const Ref<I
 			f->store_16(p_image->get_height());
 			f->store_32(p_image->get_mipmap_count());
 			f->store_32(p_image->get_format());
-
-			Vector<uint8_t> data = p_image->get_data();
-			int dl = data.size();
-			const uint8_t *r = data.ptr();
-
-			f->store_buffer(r, dl);
+			f->store_buffer(p_image->get_data());
 
 		} break;
 		case COMPRESS_BASIS_UNIVERSAL: {
@@ -336,11 +336,12 @@ void ResourceImporterTexture::save_to_ctex_format(Ref<FileAccess> f, const Ref<I
 			f->store_16(p_image->get_height());
 			f->store_32(p_image->get_mipmap_count());
 			f->store_32(p_image->get_format());
+
 			Vector<uint8_t> data = Image::basis_universal_packer(p_image, p_channels);
-			int data_len = data.size();
-			f->store_32(data_len);
-			const uint8_t *r = data.ptr();
-			f->store_buffer(r, data_len);
+			const uint64_t data_size = data.size();
+
+			f->store_32(data_size);
+			f->store_buffer(data.ptr(), data_size);
 		} break;
 	}
 }
@@ -348,14 +349,17 @@ void ResourceImporterTexture::save_to_ctex_format(Ref<FileAccess> f, const Ref<I
 void ResourceImporterTexture::_save_ctex(const Ref<Image> &p_image, const String &p_to_path, CompressMode p_compress_mode, float p_lossy_quality, Image::CompressMode p_vram_compression, bool p_mipmaps, bool p_streamable, bool p_detect_3d, bool p_detect_roughness, bool p_detect_normal, bool p_force_normal, bool p_srgb_friendly, bool p_force_po2_for_compressed, uint32_t p_limit_mipmap, const Ref<Image> &p_normal, Image::RoughnessChannel p_roughness_channel) {
 	Ref<FileAccess> f = FileAccess::open(p_to_path, FileAccess::WRITE);
 	ERR_FAIL_COND(f.is_null());
+
+	// Godot Streamable Texture 2D.
 	f->store_8('G');
 	f->store_8('S');
 	f->store_8('T');
-	f->store_8('2'); //godot streamable texture 2D
+	f->store_8('2');
 
-	//format version
+	// Current format version.
 	f->store_32(CompressedTexture2D::FORMAT_VERSION);
-	//texture may be resized later, so original size must be saved first
+
+	// Texture may be resized later, so original size must be saved first.
 	f->store_32(p_image->get_width());
 	f->store_32(p_image->get_height());
 
@@ -364,7 +368,7 @@ void ResourceImporterTexture::_save_ctex(const Ref<Image> &p_image, const String
 		flags |= CompressedTexture2D::FORMAT_BIT_STREAM;
 	}
 	if (p_mipmaps) {
-		flags |= CompressedTexture2D::FORMAT_BIT_HAS_MIPMAPS; //mipmaps bit
+		flags |= CompressedTexture2D::FORMAT_BIT_HAS_MIPMAPS;
 	}
 	if (p_detect_3d) {
 		flags |= CompressedTexture2D::FORMAT_BIT_DETECT_3D;
@@ -378,7 +382,8 @@ void ResourceImporterTexture::_save_ctex(const Ref<Image> &p_image, const String
 
 	f->store_32(flags);
 	f->store_32(p_limit_mipmap);
-	//reserved for future use
+
+	// Reserved.
 	f->store_32(0);
 	f->store_32(0);
 	f->store_32(0);
@@ -389,30 +394,37 @@ void ResourceImporterTexture::_save_ctex(const Ref<Image> &p_image, const String
 
 	Ref<Image> image = p_image->duplicate();
 
-	if (p_force_po2_for_compressed && p_mipmaps && ((p_compress_mode == COMPRESS_BASIS_UNIVERSAL) || (p_compress_mode == COMPRESS_VRAM_COMPRESSED))) {
-		image->resize_to_po2();
-	}
+	if (p_mipmaps) {
+		if (p_force_po2_for_compressed && (p_compress_mode == COMPRESS_BASIS_UNIVERSAL || p_compress_mode == COMPRESS_VRAM_COMPRESSED)) {
+			image->resize_to_po2();
+		}
 
-	if (p_mipmaps && (!image->has_mipmaps() || p_force_normal)) {
-		image->generate_mipmaps(p_force_normal);
-	}
+		if (!image->has_mipmaps() || p_force_normal) {
+			image->generate_mipmaps(p_force_normal);
+		}
 
-	if (!p_mipmaps) {
+	} else {
 		image->clear_mipmaps();
 	}
 
+	// Generate roughness mipmaps from normal texture.
 	if (image->has_mipmaps() && p_normal.is_valid()) {
 		image->generate_mipmap_roughness(p_roughness_channel, p_normal);
 	}
 
-	Image::CompressSource csource = Image::COMPRESS_SOURCE_GENERIC;
-	if (p_force_normal) {
-		csource = Image::COMPRESS_SOURCE_NORMAL;
-	} else if (p_srgb_friendly) {
-		csource = Image::COMPRESS_SOURCE_SRGB;
-	}
+	// Optimization: Only check for color channels when compressing as BasisU or VRAM.
+	Image::UsedChannels used_channels = Image::USED_CHANNELS_RGBA;
 
-	Image::UsedChannels used_channels = image->detect_used_channels(csource);
+	if (p_compress_mode == COMPRESS_BASIS_UNIVERSAL || p_compress_mode == COMPRESS_VRAM_COMPRESSED) {
+		Image::CompressSource comp_source = Image::COMPRESS_SOURCE_GENERIC;
+		if (p_force_normal) {
+			comp_source = Image::COMPRESS_SOURCE_NORMAL;
+		} else if (p_srgb_friendly) {
+			comp_source = Image::COMPRESS_SOURCE_SRGB;
+		}
+
+		used_channels = image->detect_used_channels(comp_source);
+	}
 
 	save_to_ctex_format(f, image, p_compress_mode, used_channels, p_vram_compression, p_lossy_quality);
 }
@@ -431,6 +443,50 @@ Dictionary ResourceImporterTexture::_load_editor_meta(const String &p_path) cons
 	return f->get_var();
 }
 
+void ResourceImporterTexture::_invert_y_channel(Ref<Image> &r_image) {
+	// Inverting the green channel can be used to flip a normal map's direction.
+	// There's no standard when it comes to normal map Y direction, so this is
+	// sometimes needed when using a normal map exported from another program.
+	// See <http://wiki.polycount.com/wiki/Normal_Map_Technical_Details#Common_Swizzle_Coordinates>.
+	const int height = r_image->get_height();
+	const int width = r_image->get_width();
+
+	for (int i = 0; i < width; i++) {
+		for (int j = 0; j < height; j++) {
+			const Color color = r_image->get_pixel(i, j);
+			r_image->set_pixel(i, j, Color(color.r, 1 - color.g, color.b, color.a));
+		}
+	}
+}
+
+void ResourceImporterTexture::_clamp_hdr_exposure(Ref<Image> &r_image) {
+	// Clamp HDR exposure following Filament's tonemapping formula.
+	// This can be used to reduce fireflies in environment maps or reduce the influence
+	// of the sun from an HDRI panorama on environment lighting (when a DirectionalLight3D is used instead).
+	const int height = r_image->get_height();
+	const int width = r_image->get_width();
+
+	// These values are chosen arbitrarily and seem to produce good results with 4,096 samples.
+	const float linear = 4096.0;
+	const float compressed = 16384.0;
+
+	for (int i = 0; i < width; i++) {
+		for (int j = 0; j < height; j++) {
+			const Color color = r_image->get_pixel(i, j);
+			const float luma = color.get_luminance();
+
+			Color clamped_color;
+			if (luma <= linear) {
+				clamped_color = color;
+			} else {
+				clamped_color = (color / luma) * ((linear * linear - compressed * luma) / (2 * linear - compressed - luma));
+			}
+
+			r_image->set_pixel(i, j, clamped_color);
+		}
+	}
+}
+
 Error ResourceImporterTexture::import(ResourceUID::ID p_source_id, const String &p_source_file, const String &p_save_path, const HashMap<StringName, Variant> &p_options, List<String> *r_platform_variants, List<String> *r_gen_files, Variant *r_metadata) {
 	// Parse import options.
 	int32_t loader_flags = ImageFormatLoader::FLAG_NONE;
@@ -455,10 +511,11 @@ Error ResourceImporterTexture::import(ResourceUID::ID p_source_id, const String
 	const bool fix_alpha_border = p_options["process/fix_alpha_border"];
 	const bool premult_alpha = p_options["process/premult_alpha"];
 	const bool normal_map_invert_y = p_options["process/normal_map_invert_y"];
-	// Support for texture streaming is not implemented yet.
-	const bool stream = false;
 
+	const bool hdr_as_srgb = p_options["process/hdr_as_srgb"];
+	const bool hdr_clamp_exposure = p_options["process/hdr_clamp_exposure"];
 	int size_limit = p_options["process/size_limit"];
+
 	bool using_fallback_size_limit = false;
 	if (size_limit == 0) {
 		using_fallback_size_limit = true;
@@ -479,28 +536,27 @@ Error ResourceImporterTexture::import(ResourceUID::ID p_source_id, const String
 		}
 	}
 
-	const bool hdr_as_srgb = p_options["process/hdr_as_srgb"];
-	if (hdr_as_srgb) {
-		loader_flags |= ImageFormatLoader::FLAG_FORCE_LINEAR;
-	}
-	const bool hdr_clamp_exposure = p_options["process/hdr_clamp_exposure"];
+	// Support for texture streaming is not implemented yet.
+	const bool stream = false;
 
-	float scale = 1.0;
 	// SVG-specific options.
-	if (p_options.has("svg/scale")) {
-		scale = p_options["svg/scale"];
-	}
+	float scale = p_options.has("svg/scale") ? float(p_options["svg/scale"]) : 1.0f;
 
 	// Editor-specific options.
 	bool use_editor_scale = p_options.has("editor/scale_with_editor_scale") && p_options["editor/scale_with_editor_scale"];
 	bool convert_editor_colors = p_options.has("editor/convert_colors_with_editor_theme") && p_options["editor/convert_colors_with_editor_theme"];
 
+	if (hdr_as_srgb) {
+		loader_flags |= ImageFormatLoader::FLAG_FORCE_LINEAR;
+	}
+
 	// Start importing images.
-	List<Ref<Image>> images_imported;
+	LocalVector<Ref<Image>> images_imported;
 
 	// Load the normal image.
 	Ref<Image> normal_image;
 	Image::RoughnessChannel roughness_channel = Image::ROUGHNESS_CHANNEL_R;
+
 	if (mipmaps && roughness > 1 && FileAccess::exists(normal_map)) {
 		normal_image.instantiate();
 		if (ImageLoader::load_image(normal_map, normal_image) == OK) {
@@ -520,11 +576,9 @@ Error ResourceImporterTexture::import(ResourceUID::ID p_source_id, const String
 	// Load the editor-only image.
 	Ref<Image> editor_image;
 	bool import_editor_image = use_editor_scale || convert_editor_colors;
+
 	if (import_editor_image) {
-		float editor_scale = scale;
-		if (use_editor_scale) {
-			editor_scale = scale * EDSCALE;
-		}
+		float editor_scale = use_editor_scale ? scale * EDSCALE : scale;
 
 		int32_t editor_loader_flags = loader_flags;
 		if (convert_editor_colors) {
@@ -533,8 +587,9 @@ Error ResourceImporterTexture::import(ResourceUID::ID p_source_id, const String
 
 		editor_image.instantiate();
 		err = ImageLoader::load_image(p_source_file, editor_image, nullptr, editor_loader_flags, editor_scale);
+
 		if (err != OK) {
-			WARN_PRINT("Failed to import an image resource for editor use from '" + p_source_file + "'");
+			WARN_PRINT(vformat("Failed to import an image resource for editor use from '%s'.", p_source_file));
 		} else {
 			images_imported.push_back(editor_image);
 		}
@@ -580,48 +635,12 @@ Error ResourceImporterTexture::import(ResourceUID::ID p_source_id, const String
 
 		// Invert the green channel of the image to flip the normal map it contains.
 		if (normal_map_invert_y) {
-			// Inverting the green channel can be used to flip a normal map's direction.
-			// There's no standard when it comes to normal map Y direction, so this is
-			// sometimes needed when using a normal map exported from another program.
-			// See <http://wiki.polycount.com/wiki/Normal_Map_Technical_Details#Common_Swizzle_Coordinates>.
-			const int height = target_image->get_height();
-			const int width = target_image->get_width();
-
-			for (int i = 0; i < width; i++) {
-				for (int j = 0; j < height; j++) {
-					const Color color = target_image->get_pixel(i, j);
-					target_image->set_pixel(i, j, Color(color.r, 1 - color.g, color.b, color.a));
-				}
-			}
+			_invert_y_channel(target_image);
 		}
 
 		// Clamp HDR exposure.
 		if (hdr_clamp_exposure) {
-			// Clamp HDR exposure following Filament's tonemapping formula.
-			// This can be used to reduce fireflies in environment maps or reduce the influence
-			// of the sun from an HDRI panorama on environment lighting (when a DirectionalLight3D is used instead).
-			const int height = target_image->get_height();
-			const int width = target_image->get_width();
-
-			// These values are chosen arbitrarily and seem to produce good results with 4,096 samples.
-			const float linear = 4096.0;
-			const float compressed = 16384.0;
-
-			for (int i = 0; i < width; i++) {
-				for (int j = 0; j < height; j++) {
-					const Color color = target_image->get_pixel(i, j);
-					const float luma = color.get_luminance();
-
-					Color clamped_color;
-					if (luma <= linear) {
-						clamped_color = color;
-					} else {
-						clamped_color = (color / luma) * ((linear * linear - compressed * luma) / (2 * linear - compressed - luma));
-					}
-
-					target_image->set_pixel(i, j, clamped_color);
-				}
-			}
+			_clamp_hdr_exposure(target_image);
 		}
 	}
 
@@ -634,14 +653,12 @@ Error ResourceImporterTexture::import(ResourceUID::ID p_source_id, const String
 	Array formats_imported;
 
 	if (compress_mode == COMPRESS_VRAM_COMPRESSED) {
-		// Must import in all formats, in order of priority (so platform choses the best supported one. IE, etc2 over etc).
-		// Android, GLES 2.x
-
+		// Must import in desktop and mobile formats in order of priority, so platform chooses the best supported one (e.g. s3tc over etc2 on desktop).
 		const bool is_hdr = (image->get_format() >= Image::FORMAT_RF && image->get_format() <= Image::FORMAT_RGBE9995);
 		const bool can_s3tc_bptc = ResourceImporterTextureSettings::should_import_s3tc_bptc();
 		const bool can_etc2_astc = ResourceImporterTextureSettings::should_import_etc2_astc();
 
-		// Add list of formats imported
+		// Add list of formats imported.
 		if (can_s3tc_bptc) {
 			formats_imported.push_back("s3tc_bptc");
 		}
@@ -651,7 +668,7 @@ Error ResourceImporterTexture::import(ResourceUID::ID p_source_id, const String
 
 		bool can_compress_hdr = hdr_compression > 0;
 		bool has_alpha = image->detect_alpha() != Image::ALPHA_NONE;
-		bool use_uncompressed = false;
+		bool force_uncompressed = false;
 
 		if (is_hdr) {
 			if (has_alpha) {
@@ -668,17 +685,16 @@ Error ResourceImporterTexture::import(ResourceUID::ID p_source_id, const String
 				}
 			}
 
-			if (!can_compress_hdr) {
-				// Fallback to RGBE99995.
-				if (image->get_format() != Image::FORMAT_RGBE9995) {
-					image->convert(Image::FORMAT_RGBE9995);
-					use_uncompressed = true;
-				}
+			// Fall back to RGBE99995.
+			if (!can_compress_hdr && image->get_format() != Image::FORMAT_RGBE9995) {
+				image->convert(Image::FORMAT_RGBE9995);
+				force_uncompressed = true;
 			}
 		}
 
-		if (use_uncompressed) {
-			_save_ctex(image, p_save_path + ".ctex", COMPRESS_VRAM_UNCOMPRESSED, lossy, Image::COMPRESS_S3TC /*this is ignored */, mipmaps, stream, detect_3d, detect_roughness, detect_normal, force_normal, srgb_friendly_pack, false, mipmap_limit, normal_image, roughness_channel);
+		if (force_uncompressed) {
+			_save_ctex(image, p_save_path + ".ctex", COMPRESS_VRAM_UNCOMPRESSED, lossy, Image::COMPRESS_S3TC /* This is ignored. */,
+					mipmaps, stream, detect_3d, detect_roughness, detect_normal, force_normal, srgb_friendly_pack, false, mipmap_limit, normal_image, roughness_channel);
 		} else {
 			if (can_s3tc_bptc) {
 				Image::CompressMode image_compress_mode;
@@ -690,7 +706,9 @@ Error ResourceImporterTexture::import(ResourceUID::ID p_source_id, const String
 					image_compress_mode = Image::COMPRESS_S3TC;
 					image_compress_format = "s3tc";
 				}
-				_save_ctex(image, p_save_path + "." + image_compress_format + ".ctex", compress_mode, lossy, image_compress_mode, mipmaps, stream, detect_3d, detect_roughness, detect_normal, force_normal, srgb_friendly_pack, false, mipmap_limit, normal_image, roughness_channel);
+
+				_save_ctex(image, p_save_path + "." + image_compress_format + ".ctex", compress_mode, lossy, image_compress_mode, mipmaps, stream, detect_3d,
+						detect_roughness, detect_normal, force_normal, srgb_friendly_pack, false, mipmap_limit, normal_image, roughness_channel);
 				r_platform_variants->push_back(image_compress_format);
 			}
 
@@ -704,17 +722,21 @@ Error ResourceImporterTexture::import(ResourceUID::ID p_source_id, const String
 					image_compress_mode = Image::COMPRESS_ETC2;
 					image_compress_format = "etc2";
 				}
-				_save_ctex(image, p_save_path + "." + image_compress_format + ".ctex", compress_mode, lossy, image_compress_mode, mipmaps, stream, detect_3d, detect_roughness, detect_normal, force_normal, srgb_friendly_pack, false, mipmap_limit, normal_image, roughness_channel);
+
+				_save_ctex(image, p_save_path + "." + image_compress_format + ".ctex", compress_mode, lossy, image_compress_mode, mipmaps, stream, detect_3d,
+						detect_roughness, detect_normal, force_normal, srgb_friendly_pack, false, mipmap_limit, normal_image, roughness_channel);
 				r_platform_variants->push_back(image_compress_format);
 			}
 		}
 	} else {
 		// Import normally.
-		_save_ctex(image, p_save_path + ".ctex", compress_mode, lossy, Image::COMPRESS_S3TC /*this is ignored */, mipmaps, stream, detect_3d, detect_roughness, detect_normal, force_normal, srgb_friendly_pack, false, mipmap_limit, normal_image, roughness_channel);
+		_save_ctex(image, p_save_path + ".ctex", compress_mode, lossy, Image::COMPRESS_S3TC /* This is ignored. */,
+				mipmaps, stream, detect_3d, detect_roughness, detect_normal, force_normal, srgb_friendly_pack, false, mipmap_limit, normal_image, roughness_channel);
 	}
 
 	if (editor_image.is_valid()) {
-		_save_ctex(editor_image, p_save_path + ".editor.ctex", compress_mode, lossy, Image::COMPRESS_S3TC /*this is ignored */, mipmaps, stream, detect_3d, detect_roughness, detect_normal, force_normal, srgb_friendly_pack, false, mipmap_limit, normal_image, roughness_channel);
+		_save_ctex(editor_image, p_save_path + ".editor.ctex", compress_mode, lossy, Image::COMPRESS_S3TC /* This is ignored. */,
+				mipmaps, stream, detect_3d, detect_roughness, detect_normal, force_normal, srgb_friendly_pack, false, mipmap_limit, normal_image, roughness_channel);
 
 		// Generate and save editor-specific metadata, which we cannot save to the .import file.
 		Dictionary editor_meta;
@@ -722,6 +744,7 @@ Error ResourceImporterTexture::import(ResourceUID::ID p_source_id, const String
 		if (use_editor_scale) {
 			editor_meta["editor_scale"] = EDSCALE;
 		}
+
 		if (convert_editor_colors) {
 			editor_meta["editor_dark_theme"] = EditorThemeManager::is_dark_theme();
 		}
@@ -732,6 +755,7 @@ Error ResourceImporterTexture::import(ResourceUID::ID p_source_id, const String
 	if (r_metadata) {
 		Dictionary meta;
 		meta["vram_texture"] = compress_mode == COMPRESS_VRAM_COMPRESSED;
+
 		if (formats_imported.size()) {
 			meta["imported_formats"] = formats_imported;
 		}
@@ -751,16 +775,17 @@ const char *ResourceImporterTexture::compression_formats[] = {
 	"etc2_astc",
 	nullptr
 };
+
 String ResourceImporterTexture::get_import_settings_string() const {
 	String s;
 
 	int index = 0;
 	while (compression_formats[index]) {
-		String setting_path = "rendering/textures/vram_compression/import_" + String(compression_formats[index]);
-		bool test = GLOBAL_GET(setting_path);
-		if (test) {
+		const String setting_path = "rendering/textures/vram_compression/import_" + String(compression_formats[index]);
+		if (bool(GLOBAL_GET(setting_path))) {
 			s += String(compression_formats[index]);
 		}
+
 		index++;
 	}
 
@@ -780,6 +805,7 @@ bool ResourceImporterTexture::are_import_settings_valid(const String &p_path, co
 		if (editor_meta.has("editor_scale") && (float)editor_meta["editor_scale"] != EDSCALE) {
 			return false;
 		}
+
 		if (editor_meta.has("editor_dark_theme") && (bool)editor_meta["editor_dark_theme"] != EditorThemeManager::is_dark_theme()) {
 			return false;
 		}
@@ -789,8 +815,7 @@ bool ResourceImporterTexture::are_import_settings_valid(const String &p_path, co
 		return false;
 	}
 
-	bool vram = p_meta["vram_texture"];
-	if (!vram) {
+	if (!bool(p_meta["vram_texture"])) {
 		return true; // Do not care about non-VRAM.
 	}
 
@@ -803,18 +828,16 @@ bool ResourceImporterTexture::are_import_settings_valid(const String &p_path, co
 	int index = 0;
 	bool valid = true;
 	while (compression_formats[index]) {
-		String setting_path = "rendering/textures/vram_compression/import_" + String(compression_formats[index]);
+		const String setting_path = "rendering/textures/vram_compression/import_" + String(compression_formats[index]);
 		if (ProjectSettings::get_singleton()->has_setting(setting_path)) {
-			bool test = GLOBAL_GET(setting_path);
-			if (test) {
-				if (!formats_imported.has(compression_formats[index])) {
-					valid = false;
-					break;
-				}
+			if (bool(GLOBAL_GET(setting_path)) && !formats_imported.has(compression_formats[index])) {
+				valid = false;
+				break;
 			}
 		} else {
 			WARN_PRINT("Setting for imported format not found: " + setting_path);
 		}
+
 		index++;
 	}
 
diff --git a/editor/import/resource_importer_texture.h b/editor/import/resource_importer_texture.h
index 8aa044f3c8..7fd55d9e2b 100644
--- a/editor/import/resource_importer_texture.h
+++ b/editor/import/resource_importer_texture.h
@@ -78,6 +78,10 @@ protected:
 	void _save_editor_meta(const Dictionary &p_metadata, const String &p_to_path);
 	Dictionary _load_editor_meta(const String &p_to_path) const;
 
+	static inline void _clamp_hdr_exposure(Ref<Image> &r_image);
+	static inline void _invert_y_channel(Ref<Image> &r_image);
+	static inline void _print_callback_message(const String &p_message);
+
 public:
 	static void save_to_ctex_format(Ref<FileAccess> f, const Ref<Image> &p_image, CompressMode p_compress_mode, Image::UsedChannels p_channels, Image::CompressMode p_compress_format, float p_lossy_quality);
 

From 335077a03f28bdb1cdd0dc1a09215b29a101a3dd Mon Sep 17 00:00:00 2001
From: BlueCube3310 <53150244+BlueCube3310@users.noreply.github.com>
Date: Fri, 11 Oct 2024 19:08:36 +0200
Subject: [PATCH 092/124] Image: More cleanup and reduced code duplication.

---
 core/io/image.cpp               | 363 +++++++++++++-------------------
 core/io/image.h                 |   4 +-
 modules/cvtt/register_types.cpp |   2 +-
 3 files changed, 146 insertions(+), 223 deletions(-)

diff --git a/core/io/image.cpp b/core/io/image.cpp
index f466848a52..b35c98b1eb 100644
--- a/core/io/image.cpp
+++ b/core/io/image.cpp
@@ -572,21 +572,18 @@ static bool _are_formats_compatible(Image::Format p_format0, Image::Format p_for
 
 void Image::convert(Format p_new_format) {
 	ERR_FAIL_INDEX_MSG(p_new_format, FORMAT_MAX, vformat("The Image format specified (%d) is out of range. See Image's Format enum.", p_new_format));
-	if (data.size() == 0) {
+
+	if (data.size() == 0 || p_new_format == format) {
 		return;
 	}
 
-	if (p_new_format == format) {
-		return;
-	}
+	ERR_FAIL_COND_MSG(Image::is_format_compressed(format) || Image::is_format_compressed(p_new_format),
+			"Cannot convert to (or from) compressed formats. Use compress() and decompress() instead.");
 
 	// Includes the main image.
 	const int mipmap_count = get_mipmap_count() + 1;
 
-	if (Image::is_format_compressed(format) || Image::is_format_compressed(p_new_format)) {
-		ERR_FAIL_MSG("Cannot convert to <-> from compressed formats. Use compress() and decompress() instead.");
-
-	} else if (!_are_formats_compatible(format, p_new_format)) {
+	if (!_are_formats_compatible(format, p_new_format)) {
 		// Use put/set pixel which is slower but works with non-byte formats.
 		Image new_img(width, height, mipmaps, p_new_format);
 
@@ -612,9 +609,10 @@ void Image::convert(Format p_new_format) {
 		return;
 	}
 
+	// Convert the formats in an optimized way by removing/adding color channels if necessary.
 	Image new_img(width, height, mipmaps, p_new_format);
 
-	int conversion_type = format | p_new_format << 8;
+	const int conversion_type = format | p_new_format << 8;
 
 	for (int mip = 0; mip < mipmap_count; mip++) {
 		int64_t mip_offset = 0;
@@ -1801,7 +1799,7 @@ template <typename Component, int CC, bool renormalize,
 		void (*average_func)(Component &, const Component &, const Component &, const Component &, const Component &),
 		void (*renormalize_func)(Component *)>
 static void _generate_po2_mipmap(const Component *p_src, Component *p_dst, uint32_t p_width, uint32_t p_height) {
-	//fast power of 2 mipmap generation
+	// Fast power of 2 mipmap generation.
 	uint32_t dst_w = MAX(p_width >> 1, 1u);
 	uint32_t dst_h = MAX(p_height >> 1, 1u);
 
@@ -1831,99 +1829,116 @@ static void _generate_po2_mipmap(const Component *p_src, Component *p_dst, uint3
 	}
 }
 
+void Image::_generate_mipmap_from_format(Image::Format p_format, const uint8_t *p_src, uint8_t *p_dst, uint32_t p_width, uint32_t p_height, bool p_renormalize) {
+	const float *src_float = reinterpret_cast<const float *>(p_src);
+	float *dst_float = reinterpret_cast<float *>(p_dst);
+
+	const uint16_t *src_u16 = reinterpret_cast<const uint16_t *>(p_src);
+	uint16_t *dst_u16 = reinterpret_cast<uint16_t *>(p_dst);
+
+	const uint32_t *src_u32 = reinterpret_cast<const uint32_t *>(p_src);
+	uint32_t *dst_u32 = reinterpret_cast<uint32_t *>(p_dst);
+
+	switch (p_format) {
+		case Image::FORMAT_L8:
+		case Image::FORMAT_R8:
+			_generate_po2_mipmap<uint8_t, 1, false, Image::average_4_uint8, Image::renormalize_uint8>(p_src, p_dst, p_width, p_height);
+			break;
+		case Image::FORMAT_LA8:
+			_generate_po2_mipmap<uint8_t, 2, false, Image::average_4_uint8, Image::renormalize_uint8>(p_src, p_dst, p_width, p_height);
+			break;
+		case Image::FORMAT_RG8:
+			_generate_po2_mipmap<uint8_t, 2, false, Image::average_4_uint8, Image::renormalize_uint8>(p_src, p_dst, p_width, p_height);
+			break;
+		case Image::FORMAT_RGB8: {
+			if (p_renormalize) {
+				_generate_po2_mipmap<uint8_t, 3, true, Image::average_4_uint8, Image::renormalize_uint8>(p_src, p_dst, p_width, p_height);
+			} else {
+				_generate_po2_mipmap<uint8_t, 3, false, Image::average_4_uint8, Image::renormalize_uint8>(p_src, p_dst, p_width, p_height);
+			}
+		} break;
+		case Image::FORMAT_RGBA8: {
+			if (p_renormalize) {
+				_generate_po2_mipmap<uint8_t, 4, true, Image::average_4_uint8, Image::renormalize_uint8>(p_src, p_dst, p_width, p_height);
+			} else {
+				_generate_po2_mipmap<uint8_t, 4, false, Image::average_4_uint8, Image::renormalize_uint8>(p_src, p_dst, p_width, p_height);
+			}
+		} break;
+		case Image::FORMAT_RF:
+			_generate_po2_mipmap<float, 1, false, Image::average_4_float, Image::renormalize_float>(src_float, dst_float, p_width, p_height);
+			break;
+		case Image::FORMAT_RGF:
+			_generate_po2_mipmap<float, 2, false, Image::average_4_float, Image::renormalize_float>(src_float, dst_float, p_width, p_height);
+			break;
+		case Image::FORMAT_RGBF: {
+			if (p_renormalize) {
+				_generate_po2_mipmap<float, 3, true, Image::average_4_float, Image::renormalize_float>(src_float, dst_float, p_width, p_height);
+			} else {
+				_generate_po2_mipmap<float, 3, false, Image::average_4_float, Image::renormalize_float>(src_float, dst_float, p_width, p_height);
+			}
+		} break;
+		case Image::FORMAT_RGBAF: {
+			if (p_renormalize) {
+				_generate_po2_mipmap<float, 4, true, Image::average_4_float, Image::renormalize_float>(src_float, dst_float, p_width, p_height);
+			} else {
+				_generate_po2_mipmap<float, 4, false, Image::average_4_float, Image::renormalize_float>(src_float, dst_float, p_width, p_height);
+			}
+		} break;
+		case Image::FORMAT_RH:
+			_generate_po2_mipmap<uint16_t, 1, false, Image::average_4_half, Image::renormalize_half>(src_u16, dst_u16, p_width, p_height);
+			break;
+		case Image::FORMAT_RGH:
+			_generate_po2_mipmap<uint16_t, 2, false, Image::average_4_half, Image::renormalize_half>(src_u16, dst_u16, p_width, p_height);
+			break;
+		case Image::FORMAT_RGBH: {
+			if (p_renormalize) {
+				_generate_po2_mipmap<uint16_t, 3, true, Image::average_4_half, Image::renormalize_half>(src_u16, dst_u16, p_width, p_height);
+			} else {
+				_generate_po2_mipmap<uint16_t, 3, false, Image::average_4_half, Image::renormalize_half>(src_u16, dst_u16, p_width, p_height);
+			}
+		} break;
+		case Image::FORMAT_RGBAH: {
+			if (p_renormalize) {
+				_generate_po2_mipmap<uint16_t, 4, true, Image::average_4_half, Image::renormalize_half>(src_u16, dst_u16, p_width, p_height);
+			} else {
+				_generate_po2_mipmap<uint16_t, 4, false, Image::average_4_half, Image::renormalize_half>(src_u16, dst_u16, p_width, p_height);
+			}
+		} break;
+		case Image::FORMAT_RGBE9995:
+			_generate_po2_mipmap<uint32_t, 1, false, Image::average_4_rgbe9995, Image::renormalize_rgbe9995>(src_u32, dst_u32, p_width, p_height);
+			break;
+
+		default:
+			return;
+	}
+}
+
 void Image::shrink_x2() {
 	ERR_FAIL_COND(data.is_empty());
+	Vector<uint8_t> new_data;
 
 	if (mipmaps) {
-		//just use the lower mipmap as base and copy all
-		Vector<uint8_t> new_img;
+		// Just use the lower mipmap as base and copy all.
+		int64_t ofs = get_mipmap_offset(1);
+		int64_t new_size = data.size() - ofs;
 
-		int ofs = get_mipmap_offset(1);
-
-		int new_size = data.size() - ofs;
-		new_img.resize(new_size);
-		ERR_FAIL_COND(new_img.is_empty());
-
-		{
-			uint8_t *w = new_img.ptrw();
-			const uint8_t *r = data.ptr();
-
-			memcpy(w, &r[ofs], new_size);
-		}
-
-		width = MAX(width / 2, 1);
-		height = MAX(height / 2, 1);
-		data = new_img;
+		new_data.resize(new_size);
+		ERR_FAIL_COND(new_data.is_empty());
 
+		memcpy(new_data.ptrw(), data.ptr() + ofs, new_size);
 	} else {
-		Vector<uint8_t> new_img;
-
+		// Generate a mipmap and replace the original.
 		ERR_FAIL_COND(!_can_modify(format));
-		int ps = get_format_pixel_size(format);
-		new_img.resize((width / 2) * (height / 2) * ps);
-		ERR_FAIL_COND(new_img.is_empty());
-		ERR_FAIL_COND(data.is_empty());
 
-		{
-			uint8_t *w = new_img.ptrw();
-			const uint8_t *r = data.ptr();
+		new_data.resize((width / 2) * (height / 2) * get_format_pixel_size(format));
+		ERR_FAIL_COND(data.is_empty() || new_data.is_empty());
 
-			switch (format) {
-				case FORMAT_L8:
-				case FORMAT_R8:
-					_generate_po2_mipmap<uint8_t, 1, false, Image::average_4_uint8, Image::renormalize_uint8>(r, w, width, height);
-					break;
-				case FORMAT_LA8:
-					_generate_po2_mipmap<uint8_t, 2, false, Image::average_4_uint8, Image::renormalize_uint8>(r, w, width, height);
-					break;
-				case FORMAT_RG8:
-					_generate_po2_mipmap<uint8_t, 2, false, Image::average_4_uint8, Image::renormalize_uint8>(r, w, width, height);
-					break;
-				case FORMAT_RGB8:
-					_generate_po2_mipmap<uint8_t, 3, false, Image::average_4_uint8, Image::renormalize_uint8>(r, w, width, height);
-					break;
-				case FORMAT_RGBA8:
-					_generate_po2_mipmap<uint8_t, 4, false, Image::average_4_uint8, Image::renormalize_uint8>(r, w, width, height);
-					break;
-
-				case FORMAT_RF:
-					_generate_po2_mipmap<float, 1, false, Image::average_4_float, Image::renormalize_float>(reinterpret_cast<const float *>(r), reinterpret_cast<float *>(w), width, height);
-					break;
-				case FORMAT_RGF:
-					_generate_po2_mipmap<float, 2, false, Image::average_4_float, Image::renormalize_float>(reinterpret_cast<const float *>(r), reinterpret_cast<float *>(w), width, height);
-					break;
-				case FORMAT_RGBF:
-					_generate_po2_mipmap<float, 3, false, Image::average_4_float, Image::renormalize_float>(reinterpret_cast<const float *>(r), reinterpret_cast<float *>(w), width, height);
-					break;
-				case FORMAT_RGBAF:
-					_generate_po2_mipmap<float, 4, false, Image::average_4_float, Image::renormalize_float>(reinterpret_cast<const float *>(r), reinterpret_cast<float *>(w), width, height);
-					break;
-
-				case FORMAT_RH:
-					_generate_po2_mipmap<uint16_t, 1, false, Image::average_4_half, Image::renormalize_half>(reinterpret_cast<const uint16_t *>(r), reinterpret_cast<uint16_t *>(w), width, height);
-					break;
-				case FORMAT_RGH:
-					_generate_po2_mipmap<uint16_t, 2, false, Image::average_4_half, Image::renormalize_half>(reinterpret_cast<const uint16_t *>(r), reinterpret_cast<uint16_t *>(w), width, height);
-					break;
-				case FORMAT_RGBH:
-					_generate_po2_mipmap<uint16_t, 3, false, Image::average_4_half, Image::renormalize_half>(reinterpret_cast<const uint16_t *>(r), reinterpret_cast<uint16_t *>(w), width, height);
-					break;
-				case FORMAT_RGBAH:
-					_generate_po2_mipmap<uint16_t, 4, false, Image::average_4_half, Image::renormalize_half>(reinterpret_cast<const uint16_t *>(r), reinterpret_cast<uint16_t *>(w), width, height);
-					break;
-
-				case FORMAT_RGBE9995:
-					_generate_po2_mipmap<uint32_t, 1, false, Image::average_4_rgbe9995, Image::renormalize_rgbe9995>(reinterpret_cast<const uint32_t *>(r), reinterpret_cast<uint32_t *>(w), width, height);
-					break;
-				default: {
-				}
-			}
-		}
-
-		width /= 2;
-		height /= 2;
-		data = new_img;
+		_generate_mipmap_from_format(format, data.ptr(), new_data.ptrw(), width, height, false);
 	}
+
+	width = MAX(width / 2, 1);
+	height = MAX(height / 2, 1);
+	data = new_data;
 }
 
 void Image::normalize() {
@@ -1951,107 +1966,25 @@ void Image::normalize() {
 
 Error Image::generate_mipmaps(bool p_renormalize) {
 	ERR_FAIL_COND_V_MSG(!_can_modify(format), ERR_UNAVAILABLE, "Cannot generate mipmaps in compressed or custom image formats.");
-
 	ERR_FAIL_COND_V_MSG(format == FORMAT_RGBA4444, ERR_UNAVAILABLE, "Cannot generate mipmaps from RGBA4444 format.");
-
 	ERR_FAIL_COND_V_MSG(width == 0 || height == 0, ERR_UNCONFIGURED, "Cannot generate mipmaps with width or height equal to 0.");
 
-	int mmcount;
-
-	int size = _get_dst_image_size(width, height, format, mmcount);
+	int gen_mipmap_count;
 
+	int64_t size = _get_dst_image_size(width, height, format, gen_mipmap_count);
 	data.resize(size);
-
 	uint8_t *wp = data.ptrw();
 
 	int prev_ofs = 0;
 	int prev_h = height;
 	int prev_w = width;
 
-	for (int i = 1; i <= mmcount; i++) {
+	for (int i = 1; i <= gen_mipmap_count; i++) {
 		int64_t ofs;
 		int w, h;
 		_get_mipmap_offset_and_size(i, ofs, w, h);
 
-		switch (format) {
-			case FORMAT_L8:
-			case FORMAT_R8:
-				_generate_po2_mipmap<uint8_t, 1, false, Image::average_4_uint8, Image::renormalize_uint8>(&wp[prev_ofs], &wp[ofs], prev_w, prev_h);
-				break;
-			case FORMAT_LA8:
-			case FORMAT_RG8:
-				_generate_po2_mipmap<uint8_t, 2, false, Image::average_4_uint8, Image::renormalize_uint8>(&wp[prev_ofs], &wp[ofs], prev_w, prev_h);
-				break;
-			case FORMAT_RGB8:
-				if (p_renormalize) {
-					_generate_po2_mipmap<uint8_t, 3, true, Image::average_4_uint8, Image::renormalize_uint8>(&wp[prev_ofs], &wp[ofs], prev_w, prev_h);
-				} else {
-					_generate_po2_mipmap<uint8_t, 3, false, Image::average_4_uint8, Image::renormalize_uint8>(&wp[prev_ofs], &wp[ofs], prev_w, prev_h);
-				}
-
-				break;
-			case FORMAT_RGBA8:
-				if (p_renormalize) {
-					_generate_po2_mipmap<uint8_t, 4, true, Image::average_4_uint8, Image::renormalize_uint8>(&wp[prev_ofs], &wp[ofs], prev_w, prev_h);
-				} else {
-					_generate_po2_mipmap<uint8_t, 4, false, Image::average_4_uint8, Image::renormalize_uint8>(&wp[prev_ofs], &wp[ofs], prev_w, prev_h);
-				}
-				break;
-			case FORMAT_RF:
-				_generate_po2_mipmap<float, 1, false, Image::average_4_float, Image::renormalize_float>(reinterpret_cast<const float *>(&wp[prev_ofs]), reinterpret_cast<float *>(&wp[ofs]), prev_w, prev_h);
-				break;
-			case FORMAT_RGF:
-				_generate_po2_mipmap<float, 2, false, Image::average_4_float, Image::renormalize_float>(reinterpret_cast<const float *>(&wp[prev_ofs]), reinterpret_cast<float *>(&wp[ofs]), prev_w, prev_h);
-				break;
-			case FORMAT_RGBF:
-				if (p_renormalize) {
-					_generate_po2_mipmap<float, 3, true, Image::average_4_float, Image::renormalize_float>(reinterpret_cast<const float *>(&wp[prev_ofs]), reinterpret_cast<float *>(&wp[ofs]), prev_w, prev_h);
-				} else {
-					_generate_po2_mipmap<float, 3, false, Image::average_4_float, Image::renormalize_float>(reinterpret_cast<const float *>(&wp[prev_ofs]), reinterpret_cast<float *>(&wp[ofs]), prev_w, prev_h);
-				}
-
-				break;
-			case FORMAT_RGBAF:
-				if (p_renormalize) {
-					_generate_po2_mipmap<float, 4, true, Image::average_4_float, Image::renormalize_float>(reinterpret_cast<const float *>(&wp[prev_ofs]), reinterpret_cast<float *>(&wp[ofs]), prev_w, prev_h);
-				} else {
-					_generate_po2_mipmap<float, 4, false, Image::average_4_float, Image::renormalize_float>(reinterpret_cast<const float *>(&wp[prev_ofs]), reinterpret_cast<float *>(&wp[ofs]), prev_w, prev_h);
-				}
-
-				break;
-			case FORMAT_RH:
-				_generate_po2_mipmap<uint16_t, 1, false, Image::average_4_half, Image::renormalize_half>(reinterpret_cast<const uint16_t *>(&wp[prev_ofs]), reinterpret_cast<uint16_t *>(&wp[ofs]), prev_w, prev_h);
-				break;
-			case FORMAT_RGH:
-				_generate_po2_mipmap<uint16_t, 2, false, Image::average_4_half, Image::renormalize_half>(reinterpret_cast<const uint16_t *>(&wp[prev_ofs]), reinterpret_cast<uint16_t *>(&wp[ofs]), prev_w, prev_h);
-				break;
-			case FORMAT_RGBH:
-				if (p_renormalize) {
-					_generate_po2_mipmap<uint16_t, 3, true, Image::average_4_half, Image::renormalize_half>(reinterpret_cast<const uint16_t *>(&wp[prev_ofs]), reinterpret_cast<uint16_t *>(&wp[ofs]), prev_w, prev_h);
-				} else {
-					_generate_po2_mipmap<uint16_t, 3, false, Image::average_4_half, Image::renormalize_half>(reinterpret_cast<const uint16_t *>(&wp[prev_ofs]), reinterpret_cast<uint16_t *>(&wp[ofs]), prev_w, prev_h);
-				}
-
-				break;
-			case FORMAT_RGBAH:
-				if (p_renormalize) {
-					_generate_po2_mipmap<uint16_t, 4, true, Image::average_4_half, Image::renormalize_half>(reinterpret_cast<const uint16_t *>(&wp[prev_ofs]), reinterpret_cast<uint16_t *>(&wp[ofs]), prev_w, prev_h);
-				} else {
-					_generate_po2_mipmap<uint16_t, 4, false, Image::average_4_half, Image::renormalize_half>(reinterpret_cast<const uint16_t *>(&wp[prev_ofs]), reinterpret_cast<uint16_t *>(&wp[ofs]), prev_w, prev_h);
-				}
-
-				break;
-			case FORMAT_RGBE9995:
-				if (p_renormalize) {
-					_generate_po2_mipmap<uint32_t, 1, true, Image::average_4_rgbe9995, Image::renormalize_rgbe9995>(reinterpret_cast<const uint32_t *>(&wp[prev_ofs]), reinterpret_cast<uint32_t *>(&wp[ofs]), prev_w, prev_h);
-				} else {
-					_generate_po2_mipmap<uint32_t, 1, false, Image::average_4_rgbe9995, Image::renormalize_rgbe9995>(reinterpret_cast<const uint32_t *>(&wp[prev_ofs]), reinterpret_cast<uint32_t *>(&wp[ofs]), prev_w, prev_h);
-				}
-
-				break;
-			default: {
-			}
-		}
+		_generate_mipmap_from_format(format, wp + prev_ofs, wp + ofs, prev_w, prev_h, p_renormalize);
 
 		prev_ofs = ofs;
 		prev_w = w;
@@ -2080,8 +2013,7 @@ Error Image::generate_mipmap_roughness(RoughnessChannel p_roughness_channel, con
 	normal_sat_vec.resize(normal_w * normal_h * 3);
 	double *normal_sat = normal_sat_vec.ptr();
 
-	//create summed area table
-
+	// Create summed area table.
 	for (int y = 0; y < normal_h; y++) {
 		double line_sum[3] = { 0, 0, 0 };
 		for (int x = 0; x < normal_w; x++) {
@@ -2110,15 +2042,6 @@ Error Image::generate_mipmap_roughness(RoughnessChannel p_roughness_channel, con
 		}
 	}
 
-#if 0
-	{
-		Vector3 beg(normal_sat_vec[0], normal_sat_vec[1], normal_sat_vec[2]);
-		Vector3 end(normal_sat_vec[normal_sat_vec.size() - 3], normal_sat_vec[normal_sat_vec.size() - 2], normal_sat_vec[normal_sat_vec.size() - 1]);
-		Vector3 avg = (end - beg) / (normal_w * normal_h);
-		print_line("average: " + avg);
-	}
-#endif
-
 	int mmcount;
 
 	_get_dst_image_size(width, height, format, mmcount);
@@ -2520,8 +2443,7 @@ void Image::initialize_data(const char **p_xpm) {
 	}
 
 bool Image::is_invisible() const {
-	if (format == FORMAT_L8 ||
-			format == FORMAT_RGB8 || format == FORMAT_RG8) {
+	if (format == FORMAT_L8 || format == FORMAT_RGB8 || format == FORMAT_RG8) {
 		return false;
 	}
 
@@ -2784,9 +2706,7 @@ Error Image::compress_from_channels(CompressMode p_mode, UsedChannels p_channels
 						return OK;
 					}
 				}
-
 			} break;
-
 			case COMPRESS_S3TC: {
 				if (_image_compress_bc_rd_func) {
 					Error result = _image_compress_bc_rd_func(this, p_channels);
@@ -2796,7 +2716,6 @@ Error Image::compress_from_channels(CompressMode p_mode, UsedChannels p_channels
 						return OK;
 					}
 				}
-
 			} break;
 
 			default: {
@@ -3313,6 +3232,7 @@ Color Image::_get_color_at_ofs(const uint8_t *ptr, uint32_t ofs) const {
 		case FORMAT_RGBE9995: {
 			return Color::from_rgbe9995(((uint32_t *)ptr)[ofs]);
 		}
+
 		default: {
 			ERR_FAIL_V_MSG(Color(), "Can't get_pixel() on compressed image, sorry.");
 		}
@@ -3345,7 +3265,6 @@ void Image::_set_color_at_ofs(uint8_t *ptr, uint32_t ofs, const Color &p_color)
 			ptr[ofs * 4 + 1] = uint8_t(CLAMP(p_color.g * 255.0, 0, 255));
 			ptr[ofs * 4 + 2] = uint8_t(CLAMP(p_color.b * 255.0, 0, 255));
 			ptr[ofs * 4 + 3] = uint8_t(CLAMP(p_color.a * 255.0, 0, 255));
-
 		} break;
 		case FORMAT_RGBA4444: {
 			uint16_t rgba = 0;
@@ -3356,7 +3275,6 @@ void Image::_set_color_at_ofs(uint8_t *ptr, uint32_t ofs, const Color &p_color)
 			rgba |= uint16_t(CLAMP(p_color.a * 15.0, 0, 15));
 
 			((uint16_t *)ptr)[ofs] = rgba;
-
 		} break;
 		case FORMAT_RGB565: {
 			uint16_t rgba = 0;
@@ -3366,7 +3284,6 @@ void Image::_set_color_at_ofs(uint8_t *ptr, uint32_t ofs, const Color &p_color)
 			rgba |= uint16_t(CLAMP(p_color.b * 31.0, 0, 31)) << 11;
 
 			((uint16_t *)ptr)[ofs] = rgba;
-
 		} break;
 		case FORMAT_RF: {
 			((float *)ptr)[ofs] = p_color.r;
@@ -3406,8 +3323,8 @@ void Image::_set_color_at_ofs(uint8_t *ptr, uint32_t ofs, const Color &p_color)
 		} break;
 		case FORMAT_RGBE9995: {
 			((uint32_t *)ptr)[ofs] = p_color.to_rgbe9995();
-
 		} break;
+
 		default: {
 			ERR_FAIL_MSG("Can't set_pixel() on compressed image, sorry.");
 		}
@@ -3475,30 +3392,50 @@ void Image::adjust_bcs(float p_brightness, float p_contrast, float p_saturation)
 Image::UsedChannels Image::detect_used_channels(CompressSource p_source) const {
 	ERR_FAIL_COND_V(data.is_empty(), USED_CHANNELS_RGBA);
 	ERR_FAIL_COND_V(is_compressed(), USED_CHANNELS_RGBA);
+
+	if (p_source == COMPRESS_SOURCE_NORMAL) {
+		return USED_CHANNELS_RG; // Normal maps only use RG channels.
+	}
+
+	if (format == FORMAT_L8) {
+		return USED_CHANNELS_L; // Grayscale only cannot have any channel less.
+	} else if (format == FORMAT_R8 || format == FORMAT_RH || format == FORMAT_RF) {
+		return USED_CHANNELS_R; // Red only cannot have any channel less.
+	}
+
+	const bool supports_alpha = format == FORMAT_RGBA8 || format == FORMAT_RGBA4444 || format == FORMAT_RGBAH || format == FORMAT_RGBAF;
 	bool r = false, g = false, b = false, a = false, c = false;
 
 	const uint8_t *data_ptr = data.ptr();
-
-	uint32_t data_total = width * height;
+	const uint32_t data_total = width * height;
 
 	for (uint32_t i = 0; i < data_total; i++) {
 		Color col = _get_color_at_ofs(data_ptr, i);
 
-		if (col.r > 0.001) {
+		if (!r && col.r > 0.001) {
 			r = true;
 		}
-		if (col.g > 0.001) {
+		if (!g && col.g > 0.001) {
 			g = true;
 		}
-		if (col.b > 0.001) {
+		if (!b && col.b > 0.001) {
 			b = true;
 		}
-		if (col.a < 0.999) {
+		if (!a && col.a < 0.999) {
 			a = true;
 		}
 
 		if (col.r != col.b || col.r != col.g || col.b != col.g) {
-			c = true;
+			c = true; // The image is not grayscale.
+		}
+
+		if (r && g && b && c) {
+			// All channels are used, no need to continue.
+			if (!supports_alpha) {
+				break;
+			} else if (a) {
+				break;
+			}
 		}
 	}
 
@@ -3519,13 +3456,7 @@ Image::UsedChannels Image::detect_used_channels(CompressSource p_source) const {
 	}
 
 	if (p_source == COMPRESS_SOURCE_SRGB && (used_channels == USED_CHANNELS_R || used_channels == USED_CHANNELS_RG)) {
-		//R and RG do not support SRGB
-		used_channels = USED_CHANNELS_RGB;
-	}
-
-	if (p_source == COMPRESS_SOURCE_NORMAL) {
-		//use RG channels only for normal
-		used_channels = USED_CHANNELS_RG;
+		used_channels = USED_CHANNELS_RGB; // R and RG do not support SRGB.
 	}
 
 	return used_channels;
@@ -3730,14 +3661,6 @@ void Image::_bind_methods() {
 	BIND_ENUM_CONSTANT(ASTC_FORMAT_8x8);
 }
 
-void Image::set_compress_bc_func(void (*p_compress_func)(Image *, UsedChannels)) {
-	_image_compress_bc_func = p_compress_func;
-}
-
-void Image::set_compress_bptc_func(void (*p_compress_func)(Image *, UsedChannels)) {
-	_image_compress_bptc_func = p_compress_func;
-}
-
 void Image::normal_map_to_xy() {
 	convert(Image::FORMAT_RGBA8);
 
diff --git a/core/io/image.h b/core/io/image.h
index 3149314ad8..f6504c7be8 100644
--- a/core/io/image.h
+++ b/core/io/image.h
@@ -266,6 +266,8 @@ private:
 
 	Error _load_from_buffer(const Vector<uint8_t> &p_array, ImageMemLoadFunc p_loader);
 
+	_FORCE_INLINE_ void _generate_mipmap_from_format(Image::Format p_format, const uint8_t *p_src, uint8_t *p_dst, uint32_t p_width, uint32_t p_height, bool p_renormalize = false);
+
 	static void average_4_uint8(uint8_t &p_out, const uint8_t &p_a, const uint8_t &p_b, const uint8_t &p_c, const uint8_t &p_d);
 	static void average_4_float(float &p_out, const float &p_a, const float &p_b, const float &p_c, const float &p_d);
 	static void average_4_half(uint16_t &p_out, const uint16_t &p_a, const uint16_t &p_b, const uint16_t &p_c, const uint16_t &p_d);
@@ -393,8 +395,6 @@ public:
 	Rect2i get_used_rect() const;
 	Ref<Image> get_region(const Rect2i &p_area) const;
 
-	static void set_compress_bc_func(void (*p_compress_func)(Image *, UsedChannels));
-	static void set_compress_bptc_func(void (*p_compress_func)(Image *, UsedChannels));
 	static String get_format_name(Format p_format);
 
 	Error load_png_from_buffer(const Vector<uint8_t> &p_array);
diff --git a/modules/cvtt/register_types.cpp b/modules/cvtt/register_types.cpp
index 80e3062d04..991e8a6079 100644
--- a/modules/cvtt/register_types.cpp
+++ b/modules/cvtt/register_types.cpp
@@ -39,7 +39,7 @@ void initialize_cvtt_module(ModuleInitializationLevel p_level) {
 		return;
 	}
 
-	Image::set_compress_bptc_func(image_compress_cvtt);
+	Image::_image_compress_bptc_func = image_compress_cvtt;
 }
 
 void uninitialize_cvtt_module(ModuleInitializationLevel p_level) {

From 1bc70fe2e8d71ec72164ce2a3322e31d99eaaf0a Mon Sep 17 00:00:00 2001
From: kobewi <kobewi4e@gmail.com>
Date: Thu, 12 Dec 2024 13:13:36 +0100
Subject: [PATCH 093/124] Fix 3D ruler theme overrides

---
 editor/plugins/node_3d_editor_plugin.cpp | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)

diff --git a/editor/plugins/node_3d_editor_plugin.cpp b/editor/plugins/node_3d_editor_plugin.cpp
index f000840695..8deca36ecc 100644
--- a/editor/plugins/node_3d_editor_plugin.cpp
+++ b/editor/plugins/node_3d_editor_plugin.cpp
@@ -3254,6 +3254,12 @@ void Node3DEditorViewport::_notification(int p_what) {
 
 			cinema_label->add_theme_style_override(CoreStringName(normal), information_3d_stylebox);
 			locked_label->add_theme_style_override(CoreStringName(normal), information_3d_stylebox);
+
+			ruler_label->add_theme_color_override(SceneStringName(font_color), Color(1.0, 0.9, 0.0, 1.0));
+			ruler_label->add_theme_color_override("font_outline_color", Color(0.0, 0.0, 0.0, 1.0));
+			ruler_label->add_theme_constant_override("outline_size", 4 * EDSCALE);
+			ruler_label->add_theme_font_size_override(SceneStringName(font_size), 15 * EDSCALE);
+			ruler_label->add_theme_font_override(SceneStringName(font), get_theme_font(SNAME("bold"), EditorStringName(EditorFonts)));
 		} break;
 
 		case NOTIFICATION_DRAG_END: {
@@ -5827,11 +5833,6 @@ Node3DEditorViewport::Node3DEditorViewport(Node3DEditor *p_spatial_editor, int p
 	ruler_line_xray->set_material_override(ruler_material_xray);
 
 	ruler_label = memnew(Label);
-	ruler_label->add_theme_color_override(SceneStringName(font_color), Color(1.0, 0.9, 0.0, 1.0));
-	ruler_label->add_theme_color_override("font_outline_color", Color(0.0, 0.0, 0.0, 1.0));
-	ruler_label->add_theme_constant_override("outline_size", 4 * EDSCALE);
-	ruler_label->add_theme_font_size_override(SceneStringName(font_size), 15 * EDSCALE);
-	ruler_label->add_theme_font_override(SceneStringName(font), get_theme_font(SNAME("bold"), EditorStringName(EditorFonts)));
 	ruler_label->set_visible(false);
 
 	ruler->add_child(ruler_start_point);

From dbbacf3d0e1f25d6472ecfeec3828c89010b1cee Mon Sep 17 00:00:00 2001
From: HP van Braam <hp@tmm.cx>
Date: Thu, 12 Dec 2024 14:52:37 +0100
Subject: [PATCH 094/124] Regenerate X11 wrappers with dynload-wrapper 0.7

Dynload-wrapper 0.7 introduces the --include-dir option which lets us restrict
what headers are considered for generation.

In this case it removes the `XFreeThreads` Xlib function which does not
exist on Ubuntu 22.04 and older.
---
 .../linuxbsd/x11/dynwrappers/xcursor-so_wrap.c    |  2 +-
 .../linuxbsd/x11/dynwrappers/xcursor-so_wrap.h    |  2 +-
 platform/linuxbsd/x11/dynwrappers/xext-so_wrap.c  |  2 +-
 platform/linuxbsd/x11/dynwrappers/xext-so_wrap.h  |  2 +-
 .../linuxbsd/x11/dynwrappers/xinerama-so_wrap.c   |  2 +-
 .../linuxbsd/x11/dynwrappers/xinerama-so_wrap.h   |  2 +-
 .../linuxbsd/x11/dynwrappers/xinput2-so_wrap.c    |  2 +-
 .../linuxbsd/x11/dynwrappers/xinput2-so_wrap.h    |  2 +-
 platform/linuxbsd/x11/dynwrappers/xlib-so_wrap.c  | 15 ++-------------
 platform/linuxbsd/x11/dynwrappers/xlib-so_wrap.h  |  8 ++------
 .../linuxbsd/x11/dynwrappers/xrandr-so_wrap.c     |  2 +-
 .../linuxbsd/x11/dynwrappers/xrandr-so_wrap.h     |  2 +-
 .../linuxbsd/x11/dynwrappers/xrender-so_wrap.c    |  2 +-
 .../linuxbsd/x11/dynwrappers/xrender-so_wrap.h    |  2 +-
 14 files changed, 16 insertions(+), 31 deletions(-)

diff --git a/platform/linuxbsd/x11/dynwrappers/xcursor-so_wrap.c b/platform/linuxbsd/x11/dynwrappers/xcursor-so_wrap.c
index 7e1f3b8072..3f4e0d4e26 100644
--- a/platform/linuxbsd/x11/dynwrappers/xcursor-so_wrap.c
+++ b/platform/linuxbsd/x11/dynwrappers/xcursor-so_wrap.c
@@ -1,6 +1,6 @@
 // This file is generated. Do not edit!
 // see https://github.com/hpvb/dynload-wrapper for details
-// generated by generate-wrapper.py 0.6 on 2024-11-09 02:52:36
+// generated by generate-wrapper.py 0.7 on 2024-12-12 14:50:26
 // flags: generate-wrapper.py --include ./thirdparty/linuxbsd_headers/X11/Xcursor/Xcursor.h --sys-include thirdparty/linuxbsd_headers/X11/Xcursor/Xcursor.h --soname libXcursor.so.1 --init-name xcursor --output-header ./platform/linuxbsd/x11/dynwrappers/xcursor-so_wrap.h --output-implementation ./platform/linuxbsd/x11/dynwrappers/xcursor-so_wrap.c --ignore-other
 //
 #include <stdint.h>
diff --git a/platform/linuxbsd/x11/dynwrappers/xcursor-so_wrap.h b/platform/linuxbsd/x11/dynwrappers/xcursor-so_wrap.h
index 7266cfe609..eb425ba7ba 100644
--- a/platform/linuxbsd/x11/dynwrappers/xcursor-so_wrap.h
+++ b/platform/linuxbsd/x11/dynwrappers/xcursor-so_wrap.h
@@ -2,7 +2,7 @@
 #define DYLIBLOAD_WRAPPER_XCURSOR
 // This file is generated. Do not edit!
 // see https://github.com/hpvb/dynload-wrapper for details
-// generated by generate-wrapper.py 0.6 on 2024-11-09 02:52:36
+// generated by generate-wrapper.py 0.7 on 2024-12-12 14:50:26
 // flags: generate-wrapper.py --include ./thirdparty/linuxbsd_headers/X11/Xcursor/Xcursor.h --sys-include thirdparty/linuxbsd_headers/X11/Xcursor/Xcursor.h --soname libXcursor.so.1 --init-name xcursor --output-header ./platform/linuxbsd/x11/dynwrappers/xcursor-so_wrap.h --output-implementation ./platform/linuxbsd/x11/dynwrappers/xcursor-so_wrap.c --ignore-other
 //
 #include <stdint.h>
diff --git a/platform/linuxbsd/x11/dynwrappers/xext-so_wrap.c b/platform/linuxbsd/x11/dynwrappers/xext-so_wrap.c
index 56ae854686..7b3d33fe39 100644
--- a/platform/linuxbsd/x11/dynwrappers/xext-so_wrap.c
+++ b/platform/linuxbsd/x11/dynwrappers/xext-so_wrap.c
@@ -1,6 +1,6 @@
 // This file is generated. Do not edit!
 // see https://github.com/hpvb/dynload-wrapper for details
-// generated by generate-wrapper.py 0.6 on 2024-11-09 02:52:36
+// generated by generate-wrapper.py 0.7 on 2024-12-12 14:50:47
 // flags: generate-wrapper.py --sys-include thirdparty/linuxbsd_headers/X11/extensions/Xext.h --include ./thirdparty/linuxbsd_headers/X11/extensions/shape.h --sys-include thirdparty/linuxbsd_headers/X11/extensions/shape.h --soname libXext.so.6 --init-name xext --output-header ./platform/linuxbsd/x11/dynwrappers/xext-so_wrap.h --output-implementation ./platform/linuxbsd/x11/dynwrappers/xext-so_wrap.c --ignore-other --implementation-header thirdparty/linuxbsd_headers/X11/Xlib.h
 //
 #include <stdint.h>
diff --git a/platform/linuxbsd/x11/dynwrappers/xext-so_wrap.h b/platform/linuxbsd/x11/dynwrappers/xext-so_wrap.h
index ecce399d09..41a3bfac14 100644
--- a/platform/linuxbsd/x11/dynwrappers/xext-so_wrap.h
+++ b/platform/linuxbsd/x11/dynwrappers/xext-so_wrap.h
@@ -2,7 +2,7 @@
 #define DYLIBLOAD_WRAPPER_XEXT
 // This file is generated. Do not edit!
 // see https://github.com/hpvb/dynload-wrapper for details
-// generated by generate-wrapper.py 0.6 on 2024-11-09 02:52:36
+// generated by generate-wrapper.py 0.7 on 2024-12-12 14:50:47
 // flags: generate-wrapper.py --sys-include thirdparty/linuxbsd_headers/X11/extensions/Xext.h --include ./thirdparty/linuxbsd_headers/X11/extensions/shape.h --sys-include thirdparty/linuxbsd_headers/X11/extensions/shape.h --soname libXext.so.6 --init-name xext --output-header ./platform/linuxbsd/x11/dynwrappers/xext-so_wrap.h --output-implementation ./platform/linuxbsd/x11/dynwrappers/xext-so_wrap.c --ignore-other --implementation-header thirdparty/linuxbsd_headers/X11/Xlib.h
 //
 #include <stdint.h>
diff --git a/platform/linuxbsd/x11/dynwrappers/xinerama-so_wrap.c b/platform/linuxbsd/x11/dynwrappers/xinerama-so_wrap.c
index eeb6bf7a8f..554d877556 100644
--- a/platform/linuxbsd/x11/dynwrappers/xinerama-so_wrap.c
+++ b/platform/linuxbsd/x11/dynwrappers/xinerama-so_wrap.c
@@ -1,6 +1,6 @@
 // This file is generated. Do not edit!
 // see https://github.com/hpvb/dynload-wrapper for details
-// generated by generate-wrapper.py 0.6 on 2024-11-09 02:52:36
+// generated by generate-wrapper.py 0.7 on 2024-12-12 14:51:18
 // flags: generate-wrapper.py --include ./thirdparty/linuxbsd_headers/X11/extensions/Xinerama.h --sys-include thirdparty/linuxbsd_headers/X11/extensions/Xinerama.h --soname libXinerama.so.1 --init-name xinerama --output-header ./platform/linuxbsd/x11/dynwrappers/xinerama-so_wrap.h --output-implementation ./platform/linuxbsd/x11/dynwrappers/xinerama-so_wrap.c --ignore-other
 //
 #include <stdint.h>
diff --git a/platform/linuxbsd/x11/dynwrappers/xinerama-so_wrap.h b/platform/linuxbsd/x11/dynwrappers/xinerama-so_wrap.h
index b421a0ecf5..59783eb8b6 100644
--- a/platform/linuxbsd/x11/dynwrappers/xinerama-so_wrap.h
+++ b/platform/linuxbsd/x11/dynwrappers/xinerama-so_wrap.h
@@ -2,7 +2,7 @@
 #define DYLIBLOAD_WRAPPER_XINERAMA
 // This file is generated. Do not edit!
 // see https://github.com/hpvb/dynload-wrapper for details
-// generated by generate-wrapper.py 0.6 on 2024-11-09 02:52:36
+// generated by generate-wrapper.py 0.7 on 2024-12-12 14:51:18
 // flags: generate-wrapper.py --include ./thirdparty/linuxbsd_headers/X11/extensions/Xinerama.h --sys-include thirdparty/linuxbsd_headers/X11/extensions/Xinerama.h --soname libXinerama.so.1 --init-name xinerama --output-header ./platform/linuxbsd/x11/dynwrappers/xinerama-so_wrap.h --output-implementation ./platform/linuxbsd/x11/dynwrappers/xinerama-so_wrap.c --ignore-other
 //
 #include <stdint.h>
diff --git a/platform/linuxbsd/x11/dynwrappers/xinput2-so_wrap.c b/platform/linuxbsd/x11/dynwrappers/xinput2-so_wrap.c
index 8f031acb20..3446bb8ab4 100644
--- a/platform/linuxbsd/x11/dynwrappers/xinput2-so_wrap.c
+++ b/platform/linuxbsd/x11/dynwrappers/xinput2-so_wrap.c
@@ -1,6 +1,6 @@
 // This file is generated. Do not edit!
 // see https://github.com/hpvb/dynload-wrapper for details
-// generated by generate-wrapper.py 0.6 on 2024-11-09 02:52:36
+// generated by generate-wrapper.py 0.7 on 2024-12-12 14:51:34
 // flags: generate-wrapper.py --include ./thirdparty/linuxbsd_headers/X11/extensions/XInput2.h --sys-include thirdparty/linuxbsd_headers/X11/extensions/XInput2.h --soname libXi.so.6 --init-name xinput2 --output-header ./platform/linuxbsd/x11/dynwrappers/xinput2-so_wrap.h --output-implementation ./platform/linuxbsd/x11/dynwrappers/xinput2-so_wrap.c --ignore-other
 //
 #include <stdint.h>
diff --git a/platform/linuxbsd/x11/dynwrappers/xinput2-so_wrap.h b/platform/linuxbsd/x11/dynwrappers/xinput2-so_wrap.h
index 7c54dc080a..128df4b1c9 100644
--- a/platform/linuxbsd/x11/dynwrappers/xinput2-so_wrap.h
+++ b/platform/linuxbsd/x11/dynwrappers/xinput2-so_wrap.h
@@ -2,7 +2,7 @@
 #define DYLIBLOAD_WRAPPER_XINPUT2
 // This file is generated. Do not edit!
 // see https://github.com/hpvb/dynload-wrapper for details
-// generated by generate-wrapper.py 0.6 on 2024-11-09 02:52:36
+// generated by generate-wrapper.py 0.7 on 2024-12-12 14:51:34
 // flags: generate-wrapper.py --include ./thirdparty/linuxbsd_headers/X11/extensions/XInput2.h --sys-include thirdparty/linuxbsd_headers/X11/extensions/XInput2.h --soname libXi.so.6 --init-name xinput2 --output-header ./platform/linuxbsd/x11/dynwrappers/xinput2-so_wrap.h --output-implementation ./platform/linuxbsd/x11/dynwrappers/xinput2-so_wrap.c --ignore-other
 //
 #include <stdint.h>
diff --git a/platform/linuxbsd/x11/dynwrappers/xlib-so_wrap.c b/platform/linuxbsd/x11/dynwrappers/xlib-so_wrap.c
index 8500f20198..9affd47ce6 100644
--- a/platform/linuxbsd/x11/dynwrappers/xlib-so_wrap.c
+++ b/platform/linuxbsd/x11/dynwrappers/xlib-so_wrap.c
@@ -1,7 +1,7 @@
 // This file is generated. Do not edit!
 // see https://github.com/hpvb/dynload-wrapper for details
-// generated by generate-wrapper.py 0.6 on 2024-11-09 02:52:36
-// flags: generate-wrapper.py --include ./thirdparty/linuxbsd_headers/X11/Xlib.h --include ./thirdparty/linuxbsd_headers/X11/Xutil.h --include ./thirdparty/linuxbsd_headers/X11/XKBlib.h --sys-include thirdparty/linuxbsd_headers/X11/Xlib.h --sys-include thirdparty/linuxbsd_headers/X11/Xutil.h --sys-include thirdparty/linuxbsd_headers/X11/XKBlib.h --soname libX11.so.6 --init-name xlib --omit-prefix XkbGetDeviceIndicatorState --omit-prefix XkbAddSymInterpret --output-header ./platform/linuxbsd/x11/dynwrappers/xlib-so_wrap.h --output-implementation ./platform/linuxbsd/x11/dynwrappers/xlib-so_wrap.c
+// generated by generate-wrapper.py 0.7 on 2024-12-12 14:47:34
+// flags: generate-wrapper.py --include ./thirdparty/linuxbsd_headers/X11/Xlib.h --include ./thirdparty/linuxbsd_headers/X11/Xutil.h --include ./thirdparty/linuxbsd_headers/X11/XKBlib.h --sys-include thirdparty/linuxbsd_headers/X11/Xlib.h --sys-include thirdparty/linuxbsd_headers/X11/Xutil.h --sys-include thirdparty/linuxbsd_headers/X11/XKBlib.h --soname libX11.so.6 --init-name xlib --omit-prefix XkbGetDeviceIndicatorState --omit-prefix XkbAddSymInterpret --output-header ./platform/linuxbsd/x11/dynwrappers/xlib-so_wrap.h --include-dir ./thirdparty/linuxbsd_headers/ --output-implementation ./platform/linuxbsd/x11/dynwrappers/xlib-so_wrap.c
 //
 #include <stdint.h>
 
@@ -418,7 +418,6 @@
 #define _Xwctomb _Xwctomb_dylibloader_orig_xlib
 #define XGetEventData XGetEventData_dylibloader_orig_xlib
 #define XFreeEventData XFreeEventData_dylibloader_orig_xlib
-#define XFreeThreads XFreeThreads_dylibloader_orig_xlib
 #define XAllocClassHint XAllocClassHint_dylibloader_orig_xlib
 #define XAllocIconSize XAllocIconSize_dylibloader_orig_xlib
 #define XAllocSizeHints XAllocSizeHints_dylibloader_orig_xlib
@@ -1026,7 +1025,6 @@
 #undef _Xwctomb
 #undef XGetEventData
 #undef XFreeEventData
-#undef XFreeThreads
 #undef XAllocClassHint
 #undef XAllocIconSize
 #undef XAllocSizeHints
@@ -1633,7 +1631,6 @@ int (*_Xmbtowc_dylibloader_wrapper_xlib)(wchar_t *, char *, int);
 int (*_Xwctomb_dylibloader_wrapper_xlib)(char *, wchar_t);
 int (*XGetEventData_dylibloader_wrapper_xlib)(Display *, XGenericEventCookie *);
 void (*XFreeEventData_dylibloader_wrapper_xlib)(Display *, XGenericEventCookie *);
-int (*XFreeThreads_dylibloader_wrapper_xlib)(void);
 XClassHint *(*XAllocClassHint_dylibloader_wrapper_xlib)(void);
 XIconSize *(*XAllocIconSize_dylibloader_wrapper_xlib)(void);
 XSizeHints *(*XAllocSizeHints_dylibloader_wrapper_xlib)(void);
@@ -5140,14 +5137,6 @@ int initialize_xlib(int verbose) {
       fprintf(stderr, "%s\n", error);
     }
   }
-// XFreeThreads
-  *(void **) (&XFreeThreads_dylibloader_wrapper_xlib) = dlsym(handle, "XFreeThreads");
-  if (verbose) {
-    error = dlerror();
-    if (error != NULL) {
-      fprintf(stderr, "%s\n", error);
-    }
-  }
 // XAllocClassHint
   *(void **) (&XAllocClassHint_dylibloader_wrapper_xlib) = dlsym(handle, "XAllocClassHint");
   if (verbose) {
diff --git a/platform/linuxbsd/x11/dynwrappers/xlib-so_wrap.h b/platform/linuxbsd/x11/dynwrappers/xlib-so_wrap.h
index 3f556a7cda..69802fd2e8 100644
--- a/platform/linuxbsd/x11/dynwrappers/xlib-so_wrap.h
+++ b/platform/linuxbsd/x11/dynwrappers/xlib-so_wrap.h
@@ -2,8 +2,8 @@
 #define DYLIBLOAD_WRAPPER_XLIB
 // This file is generated. Do not edit!
 // see https://github.com/hpvb/dynload-wrapper for details
-// generated by generate-wrapper.py 0.6 on 2024-11-09 02:52:36
-// flags: generate-wrapper.py --include ./thirdparty/linuxbsd_headers/X11/Xlib.h --include ./thirdparty/linuxbsd_headers/X11/Xutil.h --include ./thirdparty/linuxbsd_headers/X11/XKBlib.h --sys-include thirdparty/linuxbsd_headers/X11/Xlib.h --sys-include thirdparty/linuxbsd_headers/X11/Xutil.h --sys-include thirdparty/linuxbsd_headers/X11/XKBlib.h --soname libX11.so.6 --init-name xlib --omit-prefix XkbGetDeviceIndicatorState --omit-prefix XkbAddSymInterpret --output-header ./platform/linuxbsd/x11/dynwrappers/xlib-so_wrap.h --output-implementation ./platform/linuxbsd/x11/dynwrappers/xlib-so_wrap.c
+// generated by generate-wrapper.py 0.7 on 2024-12-12 14:47:34
+// flags: generate-wrapper.py --include ./thirdparty/linuxbsd_headers/X11/Xlib.h --include ./thirdparty/linuxbsd_headers/X11/Xutil.h --include ./thirdparty/linuxbsd_headers/X11/XKBlib.h --sys-include thirdparty/linuxbsd_headers/X11/Xlib.h --sys-include thirdparty/linuxbsd_headers/X11/Xutil.h --sys-include thirdparty/linuxbsd_headers/X11/XKBlib.h --soname libX11.so.6 --init-name xlib --omit-prefix XkbGetDeviceIndicatorState --omit-prefix XkbAddSymInterpret --output-header ./platform/linuxbsd/x11/dynwrappers/xlib-so_wrap.h --include-dir ./thirdparty/linuxbsd_headers/ --output-implementation ./platform/linuxbsd/x11/dynwrappers/xlib-so_wrap.c
 //
 #include <stdint.h>
 
@@ -420,7 +420,6 @@
 #define _Xwctomb _Xwctomb_dylibloader_orig_xlib
 #define XGetEventData XGetEventData_dylibloader_orig_xlib
 #define XFreeEventData XFreeEventData_dylibloader_orig_xlib
-#define XFreeThreads XFreeThreads_dylibloader_orig_xlib
 #define XAllocClassHint XAllocClassHint_dylibloader_orig_xlib
 #define XAllocIconSize XAllocIconSize_dylibloader_orig_xlib
 #define XAllocSizeHints XAllocSizeHints_dylibloader_orig_xlib
@@ -1028,7 +1027,6 @@
 #undef _Xwctomb
 #undef XGetEventData
 #undef XFreeEventData
-#undef XFreeThreads
 #undef XAllocClassHint
 #undef XAllocIconSize
 #undef XAllocSizeHints
@@ -1636,7 +1634,6 @@ extern "C" {
 #define _Xwctomb _Xwctomb_dylibloader_wrapper_xlib
 #define XGetEventData XGetEventData_dylibloader_wrapper_xlib
 #define XFreeEventData XFreeEventData_dylibloader_wrapper_xlib
-#define XFreeThreads XFreeThreads_dylibloader_wrapper_xlib
 #define XAllocClassHint XAllocClassHint_dylibloader_wrapper_xlib
 #define XAllocIconSize XAllocIconSize_dylibloader_wrapper_xlib
 #define XAllocSizeHints XAllocSizeHints_dylibloader_wrapper_xlib
@@ -2241,7 +2238,6 @@ extern int (*_Xmbtowc_dylibloader_wrapper_xlib)(wchar_t *, char *, int);
 extern int (*_Xwctomb_dylibloader_wrapper_xlib)(char *, wchar_t);
 extern int (*XGetEventData_dylibloader_wrapper_xlib)(Display *, XGenericEventCookie *);
 extern void (*XFreeEventData_dylibloader_wrapper_xlib)(Display *, XGenericEventCookie *);
-extern int (*XFreeThreads_dylibloader_wrapper_xlib)(void);
 extern XClassHint *(*XAllocClassHint_dylibloader_wrapper_xlib)(void);
 extern XIconSize *(*XAllocIconSize_dylibloader_wrapper_xlib)(void);
 extern XSizeHints *(*XAllocSizeHints_dylibloader_wrapper_xlib)(void);
diff --git a/platform/linuxbsd/x11/dynwrappers/xrandr-so_wrap.c b/platform/linuxbsd/x11/dynwrappers/xrandr-so_wrap.c
index bc9de62caa..f33f35c106 100644
--- a/platform/linuxbsd/x11/dynwrappers/xrandr-so_wrap.c
+++ b/platform/linuxbsd/x11/dynwrappers/xrandr-so_wrap.c
@@ -1,6 +1,6 @@
 // This file is generated. Do not edit!
 // see https://github.com/hpvb/dynload-wrapper for details
-// generated by generate-wrapper.py 0.6 on 2024-11-09 02:52:37
+// generated by generate-wrapper.py 0.7 on 2024-12-12 14:51:53
 // flags: generate-wrapper.py --include ./thirdparty/linuxbsd_headers/X11/extensions/Xrandr.h --sys-include thirdparty/linuxbsd_headers/X11/extensions/Xrandr.h --soname libXrandr.so.2 --init-name xrandr --output-header ./platform/linuxbsd/x11/dynwrappers/xrandr-so_wrap.h --output-implementation ./platform/linuxbsd/x11/dynwrappers/xrandr-so_wrap.c --ignore-other
 //
 #include <stdint.h>
diff --git a/platform/linuxbsd/x11/dynwrappers/xrandr-so_wrap.h b/platform/linuxbsd/x11/dynwrappers/xrandr-so_wrap.h
index c055705f69..12278f9079 100644
--- a/platform/linuxbsd/x11/dynwrappers/xrandr-so_wrap.h
+++ b/platform/linuxbsd/x11/dynwrappers/xrandr-so_wrap.h
@@ -2,7 +2,7 @@
 #define DYLIBLOAD_WRAPPER_XRANDR
 // This file is generated. Do not edit!
 // see https://github.com/hpvb/dynload-wrapper for details
-// generated by generate-wrapper.py 0.6 on 2024-11-09 02:52:37
+// generated by generate-wrapper.py 0.7 on 2024-12-12 14:51:53
 // flags: generate-wrapper.py --include ./thirdparty/linuxbsd_headers/X11/extensions/Xrandr.h --sys-include thirdparty/linuxbsd_headers/X11/extensions/Xrandr.h --soname libXrandr.so.2 --init-name xrandr --output-header ./platform/linuxbsd/x11/dynwrappers/xrandr-so_wrap.h --output-implementation ./platform/linuxbsd/x11/dynwrappers/xrandr-so_wrap.c --ignore-other
 //
 #include <stdint.h>
diff --git a/platform/linuxbsd/x11/dynwrappers/xrender-so_wrap.c b/platform/linuxbsd/x11/dynwrappers/xrender-so_wrap.c
index e1cd987eca..89e90bc92f 100644
--- a/platform/linuxbsd/x11/dynwrappers/xrender-so_wrap.c
+++ b/platform/linuxbsd/x11/dynwrappers/xrender-so_wrap.c
@@ -1,6 +1,6 @@
 // This file is generated. Do not edit!
 // see https://github.com/hpvb/dynload-wrapper for details
-// generated by generate-wrapper.py 0.6 on 2024-11-09 02:52:37
+// generated by generate-wrapper.py 0.7 on 2024-12-12 14:52:10
 // flags: generate-wrapper.py --include ./thirdparty/linuxbsd_headers/X11/extensions/Xrender.h --sys-include thirdparty/linuxbsd_headers/X11/extensions/Xrender.h --soname libXrender.so.1 --init-name xrender --output-header ./platform/linuxbsd/x11/dynwrappers/xrender-so_wrap.h --output-implementation ./platform/linuxbsd/x11/dynwrappers/xrender-so_wrap.c --ignore-other
 //
 #include <stdint.h>
diff --git a/platform/linuxbsd/x11/dynwrappers/xrender-so_wrap.h b/platform/linuxbsd/x11/dynwrappers/xrender-so_wrap.h
index 3d0e8901ea..339037e18d 100644
--- a/platform/linuxbsd/x11/dynwrappers/xrender-so_wrap.h
+++ b/platform/linuxbsd/x11/dynwrappers/xrender-so_wrap.h
@@ -2,7 +2,7 @@
 #define DYLIBLOAD_WRAPPER_XRENDER
 // This file is generated. Do not edit!
 // see https://github.com/hpvb/dynload-wrapper for details
-// generated by generate-wrapper.py 0.6 on 2024-11-09 02:52:37
+// generated by generate-wrapper.py 0.7 on 2024-12-12 14:52:10
 // flags: generate-wrapper.py --include ./thirdparty/linuxbsd_headers/X11/extensions/Xrender.h --sys-include thirdparty/linuxbsd_headers/X11/extensions/Xrender.h --soname libXrender.so.1 --init-name xrender --output-header ./platform/linuxbsd/x11/dynwrappers/xrender-so_wrap.h --output-implementation ./platform/linuxbsd/x11/dynwrappers/xrender-so_wrap.c --ignore-other
 //
 #include <stdint.h>

From af47df1005febdb7b2e9382f4614d2fda492946d Mon Sep 17 00:00:00 2001
From: David Snopek <dsnopek@gmail.com>
Date: Tue, 10 Sep 2024 10:29:15 -0500
Subject: [PATCH 095/124] Expose `ResourceImporter::get_format_version` via
 `EditorImportPlugin::_get_format_version()`

---
 doc/classes/EditorImportPlugin.xml     | 6 ++++++
 editor/import/editor_import_plugin.cpp | 9 +++++++++
 editor/import/editor_import_plugin.h   | 2 ++
 3 files changed, 17 insertions(+)

diff --git a/doc/classes/EditorImportPlugin.xml b/doc/classes/EditorImportPlugin.xml
index 28614b6631..c552139236 100644
--- a/doc/classes/EditorImportPlugin.xml
+++ b/doc/classes/EditorImportPlugin.xml
@@ -127,6 +127,12 @@
 				If this method is not overridden, it will return [code]true[/code] by default (i.e., safe for parallel importing).
 			</description>
 		</method>
+		<method name="_get_format_version" qualifiers="virtual const">
+			<return type="int" />
+			<description>
+				Gets the format version of this importer. Increment this version when making incompatible changes to the format of the imported resources.
+			</description>
+		</method>
 		<method name="_get_import_options" qualifiers="virtual const">
 			<return type="Dictionary[]" />
 			<param index="0" name="path" type="String" />
diff --git a/editor/import/editor_import_plugin.cpp b/editor/import/editor_import_plugin.cpp
index 3243dcf256..8ed9345691 100644
--- a/editor/import/editor_import_plugin.cpp
+++ b/editor/import/editor_import_plugin.cpp
@@ -112,6 +112,14 @@ int EditorImportPlugin::get_import_order() const {
 	ERR_FAIL_V_MSG(-1, "Unimplemented _get_import_order in add-on.");
 }
 
+int EditorImportPlugin::get_format_version() const {
+	int ret = 0;
+	if (GDVIRTUAL_CALL(_get_format_version, ret)) {
+		return ret;
+	}
+	return 0;
+}
+
 void EditorImportPlugin::get_import_options(const String &p_path, List<ResourceImporter::ImportOption> *r_options, int p_preset) const {
 	Array needed;
 	needed.push_back("name");
@@ -220,6 +228,7 @@ void EditorImportPlugin::_bind_methods() {
 	GDVIRTUAL_BIND(_get_resource_type)
 	GDVIRTUAL_BIND(_get_priority)
 	GDVIRTUAL_BIND(_get_import_order)
+	GDVIRTUAL_BIND(_get_format_version)
 	GDVIRTUAL_BIND(_get_option_visibility, "path", "option_name", "options")
 	GDVIRTUAL_BIND(_import, "source_file", "save_path", "options", "platform_variants", "gen_files");
 	GDVIRTUAL_BIND(_can_import_threaded);
diff --git a/editor/import/editor_import_plugin.h b/editor/import/editor_import_plugin.h
index ea5cfc2682..1f88bb2fd7 100644
--- a/editor/import/editor_import_plugin.h
+++ b/editor/import/editor_import_plugin.h
@@ -50,6 +50,7 @@ protected:
 	GDVIRTUAL0RC(String, _get_resource_type)
 	GDVIRTUAL0RC(float, _get_priority)
 	GDVIRTUAL0RC(int, _get_import_order)
+	GDVIRTUAL0RC(int, _get_format_version)
 	GDVIRTUAL3RC(bool, _get_option_visibility, String, StringName, Dictionary)
 	GDVIRTUAL5RC(Error, _import, String, String, Dictionary, TypedArray<String>, TypedArray<String>)
 	GDVIRTUAL0RC(bool, _can_import_threaded)
@@ -67,6 +68,7 @@ public:
 	virtual String get_resource_type() const override;
 	virtual float get_priority() const override;
 	virtual int get_import_order() const override;
+	virtual int get_format_version() const override;
 	virtual void get_import_options(const String &p_path, List<ImportOption> *r_options, int p_preset) const override;
 	virtual bool get_option_visibility(const String &p_path, const String &p_option, const HashMap<StringName, Variant> &p_options) const override;
 	virtual Error import(const String &p_source_file, const String &p_save_path, const HashMap<StringName, Variant> &p_options, List<String> *r_platform_variants, List<String> *r_gen_files, Variant *r_metadata = nullptr) override;

From 8c01fc22744e56c666af0d70afc20a09910d6f80 Mon Sep 17 00:00:00 2001
From: David Snopek <dsnopek@gmail.com>
Date: Thu, 12 Dec 2024 08:31:12 -0600
Subject: [PATCH 096/124] Fix `StreamPeerExtension::put_partial_data()` to call
 `_put_partial_data()`

---
 core/io/stream_peer.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/core/io/stream_peer.cpp b/core/io/stream_peer.cpp
index 045904fb5d..b5abf15303 100644
--- a/core/io/stream_peer.cpp
+++ b/core/io/stream_peer.cpp
@@ -467,7 +467,7 @@ Error StreamPeerExtension::put_data(const uint8_t *p_data, int p_bytes) {
 
 Error StreamPeerExtension::put_partial_data(const uint8_t *p_data, int p_bytes, int &r_sent) {
 	Error err;
-	if (GDVIRTUAL_CALL(_put_data, p_data, p_bytes, &r_sent, err)) {
+	if (GDVIRTUAL_CALL(_put_partial_data, p_data, p_bytes, &r_sent, err)) {
 		return err;
 	}
 	WARN_PRINT_ONCE("StreamPeerExtension::_put_partial_data is unimplemented!");

From 5125adf40bb9b10961953a232228b60cf66ead4b Mon Sep 17 00:00:00 2001
From: Florent Guiocheau <florent.guiocheau@gmail.com>
Date: Wed, 11 Dec 2024 22:26:31 +0100
Subject: [PATCH 097/124] Fix large range point lights shadows

---
 .../forward_clustered/render_forward_clustered.cpp          | 4 ++--
 .../renderer_rd/forward_mobile/render_forward_mobile.cpp    | 4 ++--
 servers/rendering/renderer_scene_cull.cpp                   | 6 ++++--
 3 files changed, 8 insertions(+), 6 deletions(-)

diff --git a/servers/rendering/renderer_rd/forward_clustered/render_forward_clustered.cpp b/servers/rendering/renderer_rd/forward_clustered/render_forward_clustered.cpp
index 43a50e97c4..203307355a 100644
--- a/servers/rendering/renderer_rd/forward_clustered/render_forward_clustered.cpp
+++ b/servers/rendering/renderer_rd/forward_clustered/render_forward_clustered.cpp
@@ -2613,9 +2613,9 @@ void RenderForwardClustered::_render_shadow_pass(RID p_light, RID p_shadow_atlas
 			Rect2 atlas_rect_norm = atlas_rect;
 			atlas_rect_norm.position /= float(atlas_size);
 			atlas_rect_norm.size /= float(atlas_size);
-			copy_effects->copy_cubemap_to_dp(render_texture, atlas_fb, atlas_rect_norm, atlas_rect.size, light_projection.get_z_near(), light_projection.get_z_far(), false);
+			copy_effects->copy_cubemap_to_dp(render_texture, atlas_fb, atlas_rect_norm, atlas_rect.size, light_projection.get_z_near(), zfar, false);
 			atlas_rect_norm.position += Vector2(dual_paraboloid_offset) * atlas_rect_norm.size;
-			copy_effects->copy_cubemap_to_dp(render_texture, atlas_fb, atlas_rect_norm, atlas_rect.size, light_projection.get_z_near(), light_projection.get_z_far(), true);
+			copy_effects->copy_cubemap_to_dp(render_texture, atlas_fb, atlas_rect_norm, atlas_rect.size, light_projection.get_z_near(), zfar, true);
 
 			//restore transform so it can be properly used
 			light_storage->light_instance_set_shadow_transform(p_light, Projection(), light_storage->light_instance_get_base_transform(p_light), zfar, 0, 0, 0);
diff --git a/servers/rendering/renderer_rd/forward_mobile/render_forward_mobile.cpp b/servers/rendering/renderer_rd/forward_mobile/render_forward_mobile.cpp
index 2589488d5a..eef29794c1 100644
--- a/servers/rendering/renderer_rd/forward_mobile/render_forward_mobile.cpp
+++ b/servers/rendering/renderer_rd/forward_mobile/render_forward_mobile.cpp
@@ -1368,9 +1368,9 @@ void RenderForwardMobile::_render_shadow_pass(RID p_light, RID p_shadow_atlas, i
 			Rect2 atlas_rect_norm = atlas_rect;
 			atlas_rect_norm.position /= float(atlas_size);
 			atlas_rect_norm.size /= float(atlas_size);
-			copy_effects->copy_cubemap_to_dp(render_texture, atlas_fb, atlas_rect_norm, atlas_rect.size, light_projection.get_z_near(), light_projection.get_z_far(), false);
+			copy_effects->copy_cubemap_to_dp(render_texture, atlas_fb, atlas_rect_norm, atlas_rect.size, light_projection.get_z_near(), zfar, false);
 			atlas_rect_norm.position += Vector2(dual_paraboloid_offset) * atlas_rect_norm.size;
-			copy_effects->copy_cubemap_to_dp(render_texture, atlas_fb, atlas_rect_norm, atlas_rect.size, light_projection.get_z_near(), light_projection.get_z_far(), true);
+			copy_effects->copy_cubemap_to_dp(render_texture, atlas_fb, atlas_rect_norm, atlas_rect.size, light_projection.get_z_near(), zfar, true);
 
 			//restore transform so it can be properly used
 			light_storage->light_instance_set_shadow_transform(p_light, Projection(), light_storage->light_instance_get_base_transform(p_light), zfar, 0, 0, 0);
diff --git a/servers/rendering/renderer_scene_cull.cpp b/servers/rendering/renderer_scene_cull.cpp
index af17a35535..2c1112f771 100644
--- a/servers/rendering/renderer_scene_cull.cpp
+++ b/servers/rendering/renderer_scene_cull.cpp
@@ -2558,8 +2558,9 @@ bool RendererSceneCull::_light_instance_update_shadow(Instance *p_instance, cons
 				}
 
 				real_t radius = RSG::light_storage->light_get_param(p_instance->base, RS::LIGHT_PARAM_RANGE);
+				real_t z_near = 0.005f;
 				Projection cm;
-				cm.set_perspective(90, 1, radius * 0.005f, radius);
+				cm.set_perspective(90, 1, z_near, radius);
 
 				for (int i = 0; i < 6; i++) {
 					RENDER_TIMESTAMP("Cull OmniLight3D Shadow Cube, Side " + itos(i));
@@ -2647,9 +2648,10 @@ bool RendererSceneCull::_light_instance_update_shadow(Instance *p_instance, cons
 
 			real_t radius = RSG::light_storage->light_get_param(p_instance->base, RS::LIGHT_PARAM_RANGE);
 			real_t angle = RSG::light_storage->light_get_param(p_instance->base, RS::LIGHT_PARAM_SPOT_ANGLE);
+			real_t z_near = 0.005f;
 
 			Projection cm;
-			cm.set_perspective(angle * 2.0, 1.0, 0.005f * radius, radius);
+			cm.set_perspective(angle * 2.0, 1.0, z_near, radius);
 
 			Vector<Plane> planes = cm.get_projection_planes(light_transform);
 

From 7f098041546eb877ce2b28a4a127245995725fe5 Mon Sep 17 00:00:00 2001
From: Robert Yevdokimov <robert.yevdokimov@autStand.com>
Date: Sat, 30 Mar 2024 18:42:01 -0400
Subject: [PATCH 098/124] Add inherit parameter to `open_scene_from_path`

---
 doc/classes/EditorInterface.xml                   | 3 ++-
 editor/editor_interface.compat.inc                | 5 +++++
 editor/editor_interface.cpp                       | 6 +++---
 editor/editor_interface.h                         | 4 ++--
 editor/editor_node.cpp                            | 4 ++--
 editor/editor_node.h                              | 2 +-
 misc/extension_api_validation/4.3-stable.expected | 8 ++++++++
 7 files changed, 23 insertions(+), 9 deletions(-)

diff --git a/doc/classes/EditorInterface.xml b/doc/classes/EditorInterface.xml
index d291a9e355..5c00d2c41a 100644
--- a/doc/classes/EditorInterface.xml
+++ b/doc/classes/EditorInterface.xml
@@ -247,8 +247,9 @@
 		<method name="open_scene_from_path">
 			<return type="void" />
 			<param index="0" name="scene_filepath" type="String" />
+			<param index="1" name="set_inherited" type="bool" default="false" />
 			<description>
-				Opens the scene at the given path.
+				Opens the scene at the given path. If [param set_inherited] is [code]true[/code], creates a new inherited scene.
 			</description>
 		</method>
 		<method name="play_current_scene">
diff --git a/editor/editor_interface.compat.inc b/editor/editor_interface.compat.inc
index f5b35931fe..2622386350 100644
--- a/editor/editor_interface.compat.inc
+++ b/editor/editor_interface.compat.inc
@@ -40,9 +40,14 @@ void EditorInterface::_popup_property_selector_bind_compat_94323(Object *p_objec
 	popup_property_selector(p_object, p_callback, p_type_filter, String());
 }
 
+void EditorInterface::_open_scene_from_path_bind_compat_90057(const String &scene_path) {
+	return open_scene_from_path(scene_path, false);
+}
+
 void EditorInterface::_bind_compatibility_methods() {
 	ClassDB::bind_compatibility_method(D_METHOD("popup_node_selector", "callback", "valid_types"), &EditorInterface::_popup_node_selector_bind_compat_94323, DEFVAL(TypedArray<StringName>()));
 	ClassDB::bind_compatibility_method(D_METHOD("popup_property_selector", "object", "callback", "type_filter"), &EditorInterface::_popup_property_selector_bind_compat_94323, DEFVAL(PackedInt32Array()));
+	ClassDB::bind_compatibility_method(D_METHOD("open_scene_from_path", "scene_path"), &EditorInterface::_open_scene_from_path_bind_compat_90057);
 }
 
 #endif
diff --git a/editor/editor_interface.cpp b/editor/editor_interface.cpp
index 37b4e1291b..1a51e8dbf9 100644
--- a/editor/editor_interface.cpp
+++ b/editor/editor_interface.cpp
@@ -656,12 +656,12 @@ void EditorInterface::edit_script(const Ref<Script> &p_script, int p_line, int p
 	ScriptEditor::get_singleton()->edit(p_script, p_line - 1, p_col - 1, p_grab_focus);
 }
 
-void EditorInterface::open_scene_from_path(const String &scene_path) {
+void EditorInterface::open_scene_from_path(const String &scene_path, bool p_set_inherited) {
 	if (EditorNode::get_singleton()->is_changing_scene()) {
 		return;
 	}
 
-	EditorNode::get_singleton()->open_request(scene_path);
+	EditorNode::get_singleton()->open_request(scene_path, p_set_inherited);
 }
 
 void EditorInterface::reload_scene_from_path(const String &scene_path) {
@@ -839,7 +839,7 @@ void EditorInterface::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("edit_resource", "resource"), &EditorInterface::edit_resource);
 	ClassDB::bind_method(D_METHOD("edit_node", "node"), &EditorInterface::edit_node);
 	ClassDB::bind_method(D_METHOD("edit_script", "script", "line", "column", "grab_focus"), &EditorInterface::edit_script, DEFVAL(-1), DEFVAL(0), DEFVAL(true));
-	ClassDB::bind_method(D_METHOD("open_scene_from_path", "scene_filepath"), &EditorInterface::open_scene_from_path);
+	ClassDB::bind_method(D_METHOD("open_scene_from_path", "scene_filepath", "set_inherited"), &EditorInterface::open_scene_from_path, DEFVAL(false));
 	ClassDB::bind_method(D_METHOD("reload_scene_from_path", "scene_filepath"), &EditorInterface::reload_scene_from_path);
 
 	ClassDB::bind_method(D_METHOD("get_open_scenes"), &EditorInterface::get_open_scenes);
diff --git a/editor/editor_interface.h b/editor/editor_interface.h
index ca5306fd6d..bd2cd1187a 100644
--- a/editor/editor_interface.h
+++ b/editor/editor_interface.h
@@ -90,7 +90,7 @@ protected:
 #ifndef DISABLE_DEPRECATED
 	void _popup_node_selector_bind_compat_94323(const Callable &p_callback, const TypedArray<StringName> &p_valid_types = TypedArray<StringName>());
 	void _popup_property_selector_bind_compat_94323(Object *p_object, const Callable &p_callback, const PackedInt32Array &p_type_filter = PackedInt32Array());
-
+	void _open_scene_from_path_bind_compat_90057(const String &scene_path);
 	static void _bind_compatibility_methods();
 #endif
 
@@ -167,7 +167,7 @@ public:
 	void edit_resource(const Ref<Resource> &p_resource);
 	void edit_node(Node *p_node);
 	void edit_script(const Ref<Script> &p_script, int p_line = -1, int p_col = 0, bool p_grab_focus = true);
-	void open_scene_from_path(const String &scene_path);
+	void open_scene_from_path(const String &scene_path, bool p_set_inherited = false);
 	void reload_scene_from_path(const String &scene_path);
 
 	PackedStringArray get_open_scenes() const;
diff --git a/editor/editor_node.cpp b/editor/editor_node.cpp
index 5030589f5d..1be9b0ff1a 100644
--- a/editor/editor_node.cpp
+++ b/editor/editor_node.cpp
@@ -4478,7 +4478,7 @@ void EditorNode::replace_history_reimported_nodes(Node *p_original_root_node, No
 	}
 }
 
-void EditorNode::open_request(const String &p_path) {
+void EditorNode::open_request(const String &p_path, bool p_set_inherited) {
 	if (!opening_prev) {
 		List<String>::Element *prev_scene_item = previous_scenes.find(p_path);
 		if (prev_scene_item != nullptr) {
@@ -4486,7 +4486,7 @@ void EditorNode::open_request(const String &p_path) {
 		}
 	}
 
-	load_scene(p_path); // As it will be opened in separate tab.
+	load_scene(p_path, false, p_set_inherited); // As it will be opened in separate tab.
 }
 
 bool EditorNode::has_previous_scenes() const {
diff --git a/editor/editor_node.h b/editor/editor_node.h
index fb6cc0932f..9ea3ffe313 100644
--- a/editor/editor_node.h
+++ b/editor/editor_node.h
@@ -768,7 +768,7 @@ public:
 	void replace_resources_in_scenes(
 			const Vector<Ref<Resource>> &p_source_resources,
 			const Vector<Ref<Resource>> &p_target_resource);
-	void open_request(const String &p_path);
+	void open_request(const String &p_path, bool p_set_inherited = false);
 	void edit_foreign_resource(Ref<Resource> p_resource);
 
 	bool is_resource_read_only(Ref<Resource> p_resource, bool p_foreign_resources_are_writable = false);
diff --git a/misc/extension_api_validation/4.3-stable.expected b/misc/extension_api_validation/4.3-stable.expected
index 8cade432e6..fe3e269a30 100644
--- a/misc/extension_api_validation/4.3-stable.expected
+++ b/misc/extension_api_validation/4.3-stable.expected
@@ -228,3 +228,11 @@ Validate extension JSON: Error: Field 'classes/NavigationServer3D/methods/map_ge
 
 `query_path` and `map_get_path` methods changed to be non const due to internal compatibility and server changes.
 Added optional callback parameters to `query_path` functions. Compatibility methods registered.
+
+
+GH-90057
+--------
+Validate extension JSON: Error: Field 'classes/EditorInterface/methods/open_scene_from_path/arguments': size changed value in new API, from 1 to 2.
+
+Added optional argument to open_scene_from_path to create a new inherited scene.
+Compatibility method registered.

From 180c1b1cfabb69d460eb80d9f56bba2e11af4d80 Mon Sep 17 00:00:00 2001
From: Yufeng Ying <nbyyf2002@mail.ustc.edu.cn>
Date: Thu, 12 Dec 2024 23:40:25 +0800
Subject: [PATCH 099/124] Remove dead code.

---
 scene/animation/animation_node_state_machine.cpp | 8 --------
 1 file changed, 8 deletions(-)

diff --git a/scene/animation/animation_node_state_machine.cpp b/scene/animation/animation_node_state_machine.cpp
index 844cc9d622..e367d3d181 100644
--- a/scene/animation/animation_node_state_machine.cpp
+++ b/scene/animation/animation_node_state_machine.cpp
@@ -1579,16 +1579,8 @@ void AnimationNodeStateMachine::remove_transition(const StringName &p_from, cons
 
 void AnimationNodeStateMachine::remove_transition_by_index(const int p_transition) {
 	ERR_FAIL_INDEX(p_transition, transitions.size());
-	Transition tr = transitions[p_transition];
 	transitions.write[p_transition].transition->disconnect("advance_condition_changed", callable_mp(this, &AnimationNodeStateMachine::_tree_changed));
 	transitions.remove_at(p_transition);
-
-	Vector<String> path_from = String(tr.from).split("/");
-	Vector<String> path_to = String(tr.to).split("/");
-
-	List<Vector<String>> paths;
-	paths.push_back(path_from);
-	paths.push_back(path_to);
 }
 
 void AnimationNodeStateMachine::_remove_transition(const Ref<AnimationNodeStateMachineTransition> p_transition) {

From a1b44ec7a734c3ead6d69e55f9ba3b90e591cd20 Mon Sep 17 00:00:00 2001
From: "Matias N. Goldberg" <dark_sylinc@yahoo.com.ar>
Date: Thu, 12 Dec 2024 12:47:08 -0300
Subject: [PATCH 100/124] Fix grammar / spelling in comments

Minor fixes for changes introduced in #99257 that could not be fixed in
time as the PR needed to be expedited.
---
 .../d3d12/rendering_device_driver_d3d12.cpp   |  2 +-
 .../metal/rendering_device_driver_metal.mm    |  2 +-
 .../vulkan/rendering_device_driver_vulkan.cpp | 10 ++++-----
 servers/rendering/rendering_device.cpp        | 22 +++++++++----------
 servers/rendering/rendering_device.h          |  2 +-
 5 files changed, 19 insertions(+), 19 deletions(-)

diff --git a/drivers/d3d12/rendering_device_driver_d3d12.cpp b/drivers/d3d12/rendering_device_driver_d3d12.cpp
index 917104d0df..00fc3876a9 100644
--- a/drivers/d3d12/rendering_device_driver_d3d12.cpp
+++ b/drivers/d3d12/rendering_device_driver_d3d12.cpp
@@ -3830,7 +3830,7 @@ static void _add_descriptor_count_for_uniform(RenderingDevice::UniformType p_typ
 }
 
 RDD::UniformSetID RenderingDeviceDriverD3D12::uniform_set_create(VectorView<BoundUniform> p_uniforms, ShaderID p_shader, uint32_t p_set_index, int p_linear_pool_index) {
-	// p_linear_pool_index = -1; // TODO:? Linear pools not implemented or not supported by API backend.
+	//p_linear_pool_index = -1; // TODO:? Linear pools not implemented or not supported by API backend.
 
 	// Pre-bookkeep.
 	UniformSetInfo *uniform_set_info = VersatileResource::allocate<UniformSetInfo>(resources_allocator);
diff --git a/drivers/metal/rendering_device_driver_metal.mm b/drivers/metal/rendering_device_driver_metal.mm
index 302e4dc552..dea02f6997 100644
--- a/drivers/metal/rendering_device_driver_metal.mm
+++ b/drivers/metal/rendering_device_driver_metal.mm
@@ -2570,7 +2570,7 @@ void RenderingDeviceDriverMetal::shader_destroy_modules(ShaderID p_shader) {
 /*********************/
 
 RDD::UniformSetID RenderingDeviceDriverMetal::uniform_set_create(VectorView<BoundUniform> p_uniforms, ShaderID p_shader, uint32_t p_set_index, int p_linear_pool_index) {
-	// p_linear_pool_index = -1; // TODO:? Linear pools not implemented or not supported by API backend.
+	//p_linear_pool_index = -1; // TODO:? Linear pools not implemented or not supported by API backend.
 
 	MDUniformSet *set = new MDUniformSet();
 	Vector<BoundUniform> bound_uniforms;
diff --git a/drivers/vulkan/rendering_device_driver_vulkan.cpp b/drivers/vulkan/rendering_device_driver_vulkan.cpp
index ae6cddc987..3189ab0bd1 100644
--- a/drivers/vulkan/rendering_device_driver_vulkan.cpp
+++ b/drivers/vulkan/rendering_device_driver_vulkan.cpp
@@ -1384,9 +1384,9 @@ Error RenderingDeviceDriverVulkan::initialize(uint32_t p_device_index, uint32_t
 	vkGetPhysicalDeviceProperties(physical_device, &physical_device_properties);
 
 	// Workaround a driver bug on Adreno 730 GPUs that keeps leaking memory on each call to vkResetDescriptorPool.
-	// Which eventually run out of memory. in such case we should not be using linear allocated pools
-	// Bug introduced in driver 512.597.0 and fixed in 512.671.0
-	// Confirmed by Qualcomm
+	// Which eventually run out of memory. In such case we should not be using linear allocated pools
+	// Bug introduced in driver 512.597.0 and fixed in 512.671.0.
+	// Confirmed by Qualcomm.
 	if (linear_descriptor_pools_enabled) {
 		const uint32_t reset_descriptor_pool_broken_driver_begin = VK_MAKE_VERSION(512u, 597u, 0u);
 		const uint32_t reset_descriptor_pool_fixed_driver_begin = VK_MAKE_VERSION(512u, 671u, 0u);
@@ -1749,7 +1749,7 @@ RDD::TextureID RenderingDeviceDriverVulkan::texture_create(const TextureFormat &
 			// VUID-VkImageCreateInfo-usage-00963 :
 			// If usage includes VK_IMAGE_USAGE_TRANSIENT_ATTACHMENT_BIT,
 			// then bits other than VK_IMAGE_USAGE_COLOR_ATTACHMENT_BIT, VK_IMAGE_USAGE_DEPTH_STENCIL_ATTACHMENT_BIT,
-			// and VK_IMAGE_USAGE_INPUT_ATTACHMENT_BIT must not be set
+			// and VK_IMAGE_USAGE_INPUT_ATTACHMENT_BIT must not be set.
 			create_info.usage &= (VK_IMAGE_USAGE_TRANSIENT_ATTACHMENT_BIT | VK_IMAGE_USAGE_COLOR_ATTACHMENT_BIT | VK_IMAGE_USAGE_DEPTH_STENCIL_ATTACHMENT_BIT | VK_IMAGE_USAGE_INPUT_ATTACHMENT_BIT);
 		} else {
 			alloc_create_info.preferredFlags = VK_MEMORY_PROPERTY_DEVICE_LOCAL_BIT;
@@ -5925,7 +5925,7 @@ RenderingDeviceDriverVulkan::~RenderingDeviceDriverVulkan() {
 	}
 	vmaDestroyAllocator(allocator);
 
-	// Destroy linearly allocated descriptor pools
+	// Destroy linearly allocated descriptor pools.
 	for (KeyValue<int, DescriptorSetPools> &pool_map : linear_descriptor_set_pools) {
 		for (KeyValue<DescriptorSetPoolKey, HashMap<VkDescriptorPool, uint32_t>> pools : pool_map.value) {
 			for (KeyValue<VkDescriptorPool, uint32_t> descriptor_pool : pools.value) {
diff --git a/servers/rendering/rendering_device.cpp b/servers/rendering/rendering_device.cpp
index 2fbafcbda3..b8b60e2e63 100644
--- a/servers/rendering/rendering_device.cpp
+++ b/servers/rendering/rendering_device.cpp
@@ -3829,7 +3829,7 @@ Error RenderingDevice::screen_create(DisplayServer::WindowID p_screen) {
 Error RenderingDevice::screen_prepare_for_drawing(DisplayServer::WindowID p_screen) {
 	_THREAD_SAFE_METHOD_
 
-	// After submitting work, acquire the swapchain image(s)
+	// After submitting work, acquire the swapchain image(s).
 	HashMap<DisplayServer::WindowID, RDD::SwapChainID>::ConstIterator it = screen_swap_chains.find(p_screen);
 	ERR_FAIL_COND_V_MSG(it == screen_swap_chains.end(), ERR_CANT_CREATE, "A swap chain was not created for the screen.");
 
@@ -4432,18 +4432,18 @@ void RenderingDevice::draw_list_draw(DrawListID p_list, bool p_use_indices, uint
 		}
 
 		if (!dl->state.sets[i].bound) {
-			// Batch contiguous descriptor sets in a single call
+			// Batch contiguous descriptor sets in a single call.
 			if (descriptor_set_batching) {
 				// All good, see if this requires re-binding.
 				if (i - last_set_index > 1) {
-					// If the descriptor sets are not contiguous, bind the previous ones and start a new batch
+					// If the descriptor sets are not contiguous, bind the previous ones and start a new batch.
 					draw_graph.add_draw_list_bind_uniform_sets(dl->state.pipeline_shader_driver_id, valid_descriptor_ids, first_set_index, valid_set_count);
 
 					first_set_index = i;
 					valid_set_count = 1;
 					valid_descriptor_ids[0] = dl->state.sets[i].uniform_set_driver_id;
 				} else {
-					// Otherwise, keep storing in the current batch
+					// Otherwise, keep storing in the current batch.
 					valid_descriptor_ids[valid_set_count] = dl->state.sets[i].uniform_set_driver_id;
 					valid_set_count++;
 				}
@@ -4460,7 +4460,7 @@ void RenderingDevice::draw_list_draw(DrawListID p_list, bool p_use_indices, uint
 		}
 	}
 
-	// Bind the remaining batch
+	// Bind the remaining batch.
 	if (descriptor_set_batching && valid_set_count > 0) {
 		draw_graph.add_draw_list_bind_uniform_sets(dl->state.pipeline_shader_driver_id, valid_descriptor_ids, first_set_index, valid_set_count);
 	}
@@ -5005,14 +5005,14 @@ void RenderingDevice::compute_list_dispatch(ComputeListID p_list, uint32_t p_x_g
 			if (descriptor_set_batching) {
 				// All good, see if this requires re-binding.
 				if (i - last_set_index > 1) {
-					// If the descriptor sets are not contiguous, bind the previous ones and start a new batch
+					// If the descriptor sets are not contiguous, bind the previous ones and start a new batch.
 					draw_graph.add_compute_list_bind_uniform_sets(cl->state.pipeline_shader_driver_id, valid_descriptor_ids, first_set_index, valid_set_count);
 
 					first_set_index = i;
 					valid_set_count = 1;
 					valid_descriptor_ids[0] = cl->state.sets[i].uniform_set_driver_id;
 				} else {
-					// Otherwise, keep storing in the current batch
+					// Otherwise, keep storing in the current batch.
 					valid_descriptor_ids[valid_set_count] = cl->state.sets[i].uniform_set_driver_id;
 					valid_set_count++;
 				}
@@ -5029,7 +5029,7 @@ void RenderingDevice::compute_list_dispatch(ComputeListID p_list, uint32_t p_x_g
 		}
 	}
 
-	// Bind the remaining batch
+	// Bind the remaining batch.
 	if (valid_set_count > 0) {
 		draw_graph.add_compute_list_bind_uniform_sets(cl->state.pipeline_shader_driver_id, valid_descriptor_ids, first_set_index, valid_set_count);
 	}
@@ -5150,14 +5150,14 @@ void RenderingDevice::compute_list_dispatch_indirect(ComputeListID p_list, RID p
 		if (!cl->state.sets[i].bound) {
 			// All good, see if this requires re-binding.
 			if (i - last_set_index > 1) {
-				// If the descriptor sets are not contiguous, bind the previous ones and start a new batch
+				// If the descriptor sets are not contiguous, bind the previous ones and start a new batch.
 				draw_graph.add_compute_list_bind_uniform_sets(cl->state.pipeline_shader_driver_id, valid_descriptor_ids, first_set_index, valid_set_count);
 
 				first_set_index = i;
 				valid_set_count = 1;
 				valid_descriptor_ids[0] = cl->state.sets[i].uniform_set_driver_id;
 			} else {
-				// Otherwise, keep storing in the current batch
+				// Otherwise, keep storing in the current batch.
 				valid_descriptor_ids[valid_set_count] = cl->state.sets[i].uniform_set_driver_id;
 				valid_set_count++;
 			}
@@ -5172,7 +5172,7 @@ void RenderingDevice::compute_list_dispatch_indirect(ComputeListID p_list, RID p
 		}
 	}
 
-	// Bind the remaining batch
+	// Bind the remaining batch.
 	if (valid_set_count > 0) {
 		draw_graph.add_compute_list_bind_uniform_sets(cl->state.pipeline_shader_driver_id, valid_descriptor_ids, first_set_index, valid_set_count);
 	}
diff --git a/servers/rendering/rendering_device.h b/servers/rendering/rendering_device.h
index 92045bd0d8..716fdfaa05 100644
--- a/servers/rendering/rendering_device.h
+++ b/servers/rendering/rendering_device.h
@@ -191,7 +191,7 @@ private:
 	Error _buffer_initialize(Buffer *p_buffer, const uint8_t *p_data, size_t p_data_size, uint32_t p_required_align = 32);
 
 	void update_perf_report();
-	// flag for batching descriptor sets
+	// Flag for batching descriptor sets.
 	bool descriptor_set_batching = true;
 	// When true, the final draw call that copies our offscreen result into the Swapchain is put into its
 	// own cmd buffer, so that the whole rendering can start early instead of having to wait for the

From b29e93bb51ecc9098df9a7a47e06c8ea6b9216d7 Mon Sep 17 00:00:00 2001
From: havi05 <havi05git@gmail.com>
Date: Mon, 11 Nov 2024 20:54:37 +0100
Subject: [PATCH 101/124] ItemList - Add Toggle mode

---
 doc/classes/ItemList.xml |  3 +++
 scene/gui/item_list.cpp  | 23 ++++++++++++++++++-----
 scene/gui/item_list.h    |  3 ++-
 3 files changed, 23 insertions(+), 6 deletions(-)

diff --git a/doc/classes/ItemList.xml b/doc/classes/ItemList.xml
index 7754a61e8c..ef26eaad1e 100644
--- a/doc/classes/ItemList.xml
+++ b/doc/classes/ItemList.xml
@@ -473,6 +473,9 @@
 		<constant name="SELECT_MULTI" value="1" enum="SelectMode">
 			Allows selecting multiple items by holding [kbd]Ctrl[/kbd] or [kbd]Shift[/kbd].
 		</constant>
+		<constant name="SELECT_TOGGLE" value="2" enum="SelectMode">
+			Allows selecting multiple items by toggling them on and off.
+		</constant>
 	</constants>
 	<theme_items>
 		<theme_item name="font_color" data_type="color" type="Color" default="Color(0.65, 0.65, 0.65, 1)">
diff --git a/scene/gui/item_list.cpp b/scene/gui/item_list.cpp
index 1ba5ef309b..8f15c95bd2 100644
--- a/scene/gui/item_list.cpp
+++ b/scene/gui/item_list.cpp
@@ -419,7 +419,7 @@ void ItemList::select(int p_idx, bool p_single) {
 void ItemList::deselect(int p_idx) {
 	ERR_FAIL_INDEX(p_idx, items.size());
 
-	if (select_mode != SELECT_MULTI) {
+	if (select_mode == SELECT_SINGLE) {
 		items.write[p_idx].selected = false;
 		current = -1;
 	} else {
@@ -746,7 +746,7 @@ void ItemList::gui_input(const Ref<InputEvent> &p_event) {
 					return;
 				}
 
-				if (items[i].selectable && (!items[i].selected || allow_reselect)) {
+				if (items[i].selectable && (!items[i].selected || allow_reselect) && select_mode != SELECT_TOGGLE) {
 					select(i, select_mode == SELECT_SINGLE || !mb->is_command_or_control_pressed());
 
 					if (select_mode == SELECT_SINGLE) {
@@ -756,6 +756,18 @@ void ItemList::gui_input(const Ref<InputEvent> &p_event) {
 					}
 				}
 
+				if (items[i].selectable && select_mode == SELECT_TOGGLE) {
+					if (items[i].selected) {
+						deselect(i);
+						current = i;
+						emit_signal(SNAME("multi_selected"), i, false);
+					} else {
+						select(i, false);
+						current = i;
+						emit_signal(SNAME("multi_selected"), i, true);
+					}
+				}
+
 				emit_signal(SNAME("item_clicked"), i, get_local_mouse_position(), mb->get_button_index());
 
 				if (mb->get_button_index() == MouseButton::LEFT && mb->is_double_click()) {
@@ -929,7 +941,7 @@ void ItemList::gui_input(const Ref<InputEvent> &p_event) {
 			}
 		} else if (p_event->is_action("ui_cancel", true)) {
 			search_string = "";
-		} else if (p_event->is_action("ui_select", true) && select_mode == SELECT_MULTI) {
+		} else if (p_event->is_action("ui_select", true) && (select_mode == SELECT_MULTI || select_mode == SELECT_TOGGLE)) {
 			if (current >= 0 && current < items.size()) {
 				if (CAN_SELECT(current) && !items[current].selected) {
 					select(current, false);
@@ -1353,7 +1365,7 @@ void ItemList::_notification(int p_what) {
 					}
 				}
 
-				if (select_mode == SELECT_MULTI && i == current) {
+				if (i == current && (select_mode == SELECT_MULTI || select_mode == SELECT_TOGGLE)) {
 					Rect2 r = rcache;
 					r.position += base_ofs;
 
@@ -1913,7 +1925,7 @@ void ItemList::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("force_update_list_size"), &ItemList::force_update_list_size);
 
-	ADD_PROPERTY(PropertyInfo(Variant::INT, "select_mode", PROPERTY_HINT_ENUM, "Single,Multi"), "set_select_mode", "get_select_mode");
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "select_mode", PROPERTY_HINT_ENUM, "Single,Multi,Toggle"), "set_select_mode", "get_select_mode");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "allow_reselect"), "set_allow_reselect", "get_allow_reselect");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "allow_rmb_select"), "set_allow_rmb_select", "get_allow_rmb_select");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "allow_search"), "set_allow_search", "get_allow_search");
@@ -1936,6 +1948,7 @@ void ItemList::_bind_methods() {
 
 	BIND_ENUM_CONSTANT(SELECT_SINGLE);
 	BIND_ENUM_CONSTANT(SELECT_MULTI);
+	BIND_ENUM_CONSTANT(SELECT_TOGGLE);
 
 	ADD_SIGNAL(MethodInfo("item_selected", PropertyInfo(Variant::INT, "index")));
 	ADD_SIGNAL(MethodInfo("empty_clicked", PropertyInfo(Variant::VECTOR2, "at_position"), PropertyInfo(Variant::INT, "mouse_button_index")));
diff --git a/scene/gui/item_list.h b/scene/gui/item_list.h
index 0836ea33d5..822515b3b1 100644
--- a/scene/gui/item_list.h
+++ b/scene/gui/item_list.h
@@ -47,7 +47,8 @@ public:
 
 	enum SelectMode {
 		SELECT_SINGLE,
-		SELECT_MULTI
+		SELECT_MULTI,
+		SELECT_TOGGLE,
 	};
 
 private:

From f0b6b24d9e1c3b2e174c65f5f86bcbb78d9b3aee Mon Sep 17 00:00:00 2001
From: RedworkDE <10944644+RedworkDE@users.noreply.github.com>
Date: Sat, 21 Jan 2023 23:45:32 +0100
Subject: [PATCH 102/124] C#: Update to net7.0

Because of ref safety changes in the languages, all methods that return an interop struct have to have all other reference parameters marked as scoped to signal the the method does not capture that reference.

The variant change is necessary, because for some reason a type of the exact shape godot_variant is in, crashes the .NET 7 JIT, but when changing it to be sequential with the same effective layout it works.
---
 .../mono/build_scripts/build_assemblies.py    |   2 +-
 .../Godot.SourceGenerators.Sample.csproj      |   2 +-
 .../GodotTools.Core/GodotTools.Core.csproj    |   2 +-
 .../GodotTools.ProjectEditor.csproj           |   2 +-
 .../ProjectGenerator.cs                       |   8 +-
 .../GodotTools.Shared.csproj                  |  14 +-
 .../GodotTools/GodotTools/GodotTools.csproj   |   2 +-
 .../UnmanagedCallbacksGenerator.cs            |   6 +-
 .../GodotPlugins/GodotPlugins.csproj          |   2 +-
 .../Core/NativeInterop/InteropStructs.cs      |   2 +-
 .../Core/NativeInterop/Marshaling.cs          |  49 +++---
 .../Core/NativeInterop/NativeFuncs.cs         | 149 +++++++++---------
 .../NativeInterop/NativeFuncs.extended.cs     |  10 +-
 .../Core/NativeInterop/VariantUtils.cs        |  87 +++++-----
 .../NativeInterop/VariantUtils.generic.cs     |   6 +-
 .../GodotSharp/GodotSharp/GodotSharp.csproj   |   4 +-
 .../GodotSharpEditor/GodotSharpEditor.csproj  |   2 +-
 17 files changed, 160 insertions(+), 189 deletions(-)

diff --git a/modules/mono/build_scripts/build_assemblies.py b/modules/mono/build_scripts/build_assemblies.py
index 9f88b0575e..2a1ecba332 100755
--- a/modules/mono/build_scripts/build_assemblies.py
+++ b/modules/mono/build_scripts/build_assemblies.py
@@ -229,7 +229,7 @@ def build_godot_api(msbuild_tool, module_dir, output_dir, push_nupkgs_local, pre
 
         core_src_dir = os.path.abspath(os.path.join(sln, os.pardir, "GodotSharp", "bin", build_config))
         editor_src_dir = os.path.abspath(os.path.join(sln, os.pardir, "GodotSharpEditor", "bin", build_config))
-        plugins_src_dir = os.path.abspath(os.path.join(sln, os.pardir, "GodotPlugins", "bin", build_config, "net6.0"))
+        plugins_src_dir = os.path.abspath(os.path.join(sln, os.pardir, "GodotPlugins", "bin", build_config, "net7.0"))
 
         if not os.path.isdir(editor_api_dir):
             assert not os.path.isfile(editor_api_dir)
diff --git a/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Sample/Godot.SourceGenerators.Sample.csproj b/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Sample/Godot.SourceGenerators.Sample.csproj
index fc887313d5..5152b383ea 100644
--- a/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Sample/Godot.SourceGenerators.Sample.csproj
+++ b/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Sample/Godot.SourceGenerators.Sample.csproj
@@ -1,7 +1,7 @@
 <Project Sdk="Microsoft.NET.Sdk">
 
   <PropertyGroup>
-    <TargetFramework>net6.0</TargetFramework>
+    <TargetFramework>net7.0</TargetFramework>
     <LangVersion>11</LangVersion>
   </PropertyGroup>
 
diff --git a/modules/mono/editor/GodotTools/GodotTools.Core/GodotTools.Core.csproj b/modules/mono/editor/GodotTools/GodotTools.Core/GodotTools.Core.csproj
index f692f26a8b..37563e0cc8 100644
--- a/modules/mono/editor/GodotTools/GodotTools.Core/GodotTools.Core.csproj
+++ b/modules/mono/editor/GodotTools/GodotTools.Core/GodotTools.Core.csproj
@@ -1,7 +1,7 @@
 <Project Sdk="Microsoft.NET.Sdk">
   <PropertyGroup>
     <ProjectGuid>{639E48BD-44E5-4091-8EDD-22D36DC0768D}</ProjectGuid>
-    <TargetFramework>net6.0</TargetFramework>
+    <TargetFramework>net7.0</TargetFramework>
     <LangVersion>10</LangVersion>
     <Nullable>enable</Nullable>
   </PropertyGroup>
diff --git a/modules/mono/editor/GodotTools/GodotTools.ProjectEditor/GodotTools.ProjectEditor.csproj b/modules/mono/editor/GodotTools/GodotTools.ProjectEditor/GodotTools.ProjectEditor.csproj
index 5547ffcf19..9a73a7f81d 100644
--- a/modules/mono/editor/GodotTools/GodotTools.ProjectEditor/GodotTools.ProjectEditor.csproj
+++ b/modules/mono/editor/GodotTools/GodotTools.ProjectEditor/GodotTools.ProjectEditor.csproj
@@ -1,7 +1,7 @@
 <Project Sdk="Microsoft.NET.Sdk">
   <PropertyGroup>
     <ProjectGuid>{A8CDAD94-C6D4-4B19-A7E7-76C53CC92984}</ProjectGuid>
-    <TargetFramework>net6.0</TargetFramework>
+    <TargetFramework>net7.0</TargetFramework>
     <LangVersion>10</LangVersion>
     <Nullable>enable</Nullable>
   </PropertyGroup>
diff --git a/modules/mono/editor/GodotTools/GodotTools.ProjectEditor/ProjectGenerator.cs b/modules/mono/editor/GodotTools/GodotTools.ProjectEditor/ProjectGenerator.cs
index 04ea46e9ef..dd74a0d523 100644
--- a/modules/mono/editor/GodotTools/GodotTools.ProjectEditor/ProjectGenerator.cs
+++ b/modules/mono/editor/GodotTools/GodotTools.ProjectEditor/ProjectGenerator.cs
@@ -22,13 +22,7 @@ namespace GodotTools.ProjectEditor
             root.Sdk = GodotSdkAttrValue;
 
             var mainGroup = root.AddPropertyGroup();
-            mainGroup.AddProperty("TargetFramework", "net6.0");
-
-            var net7 = mainGroup.AddProperty("TargetFramework", "net7.0");
-            net7.Condition = " '$(GodotTargetPlatform)' == 'android' ";
-
-            var net8 = mainGroup.AddProperty("TargetFramework", "net8.0");
-            net8.Condition = " '$(GodotTargetPlatform)' == 'ios' ";
+            mainGroup.AddProperty("TargetFramework", "net8.0");
 
             mainGroup.AddProperty("EnableDynamicLoading", "true");
 
diff --git a/modules/mono/editor/GodotTools/GodotTools.Shared/GodotTools.Shared.csproj b/modules/mono/editor/GodotTools/GodotTools.Shared/GodotTools.Shared.csproj
index 7c19fabaf6..e916980f15 100644
--- a/modules/mono/editor/GodotTools/GodotTools.Shared/GodotTools.Shared.csproj
+++ b/modules/mono/editor/GodotTools/GodotTools.Shared/GodotTools.Shared.csproj
@@ -1,8 +1,8 @@
-<Project Sdk="Microsoft.NET.Sdk">
-  <PropertyGroup>
-    <TargetFramework>net6.0</TargetFramework>
-    <!-- Specify compile items manually to avoid including dangling generated items. -->
-    <EnableDefaultCompileItems>false</EnableDefaultCompileItems>
-  </PropertyGroup>
-  <Import Project="GenerateGodotNupkgsVersions.targets" />
+<Project Sdk="Microsoft.NET.Sdk">
+    <PropertyGroup>
+        <TargetFramework>net8.0</TargetFramework>
+        <!-- Specify compile items manually to avoid including dangling generated items. -->
+        <EnableDefaultCompileItems>false</EnableDefaultCompileItems>
+    </PropertyGroup>
+    <Import Project="GenerateGodotNupkgsVersions.targets" />
 </Project>
diff --git a/modules/mono/editor/GodotTools/GodotTools/GodotTools.csproj b/modules/mono/editor/GodotTools/GodotTools/GodotTools.csproj
index 96d2c5c75f..a8deeb2fea 100644
--- a/modules/mono/editor/GodotTools/GodotTools/GodotTools.csproj
+++ b/modules/mono/editor/GodotTools/GodotTools/GodotTools.csproj
@@ -1,7 +1,7 @@
 <Project Sdk="Microsoft.NET.Sdk">
   <PropertyGroup>
     <ProjectGuid>{27B00618-A6F2-4828-B922-05CAEB08C286}</ProjectGuid>
-    <TargetFramework>net6.0</TargetFramework>
+    <TargetFramework>net7.0</TargetFramework>
     <EnableDynamicLoading>true</EnableDynamicLoading>
     <LangVersion>10</LangVersion>
     <Nullable>enable</Nullable>
diff --git a/modules/mono/glue/GodotSharp/Godot.SourceGenerators.Internal/UnmanagedCallbacksGenerator.cs b/modules/mono/glue/GodotSharp/Godot.SourceGenerators.Internal/UnmanagedCallbacksGenerator.cs
index 08e293afcc..3d6a5b4662 100644
--- a/modules/mono/glue/GodotSharp/Godot.SourceGenerators.Internal/UnmanagedCallbacksGenerator.cs
+++ b/modules/mono/glue/GodotSharp/Godot.SourceGenerators.Internal/UnmanagedCallbacksGenerator.cs
@@ -172,11 +172,7 @@ using Godot.NativeInterop;
             {
                 var parameter = callback.Parameters[i];
 
-                AppendRefKind(source, parameter.RefKind);
-                source.Append(' ');
-                source.Append(parameter.Type.FullQualifiedNameIncludeGlobal());
-                source.Append(' ');
-                source.Append(parameter.Name);
+                source.Append(parameter.DeclaringSyntaxReferences[0].GetSyntax().ToString());
 
                 if (parameter.RefKind == RefKind.Out)
                 {
diff --git a/modules/mono/glue/GodotSharp/GodotPlugins/GodotPlugins.csproj b/modules/mono/glue/GodotSharp/GodotPlugins/GodotPlugins.csproj
index 1e60743fb1..1deccbc09d 100644
--- a/modules/mono/glue/GodotSharp/GodotPlugins/GodotPlugins.csproj
+++ b/modules/mono/glue/GodotSharp/GodotPlugins/GodotPlugins.csproj
@@ -1,7 +1,7 @@
 <Project Sdk="Microsoft.NET.Sdk">
 
   <PropertyGroup>
-    <TargetFramework>net6.0</TargetFramework>
+    <TargetFramework>net7.0</TargetFramework>
     <LangVersion>10</LangVersion>
     <Nullable>enable</Nullable>
     <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/InteropStructs.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/InteropStructs.cs
index 555aebcd83..6e34c2bfbf 100644
--- a/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/InteropStructs.cs
+++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/InteropStructs.cs
@@ -173,7 +173,7 @@ namespace Godot.NativeInterop
         internal readonly unsafe godot_variant* GetUnsafeAddress()
             => (godot_variant*)Unsafe.AsPointer(ref Unsafe.AsRef(in _typeField));
 
-        // Variant.Type is generated as an enum of type long, so we can't use for the field as it must only take 32-bits.
+        // Variant.Type is generated as an enum of type long, so we can't use for the field as it must only take 32-bits. (the native enum actually has no fixed underlying type so it is only at least 6 bits long)
         private int _typeField;
 
         // There's padding here
diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/Marshaling.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/Marshaling.cs
index 54efaad3c8..69ea567c99 100644
--- a/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/Marshaling.cs
+++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/Marshaling.cs
@@ -268,7 +268,7 @@ namespace Godot.NativeInterop
 
         // Callable
 
-        public static godot_callable ConvertCallableToNative(in Callable p_managed_callable)
+        public static godot_callable ConvertCallableToNative(scoped in Callable p_managed_callable)
         {
             if (p_managed_callable.Delegate != null)
             {
@@ -333,7 +333,7 @@ namespace Godot.NativeInterop
 
         // Signal
 
-        public static godot_signal ConvertSignalToNative(in Signal p_managed_signal)
+        public static godot_signal ConvertSignalToNative(scoped in Signal p_managed_signal)
         {
             ulong ownerId = p_managed_signal.Owner.GetInstanceId();
             godot_string_name name;
@@ -432,12 +432,12 @@ namespace Godot.NativeInterop
             return array;
         }
 
-        public static godot_packed_byte_array ConvertSystemArrayToNativePackedByteArray(Span<byte> p_array)
+        public static godot_packed_byte_array ConvertSystemArrayToNativePackedByteArray(scoped Span<byte> p_array)
         {
             return ConvertSystemArrayToNativePackedByteArray((ReadOnlySpan<byte>)p_array);
         }
 
-        public static unsafe godot_packed_byte_array ConvertSystemArrayToNativePackedByteArray(ReadOnlySpan<byte> p_array)
+        public static unsafe godot_packed_byte_array ConvertSystemArrayToNativePackedByteArray(scoped ReadOnlySpan<byte> p_array)
         {
             if (p_array.IsEmpty)
                 return new godot_packed_byte_array();
@@ -460,12 +460,12 @@ namespace Godot.NativeInterop
             return array;
         }
 
-        public static godot_packed_int32_array ConvertSystemArrayToNativePackedInt32Array(Span<int> p_array)
+        public static godot_packed_int32_array ConvertSystemArrayToNativePackedInt32Array(scoped Span<int> p_array)
         {
             return ConvertSystemArrayToNativePackedInt32Array((ReadOnlySpan<int>)p_array);
         }
 
-        public static unsafe godot_packed_int32_array ConvertSystemArrayToNativePackedInt32Array(ReadOnlySpan<int> p_array)
+        public static unsafe godot_packed_int32_array ConvertSystemArrayToNativePackedInt32Array(scoped ReadOnlySpan<int> p_array)
         {
             if (p_array.IsEmpty)
                 return new godot_packed_int32_array();
@@ -488,12 +488,12 @@ namespace Godot.NativeInterop
             return array;
         }
 
-        public static godot_packed_int64_array ConvertSystemArrayToNativePackedInt64Array(Span<long> p_array)
+        public static godot_packed_int64_array ConvertSystemArrayToNativePackedInt64Array(scoped Span<long> p_array)
         {
             return ConvertSystemArrayToNativePackedInt64Array((ReadOnlySpan<long>)p_array);
         }
 
-        public static unsafe godot_packed_int64_array ConvertSystemArrayToNativePackedInt64Array(ReadOnlySpan<long> p_array)
+        public static unsafe godot_packed_int64_array ConvertSystemArrayToNativePackedInt64Array(scoped ReadOnlySpan<long> p_array)
         {
             if (p_array.IsEmpty)
                 return new godot_packed_int64_array();
@@ -516,13 +516,12 @@ namespace Godot.NativeInterop
             return array;
         }
 
-        public static godot_packed_float32_array ConvertSystemArrayToNativePackedFloat32Array(Span<float> p_array)
+        public static godot_packed_float32_array ConvertSystemArrayToNativePackedFloat32Array(scoped Span<float> p_array)
         {
             return ConvertSystemArrayToNativePackedFloat32Array((ReadOnlySpan<float>)p_array);
         }
 
-        public static unsafe godot_packed_float32_array ConvertSystemArrayToNativePackedFloat32Array(
-            ReadOnlySpan<float> p_array)
+        public static unsafe godot_packed_float32_array ConvertSystemArrayToNativePackedFloat32Array(scoped ReadOnlySpan<float> p_array)
         {
             if (p_array.IsEmpty)
                 return new godot_packed_float32_array();
@@ -545,13 +544,12 @@ namespace Godot.NativeInterop
             return array;
         }
 
-        public static godot_packed_float64_array ConvertSystemArrayToNativePackedFloat64Array(Span<double> p_array)
+        public static godot_packed_float64_array ConvertSystemArrayToNativePackedFloat64Array(scoped Span<double> p_array)
         {
             return ConvertSystemArrayToNativePackedFloat64Array((ReadOnlySpan<double>)p_array);
         }
 
-        public static unsafe godot_packed_float64_array ConvertSystemArrayToNativePackedFloat64Array(
-            ReadOnlySpan<double> p_array)
+        public static unsafe godot_packed_float64_array ConvertSystemArrayToNativePackedFloat64Array(scoped ReadOnlySpan<double> p_array)
         {
             if (p_array.IsEmpty)
                 return new godot_packed_float64_array();
@@ -573,12 +571,12 @@ namespace Godot.NativeInterop
             return array;
         }
 
-        public static godot_packed_string_array ConvertSystemArrayToNativePackedStringArray(Span<string> p_array)
+        public static godot_packed_string_array ConvertSystemArrayToNativePackedStringArray(scoped Span<string> p_array)
         {
             return ConvertSystemArrayToNativePackedStringArray((ReadOnlySpan<string>)p_array);
         }
 
-        public static godot_packed_string_array ConvertSystemArrayToNativePackedStringArray(ReadOnlySpan<string> p_array)
+        public static godot_packed_string_array ConvertSystemArrayToNativePackedStringArray(scoped ReadOnlySpan<string> p_array)
         {
             godot_packed_string_array dest = new godot_packed_string_array();
 
@@ -612,13 +610,12 @@ namespace Godot.NativeInterop
             return array;
         }
 
-        public static godot_packed_vector2_array ConvertSystemArrayToNativePackedVector2Array(Span<Vector2> p_array)
+        public static godot_packed_vector2_array ConvertSystemArrayToNativePackedVector2Array(scoped Span<Vector2> p_array)
         {
             return ConvertSystemArrayToNativePackedVector2Array((ReadOnlySpan<Vector2>)p_array);
         }
 
-        public static unsafe godot_packed_vector2_array ConvertSystemArrayToNativePackedVector2Array(
-            ReadOnlySpan<Vector2> p_array)
+        public static unsafe godot_packed_vector2_array ConvertSystemArrayToNativePackedVector2Array(scoped ReadOnlySpan<Vector2> p_array)
         {
             if (p_array.IsEmpty)
                 return new godot_packed_vector2_array();
@@ -641,13 +638,12 @@ namespace Godot.NativeInterop
             return array;
         }
 
-        public static godot_packed_vector3_array ConvertSystemArrayToNativePackedVector3Array(Span<Vector3> p_array)
+        public static godot_packed_vector3_array ConvertSystemArrayToNativePackedVector3Array(scoped Span<Vector3> p_array)
         {
             return ConvertSystemArrayToNativePackedVector3Array((ReadOnlySpan<Vector3>)p_array);
         }
 
-        public static unsafe godot_packed_vector3_array ConvertSystemArrayToNativePackedVector3Array(
-            ReadOnlySpan<Vector3> p_array)
+        public static unsafe godot_packed_vector3_array ConvertSystemArrayToNativePackedVector3Array(scoped ReadOnlySpan<Vector3> p_array)
         {
             if (p_array.IsEmpty)
                 return new godot_packed_vector3_array();
@@ -670,13 +666,12 @@ namespace Godot.NativeInterop
             return array;
         }
 
-        public static godot_packed_vector4_array ConvertSystemArrayToNativePackedVector4Array(Span<Vector4> p_array)
+        public static godot_packed_vector4_array ConvertSystemArrayToNativePackedVector4Array(scoped Span<Vector4> p_array)
         {
             return ConvertSystemArrayToNativePackedVector4Array((ReadOnlySpan<Vector4>)p_array);
         }
 
-        public static unsafe godot_packed_vector4_array ConvertSystemArrayToNativePackedVector4Array(
-            ReadOnlySpan<Vector4> p_array)
+        public static unsafe godot_packed_vector4_array ConvertSystemArrayToNativePackedVector4Array(scoped ReadOnlySpan<Vector4> p_array)
         {
             if (p_array.IsEmpty)
                 return new godot_packed_vector4_array();
@@ -699,12 +694,12 @@ namespace Godot.NativeInterop
             return array;
         }
 
-        public static godot_packed_color_array ConvertSystemArrayToNativePackedColorArray(Span<Color> p_array)
+        public static godot_packed_color_array ConvertSystemArrayToNativePackedColorArray(scoped Span<Color> p_array)
         {
             return ConvertSystemArrayToNativePackedColorArray((ReadOnlySpan<Color>)p_array);
         }
 
-        public static unsafe godot_packed_color_array ConvertSystemArrayToNativePackedColorArray(ReadOnlySpan<Color> p_array)
+        public static unsafe godot_packed_color_array ConvertSystemArrayToNativePackedColorArray(scoped ReadOnlySpan<Color> p_array)
         {
             if (p_array.IsEmpty)
                 return new godot_packed_color_array();
diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/NativeFuncs.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/NativeFuncs.cs
index 47998c8017..8c2bc7941b 100644
--- a/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/NativeFuncs.cs
+++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/NativeFuncs.cs
@@ -101,10 +101,10 @@ namespace Godot.NativeInterop
 
         internal static partial void godotsharp_internal_reload_registered_script(IntPtr scriptPtr);
 
-        internal static partial void godotsharp_array_filter_godot_objects_by_native(in godot_string_name p_native_name,
-            in godot_array p_input, out godot_array r_output);
+        internal static partial void godotsharp_array_filter_godot_objects_by_native(scoped in godot_string_name p_native_name,
+            scoped in godot_array p_input, out godot_array r_output);
 
-        internal static partial void godotsharp_array_filter_godot_objects_by_non_native(in godot_array p_input,
+        internal static partial void godotsharp_array_filter_godot_objects_by_non_native(scoped in godot_array p_input,
             out godot_array r_output);
 
         public static partial void godotsharp_ref_new_from_ref_counted_ptr(out godot_ref r_dest,
@@ -113,15 +113,15 @@ namespace Godot.NativeInterop
         public static partial void godotsharp_ref_destroy(ref godot_ref p_instance);
 
         public static partial void godotsharp_string_name_new_from_string(out godot_string_name r_dest,
-            in godot_string p_name);
+            scoped in godot_string p_name);
 
         public static partial void godotsharp_node_path_new_from_string(out godot_node_path r_dest,
-            in godot_string p_name);
+            scoped in godot_string p_name);
 
         public static partial void
-            godotsharp_string_name_as_string(out godot_string r_dest, in godot_string_name p_name);
+            godotsharp_string_name_as_string(out godot_string r_dest, scoped in godot_string_name p_name);
 
-        public static partial void godotsharp_node_path_as_string(out godot_string r_dest, in godot_node_path p_np);
+        public static partial void godotsharp_node_path_as_string(out godot_string r_dest, scoped in godot_node_path p_np);
 
         public static partial godot_packed_byte_array godotsharp_packed_byte_array_new_mem_copy(byte* p_src,
             int p_length);
@@ -156,10 +156,10 @@ namespace Godot.NativeInterop
         public static partial void godotsharp_callable_new_with_delegate(IntPtr p_delegate_handle, IntPtr p_trampoline,
             IntPtr p_object, out godot_callable r_callable);
 
-        internal static partial godot_bool godotsharp_callable_get_data_for_marshalling(in godot_callable p_callable,
+        internal static partial godot_bool godotsharp_callable_get_data_for_marshalling(scoped in godot_callable p_callable,
             out IntPtr r_delegate_handle, out IntPtr r_trampoline, out IntPtr r_object, out godot_string_name r_name);
 
-        internal static partial godot_variant godotsharp_callable_call(in godot_callable p_callable,
+        internal static partial godot_variant godotsharp_callable_call(scoped in godot_callable p_callable,
             godot_variant** p_args, int p_arg_count, out godot_variant_call_error p_call_error);
 
         internal static partial void godotsharp_callable_call_deferred(in godot_callable p_callable,
@@ -180,58 +180,58 @@ namespace Godot.NativeInterop
         // variant.h
 
         public static partial void
-            godotsharp_variant_new_string_name(out godot_variant r_dest, in godot_string_name p_s);
+            godotsharp_variant_new_string_name(out godot_variant r_dest, scoped in godot_string_name p_s);
 
-        public static partial void godotsharp_variant_new_copy(out godot_variant r_dest, in godot_variant p_src);
+        public static partial void godotsharp_variant_new_copy(out godot_variant r_dest, scoped in godot_variant p_src);
 
-        public static partial void godotsharp_variant_new_node_path(out godot_variant r_dest, in godot_node_path p_np);
+        public static partial void godotsharp_variant_new_node_path(out godot_variant r_dest, scoped in godot_node_path p_np);
 
         public static partial void godotsharp_variant_new_object(out godot_variant r_dest, IntPtr p_obj);
 
-        public static partial void godotsharp_variant_new_transform2d(out godot_variant r_dest, in Transform2D p_t2d);
+        public static partial void godotsharp_variant_new_transform2d(out godot_variant r_dest, scoped in Transform2D p_t2d);
 
-        public static partial void godotsharp_variant_new_basis(out godot_variant r_dest, in Basis p_basis);
+        public static partial void godotsharp_variant_new_basis(out godot_variant r_dest, scoped in Basis p_basis);
 
-        public static partial void godotsharp_variant_new_transform3d(out godot_variant r_dest, in Transform3D p_trans);
+        public static partial void godotsharp_variant_new_transform3d(out godot_variant r_dest, scoped in Transform3D p_trans);
 
-        public static partial void godotsharp_variant_new_projection(out godot_variant r_dest, in Projection p_proj);
+        public static partial void godotsharp_variant_new_projection(out godot_variant r_dest, scoped in Projection p_proj);
 
-        public static partial void godotsharp_variant_new_aabb(out godot_variant r_dest, in Aabb p_aabb);
+        public static partial void godotsharp_variant_new_aabb(out godot_variant r_dest, scoped in Aabb p_aabb);
 
         public static partial void godotsharp_variant_new_dictionary(out godot_variant r_dest,
-            in godot_dictionary p_dict);
+            scoped in godot_dictionary p_dict);
 
-        public static partial void godotsharp_variant_new_array(out godot_variant r_dest, in godot_array p_arr);
+        public static partial void godotsharp_variant_new_array(out godot_variant r_dest, scoped in godot_array p_arr);
 
         public static partial void godotsharp_variant_new_packed_byte_array(out godot_variant r_dest,
-            in godot_packed_byte_array p_pba);
+            scoped in godot_packed_byte_array p_pba);
 
         public static partial void godotsharp_variant_new_packed_int32_array(out godot_variant r_dest,
-            in godot_packed_int32_array p_pia);
+            scoped in godot_packed_int32_array p_pia);
 
         public static partial void godotsharp_variant_new_packed_int64_array(out godot_variant r_dest,
-            in godot_packed_int64_array p_pia);
+            scoped in godot_packed_int64_array p_pia);
 
         public static partial void godotsharp_variant_new_packed_float32_array(out godot_variant r_dest,
-            in godot_packed_float32_array p_pra);
+            scoped in godot_packed_float32_array p_pra);
 
         public static partial void godotsharp_variant_new_packed_float64_array(out godot_variant r_dest,
-            in godot_packed_float64_array p_pra);
+            scoped in godot_packed_float64_array p_pra);
 
         public static partial void godotsharp_variant_new_packed_string_array(out godot_variant r_dest,
-            in godot_packed_string_array p_psa);
+            scoped in godot_packed_string_array p_psa);
 
         public static partial void godotsharp_variant_new_packed_vector2_array(out godot_variant r_dest,
-            in godot_packed_vector2_array p_pv2a);
+            scoped in godot_packed_vector2_array p_pv2a);
 
         public static partial void godotsharp_variant_new_packed_vector3_array(out godot_variant r_dest,
-            in godot_packed_vector3_array p_pv3a);
+            scoped in godot_packed_vector3_array p_pv3a);
 
         public static partial void godotsharp_variant_new_packed_vector4_array(out godot_variant r_dest,
-            in godot_packed_vector4_array p_pv4a);
+            scoped in godot_packed_vector4_array p_pv4a);
 
         public static partial void godotsharp_variant_new_packed_color_array(out godot_variant r_dest,
-            in godot_packed_color_array p_pca);
+            scoped in godot_packed_color_array p_pca);
 
         public static partial godot_bool godotsharp_variant_as_bool(in godot_variant p_self);
 
@@ -239,7 +239,7 @@ namespace Godot.NativeInterop
 
         public static partial double godotsharp_variant_as_float(in godot_variant p_self);
 
-        public static partial godot_string godotsharp_variant_as_string(in godot_variant p_self);
+        public static partial godot_string godotsharp_variant_as_string(scoped in godot_variant p_self);
 
         public static partial Vector2 godotsharp_variant_as_vector2(in godot_variant p_self);
 
@@ -273,45 +273,45 @@ namespace Godot.NativeInterop
 
         public static partial Color godotsharp_variant_as_color(in godot_variant p_self);
 
-        public static partial godot_string_name godotsharp_variant_as_string_name(in godot_variant p_self);
+        public static partial godot_string_name godotsharp_variant_as_string_name(scoped in godot_variant p_self);
 
-        public static partial godot_node_path godotsharp_variant_as_node_path(in godot_variant p_self);
+        public static partial godot_node_path godotsharp_variant_as_node_path(scoped in godot_variant p_self);
 
         public static partial Rid godotsharp_variant_as_rid(in godot_variant p_self);
 
-        public static partial godot_callable godotsharp_variant_as_callable(in godot_variant p_self);
+        public static partial godot_callable godotsharp_variant_as_callable(scoped in godot_variant p_self);
 
-        public static partial godot_signal godotsharp_variant_as_signal(in godot_variant p_self);
+        public static partial godot_signal godotsharp_variant_as_signal(scoped in godot_variant p_self);
 
-        public static partial godot_dictionary godotsharp_variant_as_dictionary(in godot_variant p_self);
+        public static partial godot_dictionary godotsharp_variant_as_dictionary(scoped in godot_variant p_self);
 
-        public static partial godot_array godotsharp_variant_as_array(in godot_variant p_self);
+        public static partial godot_array godotsharp_variant_as_array(scoped in godot_variant p_self);
 
-        public static partial godot_packed_byte_array godotsharp_variant_as_packed_byte_array(in godot_variant p_self);
+        public static partial godot_packed_byte_array godotsharp_variant_as_packed_byte_array(scoped in godot_variant p_self);
 
-        public static partial godot_packed_int32_array godotsharp_variant_as_packed_int32_array(in godot_variant p_self);
+        public static partial godot_packed_int32_array godotsharp_variant_as_packed_int32_array(scoped in godot_variant p_self);
 
-        public static partial godot_packed_int64_array godotsharp_variant_as_packed_int64_array(in godot_variant p_self);
+        public static partial godot_packed_int64_array godotsharp_variant_as_packed_int64_array(scoped in godot_variant p_self);
 
         public static partial godot_packed_float32_array godotsharp_variant_as_packed_float32_array(
-            in godot_variant p_self);
+            scoped in godot_variant p_self);
 
         public static partial godot_packed_float64_array godotsharp_variant_as_packed_float64_array(
-            in godot_variant p_self);
+            scoped in godot_variant p_self);
 
         public static partial godot_packed_string_array godotsharp_variant_as_packed_string_array(
-            in godot_variant p_self);
+            scoped in godot_variant p_self);
 
         public static partial godot_packed_vector2_array godotsharp_variant_as_packed_vector2_array(
-            in godot_variant p_self);
+            scoped in godot_variant p_self);
 
         public static partial godot_packed_vector3_array godotsharp_variant_as_packed_vector3_array(
-            in godot_variant p_self);
+            scoped in godot_variant p_self);
 
         public static partial godot_packed_vector4_array godotsharp_variant_as_packed_vector4_array(
             in godot_variant p_self);
 
-        public static partial godot_packed_color_array godotsharp_variant_as_packed_color_array(in godot_variant p_self);
+        public static partial godot_packed_color_array godotsharp_variant_as_packed_color_array(scoped in godot_variant p_self);
 
         public static partial godot_bool godotsharp_variant_equals(in godot_variant p_a, in godot_variant p_b);
 
@@ -322,17 +322,17 @@ namespace Godot.NativeInterop
         // string_name.h
 
         public static partial void godotsharp_string_name_new_copy(out godot_string_name r_dest,
-            in godot_string_name p_src);
+            scoped in godot_string_name p_src);
 
         // node_path.h
 
-        public static partial void godotsharp_node_path_new_copy(out godot_node_path r_dest, in godot_node_path p_src);
+        public static partial void godotsharp_node_path_new_copy(out godot_node_path r_dest, scoped in godot_node_path p_src);
 
         // array.h
 
         public static partial void godotsharp_array_new(out godot_array r_dest);
 
-        public static partial void godotsharp_array_new_copy(out godot_array r_dest, in godot_array p_src);
+        public static partial void godotsharp_array_new_copy(out godot_array r_dest, scoped in godot_array p_src);
 
         public static partial godot_variant* godotsharp_array_ptrw(ref godot_array p_self);
 
@@ -341,7 +341,7 @@ namespace Godot.NativeInterop
         public static partial void godotsharp_dictionary_new(out godot_dictionary r_dest);
 
         public static partial void godotsharp_dictionary_new_copy(out godot_dictionary r_dest,
-            in godot_dictionary p_src);
+            scoped in godot_dictionary p_src);
 
         // destroy functions
 
@@ -389,8 +389,7 @@ namespace Godot.NativeInterop
 
         public static partial int godotsharp_array_binary_search(ref godot_array p_self, int p_index, int p_count, in godot_variant p_value);
 
-        public static partial void
-            godotsharp_array_duplicate(ref godot_array p_self, godot_bool p_deep, out godot_array r_dest);
+        public static partial void godotsharp_array_duplicate(scoped ref godot_array p_self, godot_bool p_deep, out godot_array r_dest);
 
         public static partial void godotsharp_array_fill(ref godot_array p_self, in godot_variant p_value);
 
@@ -410,11 +409,11 @@ namespace Godot.NativeInterop
 
         public static partial godot_bool godotsharp_array_is_typed(ref godot_array p_self);
 
-        public static partial void godotsharp_array_max(ref godot_array p_self, out godot_variant r_value);
+        public static partial void godotsharp_array_max(scoped ref godot_array p_self, out godot_variant r_value);
 
-        public static partial void godotsharp_array_min(ref godot_array p_self, out godot_variant r_value);
+        public static partial void godotsharp_array_min(scoped ref godot_array p_self, out godot_variant r_value);
 
-        public static partial void godotsharp_array_pick_random(ref godot_array p_self, out godot_variant r_value);
+        public static partial void godotsharp_array_pick_random(scoped ref godot_array p_self, out godot_variant r_value);
 
         public static partial godot_bool godotsharp_array_recursive_equal(ref godot_array p_self, in godot_array p_other);
 
@@ -426,7 +425,7 @@ namespace Godot.NativeInterop
 
         public static partial void godotsharp_array_shuffle(ref godot_array p_self);
 
-        public static partial void godotsharp_array_slice(ref godot_array p_self, int p_start, int p_end,
+        public static partial void godotsharp_array_slice(scoped ref godot_array p_self, int p_start, int p_end,
             int p_step, godot_bool p_deep, out godot_array r_dest);
 
         public static partial void godotsharp_array_sort(ref godot_array p_self);
@@ -435,20 +434,20 @@ namespace Godot.NativeInterop
 
         // Dictionary
 
-        public static partial godot_bool godotsharp_dictionary_try_get_value(ref godot_dictionary p_self,
-            in godot_variant p_key,
+        public static partial godot_bool godotsharp_dictionary_try_get_value(scoped ref godot_dictionary p_self,
+            scoped in godot_variant p_key,
             out godot_variant r_value);
 
         public static partial void godotsharp_dictionary_set_value(ref godot_dictionary p_self, in godot_variant p_key,
             in godot_variant p_value);
 
-        public static partial void godotsharp_dictionary_keys(ref godot_dictionary p_self, out godot_array r_dest);
+        public static partial void godotsharp_dictionary_keys(scoped ref godot_dictionary p_self, out godot_array r_dest);
 
-        public static partial void godotsharp_dictionary_values(ref godot_dictionary p_self, out godot_array r_dest);
+        public static partial void godotsharp_dictionary_values(scoped ref godot_dictionary p_self, out godot_array r_dest);
 
         public static partial int godotsharp_dictionary_count(ref godot_dictionary p_self);
 
-        public static partial void godotsharp_dictionary_key_value_pair_at(ref godot_dictionary p_self, int p_index,
+        public static partial void godotsharp_dictionary_key_value_pair_at(scoped ref godot_dictionary p_self, int p_index,
             out godot_variant r_key, out godot_variant r_value);
 
         public static partial void godotsharp_dictionary_add(ref godot_dictionary p_self, in godot_variant p_key,
@@ -459,7 +458,7 @@ namespace Godot.NativeInterop
         public static partial godot_bool godotsharp_dictionary_contains_key(ref godot_dictionary p_self,
             in godot_variant p_key);
 
-        public static partial void godotsharp_dictionary_duplicate(ref godot_dictionary p_self, godot_bool p_deep,
+        public static partial void godotsharp_dictionary_duplicate(scoped ref godot_dictionary p_self, godot_bool p_deep,
             out godot_dictionary r_dest);
 
         public static partial void godotsharp_dictionary_merge(ref godot_dictionary p_self, in godot_dictionary p_dictionary, godot_bool p_overwrite);
@@ -496,20 +495,20 @@ namespace Godot.NativeInterop
 
         public static partial void godotsharp_dictionary_get_typed_value_script(ref godot_dictionary p_self, out godot_variant r_dest);
 
-        public static partial void godotsharp_dictionary_to_string(ref godot_dictionary p_self, out godot_string r_str);
+        public static partial void godotsharp_dictionary_to_string(scoped ref godot_dictionary p_self, out godot_string r_str);
 
         // StringExtensions
 
-        public static partial void godotsharp_string_simplify_path(in godot_string p_self,
+        public static partial void godotsharp_string_simplify_path(scoped in godot_string p_self,
             out godot_string r_simplified_path);
 
-        public static partial void godotsharp_string_to_camel_case(in godot_string p_self,
+        public static partial void godotsharp_string_to_camel_case(scoped in godot_string p_self,
             out godot_string r_camel_case);
 
-        public static partial void godotsharp_string_to_pascal_case(in godot_string p_self,
+        public static partial void godotsharp_string_to_pascal_case(scoped in godot_string p_self,
             out godot_string r_pascal_case);
 
-        public static partial void godotsharp_string_to_snake_case(in godot_string p_self,
+        public static partial void godotsharp_string_to_snake_case(scoped in godot_string p_self,
             out godot_string r_snake_case);
 
         // NodePath
@@ -517,18 +516,18 @@ namespace Godot.NativeInterop
         public static partial void godotsharp_node_path_get_as_property_path(in godot_node_path p_self,
             ref godot_node_path r_dest);
 
-        public static partial void godotsharp_node_path_get_concatenated_names(in godot_node_path p_self,
+        public static partial void godotsharp_node_path_get_concatenated_names(scoped in godot_node_path p_self,
             out godot_string r_names);
 
-        public static partial void godotsharp_node_path_get_concatenated_subnames(in godot_node_path p_self,
+        public static partial void godotsharp_node_path_get_concatenated_subnames(scoped in godot_node_path p_self,
             out godot_string r_subnames);
 
-        public static partial void godotsharp_node_path_get_name(in godot_node_path p_self, int p_idx,
+        public static partial void godotsharp_node_path_get_name(scoped in godot_node_path p_self, int p_idx,
             out godot_string r_name);
 
         public static partial int godotsharp_node_path_get_name_count(in godot_node_path p_self);
 
-        public static partial void godotsharp_node_path_get_subname(in godot_node_path p_self, int p_idx,
+        public static partial void godotsharp_node_path_get_subname(scoped in godot_node_path p_self, int p_idx,
             out godot_string r_subname);
 
         public static partial int godotsharp_node_path_get_subname_count(in godot_node_path p_self);
@@ -541,11 +540,11 @@ namespace Godot.NativeInterop
 
         // GD, etc
 
-        internal static partial void godotsharp_bytes_to_var(in godot_packed_byte_array p_bytes,
+        internal static partial void godotsharp_bytes_to_var(scoped in godot_packed_byte_array p_bytes,
             godot_bool p_allow_objects,
             out godot_variant r_ret);
 
-        internal static partial void godotsharp_convert(in godot_variant p_what, int p_type,
+        internal static partial void godotsharp_convert(scoped in godot_variant p_what, int p_type,
             out godot_variant r_ret);
 
         internal static partial int godotsharp_hash(in godot_variant p_var);
@@ -582,12 +581,12 @@ namespace Godot.NativeInterop
 
         internal static partial void godotsharp_weakref(IntPtr p_obj, out godot_ref r_weak_ref);
 
-        internal static partial void godotsharp_str_to_var(in godot_string p_str, out godot_variant r_ret);
+        internal static partial void godotsharp_str_to_var(scoped in godot_string p_str, out godot_variant r_ret);
 
-        internal static partial void godotsharp_var_to_bytes(in godot_variant p_what, godot_bool p_full_objects,
+        internal static partial void godotsharp_var_to_bytes(scoped in godot_variant p_what, godot_bool p_full_objects,
             out godot_packed_byte_array r_bytes);
 
-        internal static partial void godotsharp_var_to_str(in godot_variant p_var, out godot_string r_ret);
+        internal static partial void godotsharp_var_to_str(scoped in godot_variant p_var, out godot_string r_ret);
 
         internal static partial void godotsharp_err_print_error(in godot_string p_function, in godot_string p_file, int p_line, in godot_string p_error, in godot_string p_message = default, godot_bool p_editor_notify = godot_bool.False, godot_error_handler_type p_type = godot_error_handler_type.ERR_HANDLER_ERROR);
 
diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/NativeFuncs.extended.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/NativeFuncs.extended.cs
index 9f237e4d00..b885893de0 100644
--- a/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/NativeFuncs.extended.cs
+++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/NativeFuncs.extended.cs
@@ -6,7 +6,7 @@ namespace Godot.NativeInterop
 {
     public static partial class NativeFuncs
     {
-        public static godot_variant godotsharp_variant_new_copy(in godot_variant src)
+        public static godot_variant godotsharp_variant_new_copy(scoped in godot_variant src)
         {
             switch (src.Type)
             {
@@ -48,7 +48,7 @@ namespace Godot.NativeInterop
             return ret;
         }
 
-        public static godot_string_name godotsharp_string_name_new_copy(in godot_string_name src)
+        public static godot_string_name godotsharp_string_name_new_copy(scoped in godot_string_name src)
         {
             if (src.IsEmpty)
                 return default;
@@ -56,7 +56,7 @@ namespace Godot.NativeInterop
             return ret;
         }
 
-        public static godot_node_path godotsharp_node_path_new_copy(in godot_node_path src)
+        public static godot_node_path godotsharp_node_path_new_copy(scoped in godot_node_path src)
         {
             if (src.IsEmpty)
                 return default;
@@ -70,7 +70,7 @@ namespace Godot.NativeInterop
             return ret;
         }
 
-        public static godot_array godotsharp_array_new_copy(in godot_array src)
+        public static godot_array godotsharp_array_new_copy(scoped in godot_array src)
         {
             godotsharp_array_new_copy(out godot_array ret, src);
             return ret;
@@ -82,7 +82,7 @@ namespace Godot.NativeInterop
             return ret;
         }
 
-        public static godot_dictionary godotsharp_dictionary_new_copy(in godot_dictionary src)
+        public static godot_dictionary godotsharp_dictionary_new_copy(scoped in godot_dictionary src)
         {
             godotsharp_dictionary_new_copy(out godot_dictionary ret, src);
             return ret;
diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/VariantUtils.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/VariantUtils.cs
index 222ded6895..5ff9969cfd 100644
--- a/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/VariantUtils.cs
+++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/VariantUtils.cs
@@ -117,159 +117,144 @@ namespace Godot.NativeInterop
         public static godot_variant CreateFromString(string? from)
             => CreateFromStringTakingOwnershipOfDisposableValue(Marshaling.ConvertStringToNative(from));
 
-        public static godot_variant CreateFromPackedByteArray(in godot_packed_byte_array from)
+        public static godot_variant CreateFromPackedByteArray(scoped in godot_packed_byte_array from)
         {
             NativeFuncs.godotsharp_variant_new_packed_byte_array(out godot_variant ret, from);
             return ret;
         }
 
-        public static godot_variant CreateFromPackedInt32Array(in godot_packed_int32_array from)
+        public static godot_variant CreateFromPackedInt32Array(scoped in godot_packed_int32_array from)
         {
             NativeFuncs.godotsharp_variant_new_packed_int32_array(out godot_variant ret, from);
             return ret;
         }
 
-        public static godot_variant CreateFromPackedInt64Array(in godot_packed_int64_array from)
+        public static godot_variant CreateFromPackedInt64Array(scoped in godot_packed_int64_array from)
         {
             NativeFuncs.godotsharp_variant_new_packed_int64_array(out godot_variant ret, from);
             return ret;
         }
 
-        public static godot_variant CreateFromPackedFloat32Array(in godot_packed_float32_array from)
+        public static godot_variant CreateFromPackedFloat32Array(scoped in godot_packed_float32_array from)
         {
             NativeFuncs.godotsharp_variant_new_packed_float32_array(out godot_variant ret, from);
             return ret;
         }
 
-        public static godot_variant CreateFromPackedFloat64Array(in godot_packed_float64_array from)
+        public static godot_variant CreateFromPackedFloat64Array(scoped in godot_packed_float64_array from)
         {
             NativeFuncs.godotsharp_variant_new_packed_float64_array(out godot_variant ret, from);
             return ret;
         }
 
-        public static godot_variant CreateFromPackedStringArray(in godot_packed_string_array from)
+        public static godot_variant CreateFromPackedStringArray(scoped in godot_packed_string_array from)
         {
             NativeFuncs.godotsharp_variant_new_packed_string_array(out godot_variant ret, from);
             return ret;
         }
 
-        public static godot_variant CreateFromPackedVector2Array(in godot_packed_vector2_array from)
+        public static godot_variant CreateFromPackedVector2Array(scoped in godot_packed_vector2_array from)
         {
             NativeFuncs.godotsharp_variant_new_packed_vector2_array(out godot_variant ret, from);
             return ret;
         }
 
-        public static godot_variant CreateFromPackedVector3Array(in godot_packed_vector3_array from)
+        public static godot_variant CreateFromPackedVector3Array(scoped in godot_packed_vector3_array from)
         {
             NativeFuncs.godotsharp_variant_new_packed_vector3_array(out godot_variant ret, from);
             return ret;
         }
 
-        public static godot_variant CreateFromPackedVector4Array(in godot_packed_vector4_array from)
+        public static godot_variant CreateFromPackedVector4Array(scoped in godot_packed_vector4_array from)
         {
             NativeFuncs.godotsharp_variant_new_packed_vector4_array(out godot_variant ret, from);
             return ret;
         }
 
-        public static godot_variant CreateFromPackedColorArray(in godot_packed_color_array from)
+        public static godot_variant CreateFromPackedColorArray(scoped in godot_packed_color_array from)
         {
             NativeFuncs.godotsharp_variant_new_packed_color_array(out godot_variant ret, from);
             return ret;
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static godot_variant CreateFromPackedByteArray(Span<byte> from)
+        public static godot_variant CreateFromPackedByteArray(scoped Span<byte> from)
         {
             using var nativePackedArray = Marshaling.ConvertSystemArrayToNativePackedByteArray(from);
             return CreateFromPackedByteArray(nativePackedArray);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static godot_variant CreateFromPackedInt32Array(Span<int> from)
+        public static godot_variant CreateFromPackedInt32Array(scoped Span<int> from)
         {
             using var nativePackedArray = Marshaling.ConvertSystemArrayToNativePackedInt32Array(from);
             return CreateFromPackedInt32Array(nativePackedArray);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static godot_variant CreateFromPackedInt64Array(Span<long> from)
+        public static godot_variant CreateFromPackedInt64Array(scoped Span<long> from)
         {
             using var nativePackedArray = Marshaling.ConvertSystemArrayToNativePackedInt64Array(from);
             return CreateFromPackedInt64Array(nativePackedArray);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static godot_variant CreateFromPackedFloat32Array(Span<float> from)
+        public static godot_variant CreateFromPackedFloat32Array(scoped Span<float> from)
         {
             using var nativePackedArray = Marshaling.ConvertSystemArrayToNativePackedFloat32Array(from);
             return CreateFromPackedFloat32Array(nativePackedArray);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static godot_variant CreateFromPackedFloat64Array(Span<double> from)
+        public static godot_variant CreateFromPackedFloat64Array(scoped Span<double> from)
         {
             using var nativePackedArray = Marshaling.ConvertSystemArrayToNativePackedFloat64Array(from);
             return CreateFromPackedFloat64Array(nativePackedArray);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static godot_variant CreateFromPackedStringArray(Span<string> from)
+        public static godot_variant CreateFromPackedStringArray(scoped Span<string> from)
         {
             using var nativePackedArray = Marshaling.ConvertSystemArrayToNativePackedStringArray(from);
             return CreateFromPackedStringArray(nativePackedArray);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static godot_variant CreateFromPackedVector2Array(Span<Vector2> from)
+        public static godot_variant CreateFromPackedVector2Array(scoped Span<Vector2> from)
         {
             using var nativePackedArray = Marshaling.ConvertSystemArrayToNativePackedVector2Array(from);
             return CreateFromPackedVector2Array(nativePackedArray);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static godot_variant CreateFromPackedVector3Array(Span<Vector3> from)
+        public static godot_variant CreateFromPackedVector3Array(scoped Span<Vector3> from)
         {
             using var nativePackedArray = Marshaling.ConvertSystemArrayToNativePackedVector3Array(from);
             return CreateFromPackedVector3Array(nativePackedArray);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static godot_variant CreateFromPackedVector4Array(Span<Vector4> from)
+        public static godot_variant CreateFromPackedVector4Array(scoped Span<Vector4> from)
         {
             using var nativePackedArray = Marshaling.ConvertSystemArrayToNativePackedVector4Array(from);
             return CreateFromPackedVector4Array(nativePackedArray);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static godot_variant CreateFromPackedColorArray(Span<Color> from)
+        public static godot_variant CreateFromPackedColorArray(scoped Span<Color> from)
         {
             using var nativePackedArray = Marshaling.ConvertSystemArrayToNativePackedColorArray(from);
             return CreateFromPackedColorArray(nativePackedArray);
         }
 
-        public static godot_variant CreateFromSystemArrayOfStringName(Span<StringName> from)
-        {
-            if (from == null)
-                return default;
-            using var fromGodot = new Collections.Array(from);
-            return CreateFromArray((godot_array)fromGodot.NativeValue);
-        }
+        public static godot_variant CreateFromSystemArrayOfStringName(scoped Span<StringName> from)
+            => CreateFromArray(new Collections.Array(from));
 
-        public static godot_variant CreateFromSystemArrayOfNodePath(Span<NodePath> from)
-        {
-            if (from == null)
-                return default;
-            using var fromGodot = new Collections.Array(from);
-            return CreateFromArray((godot_array)fromGodot.NativeValue);
-        }
+        public static godot_variant CreateFromSystemArrayOfNodePath(scoped Span<NodePath> from)
+            => CreateFromArray(new Collections.Array(from));
 
-        public static godot_variant CreateFromSystemArrayOfRid(Span<Rid> from)
-        {
-            if (from == null)
-                return default;
-            using var fromGodot = new Collections.Array(from);
-            return CreateFromArray((godot_array)fromGodot.NativeValue);
-        }
+        public static godot_variant CreateFromSystemArrayOfRid(scoped Span<Rid> from)
+            => CreateFromArray(new Collections.Array(from));
 
         public static godot_variant CreateFromSystemArrayOfGodotObject(GodotObject[]? from)
         {
@@ -279,7 +264,7 @@ namespace Godot.NativeInterop
             return CreateFromArray((godot_array)fromGodot.NativeValue);
         }
 
-        public static godot_variant CreateFromArray(godot_array from)
+        public static godot_variant CreateFromArray(scoped in godot_array from)
         {
             NativeFuncs.godotsharp_variant_new_array(out godot_variant ret, from);
             return ret;
@@ -293,7 +278,7 @@ namespace Godot.NativeInterop
         public static godot_variant CreateFromArray<[MustBeVariant] T>(Array<T>? from)
             => from != null ? CreateFromArray((godot_array)((Collections.Array)from).NativeValue) : default;
 
-        public static godot_variant CreateFromDictionary(godot_dictionary from)
+        public static godot_variant CreateFromDictionary(scoped in godot_dictionary from)
         {
             NativeFuncs.godotsharp_variant_new_dictionary(out godot_variant ret, from);
             return ret;
@@ -307,7 +292,7 @@ namespace Godot.NativeInterop
         public static godot_variant CreateFromDictionary<[MustBeVariant] TKey, [MustBeVariant] TValue>(Dictionary<TKey, TValue>? from)
             => from != null ? CreateFromDictionary((godot_dictionary)((Dictionary)from).NativeValue) : default;
 
-        public static godot_variant CreateFromStringName(godot_string_name from)
+        public static godot_variant CreateFromStringName(scoped in godot_string_name from)
         {
             NativeFuncs.godotsharp_variant_new_string_name(out godot_variant ret, from);
             return ret;
@@ -317,7 +302,7 @@ namespace Godot.NativeInterop
         public static godot_variant CreateFromStringName(StringName? from)
             => from != null ? CreateFromStringName((godot_string_name)from.NativeValue) : default;
 
-        public static godot_variant CreateFromNodePath(godot_node_path from)
+        public static godot_variant CreateFromNodePath(scoped in godot_node_path from)
         {
             NativeFuncs.godotsharp_variant_new_node_path(out godot_variant ret, from);
             return ret;
@@ -510,7 +495,7 @@ namespace Godot.NativeInterop
             }
         }
 
-        public static godot_string_name ConvertToNativeStringName(in godot_variant p_var)
+        public static godot_string_name ConvertToNativeStringName(scoped in godot_variant p_var)
             => p_var.Type == Variant.Type.StringName ?
                 NativeFuncs.godotsharp_string_name_new_copy(p_var.StringName) :
                 NativeFuncs.godotsharp_variant_as_string_name(p_var);
@@ -519,7 +504,7 @@ namespace Godot.NativeInterop
         public static StringName ConvertToStringName(in godot_variant p_var)
             => StringName.CreateTakingOwnershipOfDisposableValue(ConvertToNativeStringName(p_var));
 
-        public static godot_node_path ConvertToNativeNodePath(in godot_variant p_var)
+        public static godot_node_path ConvertToNativeNodePath(scoped in godot_variant p_var)
             => p_var.Type == Variant.Type.NodePath ?
                 NativeFuncs.godotsharp_node_path_new_copy(p_var.NodePath) :
                 NativeFuncs.godotsharp_variant_as_node_path(p_var);
@@ -529,7 +514,7 @@ namespace Godot.NativeInterop
             => NodePath.CreateTakingOwnershipOfDisposableValue(ConvertToNativeNodePath(p_var));
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static godot_callable ConvertToNativeCallable(in godot_variant p_var)
+        public static godot_callable ConvertToNativeCallable(scoped in godot_variant p_var)
             => NativeFuncs.godotsharp_variant_as_callable(p_var);
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
@@ -540,7 +525,7 @@ namespace Godot.NativeInterop
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static godot_signal ConvertToNativeSignal(in godot_variant p_var)
+        public static godot_signal ConvertToNativeSignal(scoped in godot_variant p_var)
             => NativeFuncs.godotsharp_variant_as_signal(p_var);
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
@@ -550,7 +535,7 @@ namespace Godot.NativeInterop
             return Marshaling.ConvertSignalToManaged(signal);
         }
 
-        public static godot_array ConvertToNativeArray(in godot_variant p_var)
+        public static godot_array ConvertToNativeArray(scoped in godot_variant p_var)
             => p_var.Type == Variant.Type.Array ?
                 NativeFuncs.godotsharp_array_new_copy(p_var.Array) :
                 NativeFuncs.godotsharp_variant_as_array(p_var);
@@ -563,7 +548,7 @@ namespace Godot.NativeInterop
         public static Array<T> ConvertToArray<[MustBeVariant] T>(in godot_variant p_var)
             => Array<T>.CreateTakingOwnershipOfDisposableValue(ConvertToNativeArray(p_var));
 
-        public static godot_dictionary ConvertToNativeDictionary(in godot_variant p_var)
+        public static godot_dictionary ConvertToNativeDictionary(scoped in godot_variant p_var)
             => p_var.Type == Variant.Type.Dictionary ?
                 NativeFuncs.godotsharp_dictionary_new_copy(p_var.Dictionary) :
                 NativeFuncs.godotsharp_variant_as_dictionary(p_var);
diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/VariantUtils.generic.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/VariantUtils.generic.cs
index 2897cc4199..a5ef11404f 100644
--- a/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/VariantUtils.generic.cs
+++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/VariantUtils.generic.cs
@@ -12,8 +12,10 @@ public partial class VariantUtils
 
     internal static class GenericConversion<T>
     {
-        public static unsafe godot_variant ToVariant(in T from) =>
+        public static unsafe godot_variant ToVariant(scoped in T from) =>
+#pragma warning disable CS9088 // the delegate pointer cannot be marked scoped, but it should be
             ToVariantCb != null ? ToVariantCb(from) : throw UnsupportedType<T>();
+#pragma warning restore CS9088
 
         public static unsafe T FromVariant(in godot_variant variant) =>
             FromVariantCb != null ? FromVariantCb(variant) : throw UnsupportedType<T>();
@@ -31,7 +33,7 @@ public partial class VariantUtils
     }
 
     [MethodImpl(MethodImplOptions.AggressiveInlining | MethodImplOptions.AggressiveOptimization)]
-    public static godot_variant CreateFrom<[MustBeVariant] T>(in T from)
+    public static godot_variant CreateFrom<[MustBeVariant] T>(scoped in T from)
     {
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         static TTo UnsafeAs<TTo>(in T f) => Unsafe.As<T, TTo>(ref Unsafe.AsRef(f));
diff --git a/modules/mono/glue/GodotSharp/GodotSharp/GodotSharp.csproj b/modules/mono/glue/GodotSharp/GodotSharp/GodotSharp.csproj
index 5aa68559d8..09516cc893 100644
--- a/modules/mono/glue/GodotSharp/GodotSharp/GodotSharp.csproj
+++ b/modules/mono/glue/GodotSharp/GodotSharp/GodotSharp.csproj
@@ -4,11 +4,11 @@
     <OutputPath>bin/$(Configuration)</OutputPath>
     <AppendTargetFrameworkToOutputPath>false</AppendTargetFrameworkToOutputPath>
     <RootNamespace>Godot</RootNamespace>
-    <TargetFramework>net6.0</TargetFramework>
+    <TargetFramework>net7.0</TargetFramework>
     <DocumentationFile>$(OutputPath)/$(AssemblyName).xml</DocumentationFile>
     <EnableDefaultItems>false</EnableDefaultItems>
     <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
-    <LangVersion>10</LangVersion>
+    <LangVersion>11</LangVersion>
 
     <AnalysisMode>Recommended</AnalysisMode>
   </PropertyGroup>
diff --git a/modules/mono/glue/GodotSharp/GodotSharpEditor/GodotSharpEditor.csproj b/modules/mono/glue/GodotSharp/GodotSharpEditor/GodotSharpEditor.csproj
index 715c1a4d51..fcc6bd7a4d 100644
--- a/modules/mono/glue/GodotSharp/GodotSharpEditor/GodotSharpEditor.csproj
+++ b/modules/mono/glue/GodotSharp/GodotSharpEditor/GodotSharpEditor.csproj
@@ -4,7 +4,7 @@
     <OutputPath>bin/$(Configuration)</OutputPath>
     <AppendTargetFrameworkToOutputPath>false</AppendTargetFrameworkToOutputPath>
     <RootNamespace>Godot</RootNamespace>
-    <TargetFramework>net6.0</TargetFramework>
+    <TargetFramework>net7.0</TargetFramework>
     <DocumentationFile>$(OutputPath)/$(AssemblyName).xml</DocumentationFile>
     <EnableDefaultItems>false</EnableDefaultItems>
     <AllowUnsafeBlocks>true</AllowUnsafeBlocks>

From 85e4d1b9d3860372c77eedeae7b2de59b87dc0e3 Mon Sep 17 00:00:00 2001
From: BlueCube3310 <53150244+BlueCube3310@users.noreply.github.com>
Date: Thu, 12 Dec 2024 17:43:47 +0100
Subject: [PATCH 103/124] LightmapGI: Automatically compress new lightmaps

---
 scene/3d/lightmap_gi.cpp | 21 +++++++++++----------
 scene/3d/lightmap_gi.h   |  2 +-
 2 files changed, 12 insertions(+), 11 deletions(-)

diff --git a/scene/3d/lightmap_gi.cpp b/scene/3d/lightmap_gi.cpp
index 4c3d1a8218..752db3f3d0 100644
--- a/scene/3d/lightmap_gi.cpp
+++ b/scene/3d/lightmap_gi.cpp
@@ -804,7 +804,7 @@ void LightmapGI::_gen_new_positions_from_octree(const GenProbesOctree *p_cell, f
 	}
 }
 
-LightmapGI::BakeError LightmapGI::_save_and_reimport_atlas_textures(const Ref<Lightmapper> p_lightmapper, const String &p_base_name, TypedArray<TextureLayered> &r_textures, bool p_is_shadowmask, bool p_compress) const {
+LightmapGI::BakeError LightmapGI::_save_and_reimport_atlas_textures(const Ref<Lightmapper> p_lightmapper, const String &p_base_name, TypedArray<TextureLayered> &r_textures, bool p_is_shadowmask) const {
 	Vector<Ref<Image>> images;
 	images.resize(p_is_shadowmask ? p_lightmapper->get_shadowmask_texture_count() : p_lightmapper->get_bake_texture_count());
 
@@ -846,7 +846,7 @@ LightmapGI::BakeError LightmapGI::_save_and_reimport_atlas_textures(const Ref<Li
 		config->set_value("remap", "type", "CompressedTexture2DArray");
 		if (!config->has_section_key("params", "compress/mode")) {
 			// Do not override an existing compression mode.
-			config->set_value("params", "compress/mode", p_compress ? 2 : 3);
+			config->set_value("params", "compress/mode", 2);
 		}
 		config->set_value("params", "compress/channel_pack", 1);
 		config->set_value("params", "mipmaps/generate", false);
@@ -1252,7 +1252,9 @@ LightmapGI::BakeError LightmapGI::bake(Node *p_from_node, String p_image_data_pa
 		}
 	}
 
-	Lightmapper::BakeError bake_err = lightmapper->bake(Lightmapper::BakeQuality(bake_quality), use_denoiser, denoiser_strength, denoiser_range, bounces, bounce_indirect_energy, bias, max_texture_size, directional, shadowmask_mode != LightmapGIData::SHADOWMASK_MODE_NONE, use_texture_for_bounces, Lightmapper::GenerateProbes(gen_probes), environment_image, environment_transform, _lightmap_bake_step_function, &bsud, exposure_normalization);
+	Lightmapper::BakeError bake_err = lightmapper->bake(Lightmapper::BakeQuality(bake_quality), use_denoiser, denoiser_strength, denoiser_range, bounces,
+			bounce_indirect_energy, bias, max_texture_size, directional, shadowmask_mode != LightmapGIData::SHADOWMASK_MODE_NONE, use_texture_for_bounces,
+			Lightmapper::GenerateProbes(gen_probes), environment_image, environment_transform, _lightmap_bake_step_function, &bsud, exposure_normalization);
 
 	if (bake_err == Lightmapper::BAKE_ERROR_TEXTURE_EXCEEDS_MAX_SIZE) {
 		return BAKE_ERROR_TEXTURE_SIZE_TOO_SMALL;
@@ -1265,7 +1267,6 @@ LightmapGI::BakeError LightmapGI::bake(Node *p_from_node, String p_image_data_pa
 	}
 
 	// POSTBAKE: Save Textures.
-
 	TypedArray<TextureLayered> lightmap_textures;
 	TypedArray<TextureLayered> shadowmask_textures;
 
@@ -1274,16 +1275,16 @@ LightmapGI::BakeError LightmapGI::bake(Node *p_from_node, String p_image_data_pa
 	const bool save_shadowmask = shadowmask_mode != LightmapGIData::SHADOWMASK_MODE_NONE && shadowmask_texture_count > 0;
 
 	// Save the lightmap atlases.
-	BakeError save_err = _save_and_reimport_atlas_textures(lightmapper, texture_filename, lightmap_textures, false, false);
+	BakeError save_err = _save_and_reimport_atlas_textures(lightmapper, texture_filename, lightmap_textures, false);
 	ERR_FAIL_COND_V(save_err != BAKE_ERROR_OK, save_err);
 
 	if (save_shadowmask) {
 		// Save the shadowmask atlases.
-		save_err = _save_and_reimport_atlas_textures(lightmapper, texture_filename + "_shadow", shadowmask_textures, true, true);
+		save_err = _save_and_reimport_atlas_textures(lightmapper, texture_filename + "_shadow", shadowmask_textures, true);
 		ERR_FAIL_COND_V(save_err != BAKE_ERROR_OK, save_err);
 	}
 
-	/* POSTBAKE: Save Light Data. */
+	// POSTBAKE: Save Light Data.
 	Ref<LightmapGIData> gi_data;
 
 	if (get_light_data().is_valid()) {
@@ -1320,7 +1321,7 @@ LightmapGI::BakeError LightmapGI::bake(Node *p_from_node, String p_image_data_pa
 	}
 
 	{
-		// create tetrahedrons
+		// Create tetrahedrons.
 		Vector<Vector3> points;
 		Vector<Color> sh;
 		points.resize(lightmapper->get_bake_probe_count());
@@ -1334,11 +1335,11 @@ LightmapGI::BakeError LightmapGI::bake(Node *p_from_node, String p_image_data_pa
 			}
 		}
 
-		//Obtain solved simplices
-
+		// Obtain solved simplices.
 		if (p_bake_step) {
 			p_bake_step(0.8, RTR("Generating Probe Volumes"), p_bake_userdata, true);
 		}
+
 		Vector<Delaunay3D::OutputSimplex> solved_simplices = Delaunay3D::tetrahedralize(points);
 
 		LocalVector<BSPSimplex> bsp_simplices;
diff --git a/scene/3d/lightmap_gi.h b/scene/3d/lightmap_gi.h
index 6afd4ea591..f4db94a3ce 100644
--- a/scene/3d/lightmap_gi.h
+++ b/scene/3d/lightmap_gi.h
@@ -270,7 +270,7 @@ private:
 	void _plot_triangle_into_octree(GenProbesOctree *p_cell, float p_cell_size, const Vector3 *p_triangle);
 	void _gen_new_positions_from_octree(const GenProbesOctree *p_cell, float p_cell_size, const Vector<Vector3> &probe_positions, LocalVector<Vector3> &new_probe_positions, HashMap<Vector3i, bool> &positions_used, const AABB &p_bounds);
 
-	BakeError _save_and_reimport_atlas_textures(const Ref<Lightmapper> p_lightmapper, const String &p_base_name, TypedArray<TextureLayered> &r_textures, bool p_is_shadowmask = false, bool p_compress = false) const;
+	BakeError _save_and_reimport_atlas_textures(const Ref<Lightmapper> p_lightmapper, const String &p_base_name, TypedArray<TextureLayered> &r_textures, bool p_is_shadowmask = false) const;
 
 protected:
 	void _validate_property(PropertyInfo &p_property) const;

From cfc0a87099dda7f5e3ca8665abb4db504c985e6b Mon Sep 17 00:00:00 2001
From: Markus Sauermann <6299227+Sauermann@users.noreply.github.com>
Date: Thu, 12 Dec 2024 20:10:23 +0100
Subject: [PATCH 104/124] Introduce a conversion for editor shortcuts

This is temporary code for reverting effects, that were caused
by the reverted 97707.
---
 editor/editor_settings.cpp | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/editor/editor_settings.cpp b/editor/editor_settings.cpp
index 7322e2aefb..ec226bedec 100644
--- a/editor/editor_settings.cpp
+++ b/editor/editor_settings.cpp
@@ -103,6 +103,14 @@ bool EditorSettings::_set_only(const StringName &p_name, const Variant &p_value)
 
 			builtin_action_overrides[action_name].clear();
 			for (int ev_idx = 0; ev_idx < events.size(); ev_idx++) {
+#ifndef DISABLE_DEPRECATED
+				// -3 was introduced in GH-97707 as a way to prevent a clash in device IDs, but as reported in GH-99243, this leads to problems.
+				// -3 was used during dev-releases, so this conversion helps to revert such affected editor shortcuts.
+				Ref<InputEvent> x = events[ev_idx];
+				if (x.is_valid() && x->get_device() == -3) {
+					x->set_device(-1);
+				}
+#endif // DISABLE_DEPRECATED
 				im->action_add_event(action_name, events[ev_idx]);
 				builtin_action_overrides[action_name].push_back(events[ev_idx]);
 			}

From 0ba875ec2939d2de0f971cdca17f3830c8272245 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?R=C3=A9mi=20Verschelde?= <rverschelde@gmail.com>
Date: Thu, 12 Dec 2024 18:56:33 +0100
Subject: [PATCH 105/124] Jolt: Fix multiple definition LTO linking issue with
 mingw-gcc

---
 modules/jolt_physics/spaces/jolt_layers.cpp | 4 ++++
 modules/jolt_physics/spaces/jolt_layers.h   | 1 +
 2 files changed, 5 insertions(+)

diff --git a/modules/jolt_physics/spaces/jolt_layers.cpp b/modules/jolt_physics/spaces/jolt_layers.cpp
index c39c3a3397..ccf7bee984 100644
--- a/modules/jolt_physics/spaces/jolt_layers.cpp
+++ b/modules/jolt_physics/spaces/jolt_layers.cpp
@@ -191,6 +191,10 @@ JoltLayers::JoltLayers() {
 	_allocate_object_layer(0);
 }
 
+// MinGW GCC using LTO will emit errors during linking if this is defined in the header file, implicitly or otherwise.
+// Likely caused by this GCC bug: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=94156
+JoltLayers::~JoltLayers() = default;
+
 JPH::ObjectLayer JoltLayers::to_object_layer(JPH::BroadPhaseLayer p_broad_phase_layer, uint32_t p_collision_layer, uint32_t p_collision_mask) {
 	const uint64_t collision = encode_collision(p_collision_layer, p_collision_mask);
 
diff --git a/modules/jolt_physics/spaces/jolt_layers.h b/modules/jolt_physics/spaces/jolt_layers.h
index f610477a66..8299b956d7 100644
--- a/modules/jolt_physics/spaces/jolt_layers.h
+++ b/modules/jolt_physics/spaces/jolt_layers.h
@@ -63,6 +63,7 @@ class JoltLayers final
 
 public:
 	JoltLayers();
+	virtual ~JoltLayers();
 
 	JPH::ObjectLayer to_object_layer(JPH::BroadPhaseLayer p_broad_phase_layer, uint32_t p_collision_layer, uint32_t p_collision_mask);
 	void from_object_layer(JPH::ObjectLayer p_encoded_layer, JPH::BroadPhaseLayer &r_broad_phase_layer, uint32_t &r_collision_layer, uint32_t &r_collision_mask) const;

From cb4ceb823c2d315826cd6c1dda51ca16806cebdb Mon Sep 17 00:00:00 2001
From: kobewi <kobewi4e@gmail.com>
Date: Tue, 23 Apr 2024 15:37:47 +0200
Subject: [PATCH 106/124] Make scan for projects threaded

---
 editor/project_manager/project_list.cpp | 103 ++++++++++++++++++++----
 editor/project_manager/project_list.h   |  17 +++-
 scene/gui/progress_bar.cpp              |   6 +-
 3 files changed, 107 insertions(+), 19 deletions(-)

diff --git a/editor/project_manager/project_list.cpp b/editor/project_manager/project_list.cpp
index 428063e6db..f530c10727 100644
--- a/editor/project_manager/project_list.cpp
+++ b/editor/project_manager/project_list.cpp
@@ -41,8 +41,10 @@
 #include "editor/project_manager/project_tag.h"
 #include "editor/themes/editor_scale.h"
 #include "scene/gui/button.h"
+#include "scene/gui/dialogs.h"
 #include "scene/gui/label.h"
 #include "scene/gui/line_edit.h"
+#include "scene/gui/progress_bar.h"
 #include "scene/gui/texture_button.h"
 #include "scene/gui/texture_rect.h"
 #include "scene/resources/image_texture.h"
@@ -358,7 +360,7 @@ bool ProjectList::project_feature_looks_like_version(const String &p_feature) {
 void ProjectList::_notification(int p_what) {
 	switch (p_what) {
 		case NOTIFICATION_PROCESS: {
-			// Load icons as a coroutine to speed up launch when you have hundreds of projects
+			// Load icons as a coroutine to speed up launch when you have hundreds of projects.
 			if (_icon_load_index < _projects.size()) {
 				Item &item = _projects.write[_icon_load_index];
 				if (item.control->should_load_project_icon()) {
@@ -366,13 +368,62 @@ void ProjectList::_notification(int p_what) {
 				}
 				_icon_load_index++;
 
+				// Scan directories in thread to avoid blocking the window.
+			} else if (scan_data && scan_data->scan_in_progress.is_set()) {
+				// Wait for the thread.
 			} else {
 				set_process(false);
+				if (scan_data) {
+					_scan_finished();
+				}
 			}
 		} break;
 	}
 }
 
+// Projects scan.
+
+void ProjectList::_scan_thread(void *p_scan_data) {
+	ScanData *scan_data = static_cast<ScanData *>(p_scan_data);
+
+	for (const String &base_path : scan_data->paths_to_scan) {
+		print_verbose(vformat("Scanning for projects in \"%s\".", base_path));
+		_scan_folder_recursive(base_path, &scan_data->found_projects, scan_data->scan_in_progress);
+
+		if (!scan_data->scan_in_progress.is_set()) {
+			print_verbose("Scan aborted.");
+			break;
+		}
+	}
+	print_verbose(vformat("Found %d project(s).", scan_data->found_projects.size()));
+	scan_data->scan_in_progress.clear();
+}
+
+void ProjectList::_scan_finished() {
+	if (scan_data->scan_in_progress.is_set()) {
+		// Abort scanning.
+		scan_data->scan_in_progress.clear();
+	}
+
+	scan_data->thread->wait_to_finish();
+	memdelete(scan_data->thread);
+	if (scan_progress) {
+		scan_progress->hide();
+	}
+
+	for (const String &E : scan_data->found_projects) {
+		add_project(E, false);
+	}
+	memdelete(scan_data);
+	scan_data = nullptr;
+
+	save_config();
+
+	if (ProjectManager::get_singleton()->is_initialized()) {
+		update_project_list();
+	}
+}
+
 // Initialization & loading.
 
 void ProjectList::_migrate_config() {
@@ -624,25 +675,39 @@ void ProjectList::find_projects(const String &p_path) {
 }
 
 void ProjectList::find_projects_multiple(const PackedStringArray &p_paths) {
-	List<String> projects;
+	if (!scan_progress && is_inside_tree()) {
+		scan_progress = memnew(AcceptDialog);
+		scan_progress->set_title(TTR("Scanning"));
+		scan_progress->set_ok_button_text(TTR("Cancel"));
 
-	for (int i = 0; i < p_paths.size(); i++) {
-		const String &base_path = p_paths.get(i);
-		print_verbose(vformat("Scanning for projects in \"%s\".", base_path));
+		VBoxContainer *vb = memnew(VBoxContainer);
+		scan_progress->add_child(vb);
 
-		_scan_folder_recursive(base_path, &projects);
-		print_verbose(vformat("Found %d project(s).", projects.size()));
+		Label *label = memnew(Label);
+		label->set_text(TTR("Scanning for projects..."));
+		vb->add_child(label);
+
+		ProgressBar *progress = memnew(ProgressBar);
+		progress->set_indeterminate(true);
+		vb->add_child(progress);
+
+		add_child(scan_progress);
+		scan_progress->connect(SceneStringName(confirmed), callable_mp(this, &ProjectList::_scan_finished));
+		scan_progress->connect("canceled", callable_mp(this, &ProjectList::_scan_finished));
 	}
 
-	for (const String &E : projects) {
-		add_project(E, false);
-	}
+	scan_data = memnew(ScanData);
+	scan_data->paths_to_scan = p_paths;
+	scan_data->scan_in_progress.set();
 
-	save_config();
+	scan_data->thread = memnew(Thread);
+	scan_data->thread->start(_scan_thread, scan_data);
 
-	if (ProjectManager::get_singleton()->is_initialized()) {
-		update_project_list();
+	if (scan_progress) {
+		scan_progress->reset_size();
+		scan_progress->popup_centered();
 	}
+	set_process(true);
 }
 
 void ProjectList::load_project_list() {
@@ -656,7 +721,11 @@ void ProjectList::load_project_list() {
 	}
 }
 
-void ProjectList::_scan_folder_recursive(const String &p_path, List<String> *r_projects) {
+void ProjectList::_scan_folder_recursive(const String &p_path, List<String> *r_projects, const SafeFlag &p_scan_active) {
+	if (!p_scan_active.is_set()) {
+		return;
+	}
+
 	Ref<DirAccess> da = DirAccess::create(DirAccess::ACCESS_FILESYSTEM);
 	Error error = da->change_dir(p_path);
 	ERR_FAIL_COND_MSG(error != OK, vformat("Failed to open the path \"%s\" for scanning (code %d).", p_path, error));
@@ -664,8 +733,12 @@ void ProjectList::_scan_folder_recursive(const String &p_path, List<String> *r_p
 	da->list_dir_begin();
 	String n = da->get_next();
 	while (!n.is_empty()) {
+		if (!p_scan_active.is_set()) {
+			return;
+		}
+
 		if (da->current_is_dir() && n[0] != '.') {
-			_scan_folder_recursive(da->get_current_dir().path_join(n), r_projects);
+			_scan_folder_recursive(da->get_current_dir().path_join(n), r_projects, p_scan_active);
 		} else if (n == "project.godot") {
 			r_projects->push_back(da->get_current_dir());
 		}
diff --git a/editor/project_manager/project_list.h b/editor/project_manager/project_list.h
index 6e0f5830ac..7a77974d4a 100644
--- a/editor/project_manager/project_list.h
+++ b/editor/project_manager/project_list.h
@@ -35,6 +35,7 @@
 #include "scene/gui/box_container.h"
 #include "scene/gui/scroll_container.h"
 
+class AcceptDialog;
 class Button;
 class Label;
 class ProjectList;
@@ -176,6 +177,20 @@ private:
 
 	VBoxContainer *project_list_vbox = nullptr;
 
+	// Projects scan.
+
+	struct ScanData {
+		Thread *thread = nullptr;
+		PackedStringArray paths_to_scan;
+		List<String> found_projects;
+		SafeFlag scan_in_progress;
+	};
+	ScanData *scan_data = nullptr;
+	AcceptDialog *scan_progress = nullptr;
+
+	static void _scan_thread(void *p_scan_data);
+	void _scan_finished();
+
 	// Initialization & loading.
 
 	void _migrate_config();
@@ -186,7 +201,7 @@ private:
 
 	// Project list updates.
 
-	void _scan_folder_recursive(const String &p_path, List<String> *r_projects);
+	static void _scan_folder_recursive(const String &p_path, List<String> *r_projects, const SafeFlag &p_scan_active);
 
 	// Project list items.
 
diff --git a/scene/gui/progress_bar.cpp b/scene/gui/progress_bar.cpp
index 90ce01e383..baf0c394ac 100644
--- a/scene/gui/progress_bar.cpp
+++ b/scene/gui/progress_bar.cpp
@@ -61,7 +61,7 @@ void ProgressBar::_notification(int p_what) {
 				Size2 size = get_size();
 				real_t fill_size = MIN(size.width, size.height) * 2;
 
-				if (Engine::get_singleton()->is_editor_hint() && !editor_preview_indeterminate) {
+				if (is_part_of_edited_scene() && !editor_preview_indeterminate) {
 					// Center the filled bar when we're not previewing the animation.
 					_inderminate_fill_progress = (MAX(size.width, size.height) / 2) + (fill_size / 2);
 				}
@@ -217,7 +217,7 @@ void ProgressBar::set_indeterminate(bool p_indeterminate) {
 	indeterminate = p_indeterminate;
 	_inderminate_fill_progress = 0;
 
-	bool should_process = !Engine::get_singleton()->is_editor_hint() || editor_preview_indeterminate;
+	bool should_process = !is_part_of_edited_scene() || editor_preview_indeterminate;
 	set_process_internal(indeterminate && should_process);
 
 	notify_property_list_changed();
@@ -235,7 +235,7 @@ void ProgressBar::set_editor_preview_indeterminate(bool p_preview_indeterminate)
 	}
 	editor_preview_indeterminate = p_preview_indeterminate;
 
-	if (Engine::get_singleton()->is_editor_hint()) {
+	if (is_part_of_edited_scene()) {
 		_inderminate_fill_progress = 0;
 		set_process_internal(indeterminate && editor_preview_indeterminate);
 		queue_redraw();

From f86b3696f6877a648960330c8b2a3c3f51ee861e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?R=C3=A9mi=20Verschelde?= <rverschelde@gmail.com>
Date: Thu, 12 Dec 2024 22:22:30 +0100
Subject: [PATCH 107/124] SCons: Properly set SSE2 as baseline on x86_32

Setting it only for release templates on Windows and macOS was inconsistent,
and Jolt requires it as a minimum.

Drop the `-mxsave` flag from the raycast module, this doesn't seem to be
used explicitly by Embree, and unnecessarily makes our config and baseline
muddy.
---
 SConstruct                 |  6 ++++++
 modules/raycast/SCsub      |  8 +-------
 platform/macos/detect.py   | 12 +++---------
 platform/windows/detect.py |  6 +-----
 4 files changed, 11 insertions(+), 21 deletions(-)

diff --git a/SConstruct b/SConstruct
index f49ef9596c..50e83dcfdb 100644
--- a/SConstruct
+++ b/SConstruct
@@ -713,6 +713,12 @@ elif env.msvc:
         )
         Exit(255)
 
+# Default architecture flags.
+if env["arch"] == "x86_32":
+    if env.msvc:
+        env.Append(CCFLAGS=["/arch:SSE2"])
+    else:
+        env.Append(CCFLAGS=["-msse2"])
 
 # Set optimize and debug_symbols flags.
 # "custom" means do nothing and let users set their own optimization flags.
diff --git a/modules/raycast/SCsub b/modules/raycast/SCsub
index bbf5ff7983..b2e86382b2 100644
--- a/modules/raycast/SCsub
+++ b/modules/raycast/SCsub
@@ -66,18 +66,12 @@ if env["builtin_embree"]:
     env_raycast.Append(CPPDEFINES=["EMBREE_TARGET_SSE2", "EMBREE_LOWEST_ISA", "TASKING_INTERNAL"])
     env_raycast.AppendUnique(CPPDEFINES=["NDEBUG"])  # No assert() even in debug builds.
 
-    if not env.msvc:
-        if env["arch"] in ["x86_64", "x86_32"]:
-            env_raycast.Append(CCFLAGS=["-msse2", "-mxsave"])
-
-        if env["platform"] == "windows":
-            env_raycast.Append(CCFLAGS=["-mstackrealign"])
-
     if env["platform"] == "windows":
         if env.msvc:
             env.Append(LINKFLAGS=["psapi.lib"])
         else:
             env.Append(LIBS=["psapi"])
+            env_raycast.Append(CCFLAGS=["-mstackrealign"])
 
     if env.msvc:  # Disable bogus warning about intentional struct padding.
         env_raycast.Append(CCFLAGS=["/wd4324"])
diff --git a/platform/macos/detect.py b/platform/macos/detect.py
index 3575e93c68..9d45957418 100644
--- a/platform/macos/detect.py
+++ b/platform/macos/detect.py
@@ -70,14 +70,6 @@ def configure(env: "SConsEnvironment"):
     supported_arches = ["x86_64", "arm64"]
     validate_arch(env["arch"], get_name(), supported_arches)
 
-    ## Build type
-
-    if env["target"] == "template_release":
-        if env["arch"] != "arm64":
-            env.Prepend(CCFLAGS=["-msse2"])
-    elif env.dev_build:
-        env.Prepend(LINKFLAGS=["-Xlinker", "-no_deduplicate"])
-
     ## Compiler configuration
 
     # Save this in environment for use by other modules
@@ -97,6 +89,7 @@ def configure(env: "SConsEnvironment"):
         env.Append(LINKFLAGS=["-arch", "x86_64", "-mmacosx-version-min=10.13"])
 
     env.Append(CCFLAGS=["-ffp-contract=off"])
+    env.Append(CCFLAGS=["-fobjc-arc"])
 
     cc_version = get_compiler_version(env)
     cc_version_major = cc_version["apple_major"]
@@ -106,7 +99,8 @@ def configure(env: "SConsEnvironment"):
     if is_apple_clang(env) and cc_version_major == 1500 and cc_version_minor == 0:
         env.Prepend(LINKFLAGS=["-ld_classic"])
 
-    env.Append(CCFLAGS=["-fobjc-arc"])
+    if env.dev_build:
+        env.Prepend(LINKFLAGS=["-Xlinker", "-no_deduplicate"])
 
     if "osxcross" not in env:  # regular native build
         if env["macports_clang"] != "no":
diff --git a/platform/windows/detect.py b/platform/windows/detect.py
index e1109db24f..dbdc560a06 100644
--- a/platform/windows/detect.py
+++ b/platform/windows/detect.py
@@ -701,11 +701,7 @@ def configure_mingw(env: "SConsEnvironment"):
         print("Detected GCC to be a wrapper for Clang.")
         env["use_llvm"] = True
 
-    # TODO: Re-evaluate the need for this / streamline with common config.
-    if env["target"] == "template_release":
-        if env["arch"] != "arm64":
-            env.Append(CCFLAGS=["-msse2"])
-    elif env.dev_build:
+    if env.dev_build:
         # Allow big objects. It's supposed not to have drawbacks but seems to break
         # GCC LTO, so enabling for debug builds only (which are not built with LTO
         # and are the only ones with too big objects).

From 20c519d0fad06486543021d640de2385b72e29a3 Mon Sep 17 00:00:00 2001
From: smix8 <52464204+smix8@users.noreply.github.com>
Date: Fri, 13 Dec 2024 00:28:42 +0100
Subject: [PATCH 108/124] Fix 3D CSG not reacting to child node order changes

Fixes 3D CSG not reacting to child node order changes.
---
 modules/csg/csg_shape.cpp | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/modules/csg/csg_shape.cpp b/modules/csg/csg_shape.cpp
index c30c7b8bcb..fba255acbe 100644
--- a/modules/csg/csg_shape.cpp
+++ b/modules/csg/csg_shape.cpp
@@ -848,6 +848,10 @@ void CSGShape3D::_notification(int p_what) {
 			parent_shape = nullptr;
 		} break;
 
+		case NOTIFICATION_CHILD_ORDER_CHANGED: {
+			_make_dirty();
+		} break;
+
 		case NOTIFICATION_VISIBILITY_CHANGED: {
 			if (!is_root_shape() && last_visible != is_visible()) {
 				// Update this node's parent only if its own visibility has changed, not the visibility of parent nodes

From 46c9089ddc2ce42f761421f92c2de8740c82ecaa Mon Sep 17 00:00:00 2001
From: Malcolm Anderson <malcolminyo@gmail.com>
Date: Fri, 16 Aug 2024 14:22:19 -0700
Subject: [PATCH 109/124] Remove incorrect statement that TextureButton needs a
 texture for normal state in docs

Update doc/classes/TextureButton.xml

Co-authored-by: Micky <66727710+Mickeon@users.noreply.github.com>

Improve explanation of texture fallback behaviors for TextureButton

Remove a bit of incorrect information from TextureButton documentation

Most other state textures are not overlaid (only `texture_focused`)

Apply suggestions from code review
---
 doc/classes/TextureButton.xml | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/doc/classes/TextureButton.xml b/doc/classes/TextureButton.xml
index 861981f979..fcab07198c 100644
--- a/doc/classes/TextureButton.xml
+++ b/doc/classes/TextureButton.xml
@@ -5,8 +5,8 @@
 	</brief_description>
 	<description>
 		[TextureButton] has the same functionality as [Button], except it uses sprites instead of Godot's [Theme] resource. It is faster to create, but it doesn't support localization like more complex [Control]s.
-		The "normal" state must contain a texture ([member texture_normal]); other textures are optional.
 		See also [BaseButton] which contains common properties and methods associated with this node.
+		[b]Note:[/b] Setting a texture for the "normal" state ([member texture_normal]) is recommended. If [member texture_normal] is not set, the [TextureButton] will still receive input events and be clickable, but the user will not be able to see it unless they activate another one of its states with a texture assigned (e.g., hover over it to show [member texture_hover]).
 	</description>
 	<tutorials>
 		<link title="3D Voxel Demo">https://godotengine.org/asset-library/asset/2755</link>
@@ -28,19 +28,19 @@
 			Pure black and white [BitMap] image to use for click detection. On the mask, white pixels represent the button's clickable area. Use it to create buttons with curved shapes.
 		</member>
 		<member name="texture_disabled" type="Texture2D" setter="set_texture_disabled" getter="get_texture_disabled">
-			Texture to display when the node is disabled. See [member BaseButton.disabled].
+			Texture to display when the node is disabled. See [member BaseButton.disabled]. If not assigned, the [TextureButton] displays [member texture_normal] instead.
 		</member>
 		<member name="texture_focused" type="Texture2D" setter="set_texture_focused" getter="get_texture_focused">
-			Texture to display when the node has mouse or keyboard focus. [member texture_focused] is displayed [i]over[/i] the base texture, so a partially transparent texture should be used to ensure the base texture remains visible. A texture that represents an outline or an underline works well for this purpose. To disable the focus visual effect, assign a fully transparent texture of any size. Note that disabling the focus visual effect will harm keyboard/controller navigation usability, so this is not recommended for accessibility reasons.
+			Texture to [i]overlay on the base texture[/i] when the node has mouse or keyboard focus. Because [member texture_focused] is displayed on top of the base texture, a partially transparent texture should be used to ensure the base texture remains visible. A texture that represents an outline or an underline works well for this purpose. To disable the focus visual effect, assign a fully transparent texture of any size. Note that disabling the focus visual effect will harm keyboard/controller navigation usability, so this is not recommended for accessibility reasons.
 		</member>
 		<member name="texture_hover" type="Texture2D" setter="set_texture_hover" getter="get_texture_hover">
-			Texture to display when the mouse hovers the node.
+			Texture to display when the mouse hovers over the node. If not assigned, the [TextureButton] displays [member texture_normal] instead when hovered over.
 		</member>
 		<member name="texture_normal" type="Texture2D" setter="set_texture_normal" getter="get_texture_normal">
 			Texture to display by default, when the node is [b]not[/b] in the disabled, hover or pressed state. This texture is still displayed in the focused state, with [member texture_focused] drawn on top.
 		</member>
 		<member name="texture_pressed" type="Texture2D" setter="set_texture_pressed" getter="get_texture_pressed">
-			Texture to display on mouse down over the node, if the node has keyboard focus and the player presses the Enter key or if the player presses the [member BaseButton.shortcut] key.
+			Texture to display on mouse down over the node, if the node has keyboard focus and the player presses the Enter key or if the player presses the [member BaseButton.shortcut] key. If not assigned, the [TextureButton] displays [member texture_hover] instead when pressed.
 		</member>
 	</members>
 	<constants>

From f6126badbab82382847f777acecc7cc0a3a436dc Mon Sep 17 00:00:00 2001
From: Mikael Hermansson <mikael@hermansson.io>
Date: Fri, 13 Dec 2024 00:39:42 +0100
Subject: [PATCH 110/124] Make RID allocation thread-safe when using Jolt
 Physics

---
 modules/jolt_physics/jolt_physics_server_3d.h | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/modules/jolt_physics/jolt_physics_server_3d.h b/modules/jolt_physics/jolt_physics_server_3d.h
index bf71daf038..7dac7610f7 100644
--- a/modules/jolt_physics/jolt_physics_server_3d.h
+++ b/modules/jolt_physics/jolt_physics_server_3d.h
@@ -47,12 +47,12 @@ class JoltPhysicsServer3D final : public PhysicsServer3D {
 
 	inline static JoltPhysicsServer3D *singleton = nullptr;
 
-	mutable RID_PtrOwner<JoltSpace3D> space_owner;
-	mutable RID_PtrOwner<JoltArea3D> area_owner;
-	mutable RID_PtrOwner<JoltBody3D> body_owner;
-	mutable RID_PtrOwner<JoltSoftBody3D> soft_body_owner;
-	mutable RID_PtrOwner<JoltShape3D> shape_owner;
-	mutable RID_PtrOwner<JoltJoint3D> joint_owner;
+	mutable RID_PtrOwner<JoltSpace3D, true> space_owner;
+	mutable RID_PtrOwner<JoltArea3D, true> area_owner;
+	mutable RID_PtrOwner<JoltBody3D, true> body_owner;
+	mutable RID_PtrOwner<JoltSoftBody3D, true> soft_body_owner;
+	mutable RID_PtrOwner<JoltShape3D, true> shape_owner;
+	mutable RID_PtrOwner<JoltJoint3D, true> joint_owner;
 
 	HashSet<JoltSpace3D *> active_spaces;
 

From 356fa37f898d320141b306cff43ce8bd2b98f2cb Mon Sep 17 00:00:00 2001
From: clayjohn <claynjohn@gmail.com>
Date: Thu, 12 Dec 2024 17:29:21 -0800
Subject: [PATCH 111/124] Small fixups for the new reflection probe blending

---
 drivers/gles3/shaders/scene.glsl              | 22 +++++++------------
 .../shaders/scene_forward_lights_inc.glsl     | 22 +++++++------------
 2 files changed, 16 insertions(+), 28 deletions(-)

diff --git a/drivers/gles3/shaders/scene.glsl b/drivers/gles3/shaders/scene.glsl
index 2e74df850f..45cf82a63f 100644
--- a/drivers/gles3/shaders/scene.glsl
+++ b/drivers/gles3/shaders/scene.glsl
@@ -1619,21 +1619,15 @@ void reflection_process(samplerCube reflection_map,
 		return;
 	}
 
-	vec3 inner_pos = abs(local_pos / box_extents);
-	vec3 blend_axes = vec3(0.0, 0.0, 0.0);
-	float blend = 0.0;
-	if (blend_distance != 0) {
-		for (int i = 0; i < 3; i++) {
-			float axis_blend_distance = min(blend_distance, box_extents[i]);
-			blend_axes[i] = (inner_pos[i] * box_extents[i]) - box_extents[i] + axis_blend_distance;
-			blend_axes[i] = blend_axes[i] / axis_blend_distance;
-			blend_axes[i] = clamp(blend_axes[i], 0.0, 1.0);
-		}
-		blend = pow((1.0 - blend_axes.x) * (1.0 - blend_axes.y) * (1.0 - blend_axes.z), 2);
-		blend = 1 - blend;
+	float blend = 1.0;
+	if (blend_distance != 0.0) {
+		vec3 axis_blend_distance = min(vec3(blend_distance), box_extents);
+		vec3 blend_axes = abs(local_pos) - box_extents + axis_blend_distance;
+		blend_axes /= axis_blend_distance;
+		blend_axes = clamp(1.0 - blend_axes, vec3(0.0), vec3(1.0));
+
+		blend = pow(blend_axes.x * blend_axes.y * blend_axes.z, 2.0);
 	}
-	blend = max(0.0, 1.0 - blend);
-	blend = clamp(blend, 0.0, 1.0);
 
 	//reflect and make local
 	vec3 ref_normal = normalize(reflect(vertex, normal));
diff --git a/servers/rendering/renderer_rd/shaders/scene_forward_lights_inc.glsl b/servers/rendering/renderer_rd/shaders/scene_forward_lights_inc.glsl
index 9904ef8eab..5ea24bff71 100644
--- a/servers/rendering/renderer_rd/shaders/scene_forward_lights_inc.glsl
+++ b/servers/rendering/renderer_rd/shaders/scene_forward_lights_inc.glsl
@@ -875,21 +875,15 @@ void reflection_process(uint ref_index, vec3 vertex, vec3 ref_vec, vec3 normal,
 		return;
 	}
 
-	vec3 inner_pos = abs(local_pos / box_extents);
-	vec3 blend_axes = vec3(0.0, 0.0, 0.0);
-	float blend = 0.0;
-	if (reflections.data[ref_index].blend_distance != 0) {
-		for (int i = 0; i < 3; i++) {
-			float axis_blend_distance = min(reflections.data[ref_index].blend_distance, box_extents[i]);
-			blend_axes[i] = (inner_pos[i] * box_extents[i]) - box_extents[i] + axis_blend_distance;
-			blend_axes[i] = blend_axes[i] / axis_blend_distance;
-			blend_axes[i] = clamp(blend_axes[i], 0.0, 1.0);
-		}
-		blend = pow((1.0 - blend_axes.x) * (1.0 - blend_axes.y) * (1.0 - blend_axes.z), 2);
-		blend = 1 - blend;
+	float blend = 1.0;
+	if (reflections.data[ref_index].blend_distance != 0.0) {
+		vec3 axis_blend_distance = min(vec3(reflections.data[ref_index].blend_distance), box_extents);
+		vec3 blend_axes = abs(local_pos) - box_extents + axis_blend_distance;
+		blend_axes /= axis_blend_distance;
+		blend_axes = clamp(1.0 - blend_axes, vec3(0.0), vec3(1.0));
+
+		blend = pow(blend_axes.x * blend_axes.y * blend_axes.z, 2.0);
 	}
-	blend = max(0.0, 1.0 - blend);
-	blend = clamp(blend, 0.0, 1.0);
 
 	if (reflections.data[ref_index].intensity > 0.0) { // compute reflection
 

From 4c34813b325cae53374e67737a50736321831cd2 Mon Sep 17 00:00:00 2001
From: Florent Guiocheau <florent.guiocheau@gmail.com>
Date: Mon, 28 Oct 2024 13:52:33 +0100
Subject: [PATCH 112/124] Fix very large radius SphereMesh and CapsuleMesh
 normals

---
 scene/resources/3d/primitive_meshes.cpp | 32 ++++++++++++-------------
 1 file changed, 16 insertions(+), 16 deletions(-)

diff --git a/scene/resources/3d/primitive_meshes.cpp b/scene/resources/3d/primitive_meshes.cpp
index bc864e6ba5..0df18a5c8d 100644
--- a/scene/resources/3d/primitive_meshes.cpp
+++ b/scene/resources/3d/primitive_meshes.cpp
@@ -448,7 +448,7 @@ void CapsuleMesh::create_mesh_array(Array &p_arr, const float radius, const floa
 			y = 0.0;
 		} else {
 			w = Math::sin(0.5 * Math_PI * v);
-			y = Math::cos(0.5 * Math_PI * v) * radius;
+			y = Math::cos(0.5 * Math_PI * v);
 		}
 
 		for (i = 0; i <= radial_segments; i++) {
@@ -463,9 +463,9 @@ void CapsuleMesh::create_mesh_array(Array &p_arr, const float radius, const floa
 				z = Math::cos(u * Math_TAU);
 			}
 
-			Vector3 p = Vector3(x * radius * w, y, -z * radius * w);
-			points.push_back(p + Vector3(0.0, 0.5 * height - radius, 0.0));
-			normals.push_back(p.normalized());
+			Vector3 p = Vector3(x * w, y, -z * w);
+			points.push_back(p * radius + Vector3(0.0, 0.5 * height - radius, 0.0));
+			normals.push_back(p);
 			ADD_TANGENT(-z, 0.0, -x, 1.0)
 			uvs.push_back(Vector2(u, v * onethird));
 			if (p_add_uv2) {
@@ -544,10 +544,10 @@ void CapsuleMesh::create_mesh_array(Array &p_arr, const float radius, const floa
 		v /= (rings + 1);
 		if (j == (rings + 1)) {
 			w = 0.0;
-			y = -radius;
+			y = -1.0;
 		} else {
 			w = Math::cos(0.5 * Math_PI * v);
-			y = -Math::sin(0.5 * Math_PI * v) * radius;
+			y = -Math::sin(0.5 * Math_PI * v);
 		}
 
 		for (i = 0; i <= radial_segments; i++) {
@@ -562,9 +562,9 @@ void CapsuleMesh::create_mesh_array(Array &p_arr, const float radius, const floa
 				z = Math::cos(u * Math_TAU);
 			}
 
-			Vector3 p = Vector3(x * radius * w, y, -z * radius * w);
-			points.push_back(p + Vector3(0.0, -0.5 * height + radius, 0.0));
-			normals.push_back(p.normalized());
+			Vector3 p = Vector3(x * w, y, -z * w);
+			points.push_back(p * radius + Vector3(0.0, -0.5 * height + radius, 0.0));
+			normals.push_back(p);
 			ADD_TANGENT(-z, 0.0, -x, 1.0)
 			uvs.push_back(Vector2(u, twothirds + v * onethird));
 			if (p_add_uv2) {
@@ -1941,14 +1941,14 @@ void SphereMesh::create_mesh_array(Array &p_arr, float radius, float height, int
 	int i, j, prevrow, thisrow, point;
 	float x, y, z;
 
-	float scale = height * (is_hemisphere ? 1.0 : 0.5);
+	float scale = height / radius * (is_hemisphere ? 1.0 : 0.5);
 
 	// Only used if we calculate UV2
 	float circumference = radius * Math_TAU;
 	float horizontal_length = circumference + p_uv2_padding;
 	float center_h = 0.5 * circumference / horizontal_length;
 
-	float height_v = scale * Math_PI / ((scale * Math_PI) + p_uv2_padding);
+	float height_v = scale * Math_PI / ((scale * Math_PI) + p_uv2_padding / radius);
 
 	// set our bounding box
 
@@ -1975,10 +1975,10 @@ void SphereMesh::create_mesh_array(Array &p_arr, float radius, float height, int
 		v /= (rings + 1);
 		if (j == (rings + 1)) {
 			w = 0.0;
-			y = -scale;
+			y = -1.0;
 		} else {
 			w = Math::sin(Math_PI * v);
-			y = Math::cos(Math_PI * v) * scale;
+			y = Math::cos(Math_PI * v);
 		}
 
 		for (i = 0; i <= radial_segments; i++) {
@@ -1997,9 +1997,9 @@ void SphereMesh::create_mesh_array(Array &p_arr, float radius, float height, int
 				points.push_back(Vector3(x * radius * w, 0.0, z * radius * w));
 				normals.push_back(Vector3(0.0, -1.0, 0.0));
 			} else {
-				Vector3 p = Vector3(x * radius * w, y, z * radius * w);
-				points.push_back(p);
-				Vector3 normal = Vector3(x * w * scale, radius * (y / scale), z * w * scale);
+				Vector3 p = Vector3(x * w, y * scale, z * w);
+				points.push_back(p * radius);
+				Vector3 normal = Vector3(x * w * scale, y, z * w * scale);
 				normals.push_back(normal.normalized());
 			}
 			ADD_TANGENT(z, 0.0, -x, 1.0)

From 02f05cf0acf8d67ceab0f8403c15542f3a349761 Mon Sep 17 00:00:00 2001
From: Florent Guiocheau <florent.guiocheau@gmail.com>
Date: Tue, 10 Dec 2024 10:06:42 +0100
Subject: [PATCH 113/124] Fix wide angle spotlight clustering

---
 .../renderer_rd/cluster_builder_rd.h          | 46 +++++++++----------
 1 file changed, 22 insertions(+), 24 deletions(-)

diff --git a/servers/rendering/renderer_rd/cluster_builder_rd.h b/servers/rendering/renderer_rd/cluster_builder_rd.h
index 1badce2b81..0e85fcb7ba 100644
--- a/servers/rendering/renderer_rd/cluster_builder_rd.h
+++ b/servers/rendering/renderer_rd/cluster_builder_rd.h
@@ -253,7 +253,11 @@ public:
 
 		radius *= p_radius;
 
-		if (p_type == LIGHT_TYPE_OMNI) {
+		// Spotlights with wide angle are trated as Omni lights.
+		// If the spot angle is above the threshold, we need a sphere instead of a cone for building the clusters
+		// since the cone gets too flat/large (spot angle close to 90 degrees) or
+		// can't even cover the affected area of the light (spot angle above 90 degrees).
+		if (p_type == LIGHT_TYPE_OMNI || (p_type == LIGHT_TYPE_SPOT && p_spot_aperture > WIDE_SPOT_ANGLE_THRESHOLD_DEG)) {
 			radius *= shared->sphere_overfit; // Overfit icosphere.
 
 			float depth = -xform.origin.z;
@@ -269,15 +273,21 @@ public:
 			e.scale[0] = radius;
 			e.scale[1] = radius;
 			e.scale[2] = radius;
-			e.type = ELEMENT_TYPE_OMNI_LIGHT;
-			e.original_index = cluster_count_by_type[ELEMENT_TYPE_OMNI_LIGHT];
+			if (p_type == LIGHT_TYPE_OMNI) {
+				e.type = ELEMENT_TYPE_OMNI_LIGHT;
+				e.original_index = cluster_count_by_type[ELEMENT_TYPE_OMNI_LIGHT];
+				cluster_count_by_type[ELEMENT_TYPE_OMNI_LIGHT]++;
+			} else { // LIGHT_TYPE_SPOT with wide angle.
+				e.type = ELEMENT_TYPE_SPOT_LIGHT;
+				e.has_wide_spot_angle = true;
+				e.original_index = cluster_count_by_type[ELEMENT_TYPE_SPOT_LIGHT];
+				cluster_count_by_type[ELEMENT_TYPE_SPOT_LIGHT]++;
+			}
 
 			RendererRD::MaterialStorage::store_transform_transposed_3x4(xform, e.transform_inv);
 
-			cluster_count_by_type[ELEMENT_TYPE_OMNI_LIGHT]++;
-
-		} else /*LIGHT_TYPE_SPOT */ {
-			radius *= shared->cone_overfit; // Overfit icosphere
+		} else /*LIGHT_TYPE_SPOT with no wide angle*/ {
+			radius *= shared->cone_overfit; // Overfit cone.
 
 			real_t len = Math::tan(Math::deg_to_rad(p_spot_aperture)) * radius;
 			// Approximate, probably better to use a cone support function.
@@ -310,24 +320,12 @@ public:
 			}
 
 			e.touches_far = max_d > z_far;
-
-			// If the spot angle is above the threshold, use a sphere instead of a cone for building the clusters
-			// since the cone gets too flat/large (spot angle close to 90 degrees) or
-			// can't even cover the affected area of the light (spot angle above 90 degrees).
-			if (p_spot_aperture > WIDE_SPOT_ANGLE_THRESHOLD_DEG) {
-				e.scale[0] = radius;
-				e.scale[1] = radius;
-				e.scale[2] = radius;
-				e.has_wide_spot_angle = true;
-			} else {
-				e.scale[0] = len * shared->cone_overfit;
-				e.scale[1] = len * shared->cone_overfit;
-				e.scale[2] = radius;
-				e.has_wide_spot_angle = false;
-			}
-
+			e.scale[0] = len * shared->cone_overfit;
+			e.scale[1] = len * shared->cone_overfit;
+			e.scale[2] = radius;
+			e.has_wide_spot_angle = false;
 			e.type = ELEMENT_TYPE_SPOT_LIGHT;
-			e.original_index = cluster_count_by_type[ELEMENT_TYPE_SPOT_LIGHT]; // Use omni light since they share index.
+			e.original_index = cluster_count_by_type[ELEMENT_TYPE_SPOT_LIGHT];
 
 			RendererRD::MaterialStorage::store_transform_transposed_3x4(xform, e.transform_inv);
 

From 4bfc6f8d70c010a598d97936c0fe0ca25ad1fffd Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Pedro=20J=2E=20Est=C3=A9banez?= <pedrojrulez@gmail.com>
Date: Fri, 13 Dec 2024 12:49:10 +0100
Subject: [PATCH 114/124] Fix deadlock possibility in threaded load of
 materials

---
 scene/resources/canvas_item_material.cpp      | 6 +++++-
 scene/resources/material.cpp                  | 6 +++++-
 scene/resources/particle_process_material.cpp | 6 +++++-
 3 files changed, 15 insertions(+), 3 deletions(-)

diff --git a/scene/resources/canvas_item_material.cpp b/scene/resources/canvas_item_material.cpp
index 6f43106ea9..91ce9704dd 100644
--- a/scene/resources/canvas_item_material.cpp
+++ b/scene/resources/canvas_item_material.cpp
@@ -157,9 +157,13 @@ void CanvasItemMaterial::flush_changes() {
 }
 
 void CanvasItemMaterial::_queue_shader_change() {
+	if (!_is_initialized()) {
+		return;
+	}
+
 	MutexLock lock(material_mutex);
 
-	if (_is_initialized() && !element.in_list()) {
+	if (!element.in_list()) {
 		dirty_materials.add(&element);
 	}
 }
diff --git a/scene/resources/material.cpp b/scene/resources/material.cpp
index 5cdb7602d4..aa4b1aef32 100644
--- a/scene/resources/material.cpp
+++ b/scene/resources/material.cpp
@@ -1970,9 +1970,13 @@ void BaseMaterial3D::flush_changes() {
 }
 
 void BaseMaterial3D::_queue_shader_change() {
+	if (!_is_initialized()) {
+		return;
+	}
+
 	MutexLock lock(material_mutex);
 
-	if (_is_initialized() && !element.in_list()) {
+	if (!element.in_list()) {
 		dirty_materials.add(&element);
 	}
 }
diff --git a/scene/resources/particle_process_material.cpp b/scene/resources/particle_process_material.cpp
index 09bc1fa8e4..ac1242138c 100644
--- a/scene/resources/particle_process_material.cpp
+++ b/scene/resources/particle_process_material.cpp
@@ -1172,9 +1172,13 @@ void ParticleProcessMaterial::flush_changes() {
 }
 
 void ParticleProcessMaterial::_queue_shader_change() {
+	if (!_is_initialized()) {
+		return;
+	}
+
 	MutexLock lock(material_mutex);
 
-	if (_is_initialized() && !element.in_list()) {
+	if (!element.in_list()) {
 		dirty_materials.add(&element);
 	}
 }

From 7c4c1100e0d65dab0cc8d205457078d8ec2de228 Mon Sep 17 00:00:00 2001
From: Pablo Andres Fuente <pfuente@heroku.com>
Date: Fri, 4 Oct 2024 10:19:06 -0300
Subject: [PATCH 115/124] Improving detection of ccache on Mac

Before this commit, ccache where only used on Mac when
`OSXCROSS_ROOT` was defined. Now, it could be used even
when that envirnment variable is not defined.
---
 methods.py               |  8 ++++++--
 platform/macos/detect.py | 19 ++++++++-----------
 2 files changed, 14 insertions(+), 13 deletions(-)

diff --git a/methods.py b/methods.py
index 6db6b9580c..1092564147 100644
--- a/methods.py
+++ b/methods.py
@@ -657,12 +657,14 @@ def detect_darwin_sdk_path(platform, env):
 
 
 def is_apple_clang(env):
+    import shlex
+
     if env["platform"] not in ["macos", "ios"]:
         return False
     if not using_clang(env):
         return False
     try:
-        version = subprocess.check_output([env.subst(env["CXX"]), "--version"]).strip().decode("utf-8")
+        version = subprocess.check_output(shlex.split(env.subst(env["CXX"])) + ["--version"]).strip().decode("utf-8")
     except (subprocess.CalledProcessError, OSError):
         print_warning("Couldn't parse CXX environment variable to infer compiler version.")
         return False
@@ -677,6 +679,8 @@ def get_compiler_version(env):
     - metadata1, metadata2: Extra information
     - date: Date of the build
     """
+    import shlex
+
     ret = {
         "major": -1,
         "minor": -1,
@@ -727,7 +731,7 @@ def get_compiler_version(env):
     # Clang used to return hardcoded 4.2.1: # https://reviews.llvm.org/D56803
     try:
         version = subprocess.check_output(
-            [env.subst(env["CXX"]), "--version"], shell=(os.name == "nt"), encoding="utf-8"
+            shlex.split(env.subst(env["CXX"])) + ["--version"], shell=(os.name == "nt"), encoding="utf-8"
         ).strip()
     except (subprocess.CalledProcessError, OSError):
         print_warning("Couldn't parse CXX environment variable to infer compiler version.")
diff --git a/platform/macos/detect.py b/platform/macos/detect.py
index 3575e93c68..b7471989a1 100644
--- a/platform/macos/detect.py
+++ b/platform/macos/detect.py
@@ -108,6 +108,10 @@ def configure(env: "SConsEnvironment"):
 
     env.Append(CCFLAGS=["-fobjc-arc"])
 
+    ccache_path = os.environ.get("CCACHE", "")
+    if ccache_path != "":
+        ccache_path = ccache_path + " "
+
     if "osxcross" not in env:  # regular native build
         if env["macports_clang"] != "no":
             mpprefix = os.environ.get("MACPORTS_PREFIX", "/opt/local")
@@ -118,8 +122,8 @@ def configure(env: "SConsEnvironment"):
             env["RANLIB"] = mpprefix + "/libexec/llvm-" + mpclangver + "/bin/llvm-ranlib"
             env["AS"] = mpprefix + "/libexec/llvm-" + mpclangver + "/bin/llvm-as"
         else:
-            env["CC"] = "clang"
-            env["CXX"] = "clang++"
+            env["CC"] = ccache_path + "clang"
+            env["CXX"] = ccache_path + "clang++"
 
         detect_darwin_sdk_path("macos", env)
         env.Append(CCFLAGS=["-isysroot", "$MACOS_SDK_PATH"])
@@ -132,15 +136,8 @@ def configure(env: "SConsEnvironment"):
         else:
             basecmd = root + "/target/bin/x86_64-apple-" + env["osxcross_sdk"] + "-"
 
-        ccache_path = os.environ.get("CCACHE")
-        if ccache_path is None:
-            env["CC"] = basecmd + "cc"
-            env["CXX"] = basecmd + "c++"
-        else:
-            # there aren't any ccache wrappers available for macOS cross-compile,
-            # to enable caching we need to prepend the path to the ccache binary
-            env["CC"] = ccache_path + " " + basecmd + "cc"
-            env["CXX"] = ccache_path + " " + basecmd + "c++"
+        env["CC"] = ccache_path + basecmd + "cc"
+        env["CXX"] = ccache_path + basecmd + "c++"
         env["AR"] = basecmd + "ar"
         env["RANLIB"] = basecmd + "ranlib"
         env["AS"] = basecmd + "as"

From fb8553e4d763543ac2aad380189c5d98011269a4 Mon Sep 17 00:00:00 2001
From: Paul Joannon <hello@pauljoannon.com>
Date: Fri, 19 Apr 2024 21:38:32 +0200
Subject: [PATCH 116/124] Move to .NET8

- Change TFM and LangVersion
- Better exception throwing (CA1510, CA1512, CA1513)
- Better exception utility method definition (CA1859)
- Prefer comparing `.Count` over calling `.Any()` (CA1860)
- Prefer `.AsSpan()` over `.Substring()` (CA1846)
- Add a few more `scoped`
- Use `RuntimeHelpers.GetUninitializedObject()` instead of `FormatterServices.GetUninitializedObject()`
- Use delegate instead of delegate pointer in variant generic conversions
- Enable EnforceExtendedAnalyzerRules in source generator projects
- Disable CS8981 on structs named movable in Godot.NativeInterop
---
 .../mono/build_scripts/build_assemblies.py    |  2 +-
 .../Godot.SourceGenerators.Sample.csproj      |  4 +-
 .../CSharpAnalyzerVerifier.cs                 |  2 +-
 .../CSharpCodeFixVerifier.cs                  |  3 +-
 .../CSharpSourceGeneratorVerifier.cs          |  2 +-
 .../Godot.SourceGenerators.Tests/Constants.cs |  8 +++
 .../Godot.SourceGenerators.Tests.csproj       |  5 +-
 .../Godot.SourceGenerators.csproj             |  1 +
 .../GodotTools.Core/GodotTools.Core.csproj    |  6 +-
 .../GodotTools.ProjectEditor.csproj           |  8 ++-
 .../GodotTools.Shared.csproj                  | 12 ++--
 .../GodotTools/GodotTools/GodotTools.csproj   | 12 +++-
 .../Godot.SourceGenerators.Internal.csproj    |  8 ++-
 .../UnmanagedCallbacksGenerator.cs            | 24 +++++---
 .../GodotPlugins/GodotPlugins.csproj          |  4 +-
 .../glue/GodotSharp/GodotSharp/Core/Array.cs  | 21 +++----
 .../Core/Bridge/ScriptManagerBridge.cs        |  5 +-
 .../GodotSharp/Core/DebuggingUtils.cs         |  4 +-
 .../GodotSharp/GodotSharp/Core/Dictionary.cs  | 12 ++--
 .../GodotSharp/Core/GodotObject.base.cs       |  3 +-
 .../GodotSharp/Core/GodotTaskScheduler.cs     |  2 +-
 .../Core/NativeInterop/InteropStructs.cs      | 13 ++++-
 .../Core/NativeInterop/NativeFuncs.cs         | 57 +++++++++----------
 .../Core/NativeInterop/VariantUtils.cs        | 21 ++++++-
 .../NativeInterop/VariantUtils.generic.cs     | 19 +++----
 .../GodotSharp/Core/StringExtensions.cs       |  2 +-
 .../GodotSharp/GodotSharp/GodotSharp.csproj   | 13 ++++-
 .../GodotSharpEditor/GodotSharpEditor.csproj  |  4 +-
 28 files changed, 167 insertions(+), 110 deletions(-)

diff --git a/modules/mono/build_scripts/build_assemblies.py b/modules/mono/build_scripts/build_assemblies.py
index 2a1ecba332..bdb0ce8921 100755
--- a/modules/mono/build_scripts/build_assemblies.py
+++ b/modules/mono/build_scripts/build_assemblies.py
@@ -229,7 +229,7 @@ def build_godot_api(msbuild_tool, module_dir, output_dir, push_nupkgs_local, pre
 
         core_src_dir = os.path.abspath(os.path.join(sln, os.pardir, "GodotSharp", "bin", build_config))
         editor_src_dir = os.path.abspath(os.path.join(sln, os.pardir, "GodotSharpEditor", "bin", build_config))
-        plugins_src_dir = os.path.abspath(os.path.join(sln, os.pardir, "GodotPlugins", "bin", build_config, "net7.0"))
+        plugins_src_dir = os.path.abspath(os.path.join(sln, os.pardir, "GodotPlugins", "bin", build_config, "net8.0"))
 
         if not os.path.isdir(editor_api_dir):
             assert not os.path.isfile(editor_api_dir)
diff --git a/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Sample/Godot.SourceGenerators.Sample.csproj b/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Sample/Godot.SourceGenerators.Sample.csproj
index 5152b383ea..bbb180914c 100644
--- a/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Sample/Godot.SourceGenerators.Sample.csproj
+++ b/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Sample/Godot.SourceGenerators.Sample.csproj
@@ -1,8 +1,8 @@
 <Project Sdk="Microsoft.NET.Sdk">
 
   <PropertyGroup>
-    <TargetFramework>net7.0</TargetFramework>
-    <LangVersion>11</LangVersion>
+    <TargetFramework>net8.0</TargetFramework>
+    <LangVersion>12</LangVersion>
   </PropertyGroup>
 
   <PropertyGroup>
diff --git a/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Tests/CSharpAnalyzerVerifier.cs b/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Tests/CSharpAnalyzerVerifier.cs
index 253889296b..09d8faa35f 100644
--- a/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Tests/CSharpAnalyzerVerifier.cs
+++ b/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Tests/CSharpAnalyzerVerifier.cs
@@ -21,7 +21,7 @@ public static class CSharpAnalyzerVerifier<TAnalyzer>
     {
         public Test()
         {
-            ReferenceAssemblies = ReferenceAssemblies.Net.Net60;
+            ReferenceAssemblies = Constants.Net80;
 
             SolutionTransforms.Add((Solution solution, ProjectId projectId) =>
             {
diff --git a/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Tests/CSharpCodeFixVerifier.cs b/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Tests/CSharpCodeFixVerifier.cs
index 51e215a17b..c00ac45b89 100644
--- a/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Tests/CSharpCodeFixVerifier.cs
+++ b/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Tests/CSharpCodeFixVerifier.cs
@@ -4,7 +4,6 @@ using Microsoft.CodeAnalysis;
 using Microsoft.CodeAnalysis.CodeFixes;
 using Microsoft.CodeAnalysis.CSharp.Testing;
 using Microsoft.CodeAnalysis.Diagnostics;
-using Microsoft.CodeAnalysis.Testing;
 using Microsoft.CodeAnalysis.Testing.Verifiers;
 
 namespace Godot.SourceGenerators.Tests;
@@ -17,7 +16,7 @@ public static class CSharpCodeFixVerifier<TCodeFix, TAnalyzer>
     {
         public Test()
         {
-            ReferenceAssemblies = ReferenceAssemblies.Net.Net60;
+            ReferenceAssemblies = Constants.Net80;
             SolutionTransforms.Add((Solution solution, ProjectId projectId) =>
             {
                 Project project = solution.GetProject(projectId)!
diff --git a/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Tests/CSharpSourceGeneratorVerifier.cs b/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Tests/CSharpSourceGeneratorVerifier.cs
index 84e319352d..975f9f4e3f 100644
--- a/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Tests/CSharpSourceGeneratorVerifier.cs
+++ b/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Tests/CSharpSourceGeneratorVerifier.cs
@@ -18,7 +18,7 @@ where TSourceGenerator : ISourceGenerator, new()
     {
         public Test()
         {
-            ReferenceAssemblies = ReferenceAssemblies.Net.Net60;
+            ReferenceAssemblies = Constants.Net80;
 
             SolutionTransforms.Add((Solution solution, ProjectId projectId) =>
             {
diff --git a/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Tests/Constants.cs b/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Tests/Constants.cs
index 783b1e4298..bfd8fc7369 100644
--- a/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Tests/Constants.cs
+++ b/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Tests/Constants.cs
@@ -1,5 +1,6 @@
 using System.IO;
 using System.Reflection;
+using Microsoft.CodeAnalysis.Testing;
 
 namespace Godot.SourceGenerators.Tests;
 
@@ -7,6 +8,13 @@ public static class Constants
 {
     public static Assembly GodotSharpAssembly => typeof(GodotObject).Assembly;
 
+    // Can't find what needs updating to be able to access ReferenceAssemblies.Net.Net80, so we're making our own one.
+    public static ReferenceAssemblies Net80 => new ReferenceAssemblies(
+        "net8.0",
+        new PackageIdentity("Microsoft.NETCore.App.Ref", "8.0.0"),
+        Path.Combine("ref", "net8.0")
+    );
+
     public static string ExecutingAssemblyPath { get; }
     public static string SourceFolderPath { get; }
     public static string GeneratedSourceFolderPath { get; }
diff --git a/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Tests/Godot.SourceGenerators.Tests.csproj b/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Tests/Godot.SourceGenerators.Tests.csproj
index 31a255dcdf..676facf90d 100644
--- a/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Tests/Godot.SourceGenerators.Tests.csproj
+++ b/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators.Tests/Godot.SourceGenerators.Tests.csproj
@@ -1,9 +1,8 @@
 <Project Sdk="Microsoft.NET.Sdk">
 
   <PropertyGroup>
-    <TargetFramework>net6.0</TargetFramework>
-
-    <LangVersion>11</LangVersion>
+    <TargetFramework>net8.0</TargetFramework>
+    <LangVersion>12</LangVersion>
 
     <Nullable>enable</Nullable>
     <IsPackable>false</IsPackable>
diff --git a/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators/Godot.SourceGenerators.csproj b/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators/Godot.SourceGenerators.csproj
index 8e407da7a6..e8302f4137 100644
--- a/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators/Godot.SourceGenerators.csproj
+++ b/modules/mono/editor/Godot.NET.Sdk/Godot.SourceGenerators/Godot.SourceGenerators.csproj
@@ -3,6 +3,7 @@
     <TargetFramework>netstandard2.0</TargetFramework>
     <LangVersion>10</LangVersion>
     <Nullable>enable</Nullable>
+    <EnforceExtendedAnalyzerRules>true</EnforceExtendedAnalyzerRules>
   </PropertyGroup>
   <PropertyGroup>
     <Description>Core C# source generator for Godot projects.</Description>
diff --git a/modules/mono/editor/GodotTools/GodotTools.Core/GodotTools.Core.csproj b/modules/mono/editor/GodotTools/GodotTools.Core/GodotTools.Core.csproj
index 37563e0cc8..833f6b269a 100644
--- a/modules/mono/editor/GodotTools/GodotTools.Core/GodotTools.Core.csproj
+++ b/modules/mono/editor/GodotTools/GodotTools.Core/GodotTools.Core.csproj
@@ -1,8 +1,10 @@
 <Project Sdk="Microsoft.NET.Sdk">
+
   <PropertyGroup>
     <ProjectGuid>{639E48BD-44E5-4091-8EDD-22D36DC0768D}</ProjectGuid>
-    <TargetFramework>net7.0</TargetFramework>
-    <LangVersion>10</LangVersion>
+    <TargetFramework>net8.0</TargetFramework>
+    <LangVersion>12</LangVersion>
     <Nullable>enable</Nullable>
   </PropertyGroup>
+
 </Project>
diff --git a/modules/mono/editor/GodotTools/GodotTools.ProjectEditor/GodotTools.ProjectEditor.csproj b/modules/mono/editor/GodotTools/GodotTools.ProjectEditor/GodotTools.ProjectEditor.csproj
index 9a73a7f81d..b08efd0964 100644
--- a/modules/mono/editor/GodotTools/GodotTools.ProjectEditor/GodotTools.ProjectEditor.csproj
+++ b/modules/mono/editor/GodotTools/GodotTools.ProjectEditor/GodotTools.ProjectEditor.csproj
@@ -1,17 +1,21 @@
 <Project Sdk="Microsoft.NET.Sdk">
+
   <PropertyGroup>
     <ProjectGuid>{A8CDAD94-C6D4-4B19-A7E7-76C53CC92984}</ProjectGuid>
-    <TargetFramework>net7.0</TargetFramework>
-    <LangVersion>10</LangVersion>
+    <TargetFramework>net8.0</TargetFramework>
+    <LangVersion>12</LangVersion>
     <Nullable>enable</Nullable>
   </PropertyGroup>
+
   <ItemGroup>
     <PackageReference Include="Microsoft.Build" Version="15.1.548" ExcludeAssets="runtime" />
     <PackageReference Include="Microsoft.Build.Locator" Version="1.2.6" />
     <PackageReference Include="Microsoft.NETFramework.ReferenceAssemblies" Version="1.0.0" PrivateAssets="All" />
   </ItemGroup>
+
   <ItemGroup>
     <ProjectReference Include="..\GodotTools.Core\GodotTools.Core.csproj" />
     <ProjectReference Include="..\GodotTools.Shared\GodotTools.Shared.csproj" />
   </ItemGroup>
+
 </Project>
diff --git a/modules/mono/editor/GodotTools/GodotTools.Shared/GodotTools.Shared.csproj b/modules/mono/editor/GodotTools/GodotTools.Shared/GodotTools.Shared.csproj
index e916980f15..a281853f68 100644
--- a/modules/mono/editor/GodotTools/GodotTools.Shared/GodotTools.Shared.csproj
+++ b/modules/mono/editor/GodotTools/GodotTools.Shared/GodotTools.Shared.csproj
@@ -1,8 +1,12 @@
-<Project Sdk="Microsoft.NET.Sdk">
+<Project Sdk="Microsoft.NET.Sdk">
+
     <PropertyGroup>
-        <TargetFramework>net8.0</TargetFramework>
-        <!-- Specify compile items manually to avoid including dangling generated items. -->
-        <EnableDefaultCompileItems>false</EnableDefaultCompileItems>
+      <TargetFramework>net8.0</TargetFramework>
+      <LangVersion>12</LangVersion>
+      <!-- Specify compile items manually to avoid including dangling generated items. -->
+      <EnableDefaultCompileItems>false</EnableDefaultCompileItems>
     </PropertyGroup>
+
     <Import Project="GenerateGodotNupkgsVersions.targets" />
+
 </Project>
diff --git a/modules/mono/editor/GodotTools/GodotTools/GodotTools.csproj b/modules/mono/editor/GodotTools/GodotTools/GodotTools.csproj
index a8deeb2fea..c82da741bd 100644
--- a/modules/mono/editor/GodotTools/GodotTools/GodotTools.csproj
+++ b/modules/mono/editor/GodotTools/GodotTools/GodotTools.csproj
@@ -1,9 +1,10 @@
 <Project Sdk="Microsoft.NET.Sdk">
+
   <PropertyGroup>
     <ProjectGuid>{27B00618-A6F2-4828-B922-05CAEB08C286}</ProjectGuid>
-    <TargetFramework>net7.0</TargetFramework>
+    <TargetFramework>net8.0</TargetFramework>
+    <LangVersion>12</LangVersion>
     <EnableDynamicLoading>true</EnableDynamicLoading>
-    <LangVersion>10</LangVersion>
     <Nullable>enable</Nullable>
     <!-- The Godot editor uses the Debug Godot API assemblies -->
     <GodotApiConfiguration>Debug</GodotApiConfiguration>
@@ -13,13 +14,16 @@
     <ProduceReferenceAssembly>false</ProduceReferenceAssembly>
     <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
   </PropertyGroup>
+
   <!-- Needed for our source generators to work despite this not being a Godot game project -->
   <PropertyGroup>
     <IsGodotToolsProject>true</IsGodotToolsProject>
   </PropertyGroup>
+
   <ItemGroup>
     <CompilerVisibleProperty Include="IsGodotToolsProject" />
   </ItemGroup>
+
   <PropertyGroup Condition=" Exists('$(GodotApiAssembliesDir)/GodotSharp.dll') ">
     <!-- The project is part of the Godot source tree -->
     <!-- Use the Godot source tree output folder instead of '$(ProjectDir)/bin' -->
@@ -27,6 +31,7 @@
     <!-- Must not append '$(TargetFramework)' to the output path in this case -->
     <AppendTargetFrameworkToOutputPath>False</AppendTargetFrameworkToOutputPath>
   </PropertyGroup>
+
   <ItemGroup>
     <PackageReference Include="JetBrains.Annotations" Version="2019.1.3.0" ExcludeAssets="runtime" PrivateAssets="all" />
     <PackageReference Include="JetBrains.Rider.PathLocator" Version="1.0.9" />
@@ -41,14 +46,17 @@
       <Private>False</Private>
     </Reference>
   </ItemGroup>
+
   <ItemGroup>
     <ProjectReference Include="..\..\Godot.NET.Sdk\Godot.SourceGenerators\Godot.SourceGenerators.csproj" OutputItemType="Analyzer" ReferenceOutputAssembly="false" />
     <ProjectReference Include="..\..\..\glue\GodotSharp\Godot.SourceGenerators.Internal\Godot.SourceGenerators.Internal.csproj" OutputItemType="Analyzer" ReferenceOutputAssembly="false" />
   </ItemGroup>
+
   <ItemGroup>
     <ProjectReference Include="..\GodotTools.BuildLogger\GodotTools.BuildLogger.csproj" />
     <ProjectReference Include="..\GodotTools.IdeMessaging\GodotTools.IdeMessaging.csproj" />
     <ProjectReference Include="..\GodotTools.ProjectEditor\GodotTools.ProjectEditor.csproj" />
     <ProjectReference Include="..\GodotTools.Core\GodotTools.Core.csproj" />
   </ItemGroup>
+
 </Project>
diff --git a/modules/mono/glue/GodotSharp/Godot.SourceGenerators.Internal/Godot.SourceGenerators.Internal.csproj b/modules/mono/glue/GodotSharp/Godot.SourceGenerators.Internal/Godot.SourceGenerators.Internal.csproj
index 81add0e44f..ee607ff279 100644
--- a/modules/mono/glue/GodotSharp/Godot.SourceGenerators.Internal/Godot.SourceGenerators.Internal.csproj
+++ b/modules/mono/glue/GodotSharp/Godot.SourceGenerators.Internal/Godot.SourceGenerators.Internal.csproj
@@ -1,11 +1,15 @@
 <Project Sdk="Microsoft.NET.Sdk">
+
   <PropertyGroup>
     <TargetFramework>netstandard2.0</TargetFramework>
     <LangVersion>10</LangVersion>
     <Nullable>enable</Nullable>
+    <EnforceExtendedAnalyzerRules>true</EnforceExtendedAnalyzerRules>
   </PropertyGroup>
+
   <ItemGroup>
-    <PackageReference Include="Microsoft.CodeAnalysis.CSharp" Version="3.10.0" PrivateAssets="all" />
-    <PackageReference Include="Microsoft.CodeAnalysis.Analyzers" Version="3.3.3" PrivateAssets="all" />
+    <PackageReference Include="Microsoft.CodeAnalysis.CSharp" Version="4.9.2" PrivateAssets="all" />
+    <PackageReference Include="Microsoft.CodeAnalysis.Analyzers" Version="3.3.4" PrivateAssets="all" />
   </ItemGroup>
+
 </Project>
diff --git a/modules/mono/glue/GodotSharp/Godot.SourceGenerators.Internal/UnmanagedCallbacksGenerator.cs b/modules/mono/glue/GodotSharp/Godot.SourceGenerators.Internal/UnmanagedCallbacksGenerator.cs
index 3d6a5b4662..80d6160f3d 100644
--- a/modules/mono/glue/GodotSharp/Godot.SourceGenerators.Internal/UnmanagedCallbacksGenerator.cs
+++ b/modules/mono/glue/GodotSharp/Godot.SourceGenerators.Internal/UnmanagedCallbacksGenerator.cs
@@ -172,7 +172,11 @@ using Godot.NativeInterop;
             {
                 var parameter = callback.Parameters[i];
 
-                source.Append(parameter.DeclaringSyntaxReferences[0].GetSyntax().ToString());
+                AppendRefKind(source, parameter.RefKind, parameter.ScopedKind);
+                source.Append(' ');
+                source.Append(parameter.Type.FullQualifiedNameIncludeGlobal());
+                source.Append(' ');
+                source.Append(parameter.Name);
 
                 if (parameter.RefKind == RefKind.Out)
                 {
@@ -204,7 +208,7 @@ using Godot.NativeInterop;
                     {
                         // If it's a by-ref param and we can't get the pointer
                         // just pass it by-ref and let it be pinned.
-                        AppendRefKind(methodCallArguments, parameter.RefKind)
+                        AppendRefKind(methodCallArguments, parameter.RefKind, parameter.ScopedKind)
                             .Append(' ')
                             .Append(parameter.Name);
                     }
@@ -342,7 +346,7 @@ using Godot.NativeInterop;
                     {
                         // If it's a by-ref param and we can't get the pointer
                         // just pass it by-ref and let it be pinned.
-                        AppendRefKind(source, parameter.RefKind)
+                        AppendRefKind(source, parameter.RefKind, parameter.ScopedKind)
                             .Append(' ')
                             .Append(parameter.Type.FullQualifiedNameIncludeGlobal());
                     }
@@ -388,14 +392,18 @@ using Godot.NativeInterop;
     private static bool IsByRefParameter(IParameterSymbol parameter) =>
         parameter.RefKind is RefKind.In or RefKind.Out or RefKind.Ref;
 
-    private static StringBuilder AppendRefKind(StringBuilder source, RefKind refKind) =>
-        refKind switch
+    private static StringBuilder AppendRefKind(StringBuilder source, RefKind refKind, ScopedKind scopedKind)
+    {
+        return (refKind, scopedKind) switch
         {
-            RefKind.In => source.Append("in"),
-            RefKind.Out => source.Append("out"),
-            RefKind.Ref => source.Append("ref"),
+            (RefKind.Out, _) => source.Append("out"),
+            (RefKind.In, ScopedKind.ScopedRef) => source.Append("scoped in"),
+            (RefKind.In, _) => source.Append("in"),
+            (RefKind.Ref, ScopedKind.ScopedRef) => source.Append("scoped ref"),
+            (RefKind.Ref, _) => source.Append("ref"),
             _ => source,
         };
+    }
 
     private static void AppendPointerType(StringBuilder source, ITypeSymbol type)
     {
diff --git a/modules/mono/glue/GodotSharp/GodotPlugins/GodotPlugins.csproj b/modules/mono/glue/GodotSharp/GodotPlugins/GodotPlugins.csproj
index 1deccbc09d..6d557c1eac 100644
--- a/modules/mono/glue/GodotSharp/GodotPlugins/GodotPlugins.csproj
+++ b/modules/mono/glue/GodotSharp/GodotPlugins/GodotPlugins.csproj
@@ -1,8 +1,8 @@
 <Project Sdk="Microsoft.NET.Sdk">
 
   <PropertyGroup>
-    <TargetFramework>net7.0</TargetFramework>
-    <LangVersion>10</LangVersion>
+    <TargetFramework>net8.0</TargetFramework>
+    <LangVersion>12</LangVersion>
     <Nullable>enable</Nullable>
     <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
 
diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Array.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Array.cs
index c4bfed8542..9879a78104 100644
--- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Array.cs
+++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Array.cs
@@ -50,8 +50,7 @@ namespace Godot.Collections
         /// <returns>A new Godot Array.</returns>
         public Array(IEnumerable<Variant> collection) : this()
         {
-            if (collection == null)
-                throw new ArgumentNullException(nameof(collection));
+            ArgumentNullException.ThrowIfNull(collection);
 
             foreach (Variant element in collection)
                 Add(element);
@@ -67,8 +66,7 @@ namespace Godot.Collections
         /// <returns>A new Godot Array.</returns>
         public Array(Variant[] array)
         {
-            if (array == null)
-                throw new ArgumentNullException(nameof(array));
+            ArgumentNullException.ThrowIfNull(array);
 
             NativeValue = (godot_array.movable)NativeFuncs.godotsharp_array_new();
             _weakReferenceToSelf = DisposablesTracker.RegisterDisposable(this);
@@ -1056,7 +1054,7 @@ namespace Godot.Collections
         IEnumerable<T>,
         IGenericGodotArray
     {
-        private static godot_variant ToVariantFunc(in Array<T> godotArray) =>
+        private static godot_variant ToVariantFunc(scoped in Array<T> godotArray) =>
             VariantUtils.CreateFromArray(godotArray);
 
         private static Array<T> FromVariantFunc(in godot_variant variant) =>
@@ -1080,8 +1078,8 @@ namespace Godot.Collections
 
         static unsafe Array()
         {
-            VariantUtils.GenericConversion<Array<T>>.ToVariantCb = &ToVariantFunc;
-            VariantUtils.GenericConversion<Array<T>>.FromVariantCb = &FromVariantFunc;
+            VariantUtils.GenericConversion<Array<T>>.ToVariantCb = ToVariantFunc;
+            VariantUtils.GenericConversion<Array<T>>.FromVariantCb = FromVariantFunc;
         }
 
         private readonly Array _underlyingArray;
@@ -1114,8 +1112,7 @@ namespace Godot.Collections
         /// <returns>A new Godot Array.</returns>
         public Array(IEnumerable<T> collection)
         {
-            if (collection == null)
-                throw new ArgumentNullException(nameof(collection));
+            ArgumentNullException.ThrowIfNull(collection);
 
             _underlyingArray = new Array();
             SetTypedForUnderlyingArray();
@@ -1134,8 +1131,7 @@ namespace Godot.Collections
         /// <returns>A new Godot Array.</returns>
         public Array(T[] array)
         {
-            if (array == null)
-                throw new ArgumentNullException(nameof(array));
+            ArgumentNullException.ThrowIfNull(array);
 
             _underlyingArray = new Array();
             SetTypedForUnderlyingArray();
@@ -1154,8 +1150,7 @@ namespace Godot.Collections
         /// <returns>A new Godot Array.</returns>
         public Array(Array array)
         {
-            if (array == null)
-                throw new ArgumentNullException(nameof(array));
+            ArgumentNullException.ThrowIfNull(array);
 
             _underlyingArray = array;
         }
diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Bridge/ScriptManagerBridge.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Bridge/ScriptManagerBridge.cs
index 93a3f52f7d..7019ea2932 100644
--- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Bridge/ScriptManagerBridge.cs
+++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Bridge/ScriptManagerBridge.cs
@@ -90,8 +90,7 @@ namespace Godot.Bridge
         }
 
         [UnmanagedCallersOnly]
-        internal static unsafe IntPtr CreateManagedForGodotObjectBinding(godot_string_name* nativeTypeName,
-            IntPtr godotObject)
+        internal static unsafe IntPtr CreateManagedForGodotObjectBinding(godot_string_name* nativeTypeName, IntPtr godotObject)
         {
             try
             {
@@ -143,7 +142,7 @@ namespace Godot.Bridge
                     }
                 }
 
-                var obj = (GodotObject)FormatterServices.GetUninitializedObject(scriptType);
+                var obj = (GodotObject)RuntimeHelpers.GetUninitializedObject(scriptType);
 
                 var parameters = ctor.GetParameters();
                 int paramCount = parameters.Length;
diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/DebuggingUtils.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/DebuggingUtils.cs
index 0d96a9a5c1..0c83fd58aa 100644
--- a/modules/mono/glue/GodotSharp/GodotSharp/Core/DebuggingUtils.cs
+++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/DebuggingUtils.cs
@@ -87,8 +87,8 @@ namespace Godot
 
             public void Resize(int size)
             {
-                if (size < 0)
-                    throw new ArgumentOutOfRangeException(nameof(size));
+                ArgumentOutOfRangeException.ThrowIfNegative(size);
+
                 var err = NativeFuncs.godotsharp_stack_info_vector_resize(ref this, size);
                 if (err != Error.Ok)
                     throw new InvalidOperationException("Failed to resize vector. Error code is: " + err.ToString());
diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Dictionary.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Dictionary.cs
index ad313dbbf0..dcb94f05fd 100644
--- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Dictionary.cs
+++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Dictionary.cs
@@ -492,7 +492,7 @@ namespace Godot.Collections
         IReadOnlyDictionary<TKey, TValue>,
         IGenericGodotDictionary
     {
-        private static godot_variant ToVariantFunc(in Dictionary<TKey, TValue> godotDictionary) =>
+        private static godot_variant ToVariantFunc(scoped in Dictionary<TKey, TValue> godotDictionary) =>
             VariantUtils.CreateFromDictionary(godotDictionary);
 
         private static Dictionary<TKey, TValue> FromVariantFunc(in godot_variant variant) =>
@@ -521,8 +521,8 @@ namespace Godot.Collections
 
         static unsafe Dictionary()
         {
-            VariantUtils.GenericConversion<Dictionary<TKey, TValue>>.ToVariantCb = &ToVariantFunc;
-            VariantUtils.GenericConversion<Dictionary<TKey, TValue>>.FromVariantCb = &FromVariantFunc;
+            VariantUtils.GenericConversion<Dictionary<TKey, TValue>>.ToVariantCb = ToVariantFunc;
+            VariantUtils.GenericConversion<Dictionary<TKey, TValue>>.FromVariantCb = FromVariantFunc;
         }
 
         private readonly Dictionary _underlyingDict;
@@ -555,8 +555,7 @@ namespace Godot.Collections
         /// <returns>A new Godot Dictionary.</returns>
         public Dictionary(IDictionary<TKey, TValue> dictionary)
         {
-            if (dictionary == null)
-                throw new ArgumentNullException(nameof(dictionary));
+            ArgumentNullException.ThrowIfNull(dictionary);
 
             _underlyingDict = new Dictionary();
             SetTypedForUnderlyingDictionary();
@@ -575,8 +574,7 @@ namespace Godot.Collections
         /// <returns>A new Godot Dictionary.</returns>
         public Dictionary(Dictionary dictionary)
         {
-            if (dictionary == null)
-                throw new ArgumentNullException(nameof(dictionary));
+            ArgumentNullException.ThrowIfNull(dictionary);
 
             _underlyingDict = dictionary;
         }
diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/GodotObject.base.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/GodotObject.base.cs
index 00eb64dd17..08aea77e84 100644
--- a/modules/mono/glue/GodotSharp/GodotSharp/Core/GodotObject.base.cs
+++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/GodotObject.base.cs
@@ -90,8 +90,7 @@ namespace Godot
             // NativePtr is assigned, that would result in UB or crashes when calling
             // native functions that receive the pointer, which can happen because the
             // debugger calls ToString() and tries to get the value of properties.
-            if (instance._disposed || instance.NativePtr == IntPtr.Zero)
-                throw new ObjectDisposedException(instance.GetType().FullName);
+            ObjectDisposedException.ThrowIf(instance._disposed || instance.NativePtr == IntPtr.Zero, instance);
 
             return instance.NativePtr;
         }
diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/GodotTaskScheduler.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/GodotTaskScheduler.cs
index f6c36455b2..f84bf9de7c 100644
--- a/modules/mono/glue/GodotSharp/GodotSharp/Core/GodotTaskScheduler.cs
+++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/GodotTaskScheduler.cs
@@ -88,7 +88,7 @@ namespace Godot
 
                 lock (_tasks)
                 {
-                    if (_tasks.Any())
+                    if (_tasks.Count > 0)
                     {
                         task = _tasks.First.Value;
                         _tasks.RemoveFirst();
diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/InteropStructs.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/InteropStructs.cs
index 6e34c2bfbf..dda776fee5 100644
--- a/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/InteropStructs.cs
+++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/InteropStructs.cs
@@ -173,7 +173,8 @@ namespace Godot.NativeInterop
         internal readonly unsafe godot_variant* GetUnsafeAddress()
             => (godot_variant*)Unsafe.AsPointer(ref Unsafe.AsRef(in _typeField));
 
-        // Variant.Type is generated as an enum of type long, so we can't use for the field as it must only take 32-bits. (the native enum actually has no fixed underlying type so it is only at least 6 bits long)
+        // Variant.Type is generated as an enum of type long, so we can't use for the field as it must only take 32-bits.
+        // The native enum actually has no fixed underlying type, so it is only at least 6 bits long.
         private int _typeField;
 
         // There's padding here
@@ -481,8 +482,10 @@ namespace Godot.NativeInterop
             Type = Variant.Type.Nil;
         }
 
+#pragma warning disable CS8981 // The type name only contains lower-cased ascii characters
         [StructLayout(LayoutKind.Explicit)]
         internal struct movable
+#pragma warning restore CS8981
         {
             // Variant.Type is generated as an enum of type long, so we can't use for the field as it must only take 32-bits.
             [FieldOffset(0)] private int _typeField;
@@ -588,8 +591,10 @@ namespace Godot.NativeInterop
             return _data.GetHashCode();
         }
 
+#pragma warning disable CS8981 // The type name only contains lower-cased ascii characters
         [StructLayout(LayoutKind.Sequential)]
         internal struct movable
+#pragma warning restore CS8981
         {
             private IntPtr _data;
 
@@ -634,8 +639,10 @@ namespace Godot.NativeInterop
             get => _data == IntPtr.Zero;
         }
 
+#pragma warning disable CS8981 // The type name only contains lower-cased ascii characters
         [StructLayout(LayoutKind.Sequential)]
         internal struct movable
+#pragma warning restore CS8981
         {
             private IntPtr _data;
 
@@ -809,8 +816,10 @@ namespace Godot.NativeInterop
             _p = null;
         }
 
+#pragma warning disable CS8981 // The type name only contains lower-cased ascii characters
         [StructLayout(LayoutKind.Sequential)]
         internal struct movable
+#pragma warning restore CS8981
         {
             private unsafe ArrayPrivate* _p;
 
@@ -876,8 +885,10 @@ namespace Godot.NativeInterop
             _p = null;
         }
 
+#pragma warning disable CS8981 // The type name only contains lower-cased ascii characters
         [StructLayout(LayoutKind.Sequential)]
         internal struct movable
+#pragma warning restore CS8981
         {
             private unsafe DictionaryPrivate* _p;
 
diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/NativeFuncs.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/NativeFuncs.cs
index 8c2bc7941b..021a701521 100644
--- a/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/NativeFuncs.cs
+++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/NativeFuncs.cs
@@ -233,51 +233,51 @@ namespace Godot.NativeInterop
         public static partial void godotsharp_variant_new_packed_color_array(out godot_variant r_dest,
             scoped in godot_packed_color_array p_pca);
 
-        public static partial godot_bool godotsharp_variant_as_bool(in godot_variant p_self);
+        public static partial godot_bool godotsharp_variant_as_bool(scoped in godot_variant p_self);
 
-        public static partial Int64 godotsharp_variant_as_int(in godot_variant p_self);
+        public static partial Int64 godotsharp_variant_as_int(scoped in godot_variant p_self);
 
-        public static partial double godotsharp_variant_as_float(in godot_variant p_self);
+        public static partial double godotsharp_variant_as_float(scoped in godot_variant p_self);
 
         public static partial godot_string godotsharp_variant_as_string(scoped in godot_variant p_self);
 
-        public static partial Vector2 godotsharp_variant_as_vector2(in godot_variant p_self);
+        public static partial Vector2 godotsharp_variant_as_vector2(scoped in godot_variant p_self);
 
-        public static partial Vector2I godotsharp_variant_as_vector2i(in godot_variant p_self);
+        public static partial Vector2I godotsharp_variant_as_vector2i(scoped in godot_variant p_self);
 
-        public static partial Rect2 godotsharp_variant_as_rect2(in godot_variant p_self);
+        public static partial Rect2 godotsharp_variant_as_rect2(scoped in godot_variant p_self);
 
-        public static partial Rect2I godotsharp_variant_as_rect2i(in godot_variant p_self);
+        public static partial Rect2I godotsharp_variant_as_rect2i(scoped in godot_variant p_self);
 
-        public static partial Vector3 godotsharp_variant_as_vector3(in godot_variant p_self);
+        public static partial Vector3 godotsharp_variant_as_vector3(scoped in godot_variant p_self);
 
-        public static partial Vector3I godotsharp_variant_as_vector3i(in godot_variant p_self);
+        public static partial Vector3I godotsharp_variant_as_vector3i(scoped in godot_variant p_self);
 
-        public static partial Transform2D godotsharp_variant_as_transform2d(in godot_variant p_self);
+        public static partial Transform2D godotsharp_variant_as_transform2d(scoped in godot_variant p_self);
 
-        public static partial Vector4 godotsharp_variant_as_vector4(in godot_variant p_self);
+        public static partial Vector4 godotsharp_variant_as_vector4(scoped in godot_variant p_self);
 
-        public static partial Vector4I godotsharp_variant_as_vector4i(in godot_variant p_self);
+        public static partial Vector4I godotsharp_variant_as_vector4i(scoped in godot_variant p_self);
 
-        public static partial Plane godotsharp_variant_as_plane(in godot_variant p_self);
+        public static partial Plane godotsharp_variant_as_plane(scoped in godot_variant p_self);
 
-        public static partial Quaternion godotsharp_variant_as_quaternion(in godot_variant p_self);
+        public static partial Quaternion godotsharp_variant_as_quaternion(scoped in godot_variant p_self);
 
-        public static partial Aabb godotsharp_variant_as_aabb(in godot_variant p_self);
+        public static partial Aabb godotsharp_variant_as_aabb(scoped in godot_variant p_self);
 
-        public static partial Basis godotsharp_variant_as_basis(in godot_variant p_self);
+        public static partial Basis godotsharp_variant_as_basis(scoped in godot_variant p_self);
 
-        public static partial Transform3D godotsharp_variant_as_transform3d(in godot_variant p_self);
+        public static partial Transform3D godotsharp_variant_as_transform3d(scoped in godot_variant p_self);
 
-        public static partial Projection godotsharp_variant_as_projection(in godot_variant p_self);
+        public static partial Projection godotsharp_variant_as_projection(scoped in godot_variant p_self);
 
-        public static partial Color godotsharp_variant_as_color(in godot_variant p_self);
+        public static partial Color godotsharp_variant_as_color(scoped in godot_variant p_self);
 
         public static partial godot_string_name godotsharp_variant_as_string_name(scoped in godot_variant p_self);
 
         public static partial godot_node_path godotsharp_variant_as_node_path(scoped in godot_variant p_self);
 
-        public static partial Rid godotsharp_variant_as_rid(in godot_variant p_self);
+        public static partial Rid godotsharp_variant_as_rid(scoped in godot_variant p_self);
 
         public static partial godot_callable godotsharp_variant_as_callable(scoped in godot_variant p_self);
 
@@ -293,27 +293,22 @@ namespace Godot.NativeInterop
 
         public static partial godot_packed_int64_array godotsharp_variant_as_packed_int64_array(scoped in godot_variant p_self);
 
-        public static partial godot_packed_float32_array godotsharp_variant_as_packed_float32_array(
-            scoped in godot_variant p_self);
+        public static partial godot_packed_float32_array godotsharp_variant_as_packed_float32_array(scoped in godot_variant p_self);
 
-        public static partial godot_packed_float64_array godotsharp_variant_as_packed_float64_array(
-            scoped in godot_variant p_self);
+        public static partial godot_packed_float64_array godotsharp_variant_as_packed_float64_array(scoped in godot_variant p_self);
 
-        public static partial godot_packed_string_array godotsharp_variant_as_packed_string_array(
-            scoped in godot_variant p_self);
+        public static partial godot_packed_string_array godotsharp_variant_as_packed_string_array(scoped in godot_variant p_self);
 
-        public static partial godot_packed_vector2_array godotsharp_variant_as_packed_vector2_array(
-            scoped in godot_variant p_self);
+        public static partial godot_packed_vector2_array godotsharp_variant_as_packed_vector2_array(scoped in godot_variant p_self);
 
-        public static partial godot_packed_vector3_array godotsharp_variant_as_packed_vector3_array(
-            scoped in godot_variant p_self);
+        public static partial godot_packed_vector3_array godotsharp_variant_as_packed_vector3_array(scoped in godot_variant p_self);
 
         public static partial godot_packed_vector4_array godotsharp_variant_as_packed_vector4_array(
             in godot_variant p_self);
 
         public static partial godot_packed_color_array godotsharp_variant_as_packed_color_array(scoped in godot_variant p_self);
 
-        public static partial godot_bool godotsharp_variant_equals(in godot_variant p_a, in godot_variant p_b);
+        public static partial godot_bool godotsharp_variant_equals(scoped in godot_variant p_a, scoped in godot_variant p_b);
 
         // string.h
 
diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/VariantUtils.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/VariantUtils.cs
index 5ff9969cfd..a48e60a30f 100644
--- a/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/VariantUtils.cs
+++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/VariantUtils.cs
@@ -248,13 +248,28 @@ namespace Godot.NativeInterop
         }
 
         public static godot_variant CreateFromSystemArrayOfStringName(scoped Span<StringName> from)
-            => CreateFromArray(new Collections.Array(from));
+        {
+            if (from == null)
+                return default;
+            using var fromGodot = new Collections.Array(from);
+            return CreateFromArray((godot_array)fromGodot.NativeValue);
+        }
 
         public static godot_variant CreateFromSystemArrayOfNodePath(scoped Span<NodePath> from)
-            => CreateFromArray(new Collections.Array(from));
+        {
+            if (from == null)
+                return default;
+            using var fromGodot = new Collections.Array(from);
+            return CreateFromArray((godot_array)fromGodot.NativeValue);
+        }
 
         public static godot_variant CreateFromSystemArrayOfRid(scoped Span<Rid> from)
-            => CreateFromArray(new Collections.Array(from));
+        {
+            if (from == null)
+                return default;
+            using var fromGodot = new Collections.Array(from);
+            return CreateFromArray((godot_array)fromGodot.NativeValue);
+        }
 
         public static godot_variant CreateFromSystemArrayOfGodotObject(GodotObject[]? from)
         {
diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/VariantUtils.generic.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/VariantUtils.generic.cs
index a5ef11404f..d8c8ea568f 100644
--- a/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/VariantUtils.generic.cs
+++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/NativeInterop/VariantUtils.generic.cs
@@ -7,24 +7,23 @@ namespace Godot.NativeInterop;
 
 public partial class VariantUtils
 {
-    private static Exception UnsupportedType<T>() => new InvalidOperationException(
+    private static InvalidOperationException UnsupportedType<T>() => new InvalidOperationException(
         $"The type is not supported for conversion to/from Variant: '{typeof(T).FullName}'");
 
     internal static class GenericConversion<T>
     {
+        internal delegate godot_variant ToVariantConverter(scoped in T from);
+        internal delegate T FromVariantConverter(in godot_variant from);
+
         public static unsafe godot_variant ToVariant(scoped in T from) =>
-#pragma warning disable CS9088 // the delegate pointer cannot be marked scoped, but it should be
-            ToVariantCb != null ? ToVariantCb(from) : throw UnsupportedType<T>();
-#pragma warning restore CS9088
+             ToVariantCb != null ? ToVariantCb(from) : throw UnsupportedType<T>();
 
         public static unsafe T FromVariant(in godot_variant variant) =>
             FromVariantCb != null ? FromVariantCb(variant) : throw UnsupportedType<T>();
 
-        // ReSharper disable once StaticMemberInGenericType
-        internal static unsafe delegate*<in T, godot_variant> ToVariantCb;
+        internal static ToVariantConverter? ToVariantCb;
 
-        // ReSharper disable once StaticMemberInGenericType
-        internal static unsafe delegate*<in godot_variant, T> FromVariantCb;
+        internal static FromVariantConverter? FromVariantCb;
 
         static GenericConversion()
         {
@@ -36,7 +35,7 @@ public partial class VariantUtils
     public static godot_variant CreateFrom<[MustBeVariant] T>(scoped in T from)
     {
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        static TTo UnsafeAs<TTo>(in T f) => Unsafe.As<T, TTo>(ref Unsafe.AsRef(f));
+        static TTo UnsafeAs<TTo>(in T f) => Unsafe.As<T, TTo>(ref Unsafe.AsRef(in f));
 
         // `typeof(T) == typeof(X)` is optimized away. We cannot cache `typeof(T)` in a local variable, as it's not optimized when done like that.
 
@@ -229,7 +228,7 @@ public partial class VariantUtils
     public static T ConvertTo<[MustBeVariant] T>(in godot_variant variant)
     {
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        static T UnsafeAsT<TFrom>(TFrom f) => Unsafe.As<TFrom, T>(ref Unsafe.AsRef(f));
+        static T UnsafeAsT<TFrom>(TFrom f) => Unsafe.As<TFrom, T>(ref Unsafe.AsRef(in f));
 
         if (typeof(T) == typeof(bool))
             return UnsafeAsT(ConvertToBool(variant));
diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/StringExtensions.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/StringExtensions.cs
index ad75195f0f..c79c45fa08 100644
--- a/modules/mono/glue/GodotSharp/GodotSharp/Core/StringExtensions.cs
+++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/StringExtensions.cs
@@ -1581,7 +1581,7 @@ namespace Godot
                 if (end < 0)
                     end = len;
                 if (allowEmpty || end > from)
-                    ret.Add(float.Parse(instance.Substring(from), CultureInfo.InvariantCulture));
+                    ret.Add(float.Parse(instance.AsSpan(from), CultureInfo.InvariantCulture));
                 if (end == len)
                     break;
 
diff --git a/modules/mono/glue/GodotSharp/GodotSharp/GodotSharp.csproj b/modules/mono/glue/GodotSharp/GodotSharp/GodotSharp.csproj
index 09516cc893..ab9c5fe18a 100644
--- a/modules/mono/glue/GodotSharp/GodotSharp/GodotSharp.csproj
+++ b/modules/mono/glue/GodotSharp/GodotSharp/GodotSharp.csproj
@@ -1,17 +1,19 @@
 <Project Sdk="Microsoft.NET.Sdk">
+
   <PropertyGroup>
     <ProjectGuid>{AEBF0036-DA76-4341-B651-A3F2856AB2FA}</ProjectGuid>
+    <TargetFramework>net8.0</TargetFramework>
+    <LangVersion>12</LangVersion>
     <OutputPath>bin/$(Configuration)</OutputPath>
     <AppendTargetFrameworkToOutputPath>false</AppendTargetFrameworkToOutputPath>
     <RootNamespace>Godot</RootNamespace>
-    <TargetFramework>net7.0</TargetFramework>
     <DocumentationFile>$(OutputPath)/$(AssemblyName).xml</DocumentationFile>
     <EnableDefaultItems>false</EnableDefaultItems>
     <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
-    <LangVersion>11</LangVersion>
 
     <AnalysisMode>Recommended</AnalysisMode>
   </PropertyGroup>
+
   <PropertyGroup>
     <Description>Godot C# Core API.</Description>
     <Authors>Godot Engine contributors</Authors>
@@ -28,23 +30,28 @@
     <IncludeSymbols>true</IncludeSymbols>
     <SymbolPackageFormat>snupkg</SymbolPackageFormat>
   </PropertyGroup>
+
   <ItemGroup>
     <!-- SdkPackageVersions.props for easy access -->
     <None Include="$(GodotSdkPackageVersionsFilePath)">
       <Link>SdkPackageVersions.props</Link>
     </None>
   </ItemGroup>
+
   <PropertyGroup>
     <DefineConstants>$(DefineConstants);GODOT</DefineConstants>
     <DefineConstants Condition=" '$(GodotFloat64)' == 'true' ">REAL_T_IS_DOUBLE;$(DefineConstants)</DefineConstants>
   </PropertyGroup>
+
   <ItemGroup>
     <PackageReference Include="ReflectionAnalyzers" Version="0.1.22-dev" PrivateAssets="all" IncludeAssets="runtime; build; native; contentfiles; analyzers" />
     <!--PackageReference Include="IDisposableAnalyzers" Version="3.4.13" PrivateAssets="all" IncludeAssets="runtime; build; native; contentfiles; analyzers" /-->
   </ItemGroup>
+
   <ItemGroup>
     <ProjectReference Include="..\Godot.SourceGenerators.Internal\Godot.SourceGenerators.Internal.csproj" OutputItemType="Analyzer" ReferenceOutputAssembly="false" />
   </ItemGroup>
+
   <!-- Sources -->
   <ItemGroup>
     <Compile Include="Core\Aabb.cs" />
@@ -135,10 +142,12 @@
     <Compile Include="GlobalUsings.cs" />
     <Compile Include="Properties\AssemblyInfo.cs" />
   </ItemGroup>
+
   <!-- Compat Sources -->
   <ItemGroup Condition=" '$(GodotNoDeprecated)' == '' ">
     <Compile Include="Compat.cs" />
   </ItemGroup>
+
   <!--
   We import a props file with auto-generated includes. This works well with Rider.
   However, Visual Studio and MonoDevelop won't list them in the solution explorer.
diff --git a/modules/mono/glue/GodotSharp/GodotSharpEditor/GodotSharpEditor.csproj b/modules/mono/glue/GodotSharp/GodotSharpEditor/GodotSharpEditor.csproj
index fcc6bd7a4d..0ccc463dd4 100644
--- a/modules/mono/glue/GodotSharp/GodotSharpEditor/GodotSharpEditor.csproj
+++ b/modules/mono/glue/GodotSharp/GodotSharpEditor/GodotSharpEditor.csproj
@@ -1,14 +1,14 @@
 <Project Sdk="Microsoft.NET.Sdk">
   <PropertyGroup>
     <ProjectGuid>{8FBEC238-D944-4074-8548-B3B524305905}</ProjectGuid>
+    <TargetFramework>net8.0</TargetFramework>
+    <LangVersion>12</LangVersion>
     <OutputPath>bin/$(Configuration)</OutputPath>
     <AppendTargetFrameworkToOutputPath>false</AppendTargetFrameworkToOutputPath>
     <RootNamespace>Godot</RootNamespace>
-    <TargetFramework>net7.0</TargetFramework>
     <DocumentationFile>$(OutputPath)/$(AssemblyName).xml</DocumentationFile>
     <EnableDefaultItems>false</EnableDefaultItems>
     <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
-    <LangVersion>10</LangVersion>
   </PropertyGroup>
   <PropertyGroup>
     <Description>Godot C# Editor API.</Description>

From e674379764e0864a001a0859bf3ee36eef4a8340 Mon Sep 17 00:00:00 2001
From: HP van Braam <hp@tmm.cx>
Date: Thu, 12 Dec 2024 17:21:15 +0100
Subject: [PATCH 117/124] Fix several ubsan reported misaligned accesses

These misaligned accesses are shown in all of our CI hooks. It turned
out to not be difficult to fix.

It is likely that this will improve performance for aarch64.
---
 drivers/vulkan/rendering_device_driver_vulkan.cpp | 11 +++++++----
 drivers/vulkan/rendering_device_driver_vulkan.h   |  3 ++-
 servers/rendering/rendering_device_graph.cpp      |  1 +
 3 files changed, 10 insertions(+), 5 deletions(-)

diff --git a/drivers/vulkan/rendering_device_driver_vulkan.cpp b/drivers/vulkan/rendering_device_driver_vulkan.cpp
index ae6cddc987..5917c41de8 100644
--- a/drivers/vulkan/rendering_device_driver_vulkan.cpp
+++ b/drivers/vulkan/rendering_device_driver_vulkan.cpp
@@ -3441,7 +3441,7 @@ Vector<uint8_t> RenderingDeviceDriverVulkan::shader_compile_binary_from_spirv(Ve
 
 	binary_data.shader_name_len = shader_name_utf.length();
 
-	uint32_t total_size = sizeof(uint32_t) * 3; // Header + version + main datasize;.
+	uint32_t total_size = sizeof(uint32_t) * 4; // Header + version + pad + main datasize;.
 	total_size += sizeof(ShaderBinary::Data);
 
 	total_size += STEPIFY(binary_data.shader_name_len, 4);
@@ -3470,6 +3470,8 @@ Vector<uint8_t> RenderingDeviceDriverVulkan::shader_compile_binary_from_spirv(Ve
 		offset += sizeof(uint32_t);
 		encode_uint32(sizeof(ShaderBinary::Data), binptr + offset);
 		offset += sizeof(uint32_t);
+		encode_uint32(0, binptr + offset); // Pad to align ShaderBinary::Data to 8 bytes.
+		offset += sizeof(uint32_t);
 		memcpy(binptr + offset, &binary_data, sizeof(ShaderBinary::Data));
 		offset += sizeof(ShaderBinary::Data);
 
@@ -3528,7 +3530,7 @@ RDD::ShaderID RenderingDeviceDriverVulkan::shader_create_from_bytecode(const Vec
 	uint32_t read_offset = 0;
 
 	// Consistency check.
-	ERR_FAIL_COND_V(binsize < sizeof(uint32_t) * 3 + sizeof(ShaderBinary::Data), ShaderID());
+	ERR_FAIL_COND_V(binsize < sizeof(uint32_t) * 4 + sizeof(ShaderBinary::Data), ShaderID());
 	ERR_FAIL_COND_V(binptr[0] != 'G' || binptr[1] != 'S' || binptr[2] != 'B' || binptr[3] != 'D', ShaderID());
 
 	uint32_t bin_version = decode_uint32(binptr + 4);
@@ -3536,7 +3538,8 @@ RDD::ShaderID RenderingDeviceDriverVulkan::shader_create_from_bytecode(const Vec
 
 	uint32_t bin_data_size = decode_uint32(binptr + 8);
 
-	const ShaderBinary::Data &binary_data = *(reinterpret_cast<const ShaderBinary::Data *>(binptr + 12));
+	// 16, not 12, to skip alignment padding.
+	const ShaderBinary::Data &binary_data = *(reinterpret_cast<const ShaderBinary::Data *>(binptr + 16));
 
 	r_shader_desc.push_constant_size = binary_data.push_constant_size;
 	shader_info.vk_push_constant_stages = binary_data.vk_push_constant_stages_mask;
@@ -3549,7 +3552,7 @@ RDD::ShaderID RenderingDeviceDriverVulkan::shader_create_from_bytecode(const Vec
 	r_shader_desc.compute_local_size[1] = binary_data.compute_local_size[1];
 	r_shader_desc.compute_local_size[2] = binary_data.compute_local_size[2];
 
-	read_offset += sizeof(uint32_t) * 3 + bin_data_size;
+	read_offset += sizeof(uint32_t) * 4 + bin_data_size;
 
 	if (binary_data.shader_name_len) {
 		r_name.parse_utf8((const char *)(binptr + read_offset), binary_data.shader_name_len);
diff --git a/drivers/vulkan/rendering_device_driver_vulkan.h b/drivers/vulkan/rendering_device_driver_vulkan.h
index ba47848345..4eec7547f5 100644
--- a/drivers/vulkan/rendering_device_driver_vulkan.h
+++ b/drivers/vulkan/rendering_device_driver_vulkan.h
@@ -405,7 +405,8 @@ private:
 		// Version 2: Added shader name.
 		// Version 3: Added writable.
 		// Version 4: 64-bit vertex input mask.
-		static const uint32_t VERSION = 4;
+		// Version 5: Add 4 bytes padding to align the Data struct after the change in version 4.
+		static const uint32_t VERSION = 5;
 
 		struct DataBinding {
 			uint32_t type = 0;
diff --git a/servers/rendering/rendering_device_graph.cpp b/servers/rendering/rendering_device_graph.cpp
index 5b17657d9f..6f17fde253 100644
--- a/servers/rendering/rendering_device_graph.cpp
+++ b/servers/rendering/rendering_device_graph.cpp
@@ -228,6 +228,7 @@ int32_t RenderingDeviceGraph::_add_to_write_list(int32_t p_command_index, Rect2i
 
 RenderingDeviceGraph::RecordedCommand *RenderingDeviceGraph::_allocate_command(uint32_t p_command_size, int32_t &r_command_index) {
 	uint32_t command_data_offset = command_data.size();
+	command_data_offset = STEPIFY(command_data_offset, 8);
 	command_data_offsets.push_back(command_data_offset);
 	command_data.resize(command_data_offset + p_command_size);
 	r_command_index = command_count++;

From 383d933f5323830e3f38c13a49c9c363ea73bced Mon Sep 17 00:00:00 2001
From: Lukas Tenbrink <lukas.tenbrink@gmail.com>
Date: Fri, 13 Dec 2024 16:12:26 +0100
Subject: [PATCH 118/124] Optimize `SWAP` macro by using move semantics.

---
 core/typedefs.h | 9 ++-------
 1 file changed, 2 insertions(+), 7 deletions(-)

diff --git a/core/typedefs.h b/core/typedefs.h
index 35c4668581..b5b44f2169 100644
--- a/core/typedefs.h
+++ b/core/typedefs.h
@@ -43,6 +43,7 @@
 // Should be available everywhere.
 #include "core/error/error_list.h"
 #include <cstdint>
+#include <utility>
 
 // Ensure that C++ standard is at least C++17. If on MSVC, also ensures that the `Zc:__cplusplus` flag is present.
 static_assert(__cplusplus >= 201703L);
@@ -129,13 +130,7 @@ constexpr auto CLAMP(const T m_a, const T2 m_min, const T3 m_max) {
 
 // Generic swap template.
 #ifndef SWAP
-#define SWAP(m_x, m_y) __swap_tmpl((m_x), (m_y))
-template <typename T>
-inline void __swap_tmpl(T &x, T &y) {
-	T aux = x;
-	x = y;
-	y = aux;
-}
+#define SWAP(m_x, m_y) std::swap((m_x), (m_y))
 #endif // SWAP
 
 /* Functions to handle powers of 2 and shifting. */

From 49ca40cdbf804d604af04b953b20f54837d76a51 Mon Sep 17 00:00:00 2001
From: Hugo Locurcio <hugo.locurcio@hugo.pro>
Date: Fri, 13 Dec 2024 16:41:41 +0100
Subject: [PATCH 119/124] Document `GPUParticles.emit_particle()` only working
 on Forward+ and Mobile

---
 doc/classes/GPUParticles2D.xml | 1 +
 doc/classes/GPUParticles3D.xml | 1 +
 2 files changed, 2 insertions(+)

diff --git a/doc/classes/GPUParticles2D.xml b/doc/classes/GPUParticles2D.xml
index 96232f4277..41f7b02ca6 100644
--- a/doc/classes/GPUParticles2D.xml
+++ b/doc/classes/GPUParticles2D.xml
@@ -38,6 +38,7 @@
 			<description>
 				Emits a single particle. Whether [param xform], [param velocity], [param color] and [param custom] are applied depends on the value of [param flags]. See [enum EmitFlags].
 				The default ParticleProcessMaterial will overwrite [param color] and use the contents of [param custom] as [code](rotation, age, animation, lifetime)[/code].
+				[b]Note:[/b] [method emit_particle] is only supported on the Forward+ and Mobile rendering methods, not Compatibility.
 			</description>
 		</method>
 		<method name="restart">
diff --git a/doc/classes/GPUParticles3D.xml b/doc/classes/GPUParticles3D.xml
index f5df857391..89958ed217 100644
--- a/doc/classes/GPUParticles3D.xml
+++ b/doc/classes/GPUParticles3D.xml
@@ -36,6 +36,7 @@
 			<description>
 				Emits a single particle. Whether [param xform], [param velocity], [param color] and [param custom] are applied depends on the value of [param flags]. See [enum EmitFlags].
 				The default ParticleProcessMaterial will overwrite [param color] and use the contents of [param custom] as [code](rotation, age, animation, lifetime)[/code].
+				[b]Note:[/b] [method emit_particle] is only supported on the Forward+ and Mobile rendering methods, not Compatibility.
 			</description>
 		</method>
 		<method name="get_draw_pass_mesh" qualifiers="const">

From 062d74bb9cb8710dbea34442a53af6aad796a453 Mon Sep 17 00:00:00 2001
From: HP van Braam <hp@tmm.cx>
Date: Fri, 13 Dec 2024 19:03:37 +0100
Subject: [PATCH 120/124] Fix ubsan reported errors in rendering

This allows the TPS demo to run without an ubsan reports from any of the
rendering code.
---
 .../d3d12/rendering_context_driver_d3d12.cpp  |  2 +-
 .../metal/rendering_context_driver_metal.mm   |  2 +-
 .../rendering_context_driver_vulkan.cpp       |  4 +--
 .../vulkan/rendering_device_driver_vulkan.cpp |  4 +--
 .../render_forward_clustered.cpp              |  6 ++--
 .../scene_shader_forward_clustered.cpp        | 30 +++++++++---------
 .../scene_shader_forward_clustered.h          | 31 ++++++++++---------
 .../renderer_rd/renderer_canvas_render_rd.h   |  2 +-
 servers/rendering/renderer_scene_cull.cpp     | 30 +++++++++---------
 servers/rendering/renderer_scene_cull.h       |  2 +-
 servers/rendering/rendering_context_driver.h  | 26 +++++++++-------
 servers/rendering/rendering_device.cpp        | 18 +++++------
 12 files changed, 81 insertions(+), 76 deletions(-)

diff --git a/drivers/d3d12/rendering_context_driver_d3d12.cpp b/drivers/d3d12/rendering_context_driver_d3d12.cpp
index 7cd36f90d2..7081d1730f 100644
--- a/drivers/d3d12/rendering_context_driver_d3d12.cpp
+++ b/drivers/d3d12/rendering_context_driver_d3d12.cpp
@@ -184,7 +184,7 @@ Error RenderingContextDriverD3D12::_initialize_devices() {
 
 		Device &device = driver_devices[i];
 		device.name = desc.Description;
-		device.vendor = Vendor(desc.VendorId);
+		device.vendor = desc.VendorId;
 		device.workarounds = Workarounds();
 
 		if (desc.Flags & DXGI_ADAPTER_FLAG_SOFTWARE) {
diff --git a/drivers/metal/rendering_context_driver_metal.mm b/drivers/metal/rendering_context_driver_metal.mm
index ac53f73447..53e5e98144 100644
--- a/drivers/metal/rendering_context_driver_metal.mm
+++ b/drivers/metal/rendering_context_driver_metal.mm
@@ -52,7 +52,7 @@ Error RenderingContextDriverMetal::initialize() {
 	}
 #endif
 	device.type = DEVICE_TYPE_INTEGRATED_GPU;
-	device.vendor = VENDOR_APPLE;
+	device.vendor = Vendor::VENDOR_APPLE;
 	device.workarounds = Workarounds();
 
 	MetalDeviceProperties props(metal_device);
diff --git a/drivers/vulkan/rendering_context_driver_vulkan.cpp b/drivers/vulkan/rendering_context_driver_vulkan.cpp
index df9bd98624..3c1aa323f4 100644
--- a/drivers/vulkan/rendering_context_driver_vulkan.cpp
+++ b/drivers/vulkan/rendering_context_driver_vulkan.cpp
@@ -843,7 +843,7 @@ Error RenderingContextDriverVulkan::_initialize_devices() {
 
 		Device &driver_device = driver_devices[i];
 		driver_device.name = String::utf8(props.deviceName);
-		driver_device.vendor = Vendor(props.vendorID);
+		driver_device.vendor = props.vendorID;
 		driver_device.type = DeviceType(props.deviceType);
 		driver_device.workarounds = Workarounds();
 
@@ -880,7 +880,7 @@ void RenderingContextDriverVulkan::_check_driver_workarounds(const VkPhysicalDev
 	// This bug was fixed in driver version 512.503.0, so we only enabled it on devices older than this.
 	//
 	r_device.workarounds.avoid_compute_after_draw =
-			r_device.vendor == VENDOR_QUALCOMM &&
+			r_device.vendor == Vendor::VENDOR_QUALCOMM &&
 			p_device_properties.deviceID >= 0x6000000 && // Adreno 6xx
 			p_device_properties.driverVersion < VK_MAKE_VERSION(512, 503, 0) &&
 			r_device.name.find("Turnip") < 0;
diff --git a/drivers/vulkan/rendering_device_driver_vulkan.cpp b/drivers/vulkan/rendering_device_driver_vulkan.cpp
index 3189ab0bd1..a9002785f0 100644
--- a/drivers/vulkan/rendering_device_driver_vulkan.cpp
+++ b/drivers/vulkan/rendering_device_driver_vulkan.cpp
@@ -1898,10 +1898,10 @@ RDD::TextureID RenderingDeviceDriverVulkan::texture_create_shared(TextureID p_or
 				vkGetPhysicalDeviceFormatProperties(physical_device, RD_TO_VK_FORMAT[p_view.format], &properties);
 				const VkFormatFeatureFlags &supported_flags = owner_tex_info->vk_create_info.tiling == VK_IMAGE_TILING_LINEAR ? properties.linearTilingFeatures : properties.optimalTilingFeatures;
 				if ((usage_info->usage & VK_IMAGE_USAGE_STORAGE_BIT) && !(supported_flags & VK_FORMAT_FEATURE_STORAGE_IMAGE_BIT)) {
-					usage_info->usage &= ~VK_IMAGE_USAGE_STORAGE_BIT;
+					usage_info->usage &= ~uint32_t(VK_IMAGE_USAGE_STORAGE_BIT);
 				}
 				if ((usage_info->usage & VK_IMAGE_USAGE_COLOR_ATTACHMENT_BIT) && !(supported_flags & VK_FORMAT_FEATURE_COLOR_ATTACHMENT_BIT)) {
-					usage_info->usage &= ~VK_IMAGE_USAGE_COLOR_ATTACHMENT_BIT;
+					usage_info->usage &= ~uint32_t(VK_IMAGE_USAGE_COLOR_ATTACHMENT_BIT);
 				}
 			}
 
diff --git a/servers/rendering/renderer_rd/forward_clustered/render_forward_clustered.cpp b/servers/rendering/renderer_rd/forward_clustered/render_forward_clustered.cpp
index 4084465033..0c62513e1a 100644
--- a/servers/rendering/renderer_rd/forward_clustered/render_forward_clustered.cpp
+++ b/servers/rendering/renderer_rd/forward_clustered/render_forward_clustered.cpp
@@ -2099,7 +2099,7 @@ void RenderForwardClustered::_render_scene(RenderDataRD *p_render_data, const Co
 				}
 			}
 
-			uint32_t opaque_color_pass_flags = using_motion_pass ? (color_pass_flags & ~COLOR_PASS_FLAG_MOTION_VECTORS) : color_pass_flags;
+			uint32_t opaque_color_pass_flags = using_motion_pass ? (color_pass_flags & ~uint32_t(COLOR_PASS_FLAG_MOTION_VECTORS)) : color_pass_flags;
 			RID opaque_framebuffer = using_motion_pass ? rb_data->get_color_pass_fb(opaque_color_pass_flags) : color_framebuffer;
 			RenderListParameters render_list_params(render_list[RENDER_LIST_OPAQUE].elements.ptr(), render_list[RENDER_LIST_OPAQUE].element_info.ptr(), render_list[RENDER_LIST_OPAQUE].elements.size(), reverse_cull, PASS_MODE_COLOR, opaque_color_pass_flags, rb_data.is_null(), p_render_data->directional_light_soft_shadows, rp_uniform_set, get_debug_draw_mode() == RS::VIEWPORT_DEBUG_DRAW_WIREFRAME, Vector2(), p_render_data->scene_data->lod_distance_multiplier, p_render_data->scene_data->screen_mesh_lod_threshold, p_render_data->scene_data->view_count, 0, base_specialization);
 			_render_list_with_draw_list(&render_list_params, opaque_framebuffer, RD::DrawFlags(load_color ? RD::DRAW_DEFAULT_ALL : RD::DRAW_CLEAR_COLOR_ALL) | (depth_pre_pass ? RD::DRAW_DEFAULT_ALL : RD::DRAW_CLEAR_DEPTH), c, 0.0f);
@@ -2293,10 +2293,10 @@ void RenderForwardClustered::_render_scene(RenderDataRD *p_render_data, const Co
 	_setup_environment(p_render_data, is_reflection_probe, screen_size, p_default_bg_color, false);
 
 	{
-		uint32_t transparent_color_pass_flags = (color_pass_flags | COLOR_PASS_FLAG_TRANSPARENT) & ~(COLOR_PASS_FLAG_SEPARATE_SPECULAR);
+		uint32_t transparent_color_pass_flags = (color_pass_flags | uint32_t(COLOR_PASS_FLAG_TRANSPARENT)) & ~uint32_t(COLOR_PASS_FLAG_SEPARATE_SPECULAR);
 		if (using_motion_pass) {
 			// Motion vectors on transparent draw calls are not required when using the reactive mask.
-			transparent_color_pass_flags &= ~(COLOR_PASS_FLAG_MOTION_VECTORS);
+			transparent_color_pass_flags &= ~uint32_t(COLOR_PASS_FLAG_MOTION_VECTORS);
 		}
 
 		RID alpha_framebuffer = rb_data.is_valid() ? rb_data->get_color_pass_fb(transparent_color_pass_flags) : color_only_framebuffer;
diff --git a/servers/rendering/renderer_rd/forward_clustered/scene_shader_forward_clustered.cpp b/servers/rendering/renderer_rd/forward_clustered/scene_shader_forward_clustered.cpp
index 6dadbf25ac..faf9927746 100644
--- a/servers/rendering/renderer_rd/forward_clustered/scene_shader_forward_clustered.cpp
+++ b/servers/rendering/renderer_rd/forward_clustered/scene_shader_forward_clustered.cpp
@@ -222,27 +222,27 @@ RS::ShaderNativeSourceCode SceneShaderForwardClustered::ShaderData::get_native_s
 	}
 }
 
-SceneShaderForwardClustered::ShaderVersion SceneShaderForwardClustered::ShaderData::_get_shader_version(PipelineVersion p_pipeline_version, uint32_t p_color_pass_flags, bool p_ubershader) const {
-	uint32_t ubershader_base = p_ubershader ? SHADER_VERSION_DEPTH_PASS_WITH_MATERIAL : 0;
+uint16_t SceneShaderForwardClustered::ShaderData::_get_shader_version(PipelineVersion p_pipeline_version, uint32_t p_color_pass_flags, bool p_ubershader) const {
+	uint32_t ubershader_base = p_ubershader ? ShaderVersion::SHADER_VERSION_DEPTH_PASS_WITH_MATERIAL : 0;
 	switch (p_pipeline_version) {
 		case PIPELINE_VERSION_DEPTH_PASS:
-			return ShaderVersion(SHADER_VERSION_DEPTH_PASS + ubershader_base);
+			return ShaderVersion::SHADER_VERSION_DEPTH_PASS + ubershader_base;
 		case PIPELINE_VERSION_DEPTH_PASS_DP:
-			return ShaderVersion(SHADER_VERSION_DEPTH_PASS_DP + ubershader_base);
+			return ShaderVersion::SHADER_VERSION_DEPTH_PASS_DP + ubershader_base;
 		case PIPELINE_VERSION_DEPTH_PASS_WITH_NORMAL_AND_ROUGHNESS:
-			return ShaderVersion(SHADER_VERSION_DEPTH_PASS_WITH_NORMAL_AND_ROUGHNESS + ubershader_base);
+			return ShaderVersion::SHADER_VERSION_DEPTH_PASS_WITH_NORMAL_AND_ROUGHNESS + ubershader_base;
 		case PIPELINE_VERSION_DEPTH_PASS_WITH_NORMAL_AND_ROUGHNESS_AND_VOXEL_GI:
-			return ShaderVersion(SHADER_VERSION_DEPTH_PASS_WITH_NORMAL_AND_ROUGHNESS_AND_VOXEL_GI + ubershader_base);
+			return ShaderVersion::SHADER_VERSION_DEPTH_PASS_WITH_NORMAL_AND_ROUGHNESS_AND_VOXEL_GI + ubershader_base;
 		case PIPELINE_VERSION_DEPTH_PASS_MULTIVIEW:
-			return ShaderVersion(SHADER_VERSION_DEPTH_PASS_MULTIVIEW + ubershader_base);
+			return ShaderVersion::SHADER_VERSION_DEPTH_PASS_MULTIVIEW + ubershader_base;
 		case PIPELINE_VERSION_DEPTH_PASS_WITH_NORMAL_AND_ROUGHNESS_MULTIVIEW:
-			return ShaderVersion(SHADER_VERSION_DEPTH_PASS_WITH_NORMAL_AND_ROUGHNESS_MULTIVIEW + ubershader_base);
+			return ShaderVersion::SHADER_VERSION_DEPTH_PASS_WITH_NORMAL_AND_ROUGHNESS_MULTIVIEW + ubershader_base;
 		case PIPELINE_VERSION_DEPTH_PASS_WITH_NORMAL_AND_ROUGHNESS_AND_VOXEL_GI_MULTIVIEW:
-			return ShaderVersion(SHADER_VERSION_DEPTH_PASS_WITH_NORMAL_AND_ROUGHNESS_AND_VOXEL_GI_MULTIVIEW + ubershader_base);
+			return ShaderVersion::SHADER_VERSION_DEPTH_PASS_WITH_NORMAL_AND_ROUGHNESS_AND_VOXEL_GI_MULTIVIEW + ubershader_base;
 		case PIPELINE_VERSION_DEPTH_PASS_WITH_MATERIAL:
-			return ShaderVersion(SHADER_VERSION_DEPTH_PASS_WITH_MATERIAL + SHADER_VERSION_DEPTH_PASS_WITH_MATERIAL);
+			return ShaderVersion::SHADER_VERSION_DEPTH_PASS_WITH_MATERIAL + ShaderVersion::SHADER_VERSION_DEPTH_PASS_WITH_MATERIAL;
 		case PIPELINE_VERSION_DEPTH_PASS_WITH_SDF:
-			return ShaderVersion(SHADER_VERSION_DEPTH_PASS_WITH_MATERIAL + SHADER_VERSION_DEPTH_PASS_WITH_SDF);
+			return ShaderVersion::SHADER_VERSION_DEPTH_PASS_WITH_MATERIAL + ShaderVersion::SHADER_VERSION_DEPTH_PASS_WITH_SDF;
 		case PIPELINE_VERSION_COLOR_PASS: {
 			int shader_flags = 0;
 
@@ -266,11 +266,11 @@ SceneShaderForwardClustered::ShaderVersion SceneShaderForwardClustered::ShaderDa
 				shader_flags |= SHADER_COLOR_PASS_FLAG_MULTIVIEW;
 			}
 
-			return ShaderVersion(SHADER_VERSION_DEPTH_PASS_WITH_MATERIAL + SHADER_VERSION_COLOR_PASS + shader_flags);
+			return ShaderVersion::SHADER_VERSION_DEPTH_PASS_WITH_MATERIAL + ShaderVersion::SHADER_VERSION_COLOR_PASS + shader_flags;
 		} break;
 		default: {
 			DEV_ASSERT(false && "Unknown pipeline version.");
-			return ShaderVersion(0);
+			return 0;
 		} break;
 	}
 }
@@ -404,7 +404,7 @@ RD::PolygonCullMode SceneShaderForwardClustered::ShaderData::get_cull_mode_from_
 	return cull_mode_rd_table[p_cull_variant][cull_mode];
 }
 
-RID SceneShaderForwardClustered::ShaderData::_get_shader_variant(ShaderVersion p_shader_version) const {
+RID SceneShaderForwardClustered::ShaderData::_get_shader_variant(uint16_t p_shader_version) const {
 	if (version.is_valid()) {
 		MutexLock lock(SceneShaderForwardClustered::singleton_mutex);
 		ERR_FAIL_NULL_V(SceneShaderForwardClustered::singleton, RID());
@@ -426,7 +426,7 @@ RID SceneShaderForwardClustered::ShaderData::get_shader_variant(PipelineVersion
 
 uint64_t SceneShaderForwardClustered::ShaderData::get_vertex_input_mask(PipelineVersion p_pipeline_version, uint32_t p_color_pass_flags, bool p_ubershader) {
 	// Vertex input masks require knowledge of the shader. Since querying the shader can be expensive due to high contention and the necessary mutex, we cache the result instead.
-	ShaderVersion shader_version = _get_shader_version(p_pipeline_version, p_color_pass_flags, p_ubershader);
+	uint16_t shader_version = _get_shader_version(p_pipeline_version, p_color_pass_flags, p_ubershader);
 	uint64_t input_mask = vertex_input_masks[shader_version].load(std::memory_order_relaxed);
 	if (input_mask == 0) {
 		RID shader_rid = _get_shader_variant(shader_version);
diff --git a/servers/rendering/renderer_rd/forward_clustered/scene_shader_forward_clustered.h b/servers/rendering/renderer_rd/forward_clustered/scene_shader_forward_clustered.h
index 1f4058a121..9e9e5dd8a2 100644
--- a/servers/rendering/renderer_rd/forward_clustered/scene_shader_forward_clustered.h
+++ b/servers/rendering/renderer_rd/forward_clustered/scene_shader_forward_clustered.h
@@ -50,18 +50,19 @@ public:
 		SHADER_GROUP_ADVANCED_MULTIVIEW,
 	};
 
-	enum ShaderVersion {
-		SHADER_VERSION_DEPTH_PASS,
-		SHADER_VERSION_DEPTH_PASS_DP,
-		SHADER_VERSION_DEPTH_PASS_WITH_NORMAL_AND_ROUGHNESS,
-		SHADER_VERSION_DEPTH_PASS_WITH_NORMAL_AND_ROUGHNESS_AND_VOXEL_GI,
-		SHADER_VERSION_DEPTH_PASS_MULTIVIEW,
-		SHADER_VERSION_DEPTH_PASS_WITH_NORMAL_AND_ROUGHNESS_MULTIVIEW,
-		SHADER_VERSION_DEPTH_PASS_WITH_NORMAL_AND_ROUGHNESS_AND_VOXEL_GI_MULTIVIEW,
-		SHADER_VERSION_DEPTH_PASS_WITH_MATERIAL,
-		SHADER_VERSION_DEPTH_PASS_WITH_SDF,
-		SHADER_VERSION_COLOR_PASS,
-		SHADER_VERSION_MAX
+	// Not an enum because these values are constants that are processed as numbers
+	// to arrive at a unique version for a particular shader.
+	struct ShaderVersion {
+		constexpr static uint16_t SHADER_VERSION_DEPTH_PASS = 0;
+		constexpr static uint16_t SHADER_VERSION_DEPTH_PASS_DP = 1;
+		constexpr static uint16_t SHADER_VERSION_DEPTH_PASS_WITH_NORMAL_AND_ROUGHNESS = 2;
+		constexpr static uint16_t SHADER_VERSION_DEPTH_PASS_WITH_NORMAL_AND_ROUGHNESS_AND_VOXEL_GI = 3;
+		constexpr static uint16_t SHADER_VERSION_DEPTH_PASS_MULTIVIEW = 4;
+		constexpr static uint16_t SHADER_VERSION_DEPTH_PASS_WITH_NORMAL_AND_ROUGHNESS_MULTIVIEW = 5;
+		constexpr static uint16_t SHADER_VERSION_DEPTH_PASS_WITH_NORMAL_AND_ROUGHNESS_AND_VOXEL_GI_MULTIVIEW = 6;
+		constexpr static uint16_t SHADER_VERSION_DEPTH_PASS_WITH_MATERIAL = 7;
+		constexpr static uint16_t SHADER_VERSION_DEPTH_PASS_WITH_SDF = 8;
+		constexpr static uint16_t SHADER_VERSION_COLOR_PASS = 9;
 	};
 
 	enum ShaderColorPassFlags {
@@ -205,7 +206,7 @@ public:
 
 		RID version;
 
-		static const uint32_t VERTEX_INPUT_MASKS_SIZE = SHADER_VERSION_DEPTH_PASS_WITH_MATERIAL + SHADER_VERSION_COLOR_PASS + SHADER_COLOR_PASS_FLAG_COUNT;
+		static const uint32_t VERTEX_INPUT_MASKS_SIZE = ShaderVersion::SHADER_VERSION_DEPTH_PASS_WITH_MATERIAL + ShaderVersion::SHADER_VERSION_COLOR_PASS + SHADER_COLOR_PASS_FLAG_COUNT;
 		std::atomic<uint64_t> vertex_input_masks[VERTEX_INPUT_MASKS_SIZE] = {};
 
 		Vector<ShaderCompiler::GeneratedCode::Texture> texture_uniforms;
@@ -281,8 +282,8 @@ public:
 		virtual bool is_animated() const;
 		virtual bool casts_shadows() const;
 		virtual RS::ShaderNativeSourceCode get_native_source_code() const;
-		ShaderVersion _get_shader_version(PipelineVersion p_pipeline_version, uint32_t p_color_pass_flags, bool p_ubershader) const;
-		RID _get_shader_variant(ShaderVersion p_shader_version) const;
+		uint16_t _get_shader_version(PipelineVersion p_pipeline_version, uint32_t p_color_pass_flags, bool p_ubershader) const;
+		RID _get_shader_variant(uint16_t p_shader_version) const;
 		void _clear_vertex_input_mask_cache();
 		RID get_shader_variant(PipelineVersion p_pipeline_version, uint32_t p_color_pass_flags, bool p_ubershader) const;
 		uint64_t get_vertex_input_mask(PipelineVersion p_pipeline_version, uint32_t p_color_pass_flags, bool p_ubershader);
diff --git a/servers/rendering/renderer_rd/renderer_canvas_render_rd.h b/servers/rendering/renderer_rd/renderer_canvas_render_rd.h
index e4f1779b09..6d8275db2e 100644
--- a/servers/rendering/renderer_rd/renderer_canvas_render_rd.h
+++ b/servers/rendering/renderer_rd/renderer_canvas_render_rd.h
@@ -138,7 +138,7 @@ class RendererCanvasRenderRD : public RendererCanvasRender {
 		uint32_t hash() const {
 			uint32_t h = hash_murmur3_one_32(variant);
 			h = hash_murmur3_one_32(framebuffer_format_id, h);
-			h = hash_murmur3_one_32(vertex_format_id, h);
+			h = hash_murmur3_one_64((uint64_t)vertex_format_id, h);
 			h = hash_murmur3_one_32(render_primitive, h);
 			h = hash_murmur3_one_32(shader_specialization.packed_0, h);
 			h = hash_murmur3_one_32(lcd_blend, h);
diff --git a/servers/rendering/renderer_scene_cull.cpp b/servers/rendering/renderer_scene_cull.cpp
index 1fcbcb9853..48b6dadd56 100644
--- a/servers/rendering/renderer_scene_cull.cpp
+++ b/servers/rendering/renderer_scene_cull.cpp
@@ -359,7 +359,7 @@ void RendererSceneCull::_instance_unpair(Instance *p_A, Instance *p_B) {
 
 			if (geom->lightmap_captures.is_empty() && A->scenario && A->array_index >= 0) {
 				InstanceData &idata = A->scenario->instance_data[A->array_index];
-				idata.flags &= ~uint32_t(InstanceData::FLAG_LIGHTMAP_CAPTURE);
+				idata.flags &= ~InstanceData::FLAG_LIGHTMAP_CAPTURE;
 			}
 
 			lightmap_data->geometries.erase(A);
@@ -532,7 +532,7 @@ void RendererSceneCull::_instance_update_mesh_instance(Instance *p_instance) con
 			if (p_instance->mesh_instance.is_valid()) {
 				idata.flags |= InstanceData::FLAG_USES_MESH_INSTANCE;
 			} else {
-				idata.flags &= ~uint32_t(InstanceData::FLAG_USES_MESH_INSTANCE);
+				idata.flags &= ~InstanceData::FLAG_USES_MESH_INSTANCE;
 			}
 		}
 	}
@@ -1232,7 +1232,7 @@ void RendererSceneCull::instance_set_ignore_culling(RID p_instance, bool p_enabl
 		if (instance->ignore_all_culling) {
 			idata.flags |= InstanceData::FLAG_IGNORE_ALL_CULLING;
 		} else {
-			idata.flags &= ~uint32_t(InstanceData::FLAG_IGNORE_ALL_CULLING);
+			idata.flags &= ~InstanceData::FLAG_IGNORE_ALL_CULLING;
 		}
 	}
 }
@@ -1324,7 +1324,7 @@ void RendererSceneCull::instance_geometry_set_flag(RID p_instance, RS::InstanceF
 				if (instance->baked_light) {
 					idata.flags |= InstanceData::FLAG_USES_BAKED_LIGHT;
 				} else {
-					idata.flags &= ~uint32_t(InstanceData::FLAG_USES_BAKED_LIGHT);
+					idata.flags &= ~InstanceData::FLAG_USES_BAKED_LIGHT;
 				}
 			}
 
@@ -1364,7 +1364,7 @@ void RendererSceneCull::instance_geometry_set_flag(RID p_instance, RS::InstanceF
 				if (instance->redraw_if_visible) {
 					idata.flags |= InstanceData::FLAG_REDRAW_IF_VISIBLE;
 				} else {
-					idata.flags &= ~uint32_t(InstanceData::FLAG_REDRAW_IF_VISIBLE);
+					idata.flags &= ~InstanceData::FLAG_REDRAW_IF_VISIBLE;
 				}
 			}
 
@@ -1377,7 +1377,7 @@ void RendererSceneCull::instance_geometry_set_flag(RID p_instance, RS::InstanceF
 				if (instance->ignore_occlusion_culling) {
 					idata.flags |= InstanceData::FLAG_IGNORE_OCCLUSION_CULLING;
 				} else {
-					idata.flags &= ~uint32_t(InstanceData::FLAG_IGNORE_OCCLUSION_CULLING);
+					idata.flags &= ~InstanceData::FLAG_IGNORE_OCCLUSION_CULLING;
 				}
 			}
 		} break;
@@ -1398,13 +1398,13 @@ void RendererSceneCull::instance_geometry_set_cast_shadows_setting(RID p_instanc
 		if (instance->cast_shadows != RS::SHADOW_CASTING_SETTING_OFF) {
 			idata.flags |= InstanceData::FLAG_CAST_SHADOWS;
 		} else {
-			idata.flags &= ~uint32_t(InstanceData::FLAG_CAST_SHADOWS);
+			idata.flags &= ~InstanceData::FLAG_CAST_SHADOWS;
 		}
 
 		if (instance->cast_shadows == RS::SHADOW_CASTING_SETTING_SHADOWS_ONLY) {
 			idata.flags |= InstanceData::FLAG_CAST_SHADOWS_ONLY;
 		} else {
-			idata.flags &= ~uint32_t(InstanceData::FLAG_CAST_SHADOWS_ONLY);
+			idata.flags &= ~InstanceData::FLAG_CAST_SHADOWS_ONLY;
 		}
 	}
 
@@ -2967,7 +2967,7 @@ void RendererSceneCull::_scene_cull(CullData &cull_data, InstanceCullResult &cul
 							}
 							cull_data.cull->lock.unlock();
 
-							idata.flags &= ~uint32_t(InstanceData::FLAG_REFLECTION_PROBE_DIRTY);
+							idata.flags &= ~InstanceData::FLAG_REFLECTION_PROBE_DIRTY;
 						}
 
 						if (RSG::light_storage->reflection_probe_instance_has_reflection(RID::from_uint64(idata.instance_data_rid))) {
@@ -3056,7 +3056,7 @@ void RendererSceneCull::_scene_cull(CullData &cull_data, InstanceCullResult &cul
 
 						ERR_FAIL_NULL(geom->geometry_instance);
 						geom->geometry_instance->pair_light_instances(instance_pair_buffer, idx);
-						idata.flags &= ~uint32_t(InstanceData::FLAG_GEOM_LIGHTING_DIRTY);
+						idata.flags &= ~InstanceData::FLAG_GEOM_LIGHTING_DIRTY;
 					}
 
 					if (idata.flags & InstanceData::FLAG_GEOM_PROJECTOR_SOFTSHADOW_DIRTY) {
@@ -3066,7 +3066,7 @@ void RendererSceneCull::_scene_cull(CullData &cull_data, InstanceCullResult &cul
 						cull_data.cull->lock.lock();
 						geom->geometry_instance->set_softshadow_projector_pairing(geom->softshadow_count > 0, geom->projector_count > 0);
 						cull_data.cull->lock.unlock();
-						idata.flags &= ~uint32_t(InstanceData::FLAG_GEOM_PROJECTOR_SOFTSHADOW_DIRTY);
+						idata.flags &= ~InstanceData::FLAG_GEOM_PROJECTOR_SOFTSHADOW_DIRTY;
 					}
 
 					if (geometry_instance_pair_mask & (1 << RS::INSTANCE_REFLECTION_PROBE) && (idata.flags & InstanceData::FLAG_GEOM_REFLECTION_DIRTY)) {
@@ -3084,7 +3084,7 @@ void RendererSceneCull::_scene_cull(CullData &cull_data, InstanceCullResult &cul
 
 						ERR_FAIL_NULL(geom->geometry_instance);
 						geom->geometry_instance->pair_reflection_probe_instances(instance_pair_buffer, idx);
-						idata.flags &= ~uint32_t(InstanceData::FLAG_GEOM_REFLECTION_DIRTY);
+						idata.flags &= ~InstanceData::FLAG_GEOM_REFLECTION_DIRTY;
 					}
 
 					if (geometry_instance_pair_mask & (1 << RS::INSTANCE_DECAL) && (idata.flags & InstanceData::FLAG_GEOM_DECAL_DIRTY)) {
@@ -3103,7 +3103,7 @@ void RendererSceneCull::_scene_cull(CullData &cull_data, InstanceCullResult &cul
 						ERR_FAIL_NULL(geom->geometry_instance);
 						geom->geometry_instance->pair_decal_instances(instance_pair_buffer, idx);
 
-						idata.flags &= ~uint32_t(InstanceData::FLAG_GEOM_DECAL_DIRTY);
+						idata.flags &= ~InstanceData::FLAG_GEOM_DECAL_DIRTY;
 					}
 
 					if (idata.flags & InstanceData::FLAG_GEOM_VOXEL_GI_DIRTY) {
@@ -3121,7 +3121,7 @@ void RendererSceneCull::_scene_cull(CullData &cull_data, InstanceCullResult &cul
 						ERR_FAIL_NULL(geom->geometry_instance);
 						geom->geometry_instance->pair_voxel_gi_instances(instance_pair_buffer, idx);
 
-						idata.flags &= ~uint32_t(InstanceData::FLAG_GEOM_VOXEL_GI_DIRTY);
+						idata.flags &= ~InstanceData::FLAG_GEOM_VOXEL_GI_DIRTY;
 					}
 
 					if ((idata.flags & InstanceData::FLAG_LIGHTMAP_CAPTURE) && idata.instance->last_frame_pass != frame_number && !idata.instance->lightmap_target_sh.is_empty() && !idata.instance->lightmap_sh.is_empty()) {
@@ -3981,7 +3981,7 @@ void RendererSceneCull::render_probes() {
 				ERR_FAIL_NULL(geom->geometry_instance);
 				geom->geometry_instance->pair_voxel_gi_instances(instance_pair_buffer, idx);
 
-				ins->scenario->instance_data[ins->array_index].flags &= ~uint32_t(InstanceData::FLAG_GEOM_VOXEL_GI_DIRTY);
+				ins->scenario->instance_data[ins->array_index].flags &= ~InstanceData::FLAG_GEOM_VOXEL_GI_DIRTY;
 			}
 
 			ERR_FAIL_NULL(geom->geometry_instance);
diff --git a/servers/rendering/renderer_scene_cull.h b/servers/rendering/renderer_scene_cull.h
index cfab52208a..d56daea240 100644
--- a/servers/rendering/renderer_scene_cull.h
+++ b/servers/rendering/renderer_scene_cull.h
@@ -259,7 +259,7 @@ public:
 	struct InstanceData {
 		// Store instance pointer as well as common instance processing information,
 		// to make processing more cache friendly.
-		enum Flags {
+		enum Flags : uint32_t {
 			FLAG_BASE_TYPE_MASK = 0xFF,
 			FLAG_CAST_SHADOWS = (1 << 8),
 			FLAG_CAST_SHADOWS_ONLY = (1 << 9),
diff --git a/servers/rendering/rendering_context_driver.h b/servers/rendering/rendering_context_driver.h
index 2e5951ae4f..cd973cf3c9 100644
--- a/servers/rendering/rendering_context_driver.h
+++ b/servers/rendering/rendering_context_driver.h
@@ -52,16 +52,20 @@ public:
 	void window_destroy(DisplayServer::WindowID p_window);
 
 public:
-	enum Vendor {
-		VENDOR_UNKNOWN = 0x0,
-		VENDOR_AMD = 0x1002,
-		VENDOR_IMGTEC = 0x1010,
-		VENDOR_APPLE = 0x106B,
-		VENDOR_NVIDIA = 0x10DE,
-		VENDOR_ARM = 0x13B5,
-		VENDOR_MICROSOFT = 0x1414,
-		VENDOR_QUALCOMM = 0x5143,
-		VENDOR_INTEL = 0x8086
+	// Not an enum as these values are matched against values returned by
+	// the various drivers, which report them in uint32_t. Casting to an
+	// enum value is dangerous in this case as we don't actually know what
+	// range the driver is reporting a value in.
+	struct Vendor {
+		constexpr static uint32_t VENDOR_UNKNOWN = 0x0;
+		constexpr static uint32_t VENDOR_AMD = 0x1002;
+		constexpr static uint32_t VENDOR_IMGTEC = 0x1010;
+		constexpr static uint32_t VENDOR_APPLE = 0x106B;
+		constexpr static uint32_t VENDOR_NVIDIA = 0x10DE;
+		constexpr static uint32_t VENDOR_ARM = 0x13B5;
+		constexpr static uint32_t VENDOR_MICROSOFT = 0x1414;
+		constexpr static uint32_t VENDOR_QUALCOMM = 0x5143;
+		constexpr static uint32_t VENDOR_INTEL = 0x8086;
 	};
 
 	enum DeviceType {
@@ -79,7 +83,7 @@ public:
 
 	struct Device {
 		String name = "Unknown";
-		Vendor vendor = VENDOR_UNKNOWN;
+		uint32_t vendor = Vendor::VENDOR_UNKNOWN;
 		DeviceType type = DEVICE_TYPE_OTHER;
 		Workarounds workarounds;
 	};
diff --git a/servers/rendering/rendering_device.cpp b/servers/rendering/rendering_device.cpp
index 33a42d4776..d62fa1f420 100644
--- a/servers/rendering/rendering_device.cpp
+++ b/servers/rendering/rendering_device.cpp
@@ -53,21 +53,21 @@
 
 static String _get_device_vendor_name(const RenderingContextDriver::Device &p_device) {
 	switch (p_device.vendor) {
-		case RenderingContextDriver::VENDOR_AMD:
+		case RenderingContextDriver::Vendor::VENDOR_AMD:
 			return "AMD";
-		case RenderingContextDriver::VENDOR_IMGTEC:
+		case RenderingContextDriver::Vendor::VENDOR_IMGTEC:
 			return "ImgTec";
-		case RenderingContextDriver::VENDOR_APPLE:
+		case RenderingContextDriver::Vendor::VENDOR_APPLE:
 			return "Apple";
-		case RenderingContextDriver::VENDOR_NVIDIA:
+		case RenderingContextDriver::Vendor::VENDOR_NVIDIA:
 			return "NVIDIA";
-		case RenderingContextDriver::VENDOR_ARM:
+		case RenderingContextDriver::Vendor::VENDOR_ARM:
 			return "ARM";
-		case RenderingContextDriver::VENDOR_MICROSOFT:
+		case RenderingContextDriver::Vendor::VENDOR_MICROSOFT:
 			return "Microsoft";
-		case RenderingContextDriver::VENDOR_QUALCOMM:
+		case RenderingContextDriver::Vendor::VENDOR_QUALCOMM:
 			return "Qualcomm";
-		case RenderingContextDriver::VENDOR_INTEL:
+		case RenderingContextDriver::Vendor::VENDOR_INTEL:
 			return "Intel";
 		default:
 			return "Unknown";
@@ -2371,7 +2371,7 @@ RDD::RenderPassID RenderingDevice::_render_pass_create(RenderingDeviceDriver *p_
 	}
 
 	LocalVector<RDD::Attachment> attachments;
-	LocalVector<int> attachment_remap;
+	LocalVector<uint32_t> attachment_remap;
 
 	for (int i = 0; i < p_attachments.size(); i++) {
 		if (p_attachments[i].usage_flags == AttachmentFormat::UNUSED_ATTACHMENT) {

From 70dbc311d1b46b84a60c55b781272bc41b8570b8 Mon Sep 17 00:00:00 2001
From: HP van Braam <hp@tmm.cx>
Date: Fri, 13 Dec 2024 22:49:28 +0100
Subject: [PATCH 121/124] Exclude known thirdparty/ ubsan errors

In my quest to make ubsan failures a build failure I've added all
current ubsan errors by type to a new suppression file which we will
then also use with CI.

The idea is to only add known ubsan errors to this file, so this is not
a blanket exclusion of thirdparty/ this means that if we get new or
unexpected ubsan in thirdparty/ builds would fail.
---
 .github/workflows/linux_builds.yml |  1 +
 misc/error_suppressions/ubsan.txt  | 32 ++++++++++++++++++++++++++++++
 2 files changed, 33 insertions(+)
 create mode 100644 misc/error_suppressions/ubsan.txt

diff --git a/.github/workflows/linux_builds.yml b/.github/workflows/linux_builds.yml
index 5a9fb57755..193942a1bb 100644
--- a/.github/workflows/linux_builds.yml
+++ b/.github/workflows/linux_builds.yml
@@ -10,6 +10,7 @@ env:
   DOTNET_NOLOGO: true
   DOTNET_CLI_TELEMETRY_OPTOUT: true
   TSAN_OPTIONS: suppressions=misc/error_suppressions/tsan.txt
+  UBSAN_OPTIONS: suppressions=misc/error_suppressions/ubsan.txt
 
 concurrency:
   group: ci-${{ github.actor }}-${{ github.head_ref || github.run_number }}-${{ github.ref }}-linux
diff --git a/misc/error_suppressions/ubsan.txt b/misc/error_suppressions/ubsan.txt
new file mode 100644
index 0000000000..b3e46a7ab1
--- /dev/null
+++ b/misc/error_suppressions/ubsan.txt
@@ -0,0 +1,32 @@
+implicit-integer-sign-change:thirdparty/basis_universal/transcoder/basisu_astc_helpers.h
+implicit-integer-sign-change:thirdparty/harfbuzz/src/hb-algs.hh
+implicit-integer-sign-change:thirdparty/harfbuzz/src/hb-buffer.hh
+implicit-integer-sign-change:thirdparty/harfbuzz/src/hb-cache.hh
+implicit-integer-sign-change:thirdparty/harfbuzz/src/hb-face.hh
+implicit-integer-sign-change:thirdparty/harfbuzz/src/hb-ot-layout-gsubgpos.hh
+implicit-integer-sign-change:thirdparty/harfbuzz/src/OT/name/name.hh
+implicit-integer-sign-change:thirdparty/icu4c/common/bmpset.cpp
+implicit-integer-sign-change:thirdparty/icu4c/common/ubidiln.cpp
+implicit-integer-sign-change:thirdparty/icu4c/common/unicode/unistr.h
+implicit-integer-sign-change:thirdparty/icu4c/common/unistr.cpp
+implicit-integer-sign-change:thirdparty/icu4c/common/uresbund.cpp
+implicit-integer-sign-change:thirdparty/icu4c/common/ustrtrns.cpp
+implicit-integer-sign-change:thirdparty/jpeg-compressor/jpgd.cpp
+implicit-integer-sign-change:thirdparty/libogg/bitwise.c
+implicit-integer-sign-change:thirdparty/libvorbis/info.c
+implicit-integer-sign-change:thirdparty/libvorbis/sharedbook.c
+implicit-integer-sign-change:thirdparty/mbedtls/library/base64.c
+implicit-integer-sign-change:thirdparty/misc/smolv.cpp
+implicit-integer-sign-change:thirdparty/recastnavigation/Recast/Include/Recast.h
+implicit-integer-sign-change:thirdparty/recastnavigation/Recast/Source/RecastMesh.cpp
+implicit-integer-sign-change:thirdparty/thorvg/src/renderer/sw_engine/tvgSwRaster.cpp
+implicit-integer-sign-change:thirdparty/tinyexr/tinyexr.h
+implicit-integer-sign-change:thirdparty/vulkan/vk_mem_alloc.h
+implicit-integer-sign-change:thirdparty/zlib/trees.c
+implicit-integer-sign-change:thirdparty/zstd/compress/zstd_cwksp.h
+integer-divide-by-zero:thirdparty/misc/ok_color.h
+integer-divide-by-zero:thirdparty/rvo2/rvo2_2d/Agent2d.cpp
+shift-base:thirdparty/libogg/bitwise.c
+shift-base:thirdparty/libvorbis/sharedbook.c
+shift-base:thirdparty/noise/FastNoiseLite.h
+signed-integer-overflow:thirdparty/noise/FastNoiseLite.h

From 49a19db0e0b84ba7a098b4305de68647e9d4b016 Mon Sep 17 00:00:00 2001
From: HP van Braam <hp@tmm.cx>
Date: Sat, 14 Dec 2024 00:08:17 +0100
Subject: [PATCH 122/124] Add thirdparty/ ubsan suppressions for TPS demo

With these suppressions it is possible to import and run the TPS demo
without any thirdparty/ reported ubsan.

This is useful because it is a complex project, so it is good for
testing if new errors show up in our own code.
---
 misc/error_suppressions/ubsan.txt | 35 +++++++++++++++++++++++++++++--
 1 file changed, 33 insertions(+), 2 deletions(-)

diff --git a/misc/error_suppressions/ubsan.txt b/misc/error_suppressions/ubsan.txt
index b3e46a7ab1..13e0ef05a1 100644
--- a/misc/error_suppressions/ubsan.txt
+++ b/misc/error_suppressions/ubsan.txt
@@ -1,9 +1,36 @@
+enum:thirdparty/glslang/SPIRV/spirv.hpp
+float-divide-by-zero:thirdparty/amd-fsr2/ffx_fsr2.cpp
+float-divide-by-zero:thirdparty/misc/ok_color.h
+float-divide-by-zero:thirdparty/rvo2/rvo2_2d/Agent2d.cpp
+float-divide-by-zero:thirdparty/thorvg/src/renderer/sw_engine/tvgSwFill.cpp
+function:thirdparty/embree/common/sys/thread.cpp
+function:thirdparty/embree/kernels/common/accel.h
+function:thirdparty/xatlas/xatlas.cpp
 implicit-integer-sign-change:thirdparty/basis_universal/transcoder/basisu_astc_helpers.h
+implicit-integer-sign-change:thirdparty/embree/common/lexers/../sys/ref.h
+implicit-integer-sign-change:thirdparty/embree/common/lexers/tokenstream.cpp
+implicit-integer-sign-change:thirdparty/embree/common/sys/sysinfo.cpp
+implicit-integer-sign-change:thirdparty/embree/common/tasking/taskschedulerinternal.cpp
+implicit-integer-sign-change:thirdparty/embree/common/tasking/taskschedulerinternal.h
+implicit-integer-sign-change:thirdparty/embree/kernels/bvh/../common/../builders/primref.h
+implicit-integer-sign-change:thirdparty/embree/kernels/bvh/../common/../../common/sys/../math/../simd/vfloat4_sse2.h
+implicit-integer-sign-change:thirdparty/embree/kernels/bvh/../common/../../common/sys/../math/../simd/vuint4_sse2.h
+implicit-integer-sign-change:thirdparty/embree/kernels/bvh/../common/vector.h
+implicit-integer-sign-change:thirdparty/embree/kernels/bvh/../geometry/triangle.h
+implicit-integer-sign-change:thirdparty/glslang/glslang/MachineIndependent/ParseHelper.cpp
+implicit-integer-sign-change:thirdparty/glslang/glslang/MachineIndependent/preprocessor/PpScanner.cpp
+implicit-integer-sign-change:thirdparty/glslang/glslang/MachineIndependent/preprocessor/PpTokens.cpp
+implicit-integer-sign-change:thirdparty/glslang/glslang/MachineIndependent/Versions.cpp
+implicit-integer-sign-change:thirdparty/glslang/SPIRV/hex_float.h
 implicit-integer-sign-change:thirdparty/harfbuzz/src/hb-algs.hh
 implicit-integer-sign-change:thirdparty/harfbuzz/src/hb-buffer.hh
 implicit-integer-sign-change:thirdparty/harfbuzz/src/hb-cache.hh
 implicit-integer-sign-change:thirdparty/harfbuzz/src/hb-face.hh
 implicit-integer-sign-change:thirdparty/harfbuzz/src/hb-ot-layout-gsubgpos.hh
+implicit-integer-sign-change:thirdparty/harfbuzz/src/OT/Layout/GDEF/../../../hb-cache.hh
+implicit-integer-sign-change:thirdparty/harfbuzz/src/OT/Layout/GPOS/../../../hb-ot-layout-gsubgpos.hh
+implicit-integer-sign-change:thirdparty/harfbuzz/src/OT/Layout/GPOS/../../../OT/Layout/GDEF/../../../hb-cache.hh
+implicit-integer-sign-change:thirdparty/harfbuzz/src/OT/Layout/GSUB/../../../hb-ot-layout-gsubgpos.hh
 implicit-integer-sign-change:thirdparty/harfbuzz/src/OT/name/name.hh
 implicit-integer-sign-change:thirdparty/icu4c/common/bmpset.cpp
 implicit-integer-sign-change:thirdparty/icu4c/common/ubidiln.cpp
@@ -19,14 +46,18 @@ implicit-integer-sign-change:thirdparty/mbedtls/library/base64.c
 implicit-integer-sign-change:thirdparty/misc/smolv.cpp
 implicit-integer-sign-change:thirdparty/recastnavigation/Recast/Include/Recast.h
 implicit-integer-sign-change:thirdparty/recastnavigation/Recast/Source/RecastMesh.cpp
+implicit-integer-sign-change:thirdparty/spirv-reflect/spirv_reflect.c
 implicit-integer-sign-change:thirdparty/thorvg/src/renderer/sw_engine/tvgSwRaster.cpp
 implicit-integer-sign-change:thirdparty/tinyexr/tinyexr.h
 implicit-integer-sign-change:thirdparty/vulkan/vk_mem_alloc.h
+implicit-integer-sign-change:thirdparty/xatlas/xatlas.cpp
 implicit-integer-sign-change:thirdparty/zlib/trees.c
 implicit-integer-sign-change:thirdparty/zstd/compress/zstd_cwksp.h
-integer-divide-by-zero:thirdparty/misc/ok_color.h
-integer-divide-by-zero:thirdparty/rvo2/rvo2_2d/Agent2d.cpp
+nonnull-attribute:thirdparty/spirv-reflect/spirv_reflect.c
+pointer-overflow:thirdparty/libogg/framing.c
 shift-base:thirdparty/libogg/bitwise.c
 shift-base:thirdparty/libvorbis/sharedbook.c
 shift-base:thirdparty/noise/FastNoiseLite.h
+shift-base:thirdparty/tinyexr/tinyexr.h
+shift-exponent:thirdparty/misc/mikktspace.c
 signed-integer-overflow:thirdparty/noise/FastNoiseLite.h

From 4e19ab8afed8407f1b077458863de1f7431b7f61 Mon Sep 17 00:00:00 2001
From: Hilderin <81109165+Hilderin@users.noreply.github.com>
Date: Thu, 1 Aug 2024 20:03:15 -0400
Subject: [PATCH 123/124] Fix connecting a signal with a double click is too
 difficult

Co-authored-by: Danil Alexeev <dalexeev12@yandex.ru>
---
 core/input/input.cpp                         |  20 +++
 core/input/input.h                           |   1 +
 editor/connections_dialog.cpp                |   4 +-
 editor/editor_help.cpp                       | 135 ++++++++++++-------
 editor/editor_help.h                         |  18 +--
 editor/editor_inspector.cpp                  |  45 +++----
 editor/plugins/theme_editor_plugin.cpp       |   4 +-
 platform/linuxbsd/x11/display_server_x11.cpp |   9 +-
 scene/main/viewport.cpp                      |   9 +-
 9 files changed, 147 insertions(+), 98 deletions(-)

diff --git a/core/input/input.cpp b/core/input/input.cpp
index a0c00d7716..5314e9f02d 100644
--- a/core/input/input.cpp
+++ b/core/input/input.cpp
@@ -308,6 +308,26 @@ bool Input::is_anything_pressed() const {
 	return false;
 }
 
+bool Input::is_anything_pressed_except_mouse() const {
+	_THREAD_SAFE_METHOD_
+
+	if (disable_input) {
+		return false;
+	}
+
+	if (!keys_pressed.is_empty() || !joy_buttons_pressed.is_empty()) {
+		return true;
+	}
+
+	for (const KeyValue<StringName, Input::ActionState> &E : action_states) {
+		if (E.value.cache.pressed) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
 bool Input::is_key_pressed(Key p_keycode) const {
 	_THREAD_SAFE_METHOD_
 
diff --git a/core/input/input.h b/core/input/input.h
index a189ae7d9a..005ddcca4f 100644
--- a/core/input/input.h
+++ b/core/input/input.h
@@ -294,6 +294,7 @@ public:
 	static Input *get_singleton();
 
 	bool is_anything_pressed() const;
+	bool is_anything_pressed_except_mouse() const;
 	bool is_key_pressed(Key p_keycode) const;
 	bool is_physical_key_pressed(Key p_keycode) const;
 	bool is_key_label_pressed(Key p_keycode) const;
diff --git a/editor/connections_dialog.cpp b/editor/connections_dialog.cpp
index 53b1f2a759..2b21b4979c 100644
--- a/editor/connections_dialog.cpp
+++ b/editor/connections_dialog.cpp
@@ -911,9 +911,7 @@ Control *ConnectionsDockTree::make_custom_tooltip(const String &p_text) const {
 		return nullptr;
 	}
 
-	EditorHelpBit *help_bit = memnew(EditorHelpBit(p_text));
-	EditorHelpBitTooltip::show_tooltip(help_bit, const_cast<ConnectionsDockTree *>(this));
-	return memnew(Control); // Make the standard tooltip invisible.
+	return EditorHelpBitTooltip::show_tooltip(const_cast<ConnectionsDockTree *>(this), p_text);
 }
 
 struct _ConnectionsDockMethodInfoSort {
diff --git a/editor/editor_help.cpp b/editor/editor_help.cpp
index 2def625f8c..294efca560 100644
--- a/editor/editor_help.cpp
+++ b/editor/editor_help.cpp
@@ -3690,7 +3690,7 @@ void EditorHelpBit::_notification(int p_what) {
 	}
 }
 
-void EditorHelpBit::parse_symbol(const String &p_symbol) {
+void EditorHelpBit::parse_symbol(const String &p_symbol, const String &p_prologue) {
 	const PackedStringArray slices = p_symbol.split("|", true, 2);
 	ERR_FAIL_COND_MSG(slices.size() < 3, "Invalid doc id. The expected format is 'item_type|class_name|item_name'.");
 
@@ -3737,6 +3737,14 @@ void EditorHelpBit::parse_symbol(const String &p_symbol) {
 	symbol_visible_type = visible_type;
 	symbol_name = name;
 
+	if (!p_prologue.is_empty()) {
+		if (help_data.description.is_empty()) {
+			help_data.description = p_prologue;
+		} else {
+			help_data.description = p_prologue + "\n" + help_data.description;
+		}
+	}
+
 	if (help_data.description.is_empty()) {
 		help_data.description = "[color=<EditorHelpBitCommentColor>][i]" + TTR("No description available.") + "[/i][/color]";
 	}
@@ -3760,14 +3768,6 @@ void EditorHelpBit::set_custom_text(const String &p_type, const String &p_name,
 	}
 }
 
-void EditorHelpBit::set_description(const String &p_text) {
-	help_data.description = p_text;
-
-	if (is_inside_tree()) {
-		_update_labels();
-	}
-}
-
 void EditorHelpBit::set_content_height_limits(float p_min, float p_max) {
 	ERR_FAIL_COND(p_min > p_max);
 	content_min_height = p_min;
@@ -3787,15 +3787,15 @@ void EditorHelpBit::update_content_height() {
 	content->set_custom_minimum_size(Size2(content->get_custom_minimum_size().x, CLAMP(content_height, content_min_height, content_max_height)));
 }
 
-EditorHelpBit::EditorHelpBit(const String &p_symbol) {
+EditorHelpBit::EditorHelpBit(const String &p_symbol, const String &p_prologue, bool p_allow_selection) {
 	add_theme_constant_override("separation", 0);
 
 	title = memnew(RichTextLabel);
 	title->set_theme_type_variation("EditorHelpBitTitle");
 	title->set_custom_minimum_size(Size2(512 * EDSCALE, 0)); // GH-93031. Set the minimum width even if `fit_content` is true.
 	title->set_fit_content(true);
-	title->set_selection_enabled(true);
-	//title->set_context_menu_enabled(true); // TODO: Fix opening context menu hides tooltip.
+	title->set_selection_enabled(p_allow_selection);
+	title->set_context_menu_enabled(p_allow_selection);
 	title->connect("meta_clicked", callable_mp(this, &EditorHelpBit::_meta_clicked));
 	title->hide();
 	add_child(title);
@@ -3806,76 +3806,108 @@ EditorHelpBit::EditorHelpBit(const String &p_symbol) {
 	content = memnew(RichTextLabel);
 	content->set_theme_type_variation("EditorHelpBitContent");
 	content->set_custom_minimum_size(Size2(512 * EDSCALE, content_min_height));
-	content->set_selection_enabled(true);
-	//content->set_context_menu_enabled(true); // TODO: Fix opening context menu hides tooltip.
+	content->set_selection_enabled(p_allow_selection);
+	content->set_context_menu_enabled(p_allow_selection);
 	content->connect("meta_clicked", callable_mp(this, &EditorHelpBit::_meta_clicked));
 	add_child(content);
 
 	if (!p_symbol.is_empty()) {
-		parse_symbol(p_symbol);
+		parse_symbol(p_symbol, p_prologue);
 	}
 }
 
 /// EditorHelpBitTooltip ///
 
+bool EditorHelpBitTooltip::_is_tooltip_visible = false;
+
+Control *EditorHelpBitTooltip::_make_invisible_control() {
+	Control *control = memnew(Control);
+	control->set_visible(false);
+	return control;
+}
+
 void EditorHelpBitTooltip::_start_timer() {
 	if (timer->is_inside_tree() && timer->is_stopped()) {
 		timer->start();
 	}
 }
 
-void EditorHelpBitTooltip::_safe_queue_free() {
-	if (_pushing_input > 0) {
-		_need_free = true;
-	} else {
-		queue_free();
-	}
-}
-
 void EditorHelpBitTooltip::_target_gui_input(const Ref<InputEvent> &p_event) {
-	const Ref<InputEventMouse> mouse_event = p_event;
-	if (mouse_event.is_valid()) {
-		_start_timer();
+	// Only scrolling is not checked in `NOTIFICATION_INTERNAL_PROCESS`.
+	const Ref<InputEventMouseButton> mb = p_event;
+	if (mb.is_valid()) {
+		switch (mb->get_button_index()) {
+			case MouseButton::WHEEL_UP:
+			case MouseButton::WHEEL_DOWN:
+			case MouseButton::WHEEL_LEFT:
+			case MouseButton::WHEEL_RIGHT:
+				queue_free();
+				break;
+			default:
+				break;
+		}
 	}
 }
 
 void EditorHelpBitTooltip::_notification(int p_what) {
 	switch (p_what) {
+		case NOTIFICATION_ENTER_TREE:
+			_is_tooltip_visible = true;
+			_enter_tree_time = OS::get_singleton()->get_ticks_msec();
+			break;
+		case NOTIFICATION_EXIT_TREE:
+			_is_tooltip_visible = false;
+			break;
 		case NOTIFICATION_WM_MOUSE_ENTER:
+			_is_mouse_inside_tooltip = true;
 			timer->stop();
 			break;
 		case NOTIFICATION_WM_MOUSE_EXIT:
+			_is_mouse_inside_tooltip = false;
 			_start_timer();
 			break;
+		case NOTIFICATION_INTERNAL_PROCESS:
+			// A workaround to hide the tooltip since the window does not receive keyboard events
+			// with `FLAG_POPUP` and `FLAG_NO_FOCUS` flags, so we can't use `_input_from_window()`.
+			if (is_inside_tree()) {
+				if (Input::get_singleton()->is_action_just_pressed(SNAME("ui_cancel"), true)) {
+					queue_free();
+					get_parent_viewport()->set_input_as_handled();
+				} else if (Input::get_singleton()->is_anything_pressed_except_mouse()) {
+					queue_free();
+				} else if (!Input::get_singleton()->get_mouse_button_mask().is_empty()) {
+					if (!_is_mouse_inside_tooltip) {
+						queue_free();
+					}
+				} else if (!Input::get_singleton()->get_last_mouse_velocity().is_zero_approx()) {
+					if (!_is_mouse_inside_tooltip && OS::get_singleton()->get_ticks_msec() - _enter_tree_time > 250) {
+						_start_timer();
+					}
+				}
+			}
+			break;
 	}
 }
 
-// Forwards non-mouse input to the parent viewport.
-void EditorHelpBitTooltip::_input_from_window(const Ref<InputEvent> &p_event) {
-	if (p_event->is_action_pressed(SNAME("ui_cancel"), false, true)) {
-		_safe_queue_free();
-	} else {
-		const Ref<InputEventMouse> mouse_event = p_event;
-		if (mouse_event.is_null()) {
-			// GH-91652. Prevents use-after-free since `ProgressDialog` calls `Main::iteration()`.
-			_pushing_input++;
-			get_parent_viewport()->push_input(p_event);
-			_pushing_input--;
-			if (_pushing_input <= 0 && _need_free) {
-				queue_free();
-			}
-		}
+Control *EditorHelpBitTooltip::show_tooltip(Control *p_target, const String &p_symbol, const String &p_prologue) {
+	// Show the custom tooltip only if it is not already visible.
+	// The viewport will retrigger `make_custom_tooltip()` every few seconds
+	// because the return control is not visible even if the custom tooltip is displayed.
+	if (_is_tooltip_visible || Input::get_singleton()->is_anything_pressed()) {
+		return _make_invisible_control();
 	}
-}
 
-void EditorHelpBitTooltip::show_tooltip(EditorHelpBit *p_help_bit, Control *p_target) {
-	ERR_FAIL_NULL(p_help_bit);
+	EditorHelpBit *help_bit = memnew(EditorHelpBit(p_symbol, p_prologue, false));
+
 	EditorHelpBitTooltip *tooltip = memnew(EditorHelpBitTooltip(p_target));
-	p_help_bit->connect("request_hide", callable_mp(tooltip, &EditorHelpBitTooltip::_safe_queue_free));
-	tooltip->add_child(p_help_bit);
+	help_bit->connect("request_hide", callable_mp(static_cast<Node *>(tooltip), &Node::queue_free));
+	tooltip->add_child(help_bit);
 	p_target->add_child(tooltip);
-	p_help_bit->update_content_height();
+
+	help_bit->update_content_height();
 	tooltip->popup_under_cursor();
+
+	return _make_invisible_control();
 }
 
 // Copy-paste from `Viewport::_gui_show_tooltip()`.
@@ -3915,6 +3947,9 @@ void EditorHelpBitTooltip::popup_under_cursor() {
 		r.position.y = vr.position.y;
 	}
 
+	// When `FLAG_POPUP` is false, it prevents the editor from losing focus when displaying the tooltip.
+	// This way, clicks and double-clicks are still available outside the tooltip.
+	set_flag(Window::FLAG_POPUP, false);
 	set_flag(Window::FLAG_NO_FOCUS, true);
 	popup(r);
 }
@@ -3923,13 +3958,15 @@ EditorHelpBitTooltip::EditorHelpBitTooltip(Control *p_target) {
 	set_theme_type_variation("TooltipPanel");
 
 	timer = memnew(Timer);
-	timer->set_wait_time(0.2);
-	timer->connect("timeout", callable_mp(this, &EditorHelpBitTooltip::_safe_queue_free));
+	timer->set_wait_time(0.25);
+	timer->connect("timeout", callable_mp(static_cast<Node *>(this), &Node::queue_free));
 	add_child(timer);
 
 	ERR_FAIL_NULL(p_target);
 	p_target->connect(SceneStringName(mouse_exited), callable_mp(this, &EditorHelpBitTooltip::_start_timer));
 	p_target->connect(SceneStringName(gui_input), callable_mp(this, &EditorHelpBitTooltip::_target_gui_input));
+
+	set_process_internal(true);
 }
 
 #if defined(MODULE_GDSCRIPT_ENABLED) || defined(MODULE_MONO_ENABLED)
diff --git a/editor/editor_help.h b/editor/editor_help.h
index 93f74cb2c1..5435b3822d 100644
--- a/editor/editor_help.h
+++ b/editor/editor_help.h
@@ -309,15 +309,13 @@ protected:
 	void _notification(int p_what);
 
 public:
-	void parse_symbol(const String &p_symbol);
+	void parse_symbol(const String &p_symbol, const String &p_prologue = String());
 	void set_custom_text(const String &p_type, const String &p_name, const String &p_description);
-	void set_description(const String &p_text);
-	_FORCE_INLINE_ String get_description() const { return help_data.description; }
 
 	void set_content_height_limits(float p_min, float p_max);
 	void update_content_height();
 
-	EditorHelpBit(const String &p_symbol = String());
+	EditorHelpBit(const String &p_symbol = String(), const String &p_prologue = String(), bool p_allow_selection = true);
 };
 
 // Standard tooltips do not allow you to hover over them.
@@ -325,20 +323,22 @@ public:
 class EditorHelpBitTooltip : public PopupPanel {
 	GDCLASS(EditorHelpBitTooltip, PopupPanel);
 
+	static bool _is_tooltip_visible;
+
 	Timer *timer = nullptr;
-	int _pushing_input = 0;
-	bool _need_free = false;
+	uint64_t _enter_tree_time = 0;
+	bool _is_mouse_inside_tooltip = false;
+
+	static Control *_make_invisible_control();
 
 	void _start_timer();
-	void _safe_queue_free();
 	void _target_gui_input(const Ref<InputEvent> &p_event);
 
 protected:
 	void _notification(int p_what);
-	virtual void _input_from_window(const Ref<InputEvent> &p_event) override;
 
 public:
-	static void show_tooltip(EditorHelpBit *p_help_bit, Control *p_target);
+	static Control *show_tooltip(Control *p_target, const String &p_symbol, const String &p_prologue = String());
 
 	void popup_under_cursor();
 
diff --git a/editor/editor_inspector.cpp b/editor/editor_inspector.cpp
index e709646d73..d3026bc611 100644
--- a/editor/editor_inspector.cpp
+++ b/editor/editor_inspector.cpp
@@ -1005,36 +1005,33 @@ void EditorProperty::_update_flags() {
 }
 
 Control *EditorProperty::make_custom_tooltip(const String &p_text) const {
-	String custom_warning;
+	String symbol;
+	String prologue;
+
 	if (object->has_method("_get_property_warning")) {
-		custom_warning = object->call("_get_property_warning", property);
+		const String custom_warning = object->call("_get_property_warning", property);
+		if (!custom_warning.is_empty()) {
+			prologue = "[b][color=" + get_theme_color(SNAME("warning_color")).to_html(false) + "]" + custom_warning + "[/color][/b]";
+		}
 	}
 
-	if (has_doc_tooltip || !custom_warning.is_empty()) {
-		EditorHelpBit *help_bit = memnew(EditorHelpBit);
+	if (has_doc_tooltip) {
+		symbol = p_text;
 
-		if (has_doc_tooltip) {
-			help_bit->parse_symbol(p_text);
-
-			const EditorInspector *inspector = get_parent_inspector();
-			if (inspector) {
-				const String custom_description = inspector->get_custom_property_description(p_text);
-				if (!custom_description.is_empty()) {
-					help_bit->set_description(custom_description);
+		const EditorInspector *inspector = get_parent_inspector();
+		if (inspector) {
+			const String custom_description = inspector->get_custom_property_description(p_text);
+			if (!custom_description.is_empty()) {
+				if (!prologue.is_empty()) {
+					prologue += '\n';
 				}
+				prologue += custom_description;
 			}
 		}
+	}
 
-		if (!custom_warning.is_empty()) {
-			String description = "[b][color=" + get_theme_color(SNAME("warning_color")).to_html(false) + "]" + custom_warning + "[/color][/b]";
-			if (!help_bit->get_description().is_empty()) {
-				description += "\n" + help_bit->get_description();
-			}
-			help_bit->set_description(description);
-		}
-
-		EditorHelpBitTooltip::show_tooltip(help_bit, const_cast<EditorProperty *>(this));
-		return memnew(Control); // Make the standard tooltip invisible.
+	if (!symbol.is_empty() || !prologue.is_empty()) {
+		return EditorHelpBitTooltip::show_tooltip(const_cast<EditorProperty *>(this), symbol, prologue);
 	}
 
 	return nullptr;
@@ -1331,9 +1328,7 @@ Control *EditorInspectorCategory::make_custom_tooltip(const String &p_text) cons
 		return nullptr;
 	}
 
-	EditorHelpBit *help_bit = memnew(EditorHelpBit(p_text));
-	EditorHelpBitTooltip::show_tooltip(help_bit, const_cast<EditorInspectorCategory *>(this));
-	return memnew(Control); // Make the standard tooltip invisible.
+	return EditorHelpBitTooltip::show_tooltip(const_cast<EditorInspectorCategory *>(this), p_text);
 }
 
 void EditorInspectorCategory::set_as_favorite(EditorInspector *p_for_inspector) {
diff --git a/editor/plugins/theme_editor_plugin.cpp b/editor/plugins/theme_editor_plugin.cpp
index c2f6896e3d..f21f70fa3a 100644
--- a/editor/plugins/theme_editor_plugin.cpp
+++ b/editor/plugins/theme_editor_plugin.cpp
@@ -2284,9 +2284,7 @@ ThemeTypeDialog::ThemeTypeDialog() {
 ///////////////////////
 
 Control *ThemeItemLabel::make_custom_tooltip(const String &p_text) const {
-	EditorHelpBit *help_bit = memnew(EditorHelpBit(p_text));
-	EditorHelpBitTooltip::show_tooltip(help_bit, const_cast<ThemeItemLabel *>(this));
-	return memnew(Control); // Make the standard tooltip invisible.
+	return EditorHelpBitTooltip::show_tooltip(const_cast<ThemeItemLabel *>(this), p_text);
 }
 
 VBoxContainer *ThemeTypeEditor::_create_item_list(Theme::DataType p_data_type) {
diff --git a/platform/linuxbsd/x11/display_server_x11.cpp b/platform/linuxbsd/x11/display_server_x11.cpp
index 90bd3d57d4..be66ea30ae 100644
--- a/platform/linuxbsd/x11/display_server_x11.cpp
+++ b/platform/linuxbsd/x11/display_server_x11.cpp
@@ -4857,14 +4857,7 @@ void DisplayServerX11::process_events() {
 
 					WindowID window_id_other = INVALID_WINDOW_ID;
 					Window wd_other_x11_window;
-					if (wd.focused) {
-						// Handle cases where an unfocused popup is open that needs to receive button-up events.
-						WindowID popup_id = _get_focused_window_or_popup();
-						if (popup_id != INVALID_WINDOW_ID && popup_id != window_id) {
-							window_id_other = popup_id;
-							wd_other_x11_window = windows[popup_id].x11_window;
-						}
-					} else {
+					if (!wd.focused) {
 						// Propagate the event to the focused window,
 						// because it's received only on the topmost window.
 						// Note: This is needed for drag & drop to work between windows,
diff --git a/scene/main/viewport.cpp b/scene/main/viewport.cpp
index bfa048ee0d..4b89167ce9 100644
--- a/scene/main/viewport.cpp
+++ b/scene/main/viewport.cpp
@@ -1455,7 +1455,14 @@ void Viewport::_gui_show_tooltip() {
 
 	// Controls can implement `make_custom_tooltip` to provide their own tooltip.
 	// This should be a Control node which will be added as child to a TooltipPanel.
-	Control *base_tooltip = tooltip_owner ? tooltip_owner->make_custom_tooltip(gui.tooltip_text) : nullptr;
+	Control *base_tooltip = tooltip_owner->make_custom_tooltip(gui.tooltip_text);
+
+	// When the custom control is not visible, don't show any tooltip.
+	// This way, the custom tooltip from `ConnectionsDockTree` can create
+	// its own tooltip without conflicting with the default one, even an empty tooltip.
+	if (base_tooltip && !base_tooltip->is_visible()) {
+		return;
+	}
 
 	if (gui.tooltip_text.is_empty() && !base_tooltip) {
 		return; // Nothing to show.

From 8f9673eeff735c3c4d468cb56c41dcbbddd47854 Mon Sep 17 00:00:00 2001
From: BlueCube3310 <53150244+BlueCube3310@users.noreply.github.com>
Date: Sat, 14 Dec 2024 17:57:44 +0100
Subject: [PATCH 124/124] bcdec: Fix logic for detecting incomplete blocks

---
 modules/bcdec/image_decompress_bcdec.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/modules/bcdec/image_decompress_bcdec.cpp b/modules/bcdec/image_decompress_bcdec.cpp
index 8a62cd2a4e..6d97ca38ab 100644
--- a/modules/bcdec/image_decompress_bcdec.cpp
+++ b/modules/bcdec/image_decompress_bcdec.cpp
@@ -64,7 +64,7 @@ static void decompress_image(BCdecFormat format, const void *src, void *dst, con
 			const uint32_t yblock = MIN(height - y, 4ul);                                                                                             \
 			const uint32_t xblock = MIN(width - x, 4ul);                                                                                              \
                                                                                                                                                       \
-			const bool incomplete = yblock < 4 && xblock < 4;                                                                                         \
+			const bool incomplete = yblock < 4 || xblock < 4;                                                                                         \
 			uint8_t *dec_out = incomplete ? output : &dec_blocks[y * 4 * width + x * color_bytesize];                                                 \
                                                                                                                                                       \
 			func(&src_blocks[src_pos], dec_out, 4 * color_components);                                                                                \