SoftBodyMotionProperties.cpp

// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
// SPDX-License-Identifier: MIT

#include <Jolt/Jolt.h>

#include <Jolt/Physics/SoftBody/SoftBodyMotionProperties.h>
#include <Jolt/Physics/SoftBody/SoftBodyCreationSettings.h>
#include <Jolt/Physics/SoftBody/SoftBodyContactListener.h>
#include <Jolt/Physics/SoftBody/SoftBodyManifold.h>
#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
#include <Jolt/Physics/Collision/SimShapeFilterWrapper.h>
#include <Jolt/Physics/PhysicsSystem.h>
#include <Jolt/Physics/Body/BodyManager.h>
#include <Jolt/Core/ScopeExit.h>
#ifdef JPH_DEBUG_RENDERER
	#include <Jolt/Renderer/DebugRenderer.h>
#endif // JPH_DEBUG_RENDERER

JPH_NAMESPACE_BEGIN

using namespace JPH::literals;

void SoftBodyMotionProperties::CalculateMassAndInertia()
{
	MassProperties mp;

	for (const Vertex &v : mVertices)
		if (v.mInvMass > 0.0f)
		{
			Vec3 pos = v.mPosition;

			// Accumulate mass
			float mass = 1.0f / v.mInvMass;
			mp.mMass += mass;

			// Inertia tensor, diagonal
			// See equations https://en.wikipedia.org/wiki/Moment_of_inertia section 'Inertia Tensor'
			for (int i = 0; i < 3; ++i)
				mp.mInertia(i, i) += mass * (Square(pos[(i + 1) % 3]) + Square(pos[(i + 2) % 3]));

			// Inertia tensor off diagonal
			for (int i = 0; i < 3; ++i)
				for (int j = 0; j < 3; ++j)
					if (i != j)
						mp.mInertia(i, j) -= mass * pos[i] * pos[j];
		}
		else
		{
			// If one vertex is kinematic, the entire body will have infinite mass and inertia
			SetInverseMass(0.0f);
			SetInverseInertia(Vec3::sZero(), Quat::sIdentity());
			return;
		}

	SetMassProperties(EAllowedDOFs::All, mp);
}

void SoftBodyMotionProperties::Initialize(const SoftBodyCreationSettings &inSettings)
{
	// Store settings
	mSettings = inSettings.mSettings;
	mNumIterations = inSettings.mNumIterations;
	mPressure = inSettings.mPressure;
	mUpdatePosition = inSettings.mUpdatePosition;
	mFacesDoubleSided = inSettings.mFacesDoubleSided;
	SetVertexRadius(inSettings.mVertexRadius);

	// Initialize vertices
	mVertices.resize(inSettings.mSettings->mVertices.size());
	Mat44 rotation = inSettings.mMakeRotationIdentity? Mat44::sRotation(inSettings.mRotation) : Mat44::sIdentity();
	for (Array<Vertex>::size_type v = 0, s = mVertices.size(); v < s; ++v)
	{
		const SoftBodySharedSettings::Vertex &in_vertex = inSettings.mSettings->mVertices[v];
		Vertex &out_vertex = mVertices[v];
		out_vertex.mPreviousPosition = out_vertex.mPosition = rotation * Vec3(in_vertex.mPosition);
		out_vertex.mVelocity = rotation.Multiply3x3(Vec3(in_vertex.mVelocity));
		out_vertex.ResetCollision();
		out_vertex.mInvMass = in_vertex.mInvMass;
		mLocalBounds.Encapsulate(out_vertex.mPosition);
	}

	// Initialize rods
	if (!inSettings.mSettings->mRodStretchShearConstraints.empty())
	{
		mRodStates.resize(inSettings.mSettings->mRodStretchShearConstraints.size());
		Quat rotation_q = rotation.GetQuaternion();
		for (Array<RodState>::size_type r = 0, s = mRodStates.size(); r < s; ++r)
		{
			const SoftBodySharedSettings::RodStretchShear &in_rod = inSettings.mSettings->mRodStretchShearConstraints[r];
			RodState &out_rod = mRodStates[r];
			out_rod.mRotation = rotation_q * in_rod.mBishop;
			out_rod.mAngularVelocity = Vec3::sZero();
		}
	}

	// Allocate space for skinned vertices
	if (!inSettings.mSettings->mSkinnedConstraints.empty())
		mSkinState.resize(mVertices.size());

	// We don't know delta time yet, so we can't predict the bounds and use the local bounds as the predicted bounds
	mLocalPredictedBounds = mLocalBounds;

	CalculateMassAndInertia();
}

float SoftBodyMotionProperties::GetVolumeTimesSix() const
{
	float six_volume = 0.0f;
	for (const Face &f : mSettings->mFaces)
	{
		Vec3 x1 = mVertices[f.mVertex[0]].mPosition;
		Vec3 x2 = mVertices[f.mVertex[1]].mPosition;
		Vec3 x3 = mVertices[f.mVertex[2]].mPosition;
		six_volume += x1.Cross(x2).Dot(x3); // We pick zero as the origin as this is the center of the bounding box so should give good accuracy
	}
	return six_volume;
}

void SoftBodyMotionProperties::DetermineCollidingShapes(const SoftBodyUpdateContext &inContext, const PhysicsSystem &inSystem, const BodyLockInterface &inBodyLockInterface)
{
	JPH_PROFILE_FUNCTION();

	// Reset flag prior to collision detection
	mNeedContactCallback.store(false, memory_order_relaxed);

	struct Collector : public CollideShapeBodyCollector
	{
									Collector(const SoftBodyUpdateContext &inContext, const PhysicsSystem &inSystem, const BodyLockInterface &inBodyLockInterface, const AABox &inLocalBounds, SimShapeFilterWrapper &inShapeFilter, Array<CollidingShape> &ioHits, Array<CollidingSensor> &ioSensors) :
										mContext(inContext),
										mInverseTransform(inContext.mCenterOfMassTransform.InversedRotationTranslation()),
										mLocalBounds(inLocalBounds),
										mBodyLockInterface(inBodyLockInterface),
										mCombineFriction(inSystem.GetCombineFriction()),
										mCombineRestitution(inSystem.GetCombineRestitution()),
										mShapeFilter(inShapeFilter),
										mHits(ioHits),
										mSensors(ioSensors)
		{
		}

		virtual void				AddHit(const BodyID &inResult) override
		{
			BodyLockRead lock(mBodyLockInterface, inResult);
			if (lock.Succeeded())
			{
				const Body &soft_body = *mContext.mBody;
				const Body &body = lock.GetBody();
				if (body.IsRigidBody() // TODO: We should support soft body vs soft body
					&& soft_body.GetCollisionGroup().CanCollide(body.GetCollisionGroup()))
				{
					SoftBodyContactSettings settings;
					settings.mIsSensor = body.IsSensor();

					if (mContext.mContactListener == nullptr)
					{
						// If we have no contact listener, we can ignore sensors
						if (settings.mIsSensor)
							return;
					}
					else
					{
						// Call the contact listener to see if we should accept this contact
						if (mContext.mContactListener->OnSoftBodyContactValidate(soft_body, body, settings) != SoftBodyValidateResult::AcceptContact)
							return;

						// Check if there will be any interaction
						if (!settings.mIsSensor
							&& settings.mInvMassScale1 == 0.0f
							&& (body.GetMotionType() != EMotionType::Dynamic || settings.mInvMassScale2 == 0.0f))
							return;
					}

					// Calculate transform of this body relative to the soft body
					Mat44 com = (mInverseTransform * body.GetCenterOfMassTransform()).ToMat44();

					// Collect leaf shapes
					mShapeFilter.SetBody2(&body);
					struct LeafShapeCollector : public TransformedShapeCollector
					{
						virtual void		AddHit(const TransformedShape &inResult) override
						{
							mHits.emplace_back(Mat44::sRotationTranslation(inResult.mShapeRotation, Vec3(inResult.mShapePositionCOM)), inResult.GetShapeScale(), inResult.mShape);
						}

						Array<LeafShape>	mHits;
					};
					LeafShapeCollector collector;
					body.GetShape()->CollectTransformedShapes(mLocalBounds, com.GetTranslation(), com.GetQuaternion(), Vec3::sOne(), SubShapeIDCreator(), collector, mShapeFilter.GetFilter());
					if (collector.mHits.empty())
						return;

					if (settings.mIsSensor)
					{
						CollidingSensor cs;
						cs.mCenterOfMassTransform = com;
						cs.mShapes = std::move(collector.mHits);
						cs.mBodyID = inResult;
						mSensors.push_back(cs);
					}
					else
					{
						CollidingShape cs;
						cs.mCenterOfMassTransform = com;
						cs.mShapes = std::move(collector.mHits);
						cs.mBodyID = inResult;
						cs.mMotionType = body.GetMotionType();
						cs.mUpdateVelocities = false;
						cs.mFriction = mCombineFriction(soft_body, SubShapeID(), body, SubShapeID());
						cs.mRestitution = mCombineRestitution(soft_body, SubShapeID(), body, SubShapeID());
						cs.mSoftBodyInvMassScale = settings.mInvMassScale1;
						if (cs.mMotionType == EMotionType::Dynamic)
						{
							const MotionProperties *mp = body.GetMotionProperties();
							cs.mInvMass = settings.mInvMassScale2 * mp->GetInverseMass();
							cs.mInvInertia = settings.mInvInertiaScale2 * mp->GetInverseInertiaForRotation(cs.mCenterOfMassTransform.GetRotation());
							cs.mOriginalLinearVelocity = cs.mLinearVelocity = mInverseTransform.Multiply3x3(mp->GetLinearVelocity());
							cs.mOriginalAngularVelocity = cs.mAngularVelocity = mInverseTransform.Multiply3x3(mp->GetAngularVelocity());
						}
						mHits.push_back(cs);
					}
				}
			}
		}

	private:
		const SoftBodyUpdateContext &mContext;
		RMat44						mInverseTransform;
		AABox						mLocalBounds;
		const BodyLockInterface &	mBodyLockInterface;
		ContactConstraintManager::CombineFunction mCombineFriction;
		ContactConstraintManager::CombineFunction mCombineRestitution;
		SimShapeFilterWrapper &		mShapeFilter;
		Array<CollidingShape> &		mHits;
		Array<CollidingSensor> &	mSensors;
	};

	// Calculate local bounding box
	AABox local_bounds = mLocalBounds;
	local_bounds.Encapsulate(mLocalPredictedBounds);
	local_bounds.ExpandBy(Vec3::sReplicate(mVertexRadius));

	// Calculate world space bounding box
	AABox world_bounds = local_bounds.Transformed(inContext.mCenterOfMassTransform);

	// Create shape filter
	SimShapeFilterWrapper shape_filter(inContext.mSimShapeFilter, inContext.mBody);

	Collector collector(inContext, inSystem, inBodyLockInterface, local_bounds, shape_filter, mCollidingShapes, mCollidingSensors);
	ObjectLayer layer = inContext.mBody->GetObjectLayer();
	DefaultBroadPhaseLayerFilter broadphase_layer_filter = inSystem.GetDefaultBroadPhaseLayerFilter(layer);
	DefaultObjectLayerFilter object_layer_filter = inSystem.GetDefaultLayerFilter(layer);
	inSystem.GetBroadPhaseQuery().CollideAABox(world_bounds, collector, broadphase_layer_filter, object_layer_filter);
	mNumSensors = uint(mCollidingSensors.size()); // Workaround for TSAN false positive: store mCollidingSensors.size() in a separate variable.
}

