Add Jolt Physics as an alternative 3D physics engine

Co-authored-by: Jorrit Rouwe <[email protected]>

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

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;
 		}
 	}
 

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>

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)

modules/jolt_physics/config.py

@@ -0,0 +1,6 @@
+def can_build(env, platform):
+    return not env["disable_3d"]
+
+
+def configure(env):
+    pass

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();
+}

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

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);
+	}
+}

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

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();
+}

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

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;
+}

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

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();
+}

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

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();
+}

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

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;
+	}
+}

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

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();
+}

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

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");
+}

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

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

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

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);
+}

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

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);
+}

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

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.");

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

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>";
+}

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

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();
+}

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

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;
+}

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

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>";
+}

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

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;
+	}
+}

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

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);
+}

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

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);
+}

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

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());
+}

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

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);
+}

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

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

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);
+}

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

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));
+}

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

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);
+}

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

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

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);
+	}
+}

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

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);
+}

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

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);
+}

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

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);
+}

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

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);
+}

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

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;
+}

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

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);
+}

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

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);
+}

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

modules/jolt_physics/spaces/jolt_body_accessor_3d.cpp

@@ -0,0 +1,196 @@
+/**************************************************************************/
+/*  jolt_body_accessor_3d.cpp                                             */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "jolt_body_accessor_3d.h"
+
+#include "jolt_space_3d.h"
+
+namespace {
+
+template <class... TTypes>
+struct VariantVisitors : TTypes... {
+	using TTypes::operator()...;
+};
+
+template <class... TTypes>
+VariantVisitors(TTypes...) -> VariantVisitors<TTypes...>;
+
+} // namespace
+
+JoltBodyAccessor3D::JoltBodyAccessor3D(const JoltSpace3D *p_space) :
+		space(p_space) {
+}
+
+JoltBodyAccessor3D::~JoltBodyAccessor3D() = default;
+
+void JoltBodyAccessor3D::acquire(const JPH::BodyID *p_ids, int p_id_count) {
+	ERR_FAIL_NULL(space);
+
+	lock_iface = &space->get_lock_iface();
+	ids = BodyIDSpan(p_ids, p_id_count);
+	_acquire_internal(p_ids, p_id_count);
+}
+
+void JoltBodyAccessor3D::acquire(const JPH::BodyID &p_id) {
+	ERR_FAIL_NULL(space);
+
+	lock_iface = &space->get_lock_iface();
+	ids = p_id;
+	_acquire_internal(&p_id, 1);
+}
+
+void JoltBodyAccessor3D::acquire_active() {
+	const JPH::PhysicsSystem &physics_system = space->get_physics_system();
+
+	acquire(physics_system.GetActiveBodiesUnsafe(JPH::EBodyType::RigidBody), (int)physics_system.GetNumActiveBodies(JPH::EBodyType::RigidBody));
+}
+
+void JoltBodyAccessor3D::acquire_all() {
+	ERR_FAIL_NULL(space);
+
+	lock_iface = &space->get_lock_iface();
+
+	JPH::BodyIDVector *vector = std::get_if<JPH::BodyIDVector>(&ids);
+
+	if (vector == nullptr) {
+		ids = JPH::BodyIDVector();
+		vector = std::get_if<JPH::BodyIDVector>(&ids);
+	}
+
+	space->get_physics_system().GetBodies(*vector);
+
+	_acquire_internal(vector->data(), (int)vector->size());
+}
+
+void JoltBodyAccessor3D::release() {
+	_release_internal();
+	lock_iface = nullptr;
+}
+
+const JPH::BodyID *JoltBodyAccessor3D::get_ids() const {
+	ERR_FAIL_COND_V(not_acquired(), nullptr);
+
+	return std::visit(
+			VariantVisitors{
+					[](const JPH::BodyID &p_id) { return &p_id; },
+					[](const JPH::BodyIDVector &p_vector) { return p_vector.data(); },
+					[](const BodyIDSpan &p_span) { return p_span.ptr; } },
+			ids);
+}
+
+int JoltBodyAccessor3D::get_count() const {
+	ERR_FAIL_COND_V(not_acquired(), 0);
+
+	return std::visit(
+			VariantVisitors{
+					[](const JPH::BodyID &p_id) { return 1; },
+					[](const JPH::BodyIDVector &p_vector) { return (int)p_vector.size(); },
+					[](const BodyIDSpan &p_span) { return p_span.count; } },
+			ids);
+}
+
+const JPH::BodyID &JoltBodyAccessor3D::get_at(int p_index) const {
+	CRASH_BAD_INDEX(p_index, get_count());
+	return get_ids()[p_index];
+}
+
+void JoltBodyReader3D::_acquire_internal(const JPH::BodyID *p_ids, int p_id_count) {
+	mutex_mask = lock_iface->GetMutexMask(p_ids, p_id_count);
+	lock_iface->LockRead(mutex_mask);
+}
+
+void JoltBodyReader3D::_release_internal() {
+	ERR_FAIL_COND(not_acquired());
+	lock_iface->UnlockRead(mutex_mask);
+}
+
+JoltBodyReader3D::JoltBodyReader3D(const JoltSpace3D *p_space) :
+		JoltBodyAccessor3D(p_space) {
+}
+
+const JPH::Body *JoltBodyReader3D::try_get(const JPH::BodyID &p_id) const {
+	if (unlikely(p_id.IsInvalid())) {
+		return nullptr;
+	}
+
+	ERR_FAIL_COND_V(not_acquired(), nullptr);
+
+	return lock_iface->TryGetBody(p_id);
+}
+
+const JPH::Body *JoltBodyReader3D::try_get(int p_index) const {
+	const int count = get_count();
+	if (unlikely(p_index < 0 || p_index >= count)) {
+		return nullptr;
+	}
+
+	return try_get(get_at(p_index));
+}
+
+const JPH::Body *JoltBodyReader3D::try_get() const {
+	return try_get(0);
+}
+
+void JoltBodyWriter3D::_acquire_internal(const JPH::BodyID *p_ids, int p_id_count) {
+	mutex_mask = lock_iface->GetMutexMask(p_ids, p_id_count);
+	lock_iface->LockWrite(mutex_mask);
+}
+
+void JoltBodyWriter3D::_release_internal() {
+	ERR_FAIL_COND(not_acquired());
+	lock_iface->UnlockWrite(mutex_mask);
+}
+
+JoltBodyWriter3D::JoltBodyWriter3D(const JoltSpace3D *p_space) :
+		JoltBodyAccessor3D(p_space) {
+}
+
+JPH::Body *JoltBodyWriter3D::try_get(const JPH::BodyID &p_id) const {
+	if (unlikely(p_id.IsInvalid())) {
+		return nullptr;
+	}
+
+	ERR_FAIL_COND_V(not_acquired(), nullptr);
+
+	return lock_iface->TryGetBody(p_id);
+}
+
+JPH::Body *JoltBodyWriter3D::try_get(int p_index) const {
+	const int count = get_count();
+	if (unlikely(p_index < 0 || p_index >= count)) {
+		return nullptr;
+	}
+
+	return try_get(get_at(p_index));
+}
+
+JPH::Body *JoltBodyWriter3D::try_get() const {
+	return try_get(0);
+}

modules/jolt_physics/spaces/jolt_body_accessor_3d.h

@@ -0,0 +1,214 @@
+/**************************************************************************/
+/*  jolt_body_accessor_3d.h                                               */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef JOLT_BODY_ACCESSOR_3D_H
+#define JOLT_BODY_ACCESSOR_3D_H
+
+#include "../objects/jolt_object_3d.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Body/BodyLockInterface.h"
+
+#include <variant>
+
+class JoltArea3D;
+class JoltBody3D;
+class JoltShapedObject3D;
+class JoltSpace3D;
+
+class JoltBodyAccessor3D {
+protected:
+	struct BodyIDSpan {
+		BodyIDSpan(const JPH::BodyID *p_ptr, int p_count) :
+				ptr(p_ptr), count(p_count) {}
+
+		const JPH::BodyID *ptr;
+		int count;
+	};
+
+	virtual void _acquire_internal(const JPH::BodyID *p_ids, int p_id_count) = 0;
+	virtual void _release_internal() = 0;
+
+	const JoltSpace3D *space = nullptr;
+
+	const JPH::BodyLockInterface *lock_iface = nullptr;
+
+	std::variant<JPH::BodyID, JPH::BodyIDVector, BodyIDSpan> ids;
+
+public:
+	explicit JoltBodyAccessor3D(const JoltSpace3D *p_space);
+	virtual ~JoltBodyAccessor3D() = 0;
+
+	void acquire(const JPH::BodyID *p_ids, int p_id_count);
+	void acquire(const JPH::BodyID &p_id);
+	void acquire_active();
+	void acquire_all();
+	void release();
+
+	bool is_acquired() const { return lock_iface != nullptr; }
+	bool not_acquired() const { return lock_iface == nullptr; }
+
+	const JoltSpace3D &get_space() const { return *space; }
+	const JPH::BodyID *get_ids() const;
+	int get_count() const;
+
+	const JPH::BodyID &get_at(int p_index) const;
+};
+
+class JoltBodyReader3D final : public JoltBodyAccessor3D {
+	virtual void _acquire_internal(const JPH::BodyID *p_ids, int p_id_count) override;
+	virtual void _release_internal() override;
+
+	JPH::BodyLockInterface::MutexMask mutex_mask = 0;
+
+public:
+	explicit JoltBodyReader3D(const JoltSpace3D *p_space);
+
+	const JPH::Body *try_get(const JPH::BodyID &p_id) const;
+	const JPH::Body *try_get(int p_index) const;
+	const JPH::Body *try_get() const;
+};
+
+class JoltBodyWriter3D final : public JoltBodyAccessor3D {
+	virtual void _acquire_internal(const JPH::BodyID *p_ids, int p_id_count) override;
+	virtual void _release_internal() override;
+
+	JPH::BodyLockInterface::MutexMask mutex_mask = 0;
+
+public:
+	explicit JoltBodyWriter3D(const JoltSpace3D *p_space);
+
+	JPH::Body *try_get(const JPH::BodyID &p_id) const;
+	JPH::Body *try_get(int p_index) const;
+	JPH::Body *try_get() const;
+};
+
+template <typename TBodyAccessor>
+class JoltScopedBodyAccessor3D {
+	TBodyAccessor inner;
+
+public:
+	JoltScopedBodyAccessor3D(const JoltSpace3D &p_space, const JPH::BodyID *p_ids, int p_id_count) :
+			inner(&p_space) { inner.acquire(p_ids, p_id_count); }
+
+	JoltScopedBodyAccessor3D(const JoltSpace3D &p_space, const JPH::BodyID &p_id) :
+			inner(&p_space) { inner.acquire(p_id); }
+
+	JoltScopedBodyAccessor3D(const JoltScopedBodyAccessor3D &p_other) = delete;
+	JoltScopedBodyAccessor3D(JoltScopedBodyAccessor3D &&p_other) = default;
+	~JoltScopedBodyAccessor3D() { inner.release(); }
+
+	const JoltSpace3D &get_space() const { return inner.get_space(); }
+	int get_count() const { return inner.get_count(); }
+	const JPH::BodyID &get_at(int p_index) const { return inner.get_at(p_index); }
+
+	JoltScopedBodyAccessor3D &operator=(const JoltScopedBodyAccessor3D &p_other) = delete;
+	JoltScopedBodyAccessor3D &operator=(JoltScopedBodyAccessor3D &&p_other) = default;
+
+	decltype(auto) try_get(const JPH::BodyID &p_id) const { return inner.try_get(p_id); }
+	decltype(auto) try_get(int p_index) const { return inner.try_get(p_index); }
+	decltype(auto) try_get() const { return inner.try_get(); }
+};
+
+template <typename TAccessor, typename TBody>
+class JoltAccessibleBody3D {
+	TAccessor accessor;
+	TBody *body = nullptr;
+
+public:
+	JoltAccessibleBody3D(const JoltSpace3D &p_space, const JPH::BodyID &p_id) :
+			accessor(p_space, p_id), body(accessor.try_get()) {}
+
+	bool is_valid() const { return body != nullptr; }
+	bool is_invalid() const { return body == nullptr; }
+
+	JoltObject3D *as_object() const {
+		if (body != nullptr) {
+			return reinterpret_cast<JoltObject3D *>(body->GetUserData());
+		} else {
+			return nullptr;
+		}
+	}
+
+	JoltShapedObject3D *as_shaped() const {
+		if (JoltObject3D *object = as_object(); object != nullptr && object->is_shaped()) {
+			return reinterpret_cast<JoltShapedObject3D *>(body->GetUserData());
+		} else {
+			return nullptr;
+		}
+	}
+
+	JoltBody3D *as_body() const {
+		if (JoltObject3D *object = as_object(); object != nullptr && object->is_body()) {
+			return reinterpret_cast<JoltBody3D *>(body->GetUserData());
+		} else {
+			return nullptr;
+		}
+	}
+
+	JoltArea3D *as_area() const {
+		if (JoltObject3D *object = as_object(); object != nullptr && object->is_area()) {
+			return reinterpret_cast<JoltArea3D *>(body->GetUserData());
+		} else {
+			return nullptr;
+		}
+	}
+
+	TBody *operator->() const { return body; }
+	TBody &operator*() const { return *body; }
+
+	explicit operator TBody *() const { return body; }
+};
+
+template <typename TAccessor, typename TBody>
+class JoltAccessibleBodies3D {
+	TAccessor accessor;
+
+public:
+	JoltAccessibleBodies3D(const JoltSpace3D &p_space, const JPH::BodyID *p_ids, int p_id_count) :
+			accessor(p_space, p_ids, p_id_count) {}
+
+	JoltAccessibleBody3D<TAccessor, TBody> operator[](int p_index) const {
+		const JPH::BodyID &body_id = p_index < accessor.get_count() ? accessor.get_at(p_index) : JPH::BodyID();
+		return JoltAccessibleBody3D<TAccessor, TBody>(accessor.get_space(), body_id);
+	}
+};
+
+typedef JoltScopedBodyAccessor3D<JoltBodyReader3D> JoltScopedBodyReader3D;
+typedef JoltScopedBodyAccessor3D<JoltBodyWriter3D> JoltScopedBodyWriter3D;
+
+typedef JoltAccessibleBody3D<JoltScopedBodyReader3D, const JPH::Body> JoltReadableBody3D;
+typedef JoltAccessibleBody3D<JoltScopedBodyWriter3D, JPH::Body> JoltWritableBody3D;
+
+typedef JoltAccessibleBodies3D<JoltScopedBodyReader3D, const JPH::Body> JoltReadableBodies3D;
+typedef JoltAccessibleBodies3D<JoltScopedBodyWriter3D, JPH::Body> JoltWritableBodies3D;
+
+#endif // JOLT_BODY_ACCESSOR_3D_H

modules/jolt_physics/spaces/jolt_broad_phase_layer.h

@@ -0,0 +1,52 @@
+/**************************************************************************/
+/*  jolt_broad_phase_layer.h                                              */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef JOLT_BROAD_PHASE_LAYER_H
+#define JOLT_BROAD_PHASE_LAYER_H
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h"
+
+#include <stdint.h>
+
+namespace JoltBroadPhaseLayer {
+
+constexpr JPH::BroadPhaseLayer BODY_STATIC(0);
+constexpr JPH::BroadPhaseLayer BODY_STATIC_BIG(1);
+constexpr JPH::BroadPhaseLayer BODY_DYNAMIC(2);
+constexpr JPH::BroadPhaseLayer AREA_DETECTABLE(3);
+constexpr JPH::BroadPhaseLayer AREA_UNDETECTABLE(4);
+
+constexpr uint32_t COUNT = 5;
+
+} // namespace JoltBroadPhaseLayer
+
+#endif // JOLT_BROAD_PHASE_LAYER_H

modules/jolt_physics/spaces/jolt_contact_listener_3d.cpp

