Added support for Cosserat rod constraints in soft bodies (#1624)

This is a stick constraint with an orientation that can be used to attach geometry. Can be used e.g. to simulate vegetation in a cheap way.

Docs/ReleaseNotes.md

@@ -6,11 +6,13 @@ For breaking API changes see [this document](https://github.com/jrouwe/JoltPhysi
 
 ### New functionality
 
+* Added Cosserat rods to soft bodies. This is an edge with an orientation that can be used to orient geometry, e.g. a plant leaf. See the new SoftBodyCosseratRodConstraintTest demo.
 * Added ability to drive hinge constraints with `Ragdoll::DriveToPoseUsingMotors`. This also adds `HingeConstraint::SetTargetOrientationBS` which sets the target angle in body space.
 * Added `JPH_USE_EXTERNAL_PROFILE` cmake option that allows overriding the behavior of the profile macros.
 
 ### Bug Fixes
 
+* The remap tables in SoftBodySharedSettings::OptimizationResults mapped from new to old index instead of from old to new as was documented. The maps now behave as documented.
 * Fixed infinite recursion when colliding a `TriangleShape` vs a `TriangleShape`.
 * 32-bit MinGW g++ doesn't call the correct overload for the new operator when a type is 16 bytes aligned. This could cause unaligned read access violations.
 * Fixed compiling in double precision and fixed issues with floating point contraction that caused unit test failures on LoongArch architecture.

Jolt/Math/Vec4.h

@@ -139,6 +139,9 @@ public:
 	/// Test if two vectors are close
 	JPH_INLINE bool				IsClose(Vec4Arg inV2, float inMaxDistSq = 1.0e-12f) const;
 
+	/// Test if vector is near zero
+	JPH_INLINE bool				IsNearZero(float inMaxDistSq = 1.0e-12f) const;
+
 	/// Test if vector is normalized
 	JPH_INLINE bool				IsNormalized(float inTolerance = 1.0e-6f) const;
 

Jolt/Math/Vec4.inl

@@ -364,6 +364,11 @@ bool Vec4::IsClose(Vec4Arg inV2, float inMaxDistSq) const
 	return (inV2 - *this).LengthSq() <= inMaxDistSq;
 }
 
+bool Vec4::IsNearZero(float inMaxDistSq) const
+{
+	return LengthSq() <= inMaxDistSq;
+}
+
 bool Vec4::IsNormalized(float inTolerance) const
 {
 	return abs(LengthSq() - 1.0f) <= inTolerance;

Jolt/Physics/Body/BodyManager.cpp

@@ -1092,6 +1092,15 @@ void BodyManager::Draw(const DrawSettings &inDrawSettings, const PhysicsSettings
 				if (inDrawSettings.mDrawSoftBodyEdgeConstraints)
 					mp->DrawEdgeConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor);
 
+				if (inDrawSettings.mDrawSoftBodyRods)
+					mp->DrawRods(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor);
+
+				if (inDrawSettings.mDrawSoftBodyRodStates)
+					mp->DrawRodStates(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor);
+
+				if (inDrawSettings.mDrawSoftBodyRodBendTwistConstraints)
+					mp->DrawRodBendTwistConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor);
+
 				if (inDrawSettings.mDrawSoftBodyBendConstraints)
 					mp->DrawBendConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor);
 

Jolt/Physics/Body/BodyManager.h

@@ -248,6 +248,9 @@ public:
 		bool						mDrawSoftBodyVolumeConstraints = false;			///< Draw the volume constraints of soft bodies
 		bool						mDrawSoftBodySkinConstraints = false;			///< Draw the skin constraints of soft bodies
 		bool						mDrawSoftBodyLRAConstraints = false;			///< Draw the LRA constraints of soft bodies
+		bool						mDrawSoftBodyRods = false;						///< Draw the rods of soft bodies
+		bool						mDrawSoftBodyRodStates = false;					///< Draw the rod states (orientation and angular velocity) of soft bodies
+		bool						mDrawSoftBodyRodBendTwistConstraints = false;	///< Draw the rod bend twist constraints of soft bodies
 		bool						mDrawSoftBodyPredictedBounds = false;			///< Draw the predicted bounds of soft bodies
 		ESoftBodyConstraintColor	mDrawSoftBodyConstraintColor = ESoftBodyConstraintColor::ConstraintType; ///< Coloring scheme to use for soft body constraints
 	};

Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp

@@ -78,6 +78,20 @@ void SoftBodyMotionProperties::Initialize(const SoftBodyCreationSettings &inSett
 		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());
@@ -318,6 +332,7 @@ void SoftBodyMotionProperties::IntegratePositions(const SoftBodyUpdateContext &i
 	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
@@ -325,17 +340,27 @@ void SoftBodyMotionProperties::IntegratePositions(const SoftBodyUpdateContext &i
 
 			// Damping
 			v.mVelocity *= linear_damping;
-
-			// Integrate
-			v.mPreviousPosition = v.mPosition;
-			v.mPosition += v.mVelocity * dt;
-		}
-		else
-		{
-			// Integrate
-			v.mPreviousPosition = v.mPosition;
-			v.mPosition += v.mVelocity * dt;
 		}
+
+		// 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(Vec4(r.mAngularVelocity, 0)) * rotation;
+		r.mPreviousRotationInternal = rotation; // Overwrites mAngularVelocity
+		r.mRotation = (rotation + delta_rotation).Normalized();
+	}
 }
 
 void SoftBodyMotionProperties::ApplyDihedralBendConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex)
@@ -574,6 +599,82 @@ void SoftBodyMotionProperties::ApplyEdgeConstraints(const SoftBodyUpdateContext
 	}
 }
 
+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(UVec4::sXor(rotation.GetXYZW().Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>().ReinterpretAsInt(), UVec4(0x80000000u, 0, 0x80000000u, 0)).ReinterpretAsFloat());
+		rotation += (2.0f * r->mInvMass * r->mLength) * Quat(Vec4(delta, 0)) * 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();
@@ -714,6 +815,11 @@ void SoftBodyMotionProperties::ApplyCollisionConstraintsAndUpdateVelocities(cons
 				}
 			}
 		}
+
+	// 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)
@@ -920,7 +1026,7 @@ SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelDetermineSen
 void SoftBodyMotionProperties::ProcessGroup(const SoftBodyUpdateContext &ioContext, uint inGroupIndex)
 {
 	// Determine start and end
-	SoftBodySharedSettings::UpdateGroup start { 0, 0, 0, 0, 0 };
+	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];
 
@@ -936,6 +1042,10 @@ void SoftBodyMotionProperties::ProcessGroup(const SoftBodyUpdateContext &ioConte
 	// 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);
 }
@@ -1192,6 +1302,62 @@ void SoftBodyMotionProperties::DrawEdgeConstraints(DebugRenderer *inRenderer, RM
 		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,
@@ -1279,11 +1445,16 @@ void SoftBodyMotionProperties::SaveState(StateRecorder &inStream) const
 
 	for (const Vertex &v : mVertices)
 	{
-		inStream.Write(v.mPreviousPosition);
 		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);
@@ -1303,11 +1474,16 @@ void SoftBodyMotionProperties::RestoreState(StateRecorder &inStream)
 
 	for (Vertex &v : mVertices)
 	{
-		inStream.Read(v.mPreviousPosition);
 		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);

Jolt/Physics/SoftBody/SoftBodyMotionProperties.h

@@ -36,6 +36,8 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties
 public:
 	using Vertex = SoftBodyVertex;
 	using Edge = SoftBodySharedSettings::Edge;
+	using RodStretchShear = SoftBodySharedSettings::RodStretchShear;
+	using RodBendTwist = SoftBodySharedSettings::RodBendTwist;
 	using Face = SoftBodySharedSettings::Face;
 	using DihedralBend = SoftBodySharedSettings::DihedralBend;
 	using Volume = SoftBodySharedSettings::Volume;
@@ -58,6 +60,10 @@ public:
 	const Vertex &						GetVertex(uint inIndex) const				{ return mVertices[inIndex]; }
 	Vertex &							GetVertex(uint inIndex)						{ return mVertices[inIndex]; }
 
+	/// Access to the state of rods
+	Quat								GetRodRotation(uint inIndex) const			{ return mRodStates[inIndex].mRotation; }
+	Vec3								GetRodAngularVelocity(uint inIndex) const	{ return mRodStates[inIndex].mAngularVelocity; }
+
 	/// Get the materials of the soft body
 	const PhysicsMaterialList &			GetMaterials() const						{ return mSettings->mMaterials; }
 
@@ -101,6 +107,9 @@ public:
 	void								DrawVertices(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const;
 	void								DrawVertexVelocities(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const;
 	void								DrawEdgeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const;
+	void								DrawRods(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const;
+	void								DrawRodStates(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const;
+	void								DrawRodBendTwistConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const;
 	void								DrawBendConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const;
 	void								DrawVolumeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const;
 	void								DrawSkinConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const;
@@ -208,6 +217,17 @@ private:
 		bool							mHasContact;								///< If the sensor collided with the soft body
 	};
 
+	// Information about the current state of a rod.
+	struct RodState
+	{
+		Quat							mRotation;									///< Rotation of the rod, relative to center of mass transform
+		union
+		{
+			Vec3						mAngularVelocity;							///< Angular velocity of the rod, relative to center of mass transform, valid only outside of the simulation.
+			Quat						mPreviousRotationInternal;					///< Internal use only. Previous rotation of the rod, relative to center of mass transform, valid only during the simulation.
+		};
+	};
+
 	// Information about the state of all skinned vertices
 	struct SkinState
 	{
@@ -240,6 +260,10 @@ private:
 	/// Enforce all edge constraints
 	void								ApplyEdgeConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex);
 
+	/// Enforce all rod constraints
+	void								ApplyRodStretchShearConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex);
+	void								ApplyRodBendTwistConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex);
+
 	/// Enforce all LRA constraints
 	void								ApplyLRAConstraints(uint inStartIndex, uint inEndIndex);
 
@@ -280,6 +304,7 @@ private:
 
 	RefConst<SoftBodySharedSettings>	mSettings;									///< Configuration of the particles and constraints
 	Array<Vertex>						mVertices;									///< Current state of all vertices in the simulation
+	Array<RodState>						mRodStates;									///< Current state of all rods in the simulation
 	Array<CollidingShape>				mCollidingShapes;							///< List of colliding shapes retrieved during the last update
 	Array<CollidingSensor>				mCollidingSensors;							///< List of colliding sensors retrieved during the last update
 	Array<SkinState>					mSkinState;									///< List of skinned positions (1-on-1 with mVertices but only those that are used by the skinning constraints are filled in)

Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp

@@ -36,6 +36,22 @@ JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Edge)
 	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Edge, mCompliance)
 }
 
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::RodStretchShear)
+{
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::RodStretchShear, mVertex)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::RodStretchShear, mLength)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::RodStretchShear, mInvMass)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::RodStretchShear, mCompliance)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::RodStretchShear, mBishop)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::RodBendTwist)
+{
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::RodBendTwist, mRod)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::RodBendTwist, mCompliance)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::RodBendTwist, mOmega0)
+}
+
 JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::DihedralBend)
 {
 	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::DihedralBend, mVertex)