void SoftBodyMotionProperties::DetermineCollisionPlanes(uint inVertexStart, uint inNumVertices)
{
	JPH_PROFILE_FUNCTION();

	// Generate collision planes
	for (const CollidingShape &cs : mCollidingShapes)
		for (const LeafShape &shape : cs.mShapes)
			shape.mShape->CollideSoftBodyVertices(shape.mTransform, shape.mScale, CollideSoftBodyVertexIterator(mVertices.data() + inVertexStart), inNumVertices, int(&cs - mCollidingShapes.data()));
}

void SoftBodyMotionProperties::DetermineSensorCollisions(CollidingSensor &ioSensor)
{
	JPH_PROFILE_FUNCTION();

	Plane collision_plane;
	float largest_penetration = -FLT_MAX;
	int colliding_shape_idx = -1;

	// Collide sensor against all vertices
	CollideSoftBodyVertexIterator vertex_iterator(
		StridedPtr<const Vec3>(&mVertices[0].mPosition, sizeof(SoftBodyVertex)), // The position and mass come from the soft body vertex
		StridedPtr<const float>(&mVertices[0].mInvMass, sizeof(SoftBodyVertex)),
		StridedPtr<Plane>(&collision_plane, 0), // We want all vertices to result in a single collision so we pass stride 0
		StridedPtr<float>(&largest_penetration, 0),
		StridedPtr<int>(&colliding_shape_idx, 0));
	for (const LeafShape &shape : ioSensor.mShapes)
		shape.mShape->CollideSoftBodyVertices(shape.mTransform, shape.mScale, vertex_iterator, uint(mVertices.size()), 0);
	ioSensor.mHasContact = largest_penetration > 0.0f;

	// We need a contact callback if one of the sensors collided
	if (ioSensor.mHasContact)
		mNeedContactCallback.store(true, memory_order_relaxed);
}

void SoftBodyMotionProperties::ApplyPressure(const SoftBodyUpdateContext &inContext)
{
	JPH_PROFILE_FUNCTION();

	float dt = inContext.mSubStepDeltaTime;
	float pressure_coefficient = mPressure;
	if (pressure_coefficient > 0.0f)
	{
		// Calculate total volume
		float six_volume = GetVolumeTimesSix();
		if (six_volume > 0.0f)
		{
			// Apply pressure
			// p = F / A = n R T / V (see https://en.wikipedia.org/wiki/Pressure)
			// Our pressure coefficient is n R T so the impulse is:
			// P = F dt = pressure_coefficient / V * A * dt
			float coefficient = pressure_coefficient * dt / six_volume; // Need to still multiply by 6 for the volume
			for (const Face &f : mSettings->mFaces)
			{
				Vec3 x1 = mVertices[f.mVertex[0]].mPosition;
				Vec3 x2 = mVertices[f.mVertex[1]].mPosition;
				Vec3 x3 = mVertices[f.mVertex[2]].mPosition;

				Vec3 impulse = coefficient * (x2 - x1).Cross(x3 - x1); // Area is half the cross product so need to still divide by 2
				for (uint32 i : f.mVertex)
				{
					Vertex &v = mVertices[i];
					v.mVelocity += v.mInvMass * impulse; // Want to divide by 3 because we spread over 3 vertices
				}
			}
		}
	}
}

void SoftBodyMotionProperties::IntegratePositions(const SoftBodyUpdateContext &inContext)
{
	JPH_PROFILE_FUNCTION();

	float dt = inContext.mSubStepDeltaTime;
	float linear_damping = max(0.0f, 1.0f - GetLinearDamping() * dt); // See: MotionProperties::ApplyForceTorqueAndDragInternal

	// Integrate
	Vec3 sub_step_gravity = inContext.mGravity * dt;
	Vec3 sub_step_impulse = GetAccumulatedForce() * dt / max(float(mVertices.size()), 1.0f);
	for (Vertex &v : mVertices)
	{
		if (v.mInvMass > 0.0f)
		{
			// Gravity
			v.mVelocity += sub_step_gravity + sub_step_impulse * v.mInvMass;

			// Damping
			v.mVelocity *= linear_damping;
		}

		// Integrate
		Vec3 position = v.mPosition;
		v.mPreviousPosition = position;
		v.mPosition = position + v.mVelocity * dt;
	}

	// Integrate rod orientations
	float half_dt = 0.5f * dt;
	for (RodState &r : mRodStates)
	{
		// Damping
		r.mAngularVelocity *= linear_damping;

		// Integrate
		Quat rotation = r.mRotation;
		Quat delta_rotation = half_dt * Quat::sMultiplyImaginary(r.mAngularVelocity, rotation);
		r.mPreviousRotationInternal = rotation; // Overwrites mAngularVelocity
		r.mRotation = (rotation + delta_rotation).Normalized();
	}
}