@@ -0,0 +1,548 @@
+/**************************************************************************/
+/*  jolt_contact_listener_3d.cpp                                          */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "jolt_contact_listener_3d.h"
+
+#include "../jolt_project_settings.h"
+#include "../misc/jolt_type_conversions.h"
+#include "../objects/jolt_area_3d.h"
+#include "../objects/jolt_body_3d.h"
+#include "../objects/jolt_soft_body_3d.h"
+#include "jolt_space_3d.h"
+
+#include "Jolt/Physics/Collision/EstimateCollisionResponse.h"
+#include "Jolt/Physics/SoftBody/SoftBodyManifold.h"
+
+void JoltContactListener3D::OnContactAdded(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold, JPH::ContactSettings &p_settings) {
+	_try_override_collision_response(p_body1, p_body2, p_settings);
+	_try_apply_surface_velocities(p_body1, p_body2, p_settings);
+	_try_add_contacts(p_body1, p_body2, p_manifold, p_settings);
+	_try_evaluate_area_overlap(p_body1, p_body2, p_manifold);
+
+#ifdef DEBUG_ENABLED
+	_try_add_debug_contacts(p_body1, p_body2, p_manifold);
+#endif
+}
+
+void JoltContactListener3D::OnContactPersisted(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold, JPH::ContactSettings &p_settings) {
+	_try_override_collision_response(p_body1, p_body2, p_settings);
+	_try_apply_surface_velocities(p_body1, p_body2, p_settings);
+	_try_add_contacts(p_body1, p_body2, p_manifold, p_settings);
+	_try_evaluate_area_overlap(p_body1, p_body2, p_manifold);
+
+#ifdef DEBUG_ENABLED
+	_try_add_debug_contacts(p_body1, p_body2, p_manifold);
+#endif
+}
+
+void JoltContactListener3D::OnContactRemoved(const JPH::SubShapeIDPair &p_shape_pair) {
+	if (_try_remove_contacts(p_shape_pair)) {
+		return;
+	}
+
+	if (_try_remove_area_overlap(p_shape_pair)) {
+		return;
+	}
+}
+
+JPH::SoftBodyValidateResult JoltContactListener3D::OnSoftBodyContactValidate(const JPH::Body &p_soft_body, const JPH::Body &p_other_body, JPH::SoftBodyContactSettings &p_settings) {
+	_try_override_collision_response(p_soft_body, p_other_body, p_settings);
+
+	return JPH::SoftBodyValidateResult::AcceptContact;
+}
+
+#ifdef DEBUG_ENABLED
+
+void JoltContactListener3D::OnSoftBodyContactAdded(const JPH::Body &p_soft_body, const JPH::SoftBodyManifold &p_manifold) {
+	_try_add_debug_contacts(p_soft_body, p_manifold);
+}
+
+#endif
+
+bool JoltContactListener3D::_is_listening_for(const JPH::Body &p_body) const {
+	return listening_for.has(p_body.GetID());
+}
+
+bool JoltContactListener3D::_try_override_collision_response(const JPH::Body &p_jolt_body1, const JPH::Body &p_jolt_body2, JPH::ContactSettings &p_settings) {
+	if (p_jolt_body1.IsSensor() || p_jolt_body2.IsSensor()) {
+		return false;
+	}
+
+	if (!p_jolt_body1.IsDynamic() && !p_jolt_body2.IsDynamic()) {
+		return false;
+	}
+
+	const JoltBody3D *body1 = reinterpret_cast<JoltBody3D *>(p_jolt_body1.GetUserData());
+	const JoltBody3D *body2 = reinterpret_cast<JoltBody3D *>(p_jolt_body2.GetUserData());
+
+	const bool can_collide1 = body1->can_collide_with(*body2);
+	const bool can_collide2 = body2->can_collide_with(*body1);
+
+	if (can_collide1 && !can_collide2) {
+		p_settings.mInvMassScale2 = 0.0f;
+		p_settings.mInvInertiaScale2 = 0.0f;
+	} else if (can_collide2 && !can_collide1) {
+		p_settings.mInvMassScale1 = 0.0f;
+		p_settings.mInvInertiaScale1 = 0.0f;
+	}
+
+	return true;
+}
+
+bool JoltContactListener3D::_try_override_collision_response(const JPH::Body &p_jolt_soft_body, const JPH::Body &p_jolt_other_body, JPH::SoftBodyContactSettings &p_settings) {
+	if (p_jolt_other_body.IsSensor()) {
+		return false;
+	}
+
+	const JoltSoftBody3D *soft_body = reinterpret_cast<JoltSoftBody3D *>(p_jolt_soft_body.GetUserData());
+	const JoltBody3D *other_body = reinterpret_cast<JoltBody3D *>(p_jolt_other_body.GetUserData());
+
+	const bool can_collide1 = soft_body->can_collide_with(*other_body);
+	const bool can_collide2 = other_body->can_collide_with(*soft_body);
+
+	if (can_collide1 && !can_collide2) {
+		p_settings.mInvMassScale2 = 0.0f;
+		p_settings.mInvInertiaScale2 = 0.0f;
+	} else if (can_collide2 && !can_collide1) {
+		p_settings.mInvMassScale1 = 0.0f;
+	}
+
+	return true;
+}
+
+bool JoltContactListener3D::_try_apply_surface_velocities(const JPH::Body &p_jolt_body1, const JPH::Body &p_jolt_body2, JPH::ContactSettings &p_settings) {
+	if (p_jolt_body1.IsSensor() || p_jolt_body2.IsSensor()) {
+		return false;
+	}
+
+	const bool supports_surface_velocity1 = !p_jolt_body1.IsDynamic();
+	const bool supports_surface_velocity2 = !p_jolt_body2.IsDynamic();
+
+	if (supports_surface_velocity1 == supports_surface_velocity2) {
+		return false;
+	}
+
+	const JoltBody3D *body1 = reinterpret_cast<JoltBody3D *>(p_jolt_body1.GetUserData());
+	const JoltBody3D *body2 = reinterpret_cast<JoltBody3D *>(p_jolt_body2.GetUserData());
+
+	const bool has_surface_velocity1 = supports_surface_velocity1 && (body1->get_linear_surface_velocity() != Vector3() || body1->get_angular_surface_velocity() != Vector3());
+	const bool has_surface_velocity2 = supports_surface_velocity2 && (body2->get_linear_surface_velocity() != Vector3() || body2->get_angular_surface_velocity() != Vector3());
+
+	if (has_surface_velocity1 == has_surface_velocity2) {
+		return false;
+	}
+
+	const JPH::Vec3 linear_velocity1 = to_jolt(body1->get_linear_surface_velocity());
+	const JPH::Vec3 angular_velocity1 = to_jolt(body1->get_angular_surface_velocity());
+
+	const JPH::Vec3 linear_velocity2 = to_jolt(body2->get_linear_surface_velocity());
+	const JPH::Vec3 angular_velocity2 = to_jolt(body2->get_angular_surface_velocity());
+
+	const JPH::RVec3 com1 = p_jolt_body1.GetCenterOfMassPosition();
+	const JPH::RVec3 com2 = p_jolt_body2.GetCenterOfMassPosition();
+	const JPH::Vec3 rel_com2 = JPH::Vec3(com2 - com1);
+
+	const JPH::Vec3 angular_linear_velocity2 = rel_com2.Cross(angular_velocity2);
+	const JPH::Vec3 total_linear_velocity2 = linear_velocity2 + angular_linear_velocity2;
+
+	p_settings.mRelativeLinearSurfaceVelocity = total_linear_velocity2 - linear_velocity1;
+	p_settings.mRelativeAngularSurfaceVelocity = angular_velocity2 - angular_velocity1;
+
+	return true;
+}
+
+bool JoltContactListener3D::_try_add_contacts(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold, JPH::ContactSettings &p_settings) {
+	if (p_body1.IsSensor() || p_body2.IsSensor()) {
+		return false;
+	}
+
+	if (!_is_listening_for(p_body1) && !_is_listening_for(p_body2)) {
+		return false;
+	}
+
+	const JPH::SubShapeIDPair shape_pair(p_body1.GetID(), p_manifold.mSubShapeID1, p_body2.GetID(), p_manifold.mSubShapeID2);
+
+	Manifold &manifold = [&]() -> Manifold & {
+		const MutexLock write_lock(write_mutex);
+		return manifolds_by_shape_pair[shape_pair];
+	}();
+
+	const JPH::uint contact_count = p_manifold.mRelativeContactPointsOn1.size();
+
+	manifold.contacts1.reserve((uint32_t)contact_count);
+	manifold.contacts2.reserve((uint32_t)contact_count);
+	manifold.depth = p_manifold.mPenetrationDepth;
+
+	JPH::CollisionEstimationResult collision;
+
+	JPH::EstimateCollisionResponse(p_body1, p_body2, p_manifold, collision, p_settings.mCombinedFriction, p_settings.mCombinedRestitution, JoltProjectSettings::get_bounce_velocity_threshold(), 5);
+
+	for (JPH::uint i = 0; i < contact_count; ++i) {
+		const JPH::RVec3 relative_point1 = JPH::RVec3(p_manifold.mRelativeContactPointsOn1[i]);
+		const JPH::RVec3 relative_point2 = JPH::RVec3(p_manifold.mRelativeContactPointsOn2[i]);
+
+		const JPH::RVec3 world_point1 = p_manifold.mBaseOffset + relative_point1;
+		const JPH::RVec3 world_point2 = p_manifold.mBaseOffset + relative_point2;
+
+		const JPH::Vec3 velocity1 = p_body1.GetPointVelocity(world_point1);
+		const JPH::Vec3 velocity2 = p_body2.GetPointVelocity(world_point2);
+
+		const JPH::CollisionEstimationResult::Impulse &impulse = collision.mImpulses[i];
+
+		const JPH::Vec3 contact_impulse = p_manifold.mWorldSpaceNormal * impulse.mContactImpulse;
+		const JPH::Vec3 friction_impulse1 = collision.mTangent1 * impulse.mFrictionImpulse1;
+		const JPH::Vec3 friction_impulse2 = collision.mTangent2 * impulse.mFrictionImpulse2;
+		const JPH::Vec3 combined_impulse = contact_impulse + friction_impulse1 + friction_impulse2;
+
+		Contact contact1;
+		contact1.point_self = to_godot(world_point1);
+		contact1.point_other = to_godot(world_point2);
+		contact1.normal = to_godot(-p_manifold.mWorldSpaceNormal);
+		contact1.velocity_self = to_godot(velocity1);
+		contact1.velocity_other = to_godot(velocity2);
+		contact1.impulse = to_godot(-combined_impulse);
+		manifold.contacts1.push_back(contact1);
+
+		Contact contact2;
+		contact2.point_self = to_godot(world_point2);
+		contact2.point_other = to_godot(world_point1);
+		contact2.normal = to_godot(p_manifold.mWorldSpaceNormal);
+		contact2.velocity_self = to_godot(velocity2);
+		contact2.velocity_other = to_godot(velocity1);
+		contact2.impulse = to_godot(combined_impulse);
+		manifold.contacts2.push_back(contact2);
+	}
+
+	return true;
+}
+
+bool JoltContactListener3D::_try_evaluate_area_overlap(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold) {
+	if (!p_body1.IsSensor() && !p_body2.IsSensor()) {
+		return false;
+	}
+
+	auto evaluate = [&](const auto &p_area, const auto &p_object, const JPH::SubShapeIDPair &p_shape_pair) {
+		const MutexLock write_lock(write_mutex);
+
+		if (p_area.can_monitor(p_object)) {
+			if (!area_overlaps.has(p_shape_pair)) {
+				area_overlaps.insert(p_shape_pair);
+				area_enters.insert(p_shape_pair);
+			}
+		} else {
+			if (area_overlaps.erase(p_shape_pair)) {
+				area_exits.insert(p_shape_pair);
+			}
+		}
+	};
+
+	const JPH::SubShapeIDPair shape_pair1(p_body1.GetID(), p_manifold.mSubShapeID1, p_body2.GetID(), p_manifold.mSubShapeID2);
+
+	const JPH::SubShapeIDPair shape_pair2(p_body2.GetID(), p_manifold.mSubShapeID2, p_body1.GetID(), p_manifold.mSubShapeID1);
+
+	const JoltObject3D *object1 = reinterpret_cast<JoltObject3D *>(p_body1.GetUserData());
+	const JoltObject3D *object2 = reinterpret_cast<JoltObject3D *>(p_body2.GetUserData());
+
+	const JoltArea3D *area1 = object1->as_area();
+	const JoltArea3D *area2 = object2->as_area();
+
+	const JoltBody3D *body1 = object1->as_body();
+	const JoltBody3D *body2 = object2->as_body();
+
+	if (area1 != nullptr && area2 != nullptr) {
+		evaluate(*area1, *area2, shape_pair1);
+		evaluate(*area2, *area1, shape_pair2);
+	} else if (area1 != nullptr && body2 != nullptr) {
+		evaluate(*area1, *body2, shape_pair1);
+	} else if (area2 != nullptr && body1 != nullptr) {
+		evaluate(*area2, *body1, shape_pair2);
+	}
+
+	return true;
+}
+
+bool JoltContactListener3D::_try_remove_contacts(const JPH::SubShapeIDPair &p_shape_pair) {
+	const MutexLock write_lock(write_mutex);
+
+	return manifolds_by_shape_pair.erase(p_shape_pair);
+}
+
+bool JoltContactListener3D::_try_remove_area_overlap(const JPH::SubShapeIDPair &p_shape_pair) {
+	const JPH::SubShapeIDPair swapped_shape_pair(p_shape_pair.GetBody2ID(), p_shape_pair.GetSubShapeID2(), p_shape_pair.GetBody1ID(), p_shape_pair.GetSubShapeID1());
+
+	const MutexLock write_lock(write_mutex);
+
+	bool removed = false;
+
+	if (area_overlaps.erase(p_shape_pair)) {
+		area_exits.insert(p_shape_pair);
+		removed = true;
+	}
+
+	if (area_overlaps.erase(swapped_shape_pair)) {
+		area_exits.insert(swapped_shape_pair);
+		removed = true;
+	}
+
+	return removed;
+}
+
+#ifdef DEBUG_ENABLED
+
+bool JoltContactListener3D::_try_add_debug_contacts(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold) {
+	if (p_body1.IsSensor() || p_body2.IsSensor()) {
+		return false;
+	}
+
+	const int64_t max_count = debug_contacts.size();
+
+	if (max_count == 0) {
+		return false;
+	}
+
+	const int additional_pairs = (int)p_manifold.mRelativeContactPointsOn1.size();
+	const int additional_contacts = additional_pairs * 2;
+
+	int current_count = debug_contact_count.load(std::memory_order_relaxed);
+	bool exchanged = false;
+
+	do {
+		const int new_count = current_count + additional_contacts;
+
+		if (new_count > max_count) {
+			return false;
+		}
+
+		exchanged = debug_contact_count.compare_exchange_weak(current_count, new_count, std::memory_order_release, std::memory_order_relaxed);
+	} while (!exchanged);
+
+	for (int i = 0; i < additional_pairs; ++i) {
+		const int pair_index = current_count + i * 2;
+
+		const JPH::RVec3 point_on_1 = p_manifold.GetWorldSpaceContactPointOn1((JPH::uint)i);
+		const JPH::RVec3 point_on_2 = p_manifold.GetWorldSpaceContactPointOn2((JPH::uint)i);
+
+		debug_contacts.write[pair_index + 0] = to_godot(point_on_1);
+		debug_contacts.write[pair_index + 1] = to_godot(point_on_2);
+	}
+
+	return true;
+}
+
+bool JoltContactListener3D::_try_add_debug_contacts(const JPH::Body &p_soft_body, const JPH::SoftBodyManifold &p_manifold) {
+	const int64_t max_count = debug_contacts.size();
+	if (max_count == 0) {
+		return false;
+	}
+
+	int additional_contacts = 0;
+
+	for (const JPH::SoftBodyVertex &vertex : p_manifold.GetVertices()) {
+		if (p_manifold.HasContact(vertex)) {
+			additional_contacts += 1;
+		}
+	}
+
+	int current_count = debug_contact_count.load(std::memory_order_relaxed);
+	bool exchanged = false;
+
+	do {
+		const int new_count = current_count + additional_contacts;
+
+		if (new_count > max_count) {
+			return false;
+		}
+
+		exchanged = debug_contact_count.compare_exchange_weak(current_count, new_count, std::memory_order_release, std::memory_order_relaxed);
+	} while (!exchanged);
+
+	int contact_index = current_count;
+
+	for (const JPH::SoftBodyVertex &vertex : p_manifold.GetVertices()) {
+		if (!p_manifold.HasContact(vertex)) {
+			continue;
+		}
+
+		const JPH::RMat44 body_com_transform = p_soft_body.GetCenterOfMassTransform();
+		const JPH::Vec3 local_contact_point = p_manifold.GetLocalContactPoint(vertex);
+		const JPH::RVec3 contact_point = body_com_transform * local_contact_point;
+
+		debug_contacts.write[contact_index++] = to_godot(contact_point);
+	}
+
+	return true;
+}
+
+#endif
+
+void JoltContactListener3D::_flush_contacts() {
+	for (KeyValue<JPH::SubShapeIDPair, Manifold> &E : manifolds_by_shape_pair) {
+		const JPH::SubShapeIDPair &shape_pair = E.key;
+		Manifold &manifold = E.value;
+
+		const JPH::BodyID body_ids[2] = { shape_pair.GetBody1ID(), shape_pair.GetBody2ID() };
+		const JoltReadableBodies3D jolt_bodies = space->read_bodies(body_ids, 2);
+
+		JoltBody3D *body1 = jolt_bodies[0].as_body();
+		ERR_FAIL_NULL(body1);
+
+		JoltBody3D *body2 = jolt_bodies[1].as_body();
+		ERR_FAIL_NULL(body2);
+
+		const int shape_index1 = body1->find_shape_index(shape_pair.GetSubShapeID1());
+		const int shape_index2 = body2->find_shape_index(shape_pair.GetSubShapeID2());
+
+		for (const Contact &contact : manifold.contacts1) {
+			body1->add_contact(body2, manifold.depth, shape_index1, shape_index2, contact.normal, contact.point_self, contact.point_other, contact.velocity_self, contact.velocity_other, contact.impulse);
+		}
+
+		for (const Contact &contact : manifold.contacts2) {
+			body2->add_contact(body1, manifold.depth, shape_index2, shape_index1, contact.normal, contact.point_self, contact.point_other, contact.velocity_self, contact.velocity_other, contact.impulse);
+		}
+
+		manifold.contacts1.clear();
+		manifold.contacts2.clear();
+	}
+}
+
+void JoltContactListener3D::_flush_area_enters() {
+	for (const JPH::SubShapeIDPair &shape_pair : area_enters) {
+		const JPH::BodyID &body_id1 = shape_pair.GetBody1ID();
+		const JPH::BodyID &body_id2 = shape_pair.GetBody2ID();
+
+		const JPH::SubShapeID &sub_shape_id1 = shape_pair.GetSubShapeID1();
+		const JPH::SubShapeID &sub_shape_id2 = shape_pair.GetSubShapeID2();
+
+		const JPH::BodyID body_ids[2] = { body_id1, body_id2 };
+		const JoltReadableBodies3D jolt_bodies = space->read_bodies(body_ids, 2);
+
+		const JoltReadableBody3D jolt_body1 = jolt_bodies[0];
+		const JoltReadableBody3D jolt_body2 = jolt_bodies[1];
+
+		if (jolt_body1.is_invalid() || jolt_body2.is_invalid()) {
+			continue;
+		}
+
+		JoltArea3D *area1 = jolt_body1.as_area();
+		JoltArea3D *area2 = jolt_body2.as_area();
+
+		if (area1 != nullptr && area2 != nullptr) {
+			area1->area_shape_entered(body_id2, sub_shape_id2, sub_shape_id1);
+		} else if (area1 != nullptr && area2 == nullptr) {
+			area1->body_shape_entered(body_id2, sub_shape_id2, sub_shape_id1);
+		} else if (area1 == nullptr && area2 != nullptr) {
+			area2->body_shape_entered(body_id1, sub_shape_id1, sub_shape_id2);
+		}
+	}
+
+	area_enters.clear();
+}
+
+void JoltContactListener3D::_flush_area_shifts() {
+	for (const JPH::SubShapeIDPair &shape_pair : area_overlaps) {
+		auto is_shifted = [&](const JPH::BodyID &p_body_id, const JPH::SubShapeID &p_sub_shape_id) {
+			const JoltReadableBody3D jolt_body = space->read_body(p_body_id);
+			const JoltShapedObject3D *object = jolt_body.as_shaped();
+			ERR_FAIL_NULL_V(object, false);
+
+			if (object->get_previous_jolt_shape() == nullptr) {
+				return false;
+			}
+
+			const JPH::Shape &current_shape = *object->get_jolt_shape();
+			const JPH::Shape &previous_shape = *object->get_previous_jolt_shape();
+
+			const uint32_t current_id = (uint32_t)current_shape.GetSubShapeUserData(p_sub_shape_id);
+			const uint32_t previous_id = (uint32_t)previous_shape.GetSubShapeUserData(p_sub_shape_id);
+
+			return current_id != previous_id;
+		};
+
+		if (is_shifted(shape_pair.GetBody1ID(), shape_pair.GetSubShapeID1()) || is_shifted(shape_pair.GetBody2ID(), shape_pair.GetSubShapeID2())) {
+			area_enters.insert(shape_pair);
+			area_exits.insert(shape_pair);
+		}
+	}
+}
+
+void JoltContactListener3D::_flush_area_exits() {
+	for (const JPH::SubShapeIDPair &shape_pair : area_exits) {
+		const JPH::BodyID &body_id1 = shape_pair.GetBody1ID();
+		const JPH::BodyID &body_id2 = shape_pair.GetBody2ID();
+
+		const JPH::SubShapeID &sub_shape_id1 = shape_pair.GetSubShapeID1();
+		const JPH::SubShapeID &sub_shape_id2 = shape_pair.GetSubShapeID2();
+
+		const JPH::BodyID body_ids[2] = { body_id1, body_id2 };
+		const JoltReadableBodies3D jolt_bodies = space->read_bodies(body_ids, 2);
+
+		const JoltReadableBody3D jolt_body1 = jolt_bodies[0];
+		const JoltReadableBody3D jolt_body2 = jolt_bodies[1];
+
+		JoltArea3D *area1 = jolt_body1.as_area();
+		JoltArea3D *area2 = jolt_body2.as_area();
+
+		const JoltBody3D *body1 = jolt_body1.as_body();
+		const JoltBody3D *body2 = jolt_body2.as_body();
+
+		if (area1 != nullptr && area2 != nullptr) {
+			area1->area_shape_exited(body_id2, sub_shape_id2, sub_shape_id1);
+		} else if (area1 != nullptr && body2 != nullptr) {
+			area1->body_shape_exited(body_id2, sub_shape_id2, sub_shape_id1);
+		} else if (body1 != nullptr && area2 != nullptr) {
+			area2->body_shape_exited(body_id1, sub_shape_id1, sub_shape_id2);
+		} else if (area1 != nullptr) {
+			area1->shape_exited(body_id2, sub_shape_id2, sub_shape_id1);
+		} else if (area2 != nullptr) {
+			area2->shape_exited(body_id1, sub_shape_id1, sub_shape_id2);
+		}
+	}
+
+	area_exits.clear();
+}
+
+void JoltContactListener3D::listen_for(JoltShapedObject3D *p_object) {
+	listening_for.insert(p_object->get_jolt_id());
+}
+
+void JoltContactListener3D::pre_step() {
+	listening_for.clear();
+
+#ifdef DEBUG_ENABLED
+	debug_contact_count = 0;
+#endif
+}
+
+void JoltContactListener3D::post_step() {
+	_flush_contacts();
+	_flush_area_shifts();
+	_flush_area_exits();
+	_flush_area_enters();
+}

modules/jolt_physics/spaces/jolt_contact_listener_3d.h