@@ -87,6 +103,8 @@ JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings)
 	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mSkinnedConstraints)
 	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mInvBindMatrices)
 	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mLRAConstraints)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mRodStretchShearConstraints)
+	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mRodBendTwistConstraints)
 	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mMaterials)
 	JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mVertexRadius)
 }
@@ -108,6 +126,11 @@ void SoftBodySharedSettings::CalculateClosestKinematic()
 		connectivity[e.mVertex[0]].push_back(e.mVertex[1]);
 		connectivity[e.mVertex[1]].push_back(e.mVertex[0]);
 	}
+	for (const RodStretchShear &r : mRodStretchShearConstraints)
+	{
+		connectivity[r.mVertex[0]].push_back(r.mVertex[1]);
+		connectivity[r.mVertex[1]].push_back(r.mVertex[0]);
+	}
 
 	// Use Dijkstra's algorithm to find the closest kinematic vertex for each vertex
 	// See: https://en.wikipedia.org/wiki/Dijkstra's_algorithm
@@ -350,15 +373,119 @@ void SoftBodySharedSettings::CalculateEdgeLengths()
 {
 	for (Edge &e : mEdgeConstraints)
 	{
+		JPH_ASSERT(e.mVertex[0] != e.mVertex[1], "Edges need to connect 2 different vertices");
 		e.mRestLength = (Vec3(mVertices[e.mVertex[1]].mPosition) - Vec3(mVertices[e.mVertex[0]].mPosition)).Length();
 		JPH_ASSERT(e.mRestLength > 0.0f);
 	}
 }
 