void SoftBodyMotionProperties::ApplyDihedralBendConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex)
{
	JPH_PROFILE_FUNCTION();

	float inv_dt_sq = 1.0f / Square(inContext.mSubStepDeltaTime);

	for (const DihedralBend *b = mSettings->mDihedralBendConstraints.data() + inStartIndex, *b_end = mSettings->mDihedralBendConstraints.data() + inEndIndex; b < b_end; ++b)
	{
		Vertex &v0 = mVertices[b->mVertex[0]];
		Vertex &v1 = mVertices[b->mVertex[1]];
		Vertex &v2 = mVertices[b->mVertex[2]];
		Vertex &v3 = mVertices[b->mVertex[3]];

		// Get positions
		Vec3 x0 = v0.mPosition;
		Vec3 x1 = v1.mPosition;
		Vec3 x2 = v2.mPosition;
		Vec3 x3 = v3.mPosition;

		/*
		   x2
		e1/  \e3
		 /    \
		x0----x1
		 \ e0 /
		e2\  /e4
		   x3
		*/

		// Calculate the shared edge of the triangles
		Vec3 e = x1 - x0;
		float e_len = e.Length();
		if (e_len < 1.0e-6f)
			continue;

		// Calculate the normals of the triangles
		Vec3 x1x2 = x2 - x1;
		Vec3 x1x3 = x3 - x1;
		Vec3 n1 = (x2 - x0).Cross(x1x2);
		Vec3 n2 = x1x3.Cross(x3 - x0);
		float n1_len_sq = n1.LengthSq();
		float n2_len_sq = n2.LengthSq();
		float n1_len_sq_n2_len_sq = n1_len_sq * n2_len_sq;
		if (n1_len_sq_n2_len_sq < 1.0e-24f)
			continue;

		// Calculate constraint equation
		// As per "Strain Based Dynamics" Appendix A we need to negate the gradients when (n1 x n2) . e > 0, instead we make sure that the sign of the constraint equation is correct
		float sign = Sign(n2.Cross(n1).Dot(e));
		float d = n1.Dot(n2) / sqrt(n1_len_sq_n2_len_sq);
		float c = sign * ACosApproximate(d) - b->mInitialAngle;

		// Ensure the range is -PI to PI
		if (c > JPH_PI)
			c -= 2.0f * JPH_PI;
		else if (c < -JPH_PI)
			c += 2.0f * JPH_PI;

		// Calculate gradient of constraint equation
		// Taken from "Strain Based Dynamics" - Matthias Muller et al. (Appendix A)
		// with p1 = x2, p2 = x3, p3 = x0 and p4 = x1
		// which in turn is based on "Simulation of Clothing with Folds and Wrinkles" - R. Bridson et al. (Section 4)
		n1 /= n1_len_sq;
		n2 /= n2_len_sq;
		Vec3 d0c = (x1x2.Dot(e) * n1 + x1x3.Dot(e) * n2) / e_len;
		Vec3 d2c = e_len * n1;
		Vec3 d3c = e_len * n2;

		// The sum of the gradients must be zero (see "Strain Based Dynamics" section 4)
		Vec3 d1c = -d0c - d2c - d3c;

		// Get masses
		float w0 = v0.mInvMass;
		float w1 = v1.mInvMass;
		float w2 = v2.mInvMass;
		float w3 = v3.mInvMass;

		// Calculate -lambda
		float denom = w0 * d0c.LengthSq() + w1 * d1c.LengthSq() + w2 * d2c.LengthSq() + w3 * d3c.LengthSq() + b->mCompliance * inv_dt_sq;
		if (denom < 1.0e-12f)
			continue;
		float minus_lambda = c / denom;

		// Apply correction
		v0.mPosition = x0 - minus_lambda * w0 * d0c;
		v1.mPosition = x1 - minus_lambda * w1 * d1c;
		v2.mPosition = x2 - minus_lambda * w2 * d2c;
		v3.mPosition = x3 - minus_lambda * w3 * d3c;
	}
}

void SoftBodyMotionProperties::ApplyVolumeConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex)
{
	JPH_PROFILE_FUNCTION();

	float inv_dt_sq = 1.0f / Square(inContext.mSubStepDeltaTime);

	// Satisfy volume constraints
	for (const Volume *v = mSettings->mVolumeConstraints.data() + inStartIndex, *v_end = mSettings->mVolumeConstraints.data() + inEndIndex; v < v_end; ++v)
	{
		Vertex &v1 = mVertices[v->mVertex[0]];
		Vertex &v2 = mVertices[v->mVertex[1]];
		Vertex &v3 = mVertices[v->mVertex[2]];
		Vertex &v4 = mVertices[v->mVertex[3]];

		Vec3 x1 = v1.mPosition;
		Vec3 x2 = v2.mPosition;
		Vec3 x3 = v3.mPosition;
		Vec3 x4 = v4.mPosition;

		// Calculate constraint equation
		Vec3 x1x2 = x2 - x1;
		Vec3 x1x3 = x3 - x1;
		Vec3 x1x4 = x4 - x1;
		float c = abs(x1x2.Cross(x1x3).Dot(x1x4)) - v->mSixRestVolume;

		// Calculate gradient of constraint equation
		Vec3 d1c = (x4 - x2).Cross(x3 - x2);
		Vec3 d2c = x1x3.Cross(x1x4);
		Vec3 d3c = x1x4.Cross(x1x2);
		Vec3 d4c = x1x2.Cross(x1x3);

		// Get masses
		float w1 = v1.mInvMass;
		float w2 = v2.mInvMass;
		float w3 = v3.mInvMass;
		float w4 = v4.mInvMass;

		// Calculate -lambda
		float denom = w1 * d1c.LengthSq() + w2 * d2c.LengthSq() + w3 * d3c.LengthSq() + w4 * d4c.LengthSq() + v->mCompliance * inv_dt_sq;
		if (denom < 1.0e-12f)
			continue;
		float minus_lambda = c / denom;

		// Apply correction
		v1.mPosition = x1 - minus_lambda * w1 * d1c;
		v2.mPosition = x2 - minus_lambda * w2 * d2c;
		v3.mPosition = x3 - minus_lambda * w3 * d3c;
		v4.mPosition = x4 - minus_lambda * w4 * d4c;
	}
}

void SoftBodyMotionProperties::ApplySkinConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex)
{
	// Early out if nothing to do
	if (mSettings->mSkinnedConstraints.empty() || !mEnableSkinConstraints)
		return;

	JPH_PROFILE_FUNCTION();

	// We're going to iterate multiple times over the skin constraints, update the skinned position accordingly.
	// If we don't do this, the simulation will see a big jump and the first iteration will cause a big velocity change in the system.
	float factor = mSkinStatePreviousPositionValid? inContext.mNextIteration.load(std::memory_order_relaxed) / float(mNumIterations) : 1.0f;
	float prev_factor = 1.0f - factor;

	// Apply the constraints
	Vertex *vertices = mVertices.data();
	const SkinState *skin_states = mSkinState.data();
	for (const Skinned *s = mSettings->mSkinnedConstraints.data() + inStartIndex, *s_end = mSettings->mSkinnedConstraints.data() + inEndIndex; s < s_end; ++s)
	{
		Vertex &vertex = vertices[s->mVertex];
		const SkinState &skin_state = skin_states[s->mVertex];
		float max_distance = s->mMaxDistance * mSkinnedMaxDistanceMultiplier;

		// Calculate the skinned position by interpolating from previous to current position
		Vec3 skin_pos = prev_factor * skin_state.mPreviousPosition + factor * skin_state.mPosition;

		if (max_distance > 0.0f)
		{
			// Move vertex if it violated the back stop
			if (s->mBackStopDistance < max_distance)
			{
				// Center of the back stop sphere
				Vec3 center = skin_pos - skin_state.mNormal * (s->mBackStopDistance + s->mBackStopRadius);

				// Check if we're inside the back stop sphere
				Vec3 delta = vertex.mPosition - center;
				float delta_len_sq = delta.LengthSq();
				if (delta_len_sq < Square(s->mBackStopRadius))
				{
					// Push the vertex to the surface of the back stop sphere
					float delta_len = sqrt(delta_len_sq);
					vertex.mPosition = delta_len > 0.0f?
						center + delta * (s->mBackStopRadius / delta_len)
						: center + skin_state.mNormal * s->mBackStopRadius;
				}
			}

			// Clamp vertex distance to max distance from skinned position
			if (max_distance < FLT_MAX)
			{
				Vec3 delta = vertex.mPosition - skin_pos;
				float delta_len_sq = delta.LengthSq();
				float max_distance_sq = Square(max_distance);
				if (delta_len_sq > max_distance_sq)
					vertex.mPosition = skin_pos + delta * sqrt(max_distance_sq / delta_len_sq);
			}
		}
		else
		{
			// Kinematic: Just update the vertex position
			vertex.mPosition = skin_pos;
		}
	}
}