@@ -0,0 +1,147 @@
+/**************************************************************************/
+/*  jolt_contact_listener_3d.h                                            */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef JOLT_CONTACT_LISTENER_3D_H
+#define JOLT_CONTACT_LISTENER_3D_H
+
+#include "core/templates/hash_map.h"
+#include "core/templates/hash_set.h"
+#include "core/templates/hashfuncs.h"
+#include "core/templates/local_vector.h"
+#include "core/templates/safe_refcount.h"
+#include "core/variant/variant.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Body/Body.h"
+#include "Jolt/Physics/Collision/ContactListener.h"
+#include "Jolt/Physics/SoftBody/SoftBodyContactListener.h"
+
+#include <stdint.h>
+#include <new>
+
+class JoltShapedObject3D;
+class JoltSpace3D;
+
+class JoltContactListener3D final
+		: public JPH::ContactListener,
+		  public JPH::SoftBodyContactListener {
+	struct BodyIDHasher {
+		static uint32_t hash(const JPH::BodyID &p_id) { return hash_fmix32(p_id.GetIndexAndSequenceNumber()); }
+	};
+
+	struct ShapePairHasher {
+		static uint32_t hash(const JPH::SubShapeIDPair &p_pair) {
+			uint32_t hash = hash_murmur3_one_32(p_pair.GetBody1ID().GetIndexAndSequenceNumber());
+			hash = hash_murmur3_one_32(p_pair.GetSubShapeID1().GetValue(), hash);
+			hash = hash_murmur3_one_32(p_pair.GetBody2ID().GetIndexAndSequenceNumber(), hash);
+			hash = hash_murmur3_one_32(p_pair.GetSubShapeID2().GetValue(), hash);
+			return hash_fmix32(hash);
+		}
+	};
+
+	struct Contact {
+		Vector3 point_self;
+		Vector3 point_other;
+		Vector3 normal;
+		Vector3 velocity_self;
+		Vector3 velocity_other;
+		Vector3 impulse;
+	};
+
+	typedef LocalVector<Contact> Contacts;
+
+	struct Manifold {
+		Contacts contacts1;
+		Contacts contacts2;
+		float depth = 0.0f;
+	};
+
+	HashMap<JPH::SubShapeIDPair, Manifold, ShapePairHasher> manifolds_by_shape_pair;
+	HashSet<JPH::BodyID, BodyIDHasher> listening_for;
+	HashSet<JPH::SubShapeIDPair, ShapePairHasher> area_overlaps;
+	HashSet<JPH::SubShapeIDPair, ShapePairHasher> area_enters;
+	HashSet<JPH::SubShapeIDPair, ShapePairHasher> area_exits;
+	Mutex write_mutex;
+	JoltSpace3D *space = nullptr;
+
+#ifdef DEBUG_ENABLED
+	PackedVector3Array debug_contacts;
+	std::atomic_int debug_contact_count;
+#endif
+
+	virtual void OnContactAdded(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold, JPH::ContactSettings &p_settings) override;
+	virtual void OnContactPersisted(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold, JPH::ContactSettings &p_settings) override;
+	virtual void OnContactRemoved(const JPH::SubShapeIDPair &p_shape_pair) override;
+
+	virtual JPH::SoftBodyValidateResult OnSoftBodyContactValidate(const JPH::Body &p_soft_body, const JPH::Body &p_other_body, JPH::SoftBodyContactSettings &p_settings) override;
+
+#ifdef DEBUG_ENABLED
+	virtual void OnSoftBodyContactAdded(const JPH::Body &p_soft_body, const JPH::SoftBodyManifold &p_manifold) override;
+#endif
+
+	bool _is_listening_for(const JPH::Body &p_body) const;
+
+	bool _try_override_collision_response(const JPH::Body &p_jolt_body1, const JPH::Body &p_jolt_body2, JPH::ContactSettings &p_settings);
+	bool _try_override_collision_response(const JPH::Body &p_jolt_soft_body, const JPH::Body &p_jolt_other_body, JPH::SoftBodyContactSettings &p_settings);
+	bool _try_apply_surface_velocities(const JPH::Body &p_jolt_body1, const JPH::Body &p_jolt_body2, JPH::ContactSettings &p_settings);
+	bool _try_add_contacts(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold, JPH::ContactSettings &p_settings);
+	bool _try_evaluate_area_overlap(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold);
+	bool _try_remove_contacts(const JPH::SubShapeIDPair &p_shape_pair);
+	bool _try_remove_area_overlap(const JPH::SubShapeIDPair &p_shape_pair);
+
+#ifdef DEBUG_ENABLED
+	bool _try_add_debug_contacts(const JPH::Body &p_body1, const JPH::Body &p_body2, const JPH::ContactManifold &p_manifold);
+	bool _try_add_debug_contacts(const JPH::Body &p_soft_body, const JPH::SoftBodyManifold &p_manifold);
+#endif
+
+	void _flush_contacts();
+	void _flush_area_enters();
+	void _flush_area_shifts();
+	void _flush_area_exits();
+
+public:
+	explicit JoltContactListener3D(JoltSpace3D *p_space) :
+			space(p_space) {}
+
+	void listen_for(JoltShapedObject3D *p_object);
+
+	void pre_step();
+	void post_step();
+
+#ifdef DEBUG_ENABLED
+	const PackedVector3Array &get_debug_contacts() const { return debug_contacts; }
+	int get_debug_contact_count() const { return debug_contact_count.load(std::memory_order_acquire); }
+	int get_max_debug_contacts() const { return (int)debug_contacts.size(); }
+	void set_max_debug_contacts(int p_count) { debug_contacts.resize(p_count); }
+#endif
+};
+
+#endif // JOLT_CONTACT_LISTENER_3D_H

modules/jolt_physics/spaces/jolt_job_system.cpp

@@ -0,0 +1,201 @@
+/**************************************************************************/
+/*  jolt_job_system.cpp                                                   */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "jolt_job_system.h"
+
+#include "../jolt_project_settings.h"
+
+#include "core/debugger/engine_debugger.h"
+#include "core/object/worker_thread_pool.h"
+#include "core/os/os.h"
+#include "core/os/time.h"
+
+#include "Jolt/Physics/PhysicsSettings.h"
+
+void JoltJobSystem::Job::_execute(void *p_user_data) {
+	Job *job = static_cast<Job *>(p_user_data);
+
+#ifdef DEBUG_ENABLED
+	const uint64_t time_start = Time::get_singleton()->get_ticks_usec();
+#endif
+
+	job->Execute();
+
+#ifdef DEBUG_ENABLED
+	const uint64_t time_end = Time::get_singleton()->get_ticks_usec();
+	const uint64_t time_elapsed = time_end - time_start;
+
+	timings_lock.lock();
+	timings_by_job[job->name] += time_elapsed;
+	timings_lock.unlock();
+#endif
+
+	job->Release();
+}
+
+JoltJobSystem::Job::Job(const char *p_name, JPH::ColorArg p_color, JPH::JobSystem *p_job_system, const JPH::JobSystem::JobFunction &p_job_function, JPH::uint32 p_dependency_count) :
+		JPH::JobSystem::Job(p_name, p_color, p_job_system, p_job_function, p_dependency_count)
+#ifdef DEBUG_ENABLED
+		,
+		name(p_name)
+#endif
+{
+}
+
+JoltJobSystem::Job::~Job() {
+	if (task_id != -1) {
+		WorkerThreadPool::get_singleton()->wait_for_task_completion(task_id);
+	}
+}
+
+void JoltJobSystem::Job::push_completed(Job *p_job) {
+	Job *prev_head = nullptr;
+
+	do {
+		prev_head = completed_head.load(std::memory_order_relaxed);
+		p_job->completed_next = prev_head;
+	} while (!completed_head.compare_exchange_weak(prev_head, p_job, std::memory_order_release, std::memory_order_relaxed));
+}
+
+JoltJobSystem::Job *JoltJobSystem::Job::pop_completed() {
+	Job *prev_head = nullptr;
+
+	do {
+		prev_head = completed_head.load(std::memory_order_relaxed);
+		if (prev_head == nullptr) {
+			return nullptr;
+		}
+	} while (!completed_head.compare_exchange_weak(prev_head, prev_head->completed_next, std::memory_order_acquire, std::memory_order_relaxed));
+
+	return prev_head;
+}
+
+void JoltJobSystem::Job::queue() {
+	AddRef();
+
+	// Ideally we would use Jolt's actual job name here, but I'd rather not incur the overhead of a memory allocation or
+	// thread-safe lookup every time we create/queue a task. So instead we use the same cached description for all of them.
+	static const String task_name("Jolt Physics");
+
+	task_id = WorkerThreadPool::get_singleton()->add_native_task(&_execute, this, true, task_name);
+}
+
+int JoltJobSystem::GetMaxConcurrency() const {
+	return thread_count;
+}
+
+JPH::JobHandle JoltJobSystem::CreateJob(const char *p_name, JPH::ColorArg p_color, const JPH::JobSystem::JobFunction &p_job_function, JPH::uint32 p_dependency_count) {
+	Job *job = nullptr;
+
+	while (true) {
+		JPH::uint32 job_index = jobs.ConstructObject(p_name, p_color, this, p_job_function, p_dependency_count);
+
+		if (job_index != JPH::FixedSizeFreeList<Job>::cInvalidObjectIndex) {
+			job = &jobs.Get(job_index);
+			break;
+		}
+
+		WARN_PRINT_ONCE("Jolt Physics job system exceeded the maximum number of jobs. This should not happen. Please report this. Waiting for jobs to become available...");
+
+		OS::get_singleton()->delay_usec(100);
+
+		_reclaim_jobs();
+	}
+
+	// This will increment the job's reference count, so must happen before we queue the job
+	JPH::JobHandle job_handle(job);
+
+	if (p_dependency_count == 0) {
+		QueueJob(job);
+	}
+
+	return job_handle;
+}
+
+void JoltJobSystem::QueueJob(JPH::JobSystem::Job *p_job) {
+	static_cast<Job *>(p_job)->queue();
+}
+
+void JoltJobSystem::QueueJobs(JPH::JobSystem::Job **p_jobs, JPH::uint p_job_count) {
+	for (JPH::uint i = 0; i < p_job_count; ++i) {
+		QueueJob(p_jobs[i]);
+	}
+}
+
+void JoltJobSystem::FreeJob(JPH::JobSystem::Job *p_job) {
+	Job::push_completed(static_cast<Job *>(p_job));
+}
+
+void JoltJobSystem::_reclaim_jobs() {
+	while (Job *job = Job::pop_completed()) {
+		jobs.DestructObject(job);
+	}
+}
+
+JoltJobSystem::JoltJobSystem() :
+		JPH::JobSystemWithBarrier(JPH::cMaxPhysicsBarriers),
+		thread_count(MAX(1, WorkerThreadPool::get_singleton()->get_thread_count())) {
+	jobs.Init(JPH::cMaxPhysicsJobs, JPH::cMaxPhysicsJobs);
+}
+
+void JoltJobSystem::pre_step() {
+	// Nothing to do.
+}
+
+void JoltJobSystem::post_step() {
+	_reclaim_jobs();
+}
+
+#ifdef DEBUG_ENABLED
+
+void JoltJobSystem::flush_timings() {
+	static const StringName profiler_name("servers");
+
+	EngineDebugger *engine_debugger = EngineDebugger::get_singleton();
+
+	if (engine_debugger->is_profiling(profiler_name)) {
+		Array timings;
+
+		for (const KeyValue<const void *, uint64_t> &E : timings_by_job) {
+			timings.push_back(static_cast<const char *>(E.key));
+			timings.push_back(USEC_TO_SEC(E.value));
+		}
+
+		timings.push_front("physics_3d");
+
+		engine_debugger->profiler_add_frame_data(profiler_name, timings);
+	}
+
+	for (KeyValue<const void *, uint64_t> &E : timings_by_job) {
+		E.value = 0;
+	}
+}
+
+#endif

modules/jolt_physics/spaces/jolt_job_system.h

@@ -0,0 +1,107 @@
+/**************************************************************************/
+/*  jolt_job_system.h                                                     */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef JOLT_JOB_SYSTEM_H
+#define JOLT_JOB_SYSTEM_H
+
+#include "core/os/spin_lock.h"
+#include "core/templates/hash_map.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Core/FixedSizeFreeList.h"
+#include "Jolt/Core/JobSystemWithBarrier.h"
+
+#include <stdint.h>
+#include <atomic>
+
+class JoltJobSystem final : public JPH::JobSystemWithBarrier {
+	class Job : public JPH::JobSystem::Job {
+		inline static std::atomic<Job *> completed_head = nullptr;
+
+#ifdef DEBUG_ENABLED
+		const char *name = nullptr;
+#endif
+
+		int64_t task_id = -1;
+
+		std::atomic<Job *> completed_next = nullptr;
+
+		static void _execute(void *p_user_data);
+
+	public:
+		Job(const char *p_name, JPH::ColorArg p_color, JPH::JobSystem *p_job_system, const JPH::JobSystem::JobFunction &p_job_function, JPH::uint32 p_dependency_count);
+		Job(const Job &p_other) = delete;
+		Job(Job &&p_other) = delete;
+		~Job();
+
+		static void push_completed(Job *p_job);
+		static Job *pop_completed();
+
+		void queue();
+
+		Job &operator=(const Job &p_other) = delete;
+		Job &operator=(Job &&p_other) = delete;
+	};
+
+#ifdef DEBUG_ENABLED
+	// We use `const void*` here to avoid the cost of hashing the actual string, since the job names
+	// are always literals and as such will point to the same address every time.
+	inline static HashMap<const void *, uint64_t> timings_by_job;
+
+	// TODO: Check whether the usage of SpinLock is justified or if this should be a mutex instead.
+	inline static SpinLock timings_lock;
+#endif
+
+	JPH::FixedSizeFreeList<Job> jobs;
+
+	int thread_count = 0;
+
+	virtual int GetMaxConcurrency() const override;
+
+	virtual JPH::JobHandle CreateJob(const char *p_name, JPH::ColorArg p_color, const JPH::JobSystem::JobFunction &p_job_function, JPH::uint32 p_dependency_count = 0) override;
+	virtual void QueueJob(JPH::JobSystem::Job *p_job) override;
+	virtual void QueueJobs(JPH::JobSystem::Job **p_jobs, JPH::uint p_job_count) override;
+	virtual void FreeJob(JPH::JobSystem::Job *p_job) override;
+
+	void _reclaim_jobs();
+
+public:
+	JoltJobSystem();
+
+	void pre_step();
+	void post_step();
+
+#ifdef DEBUG_ENABLED
+	void flush_timings();
+#endif
+};
+
+#endif // JOLT_JOB_SYSTEM_H

modules/jolt_physics/spaces/jolt_layers.cpp