+void SoftBodySharedSettings::CalculateRodProperties()
+{
+	// Mark connections through bend twist constraints
+	Array<Array<uint32>> connections;
+	connections.resize(mRodStretchShearConstraints.size());
+	for (const RodBendTwist &c : mRodBendTwistConstraints)
+	{
+		JPH_ASSERT(c.mRod[0] != c.mRod[1], "A bend twist constraint needs to be attached to different rods");
+		connections[c.mRod[1]].push_back(c.mRod[0]);
+		connections[c.mRod[0]].push_back(c.mRod[1]);
+	}
+
+	// Now calculate the Bishop frames for all rods
+	struct Entry
+	{
+		uint32	mFrom;	// Rod we're coming from
+		uint32	mTo;	// Rod we're going to
+	};
+	Array<Entry> stack;
+	stack.reserve(mRodStretchShearConstraints.size());
+	for (uint32 r0_idx = 0; r0_idx < mRodStretchShearConstraints.size(); ++r0_idx)
+	{
+		RodStretchShear &r0 = mRodStretchShearConstraints[r0_idx];
+
+		// Do not calculate a 2nd time
+		if (r0.mBishop == Quat::sZero())
+		{
+			// Calculate the frame for this rod
+			{
+				Vec3 tangent = Vec3(mVertices[r0.mVertex[1]].mPosition) - Vec3(mVertices[r0.mVertex[0]].mPosition);
+				r0.mLength = tangent.Length();
+				JPH_ASSERT(r0.mLength > 0.0f, "Rods of zero length are not supported!");
+				tangent /= r0.mLength;
+				Vec3 normal = tangent.GetNormalizedPerpendicular();
+				Vec3 binormal = tangent.Cross(normal);
+				r0.mBishop = Mat44(Vec4(normal, 0), Vec4(binormal, 0), Vec4(tangent, 0), Vec4(0, 0, 0, 1)).GetQuaternion().Normalized();
+			}
+
+			// Add connected rods to the stack if they haven't been calculated yet
+			for (uint32 r1_idx : connections[r0_idx])
+				if (mRodStretchShearConstraints[r1_idx].mBishop == Quat::sZero())
+					stack.push_back({ r0_idx, r1_idx });
+
+			// Now connect the bishop frame for all connected rods on the stack
+			// This follows the procedure outlined in "Discrete Elastic Rods" - M. Bergou et al.
+			// See: https://www.cs.columbia.edu/cg/pdfs/143-rods.pdf
+			while (!stack.empty())
+			{
+				uint32 r1_idx = stack.back().mFrom;
+				uint32 r2_idx = stack.back().mTo;
+				stack.pop_back();
+
+				const RodStretchShear &r1 = mRodStretchShearConstraints[r1_idx];
+				RodStretchShear &r2 = mRodStretchShearConstraints[r2_idx];
+
+				// Get the normal and tangent of the first rod's Bishop frame (that was already calculated)
+				Mat44 r1_frame = Mat44::sRotation(r1.mBishop);
+				Vec3 tangent1 = r1_frame.GetAxisZ();
+				Vec3 normal1 = r1_frame.GetAxisX();
+
+				// Calculate the Bishop frame for the 2nd rod
+				Vec3 tangent2 = Vec3(mVertices[r2.mVertex[1]].mPosition) - Vec3(mVertices[r2.mVertex[0]].mPosition);
+				if (tangent1.Dot(tangent2) < 0.0f)
+				{
+					// Edge is oriented in the opposite direction of the previous edge, flip it
+					std::swap(r2.mVertex[0], r2.mVertex[1]);
+					tangent2 = -tangent2;
+				}
+				r2.mLength = tangent2.Length();
+				JPH_ASSERT(r2.mLength > 0.0f, "Rods of zero length are not supported!");
+				tangent2 /= r2.mLength;
+				Vec3 t1_cross_t2 = tangent1.Cross(tangent2);
+				float sin_angle = t1_cross_t2.Length();
+				Vec3 normal2 = normal1;
+				if (sin_angle > 0.0f)
+				{
+					t1_cross_t2 /= sin_angle;
+					normal2 = Quat::sRotation(t1_cross_t2, ASin(sin_angle)) * normal2;
+				}
+				Vec3 binormal2 = tangent2.Cross(normal2);
+				r2.mBishop = Mat44(Vec4(normal2, 0), Vec4(binormal2, 0), Vec4(tangent2, 0), Vec4(0, 0, 0, 1)).GetQuaternion().Normalized();
+
+				// Add connected rods to the stack if they haven't been calculated yet
+				for (uint32 r3_idx : connections[r2_idx])
+					if (mRodStretchShearConstraints[r3_idx].mBishop == Quat::sZero())
+						stack.push_back({ r2_idx, r3_idx });
+			}
+		}
+	}
+
+	// Calculate inverse mass for all rods by taking the minimum inverse mass (aka the heaviest vertex) of both vertices
+	for (RodStretchShear &r : mRodStretchShearConstraints)
+	{
+		JPH_ASSERT(r.mVertex[0] != r.mVertex[1], "A rod stretch shear constraint requires two different vertices");
+		r.mInvMass = min(mVertices[r.mVertex[0]].mInvMass, mVertices[r.mVertex[1]].mInvMass);
+	}
+
+	// Calculate the initial rotation between the rods
+	for (RodBendTwist &r : mRodBendTwistConstraints)
+		r.mOmega0 = (mRodStretchShearConstraints[r.mRod[0]].mBishop.Conjugated() * mRodStretchShearConstraints[r.mRod[1]].mBishop).Normalized();
+}
+
 void SoftBodySharedSettings::CalculateLRALengths(float inMaxDistanceMultiplier)
 {
 	for (LRA &l : mLRAConstraints)
 	{
+		JPH_ASSERT(l.mVertex[0] != l.mVertex[1], "LRA constraints need to connect 2 different vertices");
 		l.mMaxDistance = inMaxDistanceMultiplier * (Vec3(mVertices[l.mVertex[1]].mPosition) - Vec3(mVertices[l.mVertex[0]].mPosition)).Length();
 		JPH_ASSERT(l.mMaxDistance > 0.0f);
 	}
@@ -368,6 +495,10 @@ void SoftBodySharedSettings::CalculateBendConstraintConstants()
 {
 	for (DihedralBend &b : mDihedralBendConstraints)
 	{
+		JPH_ASSERT(b.mVertex[0] != b.mVertex[1] && b.mVertex[0] != b.mVertex[2] && b.mVertex[0] != b.mVertex[3]
+			&& b.mVertex[1] != b.mVertex[2] && b.mVertex[1] != b.mVertex[3]
+			&& b.mVertex[2] != b.mVertex[3], "Bend constraints need 4 different vertices");
+
 		// Get positions
 		Vec3 x0 = Vec3(mVertices[b.mVertex[0]].mPosition);
 		Vec3 x1 = Vec3(mVertices[b.mVertex[1]].mPosition);
@@ -407,6 +538,10 @@ void SoftBodySharedSettings::CalculateVolumeConstraintVolumes()
 {
 	for (Volume &v : mVolumeConstraints)
 	{
+		JPH_ASSERT(v.mVertex[0] != v.mVertex[1] && v.mVertex[0] != v.mVertex[2] && v.mVertex[0] != v.mVertex[3]
+			&& v.mVertex[1] != v.mVertex[2] && v.mVertex[1] != v.mVertex[3]
+			&& v.mVertex[2] != v.mVertex[3], "Volume constraints need 4 different vertices");
+
 		Vec3 x1(mVertices[v.mVertex[0]].mPosition);
 		Vec3 x2(mVertices[v.mVertex[1]].mPosition);
 		Vec3 x3(mVertices[v.mVertex[2]].mPosition);
@@ -510,6 +645,15 @@ void SoftBodySharedSettings::Optimize(OptimizationResults &outResults)
 		add_connection(c.mVertex[0], c.mVertex[1]);
 	for (const LRA &c : mLRAConstraints)
 		add_connection(c.mVertex[0], c.mVertex[1]);
+	for (const RodStretchShear &c : mRodStretchShearConstraints)
+		add_connection(c.mVertex[0], c.mVertex[1]);
+	for (const RodBendTwist &c : mRodBendTwistConstraints)
+	{
+		add_connection(mRodStretchShearConstraints[c.mRod[0]].mVertex[0], mRodStretchShearConstraints[c.mRod[1]].mVertex[0]);
+		add_connection(mRodStretchShearConstraints[c.mRod[0]].mVertex[1], mRodStretchShearConstraints[c.mRod[1]].mVertex[0]);
+		add_connection(mRodStretchShearConstraints[c.mRod[0]].mVertex[0], mRodStretchShearConstraints[c.mRod[1]].mVertex[1]);
+		add_connection(mRodStretchShearConstraints[c.mRod[0]].mVertex[1], mRodStretchShearConstraints[c.mRod[1]].mVertex[1]);
+	}
 	for (const DihedralBend &c : mDihedralBendConstraints)
 	{
 		add_connection(c.mVertex[0], c.mVertex[1]);
@@ -659,11 +803,13 @@ void SoftBodySharedSettings::Optimize(OptimizationResults &outResults)
 	{
 		uint			GetSize() const
 		{
-			return (uint)mEdgeConstraints.size() + (uint)mLRAConstraints.size() + (uint)mDihedralBendConstraints.size() + (uint)mVolumeConstraints.size() + (uint)mSkinnedConstraints.size();
+			return (uint)mEdgeConstraints.size() + (uint)mLRAConstraints.size() + (uint)mRodStretchShearConstraints.size() + (uint)mRodBendTwistConstraints.size() + (uint)mDihedralBendConstraints.size() + (uint)mVolumeConstraints.size() + (uint)mSkinnedConstraints.size();
 		}
 
 		Array<uint>		mEdgeConstraints;
 		Array<uint>		mLRAConstraints;
+		Array<uint>		mRodStretchShearConstraints;
+		Array<uint>		mRodBendTwistConstraints;
 		Array<uint>		mDihedralBendConstraints;
 		Array<uint>		mVolumeConstraints;
 		Array<uint>		mSkinnedConstraints;
@@ -690,6 +836,28 @@ void SoftBodySharedSettings::Optimize(OptimizationResults &outResults)
 		else // In different groups -> parallel group
 			groups.back().mLRAConstraints.push_back(uint(&l - mLRAConstraints.data()));
 	}
+	for (const RodStretchShear &r : mRodStretchShearConstraints)
+	{
+		int g1 = group_idx[r.mVertex[0]];
+		int g2 = group_idx[r.mVertex[1]];
+		JPH_ASSERT(g1 >= 0 && g2 >= 0);
+		if (g1 == g2) // In the same group
+			groups[g1].mRodStretchShearConstraints.push_back(uint(&r - mRodStretchShearConstraints.data()));
+		else // In different groups -> parallel group
+			groups.back().mRodStretchShearConstraints.push_back(uint(&r - mRodStretchShearConstraints.data()));
+	}
+	for (const RodBendTwist &r : mRodBendTwistConstraints)
+	{
+		int g1 = group_idx[mRodStretchShearConstraints[r.mRod[0]].mVertex[0]];
+		int g2 = group_idx[mRodStretchShearConstraints[r.mRod[0]].mVertex[1]];
+		int g3 = group_idx[mRodStretchShearConstraints[r.mRod[1]].mVertex[0]];
+		int g4 = group_idx[mRodStretchShearConstraints[r.mRod[1]].mVertex[1]];
+		JPH_ASSERT(g1 >= 0 && g2 >= 0 && g3 >= 0 && g4 >= 0);
+		if (g1 == g2 && g1 == g3 && g1 == g4) // In the same group
+			groups[g1].mRodBendTwistConstraints.push_back(uint(&r - mRodBendTwistConstraints.data()));
+		else // In different groups -> parallel group
+			groups.back().mRodBendTwistConstraints.push_back(uint(&r - mRodBendTwistConstraints.data()));
+	}
 	for (const DihedralBend &d : mDihedralBendConstraints)
 	{
 		int g1 = group_idx[d.mVertex[0]];
@@ -773,6 +941,62 @@ void SoftBodySharedSettings::Optimize(OptimizationResults &outResults)
 				return inLHS < inRHS;
 			});
 
+		// Sort the rod stretch shear constraints
+		QuickSort(group.mRodStretchShearConstraints.begin(), group.mRodStretchShearConstraints.end(), [this](uint inLHS, uint inRHS)
+			{
+				const RodStretchShear &r1 = mRodStretchShearConstraints[inLHS];
+				const RodStretchShear &r2 = mRodStretchShearConstraints[inRHS];
+
+				// First sort so that the rod with the smallest distance to a kinematic vertex comes first
+				float d1 = min(mClosestKinematic[r1.mVertex[0]].mDistance, mClosestKinematic[r1.mVertex[1]].mDistance);
+				float d2 = min(mClosestKinematic[r2.mVertex[0]].mDistance, mClosestKinematic[r2.mVertex[1]].mDistance);
+				if (d1 != d2)
+					return d1 < d2;
+
+				// Order the rods so that the ones with the smallest index go first (hoping to get better cache locality when we process the rods).
+				uint32 m1 = r1.GetMinVertexIndex();
+				uint32 m2 = r2.GetMinVertexIndex();
+				if (m1 != m2)
+					return m1 < m2;
+
+				return inLHS < inRHS;
+			});
+
+		// Sort the rod bend twist constraints
+		QuickSort(group.mRodBendTwistConstraints.begin(), group.mRodBendTwistConstraints.end(), [this](uint inLHS, uint inRHS)
+			{
+				const RodBendTwist &b1 = mRodBendTwistConstraints[inLHS];
+				const RodStretchShear &b1_r1 = mRodStretchShearConstraints[b1.mRod[0]];
+				const RodStretchShear &b1_r2 = mRodStretchShearConstraints[b1.mRod[1]];
+
+				const RodBendTwist &b2 = mRodBendTwistConstraints[inRHS];
+				const RodStretchShear &b2_r1 = mRodStretchShearConstraints[b2.mRod[0]];
+				const RodStretchShear &b2_r2 = mRodStretchShearConstraints[b2.mRod[1]];
+
+				// First sort so that the rod with the smallest distance to a kinematic vertex comes first
+				float d1 = min(
+							min(mClosestKinematic[b1_r1.mVertex[0]].mDistance, mClosestKinematic[b1_r1.mVertex[1]].mDistance),
+							min(mClosestKinematic[b1_r2.mVertex[0]].mDistance, mClosestKinematic[b1_r2.mVertex[1]].mDistance));
+				float d2 = min(
+							min(mClosestKinematic[b2_r1.mVertex[0]].mDistance, mClosestKinematic[b2_r1.mVertex[1]].mDistance),
+							min(mClosestKinematic[b2_r2.mVertex[0]].mDistance, mClosestKinematic[b2_r2.mVertex[1]].mDistance));
+				if (d1 != d2)
+					return d1 < d2;
+
+				// Order the rods so that the ones with the smallest index go first
+				uint32 m1 = min(b1_r1.GetMinVertexIndex(), b1_r2.GetMinVertexIndex());
+				uint32 m2 = min(b2_r1.GetMinVertexIndex(), b2_r2.GetMinVertexIndex());
+				if (m1 != m2)
+					return m1 < m2;
+
+				return inLHS < inRHS;
+			});
+
+		// Bilateral interleaving, see figure 4 of "Position and Orientation Based Cosserat Rods" - Kugelstadt and Schoemer - SIGGRAPH 2016
+		// Keeping the twist constraints sorted often results in an unstable simulation
+		for (Array<uint>::size_type i = 1, s = group.mRodBendTwistConstraints.size(), s2 = s >> 1; i < s2; i += 2)
+			std::swap(group.mRodBendTwistConstraints[i], group.mRodBendTwistConstraints[s - 1 - i]);
+
 		// Sort the dihedral bend constraints
 		QuickSort(group.mDihedralBendConstraints.begin(), group.mDihedralBendConstraints.end(), [this](uint inLHS, uint inRHS)
 		{
@@ -841,27 +1065,37 @@ void SoftBodySharedSettings::Optimize(OptimizationResults &outResults)
 	Array<Edge> temp_edges;
 	temp_edges.swap(mEdgeConstraints);
 	mEdgeConstraints.reserve(temp_edges.size());
-	outResults.mEdgeRemap.reserve(temp_edges.size());
+	outResults.mEdgeRemap.resize(temp_edges.size(), ~uint(0));
 
 	Array<LRA> temp_lra;
 	temp_lra.swap(mLRAConstraints);
 	mLRAConstraints.reserve(temp_lra.size());
-	outResults.mLRARemap.reserve(temp_lra.size());
+	outResults.mLRARemap.resize(temp_lra.size(), ~uint(0));
+
+	Array<RodStretchShear> temp_rod_stretch_shear;
+	temp_rod_stretch_shear.swap(mRodStretchShearConstraints);
+	mRodStretchShearConstraints.reserve(temp_rod_stretch_shear.size());
+	outResults.mRodStretchShearConstraintRemap.resize(temp_rod_stretch_shear.size(), ~uint(0));
+
+	Array<RodBendTwist> temp_rod_bend_twist;
+	temp_rod_bend_twist.swap(mRodBendTwistConstraints);
+	mRodBendTwistConstraints.reserve(temp_rod_bend_twist.size());
+	outResults.mRodBendTwistConstraintRemap.resize(temp_rod_bend_twist.size(), ~uint(0));
 
 	Array<DihedralBend> temp_dihedral_bend;
 	temp_dihedral_bend.swap(mDihedralBendConstraints);
 	mDihedralBendConstraints.reserve(temp_dihedral_bend.size());
-	outResults.mDihedralBendRemap.reserve(temp_dihedral_bend.size());
+	outResults.mDihedralBendRemap.resize(temp_dihedral_bend.size(), ~uint(0));
 
 	Array<Volume> temp_volume;
 	temp_volume.swap(mVolumeConstraints);
 	mVolumeConstraints.reserve(temp_volume.size());
-	outResults.mVolumeRemap.reserve(temp_volume.size());
+	outResults.mVolumeRemap.resize(temp_volume.size(), ~uint(0));
 
 	Array<Skinned> temp_skinned;
 	temp_skinned.swap(mSkinnedConstraints);
 	mSkinnedConstraints.reserve(temp_skinned.size());
-	outResults.mSkinnedRemap.reserve(temp_skinned.size());
+	outResults.mSkinnedRemap.resize(temp_skinned.size(), ~uint(0));
 
 	// Finalize update groups
 	for (const Group &group : groups)
@@ -869,42 +1103,61 @@ void SoftBodySharedSettings::Optimize(OptimizationResults &outResults)
 		// Reorder edge constraints for this group
 		for (uint idx : group.mEdgeConstraints)
 		{
+			outResults.mEdgeRemap[idx] = (uint)mEdgeConstraints.size();
 			mEdgeConstraints.push_back(temp_edges[idx]);
-			outResults.mEdgeRemap.push_back(idx);
 		}
 
 		// Reorder LRA constraints for this group
 		for (uint idx : group.mLRAConstraints)
 		{
+			outResults.mLRARemap[idx] = (uint)mLRAConstraints.size();
 			mLRAConstraints.push_back(temp_lra[idx]);
-			outResults.mLRARemap.push_back(idx);
+		}
+
+		// Reorder rod stretch shear constraints for this group
+		for (uint idx : group.mRodStretchShearConstraints)
+		{
+			outResults.mRodStretchShearConstraintRemap[idx] = (uint)mRodStretchShearConstraints.size();
+			mRodStretchShearConstraints.push_back(temp_rod_stretch_shear[idx]);
+		}
+
+		// Reorder rod bend twist constraints for this group
+		for (uint idx : group.mRodBendTwistConstraints)
+		{
+			outResults.mRodBendTwistConstraintRemap[idx] = (uint)mRodBendTwistConstraints.size();
+			mRodBendTwistConstraints.push_back(temp_rod_bend_twist[idx]);
 		}
 
 		// Reorder dihedral bend constraints for this group
 		for (uint idx : group.mDihedralBendConstraints)
 		{
+			outResults.mDihedralBendRemap[idx] = (uint)mDihedralBendConstraints.size();
 			mDihedralBendConstraints.push_back(temp_dihedral_bend[idx]);
-			outResults.mDihedralBendRemap.push_back(idx);
 		}
 
 		// Reorder volume constraints for this group
 		for (uint idx : group.mVolumeConstraints)
 		{
+			outResults.mVolumeRemap[idx] = (uint)mVolumeConstraints.size();
 			mVolumeConstraints.push_back(temp_volume[idx]);
-			outResults.mVolumeRemap.push_back(idx);
 		}
 
 		// Reorder skinned constraints for this group
 		for (uint idx : group.mSkinnedConstraints)
 		{
+			outResults.mSkinnedRemap[idx] = (uint)mSkinnedConstraints.size();
 			mSkinnedConstraints.push_back(temp_skinned[idx]);
-			outResults.mSkinnedRemap.push_back(idx);
 		}
 
 		// Store end indices
-		mUpdateGroups.push_back({ (uint)mEdgeConstraints.size(), (uint)mLRAConstraints.size(), (uint)mDihedralBendConstraints.size(), (uint)mVolumeConstraints.size(), (uint)mSkinnedConstraints.size() });
+		mUpdateGroups.push_back({ (uint)mEdgeConstraints.size(), (uint)mLRAConstraints.size(), (uint)mRodStretchShearConstraints.size(), (uint)mRodBendTwistConstraints.size(), (uint)mDihedralBendConstraints.size(), (uint)mVolumeConstraints.size(), (uint)mSkinnedConstraints.size() });
 	}
 
+	// Remap bend twist indices because mRodStretchShearConstraints has been reordered
+	for (RodBendTwist &r : mRodBendTwistConstraints)
+		for (int i = 0; i < 2; ++i)
+			r.mRod[i] = outResults.mRodStretchShearConstraintRemap[r.mRod[i]];
+
 	// Free closest kinematic buffer
 	mClosestKinematic.clear();
 	mClosestKinematic.shrink_to_fit();
@@ -922,6 +1175,8 @@ Ref<SoftBodySharedSettings> SoftBodySharedSettings::Clone() const
 	clone->mSkinnedConstraintNormals = mSkinnedConstraintNormals;
 	clone->mInvBindMatrices = mInvBindMatrices;
 	clone->mLRAConstraints = mLRAConstraints;
+	clone->mRodStretchShearConstraints = mRodStretchShearConstraints;
+	clone->mRodBendTwistConstraints = mRodBendTwistConstraints;
 	clone->mMaterials = mMaterials;
 	clone->mVertexRadius = mVertexRadius;
 	clone->mUpdateGroups = mUpdateGroups;
@@ -941,6 +1196,22 @@ void SoftBodySharedSettings::SaveBinaryState(StreamOut &inStream) const
 	inStream.Write(mVertexRadius);
 	inStream.Write(mUpdateGroups);
 
+	// Can't write mRodStretchShearConstraints directly because the class contains padding
+	inStream.Write(mRodStretchShearConstraints, [](const RodStretchShear &inElement, StreamOut &inS) {
+		inS.Write(inElement.mVertex);
+		inS.Write(inElement.mLength);
+		inS.Write(inElement.mInvMass);
+		inS.Write(inElement.mCompliance);
+		inS.Write(inElement.mBishop);
+	});
+
+	// Can't write mRodBendTwistConstraints directly because the class contains padding
+	inStream.Write(mRodBendTwistConstraints, [](const RodBendTwist &inElement, StreamOut &inS) {
+		inS.Write(inElement.mRod);
+		inS.Write(inElement.mCompliance);
+		inS.Write(inElement.mOmega0);
+	});
+
 	// Can't write mInvBindMatrices directly because the class contains padding
 	inStream.Write(mInvBindMatrices, [](const InvBind &inElement, StreamOut &inS) {
 		inS.Write(inElement.mJointIndex);
@@ -961,6 +1232,20 @@ void SoftBodySharedSettings::RestoreBinaryState(StreamIn &inStream)
 	inStream.Read(mVertexRadius);
 	inStream.Read(mUpdateGroups);
 
+	inStream.Read(mRodStretchShearConstraints, [](StreamIn &inS, RodStretchShear &outElement) {
+		inS.Read(outElement.mVertex);
+		inS.Read(outElement.mLength);
+		inS.Read(outElement.mInvMass);
+		inS.Read(outElement.mCompliance);
+		inS.Read(outElement.mBishop);
+	});
+
+	inStream.Read(mRodBendTwistConstraints, [](StreamIn &inS, RodBendTwist &outElement) {
+		inS.Read(outElement.mRod);
+		inS.Read(outElement.mCompliance);
+		inS.Read(outElement.mOmega0);
+	});
+
 	inStream.Read(mInvBindMatrices, [](StreamIn &inS, InvBind &outElement) {
 		inS.Read(outElement.mJointIndex);
 		inS.Read(outElement.mInvBind);

Jolt/Physics/SoftBody/SoftBodySharedSettings.h

@@ -59,6 +59,10 @@ public:
 	/// Calculate the initial lengths of all springs of the edges of this soft body (if you use CreateConstraint, this is already done)
 	void				CalculateEdgeLengths();
 
+	/// Calculate the properties of the rods
+	/// Note that this can swap mVertex of the RodStretchShear constraints if two rods are connected through a RodBendTwist constraint but point in opposite directions.
+	void				CalculateRodProperties();
+
 	/// Calculate the max lengths for the long range attachment constraints based on Euclidean distance (if you use CreateConstraints, this is already done)
 	/// @param inMaxDistanceMultiplier Multiplier for the max distance of the LRA constraint, e.g. 1.01 means the max distance is 1% longer than the calculated distance in the rest pose.
 	void				CalculateLRALengths(float inMaxDistanceMultiplier = 1.0f);
@@ -78,6 +82,8 @@ public:
 	public:
 		Array<uint>		mEdgeRemap;									///< Maps old edge index to new edge index
 		Array<uint>		mLRARemap;									///< Maps old LRA index to new LRA index
+		Array<uint>		mRodStretchShearConstraintRemap;			///< Maps old rod stretch shear constraint index to new stretch shear rod constraint index
+		Array<uint>		mRodBendTwistConstraintRemap;				///< Maps old rod bend twist constraint index to new bend twist rod constraint index
 		Array<uint>		mDihedralBendRemap;							///< Maps old dihedral bend index to new dihedral bend index
 		Array<uint>		mVolumeRemap;								///< Maps old volume constraint index to new volume constraint index
 		Array<uint>		mSkinnedRemap;								///< Maps old skinned constraint index to new skinned constraint index
@@ -160,7 +166,7 @@ public:
 		uint32			GetMinVertexIndex() const					{ return min(mVertex[0], mVertex[1]); }
 
 		uint32			mVertex[2];									///< Indices of the vertices that form the edge
-		float			mRestLength = 1.0f;							///< Rest length of the spring
+		float			mRestLength = 1.0f;							///< Rest length of the spring, calculated by CalculateEdgeLengths
 		float			mCompliance = 0.0f;							///< Inverse of the stiffness of the spring
 	};
 
@@ -195,7 +201,7 @@ public:
 
 		uint32			mVertex[4];									///< Indices of the vertices of the 2 triangles that share an edge (the first 2 vertices are the shared edge)
 		float			mCompliance = 0.0f;							///< Inverse of the stiffness of the constraint
-		float			mInitialAngle = 0.0f;						///< Initial angle between the normals of the triangles (pi - dihedral angle).
+		float			mInitialAngle = 0.0f;						///< Initial angle between the normals of the triangles (pi - dihedral angle), calculated by CalculateBendConstraintConstants
 	};
 
 	/// Volume constraint, keeps the volume of a tetrahedron constant
@@ -211,7 +217,7 @@ public:
 		uint32			GetMinVertexIndex() const					{ return min(min(mVertex[0], mVertex[1]), min(mVertex[2], mVertex[3])); }
 
 		uint32			mVertex[4];									///< Indices of the vertices that form the tetrahedron
-		float			mSixRestVolume = 1.0f;						///< 6 times the rest volume of the tetrahedron (calculated by CalculateVolumeConstraintVolumes())
+		float			mSixRestVolume = 1.0f;						///< 6 times the rest volume of the tetrahedron, calculated by CalculateVolumeConstraintVolumes
 		float			mCompliance = 0.0f;							///< Inverse of the stiffness of the constraint
 	};
 
@@ -275,7 +281,7 @@ public:
 		float			mMaxDistance = FLT_MAX;						///< Maximum distance that this vertex can reach from the skinned vertex, disabled when FLT_MAX. 0 when you want to hard skin the vertex to the skinned vertex.
 		float			mBackStopDistance = FLT_MAX;				///< Disabled if mBackStopDistance >= mMaxDistance. The faces surrounding mVertex determine an average normal. mBackStopDistance behind the vertex in the opposite direction of this normal, the back stop sphere starts. The simulated vertex will be pushed out of this sphere and it can be used to approximate the volume of the skinned mesh behind the skinned vertex.
 		float			mBackStopRadius = 40.0f;					///< Radius of the backstop sphere. By default this is a fairly large radius so the sphere approximates a plane.
-		uint32			mNormalInfo = 0;							///< Information needed to calculate the normal of this vertex, lowest 24 bit is start index in mSkinnedConstraintNormals, highest 8 bit is number of faces (generated by CalculateSkinnedConstraintNormals())
+		uint32			mNormalInfo = 0;							///< Information needed to calculate the normal of this vertex, lowest 24 bit is start index in mSkinnedConstraintNormals, highest 8 bit is number of faces (generated by CalculateSkinnedConstraintNormals)
 	};
 
 	/// A long range attachment constraint, this is a constraint that sets a max distance between a kinematic vertex and a dynamic vertex
@@ -293,7 +299,46 @@ public:
 		uint32			GetMinVertexIndex() const					{ return min(mVertex[0], mVertex[1]); }
 
 		uint32			mVertex[2];									///< The vertices that are connected. The first vertex should be kinematic, the 2nd dynamic.
-		float			mMaxDistance = 0.0f;						///< The maximum distance between the vertices
+		float			mMaxDistance = 0.0f;						///< The maximum distance between the vertices, calculated by CalculateLRALengths
+	};
+
+	/// A discrete Cosserat rod connects two particles with a rigid rod that has fixed length and inertia.
+	/// A rod can be used instead of an Edge to constraint two vertices. The orientation of the rod can be
+	/// used to orient geometry attached to the rod (e.g. a plant leaf). Note that each rod needs to be constrained
+	/// by at least one RodBendTwist constraint in order to constrain the rotation of the rod. If you don't do
+	/// this then the orientation is likely to rotate around the rod axis with constant velocity.
+	/// Based on "Position and Orientation Based Cosserat Rods" - Kugelstadt and Schoemer - SIGGRAPH 2016
+	/// See: https://www.researchgate.net/publication/325597548_Position_and_Orientation_Based_Cosserat_Rods
+	struct JPH_EXPORT RodStretchShear
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, RodStretchShear)
+
+		/// Constructor
+						RodStretchShear() = default;
+						RodStretchShear(uint32 inVertex1, uint32 inVertex2, float inCompliance = 0.0f) : mVertex { inVertex1, inVertex2 }, mCompliance(inCompliance) { }
+
+		/// Return the lowest vertex index of this constraint
+		uint32			GetMinVertexIndex() const					{ return min(mVertex[0], mVertex[1]); }
+
+		uint32			mVertex[2];									///< Indices of the vertices that form the rod
+		float			mLength = 1.0f;								///< Fixed length of the rod, calculated by CalculateRodProperties
+		float			mInvMass = 1.0f;							///< Inverse of the mass of the rod (0 for static rods), calculated by CalculateRodProperties but can be overridden afterwards
+		float			mCompliance = 0.0f;							///< Inverse of the stiffness of the rod
+		Quat			mBishop	= Quat::sZero();					///< The Bishop frame of the rod (the rotation of the rod in its rest pose so that it has zero twist towards adjacent rods), calculated by CalculateRodProperties
+	};
+
+	/// A constraint that connects two Cosserat rods and limits bend and twist between the rods.
+	struct JPH_EXPORT RodBendTwist
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, RodBendTwist)
+
+		/// Constructor
+						RodBendTwist() = default;
+						RodBendTwist(uint32 inRod1, uint32 inRod2, float inCompliance = 0.0f) : mRod { inRod1, inRod2 }, mCompliance(inCompliance) { }
+
+		uint32			mRod[2];									///< Indices of rods that are constrained (index in mRodStretchShearConstraints)
+		float			mCompliance = 0.0f;							///< Inverse of the stiffness of the rod
+		Quat			mOmega0 = Quat::sZero();					///< The initial rotation between the rods: rod1.mBishop.Conjugated() * rod2.mBishop, calculated by CalculateRodProperties
 	};
 
 	/// Add a face to this soft body
@@ -307,6 +352,8 @@ public:
 	Array<Skinned>		mSkinnedConstraints;						///< The list of vertices that are constrained to a skinned vertex
 	Array<InvBind>		mInvBindMatrices;							///< The list of inverse bind matrices for skinning vertices
 	Array<LRA>			mLRAConstraints;							///< The list of long range attachment constraints
+	Array<RodStretchShear>	mRodStretchShearConstraints;			///< The list of Cosserat rod constraints that connect two vertices and that limit stretch and shear
+	Array<RodBendTwist>	mRodBendTwistConstraints;					///< The list of Cosserat rod constraints that connect two rods and limit the bend and twist
 	PhysicsMaterialList mMaterials { PhysicsMaterial::sDefault };	///< The materials of the faces of the body, referenced by Face::mMaterialIndex
 	float				mVertexRadius = 0.0f;						///< How big the particles are, can be used to push the vertices a little bit away from the surface of other bodies to prevent z-fighting
 
@@ -328,6 +375,8 @@ private:
 	{
 		uint			mEdgeEndIndex;								///< The end index of the edge constraints in this group
 		uint			mLRAEndIndex;								///< The end index of the LRA constraints in this group
+		uint			mRodStretchShearEndIndex;					///< The end index of the rod stretch shear constraints in this group
+		uint			mRodBendTwistEndIndex;						///< The end index of the rod bend twist constraints in this group
 		uint			mDihedralBendEndIndex;						///< The end index of the dihedral bend constraints in this group
 		uint			mVolumeEndIndex;							///< The end index of the volume constraints in this group
 		uint			mSkinnedEndIndex;							///< The end index of the skinned constraints in this group
@@ -335,7 +384,7 @@ private:
 
 	Array<ClosestKinematic> mClosestKinematic;						///< The closest kinematic vertex to each vertex in mVertices
 	Array<UpdateGroup>	mUpdateGroups;								///< The end indices for each group of constraints that can be updated in parallel
-	Array<uint32>		mSkinnedConstraintNormals;					///< A list of indices in the mFaces array used by mSkinnedConstraints, calculated by CalculateSkinnedConstraintNormals()
+	Array<uint32>		mSkinnedConstraintNormals;					///< A list of indices in the mFaces array used by mSkinnedConstraints, calculated by CalculateSkinnedConstraintNormals
 };
 
 JPH_NAMESPACE_END

README.md

@@ -78,6 +78,7 @@ Why create yet another physics engine? Firstly, it has been a personal learning
 * Soft body simulation (e.g. a soft ball or piece of cloth).
 	* Edge constraints.
 	* Dihedral bend constraints.
+	* Cosserat rod constraints (an edge with an orientation that can be used to orient geometry, e.g. a plant leaf).
 	* Tetrahedron volume constraints.
 	* Long range attachment constraints (also called tethers).
 	* Limiting the simulation to stay within a certain range of a skinned vertex.

Samples/Samples.cmake

@@ -161,6 +161,8 @@ set(SAMPLES_SRC_FILES
 	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyBendConstraintTest.h
 	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyContactListenerTest.cpp
 	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyContactListenerTest.h
+	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyCosseratRodConstraintTest.cpp
+	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyCosseratRodConstraintTest.h
 	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyCustomUpdateTest.cpp
 	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyCustomUpdateTest.h
 	${SAMPLES_ROOT}/Tests/SoftBody/SoftBodyForceTest.cpp

Samples/SamplesApp.cpp

@@ -362,27 +362,29 @@ JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SoftBodyCustomUpdateTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SoftBodyLRAConstraintTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SoftBodyBendConstraintTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SoftBodySkinnedConstraintTest)
+JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SoftBodyCosseratRodConstraintTest)
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, SoftBodySensorTest)
 
 static TestNameAndRTTI sSoftBodyTests[] =
 {
-	{ "Soft Body vs Shapes",			JPH_RTTI(SoftBodyShapesTest) },
-	{ "Soft Body vs Fast Moving",		JPH_RTTI(SoftBodyVsFastMovingTest) },
-	{ "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 Force",				JPH_RTTI(SoftBodyForceTest) },
-	{ "Soft Body Kinematic",			JPH_RTTI(SoftBodyKinematicTest) },
-	{ "Soft Body Update Position",		JPH_RTTI(SoftBodyUpdatePositionTest) },
-	{ "Soft Body Stress Test",			JPH_RTTI(SoftBodyStressTest) },
-	{ "Soft Body Vertex Radius Test",	JPH_RTTI(SoftBodyVertexRadiusTest) },
-	{ "Soft Body Contact Listener",		JPH_RTTI(SoftBodyContactListenerTest) },
-	{ "Soft Body Custom Update",		JPH_RTTI(SoftBodyCustomUpdateTest) },
-	{ "Soft Body LRA Constraint",		JPH_RTTI(SoftBodyLRAConstraintTest) },
-	{ "Soft Body Bend Constraint",		JPH_RTTI(SoftBodyBendConstraintTest) },
-	{ "Soft Body Skinned Constraint",	JPH_RTTI(SoftBodySkinnedConstraintTest) },
-	{ "Soft Body vs Sensor",			JPH_RTTI(SoftBodySensorTest) }
+	{ "Soft Body vs Shapes",				JPH_RTTI(SoftBodyShapesTest) },
+	{ "Soft Body vs Fast Moving",			JPH_RTTI(SoftBodyVsFastMovingTest) },
+	{ "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 Force",					JPH_RTTI(SoftBodyForceTest) },
+	{ "Soft Body Kinematic",				JPH_RTTI(SoftBodyKinematicTest) },
+	{ "Soft Body Update Position",			JPH_RTTI(SoftBodyUpdatePositionTest) },
+	{ "Soft Body Stress Test",				JPH_RTTI(SoftBodyStressTest) },
+	{ "Soft Body Vertex Radius Test",		JPH_RTTI(SoftBodyVertexRadiusTest) },
+	{ "Soft Body Contact Listener",			JPH_RTTI(SoftBodyContactListenerTest) },
+	{ "Soft Body Custom Update",			JPH_RTTI(SoftBodyCustomUpdateTest) },
+	{ "Soft Body LRA Constraint",			JPH_RTTI(SoftBodyLRAConstraintTest) },
+	{ "Soft Body Bend Constraint",			JPH_RTTI(SoftBodyBendConstraintTest) },
+	{ "Soft Body Skinned Constraint",		JPH_RTTI(SoftBodySkinnedConstraintTest) },
+	{ "Soft Body Cosserat Rod Constraint",	JPH_RTTI(SoftBodyCosseratRodConstraintTest) },
+	{ "Soft Body vs Sensor",				JPH_RTTI(SoftBodySensorTest) }
 };
 
 JPH_DECLARE_RTTI_FOR_FACTORY(JPH_NO_EXPORT, BroadPhaseCastRayTest)