void SoftBodyMotionProperties::ApplyEdgeConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex)
{
	JPH_PROFILE_FUNCTION();

	float inv_dt_sq = 1.0f / Square(inContext.mSubStepDeltaTime);

	// Satisfy edge constraints
	for (const Edge *e = mSettings->mEdgeConstraints.data() + inStartIndex, *e_end = mSettings->mEdgeConstraints.data() + inEndIndex; e < e_end; ++e)
	{
		Vertex &v0 = mVertices[e->mVertex[0]];
		Vertex &v1 = mVertices[e->mVertex[1]];

		// Get positions
		Vec3 x0 = v0.mPosition;
		Vec3 x1 = v1.mPosition;

		// Calculate current length
		Vec3 delta = x1 - x0;
		float length = delta.Length();

		// Apply correction
		float denom = length * (v0.mInvMass + v1.mInvMass + e->mCompliance * inv_dt_sq);
		if (denom < 1.0e-12f)
			continue;
		Vec3 correction = delta * (length - e->mRestLength) / denom;
		v0.mPosition = x0 + v0.mInvMass * correction;
		v1.mPosition = x1 - v1.mInvMass * correction;
	}
}

void SoftBodyMotionProperties::ApplyRodStretchShearConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex)
{
	JPH_PROFILE_FUNCTION();

	float inv_dt_sq = 1.0f / Square(inContext.mSubStepDeltaTime);

	RodState *rod_state = mRodStates.data() + inStartIndex;
	for (const RodStretchShear *r = mSettings->mRodStretchShearConstraints.data() + inStartIndex, *r_end = mSettings->mRodStretchShearConstraints.data() + inEndIndex; r < r_end; ++r, ++rod_state)
	{
		// Get positions
		Vertex &v0 = mVertices[r->mVertex[0]];
		Vertex &v1 = mVertices[r->mVertex[1]];

		// Apply stretch and shear constraint
		// Equation 37 from "Position and Orientation Based Cosserat Rods" - Kugelstadt and Schoemer - SIGGRAPH 2016
		float denom = v0.mInvMass + v1.mInvMass + 4.0f * r->mInvMass * Square(r->mLength) + r->mCompliance * inv_dt_sq;
		if (denom < 1.0e-12f)
			continue;
		Vec3 x0 = v0.mPosition;
		Vec3 x1 = v1.mPosition;
		Quat rotation = rod_state->mRotation;
		Vec3 d3 = rotation.RotateAxisZ();
		Vec3 delta = (x1 - x0 - d3 * r->mLength) / denom;
		v0.mPosition = x0 + v0.mInvMass * delta;
		v1.mPosition = x1 - v1.mInvMass * delta;
		// q * e3_bar = q * (0, 0, -1, 0) = [-qy, qx, -qw, qz]
		Quat q_e3_bar(rotation.GetXYZW().Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>().FlipSign<-1, 1, -1, 1>());
		rotation += (2.0f * r->mInvMass * r->mLength) * Quat::sMultiplyImaginary(delta, q_e3_bar);

		// Renormalize
		rod_state->mRotation = rotation.Normalized();
	}
}

void SoftBodyMotionProperties::ApplyRodBendTwistConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex)
{
	JPH_PROFILE_FUNCTION();

	float inv_dt_sq = 1.0f / Square(inContext.mSubStepDeltaTime);

	const Array<RodStretchShear> &rods = mSettings->mRodStretchShearConstraints;

	for (const RodBendTwist *r = mSettings->mRodBendTwistConstraints.data() + inStartIndex, *r_end = mSettings->mRodBendTwistConstraints.data() + inEndIndex; r < r_end; ++r)
	{
		uint32 rod1_index = r->mRod[0];
		uint32 rod2_index = r->mRod[1];
		const RodStretchShear &rod1 = rods[rod1_index];
		const RodStretchShear &rod2 = rods[rod2_index];
		RodState &rod1_state = mRodStates[rod1_index];
		RodState &rod2_state = mRodStates[rod2_index];

		// Apply bend and twist constraint
		// Equation 40 from "Position and Orientation Based Cosserat Rods" - Kugelstadt and Schoemer - SIGGRAPH 2016
		float denom = rod1.mInvMass + rod2.mInvMass + r->mCompliance * inv_dt_sq;
		if (denom < 1.0e-12f)
			continue;
		Quat rotation1 = rod1_state.mRotation;
		Quat rotation2 = rod2_state.mRotation;
		Quat omega = rotation1.Conjugated() * rotation2;
		Quat omega0 = r->mOmega0;
		Vec4 omega_min_omega0 = (omega - omega0).GetXYZW();
		Vec4 omega_plus_omega0 = (omega + omega0).GetXYZW();
		// Take the shortest of the two rotations
		Quat delta_omega(Vec4::sSelect(omega_min_omega0, omega_plus_omega0, Vec4::sLess(omega_plus_omega0.DotV(omega_plus_omega0), omega_min_omega0.DotV(omega_min_omega0))));
		delta_omega /= denom;
		delta_omega.SetW(0.0f); // Scalar part needs to be zero because the real part of the Darboux vector doesn't vanish, see text between eq. 39 and 40.
		Quat delta_rod2 = rod2.mInvMass * rotation1 * delta_omega;
		rotation1 += rod1.mInvMass * rotation2 * delta_omega;
		rotation2 -= delta_rod2;

		// Renormalize
		rod1_state.mRotation = rotation1.Normalized();
		rod2_state.mRotation = rotation2.Normalized();
	}
}

void SoftBodyMotionProperties::ApplyLRAConstraints(uint inStartIndex, uint inEndIndex)
{
	JPH_PROFILE_FUNCTION();

	// Satisfy LRA constraints
	Vertex *vertices = mVertices.data();
	for (const LRA *lra = mSettings->mLRAConstraints.data() + inStartIndex, *lra_end = mSettings->mLRAConstraints.data() + inEndIndex; lra < lra_end; ++lra)
	{
		JPH_ASSERT(lra->mVertex[0] < mVertices.size());
		JPH_ASSERT(lra->mVertex[1] < mVertices.size());
		const Vertex &vertex0 = vertices[lra->mVertex[0]];
		Vertex &vertex1 = vertices[lra->mVertex[1]];

		Vec3 x0 = vertex0.mPosition;
		Vec3 delta = vertex1.mPosition - x0;
		float delta_len_sq = delta.LengthSq();
		if (delta_len_sq > Square(lra->mMaxDistance))
			vertex1.mPosition = x0 + delta * lra->mMaxDistance / sqrt(delta_len_sq);
	}
}