@@ -0,0 +1,223 @@
+/**************************************************************************/
+/*  jolt_layers.cpp                                                       */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "jolt_layers.h"
+
+#include "../jolt_project_settings.h"
+#include "jolt_broad_phase_layer.h"
+
+#include "core/error/error_macros.h"
+#include "core/variant/variant.h"
+
+static_assert(sizeof(JPH::ObjectLayer) == 2, "Size of Jolt's object layer has changed.");
+static_assert(sizeof(JPH::BroadPhaseLayer::Type) == 1, "Size of Jolt's broadphase layer has changed.");
+static_assert(JoltBroadPhaseLayer::COUNT <= 8, "Maximum number of broadphase layers exceeded.");
+
+namespace {
+
+template <uint8_t TSize = JoltBroadPhaseLayer::COUNT>
+class JoltBroadPhaseMatrix {
+	typedef JPH::BroadPhaseLayer LayerType;
+	typedef LayerType::Type UnderlyingType;
+
+public:
+	JoltBroadPhaseMatrix() {
+		using namespace JoltBroadPhaseLayer;
+
+		allow_collision(BODY_STATIC, BODY_DYNAMIC);
+		allow_collision(BODY_STATIC_BIG, BODY_DYNAMIC);
+		allow_collision(BODY_DYNAMIC, BODY_STATIC);
+		allow_collision(BODY_DYNAMIC, BODY_STATIC_BIG);
+		allow_collision(BODY_DYNAMIC, BODY_DYNAMIC);
+		allow_collision(BODY_DYNAMIC, AREA_DETECTABLE);
+		allow_collision(BODY_DYNAMIC, AREA_UNDETECTABLE);
+		allow_collision(AREA_DETECTABLE, BODY_DYNAMIC);
+		allow_collision(AREA_DETECTABLE, AREA_DETECTABLE);
+		allow_collision(AREA_DETECTABLE, AREA_UNDETECTABLE);
+		allow_collision(AREA_UNDETECTABLE, BODY_DYNAMIC);
+		allow_collision(AREA_UNDETECTABLE, AREA_DETECTABLE);
+
+		if (JoltProjectSettings::areas_detect_static_bodies()) {
+			allow_collision(BODY_STATIC, AREA_DETECTABLE);
+			allow_collision(BODY_STATIC, AREA_UNDETECTABLE);
+			allow_collision(BODY_STATIC_BIG, AREA_DETECTABLE);
+			allow_collision(BODY_STATIC_BIG, AREA_UNDETECTABLE);
+			allow_collision(AREA_DETECTABLE, BODY_STATIC);
+			allow_collision(AREA_DETECTABLE, BODY_STATIC_BIG);
+			allow_collision(AREA_UNDETECTABLE, BODY_STATIC);
+			allow_collision(AREA_UNDETECTABLE, BODY_STATIC_BIG);
+		}
+	}
+
+	void allow_collision(UnderlyingType p_layer1, UnderlyingType p_layer2) { masks[p_layer1] |= uint8_t(1U << p_layer2); }
+	void allow_collision(LayerType p_layer1, LayerType p_layer2) { allow_collision((UnderlyingType)p_layer1, (UnderlyingType)p_layer2); }
+
+	bool should_collide(UnderlyingType p_layer1, UnderlyingType p_layer2) const { return (masks[p_layer1] & uint8_t(1U << p_layer2)) != 0; }
+	bool should_collide(LayerType p_layer1, LayerType p_layer2) const { return should_collide((UnderlyingType)p_layer1, (UnderlyingType)p_layer2); }
+
+private:
+	uint8_t masks[TSize] = {};
+};
+
+constexpr JPH::ObjectLayer encode_layers(JPH::BroadPhaseLayer p_broad_phase_layer, JPH::ObjectLayer p_object_layer) {
+	const uint16_t upper_bits = uint16_t((uint8_t)p_broad_phase_layer << 13U);
+	const uint16_t lower_bits = uint16_t(p_object_layer);
+	return JPH::ObjectLayer(upper_bits | lower_bits);
+}
+
+constexpr void decode_layers(JPH::ObjectLayer p_encoded_layers, JPH::BroadPhaseLayer &r_broad_phase_layer, JPH::ObjectLayer &r_object_layer) {
+	r_broad_phase_layer = JPH::BroadPhaseLayer(uint8_t(p_encoded_layers >> 13U));
+	r_object_layer = JPH::ObjectLayer(p_encoded_layers & 0b0001'1111'1111'1111U);
+}
+
+constexpr uint64_t encode_collision(uint32_t p_collision_layer, uint32_t p_collision_mask) {
+	const uint64_t upper_bits = (uint64_t)p_collision_layer << 32U;
+	const uint64_t lower_bits = (uint64_t)p_collision_mask;
+	return upper_bits | lower_bits;
+}
+
+constexpr void decode_collision(uint64_t p_collision, uint32_t &r_collision_layer, uint32_t &r_collision_mask) {
+	r_collision_layer = uint32_t(p_collision >> 32U);
+	r_collision_mask = uint32_t(p_collision & 0xFFFFFFFFU);
+}
+
+} // namespace
+
+uint32_t JoltLayers::GetNumBroadPhaseLayers() const {
+	return JoltBroadPhaseLayer::COUNT;
+}
+
+JPH::BroadPhaseLayer JoltLayers::GetBroadPhaseLayer(JPH::ObjectLayer p_layer) const {
+	JPH::BroadPhaseLayer broad_phase_layer = JoltBroadPhaseLayer::BODY_STATIC;
+	JPH::ObjectLayer object_layer = 0;
+	decode_layers(p_layer, broad_phase_layer, object_layer);
+
+	return broad_phase_layer;
+}
+
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+
+const char *JoltLayers::GetBroadPhaseLayerName(JPH::BroadPhaseLayer p_layer) const {
+	switch ((JPH::BroadPhaseLayer::Type)p_layer) {
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::BODY_STATIC: {
+			return "BODY_STATIC";
+		}
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::BODY_STATIC_BIG: {
+			return "BODY_STATIC_BIG";
+		}
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::BODY_DYNAMIC: {
+			return "BODY_DYNAMIC";
+		}
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::AREA_DETECTABLE: {
+			return "AREA_DETECTABLE";
+		}
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::AREA_UNDETECTABLE: {
+			return "AREA_UNDETECTABLE";
+		}
+		default: {
+			return "UNKNOWN";
+		}
+	}
+}
+
+#endif
+
+bool JoltLayers::ShouldCollide(JPH::ObjectLayer p_encoded_layer1, JPH::ObjectLayer p_encoded_layer2) const {
+	JPH::BroadPhaseLayer broad_phase_layer1 = JoltBroadPhaseLayer::BODY_STATIC;
+	uint32_t collision_layer1 = 0;
+	uint32_t collision_mask1 = 0;
+	from_object_layer(p_encoded_layer1, broad_phase_layer1, collision_layer1, collision_mask1);
+
+	JPH::BroadPhaseLayer broad_phase_layer2 = JoltBroadPhaseLayer::BODY_STATIC;
+	uint32_t collision_layer2 = 0;
+	uint32_t collision_mask2 = 0;
+	from_object_layer(p_encoded_layer2, broad_phase_layer2, collision_layer2, collision_mask2);
+
+	const bool first_scans_second = (collision_mask1 & collision_layer2) != 0;
+	const bool second_scans_first = (collision_mask2 & collision_layer1) != 0;
+
+	return first_scans_second || second_scans_first;
+}
+
+bool JoltLayers::ShouldCollide(JPH::ObjectLayer p_encoded_layer1, JPH::BroadPhaseLayer p_broad_phase_layer2) const {
+	static const JoltBroadPhaseMatrix matrix;
+
+	JPH::BroadPhaseLayer broad_phase_layer1 = JoltBroadPhaseLayer::BODY_STATIC;
+	JPH::ObjectLayer object_layer1 = 0;
+	decode_layers(p_encoded_layer1, broad_phase_layer1, object_layer1);
+
+	return matrix.should_collide(broad_phase_layer1, p_broad_phase_layer2);
+}
+
+JPH::ObjectLayer JoltLayers::_allocate_object_layer(uint64_t p_collision) {
+	const JPH::ObjectLayer new_object_layer = next_object_layer++;
+
+	collisions_by_layer.resize(new_object_layer + 1);
+	collisions_by_layer[new_object_layer] = p_collision;
+
+	layers_by_collision[p_collision] = new_object_layer;
+
+	return new_object_layer;
+}
+
+JoltLayers::JoltLayers() {
+	_allocate_object_layer(0);
+}
+
+JPH::ObjectLayer JoltLayers::to_object_layer(JPH::BroadPhaseLayer p_broad_phase_layer, uint32_t p_collision_layer, uint32_t p_collision_mask) {
+	const uint64_t collision = encode_collision(p_collision_layer, p_collision_mask);
+
+	JPH::ObjectLayer object_layer = 0;
+
+	HashMap<uint64_t, JPH::ObjectLayer>::Iterator iter = layers_by_collision.find(collision);
+	if (iter != layers_by_collision.end()) {
+		object_layer = iter->value;
+	} else {
+		constexpr uint16_t object_layer_count = 1U << 13U;
+
+		ERR_FAIL_COND_V_MSG(next_object_layer == object_layer_count, 0,
+				vformat("Maximum number of object layers (%d) reached. "
+						"This means there are %d combinations of collision layers and masks."
+						"This should not happen under normal circumstances. Consider reporting this.",
+						object_layer_count, object_layer_count));
+
+		object_layer = _allocate_object_layer(collision);
+	}
+
+	return encode_layers(p_broad_phase_layer, object_layer);
+}
+
+void JoltLayers::from_object_layer(JPH::ObjectLayer p_encoded_layer, JPH::BroadPhaseLayer &r_broad_phase_layer, uint32_t &r_collision_layer, uint32_t &r_collision_mask) const {
+	JPH::ObjectLayer object_layer = 0;
+	decode_layers(p_encoded_layer, r_broad_phase_layer, object_layer);
+
+	const uint64_t collision = collisions_by_layer[object_layer];
+	decode_collision(collision, r_collision_layer, r_collision_mask);
+}

modules/jolt_physics/spaces/jolt_layers.h

@@ -0,0 +1,71 @@
+/**************************************************************************/
+/*  jolt_layers.h                                                         */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef JOLT_LAYERS_H
+#define JOLT_LAYERS_H
+
+#include "core/templates/hash_map.h"
+#include "core/templates/local_vector.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h"
+#include "Jolt/Physics/Collision/ObjectLayer.h"
+
+#include <stdint.h>
+
+class JoltLayers final
+		: public JPH::BroadPhaseLayerInterface,
+		  public JPH::ObjectLayerPairFilter,
+		  public JPH::ObjectVsBroadPhaseLayerFilter {
+	LocalVector<uint64_t> collisions_by_layer;
+	HashMap<uint64_t, JPH::ObjectLayer> layers_by_collision;
+	JPH::ObjectLayer next_object_layer = 0;
+
+	virtual uint32_t GetNumBroadPhaseLayers() const override;
+	virtual JPH::BroadPhaseLayer GetBroadPhaseLayer(JPH::ObjectLayer p_layer) const override;
+
+#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
+	virtual const char *GetBroadPhaseLayerName(JPH::BroadPhaseLayer p_layer) const override;
+#endif
+
+	virtual bool ShouldCollide(JPH::ObjectLayer p_encoded_layer1, JPH::ObjectLayer p_encoded_layer2) const override;
+	virtual bool ShouldCollide(JPH::ObjectLayer p_encoded_layer1, JPH::BroadPhaseLayer p_broad_phase_layer2) const override;
+
+	JPH::ObjectLayer _allocate_object_layer(uint64_t p_collision);
+
+public:
+	JoltLayers();
+
+	JPH::ObjectLayer to_object_layer(JPH::BroadPhaseLayer p_broad_phase_layer, uint32_t p_collision_layer, uint32_t p_collision_mask);
+	void from_object_layer(JPH::ObjectLayer p_encoded_layer, JPH::BroadPhaseLayer &r_broad_phase_layer, uint32_t &r_collision_layer, uint32_t &r_collision_mask) const;
+};
+
+#endif // JOLT_LAYERS_H

modules/jolt_physics/spaces/jolt_motion_filter_3d.cpp

@@ -0,0 +1,114 @@
+/**************************************************************************/
+/*  jolt_motion_filter_3d.cpp                                             */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "jolt_motion_filter_3d.h"
+
+#include "../objects/jolt_body_3d.h"
+#include "../objects/jolt_object_3d.h"
+#include "../shapes/jolt_custom_motion_shape.h"
+#include "../shapes/jolt_custom_ray_shape.h"
+#include "../shapes/jolt_custom_shape_type.h"
+#include "../shapes/jolt_shape_3d.h"
+#include "jolt_broad_phase_layer.h"
+#include "jolt_space_3d.h"
+
+JoltMotionFilter3D::JoltMotionFilter3D(const JoltBody3D &p_body, const HashSet<RID> &p_excluded_bodies, const HashSet<ObjectID> &p_excluded_objects, bool p_collide_separation_ray) :
+		body_self(p_body),
+		space(*body_self.get_space()),
+		excluded_bodies(p_excluded_bodies),
+		excluded_objects(p_excluded_objects),
+		collide_separation_ray(p_collide_separation_ray) {
+}
+
+bool JoltMotionFilter3D::ShouldCollide(JPH::BroadPhaseLayer p_broad_phase_layer) const {
+	const JPH::BroadPhaseLayer::Type broad_phase_layer = (JPH::BroadPhaseLayer::Type)p_broad_phase_layer;
+
+	switch (broad_phase_layer) {
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::BODY_STATIC:
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::BODY_STATIC_BIG:
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::BODY_DYNAMIC: {
+			return true;
+		} break;
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::AREA_DETECTABLE:
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::AREA_UNDETECTABLE: {
+			return false;
+		} break;
+		default: {
+			ERR_FAIL_V_MSG(false, vformat("Unhandled broad phase layer: '%d'. This should not happen. Please report this.", broad_phase_layer));
+		}
+	}
+}
+
+bool JoltMotionFilter3D::ShouldCollide(JPH::ObjectLayer p_object_layer) const {
+	JPH::BroadPhaseLayer object_broad_phase_layer = JoltBroadPhaseLayer::BODY_STATIC;
+	uint32_t object_collision_layer = 0;
+	uint32_t object_collision_mask = 0;
+
+	space.map_from_object_layer(p_object_layer, object_broad_phase_layer, object_collision_layer, object_collision_mask);
+
+	return (body_self.get_collision_mask() & object_collision_layer) != 0;
+}
+
+bool JoltMotionFilter3D::ShouldCollide(const JPH::BodyID &p_jolt_id) const {
+	return p_jolt_id != body_self.get_jolt_id();
+}
+
+bool JoltMotionFilter3D::ShouldCollideLocked(const JPH::Body &p_jolt_body) const {
+	if (p_jolt_body.IsSoftBody()) {
+		return false;
+	}
+
+	const JoltObject3D *object = reinterpret_cast<const JoltObject3D *>(p_jolt_body.GetUserData());
+	if (excluded_objects.has(object->get_instance_id()) || excluded_bodies.has(object->get_rid())) {
+		return false;
+	}
+
+	const JoltReadableBody3D jolt_body_self = space.read_body(body_self);
+	return jolt_body_self->GetCollisionGroup().CanCollide(p_jolt_body.GetCollisionGroup());
+}
+
+bool JoltMotionFilter3D::ShouldCollide(const JPH::Shape *p_jolt_shape, const JPH::SubShapeID &p_jolt_shape_id) const {
+	return true;
+}
+
+bool JoltMotionFilter3D::ShouldCollide(const JPH::Shape *p_jolt_shape_self, const JPH::SubShapeID &p_jolt_shape_id_self, const JPH::Shape *p_jolt_shape_other, const JPH::SubShapeID &p_jolt_shape_id_other) const {
+	if (collide_separation_ray) {
+		return true;
+	}
+
+	const JoltCustomMotionShape *motion_shape = static_cast<const JoltCustomMotionShape *>(p_jolt_shape_self);
+	const JPH::ConvexShape &actual_shape_self = motion_shape->get_inner_shape();
+	if (actual_shape_self.GetSubType() == JoltCustomShapeSubType::RAY) {
+		// When `slide_on_slope` is enabled the ray shape acts as a regular shape.
+		return static_cast<const JoltCustomRayShape &>(actual_shape_self).slide_on_slope;
+	}
+
+	return true;
+}

modules/jolt_physics/spaces/jolt_motion_filter_3d.h

@@ -0,0 +1,72 @@
+/**************************************************************************/
+/*  jolt_motion_filter_3d.h                                               */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef JOLT_MOTION_FILTER_3D_H
+#define JOLT_MOTION_FILTER_3D_H
+
+#include "core/object/object_id.h"
+#include "core/templates/hash_set.h"
+#include "core/templates/rid.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Body/Body.h"
+#include "Jolt/Physics/Body/BodyFilter.h"
+#include "Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h"
+#include "Jolt/Physics/Collision/ObjectLayer.h"
+#include "Jolt/Physics/Collision/ShapeFilter.h"
+
+class JoltBody3D;
+class JoltPhysicsServer3D;
+class JoltSpace3D;
+
+class JoltMotionFilter3D final
+		: public JPH::BroadPhaseLayerFilter,
+		  public JPH::ObjectLayerFilter,
+		  public JPH::BodyFilter,
+		  public JPH::ShapeFilter {
+	const JoltBody3D &body_self;
+	const JoltSpace3D &space;
+	const HashSet<RID> &excluded_bodies;
+	const HashSet<ObjectID> &excluded_objects;
+	bool collide_separation_ray = false;
+
+public:
+	explicit JoltMotionFilter3D(const JoltBody3D &p_body, const HashSet<RID> &p_excluded_bodies, const HashSet<ObjectID> &p_excluded_objects, bool p_collide_separation_ray = true);
+
+	virtual bool ShouldCollide(JPH::BroadPhaseLayer p_broad_phase_layer) const override;
+	virtual bool ShouldCollide(JPH::ObjectLayer p_object_layer) const override;
+	virtual bool ShouldCollide(const JPH::BodyID &p_jolt_id) const override;
+	virtual bool ShouldCollideLocked(const JPH::Body &p_jolt_body) const override;
+	virtual bool ShouldCollide(const JPH::Shape *p_jolt_shape, const JPH::SubShapeID &p_jolt_shape_id) const override;
+	virtual bool ShouldCollide(const JPH::Shape *p_jolt_shape_self, const JPH::SubShapeID &p_jolt_shape_id_self, const JPH::Shape *p_jolt_shape_other, const JPH::SubShapeID &p_jolt_shape_id_other) const override;
+};
+
+#endif // JOLT_MOTION_FILTER_3D_H

modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.cpp

@@ -0,0 +1,936 @@
+/**************************************************************************/
+/*  jolt_physics_direct_space_state_3d.cpp                                */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "jolt_physics_direct_space_state_3d.h"
+
+#include "../jolt_physics_server_3d.h"
+#include "../jolt_project_settings.h"
+#include "../misc/jolt_type_conversions.h"
+#include "../objects/jolt_area_3d.h"
+#include "../objects/jolt_body_3d.h"
+#include "../objects/jolt_object_3d.h"
+#include "../shapes/jolt_custom_motion_shape.h"
+#include "../shapes/jolt_shape_3d.h"
+#include "jolt_motion_filter_3d.h"
+#include "jolt_query_collectors.h"
+#include "jolt_query_filter_3d.h"
+#include "jolt_space_3d.h"
+
+#include "Jolt/Geometry/GJKClosestPoint.h"
+#include "Jolt/Physics/Body/Body.h"
+#include "Jolt/Physics/Body/BodyFilter.h"
+#include "Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h"
+#include "Jolt/Physics/Collision/CastResult.h"
+#include "Jolt/Physics/Collision/CollidePointResult.h"
+#include "Jolt/Physics/Collision/NarrowPhaseQuery.h"
+#include "Jolt/Physics/Collision/RayCast.h"
+#include "Jolt/Physics/Collision/Shape/MeshShape.h"
+#include "Jolt/Physics/PhysicsSystem.h"
+
+bool JoltPhysicsDirectSpaceState3D::_cast_motion_impl(const JPH::Shape &p_jolt_shape, const Transform3D &p_transform_com, const Vector3 &p_scale, const Vector3 &p_motion, bool p_use_edge_removal, bool p_ignore_overlaps, const JPH::CollideShapeSettings &p_settings, const JPH::BroadPhaseLayerFilter &p_broad_phase_layer_filter, const JPH::ObjectLayerFilter &p_object_layer_filter, const JPH::BodyFilter &p_body_filter, const JPH::ShapeFilter &p_shape_filter, real_t &r_closest_safe, real_t &r_closest_unsafe) const {
+	r_closest_safe = 1.0f;
+	r_closest_unsafe = 1.0f;
+
+	ERR_FAIL_COND_V_MSG(p_jolt_shape.GetType() != JPH::EShapeType::Convex, false, "Shape-casting with non-convex shapes is not supported.");
+
+	const float motion_length = (float)p_motion.length();
+
+	if (p_ignore_overlaps && motion_length == 0.0f) {
+		return false;
+	}
+
+	const JPH::RMat44 transform_com = to_jolt_r(p_transform_com);
+	const JPH::Vec3 scale = to_jolt(p_scale);
+	const JPH::Vec3 motion = to_jolt(p_motion);
+	const JPH::Vec3 motion_local = transform_com.Multiply3x3Transposed(motion);
+
+	JPH::AABox aabb = p_jolt_shape.GetWorldSpaceBounds(transform_com, scale);
+	JPH::AABox aabb_translated = aabb;
+	aabb_translated.Translate(motion);
+	aabb.Encapsulate(aabb_translated);
+
+	JoltQueryCollectorAnyMulti<JPH::CollideShapeBodyCollector, 2048> aabb_collector;
+	space->get_broad_phase_query().CollideAABox(aabb, aabb_collector, p_broad_phase_layer_filter, p_object_layer_filter);
+
+	if (!aabb_collector.had_hit()) {
+		return false;
+	}
+
+	const JPH::RVec3 base_offset = transform_com.GetTranslation();
+
+	JoltCustomMotionShape motion_shape(static_cast<const JPH::ConvexShape &>(p_jolt_shape));
+
+	auto collides = [&](const JPH::Body &p_other_body, float p_fraction) {
+		motion_shape.set_motion(motion_local * p_fraction);
+
+		const JPH::TransformedShape other_shape = p_other_body.GetTransformedShape();
+
+		JoltQueryCollectorAny<JPH::CollideShapeCollector> collector;
+
+		if (p_use_edge_removal) {
+			JPH::CollideShapeSettings eier_settings = p_settings;
+			eier_settings.mActiveEdgeMode = JPH::EActiveEdgeMode::CollideWithAll;
+			eier_settings.mCollectFacesMode = JPH::ECollectFacesMode::CollectFaces;
+
+			JPH::InternalEdgeRemovingCollector eier_collector(collector);
+			other_shape.CollideShape(&motion_shape, scale, transform_com, eier_settings, base_offset, eier_collector, p_shape_filter);
+			eier_collector.Flush();
+		} else {
+			other_shape.CollideShape(&motion_shape, scale, transform_com, p_settings, base_offset, collector, p_shape_filter);
+		}
+
+		return collector.had_hit();
+	};
+
+	// Figure out the number of steps we need in our binary search in order to achieve millimeter precision, within reason.
+	const int step_count = CLAMP(int(logf(1000.0f * motion_length) / (float)Math_LN2), 4, 16);
+
+	bool collided = false;
+
+	for (int i = 0; i < aabb_collector.get_hit_count(); ++i) {
+		const JPH::BodyID other_jolt_id = aabb_collector.get_hit(i);
+		if (!p_body_filter.ShouldCollide(other_jolt_id)) {
+			continue;
+		}
+
+		const JoltReadableBody3D other_jolt_body = space->read_body(other_jolt_id);
+		if (!p_body_filter.ShouldCollideLocked(*other_jolt_body)) {
+			continue;
+		}
+
+		if (!collides(*other_jolt_body, 1.0f)) {
+			continue;
+		}
+
+		if (p_ignore_overlaps && collides(*other_jolt_body, 0.0f)) {
+			continue;
+		}
+
+		float lo = 0.0f;
+		float hi = 1.0f;
+		float coeff = 0.5f;
+
+		for (int j = 0; j < step_count; ++j) {
+			const float fraction = lo + (hi - lo) * coeff;
+
+			if (collides(*other_jolt_body, fraction)) {
+				collided = true;
+
+				hi = fraction;
+
+				if (j == 0 || lo > 0.0f) {
+					coeff = 0.5f;
+				} else {
+					coeff = 0.25f;
+				}
+			} else {
+				lo = fraction;
+
+				if (j == 0 || hi < 1.0f) {
+					coeff = 0.5f;
+				} else {
+					coeff = 0.75f;
+				}
+			}
+		}
+
+		if (lo < r_closest_safe) {
+			r_closest_safe = lo;
+			r_closest_unsafe = hi;
+		}
+	}
+
+	return collided;
+}
+
+bool JoltPhysicsDirectSpaceState3D::_body_motion_recover(const JoltBody3D &p_body, const Transform3D &p_transform, float p_margin, const HashSet<RID> &p_excluded_bodies, const HashSet<ObjectID> &p_excluded_objects, Vector3 &r_recovery) const {
+	const int recovery_iterations = JoltProjectSettings::get_motion_query_recovery_iterations();
+	const float recovery_amount = JoltProjectSettings::get_motion_query_recovery_amount();
+
+	const JPH::Shape *jolt_shape = p_body.get_jolt_shape();
+
+	const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
+	Transform3D transform_com = p_transform.translated_local(com_scaled);
+
+	JPH::CollideShapeSettings settings;
+	settings.mMaxSeparationDistance = p_margin;
+
+	const Vector3 &base_offset = transform_com.origin;
+
+	const JoltMotionFilter3D motion_filter(p_body, p_excluded_bodies, p_excluded_objects);
+	JoltQueryCollectorAnyMulti<JPH::CollideShapeCollector, 32> collector;
+
+	bool recovered = false;
+
+	for (int i = 0; i < recovery_iterations; ++i) {
+		collector.reset();
+
+		_collide_shape_kinematics(jolt_shape, JPH::Vec3::sReplicate(1.0f), to_jolt_r(transform_com), settings, to_jolt_r(base_offset), collector, motion_filter, motion_filter, motion_filter, motion_filter);
+
+		if (!collector.had_hit()) {
+			break;
+		}
+
+		const int hit_count = collector.get_hit_count();
+
+		float combined_priority = 0.0;
+
+		for (int j = 0; j < hit_count; j++) {
+			const JPH::CollideShapeResult &hit = collector.get_hit(j);
+
+			const JoltReadableBody3D other_jolt_body = space->read_body(hit.mBodyID2);
+			const JoltBody3D *other_body = other_jolt_body.as_body();
+			ERR_CONTINUE(other_body == nullptr);
+
+			combined_priority += other_body->get_collision_priority();
+		}
+
+		const float average_priority = MAX(combined_priority / (float)hit_count, (float)CMP_EPSILON);
+
+		recovered = true;
+
+		Vector3 recovery;
+
+		for (int j = 0; j < hit_count; ++j) {
+			const JPH::CollideShapeResult &hit = collector.get_hit(j);
+
+			const Vector3 penetration_axis = to_godot(hit.mPenetrationAxis.Normalized());
+			const Vector3 margin_offset = penetration_axis * p_margin;
+
+			const Vector3 point_on_1 = base_offset + to_godot(hit.mContactPointOn1) + margin_offset;
+			const Vector3 point_on_2 = base_offset + to_godot(hit.mContactPointOn2);
+
+			const real_t distance_to_1 = penetration_axis.dot(point_on_1 + recovery);
+			const real_t distance_to_2 = penetration_axis.dot(point_on_2);
+
+			const float penetration_depth = float(distance_to_1 - distance_to_2);
+
+			if (penetration_depth <= 0.0f) {
+				continue;
+			}
+
+			const JoltReadableBody3D other_jolt_body = space->read_body(hit.mBodyID2);
+			const JoltBody3D *other_body = other_jolt_body.as_body();
+			ERR_CONTINUE(other_body == nullptr);
+
+			const float recovery_distance = penetration_depth * recovery_amount;
+			const float other_priority = other_body->get_collision_priority();
+			const float other_priority_normalized = other_priority / average_priority;
+			const float scaled_recovery_distance = recovery_distance * other_priority_normalized;
+
+			recovery -= penetration_axis * scaled_recovery_distance;
+		}
+
+		if (recovery == Vector3()) {
+			break;
+		}
+
+		r_recovery += recovery;
+		transform_com.origin += recovery;
+	}
+
+	return recovered;
+}
+
+bool JoltPhysicsDirectSpaceState3D::_body_motion_cast(const JoltBody3D &p_body, const Transform3D &p_transform, const Vector3 &p_scale, const Vector3 &p_motion, bool p_collide_separation_ray, const HashSet<RID> &p_excluded_bodies, const HashSet<ObjectID> &p_excluded_objects, real_t &r_safe_fraction, real_t &r_unsafe_fraction) const {
+	const Transform3D body_transform = p_transform.scaled_local(p_scale);
+
+	const JPH::CollideShapeSettings settings;
+	const JoltMotionFilter3D motion_filter(p_body, p_excluded_bodies, p_excluded_objects, p_collide_separation_ray);
+
+	bool collided = false;
+
+	for (int i = 0; i < p_body.get_shape_count(); ++i) {
+		if (p_body.is_shape_disabled(i)) {
+			continue;
+		}
+
+		JoltShape3D *shape = p_body.get_shape(i);
+
+		if (!shape->is_convex()) {
+			continue;
+		}
+
+		const JPH::ShapeRefC jolt_shape = shape->try_build();
+		if (unlikely(jolt_shape == nullptr)) {
+			return false;
+		}
+
+		const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
+		const Transform3D transform_local = p_body.get_shape_transform_scaled(i);
+		const Transform3D transform_com_local = transform_local.translated_local(com_scaled);
+		Transform3D transform_com = body_transform * transform_com_local;
+
+		Vector3 scale = transform_com.basis.get_scale();
+		JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "body_test_motion was passed an invalid transform along with body '%s'. This results in invalid scaling for shape at index %d.");
+
+		transform_com.basis.orthonormalize();
+
+		real_t shape_safe_fraction = 1.0;
+		real_t shape_unsafe_fraction = 1.0;
+
+		collided |= _cast_motion_impl(*jolt_shape, transform_com, scale, p_motion, JoltProjectSettings::use_enhanced_internal_edge_removal_for_motion_queries(), false, settings, motion_filter, motion_filter, motion_filter, motion_filter, shape_safe_fraction, shape_unsafe_fraction);
+
+		r_safe_fraction = MIN(r_safe_fraction, shape_safe_fraction);
+		r_unsafe_fraction = MIN(r_unsafe_fraction, shape_unsafe_fraction);
+	}
+
+	return collided;
+}
+
+bool JoltPhysicsDirectSpaceState3D::_body_motion_collide(const JoltBody3D &p_body, const Transform3D &p_transform, const Vector3 &p_motion, float p_margin, int p_max_collisions, const HashSet<RID> &p_excluded_bodies, const HashSet<ObjectID> &p_excluded_objects, PhysicsServer3D::MotionResult *p_result) const {
+	if (p_max_collisions == 0) {
+		return false;
+	}
+
+	const JPH::Shape *jolt_shape = p_body.get_jolt_shape();
+
+	const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
+	const Transform3D transform_com = p_transform.translated_local(com_scaled);
+
+	JPH::CollideShapeSettings settings;
+	settings.mCollectFacesMode = JPH::ECollectFacesMode::CollectFaces;
+	settings.mMaxSeparationDistance = p_margin;
+
+	const Vector3 &base_offset = transform_com.origin;
+
+	const JoltMotionFilter3D motion_filter(p_body, p_excluded_bodies, p_excluded_objects);
+	JoltQueryCollectorClosestMulti<JPH::CollideShapeCollector, 32> collector(p_max_collisions);
+	_collide_shape_kinematics(jolt_shape, JPH::Vec3::sReplicate(1.0f), to_jolt_r(transform_com), settings, to_jolt_r(base_offset), collector, motion_filter, motion_filter, motion_filter, motion_filter);
+
+	if (!collector.had_hit() || p_result == nullptr) {
+		return collector.had_hit();
+	}
+
+	int count = 0;
+
+	for (int i = 0; i < collector.get_hit_count(); ++i) {
+		const JPH::CollideShapeResult &hit = collector.get_hit(i);
+
+		const float penetration_depth = hit.mPenetrationDepth + p_margin;
+
+		if (penetration_depth <= 0.0f) {
+			continue;
+		}
+
+		const Vector3 normal = to_godot(-hit.mPenetrationAxis.Normalized());
+
+		if (p_motion.length_squared() > 0) {
+			const Vector3 direction = p_motion.normalized();
+
+			if (direction.dot(normal) >= -CMP_EPSILON) {
+				continue;
+			}
+		}
+
+		JPH::ContactPoints contact_points1;
+		JPH::ContactPoints contact_points2;
+
+		if (p_max_collisions > 1) {
+			_generate_manifold(hit, contact_points1, contact_points2 JPH_IF_DEBUG_RENDERER(, to_jolt_r(base_offset)));
+		} else {
+			contact_points2.push_back(hit.mContactPointOn2);
+		}
+
+		const JoltReadableBody3D collider_jolt_body = space->read_body(hit.mBodyID2);
+		const JoltShapedObject3D *collider = collider_jolt_body.as_shaped();
+		ERR_FAIL_NULL_V(collider, false);
+
+		const int local_shape = p_body.find_shape_index(hit.mSubShapeID1);
+		ERR_FAIL_COND_V(local_shape == -1, false);
+
+		const int collider_shape = collider->find_shape_index(hit.mSubShapeID2);
+		ERR_FAIL_COND_V(collider_shape == -1, false);
+
+		for (JPH::Vec3 contact_point : contact_points2) {
+			const Vector3 position = base_offset + to_godot(contact_point);
+
+			PhysicsServer3D::MotionCollision &collision = p_result->collisions[count++];
+
+			collision.position = position;
+			collision.normal = normal;
+			collision.collider_velocity = collider->get_velocity_at_position(position);
+			collision.collider_angular_velocity = collider->get_angular_velocity();
+			collision.depth = penetration_depth;
+			collision.local_shape = local_shape;
+			collision.collider_id = collider->get_instance_id();
+			collision.collider = collider->get_rid();
+			collision.collider_shape = collider_shape;
+
+			if (count == p_max_collisions) {
+				break;
+			}
+		}
+
+		if (count == p_max_collisions) {
+			break;
+		}
+	}
+
+	p_result->collision_count = count;
+
+	return count > 0;
+}
+
+int JoltPhysicsDirectSpaceState3D::_try_get_face_index(const JPH::Body &p_body, const JPH::SubShapeID &p_sub_shape_id) {
+	if (!JoltProjectSettings::enable_ray_cast_face_index()) {
+		return -1;
+	}
+
+	const JPH::Shape *root_shape = p_body.GetShape();
+	JPH::SubShapeID sub_shape_id_remainder;
+	const JPH::Shape *leaf_shape = root_shape->GetLeafShape(p_sub_shape_id, sub_shape_id_remainder);
+
+	if (leaf_shape->GetType() != JPH::EShapeType::Mesh) {
+		return -1;
+	}
+
+	const JPH::MeshShape *mesh_shape = static_cast<const JPH::MeshShape *>(leaf_shape);
+	return (int)mesh_shape->GetTriangleUserData(sub_shape_id_remainder);
+}
+
+void JoltPhysicsDirectSpaceState3D::_generate_manifold(const JPH::CollideShapeResult &p_hit, JPH::ContactPoints &r_contact_points1, JPH::ContactPoints &r_contact_points2 JPH_IF_DEBUG_RENDERER(, JPH::RVec3Arg p_center_of_mass)) const {
+	const JPH::PhysicsSystem &physics_system = space->get_physics_system();
+	const JPH::PhysicsSettings &physics_settings = physics_system.GetPhysicsSettings();
+	const JPH::Vec3 penetration_axis = p_hit.mPenetrationAxis.Normalized();
+
+	JPH::ManifoldBetweenTwoFaces(p_hit.mContactPointOn1, p_hit.mContactPointOn2, penetration_axis, physics_settings.mManifoldToleranceSq, p_hit.mShape1Face, p_hit.mShape2Face, r_contact_points1, r_contact_points2 JPH_IF_DEBUG_RENDERER(, p_center_of_mass));
+
+	if (r_contact_points1.size() > 4) {
+		JPH::PruneContactPoints(penetration_axis, r_contact_points1, r_contact_points2 JPH_IF_DEBUG_RENDERER(, p_center_of_mass));
+	}
+}
+
+void JoltPhysicsDirectSpaceState3D::_collide_shape_queries(
+		const JPH::Shape *p_shape,
+		JPH::Vec3Arg p_scale,
+		JPH::RMat44Arg p_transform_com,
+		const JPH::CollideShapeSettings &p_settings,
+		JPH::RVec3Arg p_base_offset,
+		JPH::CollideShapeCollector &p_collector,
+		const JPH::BroadPhaseLayerFilter &p_broad_phase_layer_filter,
+		const JPH::ObjectLayerFilter &p_object_layer_filter,
+		const JPH::BodyFilter &p_body_filter,
+		const JPH::ShapeFilter &p_shape_filter) const {
+	if (JoltProjectSettings::use_enhanced_internal_edge_removal_for_queries()) {
+		space->get_narrow_phase_query().CollideShapeWithInternalEdgeRemoval(p_shape, p_scale, p_transform_com, p_settings, p_base_offset, p_collector, p_broad_phase_layer_filter, p_object_layer_filter, p_body_filter, p_shape_filter);
+	} else {
+		space->get_narrow_phase_query().CollideShape(p_shape, p_scale, p_transform_com, p_settings, p_base_offset, p_collector, p_broad_phase_layer_filter, p_object_layer_filter, p_body_filter, p_shape_filter);
+	}
+}
+
+void JoltPhysicsDirectSpaceState3D::_collide_shape_kinematics(
+		const JPH::Shape *p_shape,
+		JPH::Vec3Arg p_scale,
+		JPH::RMat44Arg p_transform_com,
+		const JPH::CollideShapeSettings &p_settings,
+		JPH::RVec3Arg p_base_offset,
+		JPH::CollideShapeCollector &p_collector,
+		const JPH::BroadPhaseLayerFilter &p_broad_phase_layer_filter,
+		const JPH::ObjectLayerFilter &p_object_layer_filter,
+		const JPH::BodyFilter &p_body_filter,
+		const JPH::ShapeFilter &p_shape_filter) const {
+	if (JoltProjectSettings::use_enhanced_internal_edge_removal_for_motion_queries()) {
+		space->get_narrow_phase_query().CollideShapeWithInternalEdgeRemoval(p_shape, p_scale, p_transform_com, p_settings, p_base_offset, p_collector, p_broad_phase_layer_filter, p_object_layer_filter, p_body_filter, p_shape_filter);
+	} else {
+		space->get_narrow_phase_query().CollideShape(p_shape, p_scale, p_transform_com, p_settings, p_base_offset, p_collector, p_broad_phase_layer_filter, p_object_layer_filter, p_body_filter, p_shape_filter);
+	}
+}
+
+JoltPhysicsDirectSpaceState3D::JoltPhysicsDirectSpaceState3D(JoltSpace3D *p_space) :
+		space(p_space) {
+}
+
+bool JoltPhysicsDirectSpaceState3D::intersect_ray(const RayParameters &p_parameters, RayResult &r_result) {
+	ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "intersect_ray must not be called while the physics space is being stepped.");
+
+	space->try_optimize();
+
+	const JoltQueryFilter3D query_filter(*this, p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas, p_parameters.exclude, p_parameters.pick_ray);
+
+	const JPH::RVec3 from = to_jolt_r(p_parameters.from);
+	const JPH::RVec3 to = to_jolt_r(p_parameters.to);
+	const JPH::Vec3 vector = JPH::Vec3(to - from);
+	const JPH::RRayCast ray(from, vector);
+
+	const JPH::EBackFaceMode back_face_mode = p_parameters.hit_back_faces ? JPH::EBackFaceMode::CollideWithBackFaces : JPH::EBackFaceMode::IgnoreBackFaces;
+
+	JPH::RayCastSettings settings;
+	settings.mTreatConvexAsSolid = p_parameters.hit_from_inside;
+	settings.mBackFaceModeTriangles = back_face_mode;
+
+	JoltQueryCollectorClosest<JPH::CastRayCollector> collector;
+	space->get_narrow_phase_query().CastRay(ray, settings, collector, query_filter, query_filter, query_filter);
+
+	if (!collector.had_hit()) {
+		return false;
+	}
+
+	const JPH::RayCastResult &hit = collector.get_hit();
+
+	const JPH::BodyID &body_id = hit.mBodyID;
+	const JPH::SubShapeID &sub_shape_id = hit.mSubShapeID2;
+
+	const JoltReadableBody3D body = space->read_body(body_id);
+	const JoltObject3D *object = body.as_object();
+	ERR_FAIL_NULL_V(object, false);
+
+	const JPH::RVec3 position = ray.GetPointOnRay(hit.mFraction);
+
+	JPH::Vec3 normal = JPH::Vec3::sZero();
+
+	if (!p_parameters.hit_from_inside || hit.mFraction > 0.0f) {
+		normal = body->GetWorldSpaceSurfaceNormal(sub_shape_id, position);
+
+		// If we got a back-face normal we need to flip it.
+		if (normal.Dot(vector) > 0) {
+			normal = -normal;
+		}
+	}
+
+	r_result.position = to_godot(position);
+	r_result.normal = to_godot(normal);
+	r_result.rid = object->get_rid();
+	r_result.collider_id = object->get_instance_id();
+	r_result.collider = object->get_instance();
+	r_result.shape = 0;
+
+	if (const JoltShapedObject3D *shaped_object = object->as_shaped()) {
+		const int shape_index = shaped_object->find_shape_index(sub_shape_id);
+		ERR_FAIL_COND_V(shape_index == -1, false);
+		r_result.shape = shape_index;
+		r_result.face_index = _try_get_face_index(*body, sub_shape_id);
+	}
+
+	return true;
+}
+
+int JoltPhysicsDirectSpaceState3D::intersect_point(const PointParameters &p_parameters, ShapeResult *r_results, int p_result_max) {
+	ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "intersect_point must not be called while the physics space is being stepped.");
+
+	if (p_result_max == 0) {
+		return 0;
+	}
+
+	space->try_optimize();
+
+	const JoltQueryFilter3D query_filter(*this, p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas, p_parameters.exclude);
+	JoltQueryCollectorAnyMulti<JPH::CollidePointCollector, 32> collector(p_result_max);
+	space->get_narrow_phase_query().CollidePoint(to_jolt_r(p_parameters.position), collector, query_filter, query_filter, query_filter);
+
+	const int hit_count = collector.get_hit_count();
+
+	for (int i = 0; i < hit_count; ++i) {
+		const JPH::CollidePointResult &hit = collector.get_hit(i);
+
+		const JoltReadableBody3D body = space->read_body(hit.mBodyID);
+		const JoltObject3D *object = body.as_object();
+		ERR_FAIL_NULL_V(object, 0);
+
+		ShapeResult &result = *r_results++;
+
+		result.shape = 0;
+
+		if (const JoltShapedObject3D *shaped_object = object->as_shaped()) {
+			const int shape_index = shaped_object->find_shape_index(hit.mSubShapeID2);
+			ERR_FAIL_COND_V(shape_index == -1, 0);
+			result.shape = shape_index;
+		}
+
+		result.rid = object->get_rid();
+		result.collider_id = object->get_instance_id();
+		result.collider = object->get_instance();
+	}
+
+	return hit_count;
+}
+
+int JoltPhysicsDirectSpaceState3D::intersect_shape(const ShapeParameters &p_parameters, ShapeResult *r_results, int p_result_max) {
+	ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "intersect_shape must not be called while the physics space is being stepped.");
+
+	if (p_result_max == 0) {
+		return 0;
+	}
+
+	space->try_optimize();
+
+	JoltShape3D *shape = JoltPhysicsServer3D::get_singleton()->get_shape(p_parameters.shape_rid);
+	ERR_FAIL_NULL_V(shape, 0);
+
+	const JPH::ShapeRefC jolt_shape = shape->try_build();
+	ERR_FAIL_NULL_V(jolt_shape, 0);
+
+	Transform3D transform = p_parameters.transform;
+	JOLT_ENSURE_SCALE_NOT_ZERO(transform, "intersect_shape was passed an invalid transform.");
+
+	Vector3 scale = transform.basis.get_scale();
+	JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "intersect_shape was passed an invalid transform.");
+
+	transform.basis.orthonormalize();
+
+	const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
+	const Transform3D transform_com = transform.translated_local(com_scaled);
+
+	JPH::CollideShapeSettings settings;
+	settings.mMaxSeparationDistance = (float)p_parameters.margin;
+
+	const JoltQueryFilter3D query_filter(*this, p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas, p_parameters.exclude);
+	JoltQueryCollectorAnyMulti<JPH::CollideShapeCollector, 32> collector(p_result_max);
+	_collide_shape_queries(jolt_shape, to_jolt(scale), to_jolt_r(transform_com), settings, to_jolt_r(transform_com.origin), collector, query_filter, query_filter, query_filter);
+
+	const int hit_count = collector.get_hit_count();
+
+	for (int i = 0; i < hit_count; ++i) {
+		const JPH::CollideShapeResult &hit = collector.get_hit(i);
+
+		const JoltReadableBody3D body = space->read_body(hit.mBodyID2);
+		const JoltObject3D *object = body.as_object();
+		ERR_FAIL_NULL_V(object, 0);
+
+		ShapeResult &result = *r_results++;
+
+		result.shape = 0;
+
+		if (const JoltShapedObject3D *shaped_object = object->as_shaped()) {
+			const int shape_index = shaped_object->find_shape_index(hit.mSubShapeID2);
+			ERR_FAIL_COND_V(shape_index == -1, 0);
+			result.shape = shape_index;
+		}
+
+		result.rid = object->get_rid();
+		result.collider_id = object->get_instance_id();
+		result.collider = object->get_instance();
+	}
+
+	return hit_count;
+}
+
+bool JoltPhysicsDirectSpaceState3D::cast_motion(const ShapeParameters &p_parameters, real_t &r_closest_safe, real_t &r_closest_unsafe, ShapeRestInfo *r_info) {
+	ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "cast_motion must not be called while the physics space is being stepped.");
+	ERR_FAIL_COND_V_MSG(r_info != nullptr, false, "Providing rest info as part of cast_motion is not supported when using Jolt Physics.");
+
+	space->try_optimize();
+
+	JoltShape3D *shape = JoltPhysicsServer3D::get_singleton()->get_shape(p_parameters.shape_rid);
+	ERR_FAIL_NULL_V(shape, false);
+
+	const JPH::ShapeRefC jolt_shape = shape->try_build();
+	ERR_FAIL_NULL_V(jolt_shape, false);
+
+	Transform3D transform = p_parameters.transform;
+	JOLT_ENSURE_SCALE_NOT_ZERO(transform, "cast_motion (maybe from ShapeCast3D?) was passed an invalid transform.");
+
+	Vector3 scale = transform.basis.get_scale();
+	JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "cast_motion (maybe from ShapeCast3D?) was passed an invalid transform.");
+
+	transform.basis.orthonormalize();
+
+	const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
+	Transform3D transform_com = transform.translated_local(com_scaled);
+
+	JPH::CollideShapeSettings settings;
+	settings.mMaxSeparationDistance = (float)p_parameters.margin;
+
+	const JoltQueryFilter3D query_filter(*this, p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas, p_parameters.exclude);
+	_cast_motion_impl(*jolt_shape, transform_com, scale, p_parameters.motion, JoltProjectSettings::use_enhanced_internal_edge_removal_for_queries(), true, settings, query_filter, query_filter, query_filter, JPH::ShapeFilter(), r_closest_safe, r_closest_unsafe);
+
+	return true;
+}
+
+bool JoltPhysicsDirectSpaceState3D::collide_shape(const ShapeParameters &p_parameters, Vector3 *r_results, int p_result_max, int &r_result_count) {
+	r_result_count = 0;
+
+	ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "collide_shape must not be called while the physics space is being stepped.");
+
+	if (p_result_max == 0) {
+		return false;
+	}
+
+	space->try_optimize();
+
+	JoltShape3D *shape = JoltPhysicsServer3D::get_singleton()->get_shape(p_parameters.shape_rid);
+	ERR_FAIL_NULL_V(shape, false);
+
+	const JPH::ShapeRefC jolt_shape = shape->try_build();
+	ERR_FAIL_NULL_V(jolt_shape, false);
+
+	Transform3D transform = p_parameters.transform;
+	JOLT_ENSURE_SCALE_NOT_ZERO(transform, "collide_shape was passed an invalid transform.");
+
+	Vector3 scale = transform.basis.get_scale();
+	JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "collide_shape was passed an invalid transform.");
+
+	transform.basis.orthonormalize();
+
+	const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
+	const Transform3D transform_com = transform.translated_local(com_scaled);
+
+	JPH::CollideShapeSettings settings;
+	settings.mCollectFacesMode = JPH::ECollectFacesMode::CollectFaces;
+	settings.mMaxSeparationDistance = (float)p_parameters.margin;
+
+	const Vector3 &base_offset = transform_com.origin;
+
+	const JoltQueryFilter3D query_filter(*this, p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas, p_parameters.exclude);
+	JoltQueryCollectorAnyMulti<JPH::CollideShapeCollector, 32> collector(p_result_max);
+	_collide_shape_queries(jolt_shape, to_jolt(scale), to_jolt_r(transform_com), settings, to_jolt_r(base_offset), collector, query_filter, query_filter, query_filter);
+
+	if (!collector.had_hit()) {
+		return false;
+	}
+
+	const int max_points = p_result_max * 2;
+
+	int point_count = 0;
+
+	for (int i = 0; i < collector.get_hit_count(); ++i) {
+		const JPH::CollideShapeResult &hit = collector.get_hit(i);
+
+		const Vector3 penetration_axis = to_godot(hit.mPenetrationAxis.Normalized());
+		const Vector3 margin_offset = penetration_axis * (float)p_parameters.margin;
+
+		JPH::ContactPoints contact_points1;
+		JPH::ContactPoints contact_points2;
+
+		_generate_manifold(hit, contact_points1, contact_points2 JPH_IF_DEBUG_RENDERER(, to_jolt_r(base_offset)));
+
+		for (JPH::uint j = 0; j < contact_points1.size(); ++j) {
+			r_results[point_count++] = base_offset + to_godot(contact_points1[j]) + margin_offset;
+			r_results[point_count++] = base_offset + to_godot(contact_points2[j]);
+
+			if (point_count >= max_points) {
+				break;
+			}
+		}
+
+		if (point_count >= max_points) {
+			break;
+		}
+	}
+
+	r_result_count = point_count / 2;
+
+	return true;
+}
+
+bool JoltPhysicsDirectSpaceState3D::rest_info(const ShapeParameters &p_parameters, ShapeRestInfo *r_info) {
+	ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "get_rest_info must not be called while the physics space is being stepped.");
+
+	space->try_optimize();
+
+	JoltShape3D *shape = JoltPhysicsServer3D::get_singleton()->get_shape(p_parameters.shape_rid);
+	ERR_FAIL_NULL_V(shape, false);
+
+	const JPH::ShapeRefC jolt_shape = shape->try_build();
+	ERR_FAIL_NULL_V(jolt_shape, false);
+
+	Transform3D transform = p_parameters.transform;
+	JOLT_ENSURE_SCALE_NOT_ZERO(transform, "get_rest_info (maybe from ShapeCast3D?) was passed an invalid transform.");
+
+	Vector3 scale = transform.basis.get_scale();
+	JOLT_ENSURE_SCALE_VALID(jolt_shape, scale, "get_rest_info (maybe from ShapeCast3D?) was passed an invalid transform.");
+
+	transform.basis.orthonormalize();
+
+	const Vector3 com_scaled = to_godot(jolt_shape->GetCenterOfMass());
+	const Transform3D transform_com = transform.translated_local(com_scaled);
+
+	JPH::CollideShapeSettings settings;
+	settings.mMaxSeparationDistance = (float)p_parameters.margin;
+
+	const Vector3 &base_offset = transform_com.origin;
+
+	const JoltQueryFilter3D query_filter(*this, p_parameters.collision_mask, p_parameters.collide_with_bodies, p_parameters.collide_with_areas, p_parameters.exclude);
+	JoltQueryCollectorClosest<JPH::CollideShapeCollector> collector;
+	_collide_shape_queries(jolt_shape, to_jolt(scale), to_jolt_r(transform_com), settings, to_jolt_r(base_offset), collector, query_filter, query_filter, query_filter);
+
+	if (!collector.had_hit()) {
+		return false;
+	}
+
+	const JPH::CollideShapeResult &hit = collector.get_hit();
+	const JoltReadableBody3D body = space->read_body(hit.mBodyID2);
+	const JoltObject3D *object = body.as_object();
+	ERR_FAIL_NULL_V(object, false);
+
+	r_info->shape = 0;
+
+	if (const JoltShapedObject3D *shaped_object = object->as_shaped()) {
+		const int shape_index = shaped_object->find_shape_index(hit.mSubShapeID2);
+		ERR_FAIL_COND_V(shape_index == -1, false);
+		r_info->shape = shape_index;
+	}
+
+	const Vector3 hit_point = base_offset + to_godot(hit.mContactPointOn2);
+
+	r_info->point = hit_point;
+	r_info->normal = to_godot(-hit.mPenetrationAxis.Normalized());
+	r_info->rid = object->get_rid();
+	r_info->collider_id = object->get_instance_id();
+	r_info->shape = 0;
+	r_info->linear_velocity = object->get_velocity_at_position(hit_point);
+
+	return true;
+}
+
+Vector3 JoltPhysicsDirectSpaceState3D::get_closest_point_to_object_volume(RID p_object, Vector3 p_point) const {
+	ERR_FAIL_COND_V_MSG(space->is_stepping(), Vector3(), "get_closest_point_to_object_volume must not be called while the physics space is being stepped.");
+
+	space->try_optimize();
+
+	JoltPhysicsServer3D *physics_server = JoltPhysicsServer3D::get_singleton();
+	JoltObject3D *object = physics_server->get_area(p_object);
+
+	if (object == nullptr) {
+		object = physics_server->get_body(p_object);
+	}
+
+	ERR_FAIL_NULL_V(object, Vector3());
+	ERR_FAIL_COND_V(object->get_space() != space, Vector3());
+
+	const JoltReadableBody3D body = space->read_body(*object);
+	const JPH::TransformedShape root_shape = body->GetTransformedShape();
+
+	JoltQueryCollectorAll<JPH::TransformedShapeCollector, 32> collector;
+	root_shape.CollectTransformedShapes(body->GetWorldSpaceBounds(), collector);
+
+	const JPH::RVec3 point = to_jolt_r(p_point);
+
+	float closest_distance_sq = FLT_MAX;
+	JPH::RVec3 closest_point = JPH::RVec3::sZero();
+
+	bool found_point = false;
+
+	for (int i = 0; i < collector.get_hit_count(); ++i) {
+		const JPH::TransformedShape &shape_transformed = collector.get_hit(i);
+		const JPH::Shape &shape = *shape_transformed.mShape;
+
+		if (shape.GetType() != JPH::EShapeType::Convex) {
+			continue;
+		}
+
+		const JPH::ConvexShape &shape_convex = static_cast<const JPH::ConvexShape &>(shape);
+
+		JPH::GJKClosestPoint gjk;
+
+		JPH::ConvexShape::SupportBuffer shape_support_buffer;
+		const JPH::ConvexShape::Support *shape_support = shape_convex.GetSupportFunction(JPH::ConvexShape::ESupportMode::IncludeConvexRadius, shape_support_buffer, shape_transformed.GetShapeScale());
+
+		const JPH::Quat &shape_rotation = shape_transformed.mShapeRotation;
+		const JPH::RVec3 &shape_pos_com = shape_transformed.mShapePositionCOM;
+		const JPH::RMat44 shape_3x3 = JPH::RMat44::sRotation(shape_rotation);
+		const JPH::Vec3 shape_com_local = shape.GetCenterOfMass();
+		const JPH::Vec3 shape_com = shape_3x3.Multiply3x3(shape_com_local);
+		const JPH::RVec3 shape_pos = shape_pos_com - JPH::RVec3(shape_com);
+		const JPH::RMat44 shape_4x4 = shape_3x3.PostTranslated(shape_pos);
+		const JPH::RMat44 shape_4x4_inv = shape_4x4.InversedRotationTranslation();
+
+		JPH::PointConvexSupport point_support;
+		point_support.mPoint = JPH::Vec3(shape_4x4_inv * point);
+
+		JPH::Vec3 separating_axis = JPH::Vec3::sAxisX();
+		JPH::Vec3 point_on_a = JPH::Vec3::sZero();
+		JPH::Vec3 point_on_b = JPH::Vec3::sZero();
+
+		const float distance_sq = gjk.GetClosestPoints(*shape_support, point_support, JPH::cDefaultCollisionTolerance, FLT_MAX, separating_axis, point_on_a, point_on_b);
+
+		if (distance_sq == 0.0f) {
+			closest_point = point;
+			found_point = true;
+			break;
+		}
+
+		if (distance_sq < closest_distance_sq) {
+			closest_distance_sq = distance_sq;
+			closest_point = shape_4x4 * point_on_a;
+			found_point = true;
+		}
+	}
+
+	if (found_point) {
+		return to_godot(closest_point);
+	} else {
+		return to_godot(body->GetPosition());
+	}
+}
+
+bool JoltPhysicsDirectSpaceState3D::body_test_motion(const JoltBody3D &p_body, const PhysicsServer3D::MotionParameters &p_parameters, PhysicsServer3D::MotionResult *r_result) const {
+	ERR_FAIL_COND_V_MSG(space->is_stepping(), false, "body_test_motion (maybe from move_and_slide?) must not be called while the physics space is being stepped.");
+
+	const float margin = MAX((float)p_parameters.margin, 0.0001f);
+	const int max_collisions = MIN(p_parameters.max_collisions, 32);
+
+	Transform3D transform = p_parameters.from;
+	JOLT_ENSURE_SCALE_NOT_ZERO(transform, vformat("body_test_motion (maybe from move_and_slide?) was passed an invalid transform along with body '%s'.", p_body.to_string()));
+
+	Vector3 scale = transform.basis.get_scale();
+	transform.basis.orthonormalize();
+
+	space->try_optimize();
+
+	Vector3 recovery;
+	const bool recovered = _body_motion_recover(p_body, transform, margin, p_parameters.exclude_bodies, p_parameters.exclude_objects, recovery);
+
+	transform.origin += recovery;
+
+	real_t safe_fraction = 1.0;
+	real_t unsafe_fraction = 1.0;
+
+	const bool hit = _body_motion_cast(p_body, transform, scale, p_parameters.motion, p_parameters.collide_separation_ray, p_parameters.exclude_bodies, p_parameters.exclude_objects, safe_fraction, unsafe_fraction);
+
+	bool collided = false;
+
+	if (hit || (recovered && p_parameters.recovery_as_collision)) {
+		collided = _body_motion_collide(p_body, transform.translated(p_parameters.motion * unsafe_fraction), p_parameters.motion, margin, max_collisions, p_parameters.exclude_bodies, p_parameters.exclude_objects, r_result);
+	}
+
+	if (r_result == nullptr) {
+		return collided;
+	}
+
+	if (collided) {
+		const PhysicsServer3D::MotionCollision &deepest = r_result->collisions[0];
+
+		r_result->travel = recovery + p_parameters.motion * safe_fraction;
+		r_result->remainder = p_parameters.motion - p_parameters.motion * safe_fraction;
+		r_result->collision_depth = deepest.depth;
+		r_result->collision_safe_fraction = safe_fraction;
+		r_result->collision_unsafe_fraction = unsafe_fraction;
+	} else {
+		r_result->travel = recovery + p_parameters.motion;
+		r_result->remainder = Vector3();
+		r_result->collision_depth = 0.0f;
+		r_result->collision_safe_fraction = 1.0f;
+		r_result->collision_unsafe_fraction = 1.0f;
+		r_result->collision_count = 0;
+	}
+
+	return collided;
+}

