Soft body physics support (#644)

This is a first implementation of soft body physics. It is in a usable state but is still missing the following things:

* Soft bodies currently can't sleep, are executed on a single CPU core and collision checks are not as efficient as they could be.
* Soft bodies can only collide with rigid bodies, collisions between soft bodies are not implemented yet.
* ContactListener callbacks are not triggered for soft bodies.
* AddForce/AddTorque/SetLinearVelocity/SetLinearVelocityClamped/SetAngularVelocity/SetAngularVelocityClamped/AddImpulse/AddAngularImpulse have no effect on soft bodies as the velocity is stored per particle rather than per body.
* Buoyancy calculations have not been implemented yet.
* Constraints cannot operate on soft bodies, set the inverse mass of a particle to zero and move it by setting a velocity to constrain a soft body to something else.
* When calculating friction / restitution an empty SubShapeID will be passed to the ContactConstraintManager::CombineFunction because this is called once per body pair rather than once per sub shape as is common for rigid bodies.

There are a number of demos in the Soft Body folder that demonstrate how the system works.

Docs/Architecture.md

@@ -12,14 +12,14 @@ In general, body ID's ([BodyID](@ref BodyID)) are used to refer to bodies. You c
 
 	JPH::BodyLockInterface lock_interface = physics_system.GetBodyLockInterface(); // Or GetBodyLockInterfaceNoLock
 	JPH::BodyID body_id = ...; // Obtain ID to body
-	
+
 	// Scoped lock
 	{
 		JPH::BodyLockRead lock(lock_interface, body_id);
 		if (lock.Succeeded()) // body_id may no longer be valid
 		{
 			const JPH::Body &body = lock.GetBody();
-	
+
 			// Do something with body
 			...
 		}
@@ -40,7 +40,7 @@ Note that there are still some restrictions:
 * You cannot read from / write to bodies or constraints while PhysicsSystem::Update is running. As soon as the Update starts, all body / constraint mutexes are locked.
 * Collision callbacks (see ContactListener) are called from within the PhysicsSystem::Update call from multiple threads. You can only read the body data during a callback.
 * Activation callbacks (see BodyActivationListener) are called in the same way. Again you should only read the body during the callback and not make any modifications.
-* Step callbacks (see PhysicsStepListener) are also called from PhysicsSystem::Update from multiple threads. You're responsible for making sure that there are no race conditions. In a step listener you can read/write bodies or constraints but you cannot add/remove them. 
+* Step callbacks (see PhysicsStepListener) are also called from PhysicsSystem::Update from multiple threads. You're responsible for making sure that there are no race conditions. In a step listener you can read/write bodies or constraints but you cannot add/remove them.
 
 If you are accessing the physics system from multiple threads, you should probably use BodyID's and the locking variant of the body interface. It is however still possible to use Body pointers if you're really careful. E.g. if there is a clear owner of a Body and you ensure that this owner does not read/write state during PhysicsSystem::Update or while other threads are reading the Body there will not be any race conditions.
 
@@ -67,7 +67,7 @@ Next to this there are a number of decorator shapes that change the behavior of
 
 ### Creating Shapes
 
-Simple shapes like spheres and boxes can be constructed immediately by simply new-ing them. Other shapes need to be converted into an optimized format in order to be usable in the physics simulation. The uncooked data is usually stored in a [ShapeSettings](@ref ShapeSettings) object and then converted to cooked format by a [Create](@ref ShapeSettings::Create) function that returns a [Result](@ref Result) object that indicates success or failure and provides the cooked object. 
+Simple shapes like spheres and boxes can be constructed immediately by simply new-ing them. Other shapes need to be converted into an optimized format in order to be usable in the physics simulation. The uncooked data is usually stored in a [ShapeSettings](@ref ShapeSettings) object and then converted to cooked format by a [Create](@ref ShapeSettings::Create) function that returns a [Result](@ref Result) object that indicates success or failure and provides the cooked object.
 
 Creating a convex hull for example looks like:
 
@@ -143,7 +143,7 @@ In order to speed up the collision detection system, all convex shapes use a con
 
 ### Center of Mass
 
-__Beware: When a shape is created, it will automatically recenter itself around its center of mass.__ The center of mass can be obtained by calling [Shape::GetCenterOfMass](@ref Shape::GetCenterOfMass) and most functions operate in this Center of Mass (COM) space. Some functions work in the original space the shape was created in, they usually have World Space (WS) or Shape Space (SS) in their name (or documentation). 
+__Beware: When a shape is created, it will automatically recenter itself around its center of mass.__ The center of mass can be obtained by calling [Shape::GetCenterOfMass](@ref Shape::GetCenterOfMass) and most functions operate in this Center of Mass (COM) space. Some functions work in the original space the shape was created in, they usually have World Space (WS) or Shape Space (SS) in their name (or documentation).
 
 ![Shape Center of Mass](Images/ShapeCenterOfMass.jpg)
 
@@ -166,7 +166,7 @@ As an example, say we create a box and then translate it:
 	JPH::RayCastResult hit;
 	bool had_hit = translated_box_shape->CastRay(ray, JPH::SubShapeIDCreator(), hit);
 	JPH_ASSERT(!had_hit); // There's no hit because we did not correct for COM!
-		
+
 	// Convert the ray to center of mass space for the shape (CORRECT!)
 	ray.mOrigin -= translated_box_shape->GetCenterOfMass();
 
@@ -177,7 +177,7 @@ As an example, say we create a box and then translate it:
 In the same way calling:
 
 	translated_box_shape->GetLocalBounds();
-	
+
 will return a box of size 2x2x2 centered around the origin, so in order to get it back to the space in which it was originally created you need to offset the bounding box:
 
 	JPH::AABox shape_bounds = translated_box_shape->GetLocalBounds();
@@ -287,6 +287,28 @@ Because Jolt Physics uses a Symplectic Euler integrator, there will still be a s
 
 Constraints can be turned on / off by calling Constraint::SetEnabled. After every simulation step, check the total 'lambda' applied on each constraint and disable the constraint if the value goes over a certain threshold. Use e.g. SliderConstraint::GetTotalLambdaPosition / HingeConstraint::GetTotalLambdaRotation. You can see 'lambda' as the linear/angular impulse applied at the constraint in the last physics step to keep the constraint together.
 
+## Soft Bodies
+
+Soft bodies (also known as deformable bodies) can be used to create e.g. a soft ball or a piece of cloth. They are created in a very similar way to normal rigid bodies:
+
+* First allocate a new SoftBodySharedSettings object on the heap. This object will contain the initial positions of all particles and the constraints between the particles. This object can be shared between multiple soft bodies and should remain constant during its lifetime.
+* Then create a SoftBodyCreationSettings object (e.g. on the stack) and fill in the desired properties of the soft body.
+* Finally construct the body and add it to the world through BodyInterface::CreateAndAddSoftBody.
+
+Soft bodies use the Body class just like rigid bodies but can be identified by checking Body::IsSoftBody. To get to the soft body state, cast the result of Body::GetMotionProperties to SoftBodyMotionProperties and use its API.
+
+Soft bodies try to implement as much as possible of the normal Body interface, but this interface provides a simplified version of reality, e.g. Body::GetLinearVelocity will return the average particle speed and Body::GetPosition returns the average particle position. During simulation, a soft body will never update its rotation. Internally it stores particle velocities in local space, so if you rotate a soft body e.g. by calling BodyInterface::SetRotation, the body will rotate but its velocity will as well.
+
+Soft bodies are currently in development, please note the following:
+
+* Soft bodies currently can't sleep, are executed on a single CPU core and collision checks are not as efficient as they could be.
+* Soft bodies can only collide with rigid bodies, collisions between soft bodies are not implemented yet.
+* ContactListener callbacks are not triggered for soft bodies.
+* AddForce/AddTorque/SetLinearVelocity/SetLinearVelocityClamped/SetAngularVelocity/SetAngularVelocityClamped/AddImpulse/AddAngularImpulse have no effect on soft bodies as the velocity is stored per particle rather than per body.
+* Buoyancy calculations have not been implemented yet.
+* Constraints cannot operate on soft bodies, set the inverse mass of a particle to zero and move it by setting a velocity to constrain a soft body to something else.
+* When calculating friction / restitution an empty SubShapeID will be passed to the ContactConstraintManager::CombineFunction because this is called once per body pair rather than once per sub shape as is common for rigid bodies.
+
 ## Broad Phase
 
 When bodies are added to the PhysicsSystem, they are inserted in the broad phase ([BroadPhaseQuadTree](@ref BroadPhaseQuadTree)). This provides quick coarse collision detection based on the axis aligned bounding box (AABB) of a body.
@@ -300,7 +322,7 @@ When doing a query against the broad phase ([BroadPhaseQuery](@ref BroadPhaseQue
 ## Narrow Phase
 
 A narrow phase query ([NarrowPhaseQuery](@ref NarrowPhaseQuery)) will first query the broad phase for intersecting bodies and will under the protection of a body lock construct a transformed shape ([TransformedShape](@ref TransformedShape)) object. This object contains the transform, a reference counted shape and a body ID. Since the shape will not be deleted until you destroy the TransformedShape object, it is a consistent snapshot of the collision information of the body. This ensures that the body is only locked for a short time frame and makes it possible to do the bulk of the collision detection work outside the protection of a lock.
- 
+
 For very long running jobs (e.g. navigation mesh creation) it is possible to query all transformed shapes in an area and then do the processing work using a long running thread without requiring additional locks (see [NarrowPhaseQuery::CollectTransformedShapes](@ref NarrowPhaseQuery::CollectTransformedShapes)).
 
 The narrow phase queries are all handled through the [GJK](@ref GJKClosestPoint) and [EPA](@ref EPAPenetrationDepth) algorithms.
@@ -342,7 +364,7 @@ Note that the physics simulation works best if you use SI units (meters, radians
 
 By default the library compiles using floats. This means that the simulation gets less accurate the further you go from the origin. If all simulation takes place within roughly 5 km from the origin, floating point precision is accurate enough.
 
-If you have a bigger world, you may want to compile the library using the JPH_DOUBLE_PRECISION define. When you do this, all positions will be stored as doubles, which will make the simulation accurate even at thousands of kilometers away from the origin. 
+If you have a bigger world, you may want to compile the library using the JPH_DOUBLE_PRECISION define. When you do this, all positions will be stored as doubles, which will make the simulation accurate even at thousands of kilometers away from the origin.
 
 Calculations with doubles are much slower than calculations with floats. A naive implementation that changes all calculations to doubles has been measured to run more than 2x slower than the same calculations using floats. Because of this, Jolt Physics will only use doubles where necessary and drop down to floats as soon as possible. In order to do this, many of the collision query functions will need a 'base offset'. All collision results will be returned as floats relative to this base offset. By choosing the base offset wisely (i.e. close to where collision results are expected) the results will be accurate. Make sure your base offset is not kilometers away from the collision result.
 
@@ -356,7 +378,7 @@ Because of the minimal use of doubles, the simulation runs 5-10% slower in doubl
 
 ## Continuous Collision Detection
 
-Each body has a motion quality setting ([EMotionQuality](@ref EMotionQuality)). By default the motion quality is [Discrete](@ref Discrete). This means that at the beginning of each simulation step we will perform collision detection and if no collision is found, the body is free to move according to its velocity. This usually works fine for big or slow moving objects. Fast and small objects can easily 'tunnel' through thin objects because they can completely move through them in a single time step. For these objects there is the motion quality [LinearCast](@ref LinearCast). Objects that have this motion quality setting will do the same collision detection at the beginning of the simulation step, but once their new position is known, they will do an additional CastShape to check for any collisions that may have been missed. If this is the case, the object is placed back to where the collision occurred and will remain there until the next time step. This is called 'time stealing' and has the disadvantage that an object may appear to move much slower for a single time step and then speed up again. The alternative, back stepping the entire simulation, is computationally heavy so was not implemented. 
+Each body has a motion quality setting ([EMotionQuality](@ref EMotionQuality)). By default the motion quality is [Discrete](@ref Discrete). This means that at the beginning of each simulation step we will perform collision detection and if no collision is found, the body is free to move according to its velocity. This usually works fine for big or slow moving objects. Fast and small objects can easily 'tunnel' through thin objects because they can completely move through them in a single time step. For these objects there is the motion quality [LinearCast](@ref LinearCast). Objects that have this motion quality setting will do the same collision detection at the beginning of the simulation step, but once their new position is known, they will do an additional CastShape to check for any collisions that may have been missed. If this is the case, the object is placed back to where the collision occurred and will remain there until the next time step. This is called 'time stealing' and has the disadvantage that an object may appear to move much slower for a single time step and then speed up again. The alternative, back stepping the entire simulation, is computationally heavy so was not implemented.
 
 |![Motion Quality](Images/MotionQuality.jpg)|
 |:-|
@@ -373,7 +395,7 @@ Fast rotating long objects are also to be avoided, as the LinearCast motion qual
 The physics simulation is deterministic provided that:
 
 * The APIs that modify the simulation are called in exactly the same order. For example, bodies and constraints need to be added/removed/modified in exactly the same order so that the state at the beginning of a simulation step is exactly the same for both simulations.
-* The same binary code is used to run the simulation. For example, when you run the simulation on Windows it doesn't matter if you have an AMD or Intel processor. 
+* The same binary code is used to run the simulation. For example, when you run the simulation on Windows it doesn't matter if you have an AMD or Intel processor.
 
 If you want cross platform determinism then please turn on the CROSS_PLATFORM_DETERMINISTIC option in CMake. This will make the library approximately 8% slower but the simulation will be deterministic regardless of:
 
@@ -393,7 +415,7 @@ It is quite difficult to verify cross platform determinism, so this feature is l
 * Windows MSVC x86 32-bit with SSE2
 * macOS clang x86 64-bit with AVX
 * Linux clang x86 64-bit with AVX2
-* Linux clang ARM 64-bit with NEON 
+* Linux clang ARM 64-bit with NEON
 
 The most important things to look out for in your own application:
 
@@ -412,8 +434,8 @@ If you wish to share saved state between server and client, you need to ensure t
 
 ## Working With Multiple Physics Systems
 
-You can create, simulate and interact with multiple PhysicsSystems at the same time provided that you do not share any objects (bodies, constraints) between the systems. 
-When a Body is created it receives a BodyID that is unique for the PhysicsSystem that it was created for, so it cannot be shared. The only object that can be shared between PhysicsSystems is a Shape. 
+You can create, simulate and interact with multiple PhysicsSystems at the same time provided that you do not share any objects (bodies, constraints) between the systems.
+When a Body is created it receives a BodyID that is unique for the PhysicsSystem that it was created for, so it cannot be shared. The only object that can be shared between PhysicsSystems is a Shape.
 If you want to move a body from one PhysicsSystem to another, use Body::GetBodyCreationSettings to get the settings needed to create the body in the other PhysicsSystem.
 
 PhysicsSystems are not completely independent:
@@ -434,7 +456,7 @@ The job graph looks like this:
 
 Note that each job indicates if it reads/writes positions/velocities and if it deactivates/activates bodies. We do not allow jobs to read/write the same data concurrently. The arrows indicate the order in which jobs are executed. Yellow blocks mean that there are multiple jobs of this type. Dotted arrows have special meaning and are explained below.
 
-### Broad Phase Update Prepare 
+### Broad Phase Update Prepare
 
 This job will refit the AABBs of the broad phase. It does this by building a new tree while keeping the old one available as described in the "Broad Phase" section.
 
@@ -456,7 +478,7 @@ This job will go through all non-contact constraints and determine which constra
 
 ### Build Islands from Constraints
 
-This job will go through all non-contact constraints and assign the involved bodies and constraint to the same island. Since we allow concurrent insertion/removal of bodies we do not want to keep island data across multiple simulation steps, so we recreate the islands from scratch every simulation step. The operation is lock-free and O(N) where N is the number of constraints. 
+This job will go through all non-contact constraints and assign the involved bodies and constraint to the same island. Since we allow concurrent insertion/removal of bodies we do not want to keep island data across multiple simulation steps, so we recreate the islands from scratch every simulation step. The operation is lock-free and O(N) where N is the number of constraints.
 
 If a constraint connects an active and a non-active body, the non-active body is woken up. One find collisions job will not start until this job has finished in order to pick up any collision testing for newly activated bodies.
 
@@ -464,7 +486,7 @@ If a constraint connects an active and a non-active body, the non-active body is
 
 This job will do broad and narrow phase checks. Initially a number of jobs are started based on the amount of active bodies. The job will do the following:
 
-- Take a batch of active bodies and collide them against the broadphase. 
+- Take a batch of active bodies and collide them against the broadphase.
 - When a collision pair is found it is inserted in a lock free queue to be processed later.
 - If the queue is full, it will be processed immediately (more Find Collisions jobs are spawned if not all CPU cores are occupied yet as the queue starts to fill up).
 - If there are no more active bodies to process, the job will start to perform narrow phase collision detection and set up contact constraints if any collisions are found.
@@ -473,7 +495,7 @@ This job will do broad and narrow phase checks. Initially a number of jobs are s
 
 Note that this job cannot start until apply gravity is done because the velocity needs to be known for elastic collisions to be calculated properly.
 
-The contact points between the two bodies will be determined by the [GJK](@ref GJKClosestPoint) and [EPA](@ref EPAPenetrationDepth) algorithms. For each contact point we will calculate the face that belongs to that contact point. The faces of both bodies are clipped against each other ([ManifoldBetweenTwoFaces](@ref ManifoldBetweenTwoFaces)) so that we have a polygon (or point / line) that represents the contact between the two bodies (contact manifold). 
+The contact points between the two bodies will be determined by the [GJK](@ref GJKClosestPoint) and [EPA](@ref EPAPenetrationDepth) algorithms. For each contact point we will calculate the face that belongs to that contact point. The faces of both bodies are clipped against each other ([ManifoldBetweenTwoFaces](@ref ManifoldBetweenTwoFaces)) so that we have a polygon (or point / line) that represents the contact between the two bodies (contact manifold).
 
 Multiple contact manifolds with similar normals are merged together (PhysicsSystem::ProcessBodyPair::ReductionCollideShapeCollector). After this the contact constraints are created in the [ContactConstraintManager](@ref ContactConstraintManager) and their Jacobians / effective masses calculated.
 
@@ -493,7 +515,7 @@ This job will finalize the building of the simulation islands. Each island conta
 
 This job does some housekeeping work that can be executed concurrent to the solver:
 
-* It will assign the island ID to all bodies (which is mainly used for debugging purposes) 
+* It will assign the island ID to all bodies (which is mainly used for debugging purposes)
 
 ### Solve Velocity Constraints
 
@@ -507,7 +529,7 @@ This job prepares the CCD buffers.
 
 ### Integrate & Clamp Velocities
 
-This job will integrate the velocity and update the position. It will clamp the velocity to the max velocity. 
+This job will integrate the velocity and update the position. It will clamp the velocity to the max velocity.
 
 Depending on the motion quality ([EMotionQuality](@ref EMotionQuality)) of the body, it will schedule a body for continuous collision detection (CCD) if its movement is bigger than some treshold based on the [inner radius](@ref Shape::GetInnerRadius)) of the shape.
 
@@ -527,13 +549,13 @@ This job will take the collision results from the previous job and update positi
 
 This job will:
 
-* Swap the read/write contact cache and prepare the contact cache for the next step. 
+* Swap the read/write contact cache and prepare the contact cache for the next step.
 * It will detect all contacts that existed previous step and do not exist anymore to fire callbacks for them through the [ContactListener](@ref ContactListener) interface.
 
 ### Solve Position Constraints, Update Bodies Broad Phase
 
 A number of these jobs will run in parallel. Each job takes the next unprocessed island and run the position based constraint solver. This fixes numerical drift that may have caused constrained bodies to separate (remember that the constraints are solved in the velocity domain, so errors get introduced when doing a linear integration step). It will run until either the applied position corrections are too small or until the max amount of iterations is reached ([PhysicsSettings::mNumPositionSteps](@ref PhysicsSettings::mNumPositionSteps)). Here there is also support for large islands, the island splits that were calculated in the Solve Velocity Constraints job are reused to solve partial islands in the same way as before.
 
-It will also notify the broad phase of the new body positions / AABBs. 
+It will also notify the broad phase of the new body positions / AABBs.
 
-When objects move too little the body will be put to sleep. This is detected by taking the biggest two axis of the local space bounding box of the shape together with the center of mass of the shape (all points in world space) and keep track of 3 bounding spheres for those points over time. If the bounding spheres become too big, the bounding spheres are reset and the timer restarted. When the timer reaches a certain time, the object has is considered non-moving and is put to sleep.
+When objects move too little the body will be put to sleep. This is detected by taking the biggest two axis of the local space bounding box of the shape together with the center of mass of the shape (all points in world space) and keep track of 3 bounding spheres for those points over time. If the bounding spheres become too big, the bounding spheres are reset and the timer restarted. When the timer reaches a certain time, the object has is considered non-moving and is put to sleep.

Jolt/Jolt.cmake

@@ -179,6 +179,7 @@ set(JOLT_PHYSICS_SRC_FILES
 	${JOLT_PHYSICS_ROOT}/Physics/Body/BodyManager.cpp
 	${JOLT_PHYSICS_ROOT}/Physics/Body/BodyManager.h
 	${JOLT_PHYSICS_ROOT}/Physics/Body/BodyPair.h
+	${JOLT_PHYSICS_ROOT}/Physics/Body/BodyType.h
 	${JOLT_PHYSICS_ROOT}/Physics/Body/MassProperties.cpp
 	${JOLT_PHYSICS_ROOT}/Physics/Body/MassProperties.h
 	${JOLT_PHYSICS_ROOT}/Physics/Body/MotionProperties.cpp
@@ -363,6 +364,15 @@ set(JOLT_PHYSICS_SRC_FILES
 	${JOLT_PHYSICS_ROOT}/Physics/PhysicsUpdateContext.h
 	${JOLT_PHYSICS_ROOT}/Physics/Ragdoll/Ragdoll.cpp
 	${JOLT_PHYSICS_ROOT}/Physics/Ragdoll/Ragdoll.h
+	${JOLT_PHYSICS_ROOT}/Physics/SoftBody/SoftBodyCreationSettings.cpp
+	${JOLT_PHYSICS_ROOT}/Physics/SoftBody/SoftBodyCreationSettings.h
+	${JOLT_PHYSICS_ROOT}/Physics/SoftBody/SoftBodyMotionProperties.h
+	${JOLT_PHYSICS_ROOT}/Physics/SoftBody/SoftBodyMotionProperties.cpp
+	${JOLT_PHYSICS_ROOT}/Physics/SoftBody/SoftBodyShape.cpp
+	${JOLT_PHYSICS_ROOT}/Physics/SoftBody/SoftBodyShape.h
+	${JOLT_PHYSICS_ROOT}/Physics/SoftBody/SoftBodySharedSettings.cpp
+	${JOLT_PHYSICS_ROOT}/Physics/SoftBody/SoftBodySharedSettings.h
+	${JOLT_PHYSICS_ROOT}/Physics/SoftBody/SoftBodyVertex.h
 	${JOLT_PHYSICS_ROOT}/Physics/StateRecorder.h
 	${JOLT_PHYSICS_ROOT}/Physics/StateRecorderImpl.cpp
 	${JOLT_PHYSICS_ROOT}/Physics/StateRecorderImpl.h
@@ -503,7 +513,7 @@ function(EMIT_X86_INSTRUCTION_SET_DEFINITIONS)
 	endif()
 	if (USE_AVX)
 		target_compile_definitions(Jolt PUBLIC JPH_USE_AVX)
-	endif()	
+	endif()
 	if (USE_SSE4_1)
 		target_compile_definitions(Jolt PUBLIC JPH_USE_SSE4_1)
 	endif()

Jolt/Physics/Body/Body.cpp

@@ -6,6 +6,7 @@
 
 #include <Jolt/Physics/Body/Body.h>
 #include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Physics/SoftBody/SoftBodyMotionProperties.h>
 #include <Jolt/Physics/PhysicsSettings.h>
 #include <Jolt/Physics/StateRecorder.h>
 #include <Jolt/Physics/Collision/Shape/SphereShape.h>
@@ -39,6 +40,7 @@ void Body::SetMotionType(EMotionType inMotionType)
 
 	JPH_ASSERT(inMotionType == EMotionType::Static || mMotionProperties != nullptr, "Body needs to be created with mAllowDynamicOrKinematic set tot true");
 	JPH_ASSERT(inMotionType != EMotionType::Static || !IsActive(), "Deactivate body first");
+	JPH_ASSERT(inMotionType == EMotionType::Dynamic || !IsSoftBody(), "Soft bodies can only be dynamic, you can make individual vertices kinematic by setting their inverse mass to 0");
 
 	// Store new motion type
 	mMotionType = inMotionType;
@@ -68,17 +70,18 @@ void Body::SetMotionType(EMotionType inMotionType)
 	}
 }
 
-void Body::SetAllowSleeping(bool inAllow)									
-{ 
-	mMotionProperties->mAllowSleeping = inAllow; 
-	if (inAllow) 
+void Body::SetAllowSleeping(bool inAllow)
+{
+	mMotionProperties->mAllowSleeping = inAllow;
+	if (inAllow)
 		ResetSleepTestSpheres();
 }
 
 void Body::MoveKinematic(RVec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime)
 {
+	JPH_ASSERT(IsRigidBody()); // Only valid for rigid bodies
 	JPH_ASSERT(!IsStatic());
-	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); 
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read));
 
 	// Calculate center of mass at end situation
 	RVec3 new_com = inTargetPosition + inTargetRotation * mShape->GetCenterOfMass();
@@ -95,12 +98,12 @@ void Body::CalculateWorldSpaceBoundsInternal()
 	mBounds = mShape->GetWorldSpaceBounds(GetCenterOfMassTransform(), Vec3::sReplicate(1.0f));
 }
 
-void Body::SetPositionAndRotationInternal(RVec3Arg inPosition, QuatArg inRotation) 
-{ 
-	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::ReadWrite)); 
+void Body::SetPositionAndRotationInternal(RVec3Arg inPosition, QuatArg inRotation)
+{
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::ReadWrite));
 
 	mPosition = inPosition + inRotation * mShape->GetCenterOfMass();
-	mRotation = inRotation; 
+	mRotation = inRotation;
 
 	// Initialize bounding box
 	CalculateWorldSpaceBoundsInternal();
@@ -122,7 +125,8 @@ void Body::UpdateCenterOfMassInternal(Vec3Arg inPreviousCenterOfMass, bool inUpd
 
 void Body::SetShapeInternal(const Shape *inShape, bool inUpdateMassProperties)
 {
-	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::ReadWrite)); 
+	JPH_ASSERT(IsRigidBody()); // Only valid for rigid bodies
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::ReadWrite));
 
 	// Get the old center of mass
 	Vec3 old_com = mShape->GetCenterOfMass();
@@ -183,6 +187,8 @@ bool Body::ApplyBuoyancyImpulse(RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNor
 {
 	JPH_PROFILE_FUNCTION();
 
+	JPH_ASSERT(IsRigidBody()); // Only implemented for rigid bodies currently
+
 	// We follow the approach from 'Game Programming Gems 6' 2.5 Exact Buoyancy for Polyhedra
 	// All quantities below are in world space
 
@@ -194,7 +200,7 @@ bool Body::ApplyBuoyancyImpulse(RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNor
 	float total_volume, submerged_volume;
 	Vec3 relative_center_of_buoyancy;
 	mShape->GetSubmergedVolume(rotation, Vec3::sReplicate(1.0f), surface_relative_to_body, total_volume, submerged_volume, relative_center_of_buoyancy JPH_IF_DEBUG_RENDERER(, mPosition));
-		
+
 	// If we're not submerged, there's no point in doing the rest of the calculations
 	if (submerged_volume > 0.0f)
 	{
@@ -288,7 +294,12 @@ void Body::SaveState(StateRecorder &inStream) const
 	inStream.Write(mMotionType);
 
 	if (mMotionProperties != nullptr)
-		mMotionProperties->SaveState(inStream);
+	{
+		if (IsSoftBody())
+			static_cast<const SoftBodyMotionProperties *>(mMotionProperties)->SaveState(inStream);
+		else
+			mMotionProperties->SaveState(inStream);
+	}
 }
 
 void Body::RestoreState(StateRecorder &inStream)
@@ -302,7 +313,11 @@ void Body::RestoreState(StateRecorder &inStream)
 
 	if (mMotionProperties != nullptr)
 	{
-		mMotionProperties->RestoreState(inStream);
+		if (IsSoftBody())
+			static_cast<SoftBodyMotionProperties *>(mMotionProperties)->RestoreState(inStream);
+		else
+			mMotionProperties->RestoreState(inStream);
+
 		JPH_IF_ENABLE_ASSERTS(mMotionProperties->mCachedMotionType = mMotionType);
 	}
 
@@ -312,6 +327,8 @@ void Body::RestoreState(StateRecorder &inStream)
 
 BodyCreationSettings Body::GetBodyCreationSettings() const
 {
+	JPH_ASSERT(IsRigidBody());
+
 	BodyCreationSettings result;
 
 	result.mPosition = GetPosition();

Jolt/Physics/Body/Body.h

@@ -14,6 +14,7 @@
 #include <Jolt/Physics/Body/MotionProperties.h>
 #include <Jolt/Physics/Body/BodyID.h>
 #include <Jolt/Physics/Body/BodyAccess.h>
+#include <Jolt/Physics/Body/BodyType.h>
 #include <Jolt/Core/StringTools.h>
 
 JPH_NAMESPACE_BEGIN
@@ -23,8 +24,8 @@ class BodyCreationSettings;
 
 /// A rigid body that can be simulated using the physics system
 ///
-/// Note that internally all properties (position, velocity etc.) are tracked relative to the center of mass of the object to simplify the simulation of the object. 
-/// 
+/// Note that internally all properties (position, velocity etc.) are tracked relative to the center of mass of the object to simplify the simulation of the object.
+///
 /// The offset between the position of the body and the center of mass position of the body is GetShape()->GetCenterOfMass().
 /// The functions that get/set the position of the body all indicate if they are relative to the center of mass or to the original position in which the shape was created.
 ///
@@ -37,12 +38,21 @@ public:
 	/// Default constructor
 							Body() = default;
 
-	/// Destructor							
+	/// Destructor
 							~Body()															{ JPH_ASSERT(mMotionProperties == nullptr); }
 
 	/// Get the id of this body
 	inline const BodyID &	GetID() const													{ return mID; }
 
+	/// Get the type of body (rigid or soft)
+	inline EBodyType		GetBodyType() const												{ return mBodyType; }
+
+	/// Check if this body is a rigid body
+	inline bool				IsRigidBody() const												{ return mBodyType == EBodyType::RigidBody; }
+
+	/// Check if this body is a soft body
+	inline bool				IsSoftBody() const												{ return mBodyType == EBodyType::SoftBody; }
+
 	/// If this body is currently actively simulating (true) or sleeping (false)
 	inline bool				IsActive() const												{ return mMotionProperties != nullptr && mMotionProperties->mIndexInActiveBodies != cInactiveIndex; }
 
@@ -63,20 +73,20 @@ public:
 	/// These sensors will only detect collisions with active Dynamic or Kinematic bodies. As soon as a body go to sleep, the contact point with the sensor will be lost.
 	/// If you make a sensor Dynamic or Kinematic and activate them, the sensor will be able to detect collisions with sleeping bodies too. An active sensor will never go to sleep automatically.
 	/// When you make a Dynamic or Kinematic sensor, make sure it is in an ObjectLayer that does not collide with Static bodies or other sensors to avoid extra overhead in the broad phase.
-	inline void				SetIsSensor(bool inIsSensor)									{ if (inIsSensor) mFlags.fetch_or(uint8(EFlags::IsSensor), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::IsSensor)), memory_order_relaxed); }
+	inline void				SetIsSensor(bool inIsSensor)									{ JPH_ASSERT(IsRigidBody()); if (inIsSensor) mFlags.fetch_or(uint8(EFlags::IsSensor), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::IsSensor)), memory_order_relaxed); }
 
 	/// Check if this body is a sensor.
 	inline bool				IsSensor() const												{ return (mFlags.load(memory_order_relaxed) & uint8(EFlags::IsSensor)) != 0; }
 
 	// If this sensor detects static objects entering it. Note that the sensor must be kinematic and active for it to detect static objects.
-	inline void				SetSensorDetectsStatic(bool inDetectsStatic)					{ if (inDetectsStatic) mFlags.fetch_or(uint8(EFlags::SensorDetectsStatic), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::SensorDetectsStatic)), memory_order_relaxed); }
+	inline void				SetSensorDetectsStatic(bool inDetectsStatic)					{ JPH_ASSERT(IsRigidBody()); if (inDetectsStatic) mFlags.fetch_or(uint8(EFlags::SensorDetectsStatic), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::SensorDetectsStatic)), memory_order_relaxed); }
 
 	/// Check if this sensor detects static objects entering it.
 	inline bool				SensorDetectsStatic() const										{ return (mFlags.load(memory_order_relaxed) & uint8(EFlags::SensorDetectsStatic)) != 0; }
 
 	/// If PhysicsSettings::mUseManifoldReduction is true, this allows turning off manifold reduction for this specific body. Manifold reduction by default will combine contacts that come from different SubShapeIDs (e.g. different triangles or different compound shapes).
 	/// If the application requires tracking exactly which SubShapeIDs are in contact, you can turn off manifold reduction. Note that this comes at a performance cost.
-	inline void				SetUseManifoldReduction(bool inUseReduction)					{ if (inUseReduction) mFlags.fetch_or(uint8(EFlags::UseManifoldReduction), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::UseManifoldReduction)), memory_order_relaxed); }
+	inline void				SetUseManifoldReduction(bool inUseReduction)					{ JPH_ASSERT(IsRigidBody()); if (inUseReduction) mFlags.fetch_or(uint8(EFlags::UseManifoldReduction), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::UseManifoldReduction)), memory_order_relaxed); }
 
 	/// Check if this body can use manifold reduction.
 	inline bool				GetUseManifoldReduction() const									{ return (mFlags.load(memory_order_relaxed) & uint8(EFlags::UseManifoldReduction)) != 0; }
@@ -89,7 +99,7 @@ public:
 	void					SetMotionType(EMotionType inMotionType);
 
 	/// Get broadphase layer, this determines in which broad phase sub-tree the object is placed
-	inline BroadPhaseLayer	GetBroadPhaseLayer() const										{ return mBroadPhaseLayer; }	
+	inline BroadPhaseLayer	GetBroadPhaseLayer() const										{ return mBroadPhaseLayer; }
 
 	/// Get object layer, this determines which other objects it collides with
 	inline ObjectLayer		GetObjectLayer() const											{ return mObjectLayer; }
@@ -167,7 +177,7 @@ public:
 
 	/// Add angular impulse in world space (unit: N m s)
 	inline void				AddAngularImpulse(Vec3Arg inAngularImpulse);
-	
+
 	/// Set velocity of body such that it will be positioned at inTargetPosition/Rotation in inDeltaTime seconds.
 	void					MoveKinematic(RVec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime);
 
@@ -245,8 +255,8 @@ public:
 	static inline bool		sFindCollidingPairsCanCollide(const Body &inBody1, const Body &inBody2);
 
 	/// Update position using an Euler step (used during position integrate & constraint solving)
-	inline void				AddPositionStep(Vec3Arg inLinearVelocityTimesDeltaTime)			{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::ReadWrite)); mPosition += mMotionProperties->LockTranslation(inLinearVelocityTimesDeltaTime); JPH_ASSERT(!mPosition.IsNaN()); }
-	inline void				SubPositionStep(Vec3Arg inLinearVelocityTimesDeltaTime) 		{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::ReadWrite)); mPosition -= mMotionProperties->LockTranslation(inLinearVelocityTimesDeltaTime); JPH_ASSERT(!mPosition.IsNaN()); }
+	inline void				AddPositionStep(Vec3Arg inLinearVelocityTimesDeltaTime)			{ JPH_ASSERT(IsRigidBody()); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::ReadWrite)); mPosition += mMotionProperties->LockTranslation(inLinearVelocityTimesDeltaTime); JPH_ASSERT(!mPosition.IsNaN()); }
+	inline void				SubPositionStep(Vec3Arg inLinearVelocityTimesDeltaTime) 		{ JPH_ASSERT(IsRigidBody()); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::ReadWrite)); mPosition -= mMotionProperties->LockTranslation(inLinearVelocityTimesDeltaTime); JPH_ASSERT(!mPosition.IsNaN()); }
 
 	/// Update rotation using an Euler step (using during position integrate & constraint solving)
 	inline void				AddRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime);
@@ -297,7 +307,7 @@ public:
 
 	///@}
 
-	static constexpr uint32	cInactiveIndex = uint32(-1);									///< Constant indicating that body is not active
+	static constexpr uint32	cInactiveIndex = MotionProperties::cInactiveIndex;				///< Constant indicating that body is not active
 
 private:
 	friend class BodyManager;
@@ -336,11 +346,12 @@ private:
 	ObjectLayer				mObjectLayer;													///< The collision layer this body belongs to (determines if two objects can collide)
 
 	// 1 byte aligned
+	EBodyType				mBodyType;														///< Type of body (rigid or soft)
 	BroadPhaseLayer			mBroadPhaseLayer;												///< The broad phase layer this body belongs to
 	EMotionType				mMotionType;													///< Type of motion (static, dynamic or kinematic)
 	atomic<uint8>			mFlags = 0;														///< See EFlags for possible flags
-	
-	// 121 bytes up to here (64-bit mode, single precision, 16-bit ObjectLayer)
+
+	// 122 bytes up to here (64-bit mode, single precision, 16-bit ObjectLayer)
 
 #if JPH_CPU_ADDRESS_BITS == 32
 	// Padding for mShape, mMotionProperties, mCollisionGroup.mGroupFilter being 4 instead of 8 bytes in 32 bit mode

Jolt/Physics/Body/Body.inl

@@ -8,21 +8,21 @@ JPH_NAMESPACE_BEGIN
 
 RMat44 Body::GetWorldTransform() const
 {
-	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); 
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read));
 
 	return RMat44::sRotationTranslation(mRotation, mPosition).PreTranslated(-mShape->GetCenterOfMass());
 }
 
 RMat44 Body::GetCenterOfMassTransform() const
 {
-	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); 
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read));
 
 	return RMat44::sRotationTranslation(mRotation, mPosition);
 }
 
 RMat44 Body::GetInverseCenterOfMassTransform() const
 {
-	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); 
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read));
 
 	return RMat44::sInverseRotationTranslation(mRotation, mPosition);
 }
@@ -32,19 +32,24 @@ inline static bool sIsValidSensorBodyPair(const Body &inSensor, const Body &inOt
 	// If the sensor is not an actual sensor then this is not a valid pair
 	if (!inSensor.IsSensor())
 		return false;
-	
+
 	if (inSensor.SensorDetectsStatic())
 		return !inOther.IsDynamic(); // If the other body is dynamic, the pair will be handled when the bodies are swapped, otherwise we'll detect the collision twice
-	else	
+	else
 		return inOther.IsKinematic(); // Only kinematic bodies are valid
 }
 
 inline bool Body::sFindCollidingPairsCanCollide(const Body &inBody1, const Body &inBody2)
 {
+	// Soft bodies collide later in the pipeline
+	JPH_ASSERT(!inBody1.IsSoftBody());
+	if (inBody2.IsSoftBody())
+		return false;
+
 	// One of these conditions must be true
 	// - One of the bodies must be dynamic to collide
 	// - A sensor can collide with non-dynamic bodies
-	if ((!inBody1.IsDynamic() && !inBody2.IsDynamic()) 
+	if ((!inBody1.IsDynamic() && !inBody2.IsDynamic())
 		&& !sIsValidSensorBodyPair(inBody1, inBody2)
 		&& !sIsValidSensorBodyPair(inBody2, inBody1))
 		return false;
@@ -61,15 +66,15 @@ inline bool Body::sFindCollidingPairsCanCollide(const Body &inBody1, const Body
 	//	- A is active and B will become active during this simulation step (4)
 	//	- A is active and B is active, we require a condition that makes A, B collide and B, A not (5)
 	//
-	// In order to implement this we use the index in the active body list and make use of the fact that 
+	// In order to implement this we use the index in the active body list and make use of the fact that
 	// a body not in the active list has Body.Index = 0xffffffff which is the highest possible value for an uint32.
 	//
 	// Because we know that A is active we know that A.Index != 0xffffffff:
 	// (1) Because A.Index != 0xffffffff, if A.Index = B.Index then A = B, so to collide A.Index != B.Index
 	// (2) A.Index != 0xffffffff, B.Index = 0xffffffff (because it's static and cannot be in the active list), so to collide A.Index != B.Index
 	// (3) A.Index != 0xffffffff, B.Index = 0xffffffff (because it's not yet active), so to collide A.Index != B.Index
-	// (4) A.Index != 0xffffffff, B.Index = 0xffffffff currently. But it can activate during the Broad/NarrowPhase step at which point it 
-	//     will be added to the end of the active list which will make B.Index > A.Index (this holds only true when we don't deactivate 
+	// (4) A.Index != 0xffffffff, B.Index = 0xffffffff currently. But it can activate during the Broad/NarrowPhase step at which point it
+	//     will be added to the end of the active list which will make B.Index > A.Index (this holds only true when we don't deactivate
 	//     bodies during the Broad/NarrowPhase step), so to collide A.Index < B.Index.
 	// (5) As tie breaker we can use the same condition A.Index < B.Index to collide, this means that if A, B collides then B, A won't
 	static_assert(Body::cInactiveIndex == 0xffffffff, "The algorithm below uses this value");
@@ -85,8 +90,9 @@ inline bool Body::sFindCollidingPairsCanCollide(const Body &inBody1, const Body
 }
 
 void Body::AddRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime)
-{ 
-	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::ReadWrite)); 
+{
+	JPH_ASSERT(IsRigidBody());
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::ReadWrite));
 
 	// This used to use the equation: d/dt R(t) = 1/2 * w(t) * R(t) so that R(t + dt) = R(t) + 1/2 * w(t) * R(t) * dt
 	// See: Appendix B of An Introduction to Physically Based Modeling: Rigid Body Simulation II-Nonpenetration Constraints
@@ -94,24 +100,25 @@ void Body::AddRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime)
 	// But this is a first order approximation and does not work well for kinematic ragdolls that are driven to a new
 	// pose if the poses differ enough. So now we split w(t) * dt into an axis and angle part and create a quaternion with it.
 	// Note that the resulting quaternion is normalized since otherwise numerical drift will eventually make the rotation non-normalized.
-	float len = inAngularVelocityTimesDeltaTime.Length(); 
-	if (len > 1.0e-6f) 
+	float len = inAngularVelocityTimesDeltaTime.Length();
+	if (len > 1.0e-6f)
 	{
-		mRotation = (Quat::sRotation(inAngularVelocityTimesDeltaTime / len, len) * mRotation).Normalized(); 
-		JPH_ASSERT(!mRotation.IsNaN()); 
+		mRotation = (Quat::sRotation(inAngularVelocityTimesDeltaTime / len, len) * mRotation).Normalized();
+		JPH_ASSERT(!mRotation.IsNaN());
 	}
 }
 
 void Body::SubRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime)
-{ 
-	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::ReadWrite)); 
+{
+	JPH_ASSERT(IsRigidBody());
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::ReadWrite));
 
 	// See comment at Body::AddRotationStep
-	float len = inAngularVelocityTimesDeltaTime.Length(); 
-	if (len > 1.0e-6f) 
+	float len = inAngularVelocityTimesDeltaTime.Length();
+	if (len > 1.0e-6f)
 	{
-		mRotation = (Quat::sRotation(inAngularVelocityTimesDeltaTime / len, -len) * mRotation).Normalized(); 
-		JPH_ASSERT(!mRotation.IsNaN()); 
+		mRotation = (Quat::sRotation(inAngularVelocityTimesDeltaTime / len, -len) * mRotation).Normalized();
+		JPH_ASSERT(!mRotation.IsNaN());
 	}
 }
 
@@ -159,7 +166,7 @@ void Body::AddAngularImpulse(Vec3Arg inAngularImpulse)
 
 void Body::GetSleepTestPoints(RVec3 *outPoints) const
 {
-	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); 
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read));
 
 	// Center of mass is the first position
 	outPoints[0] = mPosition;

Jolt/Physics/Body/BodyInterface.cpp

@@ -27,6 +27,17 @@ Body *BodyInterface::CreateBody(const BodyCreationSettings &inSettings)
 	return body;
 }
 
+Body *BodyInterface::CreateSoftBody(const SoftBodyCreationSettings &inSettings)
+{
+	Body *body = mBodyManager->AllocateSoftBody(inSettings);
+	if (!mBodyManager->AddBody(body))
+	{
+		mBodyManager->FreeBody(body);
+		return nullptr;
+	}
+	return body;
+}
+
 Body *BodyInterface::CreateBodyWithID(const BodyID &inBodyID, const BodyCreationSettings &inSettings)
 {
 	Body *body = mBodyManager->AllocateBody(inSettings);
@@ -38,11 +49,27 @@ Body *BodyInterface::CreateBodyWithID(const BodyID &inBodyID, const BodyCreation
 	return body;
 }
 
+Body *BodyInterface::CreateSoftBodyWithID(const BodyID &inBodyID, const SoftBodyCreationSettings &inSettings)
+{
+	Body *body = mBodyManager->AllocateSoftBody(inSettings);
+	if (!mBodyManager->AddBodyWithCustomID(body, inBodyID))
+	{
+		mBodyManager->FreeBody(body);
+		return nullptr;
+	}
+	return body;
+}
+
 Body *BodyInterface::CreateBodyWithoutID(const BodyCreationSettings &inSettings) const
 {
 	return mBodyManager->AllocateBody(inSettings);
 }
 
+Body *BodyInterface::CreateSoftBodyWithoutID(const SoftBodyCreationSettings &inSettings) const
+{
+	return mBodyManager->AllocateSoftBody(inSettings);
+}
+
 void BodyInterface::DestroyBodyWithoutID(Body *inBody) const
 {
 	mBodyManager->FreeBody(inBody);
@@ -108,7 +135,7 @@ void BodyInterface::RemoveBody(const BodyID &inBodyID)
 		// Deactivate body
 		if (body.IsActive())
 			mBodyManager->DeactivateBodies(&inBodyID, 1);
-	
+
 		// Remove from broadphase
 		BodyID id = inBodyID;
 		mBroadPhase->RemoveBodies(&id, 1);
@@ -130,6 +157,15 @@ BodyID BodyInterface::CreateAndAddBody(const BodyCreationSettings &inSettings, E
 	return b->GetID();
 }
 
+BodyID BodyInterface::CreateAndAddSoftBody(const SoftBodyCreationSettings &inSettings, EActivation inActivationMode)
+{
+	const Body *b = CreateSoftBody(inSettings);
+	if (b == nullptr)
+		return BodyID(); // Out of bodies
+	AddBody(b->GetID(), inActivationMode);
+	return b->GetID();
+}
+
 BodyInterface::AddState BodyInterface::AddBodiesPrepare(BodyID *ioBodies, int inNumber)
 {
 	return mBroadPhase->AddBodiesPrepare(ioBodies, inNumber);
@@ -789,7 +825,7 @@ void BodyInterface::SetMotionType(const BodyID &inBodyID, EMotionType inMotionTy
 		// Deactivate if we're making the body static
 		if (body.IsActive() && inMotionType == EMotionType::Static)
 			mBodyManager->DeactivateBodies(&inBodyID, 1);
-			
+
 		body.SetMotionType(inMotionType);
 
 		// Activate body if requested
@@ -798,6 +834,15 @@ void BodyInterface::SetMotionType(const BodyID &inBodyID, EMotionType inMotionTy
 	}
 }
 
+EBodyType BodyInterface::GetBodyType(const BodyID &inBodyID) const
+{
+	BodyLockRead lock(*mBodyLockInterface, inBodyID);
+	if (lock.Succeeded())
+		return lock.GetBody().GetBodyType();
+	else
+		return EBodyType::RigidBody;
+}
+
 EMotionType BodyInterface::GetMotionType(const BodyID &inBodyID) const
 {
 	BodyLockRead lock(*mBodyLockInterface, inBodyID);

Jolt/Physics/Body/BodyInterface.h

@@ -9,12 +9,14 @@
 #include <Jolt/Physics/Collision/ObjectLayer.h>
 #include <Jolt/Physics/Body/MotionType.h>
 #include <Jolt/Physics/Body/MotionQuality.h>
+#include <Jolt/Physics/Body/BodyType.h>
 #include <Jolt/Core/Reference.h>
 
 JPH_NAMESPACE_BEGIN
 
 class Body;
 class BodyCreationSettings;
+class SoftBodyCreationSettings;
 class BodyLockInterface;
 class BroadPhase;
 class BodyManager;
@@ -32,21 +34,31 @@ class JPH_EXPORT BodyInterface : public NonCopyable
 public:
 	/// Initialize the interface (should only be called by PhysicsSystem)
 	void						Init(BodyLockInterface &inBodyLockInterface, BodyManager &inBodyManager, BroadPhase &inBroadPhase) { mBodyLockInterface = &inBodyLockInterface; mBodyManager = &inBodyManager; mBroadPhase = &inBroadPhase; }
-	
-	/// Create a body
+
+	/// Create a rigid body
 	/// @return Created body or null when out of bodies
 	Body *						CreateBody(const BodyCreationSettings &inSettings);
-	
-	/// Create a body with specified ID. This function can be used if a simulation is to run in sync between clients or if a simulation needs to be restored exactly.
+
+	/// Create a soft body
+	/// @return Created body or null when out of bodies
+	Body *						CreateSoftBody(const SoftBodyCreationSettings &inSettings);
+
+	/// Create a rigid body with specified ID. This function can be used if a simulation is to run in sync between clients or if a simulation needs to be restored exactly.
 	/// The ID created on the server can be replicated to the client and used to create a deterministic simulation.
 	/// @return Created body or null when the body ID is invalid or a body of the same ID already exists.
 	Body *						CreateBodyWithID(const BodyID &inBodyID, const BodyCreationSettings &inSettings);
 
-	/// Advanced use only. Creates a body without specifying an ID. This body cannot be added to the physics system until it has been assigned a body ID.
+	/// Create a soft body with specified ID. See comments at CreateBodyWithID.
+	Body *						CreateSoftBodyWithID(const BodyID &inBodyID, const SoftBodyCreationSettings &inSettings);
+
+	/// Advanced use only. Creates a rigid body without specifying an ID. This body cannot be added to the physics system until it has been assigned a body ID.
 	/// This can be used to decouple allocation from registering the body. A call to CreateBodyWithoutID followed by AssignBodyID is equivalent to calling CreateBodyWithID.
 	/// @return Created body
 	Body *						CreateBodyWithoutID(const BodyCreationSettings &inSettings) const;
 
+	/// Advanced use only. Creates a body without specifying an ID. See comments at CreateBodyWithoutID.
+	Body *						CreateSoftBodyWithoutID(const SoftBodyCreationSettings &inSettings) const;
+
 	/// Advanced use only. Destroy a body previously created with CreateBodyWithoutID that hasn't gotten an ID yet through the AssignBodyID function,
 	/// or a body that has had its body ID unassigned through UnassignBodyIDs. Bodies that have an ID should be destroyed through DestroyBody.
 	void						DestroyBodyWithoutID(Body *inBody) const;
@@ -71,7 +83,7 @@ public:
 
 	/// Destroy a body
 	void						DestroyBody(const BodyID &inBodyID);
-	
+
 	/// Destroy multiple bodies
 	void						DestroyBodies(const BodyID *inBodyIDs, int inNumber);
 
@@ -80,10 +92,10 @@ public:
 	/// Adding many bodies, one at a time, results in a really inefficient broadphase until PhysicsSystem::OptimizeBroadPhase is called or when PhysicsSystem::Update rebuilds the tree!
 	/// After adding, to get a body by ID use the BodyLockRead or BodyLockWrite interface!
 	void						AddBody(const BodyID &inBodyID, EActivation inActivationMode);
-	
+
 	/// Remove body from the physics system.
 	void						RemoveBody(const BodyID &inBodyID);
-	
+
 	/// Check if a body has been added to the physics system.
 	bool						IsAdded(const BodyID &inBodyID) const;
 
@@ -91,6 +103,10 @@ public:
 	/// @return Created body ID or an invalid ID when out of bodies
 	BodyID						CreateAndAddBody(const BodyCreationSettings &inSettings, EActivation inActivationMode);
 
+	/// Combines CreateSoftBody and AddBody
+	/// @return Created body ID or an invalid ID when out of bodies
+	BodyID						CreateAndAddSoftBody(const SoftBodyCreationSettings &inSettings, EActivation inActivationMode);
+
 	/// Broadphase add state handle, used to keep track of a batch while ading to the broadphase.
 	using AddState = void *;
 
@@ -193,6 +209,11 @@ public:
 	void						AddAngularImpulse(const BodyID &inBodyID, Vec3Arg inAngularImpulse);
 	///@}
 
+	///@name Body type
+	///@{
+	EBodyType					GetBodyType(const BodyID &inBodyID) const;
+	///@}
+
 	///@name Body motion type
 	///@{
 	void						SetMotionType(const BodyID &inBodyID, EMotionType inMotionType, EActivation inActivationMode);

Jolt/Physics/Body/BodyManager.cpp

@@ -9,6 +9,9 @@
 #include <Jolt/Physics/Body/BodyCreationSettings.h>
 #include <Jolt/Physics/Body/BodyLock.h>
 #include <Jolt/Physics/Body/BodyActivationListener.h>
+#include <Jolt/Physics/SoftBody/SoftBodyMotionProperties.h>
+#include <Jolt/Physics/SoftBody/SoftBodyCreationSettings.h>
+#include <Jolt/Physics/SoftBody/SoftBodyShape.h>
 #include <Jolt/Physics/StateRecorder.h>
 #include <Jolt/Core/StringTools.h>
 #include <Jolt/Core/QuickSort.h>
@@ -50,7 +53,20 @@ class BodyWithMotionProperties : public Body
 public:
 	JPH_OVERRIDE_NEW_DELETE
 
-	MotionProperties		mMotionProperties;
+	MotionProperties			mMotionProperties;
+};
+
+// Helper class that combines a soft body its motion properties and shape
+class SoftBodyWithMotionPropertiesAndShape : public Body
+{
+public:
+								SoftBodyWithMotionPropertiesAndShape()
+	{
+		mShape.SetEmbedded();
+	}
+
+	SoftBodyMotionProperties	mMotionProperties;
+	SoftBodyShape				mShape;
 };
 
 inline void BodyManager::sDeleteBody(Body *inBody)
@@ -58,7 +74,13 @@ inline void BodyManager::sDeleteBody(Body *inBody)
 	if (inBody->mMotionProperties != nullptr)
 	{
 		JPH_IF_ENABLE_ASSERTS(inBody->mMotionProperties = nullptr;)
-		delete static_cast<BodyWithMotionProperties *>(inBody);
+		if (inBody->IsSoftBody())
+		{
+			inBody->mShape = nullptr; // Release the shape to avoid assertion on shape destruction because of embedded object with refcount > 0
+			delete static_cast<SoftBodyWithMotionPropertiesAndShape *>(inBody);
+		}
+		else
+			delete static_cast<BodyWithMotionProperties *>(inBody);
 	}
 	else
 		delete inBody;
@@ -73,7 +95,8 @@ BodyManager::~BodyManager()
 		if (sIsValidBodyPointer(b))
 			sDeleteBody(b);
 
-	delete [] mActiveBodies;
+	for (BodyID *active_bodies : mActiveBodies)
+		delete [] active_bodies;
 }
 
 void BodyManager::Init(uint inMaxBodies, uint inNumBodyMutexes, const BroadPhaseLayerInterface &inLayerInterface)
@@ -90,8 +113,11 @@ void BodyManager::Init(uint inMaxBodies, uint inNumBodyMutexes, const BroadPhase
 	mBodies.reserve(inMaxBodies);
 
 	// Allocate space for active bodies
-	JPH_ASSERT(mActiveBodies == nullptr);
-	mActiveBodies = new BodyID [inMaxBodies];
+	for (BodyID *&active_bodies : mActiveBodies)
+	{
+		JPH_ASSERT(active_bodies == nullptr);
+		active_bodies = new BodyID [inMaxBodies];
+	}
 
 	// Allocate space for sequence numbers
 	mBodySequenceNumbers.resize(inMaxBodies);
@@ -118,23 +144,32 @@ BodyManager::BodyStats BodyManager::GetBodyStats() const
 	for (const Body *body : mBodies)
 		if (sIsValidBodyPointer(body))
 		{
-			switch (body->GetMotionType())
+			if (body->IsSoftBody())
 			{
-			case EMotionType::Static:
-				stats.mNumBodiesStatic++;
-				break;
-
-			case EMotionType::Dynamic:
-				stats.mNumBodiesDynamic++;
+				stats.mNumSoftBodies++;
 				if (body->IsActive())
-					stats.mNumActiveBodiesDynamic++;
-				break;
+					stats.mNumActiveSoftBodies++;
+			}
+			else
+			{
+				switch (body->GetMotionType())
+				{
+				case EMotionType::Static:
+					stats.mNumBodiesStatic++;
+					break;
 
-			case EMotionType::Kinematic:
-				stats.mNumBodiesKinematic++;
-				if (body->IsActive())
-					stats.mNumActiveBodiesKinematic++;
-				break;
+				case EMotionType::Dynamic:
+					stats.mNumBodiesDynamic++;
+					if (body->IsActive())
+						stats.mNumActiveBodiesDynamic++;
+					break;
+
+				case EMotionType::Kinematic:
+					stats.mNumBodiesKinematic++;
+					if (body->IsActive())
+						stats.mNumActiveBodiesKinematic++;
+					break;
+				}
 			}
 		}
 
@@ -155,6 +190,7 @@ Body *BodyManager::AllocateBody(const BodyCreationSettings &inBodyCreationSettin
 	{
 	 	body = new Body;
 	}
+	body->mBodyType = EBodyType::RigidBody;
 	body->mShape = inBodyCreationSettings.GetShape();
 	body->mUserData = inBodyCreationSettings.mUserData;
 	body->SetFriction(inBodyCreationSettings.mFriction);
@@ -169,7 +205,7 @@ Body *BodyManager::AllocateBody(const BodyCreationSettings &inBodyCreationSettin
 	SetBodyObjectLayerInternal(*body, inBodyCreationSettings.mObjectLayer);
 	body->mObjectLayer = inBodyCreationSettings.mObjectLayer;
 	body->mCollisionGroup = inBodyCreationSettings.mCollisionGroup;
-	
+
 	if (inBodyCreationSettings.HasMassProperties())
 	{
 		MotionProperties *mp = body->mMotionProperties;
@@ -182,8 +218,7 @@ Body *BodyManager::AllocateBody(const BodyCreationSettings &inBodyCreationSettin
 		mp->SetGravityFactor(inBodyCreationSettings.mGravityFactor);
 		mp->mMotionQuality = inBodyCreationSettings.mMotionQuality;
 		mp->mAllowSleeping = inBodyCreationSettings.mAllowSleeping;
-		mp->mIndexInActiveBodies = Body::cInactiveIndex;
-		mp->mIslandIndex = Body::cInactiveIndex;
+		JPH_IF_ENABLE_ASSERTS(mp->mCachedBodyType = body->mBodyType;)
 		JPH_IF_ENABLE_ASSERTS(mp->mCachedMotionType = body->mMotionType;)
 		mp->SetMassProperties(inBodyCreationSettings.mAllowedDOFs, inBodyCreationSettings.GetMassProperties());
 	}
@@ -194,6 +229,43 @@ Body *BodyManager::AllocateBody(const BodyCreationSettings &inBodyCreationSettin
 	return body;
 }
 
+/// Create a soft body using creation settings. The returned body will not be part of the body manager yet.
+Body *BodyManager::AllocateSoftBody(const SoftBodyCreationSettings &inSoftBodyCreationSettings) const
+{
+	// Fill in basic properties
+	SoftBodyWithMotionPropertiesAndShape *bmp = new SoftBodyWithMotionPropertiesAndShape;
+	SoftBodyMotionProperties *mp = &bmp->mMotionProperties;
+	SoftBodyShape *shape = &bmp->mShape;
+	Body *body = bmp;
+	shape->mSoftBodyMotionProperties = mp;
+	body->mBodyType = EBodyType::SoftBody;
+	body->mMotionProperties = mp;
+	body->mShape = shape;
+	body->mUserData = inSoftBodyCreationSettings.mUserData;
+	body->SetFriction(inSoftBodyCreationSettings.mFriction);
+	body->SetRestitution(inSoftBodyCreationSettings.mRestitution);
+	body->mMotionType = EMotionType::Dynamic;
+	SetBodyObjectLayerInternal(*body, inSoftBodyCreationSettings.mObjectLayer);
+	body->mObjectLayer = inSoftBodyCreationSettings.mObjectLayer;
+	body->mCollisionGroup = inSoftBodyCreationSettings.mCollisionGroup;
+	mp->SetLinearDamping(inSoftBodyCreationSettings.mLinearDamping);
+	mp->SetAngularDamping(0);
+	mp->SetMaxLinearVelocity(FLT_MAX);
+	mp->SetMaxAngularVelocity(FLT_MAX);
+	mp->SetLinearVelocity(Vec3::sZero());
+	mp->SetAngularVelocity(Vec3::sZero());
+	mp->SetGravityFactor(inSoftBodyCreationSettings.mGravityFactor);
+	mp->mMotionQuality = EMotionQuality::Discrete;
+	mp->mAllowSleeping = false;
+	JPH_IF_ENABLE_ASSERTS(mp->mCachedBodyType = body->mBodyType;)
+	JPH_IF_ENABLE_ASSERTS(mp->mCachedMotionType = body->mMotionType;)
+	mp->Initialize(inSoftBodyCreationSettings);
+
+	body->SetPositionAndRotationInternal(inSoftBodyCreationSettings.mPosition, inSoftBodyCreationSettings.mMakeRotationIdentity? Quat::sIdentity() : inSoftBodyCreationSettings.mRotation);
+
+	return body;
+}
+
 void BodyManager::FreeBody(Body *inBody) const
 {
 	JPH_ASSERT(inBody->GetID().IsInvalid(), "This function should only be called on a body that doesn't have an ID yet, use DestroyBody otherwise");
@@ -326,7 +398,7 @@ Body *BodyManager::RemoveBodyInternal(const BodyID &inBodyID)
 	JPH_ASSERT(body->GetID() == inBodyID);
 	JPH_ASSERT(!body->IsActive());
 	JPH_ASSERT(!body->IsInBroadPhase());
-	
+
 	// Push the id onto the freelist
 	mBodies[idx] = (Body *)mBodyIDFreeListStart;
 	mBodyIDFreeListStart = (uintptr_t(idx) << cFreedBodyIndexShift) | cIsFreedBody;
@@ -416,7 +488,7 @@ void BodyManager::ActivateBodies(const BodyID *inBodyIDs, int inNumber)
 		return;
 
 	UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList));
-	
+
 	JPH_ASSERT(!mActiveBodiesLocked || sOverrideAllowActivation);
 
 	for (const BodyID *b = inBodyIDs, *b_end = inBodyIDs + inNumber; b < b_end; b++)
@@ -431,11 +503,16 @@ void BodyManager::ActivateBodies(const BodyID *inBodyIDs, int inNumber)
 			if (!body.IsStatic()
 				&& body.mMotionProperties->mIndexInActiveBodies == Body::cInactiveIndex)
 			{
-				body.mMotionProperties->mIndexInActiveBodies = mNumActiveBodies;
+				// Select the correct array to use
+				int type = (int)body.GetBodyType();
+				atomic<uint32> &num_active_bodies = mNumActiveBodies[type];
+				BodyID *active_bodies = mActiveBodies[type];
+
+				body.mMotionProperties->mIndexInActiveBodies = num_active_bodies;
 				body.ResetSleepTestSpheres();
-				JPH_ASSERT(mNumActiveBodies < GetMaxBodies());
-				mActiveBodies[mNumActiveBodies] = body_id;
-				mNumActiveBodies++; // Increment atomic after setting the body ID so that PhysicsSystem::JobFindCollisions (which doesn't lock the mActiveBodiesMutex) will only read valid IDs
+				JPH_ASSERT(num_active_bodies < GetMaxBodies());
+				active_bodies[num_active_bodies] = body_id;
+				num_active_bodies++; // Increment atomic after setting the body ID so that PhysicsSystem::JobFindCollisions (which doesn't lock the mActiveBodiesMutex) will only read valid IDs
 
 				// Count CCD bodies
 				if (body.mMotionProperties->GetMotionQuality() == EMotionQuality::LinearCast)
@@ -470,12 +547,17 @@ void BodyManager::DeactivateBodies(const BodyID *inBodyIDs, int inNumber)
 			if (body.mMotionProperties != nullptr
 				&& body.mMotionProperties->mIndexInActiveBodies != Body::cInactiveIndex)
 			{
-				uint32 last_body_index = mNumActiveBodies - 1;
+				// Select the correct array to use
+				int type = (int)body.GetBodyType();
+				atomic<uint32> &num_active_bodies = mNumActiveBodies[type];
+				BodyID *active_bodies = mActiveBodies[type];
+
+				uint32 last_body_index = num_active_bodies - 1;
 				if (body.mMotionProperties->mIndexInActiveBodies != last_body_index)
 				{
 					// This is not the last body, use the last body to fill the hole
-					BodyID last_body_id = mActiveBodies[last_body_index];
-					mActiveBodies[body.mMotionProperties->mIndexInActiveBodies] = last_body_id;
+					BodyID last_body_id = active_bodies[last_body_index];
+					active_bodies[body.mMotionProperties->mIndexInActiveBodies] = last_body_id;
 
 					// Update that body's index in the active list
 					Body &last_body = *mBodies[last_body_id.GetIndex()];
@@ -492,7 +574,7 @@ void BodyManager::DeactivateBodies(const BodyID *inBodyIDs, int inNumber)
 				body.mMotionProperties->mAngularVelocity = Vec3::sZero();
 
 				// Remove unused element from active bodies list
-				--mNumActiveBodies;
+				--num_active_bodies;
 
 				// Count CCD bodies
 				if (body.mMotionProperties->GetMotionQuality() == EMotionQuality::LinearCast)
@@ -525,13 +607,14 @@ void BodyManager::SetMotionQuality(Body &ioBody, EMotionQuality inMotionQuality)
 	}
 }
 
-void BodyManager::GetActiveBodies(BodyIDVector &outBodyIDs) const
+void BodyManager::GetActiveBodies(EBodyType inType, BodyIDVector &outBodyIDs) const
 {
 	JPH_PROFILE_FUNCTION();
 
 	UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList));
 
-	outBodyIDs.assign(mActiveBodies, mActiveBodies + mNumActiveBodies);
+	const BodyID *active_bodies = mActiveBodies[(int)inType];
+	outBodyIDs.assign(active_bodies, active_bodies + mNumActiveBodies[(int)inType]);
 }
 
 void BodyManager::GetBodyIDs(BodyIDVector &outBodies) const
@@ -553,11 +636,11 @@ void BodyManager::GetBodyIDs(BodyIDVector &outBodies) const
 	JPH_ASSERT(outBodies.size() == mNumBodies);
 }
 
-void BodyManager::SetBodyActivationListener(BodyActivationListener *inListener)	
-{ 
+void BodyManager::SetBodyActivationListener(BodyActivationListener *inListener)
+{
 	UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList));
 
-	mActivationListener = inListener; 
+	mActivationListener = inListener;
 }
 
 BodyManager::MutexMask BodyManager::GetMutexMask(const BodyID *inBodies, int inNumber) const
@@ -622,20 +705,20 @@ void BodyManager::UnlockWrite(MutexMask inMutexMask) const
 			mBodyMutexes.GetMutexByIndex(index).unlock();
 }
 
-void BodyManager::LockAllBodies() const						
-{ 
+void BodyManager::LockAllBodies() const
+{
 	JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckLock(this, EPhysicsLockTypes::PerBody));
-	mBodyMutexes.LockAll(); 
+	mBodyMutexes.LockAll();
 
 	PhysicsLock::sLock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList));
 }
 
-void BodyManager::UnlockAllBodies() const						
-{ 
+void BodyManager::UnlockAllBodies() const
+{
 	PhysicsLock::sUnlock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList));
 
 	JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckUnlock(this, EPhysicsLockTypes::PerBody));
-	mBodyMutexes.UnlockAll(); 
+	mBodyMutexes.UnlockAll();
 }
 
 void BodyManager::SaveState(StateRecorder &inStream) const
@@ -649,7 +732,7 @@ void BodyManager::SaveState(StateRecorder &inStream) const
 			if (sIsValidBodyPointer(b) && b->IsInBroadPhase())
 				++num_bodies;
 		inStream.Write(num_bodies);
-	
+
 		// Write state of bodies
 		for (const Body *b : mBodies)
 			if (sIsValidBodyPointer(b) && b->IsInBroadPhase())
@@ -664,14 +747,19 @@ void BodyManager::SaveState(StateRecorder &inStream) const
 	{
 		UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList));
 
-		// Write active bodies, sort because activation can come from multiple threads, so order is not deterministic
-		inStream.Write(mNumActiveBodies);
-		BodyIDVector sorted_active_bodies(mActiveBodies, mActiveBodies + mNumActiveBodies);
-		QuickSort(sorted_active_bodies.begin(), sorted_active_bodies.end());
-		for (const BodyID &id : sorted_active_bodies)
-			inStream.Write(id);
-
-		inStream.Write(mNumActiveCCDBodies);
+		// Loop over the body types
+		for (uint type = 0; type < cBodyTypeCount; ++type)
+		{
+			const atomic<uint32> &num_active_bodies = mNumActiveBodies[type];
+			const BodyID *active_bodies = mActiveBodies[type];
+
+			// Write active bodies, sort because activation can come from multiple threads, so order is not deterministic
+			inStream.Write(num_active_bodies);
+			BodyIDVector sorted_active_bodies(active_bodies, active_bodies + num_active_bodies);
+			QuickSort(sorted_active_bodies.begin(), sorted_active_bodies.end());
+			for (const BodyID &id : sorted_active_bodies)
+				inStream.Write(id);
+		}
 	}
 }
 
@@ -714,21 +802,32 @@ bool BodyManager::RestoreState(StateRecorder &inStream)
 	{
 		UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList));
 
-		// Mark current active bodies as deactivated
-		for (const BodyID *id = mActiveBodies, *id_end = mActiveBodies + mNumActiveBodies; id < id_end; ++id)
-			mBodies[id->GetIndex()]->mMotionProperties->mIndexInActiveBodies = Body::cInactiveIndex;
+		// Loop over the body types
+		for (uint type = 0; type < cBodyTypeCount; ++type)
+		{
+			atomic<uint32> &num_active_bodies = mNumActiveBodies[type];
+			BodyID *active_bodies = mActiveBodies[type];
 
-		QuickSort(mActiveBodies, mActiveBodies + mNumActiveBodies); // Sort for validation
+			// Mark current active bodies as deactivated
+			for (const BodyID *id = active_bodies, *id_end = active_bodies + num_active_bodies; id < id_end; ++id)
+				mBodies[id->GetIndex()]->mMotionProperties->mIndexInActiveBodies = Body::cInactiveIndex;
 
-		// Read active bodies
-		inStream.Read(mNumActiveBodies);
-		for (BodyID *id = mActiveBodies, *id_end = mActiveBodies + mNumActiveBodies; id < id_end; ++id)
-		{
-			inStream.Read(*id);
-			mBodies[id->GetIndex()]->mMotionProperties->mIndexInActiveBodies = uint32(id - mActiveBodies);
+			QuickSort(active_bodies, active_bodies + num_active_bodies); // Sort for validation
+
+			// Read active bodies
+			inStream.Read(num_active_bodies);
+			for (BodyID *id = active_bodies, *id_end = active_bodies + num_active_bodies; id < id_end; ++id)
+			{
+				inStream.Read(*id);
+				mBodies[id->GetIndex()]->mMotionProperties->mIndexInActiveBodies = uint32(id - active_bodies);
+			}
 		}
 
-		inStream.Read(mNumActiveCCDBodies);
+		// Count CCD bodies
+		mNumActiveCCDBodies = 0;
+		for (const BodyID *id = mActiveBodies[(int)EBodyType::RigidBody], *end = id + mNumActiveBodies[(int)EBodyType::RigidBody]; id < end; ++id)
+			if (mBodies[id->GetIndex()]->GetMotionProperties()->GetMotionQuality() == EMotionQuality::LinearCast)
+				mNumActiveCCDBodies++;
 	}
 
 	return true;
@@ -842,7 +941,7 @@ void BodyManager::Draw(const DrawSettings &inDrawSettings, const PhysicsSettings
 					color = Color::sBlack;
 					break;
 				}
-			
+
 			// Draw the results of GetSupportFunction
 			if (inDrawSettings.mDrawGetSupportFunction)
 				body->mShape->DrawGetSupportFunction(inRenderer, body->GetCenterOfMassTransform(), Vec3::sReplicate(1.0f), color, inDrawSettings.mDrawSupportDirection);
@@ -908,6 +1007,24 @@ void BodyManager::Draw(const DrawSettings &inDrawSettings, const PhysicsSettings
 				for (int i = 0; i < 3; ++i)
 					inRenderer->DrawWireSphere(JPH_IF_DOUBLE_PRECISION(body->mMotionProperties->GetSleepTestOffset() +) body->mMotionProperties->mSleepTestSpheres[i].GetCenter(), body->mMotionProperties->mSleepTestSpheres[i].GetRadius(), sleep_color);
 			}
+
+			if (body->IsSoftBody())
+			{
+				const SoftBodyMotionProperties *mp = static_cast<const SoftBodyMotionProperties *>(body->GetMotionProperties());
+				RMat44 com = body->GetCenterOfMassTransform();
+
+				if (inDrawSettings.mDrawSoftBodyVertices)
+					mp->DrawVertices(inRenderer, com);
+
+				if (inDrawSettings.mDrawSoftBodyEdgeConstraints)
+					mp->DrawEdgeConstraints(inRenderer, com);
+
+				if (inDrawSettings.mDrawSoftBodyVolumeConstraints)
+					mp->DrawVolumeConstraints(inRenderer, com);
+
+				if (inDrawSettings.mDrawSoftBodyPredictedBounds)
+					mp->DrawPredictedBounds(inRenderer, com);
+			}
 		}
 
 	UnlockAllBodies();
@@ -927,7 +1044,7 @@ void BodyManager::InvalidateContactCacheForBody(Body &ioBody)
 void BodyManager::ValidateContactCacheForAllBodies()
 {
 	lock_guard lock(mBodiesCacheInvalidMutex);
-	
+
 	for (const BodyID &b : mBodiesCacheInvalid)
 	{
 		// The body may have been removed between the call to InvalidateContactCacheForBody and this call, so check if it still exists
@@ -943,13 +1060,14 @@ void BodyManager::ValidateActiveBodyBounds()
 {
 	UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList));
 
-	for (BodyID *id = mActiveBodies, *id_end = mActiveBodies + mNumActiveBodies; id < id_end; ++id)
-	{
-		const Body *body = mBodies[id->GetIndex()];
-		AABox cached = body->GetWorldSpaceBounds();
-		AABox calculated = body->GetShape()->GetWorldSpaceBounds(body->GetCenterOfMassTransform(), Vec3::sReplicate(1.0f));
-		JPH_ASSERT(cached == calculated);
-	}
+	for (uint type = 0; type < cBodyTypeCount; ++type)
+		for (BodyID *id = mActiveBodies[type], *id_end = mActiveBodies[type] + mNumActiveBodies[type]; id < id_end; ++id)
+		{
+			const Body *body = mBodies[id->GetIndex()];
+			AABox cached = body->GetWorldSpaceBounds();
+			AABox calculated = body->GetShape()->GetWorldSpaceBounds(body->GetCenterOfMassTransform(), Vec3::sReplicate(1.0f));
+			JPH_ASSERT(cached == calculated);
+		}
 }
 #endif // _DEBUG
 

Jolt/Physics/Body/BodyManager.h

@@ -12,6 +12,7 @@ JPH_NAMESPACE_BEGIN
 
 // Classes
 class BodyCreationSettings;
+class SoftBodyCreationSettings;
 class BodyActivationListener;
 struct PhysicsSettings;
 #ifdef JPH_DEBUG_RENDERER
@@ -56,6 +57,9 @@ public:
 
 		uint						mNumBodiesKinematic			= 0;			///< Number of kinematic bodies
 		uint						mNumActiveBodiesKinematic	= 0;			///< Number of kinematic bodies that are currently active
+
+		uint						mNumSoftBodies				= 0;			///< Number of soft bodies
+		uint						mNumActiveSoftBodies		= 0;			///< Number of soft bodies that are currently active
 	};
 
 	/// Get stats about the bodies in the body manager (slow, iterates through all bodies)
@@ -64,6 +68,9 @@ public:
 	/// Create a body using creation settings. The returned body will not be part of the body manager yet.
 	Body *							AllocateBody(const BodyCreationSettings &inBodyCreationSettings) const;
 
+	/// Create a soft body using creation settings. The returned body will not be part of the body manager yet.
+	Body *							AllocateSoftBody(const SoftBodyCreationSettings &inSoftBodyCreationSettings) const;
+
 	/// Free a body that has not been added to the body manager yet (if it has, use DestroyBodies).
 	void							FreeBody(Body *inBody) const;
 
@@ -91,13 +98,13 @@ public:
 	void							SetMotionQuality(Body &ioBody, EMotionQuality inMotionQuality);
 
 	/// Get copy of the list of active bodies under protection of a lock.
-	void							GetActiveBodies(BodyIDVector &outBodyIDs) const;
+	void							GetActiveBodies(EBodyType inType, BodyIDVector &outBodyIDs) const;
 
 	/// Get the list of active bodies. Note: Not thread safe. The active bodies list can change at any moment.
-	const BodyID *					GetActiveBodiesUnsafe() const				{ return mActiveBodies; }
+	const BodyID *					GetActiveBodiesUnsafe(EBodyType inType) const { return mActiveBodies[(int)inType]; }
 
 	/// Get the number of active bodies.
-	uint32							GetNumActiveBodies() const					{ return mNumActiveBodies; }
+	uint32							GetNumActiveBodies(EBodyType inType) const	{ return mNumActiveBodies[(int)inType]; }
 
 	/// Get the number of active bodies that are using continuous collision detection
 	uint32							GetNumActiveCCDBodies() const				{ return mNumActiveCCDBodies; }
@@ -157,7 +164,7 @@ public:
 
 	/// Bodies are protected using an array of mutexes (so a fixed number, not 1 per body). Each bit in this mask indicates a locked mutex.
 	using MutexMask = uint64;
-	
+
 	///@name Batch body mutex access (do not use directly)
 	///@{
 	MutexMask						GetAllBodiesMutexMask() const				{ return mBodyMutexes.GetNumMutexes() == sizeof(MutexMask) * 8? ~MutexMask(0) : (MutexMask(1) << mBodyMutexes.GetNumMutexes()) - 1; }
@@ -203,7 +210,7 @@ public:
 	/// Draw settings
 	struct DrawSettings
 	{
-		bool						mDrawGetSupportFunction = false;				///< Draw the GetSupport() function, used for convex collision detection	
+		bool						mDrawGetSupportFunction = false;				///< Draw the GetSupport() function, used for convex collision detection
 		bool						mDrawSupportDirection = false;					///< When drawing the support function, also draw which direction mapped to a specific support point
 		bool						mDrawGetSupportingFace = false;					///< Draw the faces that were found colliding during collision detection
 		bool						mDrawShape = true;								///< Draw the shapes of all bodies
@@ -215,6 +222,10 @@ public:
 		bool						mDrawVelocity = false;							///< Draw the velocity vector for each body
 		bool						mDrawMassAndInertia = false;					///< Draw the mass and inertia (as the box equivalent) for each body
 		bool						mDrawSleepStats = false;						///< Draw stats regarding the sleeping algorithm of each body
+		bool						mDrawSoftBodyVertices = false;					///< Draw the vertices of soft bodies
+		bool						mDrawSoftBodyEdgeConstraints = false;			///< Draw the edge constraints of soft bodies
+		bool						mDrawSoftBodyVolumeConstraints = false;			///< Draw the volume constraints of soft bodies
+		bool						mDrawSoftBodyPredictedBounds = false;			///< Draw the predicted bounds of soft bodies
 	};
 
 	/// Draw the state of the bodies (debugging purposes)
@@ -288,7 +299,7 @@ private:
 	uintptr_t						mBodyIDFreeListStart = cBodyIDFreeListEnd;
 
 	/// Protects mBodies array (but not the bodies it points to), mNumBodies and mBodyIDFreeListStart
-	mutable Mutex					mBodiesMutex; 
+	mutable Mutex					mBodiesMutex;
 
 	/// An array of mutexes protecting the bodies in the mBodies array
 	using BodyMutexes = MutexArray<SharedMutex>;
@@ -301,10 +312,10 @@ private:
 	mutable Mutex					mActiveBodiesMutex;
 
 	/// List of all active dynamic bodies (size is equal to max amount of bodies)
-	BodyID *						mActiveBodies = nullptr;
+	BodyID *						mActiveBodies[cBodyTypeCount] = { };
 
 	/// How many bodies there are in the list of active bodies
-	atomic<uint32>					mNumActiveBodies = 0;
+	atomic<uint32>					mNumActiveBodies[cBodyTypeCount] = { };
 
 	/// How many of the active bodies have continuous collision detection enabled
 	uint32							mNumActiveCCDBodies = 0;

Jolt/Physics/Body/BodyType.h

@@ -0,0 +1,19 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+/// Type of body
+enum class EBodyType : uint8
+{
+	RigidBody,				///< Rigid body consisting of a rigid shape
+	SoftBody,				///< Soft body consisting of a deformable shape
+};
+
+/// How many types of bodies there are
+static constexpr uint cBodyTypeCount = 2;
+
+JPH_NAMESPACE_END

Jolt/Physics/Body/MotionProperties.h

@@ -9,6 +9,7 @@
 #include <Jolt/Physics/Body/MotionQuality.h>
 #include <Jolt/Physics/Body/BodyAccess.h>
 #include <Jolt/Physics/Body/MotionType.h>
+#include <Jolt/Physics/Body/BodyType.h>
 #include <Jolt/Physics/Body/MassProperties.h>
 #include <Jolt/Physics/DeterminismLog.h>
 
@@ -55,7 +56,7 @@ public:
 	/// Maximum linear velocity that a body can achieve. Used to prevent the system from exploding.
 	inline float			GetMaxLinearVelocity() const									{ return mMaxLinearVelocity; }
 	inline void				SetMaxLinearVelocity(float inLinearVelocity)					{ JPH_ASSERT(inLinearVelocity >= 0.0f); mMaxLinearVelocity = inLinearVelocity; }
-	
+
 	/// Maximum angular velocity that a body can achieve. Used to prevent the system from exploding.
 	inline float			GetMaxAngularVelocity() const									{ return mMaxAngularVelocity; }
 	inline void				SetMaxAngularVelocity(float inAngularVelocity)					{ JPH_ASSERT(inAngularVelocity >= 0.0f); mMaxAngularVelocity = inAngularVelocity; }
@@ -172,6 +173,8 @@ public:
 	/// Restoring state for replay
 	void					RestoreState(StateRecorder &inStream);
 
+	static constexpr uint32	cInactiveIndex = uint32(-1);									///< Constant indicating that body is not active
+
 private:
 	friend class BodyManager;
 	friend class Body;
@@ -193,13 +196,13 @@ private:
 	float					mMaxLinearVelocity;												///< Maximum linear velocity that this body can reach (m/s)
 	float					mMaxAngularVelocity;											///< Maximum angular velocity that this body can reach (rad/s)
 	float					mGravityFactor;													///< Factor to multiply gravity with
-	uint32					mIndexInActiveBodies;											///< If the body is active, this is the index in the active body list or cInactiveIndex if it is not active
-	uint32					mIslandIndex;													///< Index of the island that this body is part of, when the body has not yet been updated or is not active this is cInactiveIndex 
+	uint32					mIndexInActiveBodies = cInactiveIndex;							///< If the body is active, this is the index in the active body list or cInactiveIndex if it is not active (note that there are 2 lists, one for rigid and one for soft bodies)
+	uint32					mIslandIndex = cInactiveIndex;									///< Index of the island that this body is part of, when the body has not yet been updated or is not active this is cInactiveIndex
 
 	// 1 byte aligned
 	EMotionQuality			mMotionQuality;													///< Motion quality, or how well it detects collisions when it has a high velocity
 	bool					mAllowSleeping;													///< If this body can go to sleep
-	EAllowedDOFs			mAllowedDOFs;													///< Allowed degrees of freedom for this body
+	EAllowedDOFs			mAllowedDOFs = EAllowedDOFs::All;								///< Allowed degrees of freedom for this body
 
 	// 3rd cache line (least frequently used)
 	// 4 byte aligned (or 8 byte if running in double precision)
@@ -210,6 +213,7 @@ private:
 	float					mSleepTestTimer;												///< How long this body has been within the movement tolerance
 
 #ifdef JPH_ENABLE_ASSERTS
+	EBodyType				mCachedBodyType;												///< Copied from Body::mBodyType and cached for asserting purposes
 	EMotionType				mCachedMotionType;												///< Copied from Body::mMotionType and cached for asserting purposes
 #endif
 };

Jolt/Physics/Body/MotionProperties.inl

@@ -8,8 +8,9 @@ JPH_NAMESPACE_BEGIN
 
 void MotionProperties::MoveKinematic(Vec3Arg inDeltaPosition, QuatArg inDeltaRotation, float inDeltaTime)
 {
-	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); 
-	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); 
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite));
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read));
+	JPH_ASSERT(mCachedBodyType == EBodyType::RigidBody);
 	JPH_ASSERT(mCachedMotionType != EMotionType::Static);
 
 	// Calculate required linear velocity
@@ -23,27 +24,27 @@ void MotionProperties::MoveKinematic(Vec3Arg inDeltaPosition, QuatArg inDeltaRot
 }
 
 void MotionProperties::ClampLinearVelocity()
-{ 
-	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); 
+{
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite));
 
-	float len_sq = mLinearVelocity.LengthSq(); 
-	JPH_ASSERT(isfinite(len_sq)); 
-	if (len_sq > Square(mMaxLinearVelocity)) 
-		mLinearVelocity *= mMaxLinearVelocity / sqrt(len_sq); 
+	float len_sq = mLinearVelocity.LengthSq();
+	JPH_ASSERT(isfinite(len_sq));
+	if (len_sq > Square(mMaxLinearVelocity))
+		mLinearVelocity *= mMaxLinearVelocity / sqrt(len_sq);
 }
 
 void MotionProperties::ClampAngularVelocity()
-{ 
-	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); 
+{
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite));
 
-	float len_sq = mAngularVelocity.LengthSq(); 
-	JPH_ASSERT(isfinite(len_sq)); 
-	if (len_sq > Square(mMaxAngularVelocity)) 
-		mAngularVelocity *= mMaxAngularVelocity / sqrt(len_sq); 
+	float len_sq = mAngularVelocity.LengthSq();
+	JPH_ASSERT(isfinite(len_sq));
+	if (len_sq > Square(mMaxAngularVelocity))
+		mAngularVelocity *= mMaxAngularVelocity / sqrt(len_sq);
 }
 
 inline Mat44 MotionProperties::GetLocalSpaceInverseInertiaUnchecked() const
-{ 
+{
 	Mat44 rotation = Mat44::sRotation(mInertiaRotation);
 	Mat44 rotation_mul_scale_transposed(mInvInertiaDiagonal.SplatX() * rotation.GetColumn4(0), mInvInertiaDiagonal.SplatY() * rotation.GetColumn4(1), mInvInertiaDiagonal.SplatZ() * rotation.GetColumn4(2), Vec4(0, 0, 0, 1));
 	return rotation.Multiply3x3RightTransposed(rotation_mul_scale_transposed);
@@ -56,25 +57,26 @@ inline Mat44 MotionProperties::GetLocalSpaceInverseInertia() const
 }
 
 Mat44 MotionProperties::GetInverseInertiaForRotation(Mat44Arg inRotation) const
-{ 
+{
 	JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);
 
-	Mat44 rotation = inRotation * Mat44::sRotation(mInertiaRotation); 
+	Mat44 rotation = inRotation * Mat44::sRotation(mInertiaRotation);
 	Mat44 rotation_mul_scale_transposed(mInvInertiaDiagonal.SplatX() * rotation.GetColumn4(0), mInvInertiaDiagonal.SplatY() * rotation.GetColumn4(1), mInvInertiaDiagonal.SplatZ() * rotation.GetColumn4(2), Vec4(0, 0, 0, 1));
 	return rotation.Multiply3x3RightTransposed(rotation_mul_scale_transposed);
 }
 
 Vec3 MotionProperties::MultiplyWorldSpaceInverseInertiaByVector(QuatArg inBodyRotation, Vec3Arg inV) const
-{ 
+{
 	JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);
 
-	Mat44 rotation = Mat44::sRotation(inBodyRotation * mInertiaRotation); 
-	return rotation.Multiply3x3(mInvInertiaDiagonal * rotation.Multiply3x3Transposed(inV)); 
+	Mat44 rotation = Mat44::sRotation(inBodyRotation * mInertiaRotation);
+	return rotation.Multiply3x3(mInvInertiaDiagonal * rotation.Multiply3x3Transposed(inV));
 }
 
 void MotionProperties::ApplyForceTorqueAndDragInternal(QuatArg inBodyRotation, Vec3Arg inGravity, float inDeltaTime)
 {
-	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); 
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite));
+	JPH_ASSERT(mCachedBodyType == EBodyType::RigidBody);
 	JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic);
 
 	// Update linear velocity

Jolt/Physics/Collision/CastSphereVsTriangles.cpp

@@ -23,8 +23,8 @@ CastSphereVsTriangles::CastSphereVsTriangles(const ShapeCast &inShapeCast, const
 	mCenterOfMassTransform2(inCenterOfMassTransform2),
 	mScale(inScale),
 	mSubShapeIDCreator1(inSubShapeIDCreator1),
-	mCollector(ioCollector) 
-{ 
+	mCollector(ioCollector)
+{
 	// Cast to sphere shape
 	JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::Sphere);
 	const SphereShape *sphere = static_cast<const SphereShape *>(inShapeCast.mShape);
@@ -42,7 +42,7 @@ void CastSphereVsTriangles::AddHit(bool inBackFacing, const SubShapeID &inSubSha
 	Vec3 contact_point_a = mCenterOfMassTransform2 * (mStart + inContactPointA);
 	Vec3 contact_point_b = mCenterOfMassTransform2 * (mStart + inContactPointB);
 	Vec3 contact_normal_world = mCenterOfMassTransform2.Multiply3x3(inContactNormal);
-	
+
 	// Its a hit, store the sub shape id's
 	ShapeCastResult result(inFraction, contact_point_a, contact_point_b, contact_normal_world, inBackFacing, mSubShapeIDCreator1.GetID(), inSubShapeID2, TransformedShape::sGetBodyID(mCollector.GetContext()));
 
@@ -84,12 +84,12 @@ float CastSphereVsTriangles::RayCylinder(Vec3Arg inRayDirection, Vec3Arg inCylin
 	float start_dot_axis = start.Dot(axis);
 	float direction_dot_axis = inRayDirection.Dot(axis);
 	float end_dot_axis = start_dot_axis + direction_dot_axis;
-	if (start_dot_axis < 0.0f && end_dot_axis < 0.0f) 
+	if (start_dot_axis < 0.0f && end_dot_axis < 0.0f)
 		return FLT_MAX;
 
 	// Test if segment is fully on the B side of the cylinder
 	float axis_len_sq = axis.LengthSq();
-	if (start_dot_axis > axis_len_sq && end_dot_axis > axis_len_sq) 
+	if (start_dot_axis > axis_len_sq && end_dot_axis > axis_len_sq)
 		return FLT_MAX;
 
 	// Calculate a, b and c, the factors for quadratic equation
@@ -103,14 +103,14 @@ float CastSphereVsTriangles::RayCylinder(Vec3Arg inRayDirection, Vec3Arg inCylin
 	float b = axis_len_sq * start.Dot(inRayDirection) - direction_dot_axis * start_dot_axis; // should be multiplied by 2, instead we'll divide a and c by 2 when we solve the quadratic equation
 	float c = axis_len_sq * (start.LengthSq() - Square(inRadius)) - Square(start_dot_axis);
 	float det = Square(b) - a * c; // normally 4 * a * c but since both a and c need to be divided by 2 we lose the 4
-	if (det < 0.0f) 
+	if (det < 0.0f)
 		return FLT_MAX; // No solution to quadractic equation
-	
+
 	// Solve fraction t where the ray hits the cylinder
 	float t = -(b + sqrt(det)) / a; // normally divided by 2 * a but since a should be divided by 2 we lose the 2
-	if (t < 0.0f || t > 1.0f) 
+	if (t < 0.0f || t > 1.0f)
 		return FLT_MAX; // Intersection lies outside segment
-	if (start_dot_axis + t * direction_dot_axis < 0.0f || start_dot_axis + t * direction_dot_axis > axis_len_sq) 
+	if (start_dot_axis + t * direction_dot_axis < 0.0f || start_dot_axis + t * direction_dot_axis > axis_len_sq)
 		return FLT_MAX; // Intersection outside the end point of the cyclinder, stop processing, we will possibly hit a vertex
 	return t;
 }
@@ -125,7 +125,11 @@ void CastSphereVsTriangles::Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8
 	Vec3 v2 = mScale * inV2 - mStart;
 
 	// Calculate triangle normal
-	Vec3 triangle_normal = mScaleSign * (v1 - v0).Cross(v2 - v0).Normalized();
+	Vec3 triangle_normal = mScaleSign * (v1 - v0).Cross(v2 - v0);
+	float triangle_normal_len = triangle_normal.Length();
+	if (triangle_normal_len == 0.0f)
+		return; // Degenerate triangle
+	triangle_normal /= triangle_normal_len;
 
 	// Backface check
 	float normal_dot_direction = triangle_normal.Dot(mDirection);

Jolt/Physics/Collision/Shape/BoxShape.cpp

@@ -11,6 +11,7 @@
 #include <Jolt/Physics/Collision/CastResult.h>
 #include <Jolt/Physics/Collision/CollidePointResult.h>
 #include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/SoftBody/SoftBodyVertex.h>
 #include <Jolt/Geometry/RayAABox.h>
 #include <Jolt/ObjectStream/TypeDeclarations.h>
 #include <Jolt/Core/StreamIn.h>
@@ -45,17 +46,17 @@ static const Vec3 sUnitBoxTriangles[] = {
 };
 
 ShapeSettings::ShapeResult BoxShapeSettings::Create() const
-{ 
+{
 	if (mCachedResult.IsEmpty())
-		Ref<Shape> shape = new BoxShape(*this, mCachedResult); 
+		Ref<Shape> shape = new BoxShape(*this, mCachedResult);
 	return mCachedResult;
 }
 
-BoxShape::BoxShape(const BoxShapeSettings &inSettings, ShapeResult &outResult) : 
-	ConvexShape(EShapeSubType::Box, inSettings, outResult), 
-	mHalfExtent(inSettings.mHalfExtent), 
-	mConvexRadius(inSettings.mConvexRadius) 
-{ 
+BoxShape::BoxShape(const BoxShapeSettings &inSettings, ShapeResult &outResult) :
+	ConvexShape(EShapeSubType::Box, inSettings, outResult),
+	mHalfExtent(inSettings.mHalfExtent),
+	mConvexRadius(inSettings.mConvexRadius)
+{
 	// Check convex radius
 	if (inSettings.mConvexRadius < 0.0f
 		|| inSettings.mHalfExtent.ReduceMin() <= inSettings.mConvexRadius)
@@ -71,17 +72,17 @@ BoxShape::BoxShape(const BoxShapeSettings &inSettings, ShapeResult &outResult) :
 class BoxShape::Box final : public Support
 {
 public:
-					Box(const AABox &inBox, float inConvexRadius) : 
+					Box(const AABox &inBox, float inConvexRadius) :
 		mBox(inBox),
 		mConvexRadius(inConvexRadius)
-	{ 
-		static_assert(sizeof(Box) <= sizeof(SupportBuffer), "Buffer size too small"); 
+	{
+		static_assert(sizeof(Box) <= sizeof(SupportBuffer), "Buffer size too small");
 		JPH_ASSERT(IsAligned(this, alignof(Box)));
 	}
 
 	virtual Vec3	GetSupport(Vec3Arg inDirection) const override
-	{ 
-		return mBox.GetSupport(inDirection); 
+	{
+		return mBox.GetSupport(inDirection);
 	}
 
 	virtual float	GetConvexRadius() const override
@@ -125,13 +126,13 @@ const ConvexShape::Support *BoxShape::GetSupportFunction(ESupportMode inMode, Su
 	return nullptr;
 }
 
-void BoxShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const 
-{ 
+void BoxShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
 	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
 
 	Vec3 scaled_half_extent = inScale.Abs() * mHalfExtent;
 	AABox box(-scaled_half_extent, scaled_half_extent);
-	box.GetSupportingFace(inDirection, outVertices); 
+	box.GetSupportingFace(inDirection, outVertices);
 
 	// Transform to world space
 	for (Vec3 &v : outVertices)
@@ -145,9 +146,9 @@ MassProperties BoxShape::GetMassProperties() const
 	return p;
 }
 
-Vec3 BoxShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const 
-{ 
-	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); 
+Vec3 BoxShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
 
 	// Get component that is closest to the surface of the box
 	int index = (inLocalSurfacePosition.Abs() - mHalfExtent).Abs().GetLowestComponentIndex();
@@ -204,7 +205,7 @@ void BoxShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSet
 		}
 
 		// Check back side hit
-		if (inRayCastSettings.mBackFaceMode == EBackFaceMode::CollideWithBackFaces 
+		if (inRayCastSettings.mBackFaceMode == EBackFaceMode::CollideWithBackFaces
 			&& max_fraction < ioCollector.GetEarlyOutFraction())
 		{
 			hit.mFraction = max_fraction;
@@ -223,6 +224,59 @@ void BoxShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShape
 		ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
 }
 
+void BoxShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+{
+	Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation();
+	Vec3 half_extent = mHalfExtent;
+
+	for (SoftBodyVertex &v : ioVertices)
+		if (v.mInvMass > 0.0f)
+		{
+			Vec3 local_pos = inverse_transform * v.mPosition;
+			Vec3 delta = half_extent - local_pos.Abs();
+			UVec4 point_inside = Vec3::sGreaterOrEqual(delta, Vec3::sZero());
+
+			// Test if inside
+			if (point_inside.TestAllXYZTrue())
+			{
+				// Calculate closest distance to surface
+				int index = delta.GetLowestComponentIndex();
+				float penetration = delta[index];
+				if (penetration > v.mLargestPenetration)
+				{
+					v.mLargestPenetration = penetration;
+
+					// Calculate contact point and normal
+					Vec3 possible_normals[] = { Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ() };
+					Vec3 normal = local_pos.GetSign() * possible_normals[index];
+					Vec3 point = normal * half_extent;
+
+					// Store collision
+					v.mCollisionPlane = Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform);
+					v.mCollidingShapeIndex = inCollidingShapeIndex;
+				}
+			}
+			else
+			{
+				// Point is outside find point and normal of the closest surface
+				Vec3 normal = Vec3::sSelect(local_pos.GetSign(), Vec3::sZero(), point_inside);
+				Vec3 point = normal * half_extent;
+				normal = normal.Normalized();
+
+				// Penetration will be negative since we're not penetrating
+				float penetration = (point - local_pos).Dot(normal);
+				if (penetration > v.mLargestPenetration)
+				{
+					v.mLargestPenetration = penetration;
+
+					// Store collision
+					v.mCollisionPlane = Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform);
+					v.mCollidingShapeIndex = inCollidingShapeIndex;
+				}
+			}
+		}
+}
+
 void BoxShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const
 {
 	new (&ioContext) GetTrianglesContextVertexList(inPositionCOM, inRotation, inScale, Mat44::sScale(mHalfExtent), sUnitBoxTriangles, sizeof(sUnitBoxTriangles) / sizeof(Vec3), GetMaterial());

Jolt/Physics/Collision/Shape/BoxShape.h

@@ -76,6 +76,9 @@ public:
 	// See: Shape::CollidePoint
 	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
+	// See: Shape::ColideSoftBodyVertices
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+
 	// See Shape::GetTrianglesStart
 	virtual void			GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
 

Jolt/Physics/Collision/Shape/CapsuleShape.cpp

@@ -12,6 +12,7 @@
 #include <Jolt/Physics/Collision/CastResult.h>
 #include <Jolt/Physics/Collision/CollidePointResult.h>
 #include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/SoftBody/SoftBodyVertex.h>
 #include <Jolt/Geometry/RayCapsule.h>
 #include <Jolt/ObjectStream/TypeDeclarations.h>
 #include <Jolt/Core/StreamIn.h>
@@ -32,26 +33,26 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(CapsuleShapeSettings)
 
 static const int cCapsuleDetailLevel = 2;
 
-static const std::vector<Vec3> sCapsuleTopTriangles = []() { 
-	std::vector<Vec3> verts;	
+static const std::vector<Vec3> sCapsuleTopTriangles = []() {
+	std::vector<Vec3> verts;
 	GetTrianglesContextVertexList::sCreateHalfUnitSphereTop(verts, cCapsuleDetailLevel);
 	return verts;
 }();
 
-static const std::vector<Vec3> sCapsuleMiddleTriangles = []() { 
+static const std::vector<Vec3> sCapsuleMiddleTriangles = []() {
 	std::vector<Vec3> verts;
 	GetTrianglesContextVertexList::sCreateUnitOpenCylinder(verts, cCapsuleDetailLevel);
 	return verts;
 }();
 
-static const std::vector<Vec3> sCapsuleBottomTriangles = []() { 
-	std::vector<Vec3> verts;	
+static const std::vector<Vec3> sCapsuleBottomTriangles = []() {
+	std::vector<Vec3> verts;
 	GetTrianglesContextVertexList::sCreateHalfUnitSphereBottom(verts, cCapsuleDetailLevel);
 	return verts;
 }();
 
 ShapeSettings::ShapeResult CapsuleShapeSettings::Create() const
-{ 
+{
 	if (mCachedResult.IsEmpty())
 	{
 		Ref<Shape> shape;
@@ -62,22 +63,22 @@ ShapeSettings::ShapeResult CapsuleShapeSettings::Create() const
 			mCachedResult.Set(shape);
 		}
 		else
-			shape = new CapsuleShape(*this, mCachedResult); 
+			shape = new CapsuleShape(*this, mCachedResult);
 	}
 	return mCachedResult;
 }
 
-CapsuleShape::CapsuleShape(const CapsuleShapeSettings &inSettings, ShapeResult &outResult) : 
-	ConvexShape(EShapeSubType::Capsule, inSettings, outResult), 
-	mRadius(inSettings.mRadius), 
-	mHalfHeightOfCylinder(inSettings.mHalfHeightOfCylinder) 
-{ 
+CapsuleShape::CapsuleShape(const CapsuleShapeSettings &inSettings, ShapeResult &outResult) :
+	ConvexShape(EShapeSubType::Capsule, inSettings, outResult),
+	mRadius(inSettings.mRadius),
+	mHalfHeightOfCylinder(inSettings.mHalfHeightOfCylinder)
+{
 	if (inSettings.mHalfHeightOfCylinder <= 0.0f)
 	{
 		outResult.SetError("Invalid height");
 		return;
 	}
-	
+
 	if (inSettings.mRadius <= 0.0f)
 	{
 		outResult.SetError("Invalid radius");
@@ -90,16 +91,16 @@ CapsuleShape::CapsuleShape(const CapsuleShapeSettings &inSettings, ShapeResult &
 class CapsuleShape::CapsuleNoConvex final : public Support
 {
 public:
-					CapsuleNoConvex(Vec3Arg inHalfHeightOfCylinder, float inConvexRadius) : 
+					CapsuleNoConvex(Vec3Arg inHalfHeightOfCylinder, float inConvexRadius) :
 		mHalfHeightOfCylinder(inHalfHeightOfCylinder),
 		mConvexRadius(inConvexRadius)
-	{ 
-		static_assert(sizeof(CapsuleNoConvex) <= sizeof(SupportBuffer), "Buffer size too small"); 
+	{
+		static_assert(sizeof(CapsuleNoConvex) <= sizeof(SupportBuffer), "Buffer size too small");
 		JPH_ASSERT(IsAligned(this, alignof(CapsuleNoConvex)));
 	}
 
 	virtual Vec3	GetSupport(Vec3Arg inDirection) const override
-	{ 
+	{
 		if (inDirection.GetY() > 0)
 			return mHalfHeightOfCylinder;
 		else
@@ -107,7 +108,7 @@ public:
 	}
 
 	virtual float	GetConvexRadius() const override
-	{ 
+	{
 		return mConvexRadius;
 	}
 
@@ -119,16 +120,16 @@ private:
 class CapsuleShape::CapsuleWithConvex final : public Support
 {
 public:
-					CapsuleWithConvex(Vec3Arg inHalfHeightOfCylinder, float inRadius) : 
+					CapsuleWithConvex(Vec3Arg inHalfHeightOfCylinder, float inRadius) :
 		mHalfHeightOfCylinder(inHalfHeightOfCylinder),
 		mRadius(inRadius)
-	{ 
-		static_assert(sizeof(CapsuleWithConvex) <= sizeof(SupportBuffer), "Buffer size too small"); 
+	{
+		static_assert(sizeof(CapsuleWithConvex) <= sizeof(SupportBuffer), "Buffer size too small");
 		JPH_ASSERT(IsAligned(this, alignof(CapsuleWithConvex)));
 	}
 
 	virtual Vec3	GetSupport(Vec3Arg inDirection) const override
-	{ 
+	{
 		float len = inDirection.Length();
 		Vec3 radius = len > 0.0f? inDirection * (mRadius / len) : Vec3::sZero();
 
@@ -139,7 +140,7 @@ public:
 	}
 
 	virtual float	GetConvexRadius() const override
-	{ 
+	{
 		return 0.0f;
 	}
 
@@ -172,7 +173,7 @@ const ConvexShape::Support *CapsuleShape::GetSupportFunction(ESupportMode inMode
 }
 
 void CapsuleShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
-{	
+{
 	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
 	JPH_ASSERT(IsValidScale(inScale));
 
@@ -216,13 +217,13 @@ MassProperties CapsuleShape::GetMassProperties() const
 
 	float density = GetDensity();
 
-	// Calculate inertia and mass according to: 
+	// Calculate inertia and mass according to:
 	// https://www.gamedev.net/resources/_/technical/math-and-physics/capsule-inertia-tensor-r3856
 	float radius_sq = mRadius * mRadius;
 	float height = 2.0f * mHalfHeightOfCylinder;
 	float cylinder_mass = JPH_PI * height * radius_sq * density;
 	float hemisphere_mass = (2.0f * JPH_PI / 3.0f) * radius_sq * mRadius * density;
-    
+
 	// From cylinder
 	float inertia_y = radius_sq * cylinder_mass * 0.5f;
 	float inertia_x = inertia_y * 0.5f + cylinder_mass * height * height / 12.0f;
@@ -241,13 +242,13 @@ MassProperties CapsuleShape::GetMassProperties() const
 
 	// Set inertia
 	p.mInertia = Mat44::sScale(Vec3(inertia_x, inertia_y, inertia_z));
-	
+
 	return p;
 }
 
-Vec3 CapsuleShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const 
-{ 
-	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); 
+Vec3 CapsuleShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
 
 	if (inLocalSurfacePosition.GetY() > mHalfHeightOfCylinder)
 		return (inLocalSurfacePosition - Vec3(0, mHalfHeightOfCylinder, 0)).Normalized();
@@ -264,7 +265,7 @@ AABox CapsuleShape::GetLocalBounds() const
 }
 
 AABox CapsuleShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
-{ 
+{
 	JPH_ASSERT(IsValidScale(inScale));
 
 	Vec3 abs_scale = inScale.Abs();
@@ -321,6 +322,58 @@ void CapsuleShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubS
 		ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
 }
 
+void CapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+{
+	Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation();
+
+	for (SoftBodyVertex &v : ioVertices)
+		if (v.mInvMass > 0.0f)
+		{
+			// Calculate penetration
+			Vec3 local_pos = inverse_transform * v.mPosition;
+			if (abs(local_pos.GetY()) <= mHalfHeightOfCylinder)
+			{
+				// Near cylinder
+				Vec3 normal = local_pos;
+				normal.SetY(0.0f);
+				float normal_length = normal.Length();
+				float penetration = mRadius - normal_length;
+				if (penetration > v.mLargestPenetration)
+				{
+					v.mLargestPenetration = penetration;
+
+					// Calculate contact point and normal
+					normal = normal_length > 0.0f? normal / normal_length : Vec3::sAxisX();
+					Vec3 point = mRadius * normal;
+
+					// Store collision
+					v.mCollisionPlane = Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform);
+					v.mCollidingShapeIndex = inCollidingShapeIndex;
+				}
+			}
+			else
+			{
+				// Near cap
+				Vec3 center = Vec3(0, Sign(local_pos.GetY()) * mHalfHeightOfCylinder, 0);
+				Vec3 delta = local_pos - center;
+				float distance = delta.Length();
+				float penetration = mRadius - distance;
+				if (penetration > v.mLargestPenetration)
+				{
+					v.mLargestPenetration = penetration;
+
+					// Calculate contact point and normal
+					Vec3 normal = delta / distance;
+					Vec3 point = center + mRadius * normal;
+
+					// Store collision
+					v.mCollisionPlane = Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform);
+					v.mCollidingShapeIndex = inCollidingShapeIndex;
+				}
+			}
+		}
+}
+
 void CapsuleShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const
 {
 	Vec3 scale;

Jolt/Physics/Collision/Shape/CapsuleShape.h

@@ -53,7 +53,7 @@ public:
 
 	// See Shape::GetLocalBounds
 	virtual AABox			GetLocalBounds() const override;
-		
+
 	// See Shape::GetWorldSpaceBounds
 	virtual AABox			GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
 	using Shape::GetWorldSpaceBounds;
@@ -85,6 +85,9 @@ public:
 	// See: Shape::CollidePoint
 	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
+	// See: Shape::ColideSoftBodyVertices
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+
 	// See Shape::TransformShape
 	virtual void			TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override;
 

Jolt/Physics/Collision/Shape/CompoundShape.cpp

@@ -86,12 +86,12 @@ MassProperties CompoundShape::GetMassProperties() const
 
 	// Ensure that inertia is a 3x3 matrix, adding inertias causes the bottom right element to change
 	p.mInertia.SetColumn4(3, Vec4(0, 0, 0, 1));
-	
+
 	return p;
 }
 
 AABox CompoundShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
-{ 
+{
 	if (mSubShapes.size() <= 10)
 	{
 		AABox bounds;
@@ -156,8 +156,8 @@ TransformedShape CompoundShape::GetSubShapeTransformedShape(const SubShapeID &in
 	return ts;
 }
 
-Vec3 CompoundShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const 
-{ 
+Vec3 CompoundShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
 	// Decode sub shape index
 	SubShapeID remainder;
 	uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder);
@@ -246,6 +246,12 @@ void CompoundShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg i
 }
 #endif // JPH_DEBUG_RENDERER
 
+void CompoundShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+{
+	for (const SubShape &shape : mSubShapes)
+		shape.mShape->CollideSoftBodyVertices(inCenterOfMassTransform * Mat44::sRotationTranslation(shape.GetRotation(), shape.GetPositionCOM()), ioVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex);
+}
+
 void CompoundShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const
 {
 	for (const SubShape &shape : mSubShapes)
@@ -335,7 +341,7 @@ void CompoundShape::RestoreBinaryState(StreamIn &inStream)
 }
 
 void CompoundShape::SaveSubShapeState(ShapeList &outSubShapes) const
-{ 
+{
 	outSubShapes.clear();
 	outSubShapes.reserve(mSubShapes.size());
 	for (const SubShape &shape : mSubShapes)
@@ -343,7 +349,7 @@ void CompoundShape::SaveSubShapeState(ShapeList &outSubShapes) const
 }
 
 void CompoundShape::RestoreSubShapeState(const ShapeRefC *inSubShapes, uint inNumShapes)
-{ 
+{
 	JPH_ASSERT(mSubShapes.size() == inNumShapes);
 	for (uint i = 0; i < inNumShapes; ++i)
 		mSubShapes[i].mShape = inSubShapes[i];

Jolt/Physics/Collision/Shape/CompoundShape.h

@@ -62,7 +62,7 @@ public:
 
 	// See Shape::GetLocalBounds
 	virtual AABox					GetLocalBounds() const override							{ return mLocalBounds; }
-		
+
 	// See Shape::GetSubShapeIDBitsRecursive
 	virtual uint					GetSubShapeIDBitsRecursive() const override;
 
@@ -105,6 +105,9 @@ public:
 	virtual void					DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
 #endif // JPH_DEBUG_RENDERER
 
+	// See: Shape::ColideSoftBodyVertices
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+
 	// See Shape::TransformShape
 	virtual void					TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override;
 
@@ -224,7 +227,7 @@ public:
 		{
 			return mIsRotationIdentity? Quat::sIdentity() : Quat::sLoadFloat3Unsafe(mRotation);
 		}
-		
+
 		RefConst<Shape>				mShape;
 		Float3						mPositionCOM;											///< Note: Position of center of mass of sub shape!
 		Float3						mRotation;												///< Note: X, Y, Z of rotation quaternion - note we read 4 bytes beyond this so make sure there's something there
@@ -255,19 +258,19 @@ public:
 	/// Check if a sub shape ID is still valid for this shape
 	/// @param inSubShapeID Sub shape id that indicates the leaf shape relative to this shape
 	/// @return True if the ID is valid, false if not
-	inline bool						IsSubShapeIDValid(SubShapeID inSubShapeID) const 
+	inline bool						IsSubShapeIDValid(SubShapeID inSubShapeID) const
 	{
 		SubShapeID remainder;
-		return inSubShapeID.PopID(GetSubShapeIDBits(), remainder) < mSubShapes.size(); 
+		return inSubShapeID.PopID(GetSubShapeIDBits(), remainder) < mSubShapes.size();
 	}
 
 	/// Convert SubShapeID to sub shape index
 	/// @param inSubShapeID Sub shape id that indicates the leaf shape relative to this shape
 	/// @param outRemainder This is the sub shape ID for the sub shape of the compound after popping off the index
 	/// @return The index of the sub shape of this compound
-	inline uint32					GetSubShapeIndexFromID(SubShapeID inSubShapeID, SubShapeID &outRemainder) const 
-	{ 
-		uint32 idx = inSubShapeID.PopID(GetSubShapeIDBits(), outRemainder); 
+	inline uint32					GetSubShapeIndexFromID(SubShapeID inSubShapeID, SubShapeID &outRemainder) const
+	{
+		uint32 idx = inSubShapeID.PopID(GetSubShapeIDBits(), outRemainder);
 		JPH_ASSERT(idx < mSubShapes.size(), "Invalid SubShapeID");
 		return idx;
 	}

Jolt/Physics/Collision/Shape/ConvexHullShape.cpp

@@ -11,7 +11,9 @@
 #include <Jolt/Physics/Collision/CastResult.h>
 #include <Jolt/Physics/Collision/CollidePointResult.h>
 #include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/SoftBody/SoftBodyVertex.h>
 #include <Jolt/Geometry/ConvexHullBuilder.h>
+#include <Jolt/Geometry/ClosestPoint.h>
 #include <Jolt/ObjectStream/TypeDeclarations.h>
 #include <Jolt/Core/StringTools.h>
 #include <Jolt/Core/StreamIn.h>
@@ -32,9 +34,9 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(ConvexHullShapeSettings)
 }
 
 ShapeSettings::ShapeResult ConvexHullShapeSettings::Create() const
-{ 
+{
 	if (mCachedResult.IsEmpty())
-		Ref<Shape> shape = new ConvexHullShape(*this, mCachedResult); 
+		Ref<Shape> shape = new ConvexHullShape(*this, mCachedResult);
 	return mCachedResult;
 }
 
@@ -45,7 +47,7 @@ ConvexHullShape::ConvexHullShape(const ConvexHullShapeSettings &inSettings, Shap
 	using BuilderFace = ConvexHullBuilder::Face;
 	using Edge = ConvexHullBuilder::Edge;
 	using Faces = Array<BuilderFace *>;
-	
+
 	// Check convex radius
 	if (mConvexRadius < 0.0f)
 	{
@@ -82,7 +84,7 @@ ConvexHullShape::ConvexHullShape(const ConvexHullShapeSettings &inSettings, Shap
 	builder.GetCenterOfMassAndVolume(mCenterOfMass, mVolume);
 
 	// Calculate covariance matrix
-	// See: 
+	// See:
 	// - Why the inertia tensor is the inertia tensor - Jonathan Blow (http://number-none.com/blow/inertia/deriving_i.html)
 	// - How to find the inertia tensor (or other mass properties) of a 3D solid body represented by a triangle mesh (Draft) - Jonathan Blow, Atman J Binstock (http://number-none.com/blow/inertia/bb_inertia.doc)
 	Mat44 covariance_canonical(Vec4(1.0f / 60.0f, 1.0f / 120.0f, 1.0f / 120.0f, 0), Vec4(1.0f / 120.0f, 1.0f / 60.0f, 1.0f / 120.0f, 0), Vec4(1.0f / 120.0f, 1.0f / 120.0f, 1.0f / 60.0f, 0), Vec4(0, 0, 0, 1));
@@ -129,7 +131,7 @@ ConvexHullShape::ConvexHullShape(const ConvexHullShapeSettings &inSettings, Shap
 		// Determine where the vertices go
 		uint16 first_vertex = (uint16)mVertexIdx.size();
 		uint16 num_vertices = 0;
-		
+
 		// Loop over vertices in face
 		Edge *edge = builder_face->mFirstEdge;
 		do
@@ -208,9 +210,9 @@ ConvexHullShape::ConvexHullShape(const ConvexHullShapeSettings &inSettings, Shap
 			// Find the 3 normals that form the largest tetrahedron
 			// The largest tetrahedron we can get is ((1, 0, 0) x (0, 1, 0)) . (0, 0, 1) = 1, if the volume is only 5% of that,
 			// the three vectors are too coplanar and we fall back to using only 2 plane normals
-			float biggest_volume = 0.05f; 
+			float biggest_volume = 0.05f;
 			int best3[3] = { -1, -1, -1 };
-			
+
 			// When using 2 normals, we get the two with the biggest angle between them with a minimal difference of 1 degree
 			// otherwise we fall back to just using 1 plane normal
 			float smallest_dot = Cos(DegreesToRadians(1.0f));
@@ -305,7 +307,7 @@ ConvexHullShape::ConvexHullShape(const ConvexHullShapeSettings &inSettings, Shap
 					p3 = mPlanes[point.mFaces[2]];
 
 					// All 3 planes will be offset by the convex radius
-					offset_mask = Vec3::sReplicate(1); 
+					offset_mask = Vec3::sReplicate(1);
 				}
 				else
 				{
@@ -314,7 +316,7 @@ ConvexHullShape::ConvexHullShape(const ConvexHullShapeSettings &inSettings, Shap
 					p3 = Plane::sFromPointAndNormal(point.mPosition, p1.GetNormal().Cross(p2.GetNormal()));
 
 					// Only the first and 2nd plane will be offset, the 3rd plane is only there to guide the intersection point
-					offset_mask = Vec3(1, 1, 0); 
+					offset_mask = Vec3(1, 1, 0);
 				}
 
 				// Plane equation: point . normal + constant = 0
@@ -365,13 +367,13 @@ MassProperties ConvexHullShape::GetMassProperties() const
 	// Calculate inertia matrix
 	p.mInertia = density * mInertia;
 	p.mInertia(3, 3) = 1.0f;
-	
+
 	return p;
 }
 
-Vec3 ConvexHullShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const 
-{ 
-	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); 
+Vec3 ConvexHullShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
 
 	const Plane &first_plane = mPlanes[0];
 	Vec3 best_normal = first_plane.GetNormal();
@@ -396,19 +398,19 @@ Vec3 ConvexHullShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg i
 class ConvexHullShape::HullNoConvex final : public Support
 {
 public:
-	explicit				HullNoConvex(float inConvexRadius) : 
+	explicit				HullNoConvex(float inConvexRadius) :
 		mConvexRadius(inConvexRadius)
-	{ 
-		static_assert(sizeof(HullNoConvex) <= sizeof(SupportBuffer), "Buffer size too small"); 
+	{
+		static_assert(sizeof(HullNoConvex) <= sizeof(SupportBuffer), "Buffer size too small");
 		JPH_ASSERT(IsAligned(this, alignof(HullNoConvex)));
 	}
 
 	virtual Vec3			GetSupport(Vec3Arg inDirection) const override
-	{ 
+	{
 		// Find the point with the highest projection on inDirection
 		float best_dot = -FLT_MAX;
 		Vec3 best_point = Vec3::sZero();
-	
+
 		for (Vec3 point : mPoints)
 		{
 			// Check if its support is bigger than the current max
@@ -448,19 +450,19 @@ private:
 class ConvexHullShape::HullWithConvex final : public Support
 {
 public:
-	explicit				HullWithConvex(const ConvexHullShape *inShape) : 
+	explicit				HullWithConvex(const ConvexHullShape *inShape) :
 		mShape(inShape)
-	{ 
-		static_assert(sizeof(HullWithConvex) <= sizeof(SupportBuffer), "Buffer size too small"); 
+	{
+		static_assert(sizeof(HullWithConvex) <= sizeof(SupportBuffer), "Buffer size too small");
 		JPH_ASSERT(IsAligned(this, alignof(HullWithConvex)));
 	}
 
 	virtual Vec3			GetSupport(Vec3Arg inDirection) const override
-	{ 
+	{
 		// Find the point with the highest projection on inDirection
 		float best_dot = -FLT_MAX;
 		Vec3 best_point = Vec3::sZero();
-	
+
 		for (const Point &point : mShape->mPoints)
 		{
 			// Check if its support is bigger than the current max
@@ -487,20 +489,20 @@ private:
 class ConvexHullShape::HullWithConvexScaled final : public Support
 {
 public:
-							HullWithConvexScaled(const ConvexHullShape *inShape, Vec3Arg inScale) : 
+							HullWithConvexScaled(const ConvexHullShape *inShape, Vec3Arg inScale) :
 		mShape(inShape),
 		mScale(inScale)
-	{ 
-		static_assert(sizeof(HullWithConvexScaled) <= sizeof(SupportBuffer), "Buffer size too small"); 
+	{
+		static_assert(sizeof(HullWithConvexScaled) <= sizeof(SupportBuffer), "Buffer size too small");
 		JPH_ASSERT(IsAligned(this, alignof(HullWithConvexScaled)));
 	}
 
 	virtual Vec3			GetSupport(Vec3Arg inDirection) const override
-	{ 
+	{
 		// Find the point with the highest projection on inDirection
 		float best_dot = -FLT_MAX;
 		Vec3 best_point = Vec3::sZero();
-	
+
 		for (const Point &point : mShape->mPoints)
 		{
 			// Calculate scaled position
@@ -619,7 +621,7 @@ const ConvexShape::Support *ConvexHullShape::GetSupportFunction(ESupportMode inM
 				Vec3 n1 = (inv_scale * mPlanes[point.mFaces[0]].GetNormal()).Normalized();
 
 				Vec3 new_point;
-				
+
 				if (point.mNumFaces == 1)
 				{
 					// Simply shift back by the convex radius using our 1 plane
@@ -670,12 +672,12 @@ const ConvexShape::Support *ConvexHullShape::GetSupportFunction(ESupportMode inM
 	return nullptr;
 }
 
-void ConvexHullShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const 
+void ConvexHullShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
 {
 	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
 
 	Vec3 inv_scale = inScale.Reciprocal();
-	
+
 	// Need to transform the plane normals using inScale
 	// Transforming a direction with matrix M is done through multiplying by (M^-1)^T
 	// In this case M is a diagonal matrix with the scale vector, so we need to multiply our normal by 1 / scale and renormalize afterwards
@@ -695,7 +697,7 @@ void ConvexHullShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg
 	}
 
 	// Get vertices
-	const Face &best_face = mFaces[best_face_idx];		
+	const Face &best_face = mFaces[best_face_idx];
 	const uint8 *first_vtx = mVertexIdx.data() + best_face.mFirstVertex;
 	const uint8 *end_vtx = first_vtx + best_face.mNumVertices;
 
@@ -734,7 +736,7 @@ void ConvexHullShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3A
 	int num_points = int(mPoints.size());
 	PolyhedronSubmergedVolumeCalculator::Point *buffer = (PolyhedronSubmergedVolumeCalculator::Point *)JPH_STACK_ALLOC(num_points * sizeof(PolyhedronSubmergedVolumeCalculator::Point));
 	PolyhedronSubmergedVolumeCalculator submerged_vol_calc(inCenterOfMassTransform * Mat44::sScale(inScale), &mPoints[0].mPosition, sizeof(Point), num_points, inSurface, buffer JPH_IF_DEBUG_RENDERER(, inBaseOffset));
-	
+
 	if (submerged_vol_calc.AreAllAbove())
 	{
 		// We're above the water
@@ -1032,7 +1034,7 @@ void ConvexHullShape::CastRay(const RayCast &inRay, const RayCastSettings &inRay
 		}
 
 		// Check back side hit
-		if (inRayCastSettings.mBackFaceMode == EBackFaceMode::CollideWithBackFaces 
+		if (inRayCastSettings.mBackFaceMode == EBackFaceMode::CollideWithBackFaces
 			&& max_fraction < ioCollector.GetEarlyOutFraction())
 		{
 			hit.mFraction = max_fraction;
@@ -1056,6 +1058,78 @@ void ConvexHullShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inS
 	ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
 }
 
+void ConvexHullShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+{
+	Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation();
+
+	for (SoftBodyVertex &v : ioVertices)
+		if (v.mInvMass > 0.0f)
+		{
+			Vec3 local_pos = inverse_transform * v.mPosition;
+
+			// Find most facing plane
+			float max_distance = -FLT_MAX;
+			const Plane *max_plane = nullptr;
+			for (const Plane &p : mPlanes)
+			{
+				float distance = p.SignedDistance(local_pos);
+				if (distance > max_distance)
+				{
+					max_distance = distance;
+					max_plane = &p;
+				}
+			}
+
+			// Check edges if we're outside the hull (when inside we know the closest face is also the closest point to the surface)
+			float closest_edge_distance = FLT_MAX;
+			Plane closest_plane = *max_plane;
+			if (max_distance <= 0.0f)
+			{
+				// Loop over edges
+				const Face &face = mFaces[int(max_plane - mPlanes.data())];
+				for (const uint8 *v_start = &mVertexIdx[face.mFirstVertex], *v1 = v_start, *v_end = v_start + face.mNumVertices; v1 < v_end; ++v1)
+				{
+					// Find second point
+					const uint8 *v2 = v1 + 1;
+					if (v2 == v_end)
+						v2 = v_start;
+
+					// Get edge points
+					Vec3 p1 = mPoints[*v1].mPosition;
+					Vec3 p2 = mPoints[*v2].mPosition;
+
+					// Check if the position is outside the edge (if not, the face will be closer)
+					Vec3 edge_normal = (p2 - p1).Cross(max_plane->GetNormal());
+					if (edge_normal.Dot(local_pos - p1) > 0.0f)
+					{
+						// Get closest point on edge
+						uint32 set;
+						Vec3 closest = ClosestPoint::GetClosestPointOnLine(p1 - local_pos, p2 - local_pos, set);
+						float distance = closest.Length();
+						if (distance < closest_edge_distance)
+						{
+							closest_edge_distance = distance;
+							Vec3 point = local_pos + closest;
+							Vec3 normal = (local_pos - point).NormalizedOr(max_plane->GetNormal());
+							closest_plane = Plane::sFromPointAndNormal(point, normal);
+						}
+					}
+				}
+			}
+
+			// Closest point on edge
+			float penetration = -(closest_edge_distance != FLT_MAX? closest_edge_distance : max_distance);
+			if (penetration > v.mLargestPenetration)
+			{
+				v.mLargestPenetration = penetration;
+
+				// Store collision
+				v.mCollisionPlane = closest_plane.GetTransformed(inCenterOfMassTransform);
+				v.mCollidingShapeIndex = inCollidingShapeIndex;
+			}
+		}
+}
+
 class ConvexHullShape::CHSGetTrianglesContext
 {
 public:
@@ -1184,9 +1258,9 @@ Shape::Stats ConvexHullShape::GetStats() const
 		triangle_count += f.mNumVertices - 2;
 
 	return Stats(
-		sizeof(*this) 
-			+ mPoints.size() * sizeof(Point) 
-			+ mFaces.size() * sizeof(Face) 
+		sizeof(*this)
+			+ mPoints.size() * sizeof(Point)
+			+ mFaces.size() * sizeof(Face)
 			+ mPlanes.size() * sizeof(Plane)
 			+ mVertexIdx.size() * sizeof(uint8),
 		triangle_count);

Jolt/Physics/Collision/Shape/ConvexHullShape.h

@@ -22,14 +22,14 @@ public:
 	/// Default constructor for deserialization
 							ConvexHullShapeSettings() = default;
 
-	/// Create a convex hull from inPoints and maximum convex radius inMaxConvexRadius, the radius is automatically lowered if the hull requires it. 
+	/// Create a convex hull from inPoints and maximum convex radius inMaxConvexRadius, the radius is automatically lowered if the hull requires it.
 	/// (internally this will be subtracted so the total size will not grow with the convex radius).
 							ConvexHullShapeSettings(const Vec3 *inPoints, int inNumPoints, float inMaxConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mPoints(inPoints, inPoints + inNumPoints), mMaxConvexRadius(inMaxConvexRadius) { }
 							ConvexHullShapeSettings(const Array<Vec3> &inPoints, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mPoints(inPoints), mMaxConvexRadius(inConvexRadius) { }
 
 	// See: ShapeSettings
 	virtual ShapeResult		Create() const override;
-	
+
 	Array<Vec3>				mPoints;															///< Points to create the hull from
 	float					mMaxConvexRadius = 0.0f;											///< Convex radius as supplied by the constructor. Note that during hull creation the convex radius can be made smaller if the value is too big for the hull.
 	float					mMaxErrorConvexRadius = 0.05f;										///< Maximum distance between the shrunk hull + convex radius and the actual hull.
@@ -89,6 +89,9 @@ public:
 	// See: Shape::CollidePoint
 	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
+	// See: Shape::ColideSoftBodyVertices
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+
 	// See Shape::GetTrianglesStart
 	virtual void			GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
 

Jolt/Physics/Collision/Shape/CylinderShape.cpp

@@ -11,6 +11,7 @@
 #include <Jolt/Physics/Collision/CastResult.h>
 #include <Jolt/Physics/Collision/CollidePointResult.h>
 #include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/SoftBody/SoftBodyVertex.h>
 #include <Jolt/Geometry/RayCylinder.h>
 #include <Jolt/ObjectStream/TypeDeclarations.h>
 #include <Jolt/Core/StreamIn.h>
@@ -44,7 +45,7 @@ static const Vec3 cTopFace[] =
 	Vec3(-cSin45,	1.0f,	cSin45)
 };
 
-static const std::vector<Vec3> sUnitCylinderTriangles = []() { 
+static const std::vector<Vec3> sUnitCylinderTriangles = []() {
 	std::vector<Vec3> verts;
 
 	const Vec3 bottom_offset(0.0f, -2.0f, 0.0f);
@@ -81,18 +82,18 @@ static const std::vector<Vec3> sUnitCylinderTriangles = []() {
 }();
 
 ShapeSettings::ShapeResult CylinderShapeSettings::Create() const
-{ 
+{
 	if (mCachedResult.IsEmpty())
-		Ref<Shape> shape = new CylinderShape(*this, mCachedResult); 
+		Ref<Shape> shape = new CylinderShape(*this, mCachedResult);
 	return mCachedResult;
 }
 
-CylinderShape::CylinderShape(const CylinderShapeSettings &inSettings, ShapeResult &outResult) : 
+CylinderShape::CylinderShape(const CylinderShapeSettings &inSettings, ShapeResult &outResult) :
 	ConvexShape(EShapeSubType::Cylinder, inSettings, outResult),
-	mHalfHeight(inSettings.mHalfHeight), 
+	mHalfHeight(inSettings.mHalfHeight),
 	mRadius(inSettings.mRadius),
 	mConvexRadius(inSettings.mConvexRadius)
-{ 
+{
 	if (inSettings.mHalfHeight < inSettings.mConvexRadius)
 	{
 		outResult.SetError("Invalid height");
@@ -114,32 +115,32 @@ CylinderShape::CylinderShape(const CylinderShapeSettings &inSettings, ShapeResul
 	outResult.Set(this);
 }
 
-CylinderShape::CylinderShape(float inHalfHeight, float inRadius, float inConvexRadius, const PhysicsMaterial *inMaterial) : 
+CylinderShape::CylinderShape(float inHalfHeight, float inRadius, float inConvexRadius, const PhysicsMaterial *inMaterial) :
 	ConvexShape(EShapeSubType::Cylinder, inMaterial),
-	mHalfHeight(inHalfHeight), 
+	mHalfHeight(inHalfHeight),
 	mRadius(inRadius),
 	mConvexRadius(inConvexRadius)
-{ 
-	JPH_ASSERT(inHalfHeight >= inConvexRadius); 
-	JPH_ASSERT(inRadius >= inConvexRadius); 
+{
+	JPH_ASSERT(inHalfHeight >= inConvexRadius);
+	JPH_ASSERT(inRadius >= inConvexRadius);
 	JPH_ASSERT(inConvexRadius >= 0.0f);
 }
 
 class CylinderShape::Cylinder final : public Support
 {
 public:
-					Cylinder(float inHalfHeight, float inRadius, float inConvexRadius) : 
+					Cylinder(float inHalfHeight, float inRadius, float inConvexRadius) :
 		mHalfHeight(inHalfHeight),
 		mRadius(inRadius),
 		mConvexRadius(inConvexRadius)
-	{ 
-		static_assert(sizeof(Cylinder) <= sizeof(SupportBuffer), "Buffer size too small"); 
+	{
+		static_assert(sizeof(Cylinder) <= sizeof(SupportBuffer), "Buffer size too small");
 		JPH_ASSERT(IsAligned(this, alignof(Cylinder)));
 	}
 
 	virtual Vec3	GetSupport(Vec3Arg inDirection) const override
-	{ 
-		// Support mapping, taken from: 
+	{
+		// Support mapping, taken from:
 		// A Fast and Robust GJK Implementation for Collision Detection of Convex Objects - Gino van den Bergen
 		// page 8
 		float x = inDirection.GetX(), y = inDirection.GetY(), z = inDirection.GetZ();
@@ -187,7 +188,7 @@ const ConvexShape::Support *CylinderShape::GetSupportFunction(ESupportMode inMod
 }
 
 void CylinderShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
-{	
+{
 	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
 	JPH_ASSERT(IsValidScale(inScale));
 
@@ -237,13 +238,13 @@ MassProperties CylinderShape::GetMassProperties() const
 
 	// Set inertia
 	p.mInertia = Mat44::sScale(Vec3(inertia_x, inertia_y, inertia_z));
-	
+
 	return p;
 }
 
-Vec3 CylinderShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const 
-{ 
-	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); 
+Vec3 CylinderShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
 
 	// Calculate distance to infinite cylinder surface
 	Vec3 local_surface_position_xz(inLocalSurfacePosition.GetX(), 0, inLocalSurfacePosition.GetZ());
@@ -299,6 +300,58 @@ void CylinderShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSub
 		ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
 }
 
+void CylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+{
+	Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation();
+
+	for (SoftBodyVertex &v : ioVertices)
+		if (v.mInvMass > 0.0f)
+		{
+			Vec3 local_pos = inverse_transform * v.mPosition;
+
+			// Calculate penetration into side surface
+			Vec3 side_normal = local_pos;
+			side_normal.SetY(0.0f);
+			float side_normal_length = side_normal.Length();
+			float side_penetration = mRadius - side_normal_length;
+
+			// Calculate penetration into top or bottom plane
+			float top_penetration = mHalfHeight - abs(local_pos.GetY());
+
+			Vec3 point, normal;
+			if (side_penetration < 0.0f && top_penetration < 0.0f)
+			{
+				// We're outside the cylinder height and radius
+				point = side_normal * (mRadius / side_normal_length) + Vec3(0, mHalfHeight * Sign(local_pos.GetY()), 0);
+				normal = point.Normalized();
+			}
+			else if (side_penetration < top_penetration)
+			{
+				// Side surface is closest
+				normal = side_normal_length > 0.0f? side_normal / side_normal_length : Vec3::sAxisX();
+				point = mRadius * normal;
+			}
+			else
+			{
+				// Top or bottom plane is closest
+				normal = Vec3(0, Sign(local_pos.GetY()), 0);
+				point = mHalfHeight * normal;
+			}
+
+			// Calculate penetration
+			Plane plane = Plane::sFromPointAndNormal(point, normal);
+			float penetration = -plane.SignedDistance(local_pos);
+			if (penetration > v.mLargestPenetration)
+			{
+				v.mLargestPenetration = penetration;
+
+				// Store collision
+				v.mCollisionPlane = plane.GetTransformed(inCenterOfMassTransform);
+				v.mCollidingShapeIndex = inCollidingShapeIndex;
+			}
+		}
+}
+
 void CylinderShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const
 {
 	Vec3 scale;

Jolt/Physics/Collision/Shape/CylinderShape.h

@@ -80,6 +80,9 @@ public:
 	// See: Shape::CollidePoint
 	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
+	// See: Shape::ColideSoftBodyVertices
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+
 	// See Shape::TransformShape
 	virtual void			TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override;
 

Jolt/Physics/Collision/Shape/HeightFieldShape.cpp

@@ -55,7 +55,7 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(HeightFieldShapeSettings)
 	JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMaterials)
 }
 
-const uint HeightFieldShape::sGridOffsets[] = 
+const uint HeightFieldShape::sGridOffsets[] =
 {
 	0,			// level:  0, max x/y:     0, offset: 0
 	1,			// level:  1, max x/y:     1, offset: 1
@@ -98,7 +98,7 @@ HeightFieldShapeSettings::HeightFieldShapeSettings(const float *inSamples, Vec3A
 ShapeSettings::ShapeResult HeightFieldShapeSettings::Create() const
 {
 	if (mCachedResult.IsEmpty())
-		Ref<Shape> shape = new HeightFieldShape(*this, mCachedResult); 
+		Ref<Shape> shape = new HeightFieldShape(*this, mCachedResult);
 	return mCachedResult;
 }
 
@@ -198,23 +198,23 @@ void HeightFieldShape::CalculateActiveEdges()
 {
 	// Store active edges. The triangles are organized like this:
 	//  +       +
-	//  | \ T1B | \ T2B 
+	//  | \ T1B | \ T2B
 	// e0   e2  |   \
 	//  | T1A \ | T2A \
 	//  +--e1---+-------+
-	//  | \ T3B | \ T4B 
+	//  | \ T3B | \ T4B
 	//  |   \   |   \
 	//  | T3A \ | T4A \
 	//  +-------+-------+
-	// We store active edges e0 .. e2 as bits 0 .. 2. 
+	// We store active edges e0 .. e2 as bits 0 .. 2.
 	// We store triangles horizontally then vertically (order T1A, T2A, T3A and T4A).
-	// The top edge and right edge of the heightfield are always active so we do not need to store them, 
+	// The top edge and right edge of the heightfield are always active so we do not need to store them,
 	// therefore we only need to store (mSampleCount - 1)^2 * 3-bit
 	// The triangles T1B, T2B, T3B and T4B do not need to be stored, their active edges can be constructed from adjacent triangles.
 	// Add 1 byte padding so we can always read 1 uint16 to get the bits that cross an 8 bit boundary
 	uint count_min_1 = mSampleCount - 1;
 	uint count_min_1_sq = Square(count_min_1);
-	mActiveEdges.resize((count_min_1_sq * 3 + 7) / 8 + 1); 
+	mActiveEdges.resize((count_min_1_sq * 3 + 7) / 8 + 1);
 	memset(&mActiveEdges[0], 0, mActiveEdges.size());
 
 	// Calculate triangle normals and make normals zero for triangles that are missing
@@ -247,7 +247,7 @@ void HeightFieldShape::CalculateActiveEdges()
 	for (uint y = 0; y < count_min_1; ++y)
 		for (uint x = 0; x < count_min_1; ++x)
 		{
-			// Calculate vertex positions. 
+			// Calculate vertex positions.
 			// We don't check 'no colliding' since those normals will be zero and sIsEdgeActive will return true
 			Vec3 x1y1 = GetPosition(x, y);
 			Vec3 x1y2 = GetPosition(x, y + 1);
@@ -417,7 +417,7 @@ HeightFieldShape::HeightFieldShape(const HeightFieldShapeSettings &inSettings, S
 			// Ensure that the height says below the max height value so we can safely add 1 to get the upper bound for the quantized value
 			quantized_height = Clamp(quantized_height, 0, int(cMaxHeightValue16 - 1));
 
-			quantized_samples.push_back(uint16(quantized_height)); 
+			quantized_samples.push_back(uint16(quantized_height));
 		}
 
 	// Update offset and scale to account for the compression to uint16
@@ -434,7 +434,7 @@ HeightFieldShape::HeightFieldShape(const HeightFieldShapeSettings &inSettings, S
 	// Calculate amount of grids
 	uint max_level = sGetMaxLevel(n);
 
-	// Temporary data structure used during creating of a hierarchy of grids 
+	// Temporary data structure used during creating of a hierarchy of grids
 	struct Range
 	{
 		uint16	mMin;
@@ -468,7 +468,7 @@ HeightFieldShape::HeightFieldShape(const HeightFieldShapeSettings &inSettings, S
 				}
 			++range_dst;
 		}
-		
+
 	// Calculate remaining grids
 	while (n > 1)
 	{
@@ -547,7 +547,7 @@ HeightFieldShape::HeightFieldShape(const HeightFieldShapeSettings &inSettings, S
 				// Add this block
 				mRangeBlocks.push_back(rb);
 			}
-	}	
+	}
 	JPH_ASSERT(mRangeBlocks.size() == sGridOffsets[ranges.size()]);
 
 	// Quantize height samples
@@ -623,9 +623,9 @@ inline void HeightFieldShape::GetBlockOffsetAndScale(uint inBlockX, uint inBlock
 
 inline uint8 HeightFieldShape::GetHeightSample(uint inX, uint inY) const
 {
-	JPH_ASSERT(inX < mSampleCount); 
-	JPH_ASSERT(inY < mSampleCount); 
-	
+	JPH_ASSERT(inX < mSampleCount);
+	JPH_ASSERT(inY < mSampleCount);
+
 	// Determine bit position of sample
 	uint sample = (inY * mSampleCount + inX) * uint(mBitsPerSample);
 	uint byte_pos = sample >> 3;
@@ -635,17 +635,17 @@ inline uint8 HeightFieldShape::GetHeightSample(uint inX, uint inY) const
 	JPH_ASSERT(byte_pos + 1 < mHeightSamples.size());
 	const uint8 *height_samples = mHeightSamples.data() + byte_pos;
 	uint16 height_sample = uint16(height_samples[0]) | uint16(uint16(height_samples[1]) << 8);
-	return uint8(height_sample >> bit_pos) & mSampleMask; 
+	return uint8(height_sample >> bit_pos) & mSampleMask;
 }
 
 inline Vec3 HeightFieldShape::GetPosition(uint inX, uint inY, float inBlockOffset, float inBlockScale, bool &outNoCollision) const
-{ 
+{
 	// Get quantized value
 	uint8 height_sample = GetHeightSample(inX, inY);
 	outNoCollision = height_sample == mSampleMask;
 
 	// Add 0.5 to the quantized value to minimize the error (see constructor)
-	return mOffset + mScale * Vec3(float(inX), inBlockOffset + (0.5f + height_sample) * inBlockScale, float(inY)); 
+	return mOffset + mScale * Vec3(float(inX), inBlockOffset + (0.5f + height_sample) * inBlockScale, float(inY));
 }
 
 Vec3 HeightFieldShape::GetPosition(uint inX, uint inY) const
@@ -671,7 +671,7 @@ Vec3 HeightFieldShape::GetPosition(uint inX, uint inY) const
 }
 
 bool HeightFieldShape::IsNoCollision(uint inX, uint inY) const
-{ 
+{
 	return mHeightSamples.empty() || GetHeightSample(inX, inY) == mSampleMask;
 }
 
@@ -804,8 +804,8 @@ const PhysicsMaterial *HeightFieldShape::GetMaterial(const SubShapeID &inSubShap
 	return GetMaterial(x, y);
 }
 
-Vec3 HeightFieldShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const 
-{ 
+Vec3 HeightFieldShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
 	// Decode ID
 	uint x, y, triangle;
 	DecodeSubShapeID(inSubShapeID, x, y, triangle);
@@ -1024,7 +1024,7 @@ void HeightFieldShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassT
 			}
 
 			JPH_INLINE void			VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) const
-			{			
+			{
 				// Determine active edges
 				uint8 active_edges = mShape->GetEdgeFlags(inX, inY, inTriangle);
 
@@ -1061,7 +1061,7 @@ public:
 		mShape(inShape)
 	{
 		static_assert(sizeof(sGridOffsets) / sizeof(uint) == cNumBitsXY + 1, "Offsets array is not long enough");
-	
+
 		// Construct root stack entry
 		mPropertiesStack[0] = 0; // level: 0, x: 0, y: 0
 	}
@@ -1190,7 +1190,7 @@ public:
 				uint32 half_block_size = block_size >> 1;
 				uint32 block_size_x = max_x - min_x - half_block_size;
 				uint32 block_size_y = max_y - min_y - half_block_size;
-				Range ranges[] = 
+				Range ranges[] =
 				{
 					{ 0, 0,									half_block_size, half_block_size },
 					{ half_block_size, 0,					block_size_x, half_block_size },
@@ -1250,7 +1250,7 @@ public:
 				// Transpose so we have the mins and maxes of each of the blocks in rows instead of columns
 				Mat44 transposed_min = block_min.Transposed();
 				Mat44 transposed_max = block_max.Transposed();
-				
+
 				// Check which blocks collide
 				// Note: At this point we don't use our own stack but we do allow the visitor to use its own stack
 				// to store collision distances so that we can still early out when no closer hits have been found.
@@ -1324,7 +1324,7 @@ public:
 						}
 
 					// Fetch next block until we find one that the visitor wants to see
-					do 
+					do
 						--result;
 					while (result >= 0 && !ioVisitor.ShouldVisitRangeBlock(mTop + result));
 				}
@@ -1370,7 +1370,7 @@ public:
 				// Push them onto the stack
 				JPH_ASSERT(mTop + 4 < cStackSize);
 				properties.StoreInt4(&mPropertiesStack[mTop]);
-				mTop += num_results;		
+				mTop += num_results;
 			}
 
 			// Check if we're done
@@ -1378,7 +1378,7 @@ public:
 				return;
 
 			// Fetch next node until we find one that the visitor wants to see
-			do 
+			do
 				--mTop;
 			while (mTop >= 0 && !ioVisitor.ShouldVisitRangeBlock(mTop));
 		}
@@ -1410,14 +1410,14 @@ bool HeightFieldShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &in
 
 	struct Visitor
 	{
-		JPH_INLINE explicit		Visitor(const HeightFieldShape *inShape, const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) : 
+		JPH_INLINE explicit		Visitor(const HeightFieldShape *inShape, const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) :
 			mHit(ioHit),
 			mRayOrigin(inRay.mOrigin),
 			mRayDirection(inRay.mDirection),
 			mRayInvDirection(inRay.mDirection),
 			mShape(inShape),
 			mSubShapeIDCreator(inSubShapeIDCreator)
-		{ 
+		{
 		}
 
 		JPH_INLINE bool			ShouldAbort() const
@@ -1439,8 +1439,8 @@ bool HeightFieldShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &in
 			return SortReverseAndStore(distance, mHit.mFraction, ioProperties, &mDistanceStack[inStackTop]);
 		}
 
-		JPH_INLINE void			VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) 
-		{			
+		JPH_INLINE void			VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
+		{
 			float fraction = RayTriangle(mRayOrigin, mRayDirection, inV0, inV1, inV2);
 			if (fraction < mHit.mFraction)
 			{
@@ -1477,7 +1477,7 @@ void HeightFieldShape::CastRay(const RayCast &inRay, const RayCastSettings &inRa
 
 	struct Visitor
 	{
-		JPH_INLINE explicit		Visitor(const HeightFieldShape *inShape, const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector) : 
+		JPH_INLINE explicit		Visitor(const HeightFieldShape *inShape, const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector) :
 			mCollector(ioCollector),
 			mRayOrigin(inRay.mOrigin),
 			mRayDirection(inRay.mDirection),
@@ -1485,7 +1485,7 @@ void HeightFieldShape::CastRay(const RayCast &inRay, const RayCastSettings &inRa
 			mBackFaceMode(inRayCastSettings.mBackFaceMode),
 			mShape(inShape),
 			mSubShapeIDCreator(inSubShapeIDCreator)
-		{ 
+		{
 		}
 
 		JPH_INLINE bool			ShouldAbort() const
@@ -1498,17 +1498,17 @@ void HeightFieldShape::CastRay(const RayCast &inRay, const RayCastSettings &inRa
 			return mDistanceStack[inStackTop] < mCollector.GetEarlyOutFraction();
 		}
 
-		JPH_INLINE int			VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) 
+		JPH_INLINE int			VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
 		{
 			// Test bounds of 4 children
 			Vec4 distance = RayAABox4(mRayOrigin, mRayInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
-	
+
 			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
 			return SortReverseAndStore(distance, mCollector.GetEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
 		}
 
 		JPH_INLINE void			VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) const
-		{			
+		{
 			// Back facing check
 			if (mBackFaceMode == EBackFaceMode::IgnoreBackFaces && (inV2 - inV0).Cross(inV1 - inV0).Dot(mRayDirection) < 0)
 				return;
@@ -1524,7 +1524,7 @@ void HeightFieldShape::CastRay(const RayCast &inRay, const RayCastSettings &inRa
 				mCollector.AddHit(hit);
 			}
 		}
-		
+
 		CastRayCollector &		mCollector;
 		Vec3					mRayOrigin;
 		Vec3					mRayDirection;
@@ -1544,6 +1544,11 @@ void HeightFieldShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &in
 	// A height field doesn't have volume, so we can't test insideness
 }
 
+void HeightFieldShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+{
+	sCollideSoftBodyVerticesUsingRayCast(*this, inCenterOfMassTransform, ioVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex);
+}
+
 void HeightFieldShape::sCastConvexVsHeightField(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
 {
 	JPH_PROFILE_FUNCTION();
@@ -1562,9 +1567,9 @@ void HeightFieldShape::sCastConvexVsHeightField(const ShapeCast &inShapeCast, co
 			return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction();
 		}
 
-		JPH_INLINE int				VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) 
+		JPH_INLINE int				VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
 		{
-			// Scale the bounding boxes of this node 
+			// Scale the bounding boxes of this node
 			Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
 			AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
 
@@ -1573,13 +1578,13 @@ void HeightFieldShape::sCastConvexVsHeightField(const ShapeCast &inShapeCast, co
 
 			// Test bounds of 4 children
 			Vec4 distance = RayAABox4(mBoxCenter, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
-	
+
 			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
 			return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
 		}
 
 		JPH_INLINE void				VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
-		{			
+		{
 			// Create sub shape id for this part
 			SubShapeID triangle_sub_shape_id = mShape2->EncodeSubShapeID(mSubShapeIDCreator2, inX, inY, inTriangle);
 
@@ -1627,9 +1632,9 @@ void HeightFieldShape::sCastSphereVsHeightField(const ShapeCast &inShapeCast, co
 			return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction();
 		}
 
-		JPH_INLINE int				VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) 
+		JPH_INLINE int				VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
 		{
-			// Scale the bounding boxes of this node 
+			// Scale the bounding boxes of this node
 			Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
 			AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
 
@@ -1638,13 +1643,13 @@ void HeightFieldShape::sCastSphereVsHeightField(const ShapeCast &inShapeCast, co
 
 			// Test bounds of 4 children
 			Vec4 distance = RayAABox4(mStart, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
-	
+
 			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
 			return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
 		}
 
 		JPH_INLINE void				VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
-		{			
+		{
 			// Create sub shape id for this part
 			SubShapeID triangle_sub_shape_id = mShape2->EncodeSubShapeID(mSubShapeIDCreator2, inX, inY, inTriangle);
 
@@ -1672,7 +1677,7 @@ void HeightFieldShape::sCastSphereVsHeightField(const ShapeCast &inShapeCast, co
 
 struct HeightFieldShape::HSGetTrianglesContext
 {
-			HSGetTrianglesContext(const HeightFieldShape *inShape, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) : 
+			HSGetTrianglesContext(const HeightFieldShape *inShape, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) :
 		mDecodeCtx(inShape),
 		mShape(inShape),
 		mLocalBox(Mat44::sInverseRotationTranslation(inRotation, inPositionCOM), inBox),
@@ -1694,7 +1699,7 @@ struct HeightFieldShape::HSGetTrianglesContext
 
 	int		VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const
 	{
-		// Scale the bounding boxes of this node 
+		// Scale the bounding boxes of this node
 		Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
 		AABox4Scale(mHeightFieldScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
 
@@ -1703,8 +1708,8 @@ struct HeightFieldShape::HSGetTrianglesContext
 		return CountAndSortTrues(collides, ioProperties);
 	}
 
-	void	VisitTriangle(uint inX, uint inY, [[maybe_unused]] uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) 
-	{			
+	void	VisitTriangle(uint inX, uint inY, [[maybe_unused]] uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
+	{
 		// When the buffer is full and we cannot process the triangles, abort the height field walk. The next time GetTrianglesNext is called we will continue here.
 		if (mNumTrianglesFound + 1 > mMaxTrianglesRequested)
 		{
@@ -1773,7 +1778,7 @@ int HeightFieldShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMax
 	context.mMaterials = outMaterials;
 	context.mShouldAbort = false; // Reset the abort flag
 	context.mNumTrianglesFound = 0;
-	
+
 	// Continue (or start) walking the height field
 	context.mDecodeCtx.WalkHeightField(context);
 	return context.mNumTrianglesFound;
@@ -1815,7 +1820,7 @@ void HeightFieldShape::sCollideConvexVsHeightField(const Shape *inShape1, const
 		}
 
 		JPH_INLINE void				VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
-		{			
+		{
 			// Create ID for triangle
 			SubShapeID triangle_sub_shape_id = mShape2->EncodeSubShapeID(mSubShapeIDCreator2, inX, inY, inTriangle);
 
@@ -1871,7 +1876,7 @@ void HeightFieldShape::sCollideSphereVsHeightField(const Shape *inShape1, const
 		}
 
 		JPH_INLINE void				VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2)
-		{			
+		{
 			// Create ID for triangle
 			SubShapeID triangle_sub_shape_id = mShape2->EncodeSubShapeID(mSubShapeIDCreator2, inX, inY, inTriangle);
 
@@ -1930,24 +1935,24 @@ void HeightFieldShape::RestoreBinaryState(StreamIn &inStream)
 }
 
 void HeightFieldShape::SaveMaterialState(PhysicsMaterialList &outMaterials) const
-{ 
+{
 	outMaterials = mMaterials;
 }
 
-void HeightFieldShape::RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) 
-{ 
+void HeightFieldShape::RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials)
+{
 	mMaterials.assign(inMaterials, inMaterials + inNumMaterials);
 }
 
-Shape::Stats HeightFieldShape::GetStats() const 
-{ 
+Shape::Stats HeightFieldShape::GetStats() const
+{
 	return Stats(
-		sizeof(*this) 
-			+ mMaterials.size() * sizeof(Ref<PhysicsMaterial>) 
-			+ mRangeBlocks.size() * sizeof(RangeBlock) 
-			+ mHeightSamples.size() * sizeof(uint8) 
-			+ mActiveEdges.size() * sizeof(uint8) 
-			+ mMaterialIndices.size() * sizeof(uint8), 
+		sizeof(*this)
+			+ mMaterials.size() * sizeof(Ref<PhysicsMaterial>)
+			+ mRangeBlocks.size() * sizeof(RangeBlock)
+			+ mHeightSamples.size() * sizeof(uint8)
+			+ mActiveEdges.size() * sizeof(uint8)
+			+ mMaterialIndices.size() * sizeof(uint8),
 		mHeightSamples.empty()? 0 : Square(mSampleCount - 1) * 2);
 }
 

Jolt/Physics/Collision/Shape/HeightFieldShape.h

@@ -71,7 +71,7 @@ public:
 	Vec3							mScale = Vec3::sReplicate(1.0f);
 	uint32							mSampleCount = 0;
 
-	/// The heightfield is divided in blocks of mBlockSize * mBlockSize * 2 triangles and the acceleration structure culls blocks only, 
+	/// The heightfield is divided in blocks of mBlockSize * mBlockSize * 2 triangles and the acceleration structure culls blocks only,
 	/// bigger block sizes reduce memory consumption but also reduce query performance. Sensible values are [2, 8], does not need to be
 	/// a power of 2. Note that at run-time we'll perform one more grid subdivision, so the effective block size is half of what is provided here.
 	uint32							mBlockSize = 2;
@@ -112,7 +112,7 @@ public:
 
 	// See Shape::GetMassProperties
 	virtual MassProperties			GetMassProperties() const override;
-	
+
 	// See Shape::GetMaterial
 	virtual const PhysicsMaterial *	GetMaterial(const SubShapeID &inSubShapeID) const override;
 
@@ -140,6 +140,9 @@ public:
 	// See: Shape::CollidePoint
 	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
+	// See: Shape::ColideSoftBodyVertices
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+
 	// See Shape::GetTrianglesStart
 	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
 
@@ -180,7 +183,7 @@ protected:
 	// See: Shape::RestoreBinaryState
 	virtual void					RestoreBinaryState(StreamIn &inStream) override;
 
-private:	
+private:
 	class							DecodingContext;						///< Context class for walking through all nodes of a heightfield
 	struct							HSGetTrianglesContext;					///< Context class for GetTrianglesStart/Next
 
@@ -189,7 +192,7 @@ private:
 
 	/// Calculate bit mask for all active edges in the heightfield
 	void							CalculateActiveEdges();
-	
+
 	/// Store material indices in the least amount of bits per index possible
 	void							StoreMaterialIndices(const Array<uint8> &inMaterialIndices);
 
@@ -198,7 +201,7 @@ private:
 
 	/// Get the maximum level (amount of grids) of the tree
 	static inline uint				sGetMaxLevel(uint inNumBlocks)			{ return CountTrailingZeros(inNumBlocks); }
-	
+
 	/// Get the range block offset and stride for GetBlockOffsetAndScale
 	static inline void				sGetRangeBlockOffsetAndStride(uint inNumBlocks, uint inMaxLevel, uint &outRangeBlockOffset, uint &outRangeBlockStride);
 
@@ -210,7 +213,7 @@ private:
 
 	/// Faster version of GetPosition when block offset and scale are already known
 	inline Vec3						GetPosition(uint inX, uint inY, float inBlockOffset, float inBlockScale, bool &outNoCollision) const;
-		
+
 	/// Determine amount of bits needed to encode sub shape id
 	uint							GetSubShapeIDBits() const;
 
@@ -255,7 +258,7 @@ private:
 	uint16							mMaxSample = HeightFieldShapeConstants::cNoCollisionValue16;
 	Array<RangeBlock>				mRangeBlocks;						///< Hierarchical grid of range data describing the height variations within 1 block. The grid for level <level> starts at offset sGridOffsets[<level>]
 	Array<uint8>					mHeightSamples;						///< mBitsPerSample-bit height samples. Value [0, mMaxHeightValue] maps to highest detail grid in mRangeBlocks [mMin, mMax]. mNoCollisionValue is reserved to indicate no collision.
-	Array<uint8>					mActiveEdges;						///< (mSampleCount - 1)^2 * 3-bit active edge flags. 
+	Array<uint8>					mActiveEdges;						///< (mSampleCount - 1)^2 * 3-bit active edge flags.
 
 	/// Materials
 	PhysicsMaterialList				mMaterials;							///< The materials of square at (x, y) is: mMaterials[mMaterialIndices[x + y * (mSampleCount - 1)]]

Jolt/Physics/Collision/Shape/MeshShape.cpp

@@ -12,7 +12,6 @@
 #include <Jolt/Physics/Collision/ShapeCast.h>
 #include <Jolt/Physics/Collision/ShapeFilter.h>
 #include <Jolt/Physics/Collision/CastResult.h>
-#include <Jolt/Physics/Collision/CollidePointResult.h>
 #include <Jolt/Physics/Collision/CollideConvexVsTriangles.h>
 #include <Jolt/Physics/Collision/CollideSphereVsTriangles.h>
 #include <Jolt/Physics/Collision/CastConvexVsTriangles.h>
@@ -66,7 +65,7 @@ static JPH_INLINE const NodeCodec::Header *sGetNodeHeader(const ByteBuffer &inTr
 }
 
 // Get header for triangles
-static JPH_INLINE const TriangleCodec::TriangleHeader *sGetTriangleHeader(const ByteBuffer &inTree) 
+static JPH_INLINE const TriangleCodec::TriangleHeader *sGetTriangleHeader(const ByteBuffer &inTree)
 {
 	return inTree.Get<TriangleCodec::TriangleHeader>(NodeCodec::HeaderSize);
 }
@@ -109,11 +108,11 @@ void MeshShapeSettings::Sanitize()
 ShapeSettings::ShapeResult MeshShapeSettings::Create() const
 {
 	if (mCachedResult.IsEmpty())
-		Ref<Shape> shape = new MeshShape(*this, mCachedResult); 
+		Ref<Shape> shape = new MeshShape(*this, mCachedResult);
 	return mCachedResult;
 }
 
-MeshShape::MeshShape(const MeshShapeSettings &inSettings, ShapeResult &outResult) : 
+MeshShape::MeshShape(const MeshShapeSettings &inSettings, ShapeResult &outResult) :
 	Shape(EShapeType::Mesh, EShapeSubType::Mesh, inSettings, outResult)
 {
 	// Check if there are any triangles
@@ -185,7 +184,7 @@ MeshShape::MeshShape(const MeshShapeSettings &inSettings, ShapeResult &outResult
 
 	// Create triangle splitter
 	TriangleSplitterBinning splitter(inSettings.mTriangleVertices, indexed_triangles);
-	
+
 	// Build tree
 	AABBTreeBuilder builder(splitter, inSettings.mMaxTrianglesPerLeaf);
 	AABBTreeBuilderStats builder_stats;
@@ -344,7 +343,7 @@ MassProperties MeshShape::GetMassProperties() const
 void MeshShape::DecodeSubShapeID(const SubShapeID &inSubShapeID, const void *&outTriangleBlock, uint32 &outTriangleIndex) const
 {
 	// Get block
-	SubShapeID triangle_idx_subshape_id;	
+	SubShapeID triangle_idx_subshape_id;
 	uint32 block_id = inSubShapeID.PopID(NodeCodec::DecodingContext::sTriangleBlockIDBits(mTree), triangle_idx_subshape_id);
 	outTriangleBlock = NodeCodec::DecodingContext::sGetTriangleBlockStart(&mTree[0], block_id);
 
@@ -360,7 +359,7 @@ uint MeshShape::GetMaterialIndex(const SubShapeID &inSubShapeID) const
 	const void *block_start;
 	uint32 triangle_idx;
 	DecodeSubShapeID(inSubShapeID, block_start, triangle_idx);
-		
+
 	// Fetch the flags
 	uint8 flags = TriangleCodec::DecodingContext::sGetFlags(block_start, triangle_idx);
 	return flags & FLAGS_MATERIAL_MASK;
@@ -375,8 +374,8 @@ const PhysicsMaterial *MeshShape::GetMaterial(const SubShapeID &inSubShapeID) co
 	return mMaterials[GetMaterialIndex(inSubShapeID)];
 }
 
-Vec3 MeshShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const 
-{ 
+Vec3 MeshShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
 	// Decode ID
 	const void *block_start;
 	uint32 triangle_idx;
@@ -398,7 +397,7 @@ void MeshShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDire
 	uint32 triangle_idx;
 	DecodeSubShapeID(inSubShapeID, block_start, triangle_idx);
 
-	// Decode triangle	
+	// Decode triangle
 	const TriangleCodec::DecodingContext triangle_ctx(sGetTriangleHeader(mTree));
 	outVertices.resize(3);
 	triangle_ctx.GetTriangle(block_start, triangle_idx, outVertices[0], outVertices[1], outVertices[2]);
@@ -421,7 +420,7 @@ AABox MeshShape::GetLocalBounds() const
 	return AABox(Vec3::sLoadFloat3Unsafe(header->mRootBoundsMin), Vec3::sLoadFloat3Unsafe(header->mRootBoundsMax));
 }
 
-uint MeshShape::GetSubShapeIDBitsRecursive() const 
+uint MeshShape::GetSubShapeIDBitsRecursive() const
 {
 	return NodeCodec::DecodingContext::sTriangleBlockIDBits(mTree) + NumTriangleBits;
 }
@@ -459,12 +458,12 @@ JPH_INLINE void MeshShape::WalkTreePerTriangle(const SubShapeIDCreator &inSubSha
 			return mVisitor.ShouldVisitNode(inStackTop);
 		}
 
-		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) 
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
 		{
 			return mVisitor.VisitNodes(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, ioProperties, inStackTop);
 		}
 
-		JPH_INLINE void		VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, uint32 inTriangleBlockID) 
+		JPH_INLINE void		VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, uint32 inTriangleBlockID)
 		{
 			// Create ID for triangle block
 			SubShapeIDCreator block_sub_shape_id = mSubShapeIDCreator2.PushID(inTriangleBlockID, mTriangleBlockIDBits);
@@ -526,13 +525,13 @@ void MeshShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransfor
 				return true;
 			}
 
-			JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) 
+			JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
 			{
 				UVec4 valid = UVec4::sOr(UVec4::sOr(Vec4::sLess(inBoundsMinX, inBoundsMaxX), Vec4::sLess(inBoundsMinY, inBoundsMaxY)), Vec4::sLess(inBoundsMinZ, inBoundsMaxZ));
 				return CountAndSortTrues(valid, ioProperties);
 			}
 
-			JPH_INLINE void		VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, [[maybe_unused]] uint32 inTriangleBlockID) 
+			JPH_INLINE void		VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, [[maybe_unused]] uint32 inTriangleBlockID)
 			{
 				JPH_ASSERT(inNumTriangles <= MaxTrianglesPerLeaf);
 				Vec3 vertices[MaxTrianglesPerLeaf * 3];
@@ -563,7 +562,7 @@ void MeshShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransfor
 			bool									mDrawTriangleGroups;
 			int										mColorIdx = 0;
 		};
-		
+
 		Array<DebugRenderer::Triangle> triangles;
 		Visitor visitor { triangles, mMaterials, mCachedUseMaterialColors, mCachedTrianglesColoredPerGroup };
 		WalkTree(visitor);
@@ -599,13 +598,13 @@ void MeshShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransfor
 				return true;
 			}
 
-			JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) 
+			JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
 			{
 				UVec4 valid = UVec4::sOr(UVec4::sOr(Vec4::sLess(inBoundsMinX, inBoundsMaxX), Vec4::sLess(inBoundsMinY, inBoundsMaxY)), Vec4::sLess(inBoundsMinZ, inBoundsMaxZ));
 				return CountAndSortTrues(valid, ioProperties);
 			}
 
-			JPH_INLINE void		VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, uint32 inTriangleBlockID) 
+			JPH_INLINE void		VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, uint32 inTriangleBlockID)
 			{
 				// Decode vertices and flags
 				JPH_ASSERT(inNumTriangles <= MaxTrianglesPerLeaf);
@@ -648,7 +647,7 @@ bool MeshShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShap
 
 	struct Visitor
 	{
-		JPH_INLINE explicit	Visitor(RayCastResult &ioHit) : 
+		JPH_INLINE explicit	Visitor(RayCastResult &ioHit) :
 			mHit(ioHit)
 		{
 		}
@@ -663,16 +662,16 @@ bool MeshShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShap
 			return mDistanceStack[inStackTop] < mHit.mFraction;
 		}
 
-		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) 
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
 		{
 			// Test bounds of 4 children
 			Vec4 distance = RayAABox4(mRayOrigin, mRayInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
-	
+
 			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
 			return SortReverseAndStore(distance, mHit.mFraction, ioProperties, &mDistanceStack[inStackTop]);
 		}
 
-		JPH_INLINE void		VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, uint32 inTriangleBlockID) 
+		JPH_INLINE void		VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, uint32 inTriangleBlockID)
 		{
 			// Test against triangles
 			uint32 triangle_idx;
@@ -716,7 +715,7 @@ void MeshShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSe
 
 	struct Visitor
 	{
-		JPH_INLINE explicit	Visitor(CastRayCollector &ioCollector) : 
+		JPH_INLINE explicit	Visitor(CastRayCollector &ioCollector) :
 			mCollector(ioCollector)
 		{
 		}
@@ -731,16 +730,16 @@ void MeshShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSe
 			return mDistanceStack[inStackTop] < mCollector.GetEarlyOutFraction();
 		}
 
-		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) 
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
 		{
 			// Test bounds of 4 children
 			Vec4 distance = RayAABox4(mRayOrigin, mRayInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ);
-	
+
 			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
 			return SortReverseAndStore(distance, mCollector.GetEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
 		}
 
-		JPH_INLINE void		VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, [[maybe_unused]] uint8 inActiveEdges, SubShapeID inSubShapeID2) 
+		JPH_INLINE void		VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, [[maybe_unused]] uint8 inActiveEdges, SubShapeID inSubShapeID2)
 		{
 			// Back facing check
 			if (mBackFaceMode == EBackFaceMode::IgnoreBackFaces && (inV2 - inV0).Cross(inV1 - inV0).Dot(mRayDirection) < 0)
@@ -776,38 +775,12 @@ void MeshShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSe
 
 void MeshShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
 {
-	// First test if we're inside our bounding box
-	AABox bounds = GetLocalBounds();
-	if (bounds.Contains(inPoint))
-	{
-		// A collector that just counts the number of hits
-		class HitCountCollector : public CastRayCollector	
-		{
-		public:
-			virtual void	AddHit(const RayCastResult &inResult) override
-			{
-				// Store the last sub shape ID so that we can provide something to our outer hit collector
-				mSubShapeID = inResult.mSubShapeID2;
-
-				++mHitCount;
-			}
-
-			int				mHitCount = 0;
-			SubShapeID		mSubShapeID;
-		};
-		HitCountCollector collector;
-
-		// Configure the raycast
-		RayCastSettings settings;
-		settings.mBackFaceMode = EBackFaceMode::CollideWithBackFaces;
-
-		// Cast a ray that's 10% longer than the heigth of our bounding box
-		CastRay(RayCast { inPoint, 1.1f * bounds.GetSize().GetY() * Vec3::sAxisY() }, settings, inSubShapeIDCreator, collector, inShapeFilter);
+	sCollidePointUsingRayCast(*this, inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter);
+}
 
-		// Odd amount of hits means inside
-		if ((collector.mHitCount & 1) == 1)
-			ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), collector.mSubShapeID });
-	}
+void MeshShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+{
+	sCollideSoftBodyVerticesUsingRayCast(*this, inCenterOfMassTransform, ioVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex);
 }
 
 void MeshShape::sCastConvexVsMesh(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
@@ -828,7 +801,7 @@ void MeshShape::sCastConvexVsMesh(const ShapeCast &inShapeCast, const ShapeCastS
 			return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction();
 		}
 
-		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) 
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
 		{
 			// Scale the bounding boxes of this node
 			Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
@@ -839,12 +812,12 @@ void MeshShape::sCastConvexVsMesh(const ShapeCast &inShapeCast, const ShapeCastS
 
 			// Test bounds of 4 children
 			Vec4 distance = RayAABox4(mBoxCenter, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
-	
+
 			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
 			return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
 		}
 
-		JPH_INLINE void		VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, SubShapeID inSubShapeID2) 
+		JPH_INLINE void		VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, SubShapeID inSubShapeID2)
 		{
 			Cast(inV0, inV1, inV2, inActiveEdges, inSubShapeID2);
 		}
@@ -883,7 +856,7 @@ void MeshShape::sCastSphereVsMesh(const ShapeCast &inShapeCast, const ShapeCastS
 			return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction();
 		}
 
-		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) 
+		JPH_INLINE int		VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop)
 		{
 			// Scale the bounding boxes of this node
 			Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z;
@@ -894,12 +867,12 @@ void MeshShape::sCastSphereVsMesh(const ShapeCast &inShapeCast, const ShapeCastS
 
 			// Test bounds of 4 children
 			Vec4 distance = RayAABox4(mStart, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z);
-	
+
 			// Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom)
 			return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]);
 		}
 
-		JPH_INLINE void		VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, SubShapeID inSubShapeID2) 
+		JPH_INLINE void		VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, SubShapeID inSubShapeID2)
 		{
 			Cast(inV0, inV1, inV2, inActiveEdges, inSubShapeID2);
 		}
@@ -918,7 +891,7 @@ void MeshShape::sCastSphereVsMesh(const ShapeCast &inShapeCast, const ShapeCastS
 
 struct MeshShape::MSGetTrianglesContext
 {
-	JPH_INLINE 		MSGetTrianglesContext(const MeshShape *inShape, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) : 
+	JPH_INLINE 		MSGetTrianglesContext(const MeshShape *inShape, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) :
 		mDecodeCtx(sGetNodeHeader(inShape->mTree)),
 		mShape(inShape),
 		mLocalBox(Mat44::sInverseRotationTranslation(inRotation, inPositionCOM), inBox),
@@ -949,7 +922,7 @@ struct MeshShape::MSGetTrianglesContext
 		return CountAndSortTrues(collides, ioProperties);
 	}
 
-	JPH_INLINE void	VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, [[maybe_unused]] uint32 inTriangleBlockID) 
+	JPH_INLINE void	VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, [[maybe_unused]] uint32 inTriangleBlockID)
 	{
 		// When the buffer is full and we cannot process the triangles, abort the tree walk. The next time GetTrianglesNext is called we will continue here.
 		if (mNumTrianglesFound + inNumTriangles > mMaxTrianglesRequested)
@@ -995,7 +968,7 @@ struct MeshShape::MSGetTrianglesContext
 				// Decode triangle flags
 				uint8 flags[MaxTrianglesPerLeaf];
 				TriangleCodec::DecodingContext::sGetFlags(inTriangles, inNumTriangles, flags);
-	
+
 				// Store materials
 				for (const uint8 *f = flags, *f_end = f + inNumTriangles; f < f_end; ++f)
 					*mMaterials++ = mShape->mMaterials[*f & FLAGS_MATERIAL_MASK].GetPtr();
@@ -1043,7 +1016,7 @@ int MeshShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTriangl
 	context.mMaterials = outMaterials;
 	context.mShouldAbort = false; // Reset the abort flag
 	context.mNumTrianglesFound = 0;
-	
+
 	// Continue (or start) walking the tree
 	const TriangleCodec::DecodingContext triangle_ctx(sGetTriangleHeader(mTree));
 	const uint8 *buffer_start = &mTree[0];
@@ -1086,7 +1059,7 @@ void MeshShape::sCollideConvexVsMesh(const Shape *inShape1, const Shape *inShape
 			return CountAndSortTrues(collides, ioProperties);
 		}
 
-		JPH_INLINE void	VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, SubShapeID inSubShapeID2) 
+		JPH_INLINE void	VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, SubShapeID inSubShapeID2)
 		{
 			Collide(inV0, inV1, inV2, inActiveEdges, inSubShapeID2);
 		}
@@ -1131,7 +1104,7 @@ void MeshShape::sCollideSphereVsMesh(const Shape *inShape1, const Shape *inShape
 			return CountAndSortTrues(collides, ioProperties);
 		}
 
-		JPH_INLINE void	VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, SubShapeID inSubShapeID2) 
+		JPH_INLINE void	VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, SubShapeID inSubShapeID2)
 		{
 			Collide(inV0, inV1, inV2, inActiveEdges, inSubShapeID2);
 		}
@@ -1156,12 +1129,12 @@ void MeshShape::RestoreBinaryState(StreamIn &inStream)
 }
 
 void MeshShape::SaveMaterialState(PhysicsMaterialList &outMaterials) const
-{ 
+{
 	outMaterials = mMaterials;
 }
 
-void MeshShape::RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) 
-{ 
+void MeshShape::RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials)
+{
 	mMaterials.assign(inMaterials, inMaterials + inNumMaterials);
 }
 
@@ -1187,7 +1160,7 @@ Shape::Stats MeshShape::GetStats() const
 			return CountAndSortTrues(valid, ioProperties);
 		}
 
-		JPH_INLINE void		VisitTriangles([[maybe_unused]] const TriangleCodec::DecodingContext &ioContext, [[maybe_unused]] const void *inTriangles, int inNumTriangles, [[maybe_unused]] uint32 inTriangleBlockID) 
+		JPH_INLINE void		VisitTriangles([[maybe_unused]] const TriangleCodec::DecodingContext &ioContext, [[maybe_unused]] const void *inTriangles, int inNumTriangles, [[maybe_unused]] uint32 inTriangleBlockID)
 		{
 			mNumTriangles += inNumTriangles;
 		}
@@ -1197,7 +1170,7 @@ Shape::Stats MeshShape::GetStats() const
 
 	Visitor visitor;
 	WalkTree(visitor);
-	
+
 	return Stats(sizeof(*this) + mMaterials.size() * sizeof(Ref<PhysicsMaterial>) + mTree.size() * sizeof(uint8), visitor.mNumTriangles);
 }
 

Jolt/Physics/Collision/Shape/MeshShape.h

@@ -75,7 +75,7 @@ public:
 
 	// See Shape::GetMassProperties
 	virtual MassProperties			GetMassProperties() const override;
-	
+
 	// See Shape::GetMaterial
 	virtual const PhysicsMaterial *	GetMaterial(const SubShapeID &inSubShapeID) const override;
 
@@ -102,10 +102,13 @@ public:
 	virtual void					CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
 	/// See: Shape::CollidePoint
-	/// Note that for CollidePoint to work for a mesh shape, the mesh needs to be closed (a manifold) or multiple non-intersecting manifolds. Triangles may be facing the interior of the manifold. 
+	/// Note that for CollidePoint to work for a mesh shape, the mesh needs to be closed (a manifold) or multiple non-intersecting manifolds. Triangles may be facing the interior of the manifold.
 	/// Insideness is tested by counting the amount of triangles encountered when casting an infinite ray from inPoint. If the number of hits is odd we're inside, if it's even we're outside.
 	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
+	// See: Shape::ColideSoftBodyVertices
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+
 	// See Shape::GetTrianglesStart
 	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
 
@@ -143,7 +146,7 @@ private:
 	struct							MSGetTrianglesContext;										///< Context class for GetTrianglesStart/Next
 
 	static constexpr int			NumTriangleBits = 3;										///< How many bits to reserve to encode the triangle index
-	static constexpr int			MaxTrianglesPerLeaf = 1 << NumTriangleBits;					///< Number of triangles that are stored max per leaf aabb node 
+	static constexpr int			MaxTrianglesPerLeaf = 1 << NumTriangleBits;					///< Number of triangles that are stored max per leaf aabb node
 
 	/// Find and flag active edges
 	static void						sFindActiveEdges(const VertexList &inVertices, IndexedTriangleList &ioIndices);

Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.cpp

@@ -23,9 +23,9 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(OffsetCenterOfMassShapeSettings)
 }
 
 ShapeSettings::ShapeResult OffsetCenterOfMassShapeSettings::Create() const
-{ 
+{
 	if (mCachedResult.IsEmpty())
-		Ref<Shape> shape = new OffsetCenterOfMassShape(*this, mCachedResult); 
+		Ref<Shape> shape = new OffsetCenterOfMassShape(*this, mCachedResult);
 	return mCachedResult;
 }
 
@@ -48,7 +48,7 @@ AABox OffsetCenterOfMassShape::GetLocalBounds() const
 }
 
 AABox OffsetCenterOfMassShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
-{ 
+{
 	return mInnerShape->GetWorldSpaceBounds(inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale);
 }
 
@@ -62,8 +62,8 @@ TransformedShape OffsetCenterOfMassShape::GetSubShapeTransformedShape(const SubS
 	return ts;
 }
 
-Vec3 OffsetCenterOfMassShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const 
-{ 
+Vec3 OffsetCenterOfMassShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
 	// Transform surface position to local space and pass call on
 	return mInnerShape->GetSurfaceNormal(inSubShapeID, inLocalSurfacePosition + mOffset);
 }
@@ -96,7 +96,7 @@ void OffsetCenterOfMassShape::DrawGetSupportingFace(DebugRenderer *inRenderer, R
 #endif // JPH_DEBUG_RENDERER
 
 bool OffsetCenterOfMassShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
-{	
+{
 	// Transform the ray to local space
 	RayCast ray = inRay;
 	ray.mOrigin += mOffset;
@@ -127,6 +127,11 @@ void OffsetCenterOfMassShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCrea
 	mInnerShape->CollidePoint(inPoint + mOffset, inSubShapeIDCreator, ioCollector, inShapeFilter);
 }
 
+void OffsetCenterOfMassShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+{
+	mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform.PreTranslated(-mOffset), ioVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex);
+}
+
 void OffsetCenterOfMassShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const
 {
 	// Test shape filter
@@ -142,7 +147,7 @@ void OffsetCenterOfMassShape::TransformShape(Mat44Arg inCenterOfMassTransform, T
 }
 
 void OffsetCenterOfMassShape::sCollideOffsetCenterOfMassVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
-{	
+{
 	JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::OffsetCenterOfMass);
 	const OffsetCenterOfMassShape *shape1 = static_cast<const OffsetCenterOfMassShape *>(inShape1);
 

Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h

@@ -49,7 +49,7 @@ public:
 
 	// See Shape::GetLocalBounds
 	virtual AABox					GetLocalBounds() const override;
-		
+
 	// See Shape::GetWorldSpaceBounds
 	virtual AABox					GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
 	using Shape::GetWorldSpaceBounds;
@@ -95,6 +95,9 @@ public:
 	// See: Shape::CollidePoint
 	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
+	// See: Shape::ColideSoftBodyVertices
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+
 	// See Shape::CollectTransformedShapes
 	virtual void					CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override;
 

Jolt/Physics/Collision/Shape/RotatedTranslatedShape.cpp

@@ -24,9 +24,9 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(RotatedTranslatedShapeSettings)
 }
 
 ShapeSettings::ShapeResult RotatedTranslatedShapeSettings::Create() const
-{ 
+{
 	if (mCachedResult.IsEmpty())
-		Ref<Shape> shape = new RotatedTranslatedShape(*this, mCachedResult); 
+		Ref<Shape> shape = new RotatedTranslatedShape(*this, mCachedResult);
 	return mCachedResult;
 }
 
@@ -37,7 +37,7 @@ RotatedTranslatedShape::RotatedTranslatedShape(const RotatedTranslatedShapeSetti
 		return;
 
 	// Calculate center of mass position
-	mCenterOfMass = inSettings.mPosition + inSettings.mRotation * mInnerShape->GetCenterOfMass(); 
+	mCenterOfMass = inSettings.mPosition + inSettings.mRotation * mInnerShape->GetCenterOfMass();
 
 	// Store rotation (position is always zero because we center around the center of mass)
 	mRotation = inSettings.mRotation;
@@ -50,7 +50,7 @@ RotatedTranslatedShape::RotatedTranslatedShape(Vec3Arg inPosition, QuatArg inRot
 	DecoratedShape(EShapeSubType::RotatedTranslated, inShape)
 {
 	// Calculate center of mass position
-	mCenterOfMass = inPosition + inRotation * mInnerShape->GetCenterOfMass(); 
+	mCenterOfMass = inPosition + inRotation * mInnerShape->GetCenterOfMass();
 
 	// Store rotation (position is always zero because we center around the center of mass)
 	mRotation = inRotation;
@@ -71,7 +71,7 @@ AABox RotatedTranslatedShape::GetLocalBounds() const
 }
 
 AABox RotatedTranslatedShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
-{ 
+{
 	Mat44 transform = inCenterOfMassTransform * Mat44::sRotation(mRotation);
 	return mInnerShape->GetWorldSpaceBounds(transform, TransformScale(inScale));
 }
@@ -86,8 +86,8 @@ TransformedShape RotatedTranslatedShape::GetSubShapeTransformedShape(const SubSh
 	return ts;
 }
 
-Vec3 RotatedTranslatedShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const 
-{ 
+Vec3 RotatedTranslatedShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
 	// Transform surface position to local space and pass call on
 	Mat44 transform = Mat44::sRotation(mRotation.Conjugated());
 	Vec3 normal = mInnerShape->GetSurfaceNormal(inSubShapeID, transform * inLocalSurfacePosition);
@@ -129,7 +129,7 @@ void RotatedTranslatedShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RM
 #endif // JPH_DEBUG_RENDERER
 
 bool RotatedTranslatedShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
-{	
+{
 	// Transform the ray
 	Mat44 transform = Mat44::sRotation(mRotation.Conjugated());
 	RayCast ray = inRay.Transformed(transform);
@@ -161,6 +161,11 @@ void RotatedTranslatedShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreat
 	mInnerShape->CollidePoint(transform * inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter);
 }
 
+void RotatedTranslatedShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+{
+	mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform * Mat44::sRotation(mRotation), ioVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex);
+}
+
 void RotatedTranslatedShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const
 {
 	// Test shape filter
@@ -176,7 +181,7 @@ void RotatedTranslatedShape::TransformShape(Mat44Arg inCenterOfMassTransform, Tr
 }
 
 void RotatedTranslatedShape::sCollideRotatedTranslatedVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
-{	
+{
 	JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::RotatedTranslated);
 	const RotatedTranslatedShape *shape1 = static_cast<const RotatedTranslatedShape *>(inShape1);
 

Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h

@@ -56,7 +56,7 @@ public:
 
 	// See Shape::GetLocalBounds
 	virtual AABox					GetLocalBounds() const override;
-		
+
 	// See Shape::GetWorldSpaceBounds
 	virtual AABox					GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
 	using Shape::GetWorldSpaceBounds;
@@ -97,6 +97,9 @@ public:
 	// See: Shape::CollidePoint
 	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
+	// See: Shape::ColideSoftBodyVertices
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+
 	// See Shape::CollectTransformedShapes
 	virtual void					CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override;
 
@@ -144,7 +147,7 @@ private:
 
 		return ScaleHelpers::RotateScale(mRotation, inScale);
 	}
-		
+
 	bool							mIsRotationIdentity;									///< If mRotation is close to identity (put here because it falls in padding bytes)
 	Vec3							mCenterOfMass;											///< Position of the center of mass
 	Quat							mRotation;												///< Rotation of the child shape

Jolt/Physics/Collision/Shape/ScaledShape.cpp

@@ -23,9 +23,9 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(ScaledShapeSettings)
 }
 
 ShapeSettings::ShapeResult ScaledShapeSettings::Create() const
-{ 
+{
 	if (mCachedResult.IsEmpty())
-		Ref<Shape> shape = new ScaledShape(*this, mCachedResult); 
+		Ref<Shape> shape = new ScaledShape(*this, mCachedResult);
 	return mCachedResult;
 }
 
@@ -47,12 +47,12 @@ MassProperties ScaledShape::GetMassProperties() const
 }
 
 AABox ScaledShape::GetLocalBounds() const
-{ 
+{
 	return mInnerShape->GetLocalBounds().Scaled(mScale);
 }
 
 AABox ScaledShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
-{ 
+{
 	return mInnerShape->GetWorldSpaceBounds(inCenterOfMassTransform, inScale * mScale);
 }
 
@@ -66,8 +66,8 @@ TransformedShape ScaledShape::GetSubShapeTransformedShape(const SubShapeID &inSu
 	return ts;
 }
 
-Vec3 ScaledShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const 
-{ 
+Vec3 ScaledShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
 	// Transform the surface point to local space and pass the query on
 	Vec3 normal = mInnerShape->GetSurfaceNormal(inSubShapeID, inLocalSurfacePosition / mScale);
 
@@ -98,8 +98,8 @@ void ScaledShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg in
 	mInnerShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform, inScale * mScale, inColor, inDrawSupportDirection);
 }
 
-void ScaledShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const 
-{ 
+void ScaledShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+{
 	mInnerShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform, inScale * mScale);
 }
 #endif // JPH_DEBUG_RENDERER
@@ -132,7 +132,12 @@ void ScaledShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubSh
 	mInnerShape->CollidePoint(inv_scale * inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter);
 }
 
-void ScaledShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const 
+void ScaledShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+{
+	mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform * Mat44::sScale(mScale), ioVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex);
+}
+
+void ScaledShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const
 {
 	// Test shape filter
 	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
@@ -171,7 +176,7 @@ bool ScaledShape::IsValidScale(Vec3Arg inScale) const
 }
 
 void ScaledShape::sCollideScaledVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)
-{	
+{
 	JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::Scaled);
 	const ScaledShape *shape1 = static_cast<const ScaledShape *>(inShape1);
 

Jolt/Physics/Collision/Shape/ScaledShape.h

@@ -52,7 +52,7 @@ public:
 
 	// See Shape::GetLocalBounds
 	virtual AABox					GetLocalBounds() const override;
-		
+
 	// See Shape::GetWorldSpaceBounds
 	virtual AABox					GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
 	using Shape::GetWorldSpaceBounds;
@@ -93,6 +93,9 @@ public:
 	// See: Shape::CollidePoint
 	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
+	// See: Shape::ColideSoftBodyVertices
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+
 	// See Shape::CollectTransformedShapes
 	virtual void					CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override;
 

Jolt/Physics/Collision/Shape/Shape.cpp

@@ -9,6 +9,10 @@
 #include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
 #include <Jolt/Physics/Collision/TransformedShape.h>
 #include <Jolt/Physics/Collision/PhysicsMaterial.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/CollidePointResult.h>
+#include <Jolt/Physics/SoftBody/SoftBodyVertex.h>
 #include <Jolt/Core/StreamIn.h>
 #include <Jolt/Core/StreamOut.h>
 #include <Jolt/Core/Factory.h>
@@ -61,7 +65,7 @@ void Shape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCol
 }
 
 void Shape::SaveBinaryState(StreamOut &inStream) const
-{ 
+{
 	inStream.Write(mShapeSubType);
 	inStream.Write(mUserData);
 }
@@ -327,4 +331,75 @@ Shape::ShapeResult Shape::ScaleShape(Vec3Arg inScale) const
 	return compound.Create();
 }
 
+void Shape::sCollideSoftBodyVerticesUsingRayCast(const Shape &inShape, Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex)
+{
+	Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation();
+
+	for (SoftBodyVertex &v : ioVertices)
+		if (v.mInvMass > 0.0f)
+		{
+			// Calculate the distance we will move this frame
+			Vec3 movement = v.mVelocity * inDeltaTime + inDisplacementDueToGravity;
+
+			RayCastResult hit;
+			hit.mFraction = 2.0f; // Add a little extra distance in case the particle speeds up
+
+			RayCast ray(v.mPosition - 0.5f * movement, movement); // Start a little early in case we penetrated before
+
+			if (inShape.CastRay(ray.Transformed(inverse_transform), SubShapeIDCreator(), hit))
+			{
+				// Calculate penetration
+				float penetration = (hit.mFraction - 0.5f) * movement.Length();
+				if (penetration > v.mLargestPenetration)
+				{
+					v.mLargestPenetration = penetration;
+
+					// Calculate contact point and normal
+					Vec3 point = ray.GetPointOnRay(hit.mFraction);
+					Vec3 normal = inCenterOfMassTransform.Multiply3x3(inShape.GetSurfaceNormal(hit.mSubShapeID2, inverse_transform * point));
+
+					// Store collision
+					v.mCollisionPlane = Plane::sFromPointAndNormal(point, normal);
+					v.mCollidingShapeIndex = inCollidingShapeIndex;
+				}
+			}
+		}
+}
+
+void Shape::sCollidePointUsingRayCast(const Shape &inShape, Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter)
+{
+	// First test if we're inside our bounding box
+	AABox bounds = inShape.GetLocalBounds();
+	if (bounds.Contains(inPoint))
+	{
+		// A collector that just counts the number of hits
+		class HitCountCollector : public CastRayCollector
+		{
+		public:
+			virtual void	AddHit(const RayCastResult &inResult) override
+			{
+				// Store the last sub shape ID so that we can provide something to our outer hit collector
+				mSubShapeID = inResult.mSubShapeID2;
+
+				++mHitCount;
+			}
+
+			int				mHitCount = 0;
+			SubShapeID		mSubShapeID;
+		};
+		HitCountCollector collector;
+
+		// Configure the raycast
+		RayCastSettings settings;
+		settings.mBackFaceMode = EBackFaceMode::CollideWithBackFaces;
+
+		// Cast a ray that's 10% longer than the heigth of our bounding box
+		inShape.CastRay(RayCast { inPoint, 1.1f * bounds.GetSize().GetY() * Vec3::sAxisY() }, settings, inSubShapeIDCreator, collector, inShapeFilter);
+
+		// Odd amount of hits means inside
+		if ((collector.mHitCount & 1) == 1)
+			ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), collector.mSubShapeID });
+	}
+}
+
 JPH_NAMESPACE_END

Jolt/Physics/Collision/Shape/Shape.h

@@ -32,6 +32,7 @@ class SubShapeID;
 class PhysicsMaterial;
 class TransformedShape;
 class Plane;
+class SoftBodyVertex;
 class Shape;
 class StreamOut;
 class StreamIn;
@@ -58,7 +59,8 @@ enum class EShapeType : uint8
 	Decorated,						///< Used by DecoratedShape
 	Mesh,							///< Used by MeshShape
 	HeightField,					///< Used by HeightFieldShape
-	
+	SoftBody,						///< Used by SoftBodyShape
+
 	// User defined shapes
 	User1,
 	User2,
@@ -77,11 +79,11 @@ enum class EShapeSubType : uint8
 	TaperedCapsule,
 	Cylinder,
 	ConvexHull,
-	
+
 	// Compound shapes
 	StaticCompound,
 	MutableCompound,
-	
+
 	// Decorated shapes
 	RotatedTranslated,
 	Scaled,
@@ -90,7 +92,8 @@ enum class EShapeSubType : uint8
 	// Other shapes
 	Mesh,
 	HeightField,
-	
+	SoftBody,
+
 	// User defined shapes
 	User1,
 	User2,
@@ -113,7 +116,7 @@ enum class EShapeSubType : uint8
 };
 
 // Sets of shape sub types
-static constexpr EShapeSubType sAllSubShapeTypes[] = { EShapeSubType::Sphere, EShapeSubType::Box, EShapeSubType::Triangle, EShapeSubType::Capsule, EShapeSubType::TaperedCapsule, EShapeSubType::Cylinder, EShapeSubType::ConvexHull, EShapeSubType::StaticCompound, EShapeSubType::MutableCompound, EShapeSubType::RotatedTranslated, EShapeSubType::Scaled, EShapeSubType::OffsetCenterOfMass, EShapeSubType::Mesh, EShapeSubType::HeightField, EShapeSubType::User1, EShapeSubType::User2, EShapeSubType::User3, EShapeSubType::User4, EShapeSubType::User5, EShapeSubType::User6, EShapeSubType::User7, EShapeSubType::User8, EShapeSubType::UserConvex1, EShapeSubType::UserConvex2, EShapeSubType::UserConvex3, EShapeSubType::UserConvex4, EShapeSubType::UserConvex5, EShapeSubType::UserConvex6, EShapeSubType::UserConvex7, EShapeSubType::UserConvex8 };
+static constexpr EShapeSubType sAllSubShapeTypes[] = { EShapeSubType::Sphere, EShapeSubType::Box, EShapeSubType::Triangle, EShapeSubType::Capsule, EShapeSubType::TaperedCapsule, EShapeSubType::Cylinder, EShapeSubType::ConvexHull, EShapeSubType::StaticCompound, EShapeSubType::MutableCompound, EShapeSubType::RotatedTranslated, EShapeSubType::Scaled, EShapeSubType::OffsetCenterOfMass, EShapeSubType::Mesh, EShapeSubType::HeightField, EShapeSubType::SoftBody, EShapeSubType::User1, EShapeSubType::User2, EShapeSubType::User3, EShapeSubType::User4, EShapeSubType::User5, EShapeSubType::User6, EShapeSubType::User7, EShapeSubType::User8, EShapeSubType::UserConvex1, EShapeSubType::UserConvex2, EShapeSubType::UserConvex3, EShapeSubType::UserConvex4, EShapeSubType::UserConvex5, EShapeSubType::UserConvex6, EShapeSubType::UserConvex7, EShapeSubType::UserConvex8 };
 static constexpr EShapeSubType sConvexSubShapeTypes[] = { EShapeSubType::Sphere, EShapeSubType::Box, EShapeSubType::Triangle, EShapeSubType::Capsule, EShapeSubType::TaperedCapsule, EShapeSubType::Cylinder, EShapeSubType::ConvexHull, EShapeSubType::UserConvex1, EShapeSubType::UserConvex2, EShapeSubType::UserConvex3, EShapeSubType::UserConvex4, EShapeSubType::UserConvex5, EShapeSubType::UserConvex6, EShapeSubType::UserConvex7, EShapeSubType::UserConvex8 };
 static constexpr EShapeSubType sCompoundSubShapeTypes[] = { EShapeSubType::StaticCompound, EShapeSubType::MutableCompound };
 static constexpr EShapeSubType sDecoratorSubShapeTypes[] = { EShapeSubType::RotatedTranslated, EShapeSubType::Scaled, EShapeSubType::OffsetCenterOfMass };
@@ -122,7 +125,7 @@ static constexpr EShapeSubType sDecoratorSubShapeTypes[] = { EShapeSubType::Rota
 static constexpr uint NumSubShapeTypes = (uint)size(sAllSubShapeTypes);
 
 /// Names of sub shape types
-static constexpr const char *sSubShapeTypeNames[] = { "Sphere", "Box", "Triangle", "Capsule", "TaperedCapsule", "Cylinder", "ConvexHull", "StaticCompound", "MutableCompound", "RotatedTranslated", "Scaled", "OffsetCenterOfMass", "Mesh", "HeightField", "User1", "User2", "User3", "User4", "User5", "User6", "User7", "User8", "UserConvex1", "UserConvex2", "UserConvex3", "UserConvex4", "UserConvex5", "UserConvex6", "UserConvex7", "UserConvex8" };
+static constexpr const char *sSubShapeTypeNames[] = { "Sphere", "Box", "Triangle", "Capsule", "TaperedCapsule", "Cylinder", "ConvexHull", "StaticCompound", "MutableCompound", "RotatedTranslated", "Scaled", "OffsetCenterOfMass", "Mesh", "HeightField", "SoftBody", "User1", "User2", "User3", "User4", "User5", "User6", "User7", "User8", "UserConvex1", "UserConvex2", "UserConvex3", "UserConvex4", "UserConvex5", "UserConvex6", "UserConvex7", "UserConvex8" };
 static_assert(size(sSubShapeTypeNames) == NumSubShapeTypes);
 
 /// Class that can construct shapes and that is serializable using the ObjectStream system.
@@ -138,7 +141,7 @@ public:
 
 	using ShapeResult = Result<Ref<Shape>>;
 
-	/// Create a shape according to the settings specified by this object. 
+	/// Create a shape according to the settings specified by this object.
 	virtual ShapeResult				Create() const = 0;
 
 	/// User data (to be used freely by the application)
@@ -217,7 +220,7 @@ public:
 		return bounds;
 	}
 
-	/// Returns the radius of the biggest sphere that fits entirely in the shape. In case this shape consists of multiple sub shapes, it returns the smallest sphere of the parts. 
+	/// Returns the radius of the biggest sphere that fits entirely in the shape. In case this shape consists of multiple sub shapes, it returns the smallest sphere of the parts.
 	/// This can be used as a measure of how far the shape can be moved without risking going through geometry.
 	virtual float					GetInnerRadius() const = 0;
 
@@ -235,7 +238,7 @@ public:
 	using SupportingFace = StaticArray<Vec3, 32>;
 
 	/// Get the vertices of the face that faces inDirection the most (includes any convex radius). Note that this function can only return faces of
-	/// convex shapes or triangles, which is why a sub shape ID to get to that leaf must be provided. 
+	/// convex shapes or triangles, which is why a sub shape ID to get to that leaf must be provided.
 	/// @param inSubShapeID Sub shape ID of target shape
 	/// @param inDirection Direction that the face should be facing (in local space to this shape)
 	/// @param inCenterOfMassTransform Transform to transform outVertices with
@@ -270,7 +273,7 @@ public:
 		, RVec3Arg inBaseOffset
 #endif
 		) const = 0;
-	
+
 #ifdef JPH_DEBUG_RENDERER
 	/// Draw the shape at a particular location with a particular color (debugging purposes)
 	virtual void					Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const = 0;
@@ -292,18 +295,26 @@ public:
 	/// If you want the surface normal of the hit use GetSurfaceNormal(collected sub shape ID, inRay.GetPointOnRay(collected faction)).
 	virtual void					CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const = 0;
 
-	/// Check if inPoint is inside this shape. For this tests all shapes are treated as if they were solid. 
+	/// Check if inPoint is inside this shape. For this tests all shapes are treated as if they were solid.
 	/// Note that inPoint should be relative to the center of mass of this shape (i.e. subtract Shape::GetCenterOfMass() from inPoint if you want to test against the shape in the space it was created).
 	/// For a mesh shape, this test will only provide sensible information if the mesh is a closed manifold.
 	/// For each shape that collides, ioCollector will receive a hit.
 	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const = 0;
 
+	/// Collides all vertices of a soft body with this shape and updates SoftBodyVertex::mCollisionPlane, SoftBodyVertex::mCollidingShapeIndex and SoftBodyVertex::mLargestPenetration if a collision with more penetration was found.
+	/// @param inCenterOfMassTransform Center of mass transform for this shape relative to the vertices.
+	/// @param ioVertices The vertices of the soft body
+	/// @param inDeltaTime Delta time of this time step (can be used to extrapolate the position using the velocity of the particle)
+	/// @param inDisplacementDueToGravity Displacement due to gravity during this time step
+	/// @param inCollidingShapeIndex Value to store in SoftBodyVertex::mCollidingShapeIndex when a collision was found
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const = 0;
+
 	/// Collect the leaf transformed shapes of all leaf shapes of this shape.
 	/// inBox is the world space axis aligned box which leaf shapes should collide with.
 	/// inPositionCOM/inRotation/inScale describes the transform of this shape.
 	/// inSubShapeIDCeator represents the current sub shape ID of this shape.
 	virtual void					CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const;
-	   
+
 	/// Transforms this shape and all of its children with inTransform, resulting shape(s) are passed to ioCollector.
 	/// Note that not all shapes support all transforms (especially true for scaling), the resulting shape will try to match the transform as accurately as possible.
 	/// @param inCenterOfMassTransform The transform (rotation, translation, scale) that the center of mass of the shape should get
@@ -320,7 +331,7 @@ public:
 	/// This is the minimum amount of triangles that should be requested through GetTrianglesNext.
 	static constexpr int			cGetTrianglesMinTrianglesRequested = 32;
 
-	/// To start iterating over triangles, call this function first. 
+	/// To start iterating over triangles, call this function first.
 	/// ioContext is a temporary buffer and should remain untouched until the last call to GetTrianglesNext.
 	/// inBox is the world space bounding in which you want to get the triangles.
 	/// inPositionCOM/inRotation/inScale describes the transform of this shape.
@@ -335,8 +346,8 @@ public:
 	/// Note that the function may return triangles outside of the requested box, only coarse culling is performed on the returned triangles.
 	virtual int						GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const = 0;
 
-	///@name Binary serialization of the shape. Note that this saves the 'cooked' shape in a format which will not be backwards compatible for newer library versions. 
-	/// In this case you need to recreate the shape from the ShapeSettings object and save it again. The user is expected to call SaveBinaryState followed by SaveMaterialState and SaveSubShapeState. 
+	///@name Binary serialization of the shape. Note that this saves the 'cooked' shape in a format which will not be backwards compatible for newer library versions.
+	/// In this case you need to recreate the shape from the ShapeSettings object and save it again. The user is expected to call SaveBinaryState followed by SaveMaterialState and SaveSubShapeState.
 	/// The stream should be stored as is and the material and shape list should be saved using the applications own serialization system (e.g. by assigning an ID to each pointer).
 	/// When restoring data, call sRestoreFromBinararyState to get the shape and then call RestoreMaterialState and RestoreSubShapeState to restore the pointers to the external objects.
 	///@{
@@ -406,6 +417,12 @@ protected:
 	/// This function should not be called directly, it is used by sRestoreFromBinaryState.
 	virtual void					RestoreBinaryState(StreamIn &inStream);
 
+	/// A fallback version of CollidePoint that uses a ray cast and counts the number of hits to determine if the point is inside the shape. Odd number of hits means inside, even number of hits means outside.
+	static void						sCollidePointUsingRayCast(const Shape &inShape, Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter);
+
+	/// A fallback version of CollideSoftBodyVertices that uses a raycast to collide the vertices with the shape.
+	static void						sCollideSoftBodyVerticesUsingRayCast(const Shape &inShape, Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex);
+
 private:
 	uint64							mUserData = 0;
 	EShapeType						mShapeType;

Jolt/Physics/Collision/Shape/SphereShape.cpp

@@ -11,6 +11,7 @@
 #include <Jolt/Physics/Collision/CastResult.h>
 #include <Jolt/Physics/Collision/CollidePointResult.h>
 #include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/SoftBody/SoftBodyVertex.h>
 #include <Jolt/Geometry/RaySphere.h>
 #include <Jolt/Geometry/Plane.h>
 #include <Jolt/Core/StreamIn.h>
@@ -30,16 +31,16 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SphereShapeSettings)
 }
 
 ShapeSettings::ShapeResult SphereShapeSettings::Create() const
-{ 
+{
 	if (mCachedResult.IsEmpty())
-		Ref<Shape> shape = new SphereShape(*this, mCachedResult); 
+		Ref<Shape> shape = new SphereShape(*this, mCachedResult);
 	return mCachedResult;
 }
 
-SphereShape::SphereShape(const SphereShapeSettings &inSettings, ShapeResult &outResult) : 
-	ConvexShape(EShapeSubType::Sphere, inSettings, outResult), 
-	mRadius(inSettings.mRadius) 
-{ 
+SphereShape::SphereShape(const SphereShapeSettings &inSettings, ShapeResult &outResult) :
+	ConvexShape(EShapeSubType::Sphere, inSettings, outResult),
+	mRadius(inSettings.mRadius)
+{
 	if (inSettings.mRadius <= 0.0f)
 	{
 		outResult.SetError("Invalid radius");
@@ -57,19 +58,19 @@ float SphereShape::GetScaledRadius(Vec3Arg inScale) const
 	return abs_scale.GetX() * mRadius;
 }
 
-AABox SphereShape::GetLocalBounds() const 
-{ 
-	Vec3 half_extent = Vec3::sReplicate(mRadius); 
-	return AABox(-half_extent, half_extent); 
+AABox SphereShape::GetLocalBounds() const
+{
+	Vec3 half_extent = Vec3::sReplicate(mRadius);
+	return AABox(-half_extent, half_extent);
 }
-		
+
 AABox SphereShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
-{ 
+{
 	float scaled_radius = GetScaledRadius(inScale);
 	Vec3 half_extent = Vec3::sReplicate(scaled_radius);
-	AABox bounds(-half_extent, half_extent); 
-	bounds.Translate(inCenterOfMassTransform.GetTranslation()); 
-	return bounds; 
+	AABox bounds(-half_extent, half_extent);
+	bounds.Translate(inCenterOfMassTransform.GetTranslation());
+	return bounds;
 }
 
 class SphereShape::SphereNoConvex final : public Support
@@ -77,13 +78,13 @@ class SphereShape::SphereNoConvex final : public Support
 public:
 	explicit		SphereNoConvex(float inRadius) :
 		mRadius(inRadius)
-	{ 
-		static_assert(sizeof(SphereNoConvex) <= sizeof(SupportBuffer), "Buffer size too small"); 
+	{
+		static_assert(sizeof(SphereNoConvex) <= sizeof(SupportBuffer), "Buffer size too small");
 		JPH_ASSERT(IsAligned(this, alignof(SphereNoConvex)));
 	}
 
 	virtual Vec3	GetSupport(Vec3Arg inDirection) const override
-	{ 
+	{
 		return Vec3::sZero();
 	}
 
@@ -101,13 +102,13 @@ class SphereShape::SphereWithConvex final : public Support
 public:
 	explicit		SphereWithConvex(float inRadius) :
 		mRadius(inRadius)
-	{ 
-		static_assert(sizeof(SphereWithConvex) <= sizeof(SupportBuffer), "Buffer size too small"); 
+	{
+		static_assert(sizeof(SphereWithConvex) <= sizeof(SupportBuffer), "Buffer size too small");
 		JPH_ASSERT(IsAligned(this, alignof(SphereWithConvex)));
 	}
 
 	virtual Vec3	GetSupport(Vec3Arg inDirection) const override
-	{ 
+	{
 		float len = inDirection.Length();
 		return len > 0.0f? (mRadius / len) * inDirection : Vec3::sZero();
 	}
@@ -153,11 +154,11 @@ MassProperties SphereShape::GetMassProperties() const
 	return p;
 }
 
-Vec3 SphereShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const 
-{ 
-	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); 
+Vec3 SphereShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
 
-	float len = inLocalSurfacePosition.Length(); 
+	float len = inLocalSurfacePosition.Length();
 	return len != 0.0f? inLocalSurfacePosition / len : Vec3::sAxisY();
 }
 
@@ -254,7 +255,7 @@ void SphereShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCast
 		}
 
 		// Check back side hit
-		if (inRayCastSettings.mBackFaceMode == EBackFaceMode::CollideWithBackFaces 
+		if (inRayCastSettings.mBackFaceMode == EBackFaceMode::CollideWithBackFaces
 			&& num_results > 1 // Ray should have 2 intersections
 			&& max_fraction < ioCollector.GetEarlyOutFraction()) // End of ray should be before early out fraction
 		{
@@ -274,6 +275,32 @@ void SphereShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubSh
 		ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
 }
 
+void SphereShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+{
+	Vec3 center = inCenterOfMassTransform.GetTranslation();
+
+	for (SoftBodyVertex &v : ioVertices)
+		if (v.mInvMass > 0.0f)
+		{
+			// Calculate penetration
+			Vec3 delta = v.mPosition - center;
+			float distance = delta.Length();
+			float penetration = mRadius - distance;
+			if (penetration > v.mLargestPenetration)
+			{
+				v.mLargestPenetration = penetration;
+
+				// Calculate contact point and normal
+				Vec3 normal = distance > 0.0f? delta / distance : Vec3::sAxisY();
+				Vec3 point = center + mRadius * normal;
+
+				// Store collision
+				v.mCollisionPlane = Plane::sFromPointAndNormal(point, normal);
+				v.mCollidingShapeIndex = inCollidingShapeIndex;
+			}
+		}
+}
+
 void SphereShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const
 {
 	Vec3 scale;

Jolt/Physics/Collision/Shape/SphereShape.h

@@ -45,7 +45,7 @@ public:
 
 	// See Shape::GetLocalBounds
 	virtual AABox			GetLocalBounds() const override;
-		
+
 	// See Shape::GetWorldSpaceBounds
 	virtual AABox			GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
 	using Shape::GetWorldSpaceBounds;
@@ -80,6 +80,9 @@ public:
 	// See: Shape::CollidePoint
 	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
+	// See: Shape::ColideSoftBodyVertices
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+
 	// See Shape::TransformShape
 	virtual void			TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override;
 

Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp

@@ -9,6 +9,7 @@
 #include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
 #include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
 #include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/SoftBody/SoftBodyVertex.h>
 #include <Jolt/Geometry/RayCapsule.h>
 #include <Jolt/ObjectStream/TypeDeclarations.h>
 #include <Jolt/Core/StreamIn.h>
@@ -34,7 +35,7 @@ bool TaperedCapsuleShapeSettings::IsSphere() const
 }
 
 ShapeSettings::ShapeResult TaperedCapsuleShapeSettings::Create() const
-{ 
+{
 	if (mCachedResult.IsEmpty())
 	{
 		Ref<Shape> shape;
@@ -68,25 +69,25 @@ ShapeSettings::ShapeResult TaperedCapsuleShapeSettings::Create() const
 		else
 		{
 			// Normal tapered capsule shape
-			shape = new TaperedCapsuleShape(*this, mCachedResult); 
+			shape = new TaperedCapsuleShape(*this, mCachedResult);
 		}
 	}
 	return mCachedResult;
 }
 
-TaperedCapsuleShapeSettings::TaperedCapsuleShapeSettings(float inHalfHeightOfTaperedCylinder, float inTopRadius, float inBottomRadius, const PhysicsMaterial *inMaterial) : 
+TaperedCapsuleShapeSettings::TaperedCapsuleShapeSettings(float inHalfHeightOfTaperedCylinder, float inTopRadius, float inBottomRadius, const PhysicsMaterial *inMaterial) :
 	ConvexShapeSettings(inMaterial),
 	mHalfHeightOfTaperedCylinder(inHalfHeightOfTaperedCylinder),
-	mTopRadius(inTopRadius), 
+	mTopRadius(inTopRadius),
 	mBottomRadius(inBottomRadius)
-{ 
+{
 }
 
 TaperedCapsuleShape::TaperedCapsuleShape(const TaperedCapsuleShapeSettings &inSettings, ShapeResult &outResult) :
 	ConvexShape(EShapeSubType::TaperedCapsule, inSettings, outResult),
-	mTopRadius(inSettings.mTopRadius), 
+	mTopRadius(inSettings.mTopRadius),
 	mBottomRadius(inSettings.mBottomRadius)
-{ 
+{
 	if (mTopRadius <= 0.0f)
 	{
 		outResult.SetError("Invalid top radius");
@@ -135,19 +136,19 @@ TaperedCapsuleShape::TaperedCapsuleShape(const TaperedCapsuleShapeSettings &inSe
 class TaperedCapsuleShape::TaperedCapsule final : public Support
 {
 public:
-					TaperedCapsule(Vec3Arg inTopCenter, Vec3Arg inBottomCenter, float inTopRadius, float inBottomRadius, float inConvexRadius) : 
+					TaperedCapsule(Vec3Arg inTopCenter, Vec3Arg inBottomCenter, float inTopRadius, float inBottomRadius, float inConvexRadius) :
 		mTopCenter(inTopCenter),
 		mBottomCenter(inBottomCenter),
 		mTopRadius(inTopRadius),
 		mBottomRadius(inBottomRadius),
 		mConvexRadius(inConvexRadius)
-	{ 
-		static_assert(sizeof(TaperedCapsule) <= sizeof(SupportBuffer), "Buffer size too small"); 
+	{
+		static_assert(sizeof(TaperedCapsule) <= sizeof(SupportBuffer), "Buffer size too small");
 		JPH_ASSERT(IsAligned(this, alignof(TaperedCapsule)));
 	}
 
 	virtual Vec3	GetSupport(Vec3Arg inDirection) const override
-	{ 
+	{
 		// Check zero vector
 		float len = inDirection.Length();
 		if (len == 0.0f)
@@ -210,7 +211,7 @@ const ConvexShape::Support *TaperedCapsuleShape::GetSupportFunction(ESupportMode
 }
 
 void TaperedCapsuleShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
-{	
+{
 	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
 	JPH_ASSERT(IsValidScale(inScale));
 
@@ -253,9 +254,9 @@ MassProperties TaperedCapsuleShape::GetMassProperties() const
 	return p;
 }
 
-Vec3 TaperedCapsuleShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const 
-{ 
-	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); 
+Vec3 TaperedCapsuleShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
 
 	// See: TaperedCapsuleShape.gliffy
 	// We need to calculate ty and by in order to see if the position is on the top or bottom sphere
@@ -284,7 +285,7 @@ AABox TaperedCapsuleShape::GetLocalBounds() const
 }
 
 AABox TaperedCapsuleShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
-{ 
+{
 	JPH_ASSERT(IsValidScale(inScale));
 
 	Vec3 abs_scale = inScale.Abs();
@@ -299,6 +300,53 @@ AABox TaperedCapsuleShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform,
 	return AABox(p1, p2);
 }
 
+void TaperedCapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+{
+	Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation();
+
+	for (SoftBodyVertex &v : ioVertices)
+		if (v.mInvMass > 0.0f)
+		{
+			Vec3 local_pos = inverse_transform * v.mPosition;
+
+			// See comments at TaperedCapsuleShape::GetSurfaceNormal for rationale behind the math
+			Vec3 position, normal;
+			if (local_pos.GetY() > mTopCenter + mSinAlpha * mTopRadius)
+			{
+				// Top sphere
+				Vec3 top = Vec3(0, mTopCenter, 0);
+				normal = (local_pos - top).NormalizedOr(Vec3::sAxisY());
+				position = top + mTopRadius * normal;
+			}
+			else if (local_pos.GetY() < mBottomCenter + mSinAlpha * mBottomRadius)
+			{
+				// Bottom sphere
+				Vec3 bottom(0, mBottomCenter, 0);
+				normal = (local_pos - bottom).NormalizedOr(-Vec3::sAxisY());
+				position = bottom + mBottomRadius * normal;
+			}
+			else
+			{
+				// Tapered cylinder
+				normal = Vec3(local_pos.GetX(), 0, local_pos.GetZ()).NormalizedOr(Vec3::sAxisX());
+				normal.SetY(mTanAlpha);
+				normal = normal.NormalizedOr(Vec3::sAxisX());
+				position = Vec3(0, mBottomCenter, 0) + mBottomRadius * normal;
+			}
+
+			Plane plane = Plane::sFromPointAndNormal(position, normal);
+			float penetration = -plane.SignedDistance(local_pos);
+			if (penetration > v.mLargestPenetration)
+			{
+				v.mLargestPenetration = penetration;
+
+				// Store collision
+				v.mCollisionPlane = plane.GetTransformed(inCenterOfMassTransform);
+				v.mCollidingShapeIndex = inCollidingShapeIndex;
+			}
+		}
+}
+
 #ifdef JPH_DEBUG_RENDERER
 void TaperedCapsuleShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
 {

Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h

@@ -71,6 +71,9 @@ public:
 	// See ConvexShape::GetSupportFunction
 	virtual const Support *	GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override;
 
+	// See: Shape::ColideSoftBodyVertices
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+
 #ifdef JPH_DEBUG_RENDERER
 	// See Shape::Draw
 	virtual void			Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
@@ -83,7 +86,7 @@ public:
 	virtual void			SaveBinaryState(StreamOut &inStream) const override;
 
 	// See Shape::GetStats
-	virtual Stats			GetStats() const override												{ return Stats(sizeof(*this), 0); } 
+	virtual Stats			GetStats() const override												{ return Stats(sizeof(*this), 0); }
 
 	// See Shape::GetVolume
 	virtual float			GetVolume() const override												{ return GetLocalBounds().GetVolume(); } // Volume is approximate!

Jolt/Physics/Collision/Shape/TriangleShape.cpp

@@ -39,19 +39,19 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(TriangleShapeSettings)
 }
 
 ShapeSettings::ShapeResult TriangleShapeSettings::Create() const
-{ 
+{
 	if (mCachedResult.IsEmpty())
-		Ref<Shape> shape = new TriangleShape(*this, mCachedResult); 
+		Ref<Shape> shape = new TriangleShape(*this, mCachedResult);
 	return mCachedResult;
 }
 
 TriangleShape::TriangleShape(const TriangleShapeSettings &inSettings, ShapeResult &outResult) :
-	ConvexShape(EShapeSubType::Triangle, inSettings, outResult), 
-	mV1(inSettings.mV1), 
-	mV2(inSettings.mV2), 
-	mV3(inSettings.mV3), 
-	mConvexRadius(inSettings.mConvexRadius) 
-{ 
+	ConvexShape(EShapeSubType::Triangle, inSettings, outResult),
+	mV1(inSettings.mV1),
+	mV2(inSettings.mV2),
+	mV3(inSettings.mV3),
+	mConvexRadius(inSettings.mConvexRadius)
+{
 	if (inSettings.mConvexRadius < 0.0f)
 	{
 		outResult.SetError("Invalid convex radius");
@@ -61,15 +61,15 @@ TriangleShape::TriangleShape(const TriangleShapeSettings &inSettings, ShapeResul
 	outResult.Set(this);
 }
 
-AABox TriangleShape::GetLocalBounds() const 
-{ 
+AABox TriangleShape::GetLocalBounds() const
+{
 	AABox bounds(mV1, mV1);
 	bounds.Encapsulate(mV2);
 	bounds.Encapsulate(mV3);
 	bounds.ExpandBy(Vec3::sReplicate(mConvexRadius));
 	return bounds;
 }
-		
+
 AABox TriangleShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
 {
 	JPH_ASSERT(IsValidScale(inScale));
@@ -90,13 +90,13 @@ class TriangleShape::TriangleNoConvex final : public Support
 public:
 							TriangleNoConvex(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3) :
 		mTriangleSuport(inV1, inV2, inV3)
-	{ 
-		static_assert(sizeof(TriangleNoConvex) <= sizeof(SupportBuffer), "Buffer size too small"); 
+	{
+		static_assert(sizeof(TriangleNoConvex) <= sizeof(SupportBuffer), "Buffer size too small");
 		JPH_ASSERT(IsAligned(this, alignof(TriangleNoConvex)));
 	}
 
 	virtual Vec3			GetSupport(Vec3Arg inDirection) const override
-	{ 
+	{
 		return mTriangleSuport.GetSupport(inDirection);
 	}
 
@@ -115,13 +115,13 @@ public:
 							TriangleWithConvex(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, float inConvexRadius) :
 		mConvexRadius(inConvexRadius),
 		mTriangleSuport(inV1, inV2, inV3)
-	{ 
-		static_assert(sizeof(TriangleWithConvex) <= sizeof(SupportBuffer), "Buffer size too small"); 
+	{
+		static_assert(sizeof(TriangleWithConvex) <= sizeof(SupportBuffer), "Buffer size too small");
 		JPH_ASSERT(IsAligned(this, alignof(TriangleWithConvex)));
 	}
 
 	virtual Vec3			GetSupport(Vec3Arg inDirection) const override
-	{ 
+	{
 		Vec3 support = mTriangleSuport.GetSupport(inDirection);
 		float len = inDirection.Length();
 		if (len > 0.0f)
@@ -184,12 +184,12 @@ MassProperties TriangleShape::GetMassProperties() const
 	return MassProperties();
 }
 
-Vec3 TriangleShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const 
+Vec3 TriangleShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
 {
-	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); 
+	JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
 
 	Vec3 cross = (mV2 - mV1).Cross(mV3 - mV1);
-	float len = cross.Length(); 
+	float len = cross.Length();
 	return len != 0.0f? cross / len : Vec3::sAxisY();
 }
 
@@ -257,6 +257,11 @@ void TriangleShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSub
 	// Can't be inside a triangle
 }
 
+void TriangleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+{
+	sCollideSoftBodyVerticesUsingRayCast(*this, inCenterOfMassTransform, ioVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex);
+}
+
 void TriangleShape::sCollideConvexVsTriangle(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter)
 {
 	JPH_ASSERT(inShape1->GetType() == EShapeType::Convex);

Jolt/Physics/Collision/Shape/TriangleShape.h

@@ -49,7 +49,7 @@ public:
 
 	// See Shape::GetLocalBounds
 	virtual AABox			GetLocalBounds() const override;
-		
+
 	// See Shape::GetWorldSpaceBounds
 	virtual AABox			GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
 	using Shape::GetWorldSpaceBounds;
@@ -84,6 +84,9 @@ public:
 	// See: Shape::CollidePoint
 	virtual void			CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
 
+	// See: Shape::ColideSoftBodyVertices
+	virtual void			CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+
 	// See Shape::TransformShape
 	virtual void			TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override;
 

Jolt/Physics/Constraints/ContactConstraintManager.h

@@ -50,21 +50,23 @@ public:
 	/// Set the function that combines the friction of two bodies and returns it
 	/// Default method is the geometric mean: sqrt(friction1 * friction2).
 	void						SetCombineFriction(CombineFunction inCombineFriction)				{ mCombineFriction = inCombineFriction; }
+	CombineFunction				GetCombineFriction() const											{ return mCombineFriction; }
 
 	/// Set the function that combines the restitution of two bodies and returns it
 	/// Default method is max(restitution1, restitution1)
 	void						SetCombineRestitution(CombineFunction inCombineRestitution)			{ mCombineRestitution = inCombineRestitution; }
+	CombineFunction				GetCombineRestitution() const										{ return mCombineRestitution; }
 
 	/// Get the max number of contact constraints that are allowed
 	uint32						GetMaxConstraints() const											{ return mMaxConstraints; }
 
 	/// Check with the listener if inBody1 and inBody2 could collide, returns false if not
-	inline ValidateResult		ValidateContactPoint(const Body &inBody1, const Body &inBody2, RVec3Arg inBaseOffset, const CollideShapeResult &inCollisionResult) const	
-	{ 
-		if (mContactListener == nullptr) 
-			return ValidateResult::AcceptAllContactsForThisBodyPair; 
+	inline ValidateResult		ValidateContactPoint(const Body &inBody1, const Body &inBody2, RVec3Arg inBaseOffset, const CollideShapeResult &inCollisionResult) const
+	{
+		if (mContactListener == nullptr)
+			return ValidateResult::AcceptAllContactsForThisBodyPair;
 
-		return mContactListener->OnContactValidate(inBody1, inBody2, inBaseOffset, inCollisionResult); 
+		return mContactListener->OnContactValidate(inBody1, inBody2, inBaseOffset, inCollisionResult);
 	}
 
 	/// Sets up the constraint buffer. Should be called before starting collision detection.
@@ -111,7 +113,7 @@ public:
 	/// This is using the approach described in 'Modeling and Solving Constraints' by Erin Catto presented at GDC 2009 (and later years with slight modifications).
 	/// We're using the formulas from slide 50 - 53 combined.
 	///
-	/// Euler velocity integration: 
+	/// Euler velocity integration:
 	///
 	/// v1' = v1 + M^-1 P
 	///
@@ -134,7 +136,7 @@ public:
 	/// Jacobian (for position constraint)
 	///
 	/// J = [-n, -r1 x n, n, r2 x n]
-	/// 
+	///
 	/// n = contact normal (pointing away from body 1).
 	/// p1, p2 = positions of collision on body 1 and 2.
 	/// r1, r2 = contact point relative to center of mass of body 1 and body 2 (r1 = p1 - x1, r2 = p2 - x2).
@@ -183,7 +185,7 @@ public:
 	/// |lambda_T| <= mu |lambda_N|
 	///
 	/// And the constraint that needs to be applied is exactly the same as a non penetration constraint
-	/// except that we use a tangent instead of a normal. The tangent should point in the direction of the 
+	/// except that we use a tangent instead of a normal. The tangent should point in the direction of the
 	/// tangential velocity of the point:
 	///
 	/// J = [-T, -r1 x T, T, r2 x T]
@@ -271,7 +273,7 @@ private:
 
 	static_assert(sizeof(CachedContactPoint) == 36, "Unexpected size");
 	static_assert(alignof(CachedContactPoint) == 4, "Assuming 4 byte aligned");
-	
+
 	/// A single cached manifold
 	class CachedManifold
 	{
@@ -338,7 +340,7 @@ private:
 		uint32					mFirstCachedManifold;
 	};
 
-	static_assert(sizeof(CachedBodyPair) == 28, "Unexpected size"); 
+	static_assert(sizeof(CachedBodyPair) == 28, "Unexpected size");
 	static_assert(alignof(CachedBodyPair) == 4, "Assuming 4 byte aligned");
 
 	/// Define a map that maps BodyPair -> CachedBodyPair
@@ -355,7 +357,7 @@ private:
 		/// Reset all entries from the cache
 		void					Clear();
 
-		/// Prepare cache before creating new contacts. 
+		/// Prepare cache before creating new contacts.
 		/// inExpectedNumBodyPairs / inExpectedNumManifolds are the amount of body pairs / manifolds found in the previous step and is used to determine the amount of buckets the contact cache hash map will use.
 		void					Prepare(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds);
 
@@ -427,7 +429,7 @@ private:
 		AxisConstraintPart		mNonPenetrationConstraint;
 		AxisConstraintPart		mFrictionConstraint1;
 		AxisConstraintPart		mFrictionConstraint2;
-		
+
 		/// Contact cache
 		CachedContactPoint *	mContactPoint;
 	};

Jolt/Physics/PhysicsSystem.cpp

@@ -21,6 +21,7 @@
 #include <Jolt/Physics/Collision/Shape/ConvexShape.h>
 #include <Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h>
 #include <Jolt/Physics/DeterminismLog.h>
+#include <Jolt/Physics/SoftBody/SoftBodyMotionProperties.h>
 #include <Jolt/Geometry/RayAABox.h>
 #include <Jolt/Core/JobSystem.h>
 #include <Jolt/Core/TempAllocator.h>
@@ -57,6 +58,7 @@ static const Color cColorResolveCCDContacts = Color::sGetDistinctColor(16);
 static const Color cColorSolvePositionConstraints = Color::sGetDistinctColor(17);
 static const Color cColorFindCCDContacts = Color::sGetDistinctColor(18);
 static const Color cColorStepListeners = Color::sGetDistinctColor(19);
+static const Color cColorUpdateSoftBodies = Color::sGetDistinctColor(20);
 
 PhysicsSystem::~PhysicsSystem()
 {
@@ -65,12 +67,12 @@ PhysicsSystem::~PhysicsSystem()
 }
 
 void PhysicsSystem::Init(uint inMaxBodies, uint inNumBodyMutexes, uint inMaxBodyPairs, uint inMaxContactConstraints, const BroadPhaseLayerInterface &inBroadPhaseLayerInterface, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter)
-{ 
+{
 	mObjectVsBroadPhaseLayerFilter = &inObjectVsBroadPhaseLayerFilter;
 	mObjectLayerPairFilter = &inObjectLayerPairFilter;
 
 	// Initialize body manager
-	mBodyManager.Init(inMaxBodies, inNumBodyMutexes, inBroadPhaseLayerInterface); 
+	mBodyManager.Init(inMaxBodies, inNumBodyMutexes, inBroadPhaseLayerInterface);
 
 	// Create broadphase
 	mBroadPhase = new BROAD_PHASE();
@@ -115,7 +117,7 @@ void PhysicsSystem::RemoveStepListener(PhysicsStepListener *inListener)
 }
 
 EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionSteps, TempAllocator *inTempAllocator, JobSystem *inJobSystem)
-{	
+{
 	JPH_PROFILE_FUNCTION();
 
 	JPH_DET_LOG("PhysicsSystem::Update: dt: " << inDeltaTime << " steps: " << inCollisionSteps);
@@ -127,8 +129,9 @@ EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionStep
 	mBroadPhase->FrameSync();
 
 	// If there are no active bodies or there's no time delta
-	uint32 num_active_bodies = mBodyManager.GetNumActiveBodies();
-	if (num_active_bodies == 0 || inDeltaTime <= 0.0f)
+	uint32 num_active_rigid_bodies = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody);
+	uint32 num_active_soft_bodies = mBodyManager.GetNumActiveBodies(EBodyType::SoftBody);
+	if ((num_active_rigid_bodies == 0 && num_active_soft_bodies == 0) || inDeltaTime <= 0.0f)
 	{
 		mBodyManager.LockAllBodies();
 
@@ -170,7 +173,7 @@ EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionStep
 	mBroadPhase->LockModifications();
 
 	// Get max number of concurrent jobs
-	int max_concurrency = context.GetMaxConcurrency(); 
+	int max_concurrency = context.GetMaxConcurrency();
 
 	// Calculate how many step listener jobs we spawn
 	int num_step_listener_jobs = mStepListeners.empty()? 0 : max(1, min((int)mStepListeners.size() / mPhysicsSettings.mStepListenersBatchSize / mPhysicsSettings.mStepListenerBatchesPerJob, max_concurrency));
@@ -178,7 +181,7 @@ EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionStep
 	// Number of gravity jobs depends on the amount of active bodies.
 	// Launch max 1 job per batch of active bodies
 	// Leave 1 thread for update broadphase prepare and 1 for determine active constraints
-	int num_apply_gravity_jobs = max(1, min(((int)num_active_bodies + cApplyGravityBatchSize - 1) / cApplyGravityBatchSize, max_concurrency - 2));
+	int num_apply_gravity_jobs = max(1, min(((int)num_active_rigid_bodies + cApplyGravityBatchSize - 1) / cApplyGravityBatchSize, max_concurrency - 2));
 
 	// Number of determine active constraints jobs to run depends on number of constraints.
 	// Leave 1 thread for update broadphase prepare and 1 for apply gravity
@@ -187,10 +190,10 @@ EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionStep
 	// Number of find collisions jobs to run depends on number of active bodies.
 	// Note that when we have more than 1 thread, we always spawn at least 2 find collisions jobs so that the first job can wait for build islands from constraints
 	// (which may activate additional bodies that need to be processed) while the second job can start processing collision work.
-	int num_find_collisions_jobs = max(max_concurrency == 1? 1 : 2, min(((int)num_active_bodies + cActiveBodiesBatchSize - 1) / cActiveBodiesBatchSize, max_concurrency));
+	int num_find_collisions_jobs = max(max_concurrency == 1? 1 : 2, min(((int)num_active_rigid_bodies + cActiveBodiesBatchSize - 1) / cActiveBodiesBatchSize, max_concurrency));
 
 	// Number of integrate velocity jobs depends on number of active bodies.
-	int num_integrate_velocity_jobs = max(1, min(((int)num_active_bodies + cIntegrateVelocityBatchSize - 1) / cIntegrateVelocityBatchSize, max_concurrency));
+	int num_integrate_velocity_jobs = max(1, min(((int)num_active_rigid_bodies + cIntegrateVelocityBatchSize - 1) / cIntegrateVelocityBatchSize, max_concurrency));
 
 	{
 		JPH_PROFILE("Build Jobs");
@@ -208,8 +211,8 @@ EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionStep
 
 			// Create job to do broadphase finalization
 			// This job must finish before integrating velocities. Until then the positions will not be updated neither will bodies be added / removed.
-			step.mUpdateBroadphaseFinalize = inJobSystem->CreateJob("UpdateBroadPhaseFinalize", cColorUpdateBroadPhaseFinalize, [&context, &step]() 
-				{ 
+			step.mUpdateBroadphaseFinalize = inJobSystem->CreateJob("UpdateBroadPhaseFinalize", cColorUpdateBroadPhaseFinalize, [&context, &step]()
+				{
 					// Validate that all find collision jobs have stopped
 					JPH_ASSERT(step.mActiveFindCollisionJobs == 0);
 
@@ -217,7 +220,7 @@ EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionStep
 					context.mPhysicsSystem->mBroadPhase->UpdateFinalize(step.mBroadPhaseUpdateState);
 
 					// Signal that it is done
-					step.mPreIntegrateVelocity.RemoveDependency(); 
+					step.mPreIntegrateVelocity.RemoveDependency();
 				}, num_find_collisions_jobs + 2); // depends on: find collisions, broadphase prepare update, finish building jobs
 
 			// The immediate jobs below are only immediate for the first step, the all finished job will kick them for the next step
@@ -226,8 +229,8 @@ EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionStep
 			// Start job immediately: Start the prepare broadphase
 			// Must be done under body lock protection since the order is body locks then broadphase mutex
 			// If this is turned around the RemoveBody call will hang since it locks in that order
-			step.mBroadPhasePrepare = inJobSystem->CreateJob("UpdateBroadPhasePrepare", cColorUpdateBroadPhasePrepare, [&context, &step]() 
-				{ 
+			step.mBroadPhasePrepare = inJobSystem->CreateJob("UpdateBroadPhasePrepare", cColorUpdateBroadPhasePrepare, [&context, &step]()
+				{
 					// Prepare the broadphase update
 					step.mBroadPhaseUpdateState = context.mPhysicsSystem->mBroadPhase->UpdatePrepare();
 
@@ -244,9 +247,9 @@ EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionStep
 			{
 				// Build islands from constraints may activate additional bodies, so the first job will wait for this to finish in order to not miss any active bodies
 				int num_dep_build_islands_from_constraints = i == 0? 1 : 0;
-				step.mFindCollisions[i] = inJobSystem->CreateJob("FindCollisions", cColorFindCollisions, [&step, i]() 
-					{ 
-						step.mContext->mPhysicsSystem->JobFindCollisions(&step, i); 
+				step.mFindCollisions[i] = inJobSystem->CreateJob("FindCollisions", cColorFindCollisions, [&step, i]()
+					{
+						step.mContext->mPhysicsSystem->JobFindCollisions(&step, i);
 					}, num_apply_gravity_jobs + num_determine_active_constraints_jobs + 1 + num_dep_build_islands_from_constraints); // depends on: apply gravity, determine active constraints, finish building jobs, build islands from constraints
 			}
 
@@ -258,7 +261,7 @@ EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionStep
 			#endif
 
 				// Store the number of active bodies at the start of the step
-				step.mNumActiveBodiesAtStepStart = mBodyManager.GetNumActiveBodies();
+				step.mNumActiveBodiesAtStepStart = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody);
 
 				// Lock all constraints
 				mConstraintManager.LockAllConstraints();
@@ -277,36 +280,36 @@ EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionStep
 			// This job applies gravity to all active bodies
 			step.mApplyGravity.resize(num_apply_gravity_jobs);
 			for (int i = 0; i < num_apply_gravity_jobs; ++i)
-				step.mApplyGravity[i] = inJobSystem->CreateJob("ApplyGravity", cColorApplyGravity, [&context, &step]() 
-					{ 
-						context.mPhysicsSystem->JobApplyGravity(&context, &step); 
+				step.mApplyGravity[i] = inJobSystem->CreateJob("ApplyGravity", cColorApplyGravity, [&context, &step]()
+					{
+						context.mPhysicsSystem->JobApplyGravity(&context, &step);
 
 						JobHandle::sRemoveDependencies(step.mFindCollisions);
 					}, num_step_listener_jobs > 0? num_step_listener_jobs : previous_step_dependency_count); // depends on: step listeners (or previous step if no step listeners)
-	
+
 			// This job will setup velocity constraints for non-collision constraints
-			step.mSetupVelocityConstraints = inJobSystem->CreateJob("SetupVelocityConstraints", cColorSetupVelocityConstraints, [&context, &step]() 
-				{ 
+			step.mSetupVelocityConstraints = inJobSystem->CreateJob("SetupVelocityConstraints", cColorSetupVelocityConstraints, [&context, &step]()
+				{
 					context.mPhysicsSystem->JobSetupVelocityConstraints(context.mStepDeltaTime, &step);
 
 					JobHandle::sRemoveDependencies(step.mSolveVelocityConstraints);
 				}, num_determine_active_constraints_jobs + 1); // depends on: determine active constraints, finish building jobs
 
 			// This job will build islands from constraints
-			step.mBuildIslandsFromConstraints = inJobSystem->CreateJob("BuildIslandsFromConstraints", cColorBuildIslandsFromConstraints, [&context, &step]() 
-				{ 
+			step.mBuildIslandsFromConstraints = inJobSystem->CreateJob("BuildIslandsFromConstraints", cColorBuildIslandsFromConstraints, [&context, &step]()
+				{
 					context.mPhysicsSystem->JobBuildIslandsFromConstraints(&context, &step);
 
 					step.mFindCollisions[0].RemoveDependency(); // The first collisions job cannot start running until we've finished building islands and activated all bodies
-					step.mFinalizeIslands.RemoveDependency(); 
+					step.mFinalizeIslands.RemoveDependency();
 				}, num_determine_active_constraints_jobs + 1); // depends on: determine active constraints, finish building jobs
 
 			// This job determines active constraints
 			step.mDetermineActiveConstraints.resize(num_determine_active_constraints_jobs);
 			for (int i = 0; i < num_determine_active_constraints_jobs; ++i)
-				step.mDetermineActiveConstraints[i] = inJobSystem->CreateJob("DetermineActiveConstraints", cColorDetermineActiveConstraints, [&context, &step]() 
-					{ 
-						context.mPhysicsSystem->JobDetermineActiveConstraints(&step); 
+				step.mDetermineActiveConstraints[i] = inJobSystem->CreateJob("DetermineActiveConstraints", cColorDetermineActiveConstraints, [&context, &step]()
+					{
+						context.mPhysicsSystem->JobDetermineActiveConstraints(&step);
 
 						step.mSetupVelocityConstraints.RemoveDependency();
 						step.mBuildIslandsFromConstraints.RemoveDependency();
@@ -333,12 +336,12 @@ EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionStep
 				context.mSteps[step_idx - 1].mStartNextStep.RemoveDependency();
 
 			// This job will finalize the simulation islands
-			step.mFinalizeIslands = inJobSystem->CreateJob("FinalizeIslands", cColorFinalizeIslands, [&context, &step]() 
-				{ 
+			step.mFinalizeIslands = inJobSystem->CreateJob("FinalizeIslands", cColorFinalizeIslands, [&context, &step]()
+				{
 					// Validate that all find collision jobs have stopped
 					JPH_ASSERT(step.mActiveFindCollisionJobs == 0);
 
-					context.mPhysicsSystem->JobFinalizeIslands(&context); 
+					context.mPhysicsSystem->JobFinalizeIslands(&context);
 
 					JobHandle::sRemoveDependencies(step.mSolveVelocityConstraints);
 					step.mBodySetIslandIndex.RemoveDependency();
@@ -359,9 +362,9 @@ EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionStep
 
 			// This job will set the island index on each body (only used for debug drawing purposes)
 			// It will also delete any bodies that have been destroyed in the last frame
-			step.mBodySetIslandIndex = inJobSystem->CreateJob("BodySetIslandIndex", cColorBodySetIslandIndex, [&context, &step]() 
-				{ 
-					context.mPhysicsSystem->JobBodySetIslandIndex(); 
+			step.mBodySetIslandIndex = inJobSystem->CreateJob("BodySetIslandIndex", cColorBodySetIslandIndex, [&context, &step]()
+				{
+					context.mPhysicsSystem->JobBodySetIslandIndex();
 
 					if (step.mStartNextStep.IsValid())
 						step.mStartNextStep.RemoveDependency();
@@ -371,15 +374,15 @@ EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionStep
 			if (!is_last_step)
 			{
 				PhysicsUpdateContext::Step *next_step = &context.mSteps[step_idx + 1];
-				step.mStartNextStep = inJobSystem->CreateJob("StartNextStep", cColorStartNextStep, [this, next_step]() 
-					{ 
+				step.mStartNextStep = inJobSystem->CreateJob("StartNextStep", cColorStartNextStep, [this, next_step]()
+					{
 					#ifdef _DEBUG
 						// Validate that the cached bounds are correct
 						mBodyManager.ValidateActiveBodyBounds();
 					#endif // _DEBUG
 
 						// Store the number of active bodies at the start of the step
-						next_step->mNumActiveBodiesAtStepStart = mBodyManager.GetNumActiveBodies();
+						next_step->mNumActiveBodiesAtStepStart = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody);
 
 						// Clear the large island splitter
 						TempAllocator *temp_allocator = next_step->mContext->mTempAllocator;
@@ -390,7 +393,7 @@ EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionStep
 
 						// Setup island builder
 						mIslandBuilder.PrepareContactConstraints(mContactManager.GetMaxConstraints(), temp_allocator);
-						
+
 						// Restart the contact manager
 						mContactManager.RecycleConstraintBuffer();
 
@@ -407,15 +410,15 @@ EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionStep
 							// Kick the step listeners job first
 							JobHandle::sRemoveDependencies(next_step->mStepListeners);
 						}
-					}, max_concurrency + 3); // depends on: solve position constraints of the last step, body set island index, contact removed callbacks, finish building the previous step
+					}, 4); // depends on: update soft bodies, body set island index, contact removed callbacks, finish building the previous step
 			}
 
-			// This job will solve the velocity constraints 
+			// This job will solve the velocity constraints
 			step.mSolveVelocityConstraints.resize(max_concurrency);
 			for (int i = 0; i < max_concurrency; ++i)
-				step.mSolveVelocityConstraints[i] = inJobSystem->CreateJob("SolveVelocityConstraints", cColorSolveVelocityConstraints, [&context, &step]() 
-					{ 
-						context.mPhysicsSystem->JobSolveVelocityConstraints(&context, &step); 
+				step.mSolveVelocityConstraints[i] = inJobSystem->CreateJob("SolveVelocityConstraints", cColorSolveVelocityConstraints, [&context, &step]()
+					{
+						context.mPhysicsSystem->JobSolveVelocityConstraints(&context, &step);
 
 						step.mPreIntegrateVelocity.RemoveDependency();
 					}, 3); // depends on: finalize islands, setup velocity constraints, finish building jobs.
@@ -428,8 +431,8 @@ EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionStep
 			step.mFinalizeIslands.RemoveDependency();
 
 			// This job will prepare the position update of all active bodies
-			step.mPreIntegrateVelocity = inJobSystem->CreateJob("PreIntegrateVelocity", cColorPreIntegrateVelocity, [&context, &step]() 
-				{ 
+			step.mPreIntegrateVelocity = inJobSystem->CreateJob("PreIntegrateVelocity", cColorPreIntegrateVelocity, [&context, &step]()
+				{
 					context.mPhysicsSystem->JobPreIntegrateVelocity(&context, &step);
 
 					JobHandle::sRemoveDependencies(step.mIntegrateVelocity);
@@ -442,8 +445,8 @@ EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionStep
 			// This job will update the positions of all active bodies
 			step.mIntegrateVelocity.resize(num_integrate_velocity_jobs);
 			for (int i = 0; i < num_integrate_velocity_jobs; ++i)
-				step.mIntegrateVelocity[i] = inJobSystem->CreateJob("IntegrateVelocity", cColorIntegrateVelocity, [&context, &step]() 
-					{ 
+				step.mIntegrateVelocity[i] = inJobSystem->CreateJob("IntegrateVelocity", cColorIntegrateVelocity, [&context, &step]()
+					{
 						context.mPhysicsSystem->JobIntegrateVelocity(&context, &step);
 
 						step.mPostIntegrateVelocity.RemoveDependency();
@@ -453,8 +456,8 @@ EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionStep
 			step.mPreIntegrateVelocity.RemoveDependency();
 
 			// This job will finish the position update of all active bodies
-			step.mPostIntegrateVelocity = inJobSystem->CreateJob("PostIntegrateVelocity", cColorPostIntegrateVelocity, [&context, &step]() 
-				{ 
+			step.mPostIntegrateVelocity = inJobSystem->CreateJob("PostIntegrateVelocity", cColorPostIntegrateVelocity, [&context, &step]()
+				{
 					context.mPhysicsSystem->JobPostIntegrateVelocity(&context, &step);
 
 					step.mResolveCCDContacts.RemoveDependency();
@@ -477,23 +480,32 @@ EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionStep
 			// Fixes up drift in positions and updates the broadphase with new body positions
 			step.mSolvePositionConstraints.resize(max_concurrency);
 			for (int i = 0; i < max_concurrency; ++i)
-				step.mSolvePositionConstraints[i] = inJobSystem->CreateJob("SolvePositionConstraints", cColorSolvePositionConstraints, [&context, &step]() 
-					{ 
-						context.mPhysicsSystem->JobSolvePositionConstraints(&context, &step); 
-			
+				step.mSolvePositionConstraints[i] = inJobSystem->CreateJob("SolvePositionConstraints", cColorSolvePositionConstraints, [&context, &step]()
+					{
+						context.mPhysicsSystem->JobSolvePositionConstraints(&context, &step);
+
 						// Kick the next step
-						if (step.mStartNextStep.IsValid())
-							step.mStartNextStep.RemoveDependency();
+						if (step.mUpdateSoftBodies.IsValid())
+							step.mUpdateSoftBodies.RemoveDependency();
 					}, 2); // depends on: resolve ccd contacts, finish building jobs.
 
 			// Unblock previous job.
 			step.mResolveCCDContacts.RemoveDependency();
 
+			step.mUpdateSoftBodies = inJobSystem->CreateJob("UpdateSoftBodies", cColorUpdateSoftBodies, [&context, &step]()
+					{
+						context.mPhysicsSystem->JobUpdateSoftBodies(&context);
+
+						// Kick the next step
+						if (step.mStartNextStep.IsValid())
+							step.mStartNextStep.RemoveDependency();
+					}, max_concurrency); // depends on: solve position constraints.
+
 			// Unblock previous jobs
 			JobHandle::sRemoveDependencies(step.mSolvePositionConstraints);
 		}
 	}
-	
+
 	// Build the list of jobs to wait for
 	JobSystem::Barrier *barrier = context.mBarrier;
 	{
@@ -528,6 +540,8 @@ EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionStep
 			for (const JobHandle &h : step.mSolvePositionConstraints)
 				handles.push_back(h);
 			handles.push_back(step.mContactRemovedCallbacks);
+			if (step.mUpdateSoftBodies.IsValid())
+				handles.push_back(step.mUpdateSoftBodies);
 			if (step.mStartNextStep.IsValid())
 				handles.push_back(step.mStartNextStep);
 		}
@@ -565,7 +579,7 @@ EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionStep
 	// Free body pairs
 	inTempAllocator->Free(context.mBodyPairs, sizeof(BodyPair) * mPhysicsSettings.mMaxInFlightBodyPairs);
 	context.mBodyPairs = nullptr;
-	
+
 	// Unlock the broadphase
 	mBroadPhase->UnlockModifications();
 
@@ -658,7 +672,7 @@ void PhysicsSystem::JobApplyGravity(const PhysicsUpdateContext *ioContext, Physi
 	// Any body that is activated as part of the simulation step does not receive gravity this frame.
 	// Note that bodies may be activated during this job but not deactivated, this means that only elements
 	// will be added to the array. Since the array is made to not reallocate, this is a safe operation.
-	const BodyID *active_bodies = mBodyManager.GetActiveBodiesUnsafe();
+	const BodyID *active_bodies = mBodyManager.GetActiveBodiesUnsafe(EBodyType::RigidBody);
 	uint32 num_active_bodies_at_step_start = ioStep->mNumActiveBodiesAtStepStart;
 
 	// Fetch delta time once outside the loop
@@ -693,7 +707,7 @@ void PhysicsSystem::JobSetupVelocityConstraints(float inDeltaTime, PhysicsUpdate
 	BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::Read);
 #endif
 
-	ConstraintManager::sSetupVelocityConstraints(ioStep->mContext->mActiveConstraints, ioStep->mNumActiveConstraints, inDeltaTime); 
+	ConstraintManager::sSetupVelocityConstraints(ioStep->mContext->mActiveConstraints, ioStep->mNumActiveConstraints, inDeltaTime);
 }
 
 void PhysicsSystem::JobBuildIslandsFromConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep)
@@ -726,7 +740,7 @@ void PhysicsSystem::TrySpawnJobFindCollisions(PhysicsUpdateContext::Step *ioStep
 		num_body_pairs += queue.mWriteIdx - queue.mReadIdx;
 
 	// Count how many active bodies we have waiting
-	uint32 num_active_bodies = mBodyManager.GetNumActiveBodies() - ioStep->mActiveBodyReadIdx;
+	uint32 num_active_bodies = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody) - ioStep->mActiveBodyReadIdx;
 
 	// Calculate how many jobs we would like
 	uint desired_num_jobs = min((num_body_pairs + cNarrowPhaseBatchSize - 1) / cNarrowPhaseBatchSize + (num_active_bodies + cActiveBodiesBatchSize - 1) / cActiveBodiesBatchSize, max_jobs);
@@ -754,9 +768,9 @@ void PhysicsSystem::TrySpawnJobFindCollisions(PhysicsUpdateContext::Step *ioStep
 					ioStep->mFinalizeIslands.AddDependency();
 
 					// Start the job
-					JobHandle job = ioStep->mContext->mJobSystem->CreateJob("FindCollisions", cColorFindCollisions, [step = ioStep, job_index]() 
-						{ 
-							step->mContext->mPhysicsSystem->JobFindCollisions(step, job_index); 
+					JobHandle job = ioStep->mContext->mJobSystem->CreateJob("FindCollisions", cColorFindCollisions, [step = ioStep, job_index]()
+						{
+							step->mContext->mPhysicsSystem->JobFindCollisions(step, job_index);
 						});
 
 					// Add the job to the job barrier so the main updating thread can execute the job too
@@ -801,7 +815,7 @@ void PhysicsSystem::JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int in
 	{
 		// Check if there are active bodies to be processed
 		uint32 active_bodies_read_idx = ioStep->mActiveBodyReadIdx;
-		uint32 num_active_bodies = mBodyManager.GetNumActiveBodies();
+		uint32 num_active_bodies = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody);
 		if (active_bodies_read_idx < num_active_bodies)
 		{
 			// Take a batch of active bodies
@@ -849,7 +863,7 @@ void PhysicsSystem::JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int in
 				// Copy active bodies to temporary array, broadphase will reorder them
 				uint32 batch_size = active_bodies_read_idx_end - active_bodies_read_idx;
 				BodyID *active_bodies = (BodyID *)JPH_STACK_ALLOC(batch_size * sizeof(BodyID));
-				memcpy(active_bodies, mBodyManager.GetActiveBodiesUnsafe() + active_bodies_read_idx, batch_size * sizeof(BodyID));
+				memcpy(active_bodies, mBodyManager.GetActiveBodiesUnsafe(EBodyType::RigidBody) + active_bodies_read_idx, batch_size * sizeof(BodyID));
 
 				// Find pairs in the broadphase
 				mBroadPhase->FindCollidingPairs(active_bodies, batch_size, mPhysicsSettings.mSpeculativeContactDistance, *mObjectVsBroadPhaseLayerFilter, *mObjectLayerPairFilter, add_pair);
@@ -861,7 +875,7 @@ void PhysicsSystem::JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int in
 					TrySpawnJobFindCollisions(ioStep);
 			}
 		}
-		else 
+		else
 		{
 			// Lockless loop to get the next body pair from the pairs buffer
 			const PhysicsUpdateContext *context = ioStep->mContext;
@@ -900,7 +914,7 @@ void PhysicsSystem::JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int in
 
 				// Copy the body pair out of the buffer
 				const BodyPair bp = context->mBodyPairs[read_queue_idx * ioStep->mMaxBodyPairsPerQueue + pair_idx % ioStep->mMaxBodyPairsPerQueue];
-			
+
 				// Mark this pair as taken
 				if (queue.mReadIdx.compare_exchange_strong(pair_idx, pair_idx + 1))
 				{
@@ -927,7 +941,7 @@ void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const
 	// Ensure that body1 is dynamic, this ensures that we do the collision detection in the space of a moving body, which avoids accuracy problems when testing a very large static object against a small dynamic object
 	// Ensure that body1 id < body2 id for dynamic vs dynamic
 	// Keep body order unchanged when colliding with a sensor
-	if ((!body1->IsDynamic() || (body2->IsDynamic() && inBodyPair.mBodyB < inBodyPair.mBodyA)) 
+	if ((!body1->IsDynamic() || (body2->IsDynamic() && inBodyPair.mBodyB < inBodyPair.mBodyA))
 		&& !body2->IsSensor())
 		swap(body1, body2);
 	JPH_ASSERT(body1->IsDynamic() || body2->IsSensor());
@@ -976,11 +990,11 @@ void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const
 			class ReductionCollideShapeCollector : public CollideShapeCollector
 			{
 			public:
-								ReductionCollideShapeCollector(PhysicsSystem *inSystem, const Body *inBody1, const Body *inBody2) : 
-					mSystem(inSystem), 
+								ReductionCollideShapeCollector(PhysicsSystem *inSystem, const Body *inBody1, const Body *inBody2) :
+					mSystem(inSystem),
 					mBody1(inBody1),
 					mBody2(inBody2)
-				{ 
+				{
 				}
 
 				virtual void	AddHit(const CollideShapeResult &inResult) override
@@ -1018,7 +1032,7 @@ void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const
 
 					// Calculate normal
 					Vec3 world_space_normal = inResult.mPenetrationAxis.Normalized();
-			
+
 					// Check if we can add it to an existing manifold
 					Manifolds::iterator manifold;
 					float contact_normal_cos_max_delta_rot = mSystem->mPhysicsSettings.mContactNormalCosMaxDeltaRotation;
@@ -1099,13 +1113,13 @@ void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const
 			class NonReductionCollideShapeCollector : public CollideShapeCollector
 			{
 			public:
-								NonReductionCollideShapeCollector(PhysicsSystem *inSystem, ContactAllocator &ioContactAllocator, Body *inBody1, Body *inBody2, const ContactConstraintManager::BodyPairHandle &inPairHandle) : 
-					mSystem(inSystem), 
+								NonReductionCollideShapeCollector(PhysicsSystem *inSystem, ContactAllocator &ioContactAllocator, Body *inBody1, Body *inBody2, const ContactConstraintManager::BodyPairHandle &inPairHandle) :
+					mSystem(inSystem),
 					mContactAllocator(ioContactAllocator),
 					mBody1(inBody1),
 					mBody2(inBody2),
 					mBodyPairHandle(inPairHandle)
-				{ 
+				{
 				}
 
 				virtual void	AddHit(const CollideShapeResult &inResult) override
@@ -1152,7 +1166,7 @@ void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const
 
 					// Store penetration depth
 					manifold.mPenetrationDepth = inResult.mPenetrationDepth;
-			
+
 					// Prune if we have more than 4 points
 					if (manifold.mRelativeContactPointsOn1.size() > 4)
 						PruneContactPoints(manifold.mWorldSpaceNormal, manifold.mRelativeContactPointsOn1, manifold.mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold.mBaseOffset));
@@ -1209,11 +1223,11 @@ void PhysicsSystem::JobFinalizeIslands(PhysicsUpdateContext *ioContext)
 #endif
 
 	// Finish collecting the islands, at this point the active body list doesn't change so it's safe to access
-	mIslandBuilder.Finalize(mBodyManager.GetActiveBodiesUnsafe(), mBodyManager.GetNumActiveBodies(), mContactManager.GetNumConstraints(), ioContext->mTempAllocator);
+	mIslandBuilder.Finalize(mBodyManager.GetActiveBodiesUnsafe(EBodyType::RigidBody), mBodyManager.GetNumActiveBodies(EBodyType::RigidBody), mContactManager.GetNumConstraints(), ioContext->mTempAllocator);
 
 	// Prepare the large island splitter
 	if (mPhysicsSettings.mUseLargeIslandSplitter)
-		mLargeIslandSplitter.Prepare(mIslandBuilder, mBodyManager.GetNumActiveBodies(), ioContext->mTempAllocator);
+		mLargeIslandSplitter.Prepare(mIslandBuilder, mBodyManager.GetNumActiveBodies(EBodyType::RigidBody), ioContext->mTempAllocator);
 }
 
 void PhysicsSystem::JobBodySetIslandIndex()
@@ -1239,12 +1253,12 @@ void PhysicsSystem::JobSolveVelocityConstraints(PhysicsUpdateContext *ioContext,
 	// We update velocities and need to read positions to do so
 	BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::Read);
 #endif
-	
+
 	float delta_time = ioContext->mStepDeltaTime;
 	Constraint **active_constraints = ioContext->mActiveConstraints;
 
 	// Only the first step to correct for the delta time difference in the previous update
-	float warm_start_impulse_ratio = ioStep->mIsFirst? ioContext->mWarmStartImpulseRatio : 1.0f; 
+	float warm_start_impulse_ratio = ioStep->mIsFirst? ioContext->mWarmStartImpulseRatio : 1.0f;
 
 	bool check_islands = true, check_split_islands = mPhysicsSettings.mUseLargeIslandSplitter;
 	do
@@ -1379,7 +1393,7 @@ void PhysicsSystem::JobPreIntegrateVelocity(PhysicsUpdateContext *ioContext, Phy
 
 	// Initialize the mapping table between active body and CCD body
 	JPH_ASSERT(ioStep->mActiveBodyToCCDBody == nullptr);
-	ioStep->mNumActiveBodyToCCDBody = mBodyManager.GetNumActiveBodies();
+	ioStep->mNumActiveBodyToCCDBody = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody);
 	ioStep->mActiveBodyToCCDBody = (int *)temp_allocator->Allocate(ioStep->mNumActiveBodyToCCDBody * sizeof(int));
 
 	// Prepare the split island builder for solving the position constraints
@@ -1394,8 +1408,8 @@ void PhysicsSystem::JobIntegrateVelocity(const PhysicsUpdateContext *ioContext,
 #endif
 
 	float delta_time = ioContext->mStepDeltaTime;
-	const BodyID *active_bodies = mBodyManager.GetActiveBodiesUnsafe();
-	uint32 num_active_bodies = mBodyManager.GetNumActiveBodies();
+	const BodyID *active_bodies = mBodyManager.GetActiveBodiesUnsafe(EBodyType::RigidBody);
+	uint32 num_active_bodies = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody);
 	uint32 num_active_bodies_after_find_collisions = ioStep->mActiveBodyReadIdx;
 
 	// We can move bodies that are not part of an island. In this case we need to notify the broadphase of the movement.
@@ -1533,7 +1547,7 @@ void PhysicsSystem::JobPostIntegrateVelocity(PhysicsUpdateContext *ioContext, Ph
 		ioStep->mContactRemovedCallbacks.AddDependency(num_continuous_collision_jobs - 1); // Already had 1 dependency
 		for (int i = 0; i < num_continuous_collision_jobs; ++i)
 		{
-			JobHandle job = ioContext->mJobSystem->CreateJob("FindCCDContacts", cColorFindCCDContacts, [ioContext, ioStep]() 
+			JobHandle job = ioContext->mJobSystem->CreateJob("FindCCDContacts", cColorFindCCDContacts, [ioContext, ioStep]()
 			{
 				ioContext->mPhysicsSystem->JobFindCCDContacts(ioContext, ioStep);
 
@@ -1598,7 +1612,7 @@ void PhysicsSystem::JobFindCCDContacts(const PhysicsUpdateContext *ioContext, Ph
 	settings.mReturnDeepestPoint = true;
 	settings.mCollectFacesMode = ECollectFacesMode::CollectFaces;
 	settings.mActiveEdgeMode = mPhysicsSettings.mCheckActiveEdges? EActiveEdgeMode::CollideOnlyWithActive : EActiveEdgeMode::CollideWithAll;
-										
+
 	for (;;)
 	{
 		// Fetch the next body to cast
@@ -1611,7 +1625,7 @@ void PhysicsSystem::JobFindCCDContacts(const PhysicsUpdateContext *ioContext, Ph
 		// Filter out layers
 		DefaultBroadPhaseLayerFilter broadphase_layer_filter = GetDefaultBroadPhaseLayerFilter(body.GetObjectLayer());
 		DefaultObjectLayerFilter object_layer_filter = GetDefaultLayerFilter(body.GetObjectLayer());
-				
+
 	#ifdef JPH_DEBUG_RENDERER
 		// Draw start and end shape of cast
 		if (sDrawMotionQualityLinearCast)
@@ -1627,13 +1641,13 @@ void PhysicsSystem::JobFindCCDContacts(const PhysicsUpdateContext *ioContext, Ph
 		class CCDNarrowPhaseCollector : public CastShapeCollector
 		{
 		public:
-										CCDNarrowPhaseCollector(const BodyManager &inBodyManager, ContactConstraintManager &inContactConstraintManager, CCDBody &inCCDBody, ShapeCastResult &inResult, float inDeltaTime) : 
+										CCDNarrowPhaseCollector(const BodyManager &inBodyManager, ContactConstraintManager &inContactConstraintManager, CCDBody &inCCDBody, ShapeCastResult &inResult, float inDeltaTime) :
 				mBodyManager(inBodyManager),
 				mContactConstraintManager(inContactConstraintManager),
 				mCCDBody(inCCDBody),
 				mResult(inResult),
 				mDeltaTime(inDeltaTime)
-			{ 
+			{
 			}
 
 			virtual void				AddHit(const ShapeCastResult &inResult) override
@@ -1882,7 +1896,7 @@ void PhysicsSystem::JobResolveCCDContacts(PhysicsUpdateContext *ioContext, Physi
 	{
 		// Sort on fraction so that we process earliest collisions first
 		// This is needed to make the simulation deterministic and also to be able to stop contact processing
-		// between body pairs if an earlier hit was found involving the body by another CCD body 
+		// between body pairs if an earlier hit was found involving the body by another CCD body
 		// (if it's body ID < this CCD body's body ID - see filtering logic in CCDBroadPhaseCollector)
 		CCDBody **sorted_ccd_bodies = (CCDBody **)temp_allocator->Allocate(num_ccd_bodies * sizeof(CCDBody *));
 		{
@@ -1896,8 +1910,8 @@ void PhysicsSystem::JobResolveCCDContacts(PhysicsUpdateContext *ioContext, Physi
 				*(dst_ccd_bodies++) = src_ccd_bodies++;
 
 			// Which we then sort
-			QuickSort(sorted_ccd_bodies, sorted_ccd_bodies + num_ccd_bodies, [](const CCDBody *inBody1, const CCDBody *inBody2) 
-				{ 
+			QuickSort(sorted_ccd_bodies, sorted_ccd_bodies + num_ccd_bodies, [](const CCDBody *inBody1, const CCDBody *inBody2)
+				{
 					if (inBody1->mFractionPlusSlop != inBody2->mFractionPlusSlop)
 						return inBody1->mFractionPlusSlop < inBody2->mFractionPlusSlop;
 
@@ -1952,7 +1966,7 @@ void PhysicsSystem::JobResolveCCDContacts(PhysicsUpdateContext *ioContext, Physi
 					Vec3 r2 = Vec3(ccd_body->mContactPointOn2 - body2.GetCenterOfMassPosition());
 
 					// Calculate velocity of collision points
-					Vec3 v1 = body1.GetPointVelocityCOM(r1_plus_u); 
+					Vec3 v1 = body1.GetPointVelocityCOM(r1_plus_u);
 					Vec3 v2 = body2.GetPointVelocityCOM(r2);
 					Vec3 relative_velocity = v2 - v1;
 					float normal_velocity = relative_velocity.Dot(ccd_body->mContactNormal);
@@ -2099,7 +2113,7 @@ public:
 	inline					BodiesToSleep(BodyManager &inBodyManager, BodyID *inBodiesToSleepBuffer) : mBodyManager(inBodyManager), mBodiesToSleepBuffer(inBodiesToSleepBuffer), mBodiesToSleepCur(inBodiesToSleepBuffer) { }
 
 	inline					~BodiesToSleep()
-	{		
+	{
 		// Flush the bodies to sleep buffer
 		int num_bodies_in_buffer = int(mBodiesToSleepCur - mBodiesToSleepBuffer);
 		if (num_bodies_in_buffer > 0)
@@ -2315,6 +2329,46 @@ void PhysicsSystem::JobSolvePositionConstraints(PhysicsUpdateContext *ioContext,
 	while (check_islands || check_split_islands);
 }
 
+void PhysicsSystem::JobUpdateSoftBodies(const PhysicsUpdateContext *ioContext)
+{
+	JPH_PROFILE_FUNCTION();
+
+#ifdef JPH_ENABLE_ASSERTS
+	// Can activate bodies only
+	BodyManager::GrantActiveBodiesAccess grant_active(true, false);
+#endif
+
+	static constexpr int cBodiesBatch = 64;
+	BodyID *bodies_to_update_bounds = (BodyID *)JPH_STACK_ALLOC(cBodiesBatch * sizeof(BodyID));
+	int num_bodies_to_update_bounds = 0;
+
+	// Loop through active bodies
+	const BodyID *active_bodies = mBodyManager.GetActiveBodiesUnsafe(EBodyType::SoftBody);
+	const BodyID *active_bodies_end = active_bodies + mBodyManager.GetNumActiveBodies(EBodyType::SoftBody);
+	for (const BodyID *b = active_bodies; b < active_bodies_end; ++b)
+	{
+		Body &body = mBodyManager.GetBody(*b);
+		SoftBodyMotionProperties *mp = static_cast<SoftBodyMotionProperties *>(body.GetMotionProperties());
+
+		// Update the soft body
+		Vec3 delta_position;
+		mp->Update(ioContext->mStepDeltaTime, body, delta_position, *this);
+		body.SetPositionAndRotationInternal(body.GetPosition() + delta_position, body.GetRotation());
+
+		bodies_to_update_bounds[num_bodies_to_update_bounds++] = *b;
+		if (num_bodies_to_update_bounds == cBodiesBatch)
+		{
+			// Buffer full, flush now
+			mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false);
+			num_bodies_to_update_bounds = 0;
+		}
+	}
+
+	// Notify change bounds on requested bodies
+	if (num_bodies_to_update_bounds > 0)
+		mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false);
+}
+
 void PhysicsSystem::SaveState(StateRecorder &inStream) const
 {
 	JPH_PROFILE_FUNCTION();

Jolt/Physics/PhysicsSystem.h

@@ -42,7 +42,7 @@ public:
 	/// @param inObjectVsBroadPhaseLayerFilter Filter callback function that is used to determine if an object layer collides with a broad phase layer. Since this is a virtual interface, the instance needs to stay alive during the lifetime of the PhysicsSystem.
 	/// @param inObjectLayerPairFilter Filter callback function that is used to determine if two object layers collide. Since this is a virtual interface, the instance needs to stay alive during the lifetime of the PhysicsSystem.
 	void						Init(uint inMaxBodies, uint inNumBodyMutexes, uint inMaxBodyPairs, uint inMaxContactConstraints, const BroadPhaseLayerInterface &inBroadPhaseLayerInterface, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter);
-	
+
 	/// Listener that is notified whenever a body is activated/deactivated
 	void						SetBodyActivationListener(BodyActivationListener *inListener) { mBodyManager.SetBodyActivationListener(inListener); }
 	BodyActivationListener *	GetBodyActivationListener() const							{ return mBodyManager.GetBodyActivationListener(); }
@@ -54,10 +54,12 @@ public:
 	/// Set the function that combines the friction of two bodies and returns it
 	/// Default method is the geometric mean: sqrt(friction1 * friction2).
 	void						SetCombineFriction(ContactConstraintManager::CombineFunction inCombineFriction) { mContactManager.SetCombineFriction(inCombineFriction); }
+	ContactConstraintManager::CombineFunction GetCombineFriction() const					{ return mContactManager.GetCombineFriction(); }
 
 	/// Set the function that combines the restitution of two bodies and returns it
 	/// Default method is max(restitution1, restitution1)
 	void						SetCombineRestitution(ContactConstraintManager::CombineFunction inCombineRestition) { mContactManager.SetCombineRestitution(inCombineRestition); }
+	ContactConstraintManager::CombineFunction GetCombineRestitution() const					{ return mContactManager.GetCombineRestitution(); }
 
 	/// Control the main constants of the physics simulation
 	void						SetPhysicsSettings(const PhysicsSettings &inSettings)		{ mPhysicsSettings = inSettings; }
@@ -78,7 +80,7 @@ public:
 
 	/// Add constraint to the world
 	void						AddConstraint(Constraint *inConstraint)						{ mConstraintManager.Add(&inConstraint, 1); }
-	
+
 	/// Remove constraint from the world
 	void						RemoveConstraint(Constraint *inConstraint)					{ mConstraintManager.Remove(&inConstraint, 1); }
 
@@ -149,7 +151,7 @@ public:
 	uint						GetNumBodies() const										{ return mBodyManager.GetNumBodies(); }
 
 	/// Gets the current amount of active bodies that are in the body manager
-	uint32						GetNumActiveBodies() const									{ return mBodyManager.GetNumActiveBodies(); }
+	uint32						GetNumActiveBodies(EBodyType inType) const					{ return mBodyManager.GetNumActiveBodies(inType); }
 
 	/// Get the maximum amount of bodies that this physics system supports
 	uint						GetMaxBodies() const										{ return mBodyManager.GetMaxBodies(); }
@@ -165,12 +167,13 @@ public:
 	void						GetBodies(BodyIDVector &outBodyIDs) const					{ return mBodyManager.GetBodyIDs(outBodyIDs); }
 
 	/// Get copy of the list of active bodies under protection of a lock.
+	/// @param inType The type of bodies to get
 	/// @param outBodyIDs On return, this will contain the list of BodyIDs
-	void						GetActiveBodies(BodyIDVector &outBodyIDs) const				{ return mBodyManager.GetActiveBodies(outBodyIDs); }
+	void						GetActiveBodies(EBodyType inType, BodyIDVector &outBodyIDs) const { return mBodyManager.GetActiveBodies(inType, outBodyIDs); }
 
 	/// Get the list of active bodies, use GetNumActiveBodies() to find out how long the list is.
 	/// Note: Not thread safe. The active bodies list can change at any moment when other threads are doing work. Use GetActiveBodies() if you need a thread safe version.
-	const BodyID *				GetActiveBodiesUnsafe() const								{ return mBodyManager.GetActiveBodiesUnsafe(); }
+	const BodyID *				GetActiveBodiesUnsafe(EBodyType inType) const				{ return mBodyManager.GetActiveBodiesUnsafe(inType); }
 
 	/// Check if 2 bodies were in contact during the last simulation step. Since contacts are only detected between active bodies, so at least one of the bodies must be active in order for this function to work.
 	/// It queries the state at the time of the last PhysicsSystem::Update and will return true if the bodies were in contact, even if one of the bodies was moved / removed afterwards.
@@ -190,7 +193,7 @@ private:
 	// Various job entry points
 	void						JobStepListeners(PhysicsUpdateContext::Step *ioStep);
 	void						JobDetermineActiveConstraints(PhysicsUpdateContext::Step *ioStep) const;
-	void						JobApplyGravity(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep);	
+	void						JobApplyGravity(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep);
 	void						JobSetupVelocityConstraints(float inDeltaTime, PhysicsUpdateContext::Step *ioStep) const;
 	void						JobBuildIslandsFromConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep);
 	void						JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int inJobIndex);
@@ -204,6 +207,7 @@ private:
 	void						JobResolveCCDContacts(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep);
 	void						JobContactRemovedCallbacks(const PhysicsUpdateContext::Step *ioStep);
 	void						JobSolvePositionConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep);
+	void						JobUpdateSoftBodies(const PhysicsUpdateContext *ioContext);
 
 	/// Tries to spawn a new FindCollisions job if max concurrency hasn't been reached yet
 	void						TrySpawnJobFindCollisions(PhysicsUpdateContext::Step *ioStep) const;
@@ -260,7 +264,7 @@ private:
 
 	/// The broadphase does quick collision detection between body pairs
 	BroadPhase *				mBroadPhase = nullptr;
-    
+
     /// Simulation settings
 	PhysicsSettings				mPhysicsSettings;
 

Jolt/Physics/PhysicsUpdateContext.h

@@ -63,13 +63,13 @@ public:
 
 		atomic<uint32>		mConstraintReadIdx { 0 };								///< Next constraint for determine active constraints
 		uint8				mPadding1[JPH_CACHE_LINE_SIZE - sizeof(atomic<uint32>)];///< Padding to avoid sharing cache line with the next atomic
-		
+
 		atomic<uint32>		mNumActiveConstraints { 0 };							///< Number of constraints in the mActiveConstraints array
 		uint8				mPadding2[JPH_CACHE_LINE_SIZE - sizeof(atomic<uint32>)];///< Padding to avoid sharing cache line with the next atomic
-		
+
 		atomic<uint32>		mStepListenerReadIdx { 0 };								///< Next step listener to call
 		uint8				mPadding3[JPH_CACHE_LINE_SIZE - sizeof(atomic<uint32>)];///< Padding to avoid sharing cache line with the next atomic
-		
+
 		atomic<uint32>		mApplyGravityReadIdx { 0 };								///< Next body to apply gravity to
 		uint8				mPadding4[JPH_CACHE_LINE_SIZE - sizeof(atomic<uint32>)];///< Padding to avoid sharing cache line with the next atomic
 
@@ -130,6 +130,7 @@ public:
 		JobHandle			mResolveCCDContacts;									///< Updates the positions and velocities for all bodies that need continuous collision detection
 		JobHandleArray		mSolvePositionConstraints;								///< Solve all constraints in the position domain
 		JobHandle			mContactRemovedCallbacks;								///< Calls the contact removed callbacks
+		JobHandle			mUpdateSoftBodies;										///< Updates all soft bodies
 		JobHandle			mStartNextStep;											///< Job that kicks the next step (empty for the last step)
 	};
 

Jolt/Physics/SoftBody/SoftBodyCreationSettings.cpp

@@ -0,0 +1,63 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/SoftBody/SoftBodyCreationSettings.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodyCreationSettings)
+{
+	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mSettings)
+	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mPosition)
+	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mRotation)
+	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mUserData)
+	JPH_ADD_ENUM_ATTRIBUTE(SoftBodyCreationSettings, mObjectLayer)
+	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mCollisionGroup)
+	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mNumIterations)
+	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mRestitution)
+	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mFriction)
+	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mPressure)
+	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mGravityFactor)
+	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mUpdatePosition)
+	JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mMakeRotationIdentity)
+}
+
+void SoftBodyCreationSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(mPosition);
+	inStream.Write(mRotation);
+	inStream.Write(mUserData);
+	inStream.Write(mObjectLayer);
+	mCollisionGroup.SaveBinaryState(inStream);
+	inStream.Write(mNumIterations);
+	inStream.Write(mRestitution);
+	inStream.Write(mFriction);
+	inStream.Write(mPressure);
+	inStream.Write(mGravityFactor);
+	inStream.Write(mUpdatePosition);
+	inStream.Write(mMakeRotationIdentity);
+}
+
+void SoftBodyCreationSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	inStream.Read(mPosition);
+	inStream.Read(mRotation);
+	inStream.Read(mUserData);
+	inStream.Read(mObjectLayer);
+	mCollisionGroup.RestoreBinaryState(inStream);
+	inStream.Read(mNumIterations);
+	inStream.Read(mRestitution);
+	inStream.Read(mFriction);
+	inStream.Read(mPressure);
+	inStream.Read(mGravityFactor);
+	inStream.Read(mUpdatePosition);
+	inStream.Read(mMakeRotationIdentity);
+}
+
+JPH_NAMESPACE_END

Jolt/Physics/SoftBody/SoftBodyCreationSettings.h

@@ -0,0 +1,53 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/SoftBody/SoftBodySharedSettings.h>
+#include <Jolt/Physics/Collision/ObjectLayer.h>
+#include <Jolt/Physics/Collision/CollisionGroup.h>
+#include <Jolt/ObjectStream/SerializableObject.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// This class contains the information needed to create a soft body object
+/// Note: Soft bodies are still in development and come with several caveats. Read the Architecture and API documentation for more information!
+class JPH_EXPORT SoftBodyCreationSettings
+{
+public:
+	JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SoftBodyCreationSettings)
+
+	/// Constructor
+						SoftBodyCreationSettings() = default;
+						SoftBodyCreationSettings(const SoftBodySharedSettings *inSettings, RVec3Arg inPosition = RVec3::sZero(), QuatArg inRotation = Quat::sIdentity()) : mSettings(inSettings), mPosition(inPosition), mRotation(inRotation) { }
+
+	/// Saves the state of this object in binary form to inStream. Doesn't store the shared settings nor the group filter.
+	void				SaveBinaryState(StreamOut &inStream) const;
+
+	/// Restore the state of this object from inStream. Doesn't restore the shared settings nor the group filter.
+	void				RestoreBinaryState(StreamIn &inStream);
+
+	RefConst<SoftBodySharedSettings> mSettings;				///< Defines the configuration of this soft body
+
+	RVec3				mPosition { RVec3::sZero() };		///< Initial position of the soft body
+	Quat				mRotation { Quat::sIdentity() };	///< Initial rotation of the soft body
+
+	/// User data value (can be used by application)
+	uint64				mUserData = 0;
+
+	///@name Collision settings
+	ObjectLayer			mObjectLayer = 0;					///< The collision layer this body belongs to (determines if two objects can collide)
+	CollisionGroup		mCollisionGroup;					///< The collision group this body belongs to (determines if two objects can collide)
+
+	uint32				mNumIterations = 5;					///< Number of solver iterations
+	float				mLinearDamping = 0.05f;				///< Linear damping: dv/dt = -mLinearDamping * v
+	float				mRestitution = 0.0f;				///< Restitution when colliding
+	float				mFriction = 0.2f;					///< Friction coefficient when colliding
+	float				mPressure = 0.0f;					///< n * R * T, amount of substance * ideal gass constant * absolute temperature, see https://en.wikipedia.org/wiki/Pressure
+	float				mGravityFactor = 1.0f;				///< Value to multiply gravity with for this body
+	bool				mUpdatePosition = true;				///< Update the position of the body while simulating (set to false for something that is attached to the static world)
+	bool				mMakeRotationIdentity = true;		///< Bake specified mRotation in the vertices and set the body rotation to identity (simulation is slightly more accurate if the rotation of a soft body is kept to identity)
+};
+
+JPH_NAMESPACE_END

Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp

@@ -0,0 +1,539 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/SoftBody/SoftBodyMotionProperties.h>
+#include <Jolt/Physics/SoftBody/SoftBodyCreationSettings.h>
+#include <Jolt/Physics/PhysicsSystem.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+void SoftBodyMotionProperties::CalculateMassAndInertia()
+{
+	MassProperties mp;
+
+	for (const Vertex &v : mVertices)
+		if (v.mInvMass > 0.0f)
+		{
+			Vec3 pos = v.mPosition;
+
+			// Accumulate mass
+			float mass = 1.0f / v.mInvMass;
+			mp.mMass += mass;
+
+			// Inertia tensor, diagonal
+			// See equations https://en.wikipedia.org/wiki/Moment_of_inertia section 'Inertia Tensor'
+			for (int i = 0; i < 3; ++i)
+				mp.mInertia(i, i) += mass * (Square(pos[(i + 1) % 3]) + Square(pos[(i + 2) % 3]));
+
+			// Inertia tensor off diagonal
+			for (int i = 0; i < 3; ++i)
+				for (int j = 0; j < 3; ++j)
+					if (i != j)
+						mp.mInertia(i, j) -= mass * pos[i] * pos[j];
+		}
+		else
+		{
+			// If one vertex is kinematic, the entire body will have infinite mass and inertia
+			SetInverseMass(0.0f);
+			SetInverseInertia(Vec3::sZero(), Quat::sIdentity());
+			return;
+		}
+
+	SetMassProperties(EAllowedDOFs::All, mp);
+}
+
+void SoftBodyMotionProperties::Initialize(const SoftBodyCreationSettings &inSettings)
+{
+	// Store settings
+	mSettings = inSettings.mSettings;
+	mNumIterations = inSettings.mNumIterations;
+	mPressure = inSettings.mPressure;
+	mUpdatePosition = inSettings.mUpdatePosition;
+
+	// Initialize vertices
+	mVertices.resize(inSettings.mSettings->mVertices.size());
+	Mat44 rotation = inSettings.mMakeRotationIdentity? Mat44::sRotation(inSettings.mRotation) : Mat44::sIdentity();
+	for (Array<Vertex>::size_type v = 0, s = mVertices.size(); v < s; ++v)
+	{
+		const SoftBodySharedSettings::Vertex &in_vertex = inSettings.mSettings->mVertices[v];
+		Vertex &out_vertex = mVertices[v];
+		out_vertex.mPreviousPosition = out_vertex.mPosition = rotation * Vec3(in_vertex.mPosition);
+		out_vertex.mVelocity = rotation.Multiply3x3(Vec3(in_vertex.mVelocity));
+		out_vertex.mCollidingShapeIndex = -1;
+		out_vertex.mLargestPenetration = -FLT_MAX;
+		out_vertex.mInvMass = in_vertex.mInvMass;
+		out_vertex.mProjectedDistance = 0.0f;
+		mLocalBounds.Encapsulate(out_vertex.mPosition);
+	}
+
+	// We don't know delta time yet, so we can't predict the bounds and use the local bounds as the predicted bounds
+	mLocalPredictedBounds = mLocalBounds;
+
+	CalculateMassAndInertia();
+}
+
+float SoftBodyMotionProperties::GetVolumeTimesSix() const
+{
+	float six_volume = 0.0f;
+	for (const Face &f : mSettings->mFaces)
+	{
+		Vec3 x1 = mVertices[f.mVertex[0]].mPosition;
+		Vec3 x2 = mVertices[f.mVertex[1]].mPosition;
+		Vec3 x3 = mVertices[f.mVertex[2]].mPosition;
+		six_volume += x1.Cross(x2).Dot(x3); // We pick zero as the origin as this is the center of the bounding box so should give good accuracy
+	}
+	return six_volume;
+}
+
+void SoftBodyMotionProperties::Update(float inDeltaTime, Body &inSoftBody, Vec3 &outDeltaPosition, PhysicsSystem &inSystem)
+{
+	// Based on: XPBD, Extended Position Based Dynamics, Matthias Muller, Ten Minute Physics
+	// See: https://matthias-research.github.io/pages/tenMinutePhysics/09-xpbd.pdf
+
+	// Convert gravity to local space
+	RMat44 body_transform = inSoftBody.GetCenterOfMassTransform();
+	Vec3 gravity = body_transform.Multiply3x3Transposed(GetGravityFactor() * inSystem.GetGravity());
+
+	// Collect information about the colliding bodies
+	struct CollidingShape
+	{
+		/// Get the velocity of a point on this body
+		Vec3			GetPointVelocity(Vec3Arg inPointRelativeToCOM) const
+		{
+			return mLinearVelocity + mAngularVelocity.Cross(inPointRelativeToCOM);
+		}
+
+		Mat44			mCenterOfMassTransform;				///< Transform of the body relative to the soft body
+		RefConst<Shape>	mShape;
+		BodyID			mBodyID;							///< Body ID of the body we hit
+		EMotionType		mMotionType;						///< Motion type of the body we hit
+		float			mInvMass;							///< Inverse mass of the body we hit
+		float			mFriction;							///< Combined friction of the two bodies
+		float			mRestitution;						///< Combined restitution of the two bodies
+		bool 			mUpdateVelocities;					///< If the linear/angular velocity changed and the body needs to be updated
+		Mat44			mInvInertia;						///< Inverse inertia in local space to the soft body
+		Vec3			mLinearVelocity;					///< Linear velocity of the body in local space to the soft body
+		Vec3			mAngularVelocity;					///< Angular velocity of the body in local space to the soft body
+	};
+	struct Collector : public CollideShapeBodyCollector
+	{
+									Collector(Body &inSoftBody, RMat44Arg inTransform, const PhysicsSystem &inSystem) :
+										mSoftBody(inSoftBody),
+										mInverseTransform(inTransform.InversedRotationTranslation()),
+										mBodyLockInterface(inSystem.GetBodyLockInterfaceNoLock()),
+										mCombineFriction(inSystem.GetCombineFriction()),
+										mCombineRestitution(inSystem.GetCombineRestitution())
+		{
+		}
+
+		virtual void				AddHit(const BodyID &inResult) override
+		{
+			BodyLockRead lock(mBodyLockInterface, inResult);
+			if (lock.Succeeded())
+			{
+				const Body &body = lock.GetBody();
+				if (body.IsRigidBody() // TODO: We should support soft body vs soft body
+					&& mSoftBody.GetCollisionGroup().CanCollide(body.GetCollisionGroup()))
+				{
+					CollidingShape cs;
+					cs.mCenterOfMassTransform = (mInverseTransform * body.GetCenterOfMassTransform()).ToMat44();
+					cs.mShape = body.GetShape();
+					cs.mBodyID = inResult;
+					cs.mMotionType = body.GetMotionType();
+					cs.mUpdateVelocities = false;
+					cs.mFriction = mCombineFriction(mSoftBody, SubShapeID(), body, SubShapeID());
+					cs.mRestitution = mCombineRestitution(mSoftBody, SubShapeID(), body, SubShapeID());
+					if (cs.mMotionType == EMotionType::Dynamic)
+					{
+						const MotionProperties *mp = body.GetMotionProperties();
+						cs.mInvMass = mp->GetInverseMass();
+						cs.mInvInertia = mp->GetInverseInertiaForRotation(cs.mCenterOfMassTransform.GetRotation());
+						cs.mLinearVelocity = mInverseTransform.Multiply3x3(mp->GetLinearVelocity());
+						cs.mAngularVelocity = mInverseTransform.Multiply3x3(mp->GetAngularVelocity());
+					}
+					mHits.push_back(cs);
+				}
+			}
+		}
+
+		Body &						mSoftBody;
+		RMat44						mInverseTransform;
+		const BodyLockInterface &	mBodyLockInterface;
+		ContactConstraintManager::CombineFunction mCombineFriction;
+		ContactConstraintManager::CombineFunction mCombineRestitution;
+		Array<CollidingShape>		mHits;
+	};
+	Collector collector(inSoftBody, body_transform, inSystem);
+	AABox bounds = mLocalBounds;
+	bounds.Encapsulate(mLocalPredictedBounds);
+	bounds = bounds.Transformed(body_transform);
+	DefaultBroadPhaseLayerFilter broadphase_layer_filter = inSystem.GetDefaultBroadPhaseLayerFilter(inSoftBody.GetObjectLayer());
+	DefaultObjectLayerFilter object_layer_filter = inSystem.GetDefaultLayerFilter(inSoftBody.GetObjectLayer());
+	inSystem.GetBroadPhaseQuery().CollideAABox(bounds, collector, broadphase_layer_filter, object_layer_filter);
+
+	// Calculate delta time for sub step
+	float dt = inDeltaTime / mNumIterations;
+	float dt_sq = Square(dt);
+
+	// Calculate total displacement we'll have due to gravity over all sub steps
+	// The total displacement as produced by our integrator can be written as: Sum(i * g * dt^2, i = 0..mNumIterations).
+	// This is bigger than 0.5 * g * dt^2 because we first increment the velocity and then update the position
+	// Using Sum(i, i = 0..n) = n * (n + 1) / 2 we can write this as:
+	Vec3 displacement_due_to_gravity = (0.5f * mNumIterations * (mNumIterations + 1) * dt_sq) * gravity;
+
+	// Generate collision planes
+	for (const CollidingShape &cs : collector.mHits)
+		cs.mShape->CollideSoftBodyVertices(cs.mCenterOfMassTransform, mVertices, inDeltaTime, displacement_due_to_gravity, int(&cs - collector.mHits.data()));
+
+	float inv_dt_sq = 1.0f / dt_sq;
+	float linear_damping = max(0.0f, 1.0f - GetLinearDamping() * dt); // See: MotionProperties::ApplyForceTorqueAndDragInternal
+
+	for (uint iteration = 0; iteration < mNumIterations; ++iteration)
+	{
+		float pressure_coefficient = mPressure;
+		if (pressure_coefficient > 0.0f)
+		{
+			// Calculate total volume
+			float six_volume = GetVolumeTimesSix();
+			if (six_volume > 0.0f)
+			{
+				// Apply pressure
+				// p = F / A = n R T / V (see https://en.wikipedia.org/wiki/Pressure)
+				// Our pressure coefficient is n R T so the impulse is:
+				// P = F dt = pressure_coefficient / V * A * dt
+				float coefficient = pressure_coefficient * dt / six_volume; // Need to still multiply by 6 for the volume
+				for (const Face &f : mSettings->mFaces)
+				{
+					Vec3 x1 = mVertices[f.mVertex[0]].mPosition;
+					Vec3 x2 = mVertices[f.mVertex[1]].mPosition;
+					Vec3 x3 = mVertices[f.mVertex[2]].mPosition;
+
+					Vec3 impulse = coefficient * (x2 - x1).Cross(x3 - x1); // Area is half the cross product so need to still divide by 2
+					for (uint32 i : f.mVertex)
+					{
+						Vertex &v = mVertices[i];
+						v.mVelocity += v.mInvMass * impulse; // Want to divide by 3 because we spread over 3 vertices
+					}
+				}
+			}
+		}
+
+		// Integrate
+		Vec3 sub_step_gravity = gravity * dt;
+		for (Vertex &v : mVertices)
+			if (v.mInvMass > 0.0f)
+			{
+				// Gravity
+				v.mVelocity += sub_step_gravity;
+
+				// Damping
+				v.mVelocity *= linear_damping;
+
+				// Integrate
+				v.mPreviousPosition = v.mPosition;
+				v.mPosition += v.mVelocity * dt;
+
+				// Reset projected distance
+				v.mProjectedDistance = 0.0f;
+			}
+			else
+			{
+				// Integrate
+				v.mPreviousPosition = v.mPosition;
+				v.mPosition += v.mVelocity * dt;
+			}
+
+		// Satisfy volume constraints
+		for (const Volume &v : mSettings->mVolumeConstraints)
+		{
+			Vertex &v1 = mVertices[v.mVertex[0]];
+			Vertex &v2 = mVertices[v.mVertex[1]];
+			Vertex &v3 = mVertices[v.mVertex[2]];
+			Vertex &v4 = mVertices[v.mVertex[3]];
+
+			Vec3 x1 = v1.mPosition;
+			Vec3 x2 = v2.mPosition;
+			Vec3 x3 = v3.mPosition;
+			Vec3 x4 = v4.mPosition;
+
+			// Calculate constraint equation
+			Vec3 x1x2 = x2 - x1;
+			Vec3 x1x3 = x3 - x1;
+			Vec3 x1x4 = x4 - x1;
+			float c = abs(x1x2.Cross(x1x3).Dot(x1x4)) - v.mSixRestVolume;
+
+			// Calculate gradient of constraint equation
+			Vec3 d1c = (x4 - x2).Cross(x3 - x2);
+			Vec3 d2c = x1x3.Cross(x1x4);
+			Vec3 d3c = x1x4.Cross(x1x2);
+			Vec3 d4c = x1x2.Cross(x1x3);
+
+			float w1 = v1.mInvMass;
+			float w2 = v2.mInvMass;
+			float w3 = v3.mInvMass;
+			float w4 = v4.mInvMass;
+			JPH_ASSERT(w1 > 0.0f || w2 > 0.0f || w3 > 0.0f || w4 > 0.0f);
+
+			// Apply correction
+			float lambda = -c / (w1 * d1c.LengthSq() + w2 * d2c.LengthSq() + w3 * d3c.LengthSq() + w4 * d4c.LengthSq() + v.mCompliance * inv_dt_sq);
+			v1.mPosition += lambda * w1 * d1c;
+			v2.mPosition += lambda * w2 * d2c;
+			v3.mPosition += lambda * w3 * d3c;
+			v4.mPosition += lambda * w4 * d4c;
+		}
+
+		// Satisfy edge constraints
+		for (const Edge &e : mSettings->mEdgeConstraints)
+		{
+			Vertex &v0 = mVertices[e.mVertex[0]];
+			Vertex &v1 = mVertices[e.mVertex[1]];
+
+			// Calculate current length
+			Vec3 delta = v1.mPosition - v0.mPosition;
+			float length = delta.Length();
+			if (length > 0.0f)
+			{
+				// Apply correction
+				Vec3 correction = delta * (length - e.mRestLength) / (length * (v0.mInvMass + v1.mInvMass + e.mCompliance * inv_dt_sq));
+				v0.mPosition += v0.mInvMass * correction;
+				v1.mPosition -= v1.mInvMass * correction;
+			}
+		}
+
+		// Satisfy collision
+		for (Vertex &v : mVertices)
+			if (v.mCollidingShapeIndex >= 0)
+			{
+				float distance = v.mCollisionPlane.SignedDistance(v.mPosition);
+				if (distance < 0.0f)
+				{
+					Vec3 delta = v.mCollisionPlane.GetNormal() * distance;
+					v.mPosition -= delta;
+					v.mPreviousPosition -= delta; // Apply delta to previous position so that we will not accumulate velocity by being pushed out of collision
+					v.mProjectedDistance -= distance; // For friction calculation
+				}
+			}
+
+		// Update velocity
+		float restitution_treshold = -2.0f * gravity.Length() * dt;
+		for (Vertex &v : mVertices)
+			if (v.mInvMass > 0.0f)
+			{
+				Vec3 prev_v = v.mVelocity;
+
+				// XPBD velocity update
+				v.mVelocity = (v.mPosition - v.mPreviousPosition) / dt;
+
+				// If there was a collision
+				if (v.mProjectedDistance > 0.0f)
+				{
+					JPH_ASSERT(v.mCollidingShapeIndex >= 0);
+					CollidingShape &cs = collector.mHits[v.mCollidingShapeIndex];
+
+					// Apply friction as described in Detailed Rigid Body Simulation with Extended Position Based Dynamics - Matthias Muller et al.
+					// See section 3.6:
+					// Inverse mass: w1 = 1 / m1, w2 = 1 / m2 + (r2 x n)^T I^-1 (r2 x n) = 0 for a static object
+					// r2 are the contact point relative to the center of mass of body 2
+					// Lagrange multiplier for contact: lambda = -c / (w1 + w2)
+					// Where c is the constraint equation (the distance to the plane, negative because penetrating)
+					// Contact normal force: fn = lambda / dt^2
+					// Delta velocity due to friction dv = -vt / |vt| * min(dt * friction * fn * (w1 + w2), |vt|) = -vt * min(-friction * c / (|vt| * dt), 1)
+					// Note that I think there is an error in the paper, I added a mass term, see: https://github.com/matthias-research/pages/issues/29
+					// Relative velocity: vr = v1 - v2 - omega2 x r2
+					// Normal velocity: vn = vr . contact_normal
+					// Tangential velocity: vt = vr - contact_normal * vn
+					// Impulse: p = dv / (w1 + w2)
+					// Changes in particle velocities:
+					// v1 = v1 + p / m1
+					// v2 = v2 - p / m2 (no change when colliding with a static body)
+					// w2 = w2 - I^-1 (r2 x p) (no change when colliding with a static body)
+					Vec3 contact_normal = v.mCollisionPlane.GetNormal();
+					if (cs.mMotionType == EMotionType::Dynamic)
+					{
+						// Calculate normal and tangential velocity (equation 30)
+						Vec3 r2 = v.mPosition - cs.mCenterOfMassTransform.GetTranslation();
+						Vec3 v2 = cs.GetPointVelocity(r2);
+						Vec3 relative_velocity = v.mVelocity - v2;
+						Vec3 v_normal = contact_normal * contact_normal.Dot(relative_velocity);
+						Vec3 v_tangential = relative_velocity - v_normal;
+						float v_tangential_length = v_tangential.Length();
+
+						// Calculate inverse effective mass
+						Vec3 r2_cross_n = r2.Cross(contact_normal);
+						float w2 = cs.mInvMass + r2_cross_n.Dot(cs.mInvInertia * r2_cross_n);
+						float w1_plus_w2 = v.mInvMass + w2;
+
+						// Calculate delta relative velocity due to friction (modified equation 31)
+						Vec3 dv;
+						if (v_tangential_length > 0.0f)
+							dv = v_tangential * min(cs.mFriction * v.mProjectedDistance / (v_tangential_length * dt), 1.0f);
+						else
+							dv = Vec3::sZero();
+
+						// Calculate delta relative velocity due to restitution (equation 35)
+						dv += v_normal;
+						float prev_v_normal = (prev_v - v2).Dot(contact_normal);
+						if (prev_v_normal < restitution_treshold)
+							dv += cs.mRestitution * prev_v_normal * contact_normal;
+
+						// Calculate impulse
+						Vec3 p = dv / w1_plus_w2;
+
+						// Apply impulse to particle
+						v.mVelocity -= p * v.mInvMass;
+
+						// Apply impulse to rigid body
+						cs.mLinearVelocity += p * cs.mInvMass;
+						cs.mAngularVelocity += cs.mInvInertia * r2.Cross(p);
+
+						// Mark that the velocities of the body we hit need to be updated
+						cs.mUpdateVelocities = true;
+					}
+					else
+					{
+						// Body is not moveable, equations are simpler
+
+						// Calculate normal and tangential velocity (equation 30)
+						Vec3 v_normal = contact_normal * contact_normal.Dot(v.mVelocity);
+						Vec3 v_tangential = v.mVelocity - v_normal;
+						float v_tangential_length = v_tangential.Length();
+
+						// Apply friction (modified equation 31)
+						if (v_tangential_length > 0.0f)
+							v.mVelocity -= v_tangential * min(cs.mFriction * v.mProjectedDistance / (v_tangential_length * dt), 1.0f);
+
+						// Apply restitution (equation 35)
+						v.mVelocity -= v_normal;
+						float prev_v_normal = prev_v.Dot(contact_normal);
+						if (prev_v_normal < restitution_treshold)
+							v.mVelocity -= cs.mRestitution * prev_v_normal * contact_normal;
+					}
+				}
+			}
+		}
+
+	// Loop through vertices once more to update the global state
+	Vec3 linear_velocity = Vec3::sZero(), angular_velocity = Vec3::sZero();
+	mLocalPredictedBounds = mLocalBounds = { };
+	for (Vertex &v : mVertices)
+	{
+		// Calculate local linear/angular velocity
+		linear_velocity += v.mVelocity;
+		angular_velocity += v.mPosition.Cross(v.mVelocity);
+
+		// Update local bounding box
+		mLocalBounds.Encapsulate(v.mPosition);
+
+		// Create predicted position for the next frame in order to detect collisions before they happen
+		mLocalPredictedBounds.Encapsulate(v.mPosition + v.mVelocity * inDeltaTime + displacement_due_to_gravity);
+
+		// Reset collision data for the next iteration
+		v.mCollidingShapeIndex = -1;
+		v.mLargestPenetration = -FLT_MAX;
+	}
+
+	// Calculate linear/angular velocity of the body by averaging all vertices and bringing the value to world space
+	float num_vertices_divider = float(max(int(mVertices.size()), 1));
+	SetLinearVelocity(body_transform.Multiply3x3(linear_velocity / num_vertices_divider));
+	SetAngularVelocity(body_transform.Multiply3x3(angular_velocity / num_vertices_divider));
+
+	if (mUpdatePosition)
+	{
+		// Shift the body so that the position is the center of the local bounds
+		Vec3 delta = mLocalBounds.GetCenter();
+		outDeltaPosition = body_transform.Multiply3x3(delta);
+		for (Vertex &v : mVertices)
+			v.mPosition -= delta;
+
+		// Offset bounds to match new position
+		mLocalBounds.Translate(-delta);
+		mLocalPredictedBounds.Translate(-delta);
+	}
+	else
+		outDeltaPosition = Vec3::sZero();
+
+	// Write back velocities
+	BodyInterface &body_interface = inSystem.GetBodyInterfaceNoLock();
+	for (const CollidingShape &cs : collector.mHits)
+		if (cs.mUpdateVelocities)
+			body_interface.SetLinearAndAngularVelocity(cs.mBodyID, body_transform.Multiply3x3(cs.mLinearVelocity), body_transform.Multiply3x3(cs.mAngularVelocity));
+}
+
+#ifdef JPH_DEBUG_RENDERER
+
+void SoftBodyMotionProperties::DrawVertices(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const
+{
+	for (const Vertex &v : mVertices)
+		inRenderer->DrawMarker(inCenterOfMassTransform * v.mPosition, Color::sRed, 0.05f);
+}
+
+void SoftBodyMotionProperties::DrawEdgeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const
+{
+	for (const Edge &e : mSettings->mEdgeConstraints)
+		inRenderer->DrawLine(inCenterOfMassTransform * mVertices[e.mVertex[0]].mPosition, inCenterOfMassTransform * mVertices[e.mVertex[1]].mPosition, Color::sWhite);
+}
+
+void SoftBodyMotionProperties::DrawVolumeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const
+{
+	for (const Volume &v : mSettings->mVolumeConstraints)
+	{
+		RVec3 x1 = inCenterOfMassTransform * mVertices[v.mVertex[0]].mPosition;
+		RVec3 x2 = inCenterOfMassTransform * mVertices[v.mVertex[1]].mPosition;
+		RVec3 x3 = inCenterOfMassTransform * mVertices[v.mVertex[2]].mPosition;
+		RVec3 x4 = inCenterOfMassTransform * mVertices[v.mVertex[3]].mPosition;
+
+		inRenderer->DrawTriangle(x1, x3, x2, Color::sYellow, DebugRenderer::ECastShadow::On);
+		inRenderer->DrawTriangle(x2, x3, x4, Color::sYellow, DebugRenderer::ECastShadow::On);
+		inRenderer->DrawTriangle(x1, x4, x3, Color::sYellow, DebugRenderer::ECastShadow::On);
+		inRenderer->DrawTriangle(x1, x2, x4, Color::sYellow, DebugRenderer::ECastShadow::On);
+	}
+}
+
+void SoftBodyMotionProperties::DrawPredictedBounds(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const
+{
+	inRenderer->DrawWireBox(inCenterOfMassTransform, mLocalPredictedBounds, Color::sRed);
+}
+
+#endif // JPH_DEBUG_RENDERER
+
+void SoftBodyMotionProperties::SaveState(StateRecorder &inStream) const
+{
+	MotionProperties::SaveState(inStream);
+
+	for (const Vertex &v : mVertices)
+	{
+		inStream.Write(v.mPreviousPosition);
+		inStream.Write(v.mPosition);
+		inStream.Write(v.mVelocity);
+	}
+
+	inStream.Write(mLocalBounds.mMin);
+	inStream.Write(mLocalBounds.mMax);
+	inStream.Write(mLocalPredictedBounds.mMin);
+	inStream.Write(mLocalPredictedBounds.mMax);
+}
+
+void SoftBodyMotionProperties::RestoreState(StateRecorder &inStream)
+{
+	MotionProperties::RestoreState(inStream);
+
+	for (Vertex &v : mVertices)
+	{
+		inStream.Read(v.mPreviousPosition);
+		inStream.Read(v.mPosition);
+		inStream.Read(v.mVelocity);
+	}
+
+	inStream.Read(mLocalBounds.mMin);
+	inStream.Read(mLocalBounds.mMax);
+	inStream.Read(mLocalPredictedBounds.mMin);
+	inStream.Read(mLocalPredictedBounds.mMax);
+}
+
+JPH_NAMESPACE_END

Jolt/Physics/SoftBody/SoftBodyMotionProperties.h

@@ -0,0 +1,104 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/AABox.h>
+#include <Jolt/Physics/Body/MotionProperties.h>
+#include <Jolt/Physics/SoftBody/SoftBodySharedSettings.h>
+#include <Jolt/Physics/SoftBody/SoftBodyVertex.h>
+
+JPH_NAMESPACE_BEGIN
+
+class PhysicsSystem;
+class Body;
+class SoftBodyCreationSettings;
+#ifdef JPH_DEBUG_RENDERER
+class DebugRenderer;
+#endif // JPH_DEBUG_RENDERER
+
+/// This class contains the runtime information of a soft body. Soft bodies are implemented using XPBD, a particle and springs based approach.
+class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties
+{
+public:
+	using Vertex = SoftBodyVertex;
+	using Edge = SoftBodySharedSettings::Edge;
+	using Face = SoftBodySharedSettings::Face;
+	using Volume = SoftBodySharedSettings::Volume;
+
+	/// Initialize the soft body motion properties
+	void								Initialize(const SoftBodyCreationSettings &inSettings);
+
+	/// Update the soft body
+	void								Update(float inDeltaTime, Body &inSoftBody, Vec3 &outDeltaPosition, PhysicsSystem &inSystem);
+
+	/// Get the shared settings of the soft body
+	const SoftBodySharedSettings *		GetSettings() const							{ return mSettings; }
+
+	/// Get the vertices of the soft body
+	const Array<Vertex> &				GetVertices() const							{ return mVertices; }
+	Array<Vertex> &						GetVertices()								{ return mVertices; }
+
+	/// Access an individual vertex
+	const Vertex &						GetVertex(uint inIndex) const				{ return mVertices[inIndex]; }
+	Vertex &							GetVertex(uint inIndex)						{ return mVertices[inIndex]; }
+
+	/// Get the materials of the soft body
+	const PhysicsMaterialList &			GetMaterials() const						{ return mSettings->mMaterials; }
+
+	/// Get the faces of the soft body
+	const Array<Face> &					GetFaces() const							{ return mSettings->mFaces; }
+
+	/// Access to an individual face
+	const Face &						GetFace(uint inIndex) const					{ return mSettings->mFaces[inIndex]; }
+
+	/// Get the number of solver iterations
+	uint32								GetNumIterations() const					{ return mNumIterations; }
+	void								SetNumIterations(uint32 inNumIterations)	{ mNumIterations = inNumIterations; }
+
+	/// Get the pressure of the soft body
+	float								GetPressure() const							{ return mPressure; }
+	void								SetPressure(float inPressure)				{ mPressure = inPressure; }
+
+	/// Update the position of the body while simulating (set to false for something that is attached to the static world)
+	bool								GetUpdatePosition() const					{ return mUpdatePosition; }
+	void								SetUpdatePosition(bool inUpdatePosition)	{ mUpdatePosition = inUpdatePosition; }
+
+	/// Get local bounding box
+	const AABox &						GetLocalBounds() const						{ return mLocalBounds; }
+
+	/// Get the volume of the soft body. Note can become negative if the shape is inside out!
+	float								GetVolume() const							{ return GetVolumeTimesSix() / 6.0f; }
+
+	/// Calculate the total mass and inertia of this body based on the current state of the vertices
+	void								CalculateMassAndInertia();
+
+#ifdef JPH_DEBUG_RENDERER
+	/// Draw the state of a soft body
+	void								DrawVertices(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const;
+	void								DrawEdgeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const;
+	void								DrawVolumeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const;
+	void								DrawPredictedBounds(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const;
+#endif // JPH_DEBUG_RENDERER
+
+	/// Saving state for replay
+	void								SaveState(StateRecorder &inStream) const;
+
+	/// Restoring state for replay
+	void								RestoreState(StateRecorder &inStream);
+
+private:
+	/// Returns 6 times the volume of the soft body
+	float								GetVolumeTimesSix() const;
+
+	RefConst<SoftBodySharedSettings>	mSettings;									///< Configuration of the particles and constraints
+	Array<Vertex>						mVertices;									///< Current state of all vertices in the simulation
+	AABox								mLocalBounds;								///< Bounding box of all vertices
+	AABox								mLocalPredictedBounds;						///< Predicted bounding box for all vertices using extrapolation of velocity by last step delta time
+	uint32								mNumIterations;								///< Number of solver iterations
+	float								mPressure;									///< n * R * T, amount of substance * ideal gass constant * absolute temperature, see https://en.wikipedia.org/wiki/Pressure
+	bool								mUpdatePosition;							///< Update the position of the body while simulating (set to false for something that is attached to the static world)
+};
+
+JPH_NAMESPACE_END

Jolt/Physics/SoftBody/SoftBodyShape.cpp

@@ -0,0 +1,330 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/SoftBody/SoftBodyShape.h>
+#include <Jolt/Core/Profiler.h>
+#include <Jolt/Geometry/RayTriangle.h>
+#include <Jolt/Physics/Collision/RayCast.h>
+#include <Jolt/Physics/Collision/CastResult.h>
+#include <Jolt/Physics/Collision/TransformedShape.h>
+#include <Jolt/Physics/SoftBody/SoftBodyMotionProperties.h>
+#include <Jolt/Physics/Collision/CastConvexVsTriangles.h>
+#include <Jolt/Physics/Collision/CastSphereVsTriangles.h>
+#include <Jolt/Physics/Collision/CollideConvexVsTriangles.h>
+#include <Jolt/Physics/Collision/CollideSphereVsTriangles.h>
+#include <Jolt/Physics/Collision/CollisionDispatch.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+uint SoftBodyShape::GetSubShapeIDBits() const
+{
+	// Ensure we have enough bits to encode our shape [0, n - 1]
+	uint32 n = (uint32)mSoftBodyMotionProperties->GetFaces().size() - 1;
+	return 32 - CountLeadingZeros(n);
+}
+
+AABox SoftBodyShape::GetLocalBounds() const
+{
+	return mSoftBodyMotionProperties->GetLocalBounds();
+}
+
+bool SoftBodyShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	uint num_triangle_bits = GetSubShapeIDBits();
+	uint triangle_idx = uint(-1);
+
+	const Array<SoftBodyVertex> &vertices = mSoftBodyMotionProperties->GetVertices();
+	for (const SoftBodyMotionProperties::Face &f : mSoftBodyMotionProperties->GetFaces())
+	{
+		Vec3 x1 = vertices[f.mVertex[0]].mPosition;
+		Vec3 x2 = vertices[f.mVertex[1]].mPosition;
+		Vec3 x3 = vertices[f.mVertex[2]].mPosition;
+
+		float fraction = RayTriangle(inRay.mOrigin, inRay.mDirection, x1, x2, x3);
+		if (fraction < ioHit.mFraction)
+		{
+			// Store fraction
+			ioHit.mFraction = fraction;
+
+			// Store triangle index
+			triangle_idx = uint(&f - mSoftBodyMotionProperties->GetFaces().data());
+		}
+	}
+
+	if (triangle_idx == uint(-1))
+		return false;
+
+	ioHit.mSubShapeID2 = inSubShapeIDCreator.PushID(triangle_idx, num_triangle_bits).GetID();
+	return true;
+}
+
+void SoftBodyShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	JPH_PROFILE_FUNCTION();
+
+	// Test shape filter
+	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
+		return;
+
+	uint num_triangle_bits = GetSubShapeIDBits();
+
+	const Array<SoftBodyVertex> &vertices = mSoftBodyMotionProperties->GetVertices();
+	for (const SoftBodyMotionProperties::Face &f : mSoftBodyMotionProperties->GetFaces())
+	{
+		Vec3 x1 = vertices[f.mVertex[0]].mPosition;
+		Vec3 x2 = vertices[f.mVertex[1]].mPosition;
+		Vec3 x3 = vertices[f.mVertex[2]].mPosition;
+
+		// Back facing check
+		if (inRayCastSettings.mBackFaceMode == EBackFaceMode::IgnoreBackFaces && (x2 - x1).Cross(x3 - x1).Dot(inRay.mDirection) > 0.0f)
+			return;
+
+		// Test ray against triangle
+		float fraction = RayTriangle(inRay.mOrigin, inRay.mDirection, x1, x2, x3);
+		if (fraction < ioCollector.GetEarlyOutFraction())
+		{
+			// Better hit than the current hit
+			RayCastResult hit;
+			hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext());
+			hit.mFraction = fraction;
+			hit.mSubShapeID2 = inSubShapeIDCreator.PushID(uint(&f - mSoftBodyMotionProperties->GetFaces().data()), num_triangle_bits).GetID();
+			ioCollector.AddHit(hit);
+		}
+	}
+}
+
+void SoftBodyShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+{
+	sCollidePointUsingRayCast(*this, inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter);
+}
+
+void SoftBodyShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const
+{
+	JPH_ASSERT(false, "Should not be called");
+}
+
+const PhysicsMaterial *SoftBodyShape::GetMaterial(const SubShapeID &inSubShapeID) const
+{
+	SubShapeID remainder;
+	uint triangle_idx = inSubShapeID.PopID(GetSubShapeIDBits(), remainder);
+	JPH_ASSERT(remainder.IsEmpty());
+
+	const SoftBodyMotionProperties::Face &f = mSoftBodyMotionProperties->GetFace(triangle_idx);
+	return mSoftBodyMotionProperties->GetMaterials()[f.mMaterialIndex];
+}
+
+Vec3 SoftBodyShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
+{
+	SubShapeID remainder;
+	uint triangle_idx = inSubShapeID.PopID(GetSubShapeIDBits(), remainder);
+	JPH_ASSERT(remainder.IsEmpty());
+
+	const SoftBodyMotionProperties::Face &f = mSoftBodyMotionProperties->GetFace(triangle_idx);
+	const Array<SoftBodyVertex> &vertices = mSoftBodyMotionProperties->GetVertices();
+
+	Vec3 x1 = vertices[f.mVertex[0]].mPosition;
+	Vec3 x2 = vertices[f.mVertex[1]].mPosition;
+	Vec3 x3 = vertices[f.mVertex[2]].mPosition;
+
+	return (x2 - x1).Cross(x3 - x1).NormalizedOr(Vec3::sAxisY());
+}
+
+void SoftBodyShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
+{
+	SubShapeID remainder;
+	uint triangle_idx = inSubShapeID.PopID(GetSubShapeIDBits(), remainder);
+	JPH_ASSERT(remainder.IsEmpty());
+
+	const SoftBodyMotionProperties::Face &f = mSoftBodyMotionProperties->GetFace(triangle_idx);
+	const Array<SoftBodyVertex> &vertices = mSoftBodyMotionProperties->GetVertices();
+
+	for (uint32 i : f.mVertex)
+		outVertices.push_back(inCenterOfMassTransform * (inScale * vertices[i].mPosition));
+}
+
+void SoftBodyShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const
+{
+	outSubmergedVolume = 0.0f;
+	outTotalVolume = mSoftBodyMotionProperties->GetVolume();
+	outCenterOfBuoyancy = Vec3::sZero();
+}
+
+#ifdef JPH_DEBUG_RENDERER
+
+void SoftBodyShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+{
+	const Array<SoftBodyVertex> &vertices = mSoftBodyMotionProperties->GetVertices();
+	for (const SoftBodyMotionProperties::Face &f : mSoftBodyMotionProperties->GetFaces())
+	{
+		RVec3 x1 = inCenterOfMassTransform * vertices[f.mVertex[0]].mPosition;
+		RVec3 x2 = inCenterOfMassTransform * vertices[f.mVertex[1]].mPosition;
+		RVec3 x3 = inCenterOfMassTransform * vertices[f.mVertex[2]].mPosition;
+
+		inRenderer->DrawTriangle(x1, x2, x3, inColor, DebugRenderer::ECastShadow::On);
+	}
+}
+
+#endif // JPH_DEBUG_RENDERER
+
+struct SoftBodyShape::SBSGetTrianglesContext
+{
+	Mat44		mCenterOfMassTransform;
+	int			mTriangleIndex;
+};
+
+void SoftBodyShape::GetTrianglesStart(GetTrianglesContext &ioContext, [[maybe_unused]] const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const
+{
+	SBSGetTrianglesContext &context = reinterpret_cast<SBSGetTrianglesContext &>(ioContext);
+	context.mCenterOfMassTransform = Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale);
+	context.mTriangleIndex = 0;
+}
+
+int SoftBodyShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const
+{
+	SBSGetTrianglesContext &context = reinterpret_cast<SBSGetTrianglesContext &>(ioContext);
+
+	const Array<SoftBodyMotionProperties::Face> &faces = mSoftBodyMotionProperties->GetFaces();
+	const Array<SoftBodyVertex> &vertices = mSoftBodyMotionProperties->GetVertices();
+	const PhysicsMaterialList &materials = mSoftBodyMotionProperties->GetMaterials();
+
+	int num_triangles = min(inMaxTrianglesRequested, (int)faces.size() - context.mTriangleIndex);
+	for (int i = 0; i < num_triangles; ++i)
+	{
+		const SoftBodyMotionProperties::Face &f = faces[context.mTriangleIndex + i];
+
+		Vec3 x1 = context.mCenterOfMassTransform * vertices[f.mVertex[0]].mPosition;
+		Vec3 x2 = context.mCenterOfMassTransform * vertices[f.mVertex[1]].mPosition;
+		Vec3 x3 = context.mCenterOfMassTransform * vertices[f.mVertex[2]].mPosition;
+
+		x1.StoreFloat3(outTriangleVertices++);
+		x2.StoreFloat3(outTriangleVertices++);
+		x3.StoreFloat3(outTriangleVertices++);
+
+		if (outMaterials != nullptr)
+			*outMaterials++ = materials[f.mMaterialIndex];
+	}
+
+	context.mTriangleIndex += num_triangles;
+	return num_triangles;
+}
+
+Shape::Stats SoftBodyShape::GetStats() const
+{
+	return Stats(sizeof(this), (uint)mSoftBodyMotionProperties->GetFaces().size());
+}
+
+float SoftBodyShape::GetVolume() const
+{
+	return mSoftBodyMotionProperties->GetVolume();
+}
+
+void SoftBodyShape::sCollideConvexVsSoftBody(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter)
+{
+	JPH_ASSERT(inShape1->GetType() == EShapeType::Convex);
+	const ConvexShape *shape1 = static_cast<const ConvexShape *>(inShape1);
+	JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::SoftBody);
+	const SoftBodyShape *shape2 = static_cast<const SoftBodyShape *>(inShape2);
+
+	const Array<SoftBodyVertex> &vertices = shape2->mSoftBodyMotionProperties->GetVertices();
+	const Array<SoftBodyMotionProperties::Face> &faces = shape2->mSoftBodyMotionProperties->GetFaces();
+	uint num_triangle_bits = shape2->GetSubShapeIDBits();
+
+	CollideConvexVsTriangles collider(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector);
+	for (const SoftBodyMotionProperties::Face &f : faces)
+	{
+		Vec3 x1 = vertices[f.mVertex[0]].mPosition;
+		Vec3 x2 = vertices[f.mVertex[1]].mPosition;
+		Vec3 x3 = vertices[f.mVertex[2]].mPosition;
+
+		collider.Collide(x1, x2, x3, 0b111, inSubShapeIDCreator2.PushID(uint(&f - faces.data()), num_triangle_bits).GetID());
+	}
+}
+
+void SoftBodyShape::sCollideSphereVsSoftBody(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter)
+{
+	JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::Sphere);
+	const SphereShape *shape1 = static_cast<const SphereShape *>(inShape1);
+	JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::SoftBody);
+	const SoftBodyShape *shape2 = static_cast<const SoftBodyShape *>(inShape2);
+
+	const Array<SoftBodyVertex> &vertices = shape2->mSoftBodyMotionProperties->GetVertices();
+	const Array<SoftBodyMotionProperties::Face> &faces = shape2->mSoftBodyMotionProperties->GetFaces();
+	uint num_triangle_bits = shape2->GetSubShapeIDBits();
+
+	CollideSphereVsTriangles collider(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector);
+	for (const SoftBodyMotionProperties::Face &f : faces)
+	{
+		Vec3 x1 = vertices[f.mVertex[0]].mPosition;
+		Vec3 x2 = vertices[f.mVertex[1]].mPosition;
+		Vec3 x3 = vertices[f.mVertex[2]].mPosition;
+
+		collider.Collide(x1, x2, x3, 0b111, inSubShapeIDCreator2.PushID(uint(&f - faces.data()), num_triangle_bits).GetID());
+	}
+}
+
+void SoftBodyShape::sCastConvexVsSoftBody(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::SoftBody);
+	const SoftBodyShape *shape = static_cast<const SoftBodyShape *>(inShape);
+
+	const Array<SoftBodyVertex> &vertices = shape->mSoftBodyMotionProperties->GetVertices();
+	const Array<SoftBodyMotionProperties::Face> &faces = shape->mSoftBodyMotionProperties->GetFaces();
+	uint num_triangle_bits = shape->GetSubShapeIDBits();
+
+	CastConvexVsTriangles caster(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector);
+	for (const SoftBodyMotionProperties::Face &f : faces)
+	{
+		Vec3 x1 = vertices[f.mVertex[0]].mPosition;
+		Vec3 x2 = vertices[f.mVertex[1]].mPosition;
+		Vec3 x3 = vertices[f.mVertex[2]].mPosition;
+
+		caster.Cast(x1, x2, x3, 0b111, inSubShapeIDCreator2.PushID(uint(&f - faces.data()), num_triangle_bits).GetID());
+	}
+}
+
+void SoftBodyShape::sCastSphereVsSoftBody(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
+{
+	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::SoftBody);
+	const SoftBodyShape *shape = static_cast<const SoftBodyShape *>(inShape);
+
+	const Array<SoftBodyVertex> &vertices = shape->mSoftBodyMotionProperties->GetVertices();
+	const Array<SoftBodyMotionProperties::Face> &faces = shape->mSoftBodyMotionProperties->GetFaces();
+	uint num_triangle_bits = shape->GetSubShapeIDBits();
+
+	CastSphereVsTriangles caster(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector);
+	for (const SoftBodyMotionProperties::Face &f : faces)
+	{
+		Vec3 x1 = vertices[f.mVertex[0]].mPosition;
+		Vec3 x2 = vertices[f.mVertex[1]].mPosition;
+		Vec3 x3 = vertices[f.mVertex[2]].mPosition;
+
+		caster.Cast(x1, x2, x3, 0b111, inSubShapeIDCreator2.PushID(uint(&f - faces.data()), num_triangle_bits).GetID());
+	}
+}
+
+void SoftBodyShape::sRegister()
+{
+	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::SoftBody);
+	f.mConstruct = nullptr; // Not supposed to be constructed by users!
+	f.mColor = Color::sDarkGreen;
+
+	for (EShapeSubType s : sConvexSubShapeTypes)
+	{
+		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::SoftBody, sCollideConvexVsSoftBody);
+		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::SoftBody, sCastConvexVsSoftBody);
+	}
+
+	// Specialized collision functions
+	CollisionDispatch::sRegisterCollideShape(EShapeSubType::Sphere, EShapeSubType::SoftBody, sCollideSphereVsSoftBody);
+	CollisionDispatch::sRegisterCastShape(EShapeSubType::Sphere, EShapeSubType::SoftBody, sCastSphereVsSoftBody);
+}
+
+JPH_NAMESPACE_END

Jolt/Physics/SoftBody/SoftBodyShape.h

@@ -0,0 +1,70 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/Collision/Shape/Shape.h>
+
+JPH_NAMESPACE_BEGIN
+
+class SoftBodyMotionProperties;
+class CollideShapeSettings;
+
+/// Shape used exclusively for soft bodies. Adds the ability to perform collision checks against soft bodies.
+class JPH_EXPORT SoftBodyShape final : public Shape
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	/// Constructor
+									SoftBodyShape()											: Shape(EShapeType::SoftBody, EShapeSubType::SoftBody) { }
+
+	/// Determine amount of bits needed to encode sub shape id
+	uint							GetSubShapeIDBits() const;
+
+	// See Shape
+	virtual bool					MustBeStatic() const override							{ return false; }
+	virtual Vec3					GetCenterOfMass() const override						{ return Vec3::sZero(); }
+	virtual AABox					GetLocalBounds() const override;
+	virtual uint					GetSubShapeIDBitsRecursive() const override				{ return GetSubShapeIDBits(); }
+	virtual float					GetInnerRadius() const override							{ return 0.0f; }
+	virtual MassProperties			GetMassProperties() const override						{ return MassProperties(); }
+	virtual const PhysicsMaterial *	GetMaterial(const SubShapeID &inSubShapeID) const override;
+	virtual Vec3					GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override;
+	virtual void					GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
+	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy
+#ifdef JPH_DEBUG_RENDERER // Not using JPH_IF_DEBUG_RENDERER for Doxygen
+		, RVec3Arg inBaseOffset
+#endif
+		) const override;
+#ifdef JPH_DEBUG_RENDERER
+	virtual void					Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+#endif // JPH_DEBUG_RENDERER
+	virtual bool					CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override;
+	virtual void					CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+	virtual void					CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override;
+	virtual void					CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Array<SoftBodyVertex> &ioVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override;
+	virtual void					GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override;
+	virtual int						GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override;
+	virtual Stats					GetStats() const override;
+	virtual float					GetVolume() const override;
+
+	// Register shape functions with the registry
+	static void						sRegister();
+
+private:
+	// Helper functions called by CollisionDispatch
+	static void						sCollideConvexVsSoftBody(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCollideSphereVsSoftBody(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter);
+	static void						sCastConvexVsSoftBody(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+	static void						sCastSphereVsSoftBody(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector);
+
+	struct SBSGetTrianglesContext;
+
+	friend class BodyManager;
+
+	const SoftBodyMotionProperties *mSoftBodyMotionProperties;
+};
+
+JPH_NAMESPACE_END

Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp

@@ -0,0 +1,92 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Physics/SoftBody/SoftBodySharedSettings.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Vertex)
+{
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Vertex, mPosition)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Vertex, mVelocity)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Vertex, mInvMass)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Face)
+{
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Face, mVertex)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Face, mMaterialIndex)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Edge)
+{
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Edge, mVertex)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Edge, mRestLength)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Edge, mCompliance)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Volume)
+{
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Volume, mVertex)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Volume, mSixRestVolume)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Volume, mCompliance)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings)
+{
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mVertices)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mFaces)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mEdgeConstraints)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mVolumeConstraints)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mMaterials)
+}
+
+void SoftBodySharedSettings::CalculateEdgeLengths()
+{
+	for (Edge &e : mEdgeConstraints)
+	{
+		e.mRestLength = (Vec3(mVertices[e.mVertex[1]].mPosition) - Vec3(mVertices[e.mVertex[0]].mPosition)).Length();
+		JPH_ASSERT(e.mRestLength > 0.0f);
+	}
+}
+
+void SoftBodySharedSettings::CalculateVolumeConstraintVolumes()
+{
+	for (Volume &v : mVolumeConstraints)
+	{
+		Vec3 x1(mVertices[v.mVertex[0]].mPosition);
+		Vec3 x2(mVertices[v.mVertex[1]].mPosition);
+		Vec3 x3(mVertices[v.mVertex[2]].mPosition);
+		Vec3 x4(mVertices[v.mVertex[3]].mPosition);
+
+		Vec3 x1x2 = x2 - x1;
+		Vec3 x1x3 = x3 - x1;
+		Vec3 x1x4 = x4 - x1;
+
+		v.mSixRestVolume = abs(x1x2.Cross(x1x3).Dot(x1x4));
+	}
+}
+
+void SoftBodySharedSettings::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write(mVertices);
+	inStream.Write(mFaces);
+	inStream.Write(mEdgeConstraints);
+	inStream.Write(mVolumeConstraints);
+}
+
+void SoftBodySharedSettings::RestoreBinaryState(StreamIn &inStream)
+{
+	inStream.Read(mVertices);
+	inStream.Read(mFaces);
+	inStream.Read(mEdgeConstraints);
+	inStream.Read(mVolumeConstraints);
+}
+
+JPH_NAMESPACE_END

Jolt/Physics/SoftBody/SoftBodySharedSettings.h

@@ -0,0 +1,83 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/Reference.h>
+#include <Jolt/Physics/Collision/PhysicsMaterial.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// This class defines the setup of all particles and their constraints.
+/// It is used during the simulation and can be shared between multiple soft bodies.
+class JPH_EXPORT SoftBodySharedSettings : public RefTarget<SoftBodySharedSettings>
+{
+public:
+	JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SoftBodySharedSettings)
+
+	/// Calculate the initial lengths of all springs of the edges of this soft body
+	void				CalculateEdgeLengths();
+
+	/// Calculates the initial volume of all tetrahedra of this soft body
+	void				CalculateVolumeConstraintVolumes();
+
+	/// Saves the state of this object in binary form to inStream. Doesn't store the material list.
+	void				SaveBinaryState(StreamOut &inStream) const;
+
+	/// Restore the state of this object from inStream. Doesn't restore the material list.
+	void				RestoreBinaryState(StreamIn &inStream);
+
+	/// A vertex is a particle, the data in this structure is only used during creation of the soft body and not during simulation
+	struct JPH_EXPORT Vertex
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Vertex)
+
+		Float3			mPosition { 0, 0, 0 };						///< Initial position of the vertex
+		Float3			mVelocity { 0, 0, 0 };						///< Initial velocity of the vertex
+		float			mInvMass = 1.0f;							///< Initial inverse of the mass of the vertex
+	};
+
+	/// A face defines the surface of the body
+	struct JPH_EXPORT Face
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Face)
+
+		/// Check if this is a degenerate face (a face which points to the same vertex twice)
+		bool			IsDegenerate() const						{ return mVertex[0] == mVertex[1] || mVertex[0] == mVertex[2] || mVertex[1] == mVertex[2]; }
+
+		uint32			mVertex[3];									///< Indices of the vertices that form the face
+		uint32			mMaterialIndex = 0;							///< Index of the material of the face in SoftBodySharedSettings::mMaterials
+	};
+
+	/// An edge keeps two vertices at a constant distance using a spring: |x1 - x2| = rest length
+	struct JPH_EXPORT Edge
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Edge)
+
+		uint32			mVertex[2];									///< Indices of the vertices that form the edge
+		float			mRestLength = 1.0f;							///< Rest length of the spring
+		float			mCompliance = 0.0f;							///< Inverse of the stiffness of the spring
+	};
+
+	/// Volume constraint, keeps the volume of a tetrahedron constant
+	struct JPH_EXPORT Volume
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Volume)
+
+		uint32			mVertex[4];									///< Indices of the vertices that form the tetrhedron
+		float			mSixRestVolume = 1.0f;						///< 6 times the rest volume of the tetrahedron
+		float			mCompliance = 0.0f;							///< Inverse of the stiffness of the constraint
+	};
+
+	/// Add a face to this soft body
+	void				AddFace(const Face &inFace)					{ JPH_ASSERT(!inFace.IsDegenerate()); mFaces.push_back(inFace); }
+
+	Array<Vertex>		mVertices;									///< The list of vertices or particles of the body
+	Array<Face>			mFaces;										///< The list of faces of the body
+	Array<Edge>			mEdgeConstraints;							///< The list of edges or springs of the body
+	Array<Volume>		mVolumeConstraints;							///< The list of volume constraints of the body that keep the volume of tetrahedra in the soft body constant
+	PhysicsMaterialList mMaterials { PhysicsMaterial::sDefault };	///< The materials of the faces of the body, referenced by Face::mMaterialIndex
+};
+
+JPH_NAMESPACE_END

Jolt/Physics/SoftBody/SoftBodyVertex.h

@@ -0,0 +1,28 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Geometry/Plane.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Run time information for a single particle of a soft body
+/// Note that at run-time you should only modify the inverse mass and/or velocity of a vertex to control the soft body.
+/// Modifying the position can lead to missed collisions.
+/// The other members are used internally by the soft body solver.
+class SoftBodyVertex
+{
+public:
+	Vec3			mPreviousPosition;					///< Position at the previous time step
+	Vec3 			mPosition;							///< Position, relative to the center of mass of the soft body
+	Vec3 			mVelocity;							///< Velocity, relative to the center of mass of the soft body
+	Plane			mCollisionPlane;					///< Nearest collision plane, relative to the center of mass of the soft body
+	int				mCollidingShapeIndex;				///< Index in the colliding shapes list of the body we may collide with
+	float			mLargestPenetration;				///< Used while finding the collision plane, stores the largest penetration found so far
+	float			mInvMass;							///< Inverse mass (1 / mass)
+	float			mProjectedDistance;					///< Distance along the normal of the collision plane along which the particle was moved to resolve the collision
+};
+
+JPH_NAMESPACE_END

Jolt/RegisterTypes.cpp

@@ -23,6 +23,7 @@
 #include <Jolt/Physics/Collision/Shape/MutableCompoundShape.h>
 #include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
 #include <Jolt/Physics/Collision/PhysicsMaterialSimple.h>
+#include <Jolt/Physics/SoftBody/SoftBodyShape.h>
 
 JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, Skeleton)
 JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, SkeletalAnimation)
@@ -63,6 +64,8 @@ JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PhysicsScene)
 JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PhysicsMaterial)
 JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, GroupFilter)
 JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, GroupFilterTable)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, BodyCreationSettings)
+JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, SoftBodyCreationSettings)
 
 JPH_NAMESPACE_BEGIN
 
@@ -102,6 +105,7 @@ void RegisterTypesInternal(uint64 inVersionID)
 	MeshShape::sRegister();
 	ConvexHullShape::sRegister();
 	HeightFieldShape::sRegister();
+	SoftBodyShape::sRegister();
 
 	// Register these last because their collision functions are simple so we want to execute them first (register them in reverse order of collision complexity)
 	RotatedTranslatedShape::sRegister();
@@ -149,7 +153,9 @@ void RegisterTypesInternal(uint64 inVersionID)
 		JPH_RTTI(PhysicsMaterial),
 		JPH_RTTI(PhysicsMaterialSimple),
 		JPH_RTTI(GroupFilter),
-		JPH_RTTI(GroupFilterTable)
+		JPH_RTTI(GroupFilterTable),
+		JPH_RTTI(BodyCreationSettings),
+		JPH_RTTI(SoftBodyCreationSettings)
 	};
 
 	// Register them all

README.md

@@ -76,7 +76,11 @@ For more information see the [Architecture and API documentation](https://jrouwe
 * Game character simulation (capsule)
 	* Rigid body character. Moves during the physics simulation. Cheapest option and most accurate collision response between character and dynamic bodies.
 	* Virtual character. Does not have a rigid body in the world but simulates one using collision checks. Updated outside of the physics update for more control. Less accurate interaction with dynamic bodies.
-* Vehicle simulation of wheeled and tracked vehicles.
+* Vehicles
+	* Wheeled vehicles.
+	* Tracked vehicles.
+	* Motorcycles.
+* Soft body simulation (e.g. a soft ball or piece of cloth).
 * Water buoyancy calculations.
 * An optional double precision mode that allows large worlds.
 

Samples/Samples.cmake

@@ -161,6 +161,20 @@ set(SAMPLES_SRC_FILES
 	${SAMPLES_ROOT}/Tests/Rig/RigPileTest.h
 	${SAMPLES_ROOT}/Tests/Rig/SkeletonMapperTest.cpp
 	${SAMPLES_ROOT}/Tests/Rig/SkeletonMapperTest.h
+	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyFrictionTest.cpp
+	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyFrictionTest.h
+	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyGravityFactorTest.cpp
+	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyGravityFactorTest.h
+	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyKinematicTest.cpp
+	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyKinematicTest.h
+	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyPressureTest.cpp
+	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyPressureTest.h
+	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyRestitutionTest.cpp
+	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyRestitutionTest.h
+	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyShapesTest.cpp
+	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyShapesTest.h
+	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyUpdatePositionTest.cpp
+	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyUpdatePositionTest.h
 	${SAMPLES_ROOT}/Tests/Test.cpp
 	${SAMPLES_ROOT}/Tests/Test.h
 	${SAMPLES_ROOT}/Tests/Tools/LoadSnapshotTest.cpp
@@ -234,6 +248,8 @@ set(SAMPLES_SRC_FILES
 	${SAMPLES_ROOT}/Utils/RagdollLoader.h
 	${SAMPLES_ROOT}/Utils/ShapeCreator.cpp
 	${SAMPLES_ROOT}/Utils/ShapeCreator.h
+	${SAMPLES_ROOT}/Utils/SoftBodyCreator.cpp
+	${SAMPLES_ROOT}/Utils/SoftBodyCreator.h
 )
 
 # Group source files

Samples/SamplesApp.cpp

@@ -14,6 +14,7 @@
 #include <Jolt/Physics/PhysicsSystem.h>
 #include <Jolt/Physics/StateRecorderImpl.h>
 #include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Physics/SoftBody/SoftBodyMotionProperties.h>
 #include <Jolt/Physics/PhysicsScene.h>
 #include <Jolt/Physics/Collision/RayCast.h>
 #include <Jolt/Physics/Collision/ShapeCast.h>
@@ -295,6 +296,25 @@ static TestNameAndRTTI sVehicleTests[] =
 	{ "Car (SixDOFConstraint)",				JPH_RTTI(VehicleSixDOFTest) },
 };
 
+JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SoftBodyShapesTest)
+JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SoftBodyFrictionTest)
+JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SoftBodyRestitutionTest)
+JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SoftBodyPressureTest)
+JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SoftBodyGravityFactorTest)
+JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SoftBodyKinematicTest)
+JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SoftBodyUpdatePositionTest)
+
+static TestNameAndRTTI sSoftBodyTests[] =
+{
+	{ "Soft Body vs Shapes",			JPH_RTTI(SoftBodyShapesTest) },
+	{ "Soft Body Friction",				JPH_RTTI(SoftBodyFrictionTest) },
+	{ "Soft Body Restitution",			JPH_RTTI(SoftBodyRestitutionTest) },
+	{ "Soft Body Pressure",				JPH_RTTI(SoftBodyPressureTest) },
+	{ "Soft Body Gravity Factor",		JPH_RTTI(SoftBodyGravityFactorTest) },
+	{ "Soft Body Kinematic",			JPH_RTTI(SoftBodyKinematicTest) },
+	{ "Soft Body Update Position",		JPH_RTTI(SoftBodyUpdatePositionTest) },
+};
+
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, BroadPhaseCastRayTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, BroadPhaseInsertionTest)
 
@@ -340,6 +360,7 @@ static TestCategory sAllCategories[] =
 	{ "Character", sCharacterTests, size(sCharacterTests) },
 	{ "Water", sWaterTests, size(sWaterTests) },
 	{ "Vehicle", sVehicleTests, size(sVehicleTests) },
+	{ "Soft Body", sSoftBodyTests, size(sSoftBodyTests) },
 	{ "Broad Phase", sBroadPhaseTests, size(sBroadPhaseTests) },
 	{ "Convex Collision", sConvexCollisionTests, size(sConvexCollisionTests) },
 	{ "Tools", sTools, size(sTools) }
@@ -463,6 +484,10 @@ SamplesApp::SamplesApp()
 			mDebugUI->CreateCheckBox(drawing_options, "Draw Character Virtual Constraints", CharacterVirtual::sDrawConstraints, [](UICheckBox::EState inState) { CharacterVirtual::sDrawConstraints = inState == UICheckBox::STATE_CHECKED; });
 			mDebugUI->CreateCheckBox(drawing_options, "Draw Character Virtual Walk Stairs", CharacterVirtual::sDrawWalkStairs, [](UICheckBox::EState inState) { CharacterVirtual::sDrawWalkStairs = inState == UICheckBox::STATE_CHECKED; });
 			mDebugUI->CreateCheckBox(drawing_options, "Draw Character Virtual Stick To Floor", CharacterVirtual::sDrawStickToFloor, [](UICheckBox::EState inState) { CharacterVirtual::sDrawStickToFloor = inState == UICheckBox::STATE_CHECKED; });
+			mDebugUI->CreateCheckBox(drawing_options, "Draw Soft Body Vertices", mBodyDrawSettings.mDrawSoftBodyVertices, [this](UICheckBox::EState inState) { mBodyDrawSettings.mDrawSoftBodyVertices = inState == UICheckBox::STATE_CHECKED; });
+			mDebugUI->CreateCheckBox(drawing_options, "Draw Soft Body Edge Constraints", mBodyDrawSettings.mDrawSoftBodyEdgeConstraints, [this](UICheckBox::EState inState) { mBodyDrawSettings.mDrawSoftBodyEdgeConstraints = inState == UICheckBox::STATE_CHECKED; });
+			mDebugUI->CreateCheckBox(drawing_options, "Draw Soft Body Volume Constraints", mBodyDrawSettings.mDrawSoftBodyVolumeConstraints, [this](UICheckBox::EState inState) { mBodyDrawSettings.mDrawSoftBodyVolumeConstraints = inState == UICheckBox::STATE_CHECKED; });
+			mDebugUI->CreateCheckBox(drawing_options, "Draw Soft Body Predicted Bounds", mBodyDrawSettings.mDrawSoftBodyPredictedBounds, [this](UICheckBox::EState inState) { mBodyDrawSettings.mDrawSoftBodyPredictedBounds = inState == UICheckBox::STATE_CHECKED; });
 			mDebugUI->ShowMenu(drawing_options);
 		});
 	#endif // JPH_DEBUG_RENDERER
@@ -593,7 +618,11 @@ void SamplesApp::StartTest(const RTTI *inRTTI)
 
 	// Reset dragging
 	mDragAnchor = nullptr;
+	mDragBody = BodyID();
 	mDragConstraint = nullptr;
+	mDragVertexIndex = ~uint(0);
+	mDragVertexPreviousInvMass = 0.0f;
+	mDragFraction = 0.0f;
 
 	// Reset playback state
 	mPlaybackFrames.clear();
@@ -1655,7 +1684,7 @@ bool SamplesApp::CastProbe(float inProbeLength, float &outFraction, RVec3 &outPo
 	return had_hit;
 }
 
-void SamplesApp::UpdateDebug()
+void SamplesApp::UpdateDebug(float inDeltaTime)
 {
 	JPH_PROFILE_FUNCTION();
 
@@ -1672,13 +1701,12 @@ void SamplesApp::UpdateDebug()
 			break;
 		}
 
-	// Allow the user to drag rigid bodies around
-	if (mDragConstraint == nullptr)
+	// Allow the user to drag rigid/soft bodies around
+	if (mDragConstraint == nullptr && mDragVertexIndex == ~uint(0))
 	{
 		// Not dragging yet
 		RVec3 hit_position;
-		float hit_fraction;
-		if (CastProbe(cDragRayLength, hit_fraction, hit_position, mDragBody))
+		if (CastProbe(cDragRayLength, mDragFraction, hit_position, mDragBody))
 		{
 			// If key is pressed create constraint to start dragging
 			if (mKeyboard->IsKeyPressed(DIK_SPACE))
@@ -1688,7 +1716,29 @@ void SamplesApp::UpdateDebug()
 				if (lock.Succeeded())
 				{
 					Body &drag_body = lock.GetBody();
-					if (drag_body.IsDynamic())
+					if (drag_body.IsSoftBody())
+					{
+						SoftBodyMotionProperties *mp = static_cast<SoftBodyMotionProperties *>(drag_body.GetMotionProperties());
+
+						// Find closest vertex
+						Vec3 local_hit_position = Vec3(drag_body.GetInverseCenterOfMassTransform() * hit_position);
+						float closest_dist_sq = FLT_MAX;
+						for (SoftBodyVertex &v : mp->GetVertices())
+						{
+							float dist_sq = (v.mPosition - local_hit_position).LengthSq();
+							if (dist_sq < closest_dist_sq)
+							{
+								closest_dist_sq = dist_sq;
+								mDragVertexIndex = uint(&v - mp->GetVertices().data());
+							}
+						}
+
+						// Make the vertex kinematic
+						SoftBodyVertex &v = mp->GetVertex(mDragVertexIndex);
+						mDragVertexPreviousInvMass = v.mInvMass;
+						v.mInvMass = 0.0f;
+					}
+					else if (drag_body.IsDynamic())
 					{
 						// Create constraint to drag body
 						DistanceConstraintSettings settings;
@@ -1705,8 +1755,6 @@ void SamplesApp::UpdateDebug()
 						// Construct constraint that connects the drag anchor with the body that we want to drag
 						mDragConstraint = settings.Create(*drag_anchor, drag_body);
 						mPhysicsSystem->AddConstraint(mDragConstraint);
-
-						mDragFraction = hit_fraction;
 					}
 				}
 			}
@@ -1718,20 +1766,61 @@ void SamplesApp::UpdateDebug()
 		{
 			// If key released, destroy constraint
 			if (mDragConstraint != nullptr)
+			{
 				mPhysicsSystem->RemoveConstraint(mDragConstraint);
-			mDragConstraint = nullptr;
+				mDragConstraint = nullptr;
+			}
 
 			// Destroy drag anchor
-			bi.DestroyBody(mDragAnchor->GetID());
-			mDragAnchor = nullptr;
+			if (mDragAnchor != nullptr)
+			{
+				bi.DestroyBody(mDragAnchor->GetID());
+				mDragAnchor = nullptr;
+			}
+
+			// Release dragged vertex
+			if (mDragVertexIndex != ~uint(0))
+			{
+				// Restore vertex mass
+				BodyLockWrite lock(mPhysicsSystem->GetBodyLockInterface(), mDragBody);
+				if (lock.Succeeded())
+				{
+					Body &body = lock.GetBody();
+					JPH_ASSERT(body.IsSoftBody());
+					SoftBodyMotionProperties *mp = static_cast<SoftBodyMotionProperties *>(body.GetMotionProperties());
+					mp->GetVertex(mDragVertexIndex).mInvMass = mDragVertexPreviousInvMass;
+				}
+				mDragVertexIndex = ~uint(0);
+				mDragVertexPreviousInvMass = 0;
+			}
 
 			// Forget the drag body
 			mDragBody = BodyID();
 		}
 		else
 		{
-			// Else update position of anchor
-			bi.SetPositionAndRotation(mDragAnchor->GetID(), GetCamera().mPos + cDragRayLength * mDragFraction * GetCamera().mForward, Quat::sIdentity(), EActivation::DontActivate);
+			// Else drag the body to the new position
+			RVec3 new_pos = GetCamera().mPos + cDragRayLength * mDragFraction * GetCamera().mForward;
+
+			switch (bi.GetBodyType(mDragBody))
+			{
+			case EBodyType::RigidBody:
+				bi.SetPositionAndRotation(mDragAnchor->GetID(), new_pos, Quat::sIdentity(), EActivation::DontActivate);
+				break;
+
+			case EBodyType::SoftBody:
+				{
+					BodyLockWrite lock(mPhysicsSystem->GetBodyLockInterface(), mDragBody);
+					if (lock.Succeeded())
+					{
+						Body &body = lock.GetBody();
+						SoftBodyMotionProperties *mp = static_cast<SoftBodyMotionProperties *>(body.GetMotionProperties());
+						SoftBodyVertex &v = mp->GetVertex(mDragVertexIndex);
+						v.mVelocity = body.GetRotation().Conjugated() * Vec3(new_pos - body.GetCenterOfMassTransform() * v.mPosition) / inDeltaTime;
+					}
+				}
+				break;
+			}
 
 			// Activate other body
 			bi.ActivateBody(mDragBody);
@@ -1955,7 +2044,7 @@ bool SamplesApp::RenderFrame(float inDeltaTime)
 		if (inDeltaTime > 0.0f)
 		{
 			// Debugging functionality like shooting a ball and dragging objects
-			UpdateDebug();
+			UpdateDebug(inDeltaTime);
 
 			if (mRecordState || check_determinism)
 			{
@@ -2112,7 +2201,9 @@ void SamplesApp::DrawPhysics()
 					}
 
 					// Ensure that we cache the geometry for next frame
-					shape_to_geometry[transformed_shape.mShape] = geometry;
+					// Don't cache soft bodies as their shape changes every frame
+					if (!body.IsSoftBody())
+						shape_to_geometry[transformed_shape.mShape] = geometry;
 
 					// Determine color
 					Color color;

Samples/SamplesApp.h

@@ -27,7 +27,7 @@ public:
 	// Constructor / destructor
 							SamplesApp();
 	virtual					~SamplesApp() override;
-		
+
 	// Render the frame.
 	virtual bool			RenderFrame(float inDeltaTime) override;
 
@@ -68,7 +68,7 @@ private:
 	void					ShootObject();
 
 	// Debug functionality: firing a ball, mouse dragging
-	void					UpdateDebug();
+	void					UpdateDebug(float inDeltaTime);
 
 	// Draw the state of the physics system
 	void					DrawPhysics();
@@ -211,9 +211,11 @@ private:
 	Vec3					mShootObjectShapeScale = Vec3::sReplicate(1.0f);			// Scale of the object to shoot
 
 	// Mouse dragging
-	Body *					mDragAnchor = nullptr;										// A anchor point for the distance constraint. Corresponds to the current crosshair position.
+	Body *					mDragAnchor = nullptr;										// Rigid bodies only: A anchor point for the distance constraint. Corresponds to the current crosshair position.
 	BodyID					mDragBody;													// The body ID of the body that the user is currently dragging.
-	Ref<Constraint>			mDragConstraint;											// The distance constraint that connects the body to be dragged and the anchor point.
+	Ref<Constraint>			mDragConstraint;											// Rigid bodies only: The distance constraint that connects the body to be dragged and the anchor point.
+	uint					mDragVertexIndex = ~uint(0);								// Soft bodies only: The vertex index of the body that the user is currently dragging.
+	float					mDragVertexPreviousInvMass = 0.0f;							// Soft bodies only: The inverse mass of the vertex that the user is currently dragging.
 	float					mDragFraction;												// Fraction along cDragRayLength (see cpp) where the hit occurred. This will be combined with the crosshair position to get a 3d anchor point.
 
 	// Timing

Samples/Tests/SoftBody/SoftBodyFrictionTest.cpp

@@ -0,0 +1,50 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/SoftBody/SoftBodyFrictionTest.h>
+#include <Jolt/Physics/SoftBody/SoftBodyCreationSettings.h>
+#include <Utils/SoftBodyCreator.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(SoftBodyFrictionTest)
+{
+	JPH_ADD_BASE_CLASS(SoftBodyFrictionTest, Test)
+}
+
+void SoftBodyFrictionTest::Initialize()
+{
+	// Floor
+	Body &floor = CreateFloor();
+	floor.SetFriction(1.0f);
+
+	// Bodies with increasing friction
+	Ref<SoftBodySharedSettings> sphere_settings = SoftBodyCreator::CreateSphere();
+	for (SoftBodySharedSettings::Vertex &v : sphere_settings->mVertices)
+		v.mVelocity = Float3(0, 0, 10);
+	SoftBodyCreationSettings sphere(sphere_settings);
+	sphere.mObjectLayer = Layers::MOVING;
+	sphere.mPressure = 2000.0f;
+
+	for (int i = 0; i <= 10; ++i)
+	{
+		sphere.mPosition = RVec3(-50.0f + i * 10.0f, 1.0f, 0);
+		sphere.mFriction = 0.1f * i;
+		mBodyInterface->CreateAndAddSoftBody(sphere, EActivation::Activate);
+	}
+
+	Ref<SoftBodySharedSettings> cube_settings = SoftBodyCreator::CreateCube();
+	for (SoftBodySharedSettings::Vertex &v : cube_settings->mVertices)
+		v.mVelocity = Float3(0, 0, 10);
+	SoftBodyCreationSettings cube(cube_settings);
+	cube.mObjectLayer = Layers::MOVING;
+
+	for (int i = 0; i <= 10; ++i)
+	{
+		cube.mPosition = RVec3(-50.0f + i * 10.0f, 1.0f, -5.0f);
+		cube.mFriction = 0.1f * i;
+		mBodyInterface->CreateAndAddSoftBody(cube, EActivation::Activate);
+	}
+}

Samples/Tests/SoftBody/SoftBodyFrictionTest.h

@@ -0,0 +1,17 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+// This test tests soft bodies with various values for friction
+class SoftBodyFrictionTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, SoftBodyFrictionTest)
+
+	// See: Test
+	virtual void		Initialize() override;
+};

Samples/Tests/SoftBody/SoftBodyGravityFactorTest.cpp

@@ -0,0 +1,43 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/SoftBody/SoftBodyGravityFactorTest.h>
+#include <Jolt/Physics/SoftBody/SoftBodyCreationSettings.h>
+#include <Utils/SoftBodyCreator.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(SoftBodyGravityFactorTest)
+{
+	JPH_ADD_BASE_CLASS(SoftBodyGravityFactorTest, Test)
+}
+
+void SoftBodyGravityFactorTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	// Bodies with increasing gravity factor
+	SoftBodyCreationSettings sphere(SoftBodyCreator::CreateSphere());
+	sphere.mObjectLayer = Layers::MOVING;
+	sphere.mPressure = 2000.0f;
+
+	for (int i = 0; i <= 10; ++i)
+	{
+		sphere.mPosition = RVec3(-50.0f + i * 10.0f, 10.0f, 0);
+		sphere.mGravityFactor = 0.1f * i;
+		mBodyInterface->CreateAndAddSoftBody(sphere, EActivation::Activate);
+	}
+
+	SoftBodyCreationSettings cube(SoftBodyCreator::CreateCube());
+	cube.mObjectLayer = Layers::MOVING;
+
+	for (int i = 0; i <= 10; ++i)
+	{
+		cube.mPosition = RVec3(-50.0f + i * 10.0f, 10.0f, -5.0f);
+		cube.mGravityFactor = 0.1f * i;
+		mBodyInterface->CreateAndAddSoftBody(cube, EActivation::Activate);
+	}
+}

Samples/Tests/SoftBody/SoftBodyGravityFactorTest.h

@@ -0,0 +1,17 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+// This test tests soft bodies with various gravity factor values
+class SoftBodyGravityFactorTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, SoftBodyGravityFactorTest)
+
+	// See: Test
+	virtual void		Initialize() override;
+};

Samples/Tests/SoftBody/SoftBodyKinematicTest.cpp

@@ -0,0 +1,47 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/SoftBody/SoftBodyKinematicTest.h>
+#include <Jolt/Physics/SoftBody/SoftBodyCreationSettings.h>
+#include <Jolt/Physics/SoftBody/SoftBodyMotionProperties.h>
+#include <Utils/SoftBodyCreator.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(SoftBodyKinematicTest)
+{
+	JPH_ADD_BASE_CLASS(SoftBodyKinematicTest, Test)
+}
+
+void SoftBodyKinematicTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	// A sphere
+	Ref<SoftBodySharedSettings> sphere_settings = SoftBodyCreator::CreateSphere();
+	sphere_settings->mVertices[0].mInvMass = 0.0f;
+	sphere_settings->mVertices[0].mVelocity = Float3(0, 0, 5);
+	SoftBodyCreationSettings sphere(sphere_settings, RVec3(0, 5, 0));
+	sphere.mObjectLayer = Layers::MOVING;
+	sphere.mPressure = 2000.0f;
+	mSphereID = mBodyInterface->CreateAndAddSoftBody(sphere, EActivation::Activate);
+}
+
+void SoftBodyKinematicTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	// Update the velocity of the first vertex
+	BodyLockWrite body_lock(mPhysicsSystem->GetBodyLockInterface(), mSphereID);
+	if (body_lock.Succeeded())
+	{
+		Body &body = body_lock.GetBody();
+		SoftBodyMotionProperties *mp = static_cast<SoftBodyMotionProperties *>(body.GetMotionProperties());
+		RVec3 com = body.GetCenterOfMassPosition();
+		if (com.GetZ() >= 10.0f)
+			mp->GetVertex(0).mVelocity = Vec3(0, 0, -5);
+		else if (com.GetZ() <= -10.0f)
+			mp->GetVertex(0).mVelocity = Vec3(0, 0, 5);
+	}
+}

Samples/Tests/SoftBody/SoftBodyKinematicTest.h

@@ -0,0 +1,21 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+// This test tests soft bodies with kinematic vertices
+class SoftBodyKinematicTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, SoftBodyKinematicTest)
+
+	// See: Test
+	virtual void		Initialize() override;
+	virtual void		PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+
+private:
+	BodyID				mSphereID;
+};

Samples/Tests/SoftBody/SoftBodyPressureTest.cpp

@@ -0,0 +1,32 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/SoftBody/SoftBodyPressureTest.h>
+#include <Jolt/Physics/SoftBody/SoftBodyCreationSettings.h>
+#include <Utils/SoftBodyCreator.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(SoftBodyPressureTest)
+{
+	JPH_ADD_BASE_CLASS(SoftBodyPressureTest, Test)
+}
+
+void SoftBodyPressureTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+
+	// Bodies with increasing pressure
+	SoftBodyCreationSettings sphere(SoftBodyCreator::CreateSphere(2.0f));
+	sphere.mObjectLayer = Layers::MOVING;
+
+	for (int i = 0; i <= 10; ++i)
+	{
+		sphere.mPosition = RVec3(-50.0f + i * 10.0f, 10.0f, 0);
+		sphere.mPressure = 1000.0f * i;
+		mBodyInterface->CreateAndAddSoftBody(sphere, EActivation::Activate);
+	}
+}

Samples/Tests/SoftBody/SoftBodyPressureTest.h

@@ -0,0 +1,17 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+// This test tests soft bodies with various values for pressure
+class SoftBodyPressureTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, SoftBodyPressureTest)
+
+	// See: Test
+	virtual void		Initialize() override;
+};

Samples/Tests/SoftBody/SoftBodyRestitutionTest.cpp

@@ -0,0 +1,44 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/SoftBody/SoftBodyRestitutionTest.h>
+#include <Jolt/Physics/SoftBody/SoftBodyCreationSettings.h>
+#include <Utils/SoftBodyCreator.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(SoftBodyRestitutionTest)
+{
+	JPH_ADD_BASE_CLASS(SoftBodyRestitutionTest, Test)
+}
+
+void SoftBodyRestitutionTest::Initialize()
+{
+	// Floor
+	Body &floor = CreateFloor();
+	floor.SetRestitution(0.0f);
+
+	// Bodies with increasing restitution
+	SoftBodyCreationSettings sphere(SoftBodyCreator::CreateSphere());
+	sphere.mObjectLayer = Layers::MOVING;
+	sphere.mPressure = 2000.0f;
+
+	for (int i = 0; i <= 10; ++i)
+	{
+		sphere.mPosition = RVec3(-50.0f + i * 10.0f, 10.0f, 0);
+		sphere.mRestitution = 0.1f * i;
+		mBodyInterface->CreateAndAddSoftBody(sphere, EActivation::Activate);
+	}
+
+	SoftBodyCreationSettings cube(SoftBodyCreator::CreateCube());
+	cube.mObjectLayer = Layers::MOVING;
+
+	for (int i = 0; i <= 10; ++i)
+	{
+		cube.mPosition = RVec3(-50.0f + i * 10.0f, 10.0f, -5.0f);
+		cube.mRestitution = 0.1f * i;
+		mBodyInterface->CreateAndAddSoftBody(cube, EActivation::Activate);
+	}
+}

Samples/Tests/SoftBody/SoftBodyRestitutionTest.h

@@ -0,0 +1,17 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+// This test tests soft bodies with various values for restitution
+class SoftBodyRestitutionTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, SoftBodyRestitutionTest)
+
+	// See: Test
+	virtual void		Initialize() override;
+};

Samples/Tests/SoftBody/SoftBodyShapesTest.cpp

@@ -0,0 +1,88 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/SoftBody/SoftBodyShapesTest.h>
+#include <Jolt/Physics/Body/BodyCreationSettings.h>
+#include <Jolt/Physics/SoftBody/SoftBodyCreationSettings.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/CylinderShape.h>
+#include <Jolt/Physics/Collision/Shape/ConvexHullShape.h>
+#include <Jolt/Physics/Collision/Shape/StaticCompoundShape.h>
+#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
+#include <Utils/SoftBodyCreator.h>
+#include <Renderer/DebugRendererImp.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(SoftBodyShapesTest)
+{
+	JPH_ADD_BASE_CLASS(SoftBodyShapesTest, Test)
+}
+
+void SoftBodyShapesTest::Initialize()
+{
+	const Quat cCubeOrientation = Quat::sRotation(Vec3::sReplicate(sqrt(1.0f / 3.0f)), DegreesToRadians(45.0f));
+
+	// Floor
+	CreateMeshTerrain();
+
+	// Create cloth that's fixated at the corners
+	SoftBodyCreationSettings cloth(SoftBodyCreator::CreateCloth(), RVec3(0, 10.0f, 0), Quat::sRotation(Vec3::sAxisY(), 0.25f * JPH_PI));
+	cloth.mObjectLayer = Layers::MOVING;
+	cloth.mUpdatePosition = false; // Don't update the position of the cloth as it is fixed to the world
+	cloth.mMakeRotationIdentity = false; // Test explicitly checks if soft bodies with a rotation collide with shapes properly
+	mBodyInterface->CreateAndAddSoftBody(cloth, EActivation::Activate);
+
+	// Create cube
+	SoftBodyCreationSettings cube(SoftBodyCreator::CreateCube(), RVec3(15.0f, 10.0f, 0.0f), cCubeOrientation);
+	cube.mObjectLayer = Layers::MOVING;
+	cube.mRestitution = 0.0f;
+	mBodyInterface->CreateAndAddSoftBody(cube, EActivation::Activate);
+
+	// Create pressurized sphere
+	SoftBodyCreationSettings sphere(SoftBodyCreator::CreateSphere(), RVec3(15.0f, 10.0f, 15.0f));
+	sphere.mObjectLayer = Layers::MOVING;
+	sphere.mPressure = 2000.0f;
+	mBodyInterface->CreateAndAddSoftBody(sphere, EActivation::Activate);
+
+	// Sphere below pressurized sphere
+	RefConst<Shape> sphere_shape = new SphereShape(1.0f);
+	BodyCreationSettings bcs(sphere_shape, RVec3(15.5f, 7.0f, 15.0f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+	bcs.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia;
+	bcs.mMassPropertiesOverride.mMass = 100.0f;
+	mBodyInterface->CreateAndAddBody(bcs, EActivation::Activate);
+
+	// Various shapes above cloth
+	ConvexHullShapeSettings tetrahedron({ Vec3(-2, -2, -2), Vec3(0, -2, 2), Vec3(2, -2, -2), Vec3(0, 2, 0) });
+	tetrahedron.SetEmbedded();
+
+	StaticCompoundShapeSettings compound_shape;
+	compound_shape.SetEmbedded();
+	Quat rotate_x = Quat::sRotation(Vec3::sAxisX(), 0.5f * JPH_PI);
+	compound_shape.AddShape(Vec3::sZero(), rotate_x, new CapsuleShape(2, 0.5f));
+	compound_shape.AddShape(Vec3(0, 0, -2), Quat::sIdentity(), new SphereShape(1));
+	compound_shape.AddShape(Vec3(0, 0, 2), Quat::sIdentity(), new SphereShape(1));
+
+	RefConst<Shape> shapes[] = {
+		sphere_shape,
+		new BoxShape(Vec3(0.75f, 1.0f, 1.25f)),
+		new RotatedTranslatedShape(Vec3::sZero(), rotate_x, new CapsuleShape(1, 0.5f)),
+		new RotatedTranslatedShape(Vec3::sZero(), rotate_x, TaperedCapsuleShapeSettings(1.0f, 1.0f, 0.5f).Create().Get()),
+		new RotatedTranslatedShape(Vec3::sZero(), rotate_x, new CylinderShape(1, 0.5f)),
+		tetrahedron.Create().Get(),
+		compound_shape.Create().Get(),
+	};
+	int num_shapes = (int)std::size(shapes);
+
+	for (int i = 0; i < num_shapes; ++i)
+	{
+		bcs.SetShape(shapes[i % num_shapes]);
+		bcs.mPosition = RVec3(-float(num_shapes) + 2.0f * i, 15.0f, 0);
+		mBodyInterface->CreateAndAddBody(bcs, EActivation::Activate);
+	}
+}

Samples/Tests/SoftBody/SoftBodyShapesTest.h

@@ -0,0 +1,17 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+// This test shows interaction between various collision shapes and soft bodies
+class SoftBodyShapesTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, SoftBodyShapesTest)
+
+	// See: Test
+	virtual void		Initialize() override;
+};

Samples/Tests/SoftBody/SoftBodyUpdatePositionTest.cpp

@@ -0,0 +1,34 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/SoftBody/SoftBodyUpdatePositionTest.h>
+#include <Jolt/Physics/SoftBody/SoftBodyCreationSettings.h>
+#include <Utils/SoftBodyCreator.h>
+#include <Layers.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(SoftBodyUpdatePositionTest)
+{
+	JPH_ADD_BASE_CLASS(SoftBodyUpdatePositionTest, Test)
+}
+
+void SoftBodyUpdatePositionTest::Initialize()
+{
+	// Floor
+	CreateFloor();
+	
+	// Bodies with various settings for 'make rotation identity' and 'update position'
+	SoftBodyCreationSettings sphere(SoftBodyCreator::CreateCube(), RVec3::sZero(), Quat::sRotation(Vec3::sReplicate(1.0f / sqrt(3.0f)), 0.25f * JPH_PI));
+	sphere.mObjectLayer = Layers::MOVING;
+
+	for (int update_position = 0; update_position < 2; ++update_position)
+		for (int make_rotation_identity = 0; make_rotation_identity < 2; ++make_rotation_identity)
+		{
+			sphere.mPosition = RVec3(update_position * 10.0f, 10.0f, make_rotation_identity * 10.0f);
+			sphere.mUpdatePosition = update_position != 0;
+			sphere.mMakeRotationIdentity = make_rotation_identity != 0;
+			mBodyInterface->CreateAndAddSoftBody(sphere, EActivation::Activate);
+		}
+}

Samples/Tests/SoftBody/SoftBodyUpdatePositionTest.h

@@ -0,0 +1,19 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+// This test tests soft bodies with and without 'update position' and 'make rotation identity'
+// If you turn on 'Draw World Transforms' you will see that 2 cubes will stay at their initial position.
+// If you turn on 'Draw Bounding Boxes' then you will see that the cubes that didn't have 'make rotation identity' will have a bigger bounding box.
+class SoftBodyUpdatePositionTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, SoftBodyUpdatePositionTest)
+
+	// See: Test
+	virtual void		Initialize() override;
+};

Samples/Utils/SoftBodyCreator.cpp

@@ -0,0 +1,309 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Utils/SoftBodyCreator.h>
+
+namespace SoftBodyCreator {
+
+Ref<SoftBodySharedSettings> CreateCloth(uint inGridSize, float inGridSpacing, bool inFixateCorners)
+{
+	const float cOffset = -0.5f * inGridSpacing * (inGridSize - 1);
+
+	// Create settings
+	SoftBodySharedSettings *settings = new SoftBodySharedSettings;
+	for (uint y = 0; y < inGridSize; ++y)
+		for (uint x = 0; x < inGridSize; ++x)
+		{
+			SoftBodySharedSettings::Vertex v;
+			v.mPosition = Float3(cOffset + x * inGridSpacing, 0.0f, cOffset + y * inGridSpacing);
+			settings->mVertices.push_back(v);
+		}
+
+	// Function to get the vertex index of a point on the cloth
+	auto vertex_index = [inGridSize](uint inX, uint inY) -> uint
+	{
+		return inX + inY * inGridSize;
+	};
+
+	if (inFixateCorners)
+	{
+		// Fixate corners
+		settings->mVertices[vertex_index(0, 0)].mInvMass = 0.0f;
+		settings->mVertices[vertex_index(inGridSize - 1, 0)].mInvMass = 0.0f;
+		settings->mVertices[vertex_index(0, inGridSize - 1)].mInvMass = 0.0f;
+		settings->mVertices[vertex_index(inGridSize - 1, inGridSize - 1)].mInvMass = 0.0f;
+	}
+
+	// Create edges
+	for (uint y = 0; y < inGridSize; ++y)
+		for (uint x = 0; x < inGridSize; ++x)
+		{
+			SoftBodySharedSettings::Edge e;
+			e.mCompliance = 0.00001f;
+			e.mVertex[0] = vertex_index(x, y);
+			if (x < inGridSize - 1)
+			{
+				e.mVertex[1] = vertex_index(x + 1, y);
+				settings->mEdgeConstraints.push_back(e);
+			}
+			if (y < inGridSize - 1)
+			{
+				e.mVertex[1] = vertex_index(x, y + 1);
+				settings->mEdgeConstraints.push_back(e);
+			}
+			if (x < inGridSize - 1 && y < inGridSize - 1)
+			{
+				e.mVertex[1] = vertex_index(x + 1, y + 1);
+				settings->mEdgeConstraints.push_back(e);
+
+				e.mVertex[0] = vertex_index(x + 1, y);
+				e.mVertex[1] = vertex_index(x, y + 1);
+				settings->mEdgeConstraints.push_back(e);
+			}
+		}
+	settings->CalculateEdgeLengths();
+
+	// Create faces
+	for (uint y = 0; y < inGridSize - 1; ++y)
+		for (uint x = 0; x < inGridSize - 1; ++x)
+		{
+			SoftBodySharedSettings::Face f;
+			f.mVertex[0] = vertex_index(x, y);
+			f.mVertex[1] = vertex_index(x, y + 1);
+			f.mVertex[2] = vertex_index(x + 1, y + 1);
+			settings->AddFace(f);
+
+			f.mVertex[1] = vertex_index(x + 1, y + 1);
+			f.mVertex[2] = vertex_index(x + 1, y);
+			settings->AddFace(f);
+		}
+
+	return settings;
+}
+
+Ref<SoftBodySharedSettings> CreateCube(uint inGridSize, float inGridSpacing)
+{
+	const Vec3 cOffset = Vec3::sReplicate(-0.5f * inGridSpacing * (inGridSize - 1));
+
+	// Create settings
+	SoftBodySharedSettings *settings = new SoftBodySharedSettings;
+	for (uint z = 0; z < inGridSize; ++z)
+		for (uint y = 0; y < inGridSize; ++y)
+			for (uint x = 0; x < inGridSize; ++x)
+			{
+				SoftBodySharedSettings::Vertex v;
+				(cOffset + Vec3::sReplicate(inGridSpacing) * Vec3(float(x), float(y), float(z))).StoreFloat3(&v.mPosition);
+				settings->mVertices.push_back(v);
+			}
+
+	// Function to get the vertex index of a point on the cloth
+	auto vertex_index = [inGridSize](uint inX, uint inY, uint inZ) -> uint
+	{
+		return inX + inY * inGridSize + inZ * inGridSize * inGridSize;
+	};
+
+	// Create edges
+	for (uint z = 0; z < inGridSize; ++z)
+		for (uint y = 0; y < inGridSize; ++y)
+			for (uint x = 0; x < inGridSize; ++x)
+			{
+				SoftBodySharedSettings::Edge e;
+				e.mVertex[0] = vertex_index(x, y, z);
+				if (x < inGridSize - 1)
+				{
+					e.mVertex[1] = vertex_index(x + 1, y, z);
+					settings->mEdgeConstraints.push_back(e);
+				}
+				if (y < inGridSize - 1)
+				{
+					e.mVertex[1] = vertex_index(x, y + 1, z);
+					settings->mEdgeConstraints.push_back(e);
+				}
+				if (z < inGridSize - 1)
+				{
+					e.mVertex[1] = vertex_index(x, y, z + 1);
+					settings->mEdgeConstraints.push_back(e);
+				}
+			}
+	settings->CalculateEdgeLengths();
+
+	// Tetrahedrons to fill a cube
+	const int tetra_indices[6][4][3] = {
+		{ {0, 0, 0}, {0, 1, 1}, {0, 0, 1}, {1, 1, 1} },
+		{ {0, 0, 0}, {0, 1, 0}, {0, 1, 1}, {1, 1, 1} },
+		{ {0, 0, 0}, {0, 0, 1}, {1, 0, 1}, {1, 1, 1} },
+		{ {0, 0, 0}, {1, 0, 1}, {1, 0, 0}, {1, 1, 1} },
+		{ {0, 0, 0}, {1, 1, 0}, {0, 1, 0}, {1, 1, 1} },
+		{ {0, 0, 0}, {1, 0, 0}, {1, 1, 0}, {1, 1, 1} }
+	};
+
+	// Create volume constraints
+	for (uint z = 0; z < inGridSize - 1; ++z)
+		for (uint y = 0; y < inGridSize - 1; ++y)
+			for (uint x = 0; x < inGridSize - 1; ++x)
+				for (uint t = 0; t < 6; ++t)
+				{
+					SoftBodySharedSettings::Volume v;
+					for (uint i = 0; i < 4; ++i)
+						v.mVertex[i] = vertex_index(x + tetra_indices[t][i][0], y + tetra_indices[t][i][1], z + tetra_indices[t][i][2]);
+					settings->mVolumeConstraints.push_back(v);
+				}
+
+	settings->CalculateVolumeConstraintVolumes();
+
+	// Create faces
+	for (uint y = 0; y < inGridSize - 1; ++y)
+		for (uint x = 0; x < inGridSize - 1; ++x)
+		{
+			SoftBodySharedSettings::Face f;
+
+			// Face 1
+			f.mVertex[0] = vertex_index(x, y, 0);
+			f.mVertex[1] = vertex_index(x, y + 1, 0);
+			f.mVertex[2] = vertex_index(x + 1, y + 1, 0);
+			settings->AddFace(f);
+
+			f.mVertex[1] = vertex_index(x + 1, y + 1, 0);
+			f.mVertex[2] = vertex_index(x + 1, y, 0);
+			settings->AddFace(f);
+
+			// Face 2
+			f.mVertex[0] = vertex_index(x, y, inGridSize - 1);
+			f.mVertex[1] = vertex_index(x + 1, y + 1, inGridSize - 1);
+			f.mVertex[2] = vertex_index(x, y + 1, inGridSize - 1);
+			settings->AddFace(f);
+
+			f.mVertex[1] = vertex_index(x + 1, y, inGridSize - 1);
+			f.mVertex[2] = vertex_index(x + 1, y + 1, inGridSize - 1);
+			settings->AddFace(f);
+
+			// Face 3
+			f.mVertex[0] = vertex_index(x, 0, y);
+			f.mVertex[1] = vertex_index(x + 1, 0, y + 1);
+			f.mVertex[2] = vertex_index(x, 0, y + 1);
+			settings->AddFace(f);
+
+			f.mVertex[1] = vertex_index(x + 1, 0, y);
+			f.mVertex[2] = vertex_index(x + 1, 0, y + 1);
+			settings->AddFace(f);
+
+			// Face 4
+			f.mVertex[0] = vertex_index(x, inGridSize - 1, y);
+			f.mVertex[1] = vertex_index(x, inGridSize - 1, y + 1);
+			f.mVertex[2] = vertex_index(x + 1, inGridSize - 1, y + 1);
+			settings->AddFace(f);
+
+			f.mVertex[1] = vertex_index(x + 1, inGridSize - 1, y + 1);
+			f.mVertex[2] = vertex_index(x + 1, inGridSize - 1, y);
+			settings->AddFace(f);
+
+			// Face 5
+			f.mVertex[0] = vertex_index(0, x, y);
+			f.mVertex[1] = vertex_index(0, x, y + 1);
+			f.mVertex[2] = vertex_index(0, x + 1, y + 1);
+			settings->AddFace(f);
+
+			f.mVertex[1] = vertex_index(0, x + 1, y + 1);
+			f.mVertex[2] = vertex_index(0, x + 1, y);
+			settings->AddFace(f);
+
+			// Face 6
+			f.mVertex[0] = vertex_index(inGridSize - 1, x, y);
+			f.mVertex[1] = vertex_index(inGridSize - 1, x + 1, y + 1);
+			f.mVertex[2] = vertex_index(inGridSize - 1, x, y + 1);
+			settings->AddFace(f);
+
+			f.mVertex[1] = vertex_index(inGridSize - 1, x + 1, y);
+			f.mVertex[2] = vertex_index(inGridSize - 1, x + 1, y + 1);
+			settings->AddFace(f);
+		}
+
+	return settings;
+}
+
+Ref<SoftBodySharedSettings> CreateSphere(float inRadius, uint inNumTheta, uint inNumPhi)
+{
+	// Create settings
+	SoftBodySharedSettings *settings = new SoftBodySharedSettings;
+
+	// Create vertices
+	SoftBodySharedSettings::Vertex v;
+	(inRadius * Vec3::sUnitSpherical(0, 0)).StoreFloat3(&v.mPosition);
+	settings->mVertices.push_back(v);
+	(inRadius * Vec3::sUnitSpherical(JPH_PI, 0)).StoreFloat3(&v.mPosition);
+	settings->mVertices.push_back(v);
+	for (uint theta = 1; theta < inNumTheta - 1; ++theta)
+		for (uint phi = 0; phi < inNumPhi; ++phi)
+		{
+			(inRadius * Vec3::sUnitSpherical(JPH_PI * theta / (inNumTheta - 1), 2.0f * JPH_PI * phi / inNumPhi)).StoreFloat3(&v.mPosition);
+			settings->mVertices.push_back(v);
+		}
+
+	// Function to get the vertex index of a point on the sphere
+	auto vertex_index = [inNumTheta, inNumPhi](uint inTheta, uint inPhi) -> uint
+	{
+		if (inTheta == 0)
+			return 0;
+		else if (inTheta == inNumTheta - 1)
+			return 1;
+		else
+			return 2 + (inTheta - 1) * inNumPhi + inPhi % inNumPhi;
+	};
+
+	// Create edge constraints
+	for (uint phi = 0; phi < inNumPhi; ++phi)
+	{
+		for (uint theta = 0; theta < inNumTheta - 1; ++theta)
+		{
+			SoftBodySharedSettings::Edge e;
+			e.mCompliance = 0.0001f;
+			e.mVertex[0] = vertex_index(theta, phi);
+
+			e.mVertex[1] = vertex_index(theta + 1, phi);
+			settings->mEdgeConstraints.push_back(e);
+
+			e.mVertex[1] = vertex_index(theta + 1, phi + 1);
+			settings->mEdgeConstraints.push_back(e);
+
+			if (theta > 0)
+			{
+				e.mVertex[1] =  vertex_index(theta, phi + 1);
+				settings->mEdgeConstraints.push_back(e);
+			}
+		}
+	}
+
+	settings->CalculateEdgeLengths();
+
+	// Create faces
+	SoftBodySharedSettings::Face f;
+	for (uint phi = 0; phi < inNumPhi; ++phi)
+	{
+		for (uint theta = 0; theta < inNumTheta - 2; ++theta)
+		{
+			f.mVertex[0] = vertex_index(theta, phi);
+			f.mVertex[1] = vertex_index(theta + 1, phi);
+			f.mVertex[2] = vertex_index(theta + 1, phi + 1);
+			settings->AddFace(f);
+
+			if (theta > 0)
+			{
+				f.mVertex[1] = vertex_index(theta + 1, phi + 1);
+				f.mVertex[2] = vertex_index(theta, phi + 1);
+				settings->AddFace(f);
+			}
+		}
+
+		f.mVertex[0] = vertex_index(inNumTheta - 2, phi + 1);
+		f.mVertex[1] = vertex_index(inNumTheta - 2, phi);
+		f.mVertex[2] = vertex_index(inNumTheta - 1, 0);
+		settings->AddFace(f);
+	}
+
+	return settings;
+}
+
+};

Samples/Utils/SoftBodyCreator.h

@@ -0,0 +1,27 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Physics/SoftBody/SoftBodySharedSettings.h>
+
+namespace SoftBodyCreator
+{
+	/// Create a square cloth
+	/// @param inGridSize Number of points along each axis
+	/// @param inGridSpacing Distance between points
+	/// @param inFixateCorners If the corners should be fixated and have infinite mass
+	Ref<SoftBodySharedSettings>	CreateCloth(uint inGridSize = 30, float inGridSpacing = 0.75f, bool inFixateCorners = true);
+
+	/// Create a cube
+	/// @param inGridSize Number of points along each axis
+	/// @param inGridSpacing Distance between points
+	Ref<SoftBodySharedSettings>	CreateCube(uint inGridSize = 5, float inGridSpacing = 0.5f);
+
+	/// Create a hollow sphere
+	/// @param inRadius Radius of the sphere
+	/// @param inNumTheta Number of segments in the theta direction
+	/// @param inNumPhi Number of segments in the phi direction
+	Ref<SoftBodySharedSettings>	CreateSphere(float inRadius = 1.0f, uint inNumTheta = 10, uint inNumPhi = 20);
+};