void SoftBodyMotionProperties::ApplyCollisionConstraintsAndUpdateVelocities(const SoftBodyUpdateContext &inContext)
{
	JPH_PROFILE_FUNCTION();

	float dt = inContext.mSubStepDeltaTime;
	float restitution_threshold = -2.0f * inContext.mGravity.Length() * dt;
	float vertex_radius = mVertexRadius;
	for (Vertex &v : mVertices)
		if (v.mInvMass > 0.0f)
		{
			// Remember previous velocity for restitution calculations
			Vec3 prev_v = v.mVelocity;

			// XPBD velocity update
			v.mVelocity = (v.mPosition - v.mPreviousPosition) / dt;

			// Satisfy collision constraint
			if (v.mCollidingShapeIndex >= 0)
			{
				// Check if there is a collision
				float projected_distance = -v.mCollisionPlane.SignedDistance(v.mPosition) + vertex_radius;
				if (projected_distance > 0.0f)
				{
					// Remember that there was a collision
					v.mHasContact = true;

					// We need a contact callback if one of the vertices collided
					mNeedContactCallback.store(true, memory_order_relaxed);

					// Note that we already calculated the velocity, so this does not affect the velocity (next iteration starts by setting previous position to current position)
					CollidingShape &cs = mCollidingShapes[v.mCollidingShapeIndex];
					Vec3 contact_normal = v.mCollisionPlane.GetNormal();
					v.mPosition += contact_normal * projected_distance;

					// Apply friction as described in Detailed Rigid Body Simulation with Extended Position Based Dynamics - Matthias Muller et al.
					// See section 3.6:
					// Inverse mass: w1 = 1 / m1, w2 = 1 / m2 + (r2 x n)^T I^-1 (r2 x n) = 0 for a static object
					// r2 are the contact point relative to the center of mass of body 2
					// Lagrange multiplier for contact: lambda = -c / (w1 + w2)
					// Where c is the constraint equation (the distance to the plane, negative because penetrating)
					// Contact normal force: fn = lambda / dt^2
					// Delta velocity due to friction dv = -vt / |vt| * min(dt * friction * fn * (w1 + w2), |vt|) = -vt * min(-friction * c / (|vt| * dt), 1)
					// Note that I think there is an error in the paper, I added a mass term, see: https://github.com/matthias-research/pages/issues/29
					// Relative velocity: vr = v1 - v2 - omega2 x r2
					// Normal velocity: vn = vr . contact_normal
					// Tangential velocity: vt = vr - contact_normal * vn
					// Impulse: p = dv / (w1 + w2)
					// Changes in particle velocities:
					// v1 = v1 + p / m1
					// v2 = v2 - p / m2 (no change when colliding with a static body)
					// w2 = w2 - I^-1 (r2 x p) (no change when colliding with a static body)
					if (cs.mMotionType == EMotionType::Dynamic)
					{
						// Calculate normal and tangential velocity (equation 30)
						Vec3 r2 = v.mPosition - cs.mCenterOfMassTransform.GetTranslation();
						Vec3 v2 = cs.GetPointVelocity(r2);
						Vec3 relative_velocity = v.mVelocity - v2;
						Vec3 v_normal = contact_normal * contact_normal.Dot(relative_velocity);
						Vec3 v_tangential = relative_velocity - v_normal;
						float v_tangential_length = v_tangential.Length();

						// Calculate resulting inverse mass of vertex
						float vertex_inv_mass = cs.mSoftBodyInvMassScale * v.mInvMass;

						// Calculate inverse effective mass
						Vec3 r2_cross_n = r2.Cross(contact_normal);
						float w2 = cs.mInvMass + r2_cross_n.Dot(cs.mInvInertia * r2_cross_n);
						float w1_plus_w2 = vertex_inv_mass + w2;
						if (w1_plus_w2 > 0.0f)
						{
							// Calculate delta relative velocity due to friction (modified equation 31)
							Vec3 dv;
							if (v_tangential_length > 0.0f)
								dv = v_tangential * min(cs.mFriction * projected_distance / (v_tangential_length * dt), 1.0f);
							else
								dv = Vec3::sZero();

							// Calculate delta relative velocity due to restitution (equation 35)
							dv += v_normal;
							float prev_v_normal = (prev_v - v2).Dot(contact_normal);
							if (prev_v_normal < restitution_threshold)
								dv += cs.mRestitution * prev_v_normal * contact_normal;

							// Calculate impulse
							Vec3 p = dv / w1_plus_w2;

							// Apply impulse to particle
							v.mVelocity -= p * vertex_inv_mass;

							// Apply impulse to rigid body
							cs.mLinearVelocity += p * cs.mInvMass;
							cs.mAngularVelocity += cs.mInvInertia * r2.Cross(p);

							// Mark that the velocities of the body we hit need to be updated
							cs.mUpdateVelocities = true;
						}
					}
					else if (cs.mSoftBodyInvMassScale > 0.0f)
					{
						// Body is not movable, equations are simpler

						// Calculate normal and tangential velocity (equation 30)
						Vec3 v_normal = contact_normal * contact_normal.Dot(v.mVelocity);
						Vec3 v_tangential = v.mVelocity - v_normal;
						float v_tangential_length = v_tangential.Length();

						// Apply friction (modified equation 31)
						if (v_tangential_length > 0.0f)
							v.mVelocity -= v_tangential * min(cs.mFriction * projected_distance / (v_tangential_length * dt), 1.0f);

						// Apply restitution (equation 35)
						v.mVelocity -= v_normal;
						float prev_v_normal = prev_v.Dot(contact_normal);
						if (prev_v_normal < restitution_threshold)
							v.mVelocity -= cs.mRestitution * prev_v_normal * contact_normal;
					}
				}
			}
		}

	// Calculate the new angular velocity for all rods
	float two_div_dt = 2.0f / dt;
	for (RodState &r : mRodStates)
		r.mAngularVelocity = two_div_dt * (r.mRotation * r.mPreviousRotationInternal.Conjugated()).GetXYZ(); // Overwrites mPreviousRotationInternal
}

void SoftBodyMotionProperties::UpdateSoftBodyState(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings)
{
	JPH_PROFILE_FUNCTION();

	// Contact callback
	if (mNeedContactCallback.load(memory_order_relaxed) && ioContext.mContactListener != nullptr)
	{
		// Remove non-colliding sensors from the list
		for (int i = int(mCollidingSensors.size()) - 1; i >= 0; --i)
			if (!mCollidingSensors[i].mHasContact)
			{
				mCollidingSensors[i] = std::move(mCollidingSensors.back());
				mCollidingSensors.pop_back();
			}

		ioContext.mContactListener->OnSoftBodyContactAdded(*ioContext.mBody, SoftBodyManifold(this));
	}

	// Loop through vertices once more to update the global state
	float dt = ioContext.mDeltaTime;
	float max_linear_velocity_sq = Square(GetMaxLinearVelocity());
	float max_v_sq = 0.0f;
	Vec3 linear_velocity = Vec3::sZero(), angular_velocity = Vec3::sZero();
	mLocalPredictedBounds = mLocalBounds = { };
	for (Vertex &v : mVertices)
	{
		// Calculate max square velocity
		float v_sq = v.mVelocity.LengthSq();
		max_v_sq = max(max_v_sq, v_sq);

		// Clamp if velocity is too high
		if (v_sq > max_linear_velocity_sq)
			v.mVelocity *= sqrt(max_linear_velocity_sq / v_sq);

		// Calculate local linear/angular velocity
		linear_velocity += v.mVelocity;
		angular_velocity += v.mPosition.Cross(v.mVelocity);

		// Update local bounding box
		mLocalBounds.Encapsulate(v.mPosition);

		// Create predicted position for the next frame in order to detect collisions before they happen
		mLocalPredictedBounds.Encapsulate(v.mPosition + v.mVelocity * dt + ioContext.mDisplacementDueToGravity);

		// Reset collision data for the next iteration
		v.ResetCollision();
	}

	// Calculate linear/angular velocity of the body by averaging all vertices and bringing the value to world space
	float num_vertices_divider = float(max(int(mVertices.size()), 1));
	SetLinearVelocityClamped(ioContext.mCenterOfMassTransform.Multiply3x3(linear_velocity / num_vertices_divider));
	SetAngularVelocity(ioContext.mCenterOfMassTransform.Multiply3x3(angular_velocity / num_vertices_divider));

	if (mUpdatePosition)
	{
		// Shift the body so that the position is the center of the local bounds
		Vec3 delta = mLocalBounds.GetCenter();
		ioContext.mDeltaPosition = ioContext.mCenterOfMassTransform.Multiply3x3(delta);
		for (Vertex &v : mVertices)
			v.mPosition -= delta;

		// Update the skin state too since we will use this position as the previous position in the next update
		for (SkinState &s : mSkinState)
			s.mPosition -= delta;
		JPH_IF_DEBUG_RENDERER(mSkinStateTransform.SetTranslation(mSkinStateTransform.GetTranslation() + ioContext.mDeltaPosition);)

		// Offset bounds to match new position
		mLocalBounds.Translate(-delta);
		mLocalPredictedBounds.Translate(-delta);
	}
	else
		ioContext.mDeltaPosition = Vec3::sZero();

	// Test if we should go to sleep
	if (GetAllowSleeping())
	{
		if (max_v_sq > inPhysicsSettings.mPointVelocitySleepThreshold)
		{
			ResetSleepTestTimer();
			ioContext.mCanSleep = ECanSleep::CannotSleep;
		}
		else
			ioContext.mCanSleep = AccumulateSleepTime(dt, inPhysicsSettings.mTimeBeforeSleep);
	}
	else
		ioContext.mCanSleep = ECanSleep::CannotSleep;

	// If SkinVertices is not called after this then don't use the previous position as the skin is static
	mSkinStatePreviousPositionValid = false;

	// Reset force accumulator
	ResetForce();
}

void SoftBodyMotionProperties::UpdateRigidBodyVelocities(const SoftBodyUpdateContext &inContext, BodyInterface &inBodyInterface)
{
	JPH_PROFILE_FUNCTION();

	// Write back velocity deltas
	for (const CollidingShape &cs : mCollidingShapes)
		if (cs.mUpdateVelocities)
			inBodyInterface.AddLinearAndAngularVelocity(cs.mBodyID, inContext.mCenterOfMassTransform.Multiply3x3(cs.mLinearVelocity - cs.mOriginalLinearVelocity), inContext.mCenterOfMassTransform.Multiply3x3(cs.mAngularVelocity - cs.mOriginalAngularVelocity));

	// Clear colliding shapes/sensors to avoid hanging on to references to shapes
	mCollidingShapes.clear();
	mCollidingSensors.clear();
}