modules/jolt_physics/spaces/jolt_physics_direct_space_state_3d.h

@@ -0,0 +1,87 @@
+/**************************************************************************/
+/*  jolt_physics_direct_space_state_3d.h                                  */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef JOLT_PHYSICS_DIRECT_SPACE_STATE_3D_H
+#define JOLT_PHYSICS_DIRECT_SPACE_STATE_3D_H
+
+#include "servers/physics_server_3d.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Body/Body.h"
+#include "Jolt/Physics/Body/BodyFilter.h"
+#include "Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h"
+#include "Jolt/Physics/Collision/ContactListener.h"
+#include "Jolt/Physics/Collision/ObjectLayer.h"
+#include "Jolt/Physics/Collision/Shape/Shape.h"
+#include "Jolt/Physics/Collision/ShapeFilter.h"
+
+class JoltBody3D;
+class JoltShape3D;
+class JoltSpace3D;
+
+class JoltPhysicsDirectSpaceState3D final : public PhysicsDirectSpaceState3D {
+	GDCLASS(JoltPhysicsDirectSpaceState3D, PhysicsDirectSpaceState3D)
+
+	JoltSpace3D *space = nullptr;
+
+	static void _bind_methods() {}
+
+	bool _cast_motion_impl(const JPH::Shape &p_jolt_shape, const Transform3D &p_transform_com, const Vector3 &p_scale, const Vector3 &p_motion, bool p_use_edge_removal, bool p_ignore_overlaps, const JPH::CollideShapeSettings &p_settings, const JPH::BroadPhaseLayerFilter &p_broad_phase_layer_filter, const JPH::ObjectLayerFilter &p_object_layer_filter, const JPH::BodyFilter &p_body_filter, const JPH::ShapeFilter &p_shape_filter, real_t &r_closest_safe, real_t &r_closest_unsafe) const;
+
+	bool _body_motion_recover(const JoltBody3D &p_body, const Transform3D &p_transform, float p_margin, const HashSet<RID> &p_excluded_bodies, const HashSet<ObjectID> &p_excluded_objects, Vector3 &r_recovery) const;
+	bool _body_motion_cast(const JoltBody3D &p_body, const Transform3D &p_transform, const Vector3 &p_scale, const Vector3 &p_motion, bool p_collide_separation_ray, const HashSet<RID> &p_excluded_bodies, const HashSet<ObjectID> &p_excluded_objects, real_t &r_safe_fraction, real_t &r_unsafe_fraction) const;
+	bool _body_motion_collide(const JoltBody3D &p_body, const Transform3D &p_transform, const Vector3 &p_motion, float p_margin, int p_max_collisions, const HashSet<RID> &p_excluded_bodies, const HashSet<ObjectID> &p_excluded_objects, PhysicsServer3D::MotionResult *r_result) const;
+
+	int _try_get_face_index(const JPH::Body &p_body, const JPH::SubShapeID &p_sub_shape_id);
+
+	void _generate_manifold(const JPH::CollideShapeResult &p_hit, JPH::ContactPoints &r_contact_points1, JPH::ContactPoints &r_contact_points2 JPH_IF_DEBUG_RENDERER(, JPH::RVec3Arg p_center_of_mass)) const;
+
+	void _collide_shape_queries(const JPH::Shape *p_shape, JPH::Vec3Arg p_scale, JPH::RMat44Arg p_transform_com, const JPH::CollideShapeSettings &p_settings, JPH::RVec3Arg p_base_offset, JPH::CollideShapeCollector &p_collector, const JPH::BroadPhaseLayerFilter &p_broad_phase_layer_filter = JPH::BroadPhaseLayerFilter(), const JPH::ObjectLayerFilter &p_object_layer_filter = JPH::ObjectLayerFilter(), const JPH::BodyFilter &p_body_filter = JPH::BodyFilter(), const JPH::ShapeFilter &p_shape_filter = JPH::ShapeFilter()) const;
+	void _collide_shape_kinematics(const JPH::Shape *p_shape, JPH::Vec3Arg p_scale, JPH::RMat44Arg p_transform_com, const JPH::CollideShapeSettings &p_settings, JPH::RVec3Arg p_base_offset, JPH::CollideShapeCollector &p_collector, const JPH::BroadPhaseLayerFilter &p_broad_phase_layer_filter = JPH::BroadPhaseLayerFilter(), const JPH::ObjectLayerFilter &p_object_layer_filter = JPH::ObjectLayerFilter(), const JPH::BodyFilter &p_body_filter = JPH::BodyFilter(), const JPH::ShapeFilter &p_shape_filter = JPH::ShapeFilter()) const;
+
+public:
+	JoltPhysicsDirectSpaceState3D() = default;
+	explicit JoltPhysicsDirectSpaceState3D(JoltSpace3D *p_space);
+
+	virtual bool intersect_ray(const RayParameters &p_parameters, RayResult &r_result) override;
+	virtual int intersect_point(const PointParameters &p_parameters, ShapeResult *r_results, int p_result_max) override;
+	virtual int intersect_shape(const ShapeParameters &p_parameters, ShapeResult *r_results, int p_result_max) override;
+	virtual bool cast_motion(const ShapeParameters &p_parameters, real_t &r_closest_safe, real_t &r_closest_unsafe, ShapeRestInfo *r_info = nullptr) override;
+	virtual bool collide_shape(const ShapeParameters &p_parameters, Vector3 *r_results, int p_result_max, int &r_result_count) override;
+	virtual bool rest_info(const ShapeParameters &p_parameters, ShapeRestInfo *r_info) override;
+	virtual Vector3 get_closest_point_to_object_volume(RID p_object, Vector3 p_point) const override;
+
+	bool body_test_motion(const JoltBody3D &p_body, const PhysicsServer3D::MotionParameters &p_parameters, PhysicsServer3D::MotionResult *r_result) const;
+
+	JoltSpace3D &get_space() const { return *space; }
+};
+
+#endif // JOLT_PHYSICS_DIRECT_SPACE_STATE_3D_H