@@ -558,18 +560,29 @@ SamplesApp::SamplesApp(const String &inCommandLine) :
 			mDebugUI->CreateCheckBox(drawing_options, "Draw Mesh Shape Triangle Outlines", MeshShape::sDrawTriangleOutlines, [](UICheckBox::EState inState) { MeshShape::sDrawTriangleOutlines = inState == UICheckBox::STATE_CHECKED; });
 			mDebugUI->CreateCheckBox(drawing_options, "Draw Height Field Shape Triangle Outlines", HeightFieldShape::sDrawTriangleOutlines, [](UICheckBox::EState inState) { HeightFieldShape::sDrawTriangleOutlines = inState == UICheckBox::STATE_CHECKED; });
 			mDebugUI->CreateCheckBox(drawing_options, "Draw Submerged Volumes", Shape::sDrawSubmergedVolumes, [](UICheckBox::EState inState) { Shape::sDrawSubmergedVolumes = inState == UICheckBox::STATE_CHECKED; });
-			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 Vertex Velocities", mBodyDrawSettings.mDrawSoftBodyVertexVelocities, [this](UICheckBox::EState inState) { mBodyDrawSettings.mDrawSoftBodyVertexVelocities = 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 Bend Constraints", mBodyDrawSettings.mDrawSoftBodyBendConstraints, [this](UICheckBox::EState inState) { mBodyDrawSettings.mDrawSoftBodyBendConstraints = 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 Skin Constraints", mBodyDrawSettings.mDrawSoftBodySkinConstraints, [this](UICheckBox::EState inState) { mBodyDrawSettings.mDrawSoftBodySkinConstraints = inState == UICheckBox::STATE_CHECKED; });
-			mDebugUI->CreateCheckBox(drawing_options, "Draw Soft Body LRA Constraints", mBodyDrawSettings.mDrawSoftBodyLRAConstraints, [this](UICheckBox::EState inState) { mBodyDrawSettings.mDrawSoftBodyLRAConstraints = 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->CreateComboBox(drawing_options, "Draw Soft Body Constraint Color", { "Constraint Type", "Constraint Group", "Constraint Order" }, (int)mBodyDrawSettings.mDrawSoftBodyConstraintColor, [this](int inItem) { mBodyDrawSettings.mDrawSoftBodyConstraintColor = (ESoftBodyConstraintColor)inItem; });
+			mDebugUI->CreateTextButton(drawing_options, "Draw Character Virtual", [this](){
+				UIElement *draw_character = mDebugUI->CreateMenu();
+				mDebugUI->CreateCheckBox(draw_character, "Draw Character Virtual Constraints", CharacterVirtual::sDrawConstraints, [](UICheckBox::EState inState) { CharacterVirtual::sDrawConstraints = inState == UICheckBox::STATE_CHECKED; });
+				mDebugUI->CreateCheckBox(draw_character, "Draw Character Virtual Walk Stairs", CharacterVirtual::sDrawWalkStairs, [](UICheckBox::EState inState) { CharacterVirtual::sDrawWalkStairs = inState == UICheckBox::STATE_CHECKED; });
+				mDebugUI->CreateCheckBox(draw_character, "Draw Character Virtual Stick To Floor", CharacterVirtual::sDrawStickToFloor, [](UICheckBox::EState inState) { CharacterVirtual::sDrawStickToFloor = inState == UICheckBox::STATE_CHECKED; });
+				mDebugUI->ShowMenu(draw_character);
+			});
+			mDebugUI->CreateTextButton(drawing_options, "Draw Soft Body", [this](){
+				UIElement *draw_soft_body = mDebugUI->CreateMenu();
+				mDebugUI->CreateCheckBox(draw_soft_body, "Draw Vertices", mBodyDrawSettings.mDrawSoftBodyVertices, [this](UICheckBox::EState inState) { mBodyDrawSettings.mDrawSoftBodyVertices = inState == UICheckBox::STATE_CHECKED; });
+				mDebugUI->CreateCheckBox(draw_soft_body, "Draw Vertex Velocities", mBodyDrawSettings.mDrawSoftBodyVertexVelocities, [this](UICheckBox::EState inState) { mBodyDrawSettings.mDrawSoftBodyVertexVelocities = inState == UICheckBox::STATE_CHECKED; });
+				mDebugUI->CreateCheckBox(draw_soft_body, "Draw Edge Constraints", mBodyDrawSettings.mDrawSoftBodyEdgeConstraints, [this](UICheckBox::EState inState) { mBodyDrawSettings.mDrawSoftBodyEdgeConstraints = inState == UICheckBox::STATE_CHECKED; });
+				mDebugUI->CreateCheckBox(draw_soft_body, "Draw Bend Constraints", mBodyDrawSettings.mDrawSoftBodyBendConstraints, [this](UICheckBox::EState inState) { mBodyDrawSettings.mDrawSoftBodyBendConstraints = inState == UICheckBox::STATE_CHECKED; });
+				mDebugUI->CreateCheckBox(draw_soft_body, "Draw Volume Constraints", mBodyDrawSettings.mDrawSoftBodyVolumeConstraints, [this](UICheckBox::EState inState) { mBodyDrawSettings.mDrawSoftBodyVolumeConstraints = inState == UICheckBox::STATE_CHECKED; });
+				mDebugUI->CreateCheckBox(draw_soft_body, "Draw Skin Constraints", mBodyDrawSettings.mDrawSoftBodySkinConstraints, [this](UICheckBox::EState inState) { mBodyDrawSettings.mDrawSoftBodySkinConstraints = inState == UICheckBox::STATE_CHECKED; });
+				mDebugUI->CreateCheckBox(draw_soft_body, "Draw LRA Constraints", mBodyDrawSettings.mDrawSoftBodyLRAConstraints, [this](UICheckBox::EState inState) { mBodyDrawSettings.mDrawSoftBodyLRAConstraints = inState == UICheckBox::STATE_CHECKED; });
+				mDebugUI->CreateCheckBox(draw_soft_body, "Draw Rods", mBodyDrawSettings.mDrawSoftBodyRods, [this](UICheckBox::EState inState) { mBodyDrawSettings.mDrawSoftBodyRods = inState == UICheckBox::STATE_CHECKED; });
+				mDebugUI->CreateCheckBox(draw_soft_body, "Draw Rod States", mBodyDrawSettings.mDrawSoftBodyRodStates, [this](UICheckBox::EState inState) { mBodyDrawSettings.mDrawSoftBodyRodStates = inState == UICheckBox::STATE_CHECKED; });
+				mDebugUI->CreateCheckBox(draw_soft_body, "Draw Rod Bend Twist Constraints", mBodyDrawSettings.mDrawSoftBodyRodBendTwistConstraints, [this](UICheckBox::EState inState) { mBodyDrawSettings.mDrawSoftBodyRodBendTwistConstraints = inState == UICheckBox::STATE_CHECKED; });
+				mDebugUI->CreateCheckBox(draw_soft_body, "Draw Predicted Bounds", mBodyDrawSettings.mDrawSoftBodyPredictedBounds, [this](UICheckBox::EState inState) { mBodyDrawSettings.mDrawSoftBodyPredictedBounds = inState == UICheckBox::STATE_CHECKED; });
+				mDebugUI->CreateComboBox(draw_soft_body, "Draw Constraint Color", { "Constraint Type", "Constraint Group", "Constraint Order" }, (int)mBodyDrawSettings.mDrawSoftBodyConstraintColor, [this](int inItem) { mBodyDrawSettings.mDrawSoftBodyConstraintColor = (ESoftBodyConstraintColor)inItem; });
+				mDebugUI->ShowMenu(draw_soft_body);
+			});
 			mDebugUI->ShowMenu(drawing_options);
 		});
 	#endif // JPH_DEBUG_RENDERER

Samples/Tests/SoftBody/SoftBodyBendConstraintTest.cpp

@@ -52,6 +52,57 @@ void SoftBodyBendConstraintTest::Initialize()
 		mBodyInterface->CreateAndAddSoftBody(cloth, EActivation::Activate);
 	}
 