void SoftBodyMotionProperties::InitializeUpdateContext(float inDeltaTime, Body &inSoftBody, const PhysicsSystem &inSystem, SoftBodyUpdateContext &ioContext)
{
	JPH_PROFILE_FUNCTION();

	// Store body
	ioContext.mBody = &inSoftBody;
	ioContext.mMotionProperties = this;
	ioContext.mContactListener = inSystem.GetSoftBodyContactListener();
	ioContext.mSimShapeFilter = inSystem.GetSimShapeFilter();

	// Convert gravity to local space
	ioContext.mCenterOfMassTransform = inSoftBody.GetCenterOfMassTransform();
	ioContext.mGravity = ioContext.mCenterOfMassTransform.Multiply3x3Transposed(GetGravityFactor() * inSystem.GetGravity());

	// Calculate delta time for sub step
	ioContext.mDeltaTime = inDeltaTime;
	ioContext.mSubStepDeltaTime = inDeltaTime / mNumIterations;

	// Calculate total displacement we'll have due to gravity over all sub steps
	// The total displacement as produced by our integrator can be written as: Sum(i * g * dt^2, i = 0..mNumIterations).
	// This is bigger than 0.5 * g * dt^2 because we first increment the velocity and then update the position
	// Using Sum(i, i = 0..n) = n * (n + 1) / 2 we can write this as:
	ioContext.mDisplacementDueToGravity = (0.5f * mNumIterations * (mNumIterations + 1) * Square(ioContext.mSubStepDeltaTime)) * ioContext.mGravity;
}

void SoftBodyMotionProperties::StartNextIteration(const SoftBodyUpdateContext &ioContext)
{
	ApplyPressure(ioContext);

	IntegratePositions(ioContext);
}

void SoftBodyMotionProperties::StartFirstIteration(SoftBodyUpdateContext &ioContext)
{
	// Start the first iteration
	JPH_IF_ENABLE_ASSERTS(uint iteration =) ioContext.mNextIteration.fetch_add(1, memory_order_relaxed);
	JPH_ASSERT(iteration == 0);
	StartNextIteration(ioContext);
	ioContext.mState.store(SoftBodyUpdateContext::EState::ApplyConstraints, memory_order_release);
}

SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelDetermineCollisionPlanes(SoftBodyUpdateContext &ioContext)
{
	// Do a relaxed read first to see if there is any work to do (this prevents us from doing expensive atomic operations and also prevents us from continuously incrementing the counter and overflowing it)
	uint num_vertices = (uint)mVertices.size();
	if (ioContext.mNextCollisionVertex.load(memory_order_relaxed) < num_vertices)
	{
		// Fetch next batch of vertices to process
		uint next_vertex = ioContext.mNextCollisionVertex.fetch_add(SoftBodyUpdateContext::cVertexCollisionBatch, memory_order_acquire);
		if (next_vertex < num_vertices)
		{
			// Process collision planes
			uint num_vertices_to_process = min(SoftBodyUpdateContext::cVertexCollisionBatch, num_vertices - next_vertex);
			DetermineCollisionPlanes(next_vertex, num_vertices_to_process);
			uint vertices_processed = ioContext.mNumCollisionVerticesProcessed.fetch_add(SoftBodyUpdateContext::cVertexCollisionBatch, memory_order_acq_rel) + num_vertices_to_process;
			if (vertices_processed >= num_vertices)
			{
				// Determine next state
				if (mCollidingSensors.empty())
					StartFirstIteration(ioContext);
				else
					ioContext.mState.store(SoftBodyUpdateContext::EState::DetermineSensorCollisions, memory_order_release);
			}
			return EStatus::DidWork;
		}
	}

	return EStatus::NoWork;
}

SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelDetermineSensorCollisions(SoftBodyUpdateContext &ioContext)
{
	// Do a relaxed read to see if there are more sensors to process
	if (ioContext.mNextSensorIndex.load(memory_order_relaxed) < mNumSensors)
	{
		// Fetch next sensor to process
		uint sensor_index = ioContext.mNextSensorIndex.fetch_add(1, memory_order_acquire);
		if (sensor_index < mNumSensors)
		{
			// Process this sensor
			DetermineSensorCollisions(mCollidingSensors[sensor_index]);

			// Determine next state
			uint sensors_processed = ioContext.mNumSensorsProcessed.fetch_add(1, memory_order_acq_rel) + 1;
			if (sensors_processed >= mNumSensors)
				StartFirstIteration(ioContext);
			return EStatus::DidWork;
		}
	}

	return EStatus::NoWork;
}

void SoftBodyMotionProperties::ProcessGroup(const SoftBodyUpdateContext &ioContext, uint inGroupIndex)
{
	// Determine start and end
	SoftBodySharedSettings::UpdateGroup start { 0, 0, 0, 0, 0, 0, 0 };
	const SoftBodySharedSettings::UpdateGroup &prev = inGroupIndex > 0? mSettings->mUpdateGroups[inGroupIndex - 1] : start;
	const SoftBodySharedSettings::UpdateGroup &current = mSettings->mUpdateGroups[inGroupIndex];

	// Process volume constraints
	ApplyVolumeConstraints(ioContext, prev.mVolumeEndIndex, current.mVolumeEndIndex);

	// Process bend constraints
	ApplyDihedralBendConstraints(ioContext, prev.mDihedralBendEndIndex, current.mDihedralBendEndIndex);

	// Process skinned constraints
	ApplySkinConstraints(ioContext, prev.mSkinnedEndIndex, current.mSkinnedEndIndex);

	// Process edges
	ApplyEdgeConstraints(ioContext, prev.mEdgeEndIndex, current.mEdgeEndIndex);

	// Process rods
	ApplyRodStretchShearConstraints(ioContext, prev.mRodStretchShearEndIndex, current.mRodStretchShearEndIndex);
	ApplyRodBendTwistConstraints(ioContext, prev.mRodBendTwistEndIndex, current.mRodBendTwistEndIndex);

	// Process LRA constraints
	ApplyLRAConstraints(prev.mLRAEndIndex, current.mLRAEndIndex);
}

SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelApplyConstraints(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings)
{
	uint num_groups = (uint)mSettings->mUpdateGroups.size();
	JPH_ASSERT(num_groups > 0, "SoftBodySharedSettings::Optimize should have been called!");
	--num_groups; // Last group is the non-parallel group, we don't want to execute it in parallel

	// Do a relaxed read first to see if there is any work to do (this prevents us from doing expensive atomic operations and also prevents us from continuously incrementing the counter and overflowing it)
	uint next_group = ioContext.mNextConstraintGroup.load(memory_order_relaxed);
	if (next_group < num_groups || (num_groups == 0 && next_group == 0))
	{
		// Fetch the next group process
		next_group = ioContext.mNextConstraintGroup.fetch_add(1, memory_order_acquire);
		if (next_group < num_groups || (num_groups == 0 && next_group == 0))
		{
			uint num_groups_processed = 0;
			if (num_groups > 0)
			{
				// Process this group
				ProcessGroup(ioContext, next_group);

				// Increment total number of groups processed
				num_groups_processed = ioContext.mNumConstraintGroupsProcessed.fetch_add(1, memory_order_acq_rel) + 1;
			}

			if (num_groups_processed >= num_groups)
			{
				// Finish the iteration
				JPH_PROFILE("FinishIteration");

				// Process non-parallel group
				ProcessGroup(ioContext, num_groups);

				ApplyCollisionConstraintsAndUpdateVelocities(ioContext);

				uint iteration = ioContext.mNextIteration.fetch_add(1, memory_order_relaxed);
				if (iteration < mNumIterations)
				{
					// Start a new iteration
					StartNextIteration(ioContext);

					// Reset group logic
					ioContext.mNumConstraintGroupsProcessed.store(0, memory_order_release);
					ioContext.mNextConstraintGroup.store(0, memory_order_release);
				}
				else
				{
					// On final iteration we update the state
					UpdateSoftBodyState(ioContext, inPhysicsSettings);

					ioContext.mState.store(SoftBodyUpdateContext::EState::Done, memory_order_release);
					return EStatus::Done;
				}
			}

			return EStatus::DidWork;
		}
	}
	return EStatus::NoWork;
}

SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelUpdate(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings)
{
	switch (ioContext.mState.load(memory_order_acquire))
	{
	case SoftBodyUpdateContext::EState::DetermineCollisionPlanes:
		return ParallelDetermineCollisionPlanes(ioContext);

	case SoftBodyUpdateContext::EState::DetermineSensorCollisions:
		return ParallelDetermineSensorCollisions(ioContext);

	case SoftBodyUpdateContext::EState::ApplyConstraints:
		return ParallelApplyConstraints(ioContext, inPhysicsSettings);

	case SoftBodyUpdateContext::EState::Done:
		return EStatus::Done;

	default:
		JPH_ASSERT(false);
		return EStatus::NoWork;
	}
}