modules/jolt_physics/spaces/jolt_query_collectors.h

@@ -0,0 +1,238 @@
+/**************************************************************************/
+/*  jolt_query_collectors.h                                               */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef JOLT_QUERY_COLLECTORS_H
+#define JOLT_QUERY_COLLECTORS_H
+
+#include "../jolt_project_settings.h"
+#include "jolt_space_3d.h"
+
+#include "core/templates/local_vector.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Collision/InternalEdgeRemovingCollector.h"
+#include "Jolt/Physics/Collision/Shape/Shape.h"
+
+template <typename TBase, int TDefaultCapacity>
+class JoltQueryCollectorAll final : public TBase {
+public:
+	typedef typename TBase::ResultType Hit;
+
+private:
+	JPH::Array<Hit> hits;
+
+public:
+	bool had_hit() const {
+		return !hits.is_empty();
+	}
+
+	int get_hit_count() const {
+		return hits.size();
+	}
+
+	const Hit &get_hit(int p_index) const {
+		return hits[p_index];
+	}
+
+	void reset() { Reset(); }
+
+	virtual void Reset() override {
+		TBase::Reset();
+		hits.clear();
+	}
+
+	virtual void AddHit(const Hit &p_hit) override {
+		hits.push_back(p_hit);
+	}
+};
+
+template <typename TBase>
+class JoltQueryCollectorAny final : public TBase {
+public:
+	typedef typename TBase::ResultType Hit;
+
+private:
+	Hit hit;
+	bool valid = false;
+
+public:
+	bool had_hit() const { return valid; }
+
+	const Hit &get_hit() const { return hit; }
+
+	void reset() {
+		Reset();
+	}
+
+	virtual void Reset() override {
+		TBase::Reset();
+		valid = false;
+	}
+
+	virtual void AddHit(const Hit &p_hit) override {
+		hit = p_hit;
+		valid = true;
+
+		TBase::ForceEarlyOut();
+	}
+};
+
+template <typename TBase, int TDefaultCapacity>
+class JoltQueryCollectorAnyMulti final : public TBase {
+public:
+	typedef typename TBase::ResultType Hit;
+
+private:
+	JPH::Array<Hit> hits;
+	int max_hits = 0;
+
+public:
+	explicit JoltQueryCollectorAnyMulti(int p_max_hits = TDefaultCapacity) :
+			max_hits(p_max_hits) {}
+
+	bool had_hit() const {
+		return hits.size() > 0;
+	}
+
+	int get_hit_count() const {
+		return hits.size();
+	}
+
+	const Hit &get_hit(int p_index) const {
+		return hits[p_index];
+	}
+
+	void reset() {
+		Reset();
+	}
+
+	virtual void Reset() override {
+		TBase::Reset();
+		hits.clear();
+	}
+
+	virtual void AddHit(const Hit &p_hit) override {
+		if ((int)hits.size() < max_hits) {
+			hits.push_back(p_hit);
+		}
+
+		if ((int)hits.size() == max_hits) {
+			TBase::ForceEarlyOut();
+		}
+	}
+};
+
+template <typename TBase>
+class JoltQueryCollectorClosest final : public TBase {
+public:
+	typedef typename TBase::ResultType Hit;
+
+private:
+	Hit hit;
+	bool valid = false;
+
+public:
+	bool had_hit() const { return valid; }
+
+	const Hit &get_hit() const { return hit; }
+
+	void reset() {
+		Reset();
+	}
+
+	virtual void Reset() override {
+		TBase::Reset();
+		valid = false;
+	}
+
+	virtual void AddHit(const Hit &p_hit) override {
+		const float early_out = p_hit.GetEarlyOutFraction();
+
+		if (!valid || early_out < hit.GetEarlyOutFraction()) {
+			TBase::UpdateEarlyOutFraction(early_out);
+
+			hit = p_hit;
+			valid = true;
+		}
+	}
+};
+
+template <typename TBase, int TDefaultCapacity>
+class JoltQueryCollectorClosestMulti final : public TBase {
+public:
+	typedef typename TBase::ResultType Hit;
+
+private:
+	JPH::Array<Hit> hits;
+	int max_hits = 0;
+
+public:
+	explicit JoltQueryCollectorClosestMulti(int p_max_hits = TDefaultCapacity) :
+			max_hits(p_max_hits) {}
+
+	bool had_hit() const {
+		return hits.size() > 0;
+	}
+
+	int get_hit_count() const {
+		return hits.size();
+	}
+
+	const Hit &get_hit(int p_index) const {
+		return hits[p_index];
+	}
+
+	void reset() {
+		Reset();
+	}
+
+	virtual void Reset() override {
+		TBase::Reset();
+		hits.clear();
+	}
+
+	virtual void AddHit(const Hit &p_hit) override {
+		typename JPH::Array<Hit>::const_iterator E = hits.cbegin();
+		for (; E != hits.cend(); ++E) {
+			if (p_hit.GetEarlyOutFraction() < E->GetEarlyOutFraction()) {
+				break;
+			}
+		}
+
+		hits.insert(E, p_hit);
+
+		if ((int)hits.size() > max_hits) {
+			hits.resize(max_hits);
+		}
+	}
+};
+
+#endif // JOLT_QUERY_COLLECTORS_H