+	{
+		random.seed(1234);
+
+		// Cloth with Cosserat rod constraints
+		Ref<SoftBodySharedSettings> cloth_settings = SoftBodyCreator::CreateCloth(cNumVerticesX, cNumVerticesZ, cVertexSpacing, inv_mass, perturbation, SoftBodySharedSettings::EBendType::None);
+
+		// Get rid of created edges, we're replacing them with rods
+		cloth_settings->mEdgeConstraints.clear();
+
+		// Copy of SoftBodyCreator::CreateCloth: Function to get the vertex index of a point on the cloth
+		auto vertex_index = [](uint inX, uint inY)
+		{
+			return inX + inY * cNumVerticesX;
+		};
+
+		// Create bend twist constraints
+		constexpr float cCompliance = 1.0e-5f;
+		auto get_rod = [&constraints = cloth_settings->mRodStretchShearConstraints, vertex_index, cCompliance](uint inX1, uint inZ1, uint inX2, uint inZ2)
+		{
+			uint32 v0 = vertex_index(inX1, inZ1);
+			uint32 v1 = vertex_index(inX2, inZ2);
+			JPH_ASSERT(v0 < v1);
+
+			for (uint i = 0; i < uint(constraints.size()); ++i)
+				if (constraints[i].mVertex[0] == v0 && constraints[i].mVertex[1] == v1)
+					return i;
+
+			constraints.emplace_back(v0, v1, cCompliance);
+			return uint(constraints.size() - 1);
+		};
+		for (uint z = 1; z < cNumVerticesZ - 1; ++z)
+			for (uint x = 0; x < cNumVerticesX - 1; ++x)
+			{
+				if (z > 1 && x < cNumVerticesX - 2)
+					cloth_settings->mRodBendTwistConstraints.emplace_back(get_rod(x, z, x + 1, z), get_rod(x + 1, z, x + 2, z), cCompliance);
+				if (z < cNumVerticesZ - 2)
+					cloth_settings->mRodBendTwistConstraints.emplace_back(get_rod(x, z, x, z + 1), get_rod(x, z + 1, x, z + 2), cCompliance);
+				if (x < cNumVerticesX - 2 && z < cNumVerticesZ - 2)
+				{
+					cloth_settings->mRodBendTwistConstraints.emplace_back(get_rod(x, z, x + 1, z + 1), get_rod(x + 1, z + 1, x + 2, z + 2), cCompliance);
+					cloth_settings->mRodBendTwistConstraints.emplace_back(get_rod(x + 2, z, x + 1, z + 1), get_rod(x + 1, z + 1, x, z + 2), cCompliance);
+				}
+			}
+		cloth_settings->CalculateRodProperties();
+
+		// Optimize the settings
+		cloth_settings->Optimize();
+		SoftBodyCreationSettings cloth(cloth_settings, RVec3(10.0f, 5.0f, 0), Quat::sIdentity(), Layers::MOVING);
+		mBodyInterface->CreateAndAddSoftBody(cloth, EActivation::Activate);
+	}
+
 	{
 		// Create sphere
 		SoftBodyCreationSettings sphere(SoftBodyCreator::CreateSphere(1.0f, 10, 20, SoftBodySharedSettings::EBendType::None), RVec3(-5.0f, 5.0f, 10.0f), Quat::sIdentity(), Layers::MOVING);
@@ -69,6 +120,68 @@ void SoftBodyBendConstraintTest::Initialize()
 		SoftBodyCreationSettings sphere(SoftBodyCreator::CreateSphere(1.0f, 10, 20, SoftBodySharedSettings::EBendType::Dihedral), RVec3(5.0f, 5.0f, 10.0f), Quat::sIdentity(), Layers::MOVING);
 		mBodyInterface->CreateAndAddSoftBody(sphere, EActivation::Activate);
 	}