void SoftBodyMotionProperties::SkinVertices([[maybe_unused]] RMat44Arg inCenterOfMassTransform, const Mat44 *inJointMatrices, [[maybe_unused]] uint inNumJoints, bool inHardSkinAll, TempAllocator &ioTempAllocator)
{
	// Calculate the skin matrices
	uint num_skin_matrices = uint(mSettings->mInvBindMatrices.size());
	uint skin_matrices_size = num_skin_matrices * sizeof(Mat44);
	Mat44 *skin_matrices = (Mat44 *)ioTempAllocator.Allocate(skin_matrices_size);
	JPH_SCOPE_EXIT([&ioTempAllocator, skin_matrices, skin_matrices_size]{ ioTempAllocator.Free(skin_matrices, skin_matrices_size); });
	const Mat44 *skin_matrices_end = skin_matrices + num_skin_matrices;
	const InvBind *inv_bind_matrix = mSettings->mInvBindMatrices.data();
	for (Mat44 *s = skin_matrices; s < skin_matrices_end; ++s, ++inv_bind_matrix)
	{
		JPH_ASSERT(inv_bind_matrix->mJointIndex < inNumJoints);
		*s = inJointMatrices[inv_bind_matrix->mJointIndex] * inv_bind_matrix->mInvBind;
	}

	// Skin the vertices
	JPH_IF_DEBUG_RENDERER(mSkinStateTransform = inCenterOfMassTransform;)
	JPH_IF_ENABLE_ASSERTS(uint num_vertices = uint(mSettings->mVertices.size());)
	JPH_ASSERT(mSkinState.size() == num_vertices);
	const SoftBodySharedSettings::Vertex *in_vertices = mSettings->mVertices.data();
	for (const Skinned &s : mSettings->mSkinnedConstraints)
	{
		// Get bind pose
		JPH_ASSERT(s.mVertex < num_vertices);
		Vec3 bind_pos = Vec3::sLoadFloat3Unsafe(in_vertices[s.mVertex].mPosition);

		// Skin vertex
		Vec3 pos = Vec3::sZero();
		for (const SkinWeight &w : s.mWeights)
		{
			// We assume that the first zero weight is the end of the list
			if (w.mWeight == 0.0f)
				break;

			JPH_ASSERT(w.mInvBindIndex < num_skin_matrices);
			pos += w.mWeight * (skin_matrices[w.mInvBindIndex] * bind_pos);
		}
		SkinState &skin_state = mSkinState[s.mVertex];
		skin_state.mPreviousPosition = skin_state.mPosition;
		skin_state.mPosition = pos;
	}

	// Calculate the normals
	for (const Skinned &s : mSettings->mSkinnedConstraints)
	{
		Vec3 normal = Vec3::sZero();
		uint32 num_faces = s.mNormalInfo >> 24;
		if (num_faces > 0)
		{
			// Calculate normal
			const uint32 *f = &mSettings->mSkinnedConstraintNormals[s.mNormalInfo & 0xffffff];
			const uint32 *f_end = f + num_faces;
			while (f < f_end)
			{
				const Face &face = mSettings->mFaces[*f];
				Vec3 v0 = mSkinState[face.mVertex[0]].mPosition;
				Vec3 v1 = mSkinState[face.mVertex[1]].mPosition;
				Vec3 v2 = mSkinState[face.mVertex[2]].mPosition;
				normal += (v1 - v0).Cross(v2 - v0).NormalizedOr(Vec3::sZero());
				++f;
			}
			normal = normal.NormalizedOr(Vec3::sZero());
		}
		mSkinState[s.mVertex].mNormal = normal;
	}

	if (inHardSkinAll)
	{
		// Hard skin all vertices and reset their velocities
		for (const Skinned &s : mSettings->mSkinnedConstraints)
		{
			Vertex &vertex = mVertices[s.mVertex];
			SkinState &skin_state = mSkinState[s.mVertex];
			skin_state.mPreviousPosition = skin_state.mPosition;
			vertex.mPosition = skin_state.mPosition;
			vertex.mVelocity = Vec3::sZero();
		}
	}
	else if (!mEnableSkinConstraints)
	{
		// Hard skin only the kinematic vertices as we will not solve the skin constraints later
		for (const Skinned &s : mSettings->mSkinnedConstraints)
			if (s.mMaxDistance == 0.0f)
			{
				Vertex &vertex = mVertices[s.mVertex];
				vertex.mPosition = mSkinState[s.mVertex].mPosition;
			}
	}

	// Indicate that the previous positions are valid for the coming update
	mSkinStatePreviousPositionValid = true;
}

void SoftBodyMotionProperties::CustomUpdate(float inDeltaTime, Body &ioSoftBody, PhysicsSystem &inSystem)
{
	JPH_PROFILE_FUNCTION();

	// Create update context
	SoftBodyUpdateContext context;
	InitializeUpdateContext(inDeltaTime, ioSoftBody, inSystem, context);

	// Determine bodies we're colliding with
	DetermineCollidingShapes(context, inSystem, inSystem.GetBodyLockInterface());

	// Call the internal update until it finishes
	EStatus status;
	const PhysicsSettings &settings = inSystem.GetPhysicsSettings();
	while ((status = ParallelUpdate(context, settings)) == EStatus::DidWork)
		continue;
	JPH_ASSERT(status == EStatus::Done);

	// Update the state of the bodies we've collided with
	UpdateRigidBodyVelocities(context, inSystem.GetBodyInterface());

	// Update position of the soft body
	if (mUpdatePosition)
		inSystem.GetBodyInterface().SetPosition(ioSoftBody.GetID(), ioSoftBody.GetPosition() + context.mDeltaPosition, EActivation::DontActivate);
}

#ifdef JPH_DEBUG_RENDERER