modules/jolt_physics/spaces/jolt_query_filter_3d.cpp

@@ -0,0 +1,83 @@
+/**************************************************************************/
+/*  jolt_query_filter_3d.cpp                                              */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "jolt_query_filter_3d.h"
+
+#include "../objects/jolt_object_3d.h"
+#include "jolt_broad_phase_layer.h"
+#include "jolt_physics_direct_space_state_3d.h"
+#include "jolt_space_3d.h"
+
+JoltQueryFilter3D::JoltQueryFilter3D(const JoltPhysicsDirectSpaceState3D &p_space_state, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, const HashSet<RID> &p_excluded, bool p_picking) :
+		space(p_space_state.get_space()),
+		excluded(p_excluded),
+		collision_mask(p_collision_mask),
+		collide_with_bodies(p_collide_with_bodies),
+		collide_with_areas(p_collide_with_areas),
+		picking(p_picking) {
+}
+
+bool JoltQueryFilter3D::ShouldCollide(JPH::BroadPhaseLayer p_broad_phase_layer) const {
+	const JPH::BroadPhaseLayer::Type broad_phase_layer = (JPH::BroadPhaseLayer::Type)p_broad_phase_layer;
+
+	switch (broad_phase_layer) {
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::BODY_STATIC:
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::BODY_STATIC_BIG:
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::BODY_DYNAMIC: {
+			return collide_with_bodies;
+		} break;
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::AREA_DETECTABLE:
+		case (JPH::BroadPhaseLayer::Type)JoltBroadPhaseLayer::AREA_UNDETECTABLE: {
+			return collide_with_areas;
+		} break;
+		default: {
+			ERR_FAIL_V_MSG(false, vformat("Unhandled broad phase layer: '%d'. This should not happen. Please report this.", broad_phase_layer));
+		}
+	}
+}
+
+bool JoltQueryFilter3D::ShouldCollide(JPH::ObjectLayer p_object_layer) const {
+	JPH::BroadPhaseLayer object_broad_phase_layer = JoltBroadPhaseLayer::BODY_STATIC;
+	uint32_t object_collision_layer = 0;
+	uint32_t object_collision_mask = 0;
+
+	space.map_from_object_layer(p_object_layer, object_broad_phase_layer, object_collision_layer, object_collision_mask);
+
+	return (collision_mask & object_collision_layer) != 0;
+}
+
+bool JoltQueryFilter3D::ShouldCollide(const JPH::BodyID &p_body_id) const {
+	return true;
+}
+
+bool JoltQueryFilter3D::ShouldCollideLocked(const JPH::Body &p_body) const {
+	JoltObject3D *object = reinterpret_cast<JoltObject3D *>(p_body.GetUserData());
+	return (!picking || object->is_pickable()) && !excluded.has(object->get_rid());
+}

modules/jolt_physics/spaces/jolt_query_filter_3d.h

@@ -0,0 +1,67 @@
+/**************************************************************************/
+/*  jolt_query_filter_3d.h                                                */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef JOLT_QUERY_FILTER_3D_H
+#define JOLT_QUERY_FILTER_3D_H
+
+#include "core/templates/hash_set.h"
+#include "core/templates/rid.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Physics/Body/Body.h"
+#include "Jolt/Physics/Body/BodyFilter.h"
+#include "Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h"
+#include "Jolt/Physics/Collision/ObjectLayer.h"
+
+class JoltPhysicsDirectSpaceState3D;
+class JoltSpace3D;
+
+class JoltQueryFilter3D final
+		: public JPH::BroadPhaseLayerFilter,
+		  public JPH::ObjectLayerFilter,
+		  public JPH::BodyFilter {
+	const JoltSpace3D &space;
+	const HashSet<RID> &excluded;
+	uint32_t collision_mask = 0;
+	bool collide_with_bodies = false;
+	bool collide_with_areas = false;
+	bool picking = false;
+
+public:
+	JoltQueryFilter3D(const JoltPhysicsDirectSpaceState3D &p_space_state, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, const HashSet<RID> &p_excluded, bool p_picking = false);
+
+	virtual bool ShouldCollide(JPH::BroadPhaseLayer p_broad_phase_layer) const override;
+	virtual bool ShouldCollide(JPH::ObjectLayer p_object_layer) const override;
+	virtual bool ShouldCollide(const JPH::BodyID &p_body_id) const override;
+	virtual bool ShouldCollideLocked(const JPH::Body &p_body) const override;
+};
+
+#endif // JOLT_QUERY_FILTER_3D_H

modules/jolt_physics/spaces/jolt_space_3d.cpp