+
+	{
+		// Create sphere with Cosserat rod constraints
+		uint cNumTheta = 10;
+		uint cNumPhi = 20;
+		Ref<SoftBodySharedSettings> sphere_settings = SoftBodyCreator::CreateSphere(1.0f, cNumTheta, cNumPhi, SoftBodySharedSettings::EBendType::None);
+
+		// Get rid of created edges, we're replacing them with rods
+		sphere_settings->mEdgeConstraints.clear();
+
+		// Copy of SoftBodyCreator::CreateSphere: Function to get the vertex index of a point on the sphere
+		auto vertex_index = [cNumTheta, cNumPhi](uint inTheta, uint inPhi) -> uint
+		{
+			if (inTheta == 0)
+				return 0;
+			else if (inTheta == cNumTheta - 1)
+				return 1;
+			else
+				return 2 + (inTheta - 1) * cNumPhi + inPhi % cNumPhi;
+		};
+
+		// Create bend twist constraints
+		constexpr float cCompliance = 1.0e-4f;
+		auto get_rod = [&constraints = sphere_settings->mRodStretchShearConstraints, vertex_index, cCompliance](uint inTheta1, uint inPhi1, uint inTheta2, uint inPhi2)
+		{
+			uint32 v0 = vertex_index(inTheta1, inPhi1);
+			uint32 v1 = vertex_index(inTheta2, inPhi2);
+			JPH_ASSERT(v0 != v1);
+
+			for (uint i = 0; i < uint(constraints.size()); ++i)
+				if ((constraints[i].mVertex[0] == v0 && constraints[i].mVertex[1] == v1)
+					|| (constraints[i].mVertex[0] == v1 && constraints[i].mVertex[1] == v0))
+					return i;
+
+			constraints.emplace_back(v0, v1, cCompliance);
+			return uint(constraints.size() - 1);
+		};
+
+		// Rings along the side
+		for (uint phi = 0; phi < cNumPhi; ++phi)
+			for (uint theta = 0; theta < cNumTheta - 1; ++theta)
+			{
+				if (theta < cNumTheta - 2)
+					sphere_settings->mRodBendTwistConstraints.emplace_back(get_rod(theta, phi, theta + 1, phi), get_rod(theta + 1, phi, theta + 2, phi), cCompliance);
+				if (theta > 0 && phi < cNumPhi - 1)
+					sphere_settings->mRodBendTwistConstraints.emplace_back(get_rod(theta, phi, theta, phi + 1), get_rod(theta, phi + 1, theta, (phi + 2) % cNumPhi), cCompliance);
+			}
+
+		// Close the caps
+		for (uint phi1 = 0, phi2 = cNumPhi / 2; phi1 < cNumPhi / 2; ++phi1, phi2 = (phi2 + 1) % cNumPhi)
+		{
+			sphere_settings->mRodBendTwistConstraints.emplace_back(get_rod(0, phi1, 1, phi1), get_rod(0, phi2, 1, phi2), cCompliance);
+			sphere_settings->mRodBendTwistConstraints.emplace_back(get_rod(cNumTheta - 2, phi1, cNumTheta - 1, phi1), get_rod(cNumTheta - 2, phi2, cNumTheta - 1, phi2), cCompliance);
+		}
+
+		sphere_settings->CalculateRodProperties();
+
+		// Optimize the settings
+		sphere_settings->Optimize();
+		SoftBodyCreationSettings sphere(sphere_settings, RVec3(10.0f, 5.0f, 10.0f), Quat::sIdentity(), Layers::MOVING);
+		mBodyInterface->CreateAndAddSoftBody(sphere, EActivation::Activate);
+	}
 }
 
 void SoftBodyBendConstraintTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