void SoftBodyMotionProperties::DrawVertices(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const
{
	for (const Vertex &v : mVertices)
		inRenderer->DrawMarker(inCenterOfMassTransform * v.mPosition, v.mInvMass > 0.0f? Color::sGreen : Color::sRed, 0.05f);
}

void SoftBodyMotionProperties::DrawVertexVelocities(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const
{
	for (const Vertex &v : mVertices)
		inRenderer->DrawArrow(inCenterOfMassTransform * v.mPosition, inCenterOfMassTransform * (v.mPosition + v.mVelocity), Color::sYellow, 0.01f);
}

template <typename GetEndIndex, typename DrawConstraint>
inline void SoftBodyMotionProperties::DrawConstraints(ESoftBodyConstraintColor inConstraintColor, const GetEndIndex &inGetEndIndex, const DrawConstraint &inDrawConstraint, ColorArg inBaseColor) const
{
	uint start = 0;
	for (uint i = 0; i < (uint)mSettings->mUpdateGroups.size(); ++i)
	{
		uint end = inGetEndIndex(mSettings->mUpdateGroups[i]);

		Color base_color;
		if (inConstraintColor != ESoftBodyConstraintColor::ConstraintType)
			base_color = Color::sGetDistinctColor((uint)mSettings->mUpdateGroups.size() - i - 1); // Ensure that color 0 is always the last group
		else
			base_color = inBaseColor;

		for (uint idx = start; idx < end; ++idx)
		{
			Color color = inConstraintColor == ESoftBodyConstraintColor::ConstraintOrder? base_color * (float(idx - start) / (end - start)) : base_color;
			inDrawConstraint(idx, color);
		}

		start = end;
	}
}

void SoftBodyMotionProperties::DrawEdgeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const
{
	DrawConstraints(inConstraintColor,
		[](const SoftBodySharedSettings::UpdateGroup &inGroup) {
			return inGroup.mEdgeEndIndex;
		},
		[this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) {
			const Edge &e = mSettings->mEdgeConstraints[inIndex];
			inRenderer->DrawLine(inCenterOfMassTransform * mVertices[e.mVertex[0]].mPosition, inCenterOfMassTransform * mVertices[e.mVertex[1]].mPosition, inColor);
		},
		Color::sWhite);
}

void SoftBodyMotionProperties::DrawRods(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const
{
	DrawConstraints(inConstraintColor,
		[](const SoftBodySharedSettings::UpdateGroup &inGroup) {
			return inGroup.mRodStretchShearEndIndex;
		},
		[this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) {
			const RodStretchShear &r = mSettings->mRodStretchShearConstraints[inIndex];
			inRenderer->DrawLine(inCenterOfMassTransform * mVertices[r.mVertex[0]].mPosition, inCenterOfMassTransform * mVertices[r.mVertex[1]].mPosition, inColor);
		},
		Color::sWhite);
}

void SoftBodyMotionProperties::DrawRodStates(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const
{
	DrawConstraints(inConstraintColor,
		[](const SoftBodySharedSettings::UpdateGroup &inGroup) {
			return inGroup.mRodStretchShearEndIndex;
		},
		[this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) {
			const RodState &state = mRodStates[inIndex];
			const RodStretchShear &rod = mSettings->mRodStretchShearConstraints[inIndex];

			RVec3 x0 = inCenterOfMassTransform * mVertices[rod.mVertex[0]].mPosition;
			RVec3 x1 = inCenterOfMassTransform * mVertices[rod.mVertex[1]].mPosition;

			RMat44 rod_center = inCenterOfMassTransform;
			rod_center.SetTranslation(0.5_r * (x0 + x1));
			inRenderer->DrawArrow(rod_center.GetTranslation(), rod_center.GetTranslation() + state.mAngularVelocity, inColor, 0.01f * rod.mLength);

			RMat44 rod_frame = rod_center * RMat44::sRotation(state.mRotation);
			inRenderer->DrawCoordinateSystem(rod_frame, 0.3f * rod.mLength);
		},
		Color::sOrange);
}

void SoftBodyMotionProperties::DrawRodBendTwistConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const
{
	DrawConstraints(inConstraintColor,
		[](const SoftBodySharedSettings::UpdateGroup &inGroup) {
			return inGroup.mRodBendTwistEndIndex;
		},
		[this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) {
			uint r1 = mSettings->mRodBendTwistConstraints[inIndex].mRod[0];
			uint r2 = mSettings->mRodBendTwistConstraints[inIndex].mRod[1];
			const RodStretchShear &rod1 = mSettings->mRodStretchShearConstraints[r1];
			const RodStretchShear &rod2 = mSettings->mRodStretchShearConstraints[r2];

			RVec3 x0 = inCenterOfMassTransform * (0.4f * mVertices[rod1.mVertex[0]].mPosition + 0.6f * mVertices[rod1.mVertex[1]].mPosition);
			RVec3 x1 = inCenterOfMassTransform * (0.6f * mVertices[rod2.mVertex[0]].mPosition + 0.4f * mVertices[rod2.mVertex[1]].mPosition);

			inRenderer->DrawLine(x0, x1, inColor);
		},
		Color::sGreen);
}

void SoftBodyMotionProperties::DrawBendConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const
{
	DrawConstraints(inConstraintColor,
		[](const SoftBodySharedSettings::UpdateGroup &inGroup) {
			return inGroup.mDihedralBendEndIndex;
		},
		[this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) {
			const DihedralBend &b = mSettings->mDihedralBendConstraints[inIndex];

			RVec3 x0 = inCenterOfMassTransform * mVertices[b.mVertex[0]].mPosition;
			RVec3 x1 = inCenterOfMassTransform * mVertices[b.mVertex[1]].mPosition;
			RVec3 x2 = inCenterOfMassTransform * mVertices[b.mVertex[2]].mPosition;
			RVec3 x3 = inCenterOfMassTransform * mVertices[b.mVertex[3]].mPosition;
			RVec3 c_edge = 0.5_r * (x0 + x1);
			RVec3 c0 = (x0 + x1 + x2) / 3.0_r;
			RVec3 c1 = (x0 + x1 + x3) / 3.0_r;

			inRenderer->DrawArrow(0.9_r * x0 + 0.1_r * x1, 0.1_r * x0 + 0.9_r * x1, inColor, 0.01f);
			inRenderer->DrawLine(c_edge, 0.1_r * c_edge + 0.9_r * c0, inColor);
			inRenderer->DrawLine(c_edge, 0.1_r * c_edge + 0.9_r * c1, inColor);
		},
		Color::sGreen);
}

void SoftBodyMotionProperties::DrawVolumeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const
{
	DrawConstraints(inConstraintColor,
		[](const SoftBodySharedSettings::UpdateGroup &inGroup) {
			return inGroup.mVolumeEndIndex;
		},
		[this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) {
			const Volume &v = mSettings->mVolumeConstraints[inIndex];

			RVec3 x1 = inCenterOfMassTransform * mVertices[v.mVertex[0]].mPosition;
			RVec3 x2 = inCenterOfMassTransform * mVertices[v.mVertex[1]].mPosition;
			RVec3 x3 = inCenterOfMassTransform * mVertices[v.mVertex[2]].mPosition;
			RVec3 x4 = inCenterOfMassTransform * mVertices[v.mVertex[3]].mPosition;

			inRenderer->DrawTriangle(x1, x3, x2, inColor, DebugRenderer::ECastShadow::On);
			inRenderer->DrawTriangle(x2, x3, x4, inColor, DebugRenderer::ECastShadow::On);
			inRenderer->DrawTriangle(x1, x4, x3, inColor, DebugRenderer::ECastShadow::On);
			inRenderer->DrawTriangle(x1, x2, x4, inColor, DebugRenderer::ECastShadow::On);
		},
		Color::sYellow);
}

void SoftBodyMotionProperties::DrawSkinConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const
{
	DrawConstraints(inConstraintColor,
		[](const SoftBodySharedSettings::UpdateGroup &inGroup) {
			return inGroup.mSkinnedEndIndex;
		},
		[this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) {
			const Skinned &s = mSettings->mSkinnedConstraints[inIndex];
			const SkinState &skin_state = mSkinState[s.mVertex];
			inRenderer->DrawArrow(mSkinStateTransform * skin_state.mPosition, mSkinStateTransform * (skin_state.mPosition + 0.1f * skin_state.mNormal), inColor, 0.01f);
			inRenderer->DrawLine(mSkinStateTransform * skin_state.mPosition, inCenterOfMassTransform * mVertices[s.mVertex].mPosition, Color::sBlue);
		},
		Color::sOrange);
}

void SoftBodyMotionProperties::DrawLRAConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const
{
	DrawConstraints(inConstraintColor,
		[](const SoftBodySharedSettings::UpdateGroup &inGroup) {
			return inGroup.mLRAEndIndex;
		},
		[this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) {
			const LRA &l = mSettings->mLRAConstraints[inIndex];
			inRenderer->DrawLine(inCenterOfMassTransform * mVertices[l.mVertex[0]].mPosition, inCenterOfMassTransform * mVertices[l.mVertex[1]].mPosition, inColor);
		},
		Color::sGrey);
}

void SoftBodyMotionProperties::DrawPredictedBounds(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const
{
	inRenderer->DrawWireBox(inCenterOfMassTransform, mLocalPredictedBounds, Color::sRed);
}

#endif // JPH_DEBUG_RENDERER

void SoftBodyMotionProperties::SaveState(StateRecorder &inStream) const
{
	MotionProperties::SaveState(inStream);

	for (const Vertex &v : mVertices)
	{
		inStream.Write(v.mPosition);
		inStream.Write(v.mVelocity);
	}

	for (const RodState &r : mRodStates)
	{
		inStream.Write(r.mRotation);
		inStream.Write(r.mAngularVelocity);
	}

	for (const SkinState &s : mSkinState)
	{
		inStream.Write(s.mPreviousPosition);
		inStream.Write(s.mPosition);
		inStream.Write(s.mNormal);
	}

	inStream.Write(mLocalBounds.mMin);
	inStream.Write(mLocalBounds.mMax);
	inStream.Write(mLocalPredictedBounds.mMin);
	inStream.Write(mLocalPredictedBounds.mMax);
}

void SoftBodyMotionProperties::RestoreState(StateRecorder &inStream)
{
	MotionProperties::RestoreState(inStream);

	for (Vertex &v : mVertices)
	{
		inStream.Read(v.mPosition);
		inStream.Read(v.mVelocity);
	}

	for (RodState &r : mRodStates)
	{
		inStream.Read(r.mRotation);
		inStream.Read(r.mAngularVelocity);
	}

	for (SkinState &s : mSkinState)
	{
		inStream.Read(s.mPreviousPosition);
		inStream.Read(s.mPosition);
		inStream.Read(s.mNormal);
	}

	inStream.Read(mLocalBounds.mMin);
	inStream.Read(mLocalBounds.mMax);
	inStream.Read(mLocalPredictedBounds.mMin);
	inStream.Read(mLocalPredictedBounds.mMax);
}

JPH_NAMESPACE_END