@@ -0,0 +1,514 @@
+/**************************************************************************/
+/*  jolt_space_3d.cpp                                                     */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "jolt_space_3d.h"
+
+#include "../joints/jolt_joint_3d.h"
+#include "../jolt_physics_server_3d.h"
+#include "../jolt_project_settings.h"
+#include "../misc/jolt_stream_wrappers.h"
+#include "../objects/jolt_area_3d.h"
+#include "../objects/jolt_body_3d.h"
+#include "../shapes/jolt_custom_shape_type.h"
+#include "../shapes/jolt_shape_3d.h"
+#include "jolt_contact_listener_3d.h"
+#include "jolt_layers.h"
+#include "jolt_physics_direct_space_state_3d.h"
+#include "jolt_temp_allocator.h"
+
+#include "core/io/file_access.h"
+#include "core/os/time.h"
+#include "core/string/print_string.h"
+#include "core/variant/variant_utility.h"
+
+#include "Jolt/Physics/PhysicsScene.h"
+
+namespace {
+
+constexpr double DEFAULT_CONTACT_RECYCLE_RADIUS = 0.01;
+constexpr double DEFAULT_CONTACT_MAX_SEPARATION = 0.05;
+constexpr double DEFAULT_CONTACT_MAX_ALLOWED_PENETRATION = 0.01;
+constexpr double DEFAULT_CONTACT_DEFAULT_BIAS = 0.8;
+constexpr double DEFAULT_SLEEP_THRESHOLD_LINEAR = 0.1;
+constexpr double DEFAULT_SLEEP_THRESHOLD_ANGULAR = 8.0 * Math_PI / 180;
+constexpr double DEFAULT_SOLVER_ITERATIONS = 8;
+
+} // namespace
+
+void JoltSpace3D::_pre_step(float p_step) {
+	body_accessor.acquire_all();
+
+	contact_listener->pre_step();
+
+	const int body_count = body_accessor.get_count();
+
+	for (int i = 0; i < body_count; ++i) {
+		if (JPH::Body *jolt_body = body_accessor.try_get(i)) {
+			if (jolt_body->IsSoftBody()) {
+				continue;
+			}
+
+			JoltShapedObject3D *object = reinterpret_cast<JoltShapedObject3D *>(jolt_body->GetUserData());
+
+			object->pre_step(p_step, *jolt_body);
+
+			if (object->reports_contacts()) {
+				contact_listener->listen_for(object);
+			}
+		}
+	}
+
+	body_accessor.release();
+}
+
+void JoltSpace3D::_post_step(float p_step) {
+	body_accessor.acquire_all();
+
+	contact_listener->post_step();
+
+	const int body_count = body_accessor.get_count();
+
+	for (int i = 0; i < body_count; ++i) {
+		if (JPH::Body *jolt_body = body_accessor.try_get(i)) {
+			if (jolt_body->IsSoftBody()) {
+				continue;
+			}
+
+			JoltObject3D *object = reinterpret_cast<JoltObject3D *>(jolt_body->GetUserData());
+
+			object->post_step(p_step, *jolt_body);
+		}
+	}
+
+	body_accessor.release();
+}
+
+JoltSpace3D::JoltSpace3D(JPH::JobSystem *p_job_system) :
+		body_accessor(this),
+		job_system(p_job_system),
+		temp_allocator(new JoltTempAllocator()),
+		layers(new JoltLayers()),
+		contact_listener(new JoltContactListener3D(this)),
+		physics_system(new JPH::PhysicsSystem()) {
+	physics_system->Init((JPH::uint)JoltProjectSettings::get_max_bodies(), 0, (JPH::uint)JoltProjectSettings::get_max_pairs(), (JPH::uint)JoltProjectSettings::get_max_contact_constraints(), *layers, *layers, *layers);
+
+	JPH::PhysicsSettings settings;
+	settings.mBaumgarte = JoltProjectSettings::get_baumgarte_stabilization_factor();
+	settings.mSpeculativeContactDistance = JoltProjectSettings::get_speculative_contact_distance();
+	settings.mPenetrationSlop = JoltProjectSettings::get_penetration_slop();
+	settings.mLinearCastThreshold = JoltProjectSettings::get_ccd_movement_threshold();
+	settings.mLinearCastMaxPenetration = JoltProjectSettings::get_ccd_max_penetration();
+	settings.mBodyPairCacheMaxDeltaPositionSq = JoltProjectSettings::get_body_pair_cache_distance_sq();
+	settings.mBodyPairCacheCosMaxDeltaRotationDiv2 = JoltProjectSettings::get_body_pair_cache_angle_cos_div2();
+	settings.mNumVelocitySteps = (JPH::uint)JoltProjectSettings::get_simulation_velocity_steps();
+	settings.mNumPositionSteps = (JPH::uint)JoltProjectSettings::get_simulation_position_steps();
+	settings.mMinVelocityForRestitution = JoltProjectSettings::get_bounce_velocity_threshold();
+	settings.mTimeBeforeSleep = JoltProjectSettings::get_sleep_time_threshold();
+	settings.mPointVelocitySleepThreshold = JoltProjectSettings::get_sleep_velocity_threshold();
+	settings.mUseBodyPairContactCache = JoltProjectSettings::is_body_pair_contact_cache_enabled();
+	settings.mAllowSleeping = JoltProjectSettings::is_sleep_allowed();
+
+	physics_system->SetPhysicsSettings(settings);
+	physics_system->SetGravity(JPH::Vec3::sZero());
+	physics_system->SetContactListener(contact_listener);
+	physics_system->SetSoftBodyContactListener(contact_listener);
+
+	physics_system->SetCombineFriction([](const JPH::Body &p_body1, const JPH::SubShapeID &p_sub_shape_id1, const JPH::Body &p_body2, const JPH::SubShapeID &p_sub_shape_id2) {
+		return ABS(MIN(p_body1.GetFriction(), p_body2.GetFriction()));
+	});
+
+	physics_system->SetCombineRestitution([](const JPH::Body &p_body1, const JPH::SubShapeID &p_sub_shape_id1, const JPH::Body &p_body2, const JPH::SubShapeID &p_sub_shape_id2) {
+		return CLAMP(p_body1.GetRestitution() + p_body2.GetRestitution(), 0.0f, 1.0f);
+	});
+}
+
+JoltSpace3D::~JoltSpace3D() {
+	if (direct_state != nullptr) {
+		memdelete(direct_state);
+		direct_state = nullptr;
+	}
+
+	if (physics_system != nullptr) {
+		delete physics_system;
+		physics_system = nullptr;
+	}
+
+	if (contact_listener != nullptr) {
+		delete contact_listener;
+		contact_listener = nullptr;
+	}
+
+	if (layers != nullptr) {
+		delete layers;
+		layers = nullptr;
+	}
+
+	if (temp_allocator != nullptr) {
+		delete temp_allocator;
+		temp_allocator = nullptr;
+	}
+}
+
+void JoltSpace3D::step(float p_step) {
+	stepping = true;
+	last_step = p_step;
+
+	_pre_step(p_step);
+
+	const JPH::EPhysicsUpdateError update_error = physics_system->Update(p_step, 1, temp_allocator, job_system);
+
+	if ((update_error & JPH::EPhysicsUpdateError::ManifoldCacheFull) != JPH::EPhysicsUpdateError::None) {
+		WARN_PRINT_ONCE(vformat("Jolt Physics manifold cache exceeded capacity and contacts were ignored. "
+								"Consider increasing maximum number of contact constraints in project settings. "
+								"Maximum number of contact constraints is currently set to %d.",
+				JoltProjectSettings::get_max_contact_constraints()));
+	}
+
+	if ((update_error & JPH::EPhysicsUpdateError::BodyPairCacheFull) != JPH::EPhysicsUpdateError::None) {
+		WARN_PRINT_ONCE(vformat("Jolt Physics body pair cache exceeded capacity and contacts were ignored. "
+								"Consider increasing maximum number of body pairs in project settings. "
+								"Maximum number of body pairs is currently set to %d.",
+				JoltProjectSettings::get_max_pairs()));
+	}
+
+	if ((update_error & JPH::EPhysicsUpdateError::ContactConstraintsFull) != JPH::EPhysicsUpdateError::None) {
+		WARN_PRINT_ONCE(vformat("Jolt Physics contact constraint buffer exceeded capacity and contacts were ignored. "
+								"Consider increasing maximum number of contact constraints in project settings. "
+								"Maximum number of contact constraints is currently set to %d.",
+				JoltProjectSettings::get_max_contact_constraints()));
+	}
+
+	_post_step(p_step);
+
+	bodies_added_since_optimizing = 0;
+	has_stepped = true;
+	stepping = false;
+}
+
+void JoltSpace3D::call_queries() {
+	if (!has_stepped) {
+		// We need to skip the first invocation of this method, because there will be pending notifications that need to
+		// be flushed first, which can cause weird conflicts with things like `_integrate_forces`. This happens to also
+		// emulate the behavior of Godot Physics, where (active) collision objects must register to have `call_queries`
+		// invoked, which they don't do until the physics step, which happens after this.
+		//
+		// TODO: This would be better solved by just doing what Godot Physics does with `GodotSpace*D::active_list`.
+		return;
+	}
+
+	body_accessor.acquire_all();
+
+	const int body_count = body_accessor.get_count();
+
+	for (int i = 0; i < body_count; ++i) {
+		if (JPH::Body *jolt_body = body_accessor.try_get(i)) {
+			if (!jolt_body->IsSensor() && !jolt_body->IsSoftBody()) {
+				JoltBody3D *body = reinterpret_cast<JoltBody3D *>(jolt_body->GetUserData());
+				body->call_queries(*jolt_body);
+			}
+		}
+	}
+
+	for (int i = 0; i < body_count; ++i) {
+		if (JPH::Body *jolt_body = body_accessor.try_get(i)) {
+			if (jolt_body->IsSensor()) {
+				JoltArea3D *area = reinterpret_cast<JoltArea3D *>(jolt_body->GetUserData());
+				area->call_queries(*jolt_body);
+			}
+		}
+	}
+
+	body_accessor.release();
+}
+
+double JoltSpace3D::get_param(PhysicsServer3D::SpaceParameter p_param) const {
+	switch (p_param) {
+		case PhysicsServer3D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: {
+			return DEFAULT_CONTACT_RECYCLE_RADIUS;
+		}
+		case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_SEPARATION: {
+			return DEFAULT_CONTACT_MAX_SEPARATION;
+		}
+		case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION: {
+			return DEFAULT_CONTACT_MAX_ALLOWED_PENETRATION;
+		}
+		case PhysicsServer3D::SPACE_PARAM_CONTACT_DEFAULT_BIAS: {
+			return DEFAULT_CONTACT_DEFAULT_BIAS;
+		}
+		case PhysicsServer3D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: {
+			return DEFAULT_SLEEP_THRESHOLD_LINEAR;
+		}
+		case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: {
+			return DEFAULT_SLEEP_THRESHOLD_ANGULAR;
+		}
+		case PhysicsServer3D::SPACE_PARAM_BODY_TIME_TO_SLEEP: {
+			return JoltProjectSettings::get_sleep_time_threshold();
+		}
+		case PhysicsServer3D::SPACE_PARAM_SOLVER_ITERATIONS: {
+			return DEFAULT_SOLVER_ITERATIONS;
+		}
+		default: {
+			ERR_FAIL_V_MSG(0.0, vformat("Unhandled space parameter: '%d'. This should not happen. Please report this.", p_param));
+		}
+	}
+}
+
+void JoltSpace3D::set_param(PhysicsServer3D::SpaceParameter p_param, double p_value) {
+	switch (p_param) {
+		case PhysicsServer3D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: {
+			WARN_PRINT("Space-specific contact recycle radius is not supported when using Jolt Physics. Any such value will be ignored.");
+		} break;
+		case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_SEPARATION: {
+			WARN_PRINT("Space-specific contact max separation is not supported when using Jolt Physics. Any such value will be ignored.");
+		} break;
+		case PhysicsServer3D::SPACE_PARAM_CONTACT_MAX_ALLOWED_PENETRATION: {
+			WARN_PRINT("Space-specific contact max allowed penetration is not supported when using Jolt Physics. Any such value will be ignored.");
+		} break;
+		case PhysicsServer3D::SPACE_PARAM_CONTACT_DEFAULT_BIAS: {
+			WARN_PRINT("Space-specific contact default bias is not supported when using Jolt Physics. Any such value will be ignored.");
+		} break;
+		case PhysicsServer3D::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_THRESHOLD: {
+			WARN_PRINT("Space-specific linear velocity sleep threshold is not supported when using Jolt Physics. Any such value will be ignored.");
+		} break;
+		case PhysicsServer3D::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_THRESHOLD: {
+			WARN_PRINT("Space-specific angular velocity sleep threshold is not supported when using Jolt Physics. Any such value will be ignored.");
+		} break;
+		case PhysicsServer3D::SPACE_PARAM_BODY_TIME_TO_SLEEP: {
+			WARN_PRINT("Space-specific body sleep time is not supported when using Jolt Physics. Any such value will be ignored.");
+		} break;
+		case PhysicsServer3D::SPACE_PARAM_SOLVER_ITERATIONS: {
+			WARN_PRINT("Space-specific solver iterations is not supported when using Jolt Physics. Any such value will be ignored.");
+		} break;
+		default: {
+			ERR_FAIL_MSG(vformat("Unhandled space parameter: '%d'. This should not happen. Please report this.", p_param));
+		} break;
+	}
+}
+
+JPH::BodyInterface &JoltSpace3D::get_body_iface() {
+	return physics_system->GetBodyInterfaceNoLock();
+}
+
+const JPH::BodyInterface &JoltSpace3D::get_body_iface() const {
+	return physics_system->GetBodyInterfaceNoLock();
+}
+
+const JPH::BodyLockInterface &JoltSpace3D::get_lock_iface() const {
+	return physics_system->GetBodyLockInterfaceNoLock();
+}
+
+const JPH::BroadPhaseQuery &JoltSpace3D::get_broad_phase_query() const {
+	return physics_system->GetBroadPhaseQuery();
+}
+
+const JPH::NarrowPhaseQuery &JoltSpace3D::get_narrow_phase_query() const {
+	return physics_system->GetNarrowPhaseQueryNoLock();
+}
+
+JPH::ObjectLayer JoltSpace3D::map_to_object_layer(JPH::BroadPhaseLayer p_broad_phase_layer, uint32_t p_collision_layer, uint32_t p_collision_mask) {
+	return layers->to_object_layer(p_broad_phase_layer, p_collision_layer, p_collision_mask);
+}
+
+void JoltSpace3D::map_from_object_layer(JPH::ObjectLayer p_object_layer, JPH::BroadPhaseLayer &r_broad_phase_layer, uint32_t &r_collision_layer, uint32_t &r_collision_mask) const {
+	layers->from_object_layer(p_object_layer, r_broad_phase_layer, r_collision_layer, r_collision_mask);
+}
+
+JoltReadableBody3D JoltSpace3D::read_body(const JPH::BodyID &p_body_id) const {
+	return JoltReadableBody3D(*this, p_body_id);
+}
+
+JoltReadableBody3D JoltSpace3D::read_body(const JoltObject3D &p_object) const {
+	return read_body(p_object.get_jolt_id());
+}
+
+JoltWritableBody3D JoltSpace3D::write_body(const JPH::BodyID &p_body_id) const {
+	return JoltWritableBody3D(*this, p_body_id);
+}
+
+JoltWritableBody3D JoltSpace3D::write_body(const JoltObject3D &p_object) const {
+	return write_body(p_object.get_jolt_id());
+}
+
+JoltReadableBodies3D JoltSpace3D::read_bodies(const JPH::BodyID *p_body_ids, int p_body_count) const {
+	return JoltReadableBodies3D(*this, p_body_ids, p_body_count);
+}
+
+JoltWritableBodies3D JoltSpace3D::write_bodies(const JPH::BodyID *p_body_ids, int p_body_count) const {
+	return JoltWritableBodies3D(*this, p_body_ids, p_body_count);
+}
+
+JoltPhysicsDirectSpaceState3D *JoltSpace3D::get_direct_state() {
+	if (direct_state == nullptr) {
+		direct_state = memnew(JoltPhysicsDirectSpaceState3D(this));
+	}
+
+	return direct_state;
+}
+
+void JoltSpace3D::set_default_area(JoltArea3D *p_area) {
+	if (default_area == p_area) {
+		return;
+	}
+
+	if (default_area != nullptr) {
+		default_area->set_default_area(false);
+	}
+
+	default_area = p_area;
+
+	if (default_area != nullptr) {
+		default_area->set_default_area(true);
+	}
+}
+
+JPH::BodyID JoltSpace3D::add_rigid_body(const JoltObject3D &p_object, const JPH::BodyCreationSettings &p_settings, bool p_sleeping) {
+	const JPH::BodyID body_id = get_body_iface().CreateAndAddBody(p_settings, p_sleeping ? JPH::EActivation::DontActivate : JPH::EActivation::Activate);
+
+	if (unlikely(body_id.IsInvalid())) {
+		ERR_PRINT_ONCE(vformat("Failed to create underlying Jolt Physics body for '%s'. "
+							   "Consider increasing maximum number of bodies in project settings. "
+							   "Maximum number of bodies is currently set to %d.",
+				p_object.to_string(), JoltProjectSettings::get_max_bodies()));
+
+		return JPH::BodyID();
+	}
+
+	bodies_added_since_optimizing += 1;
+
+	return body_id;
+}
+
+JPH::BodyID JoltSpace3D::add_soft_body(const JoltObject3D &p_object, const JPH::SoftBodyCreationSettings &p_settings, bool p_sleeping) {
+	const JPH::BodyID body_id = get_body_iface().CreateAndAddSoftBody(p_settings, p_sleeping ? JPH::EActivation::DontActivate : JPH::EActivation::Activate);
+
+	if (unlikely(body_id.IsInvalid())) {
+		ERR_PRINT_ONCE(vformat("Failed to create underlying Jolt Physics body for '%s'. "
+							   "Consider increasing maximum number of bodies in project settings. "
+							   "Maximum number of bodies is currently set to %d.",
+				p_object.to_string(), JoltProjectSettings::get_max_bodies()));
+
+		return JPH::BodyID();
+	}
+
+	bodies_added_since_optimizing += 1;
+
+	return body_id;
+}
+
+void JoltSpace3D::remove_body(const JPH::BodyID &p_body_id) {
+	JPH::BodyInterface &body_iface = get_body_iface();
+
+	body_iface.RemoveBody(p_body_id);
+	body_iface.DestroyBody(p_body_id);
+}
+
+void JoltSpace3D::try_optimize() {
+	// This makes assumptions about the underlying acceleration structure of Jolt's broad-phase, which currently uses a
+	// quadtree, and which gets walked with a fixed-size node stack of 128. This means that when the quadtree is
+	// completely unbalanced, as is the case if we add bodies one-by-one without ever stepping the simulation, like in
+	// the editor viewport, we would exceed this stack size (resulting in an incomplete search) as soon as we perform a
+	// physics query after having added somewhere in the order of 128 * 3 bodies. We leave a hefty margin just in case.
+
+	if (likely(bodies_added_since_optimizing < 128)) {
+		return;
+	}
+
+	physics_system->OptimizeBroadPhase();
+
+	bodies_added_since_optimizing = 0;
+}
+
+void JoltSpace3D::add_joint(JPH::Constraint *p_jolt_ref) {
+	physics_system->AddConstraint(p_jolt_ref);
+}
+
+void JoltSpace3D::add_joint(JoltJoint3D *p_joint) {
+	add_joint(p_joint->get_jolt_ref());
+}
+
+void JoltSpace3D::remove_joint(JPH::Constraint *p_jolt_ref) {
+	physics_system->RemoveConstraint(p_jolt_ref);
+}
+
+void JoltSpace3D::remove_joint(JoltJoint3D *p_joint) {
+	remove_joint(p_joint->get_jolt_ref());
+}
+
+#ifdef DEBUG_ENABLED
+
+void JoltSpace3D::dump_debug_snapshot(const String &p_dir) {
+	const Dictionary datetime = Time::get_singleton()->get_datetime_dict_from_system();
+	const String datetime_str = vformat("%04d-%02d-%02d_%02d-%02d-%02d", datetime["year"], datetime["month"], datetime["day"], datetime["hour"], datetime["minute"], datetime["second"]);
+	const String path = p_dir + vformat("/jolt_snapshot_%s_%d.bin", datetime_str, rid.get_id());
+
+	Ref<FileAccess> file_access = FileAccess::open(path, FileAccess::ModeFlags::WRITE);
+	ERR_FAIL_COND_MSG(file_access.is_null(), vformat("Failed to open '%s' for writing when saving snapshot of physics space with RID '%d'.", path, rid.get_id()));
+
+	JPH::PhysicsScene physics_scene;
+	physics_scene.FromPhysicsSystem(physics_system);
+
+	for (JPH::BodyCreationSettings &settings : physics_scene.GetBodies()) {
+		const JoltObject3D *object = reinterpret_cast<const JoltObject3D *>(settings.mUserData);
+
+		if (const JoltBody3D *body = object->as_body()) {
+			// Since we do our own integration of gravity and damping, while leaving Jolt's own values at zero, we need to transfer over the correct values.
+			settings.mGravityFactor = body->get_gravity_scale();
+			settings.mLinearDamping = body->get_total_linear_damp();
+			settings.mAngularDamping = body->get_total_angular_damp();
+		}
+
+		settings.SetShape(JoltShape3D::without_custom_shapes(settings.GetShape()));
+	}
+
+	JoltStreamOutputWrapper output_stream(file_access);
+	physics_scene.SaveBinaryState(output_stream, true, false);
+
+	ERR_FAIL_COND_MSG(file_access->get_error() != OK, vformat("Writing snapshot of physics space with RID '%d' to '%s' failed with error '%s'.", rid.get_id(), path, VariantUtilityFunctions::error_string(file_access->get_error())));
+
+	print_line(vformat("Snapshot of physics space with RID '%d' saved to '%s'.", rid.get_id(), path));
+}
+
+const PackedVector3Array &JoltSpace3D::get_debug_contacts() const {
+	return contact_listener->get_debug_contacts();
+}
+
+int JoltSpace3D::get_debug_contact_count() const {
+	return contact_listener->get_debug_contact_count();
+}
+
+int JoltSpace3D::get_max_debug_contacts() const {
+	return contact_listener->get_max_debug_contacts();
+}
+
+void JoltSpace3D::set_max_debug_contacts(int p_count) {
+	contact_listener->set_max_debug_contacts(p_count);
+}
+
+#endif

modules/jolt_physics/spaces/jolt_space_3d.h

@@ -0,0 +1,149 @@
+/**************************************************************************/
+/*  jolt_space_3d.h                                                       */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef JOLT_SPACE_3D_H
+#define JOLT_SPACE_3D_H
+
+#include "jolt_body_accessor_3d.h"
+
+#include "servers/physics_server_3d.h"
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Core/JobSystem.h"
+#include "Jolt/Core/TempAllocator.h"
+#include "Jolt/Physics/Body/BodyInterface.h"
+#include "Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h"
+#include "Jolt/Physics/Collision/NarrowPhaseQuery.h"
+#include "Jolt/Physics/Constraints/Constraint.h"
+#include "Jolt/Physics/PhysicsSystem.h"
+
+#include <stdint.h>
+
+class JoltArea3D;
+class JoltContactListener3D;
+class JoltJoint3D;
+class JoltLayers;
+class JoltObject3D;
+class JoltPhysicsDirectSpaceState3D;
+
+class JoltSpace3D {
+	JoltBodyWriter3D body_accessor;
+
+	RID rid;
+
+	JPH::JobSystem *job_system = nullptr;
+	JPH::TempAllocator *temp_allocator = nullptr;
+	JoltLayers *layers = nullptr;
+	JoltContactListener3D *contact_listener = nullptr;
+	JPH::PhysicsSystem *physics_system = nullptr;
+	JoltPhysicsDirectSpaceState3D *direct_state = nullptr;
+	JoltArea3D *default_area = nullptr;
+
+	float last_step = 0.0f;
+
+	int bodies_added_since_optimizing = 0;
+
+	bool active = false;
+	bool stepping = false;
+	bool has_stepped = false;
+
+	void _pre_step(float p_step);
+	void _post_step(float p_step);
+
+public:
+	explicit JoltSpace3D(JPH::JobSystem *p_job_system);
+	~JoltSpace3D();
+
+	void step(float p_step);
+
+	void call_queries();
+
+	RID get_rid() const { return rid; }
+	void set_rid(const RID &p_rid) { rid = p_rid; }
+
+	bool is_active() const { return active; }
+	void set_active(bool p_active) { active = p_active; }
+
+	bool is_stepping() const { return stepping; }
+
+	double get_param(PhysicsServer3D::SpaceParameter p_param) const;
+	void set_param(PhysicsServer3D::SpaceParameter p_param, double p_value);
+
+	JPH::PhysicsSystem &get_physics_system() const { return *physics_system; }
+
+	JPH::BodyInterface &get_body_iface();
+	const JPH::BodyInterface &get_body_iface() const;
+	const JPH::BodyLockInterface &get_lock_iface() const;
+
+	const JPH::BroadPhaseQuery &get_broad_phase_query() const;
+	const JPH::NarrowPhaseQuery &get_narrow_phase_query() const;
+
+	JPH::ObjectLayer map_to_object_layer(JPH::BroadPhaseLayer p_broad_phase_layer, uint32_t p_collision_layer, uint32_t p_collision_mask);
+	void map_from_object_layer(JPH::ObjectLayer p_object_layer, JPH::BroadPhaseLayer &r_broad_phase_layer, uint32_t &r_collision_layer, uint32_t &r_collision_mask) const;
+
+	JoltReadableBody3D read_body(const JPH::BodyID &p_body_id) const;
+	JoltReadableBody3D read_body(const JoltObject3D &p_object) const;
+
+	JoltWritableBody3D write_body(const JPH::BodyID &p_body_id) const;
+	JoltWritableBody3D write_body(const JoltObject3D &p_object) const;
+
+	JoltReadableBodies3D read_bodies(const JPH::BodyID *p_body_ids, int p_body_count) const;
+	JoltWritableBodies3D write_bodies(const JPH::BodyID *p_body_ids, int p_body_count) const;
+
+	JoltPhysicsDirectSpaceState3D *get_direct_state();
+
+	JoltArea3D *get_default_area() const { return default_area; }
+	void set_default_area(JoltArea3D *p_area);
+
+	float get_last_step() const { return last_step; }
+
+	JPH::BodyID add_rigid_body(const JoltObject3D &p_object, const JPH::BodyCreationSettings &p_settings, bool p_sleeping = false);
+	JPH::BodyID add_soft_body(const JoltObject3D &p_object, const JPH::SoftBodyCreationSettings &p_settings, bool p_sleeping = false);
+
+	void remove_body(const JPH::BodyID &p_body_id);
+
+	void try_optimize();
+
+	void add_joint(JPH::Constraint *p_jolt_ref);
+	void add_joint(JoltJoint3D *p_joint);
+	void remove_joint(JPH::Constraint *p_jolt_ref);
+	void remove_joint(JoltJoint3D *p_joint);
+
+#ifdef DEBUG_ENABLED
+	void dump_debug_snapshot(const String &p_dir);
+	const PackedVector3Array &get_debug_contacts() const;
+	int get_debug_contact_count() const;
+	int get_max_debug_contacts() const;
+	void set_max_debug_contacts(int p_count);
+#endif
+};
+
+#endif // JOLT_SPACE_3D_H

modules/jolt_physics/spaces/jolt_temp_allocator.cpp

@@ -0,0 +1,120 @@
+/**************************************************************************/
+/*  jolt_temp_allocator.cpp                                               */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+/*
+Adapted to Godot from the Jolt Physics library.
+*/
+
+/*
+Copyright 2021 Jorrit Rouwe
+
+Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in
+the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
+the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR
+A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "jolt_temp_allocator.h"
+
+#include "../jolt_project_settings.h"
+
+#include "core/variant/variant.h"
+
+#include "Jolt/Core/Memory.h"
+
+namespace {
+
+template <typename TValue, typename TAlignment>
+constexpr TValue align_up(TValue p_value, TAlignment p_alignment) {
+	return (p_value + p_alignment - 1) & ~(p_alignment - 1);
+}
+
+} //namespace
+
+JoltTempAllocator::JoltTempAllocator() :
+		capacity((uint64_t)JoltProjectSettings::get_temp_memory_b()),
+		base(static_cast<uint8_t *>(JPH::Allocate((size_t)capacity))) {
+}
+
+JoltTempAllocator::~JoltTempAllocator() {
+	JPH::Free(base);
+}
+
+void *JoltTempAllocator::Allocate(uint32_t p_size) {
+	if (p_size == 0) {
+		return nullptr;
+	}
+
+	p_size = align_up(p_size, 16U);
+
+	const uint64_t new_top = top + p_size;
+
+	void *ptr = nullptr;
+
+	if (new_top <= capacity) {
+		ptr = base + top;
+	} else {
+		WARN_PRINT_ONCE(vformat("Jolt Physics temporary memory allocator exceeded capacity of %d MiB. "
+								"Falling back to slower general-purpose allocator. "
+								"Consider increasing maximum temporary memory in project settings.",
+				JoltProjectSettings::get_temp_memory_mib()));
+
+		ptr = JPH::Allocate(p_size);
+	}
+
+	top = new_top;
+
+	return ptr;
+}
+
+void JoltTempAllocator::Free(void *p_ptr, uint32_t p_size) {
+	if (p_ptr == nullptr) {
+		return;
+	}
+
+	p_size = align_up(p_size, 16U);
+
+	const uint64_t new_top = top - p_size;
+
+	if (top <= capacity) {
+		if (base + new_top != p_ptr) {
+			CRASH_NOW_MSG("Jolt Physics temporary memory was freed in the wrong order.");
+		}
+	} else {
+		JPH::Free(p_ptr);
+	}
+
+	top = new_top;
+}

modules/jolt_physics/spaces/jolt_temp_allocator.h

@@ -0,0 +1,53 @@
+/**************************************************************************/
+/*  jolt_temp_allocator.h                                                 */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef JOLT_TEMP_ALLOCATOR_H
+#define JOLT_TEMP_ALLOCATOR_H
+
+#include "Jolt/Jolt.h"
+
+#include "Jolt/Core/TempAllocator.h"
+
+#include <stdint.h>
+
+class JoltTempAllocator final : public JPH::TempAllocator {
+	uint64_t capacity = 0;
+	uint64_t top = 0;
+	uint8_t *base = nullptr;
+
+public:
+	explicit JoltTempAllocator();
+	virtual ~JoltTempAllocator() override;
+
+	virtual void *Allocate(JPH::uint p_size) override;
+	virtual void Free(void *p_ptr, JPH::uint p_size) override;
+};
+
+#endif // JOLT_TEMP_ALLOCATOR_H

pyproject.toml

@@ -84,6 +84,7 @@ ignore-words-list = [
 	"outin",
 	"parm",
 	"requestor",
+	"streamin",
 	"te",
 	"textin",
 	"thirdparty",

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() {

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() {

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

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

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

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