@@ -76,4 +189,5 @@ void SoftBodyBendConstraintTest::PrePhysicsUpdate(const PreUpdateParams &inParam
 	mDebugRenderer->DrawText3D(RVec3(-5.0f, 7.5f, 0), "No bend constraints", Color::sWhite);
 	mDebugRenderer->DrawText3D(RVec3(0.0f, 7.5f, 0), "Distance bend constraints", Color::sWhite);
 	mDebugRenderer->DrawText3D(RVec3(5.0f, 7.5f, 0), "Dihedral angle bend constraints", Color::sWhite);
+	mDebugRenderer->DrawText3D(RVec3(10.0f, 7.5f, 0), "Cosserat rod constraints", Color::sWhite);
 }

Samples/Tests/SoftBody/SoftBodyCosseratRodConstraintTest.cpp

@@ -0,0 +1,134 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2025 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <TestFramework.h>
+
+#include <Tests/SoftBody/SoftBodyCosseratRodConstraintTest.h>
+#include <Jolt/Physics/SoftBody/SoftBodyCreationSettings.h>
+#include <Jolt/Physics/SoftBody/SoftBodyMotionProperties.h>
+#include <Utils/SoftBodyCreator.h>
+#include <Layers.h>
+#include <Renderer/DebugRendererImp.h>
+
+JPH_IMPLEMENT_RTTI_VIRTUAL(SoftBodyCosseratRodConstraintTest)
+{
+	JPH_ADD_BASE_CLASS(SoftBodyCosseratRodConstraintTest, Test)
+}
+
+void SoftBodyCosseratRodConstraintTest::Initialize()
+{
+	CreateFloor();
+
+	// Create a hanging helix
+	{
+		constexpr float cRadius = 0.5f;
+		constexpr int cNumVertices = 128;
+		constexpr float cHeight = 5.0f;
+		constexpr float cNumCycles = 10;
+
+		Ref<SoftBodySharedSettings> helix_settings = new SoftBodySharedSettings;
+		for (int i = 0; i < cNumVertices; ++i)
+		{
+			float fraction = float(i) / (cNumVertices - 1);
+
+			SoftBodySharedSettings::Vertex v;
+			float alpha = cNumCycles * 2.0f * JPH_PI * fraction;
+			v.mPosition = Float3(cRadius * Sin(alpha), 0.5f * (1.0f - fraction * cHeight), cRadius * Cos(alpha));
+			v.mInvMass = i == 0? 0.0f : 1.0e-2f;
+			helix_settings->mVertices.push_back(v);
+
+			if (i > 0)
+				helix_settings->mRodStretchShearConstraints.push_back(SoftBodySharedSettings::RodStretchShear(i - 1, i));
+
+			if (i > 1)
+				helix_settings->mRodBendTwistConstraints.push_back(SoftBodySharedSettings::RodBendTwist(i - 2, i - 1));
+		}
+
+		helix_settings->CalculateRodProperties();
+		helix_settings->Optimize();
+
+		SoftBodyCreationSettings helix(helix_settings, RVec3(0, 10, 0), Quat::sIdentity(), Layers::MOVING);
+		mSoftBodies.push_back(mBodyInterface->CreateAndAddSoftBody(helix, EActivation::Activate));
+	}
+
+	// Create a tree with a static root
+	{
+		// Root particle
+		Ref<SoftBodySharedSettings> tree_settings = new SoftBodySharedSettings;
+		SoftBodySharedSettings::Vertex v;
+		v.mPosition = Float3(0, 0, 0);
+		v.mInvMass = 0.0f;
+		tree_settings->mVertices.push_back(v);
+
+		// Create branches
+		struct Branch
+		{
+			uint32	mPreviousVertex;
+			uint32	mPreviousRod;
+			Vec3	mDirection;
+			uint32	mDepth;
+		};
+		Array<Branch> branches;
+		branches.push_back({ 0, uint32(-1), Vec3::sAxisY(), 0 });
+		while (!branches.empty())
+		{
+			// Take the next branch
+			Branch branch = branches.front();
+			branches.erase(branches.begin());
+
+			// Create vertex
+			SoftBodySharedSettings::Vertex &previous_vertex = tree_settings->mVertices[branch.mPreviousVertex];
+			(Vec3(previous_vertex.mPosition) + branch.mDirection).StoreFloat3(&v.mPosition);
+			v.mInvMass = branch.mDepth > 0? 2.0f * previous_vertex.mInvMass : 1.0e-3f;
+			uint32 new_vertex = uint32(tree_settings->mVertices.size());
+			tree_settings->mVertices.push_back(v);
+
+			// Create rod
+			uint32 new_rod = uint32(tree_settings->mRodStretchShearConstraints.size());
+			tree_settings->mRodStretchShearConstraints.push_back(SoftBodySharedSettings::RodStretchShear(branch.mPreviousVertex, new_vertex));
+			if (branch.mPreviousRod != uint32(-1))
+				tree_settings->mRodBendTwistConstraints.push_back(SoftBodySharedSettings::RodBendTwist(branch.mPreviousRod, new_rod));
+
+			// Create sub branches
+			if (branch.mDepth < 10)
+				for (uint32 i = 0; i < 2; ++i)
+				{
+					// Determine new child direction
+					float angle = DegreesToRadians(-15.0f + i * 30.0f);
+					Vec3 new_direction = Quat::sRotation(branch.mDepth & 1? Vec3::sAxisZ() : Vec3::sAxisX(), angle) * branch.mDirection;
+
+					// Create new branch
+					branches.push_back({ new_vertex, new_rod, new_direction, branch.mDepth + 1 });
+				}
+		}
+
+		tree_settings->CalculateRodProperties();
+		tree_settings->Optimize();
+
+		SoftBodyCreationSettings tree(tree_settings, RVec3(10, 0, 0), Quat::sIdentity(), Layers::MOVING);
+		mSoftBodies.push_back(mBodyInterface->CreateAndAddSoftBody(tree, EActivation::Activate));
+	}
+}
+
+void SoftBodyCosseratRodConstraintTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
+{
+	// Draw the soft body rods
+	for (BodyID id : mSoftBodies)
+	{
+		BodyLockRead lock(mPhysicsSystem->GetBodyLockInterface(), id);
+		if (lock.Succeeded())
+		{
+			const Body &body = lock.GetBody();
+			const SoftBodyMotionProperties *mp = static_cast<const SoftBodyMotionProperties *>(body.GetMotionProperties());
+			RMat44 com = body.GetCenterOfMassTransform();
+
+			for (const SoftBodySharedSettings::RodStretchShear &r : mp->GetSettings()->mRodStretchShearConstraints)
+			{
+				RVec3 x0 = com * mp->GetVertex(r.mVertex[0]).mPosition;
+				RVec3 x1 = com * mp->GetVertex(r.mVertex[1]).mPosition;
+				mDebugRenderer->DrawLine(x0, x1, Color::sWhite);
+			}
+		}
+	}
+}

Samples/Tests/SoftBody/SoftBodyCosseratRodConstraintTest.h

@@ -0,0 +1,26 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2025 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Tests/Test.h>
+
+class SoftBodyCosseratRodConstraintTest : public Test
+{
+public:
+	JPH_DECLARE_RTTI_VIRTUAL(JPH_NO_EXPORT, SoftBodyCosseratRodConstraintTest)
+
+	// Description of the test
+	virtual const char *	GetDescription() const override
+	{
+		return "Shows the effect of Cosserat rod constraints in a soft body that control bend, twist and shear between particles.";
+	}
+
+	// See: Test
+	virtual void			Initialize() override;
+	virtual void			PrePhysicsUpdate(const PreUpdateParams &inParams) override;
+
+private:
+	BodyIDVector			mSoftBodies;
+};

UnitTests/Math/Vec4Tests.cpp

@@ -140,6 +140,9 @@ TEST_SUITE("Vec4Tests")
 
 		CHECK(Vec4(1.001f, 0, 0, 0).IsNormalized(1.0e-2f));
 		CHECK(!Vec4(0, 1.001f, 0, 0).IsNormalized(1.0e-4f));
+
+		CHECK(Vec4(-1.0e-7f, 1.0e-7f, 1.0e-8f, -1.0e-8f).IsNearZero());
+		CHECK(!Vec4(-1.0e-7f, 1.0e-7f, -1.0e-5f, 1.0e-5f).IsNearZero());
 	}
 
 	TEST_CASE("TestVec4Operators")