diff --git a/Jolt/Physics/Body/BodyManager.cpp b/Jolt/Physics/Body/BodyManager.cpp index e8ebd4490..0f63f8af0 100644 --- a/Jolt/Physics/Body/BodyManager.cpp +++ b/Jolt/Physics/Body/BodyManager.cpp @@ -1085,6 +1085,9 @@ void BodyManager::Draw(const DrawSettings &inDrawSettings, const PhysicsSettings if (inDrawSettings.mDrawSoftBodyVolumeConstraints) mp->DrawVolumeConstraints(inRenderer, com); + if (inDrawSettings.mDrawSoftBodySkinConstraints) + mp->DrawSkinConstraints(inRenderer); + if (inDrawSettings.mDrawSoftBodyPredictedBounds) mp->DrawPredictedBounds(inRenderer, com); } diff --git a/Jolt/Physics/Body/BodyManager.h b/Jolt/Physics/Body/BodyManager.h index d39c13695..1ca9e9368 100644 --- a/Jolt/Physics/Body/BodyManager.h +++ b/Jolt/Physics/Body/BodyManager.h @@ -232,6 +232,7 @@ class JPH_EXPORT BodyManager : public NonCopyable bool mDrawSoftBodyVertices = false; ///< Draw the vertices of soft bodies bool mDrawSoftBodyEdgeConstraints = false; ///< Draw the edge constraints of soft bodies bool mDrawSoftBodyVolumeConstraints = false; ///< Draw the volume constraints of soft bodies + bool mDrawSoftBodySkinConstraints = false; ///< Draw the skin constraints of soft bodies bool mDrawSoftBodyPredictedBounds = false; ///< Draw the predicted bounds of soft bodies }; diff --git a/Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp b/Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp index c8358eb24..93fa42111 100644 --- a/Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp +++ b/Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp @@ -74,6 +74,10 @@ void SoftBodyMotionProperties::Initialize(const SoftBodyCreationSettings &inSett mLocalBounds.Encapsulate(out_vertex.mPosition); } + // Allocate space for skinned vertices + if (!inSettings.mSettings->mSkinnedConstraints.empty()) + mSkinState.resize(mVertices.size()); + // We don't know delta time yet, so we can't predict the bounds and use the local bounds as the predicted bounds mLocalPredictedBounds = mLocalBounds; @@ -288,6 +292,50 @@ void SoftBodyMotionProperties::ApplyVolumeConstraints(const SoftBodyUpdateContex } } +void SoftBodyMotionProperties::ApplySkinConstraints([[maybe_unused]] const SoftBodyUpdateContext &inContext) +{ + // Early out if nothing to do + if (mSettings->mSkinnedConstraints.empty()) + return; + + JPH_ASSERT(mSkinStateTransform == inContext.mCenterOfMassTransform, "Skinning state is stale, artifacts will show!"); + + // Apply the constraints + Vertex *vertices = mVertices.data(); + const SkinState *skin_states = mSkinState.data(); + for (const Skinned &s : mSettings->mSkinnedConstraints) + { + Vertex &vertex = vertices[s.mVertex]; + const SkinState &skin_state = skin_states[s.mVertex]; + if (vertex.mInvMass > 0.0f) + { + // Clamp vertex distance to max distance from skinned position + if (s.mMaxDistance < FLT_MAX) + { + Vec3 delta = vertex.mPosition - skin_state.mPosition; + float delta_len_sq = delta.LengthSq(); + float max_distance_sq = Square(s.mMaxDistance); + if (delta_len_sq > max_distance_sq) + vertex.mPosition = skin_state.mPosition + delta * sqrt(max_distance_sq / delta_len_sq); + } + + // Move position if it violated the back stop + if (s.mBackStop < s.mMaxDistance) + { + Vec3 delta = vertex.mPosition - skin_state.mPosition; + float violation = -s.mBackStop - skin_state.mNormal.Dot(delta); + if (violation > 0.0f) + vertex.mPosition += violation * skin_state.mNormal; + } + } + else + { + // Kinematic: Just update the vertex position + vertex.mPosition = skin_state.mPosition; + } + } +} + void SoftBodyMotionProperties::ApplyEdgeConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex) { JPH_PROFILE_FUNCTION(); @@ -624,6 +672,8 @@ SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelApplyEdgeCon // Finish the iteration ApplyCollisionConstraintsAndUpdateVelocities(ioContext); + ApplySkinConstraints(ioContext); + uint iteration = ioContext.mNextIteration.fetch_add(1, memory_order_relaxed); if (iteration < mNumIterations) { @@ -677,6 +727,76 @@ SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelUpdate(SoftB } } +void SoftBodyMotionProperties::SkinVertices(RMat44Arg inRootTransform, 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); + const Mat44 *skin_matrices_end = skin_matrices + num_skin_matrices; + const InvBind *inv_bind_matrix = mSettings->mInvBindMatrices.data(); + for (Mat44 *s = skin_matrices; s < skin_matrices_end; ++s, ++inv_bind_matrix) + *s = inJointMatrices[inv_bind_matrix->mJointIndex] * inv_bind_matrix->mInvBind; + + // Skin the vertices + mSkinStateTransform = inRootTransform; + 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) + { + JPH_ASSERT(w.mInvBindIndex < num_skin_matrices); + pos += w.mWeight * (skin_matrices[w.mInvBindIndex] * bind_pos); + } + mSkinState[s.mVertex].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 /= float(num_faces); + } + mSkinState[s.mVertex].mNormal = normal; + } + + ioTempAllocator.Free(skin_matrices, skin_matrices_size); + + if (inHardSkinAll) + { + // Hard skin all vertices and reset their velocities + for (const Skinned &s : mSettings->mSkinnedConstraints) + { + Vertex &vertex = mVertices[s.mVertex]; + vertex.mPosition = mSkinState[s.mVertex].mPosition; + vertex.mVelocity = Vec3::sZero(); + } + } +} + void SoftBodyMotionProperties::CustomUpdate(float inDeltaTime, Body &ioSoftBody, PhysicsSystem &inSystem) { JPH_PROFILE_FUNCTION(); @@ -733,6 +853,15 @@ void SoftBodyMotionProperties::DrawVolumeConstraints(DebugRenderer *inRenderer, } } +void SoftBodyMotionProperties::DrawSkinConstraints(DebugRenderer *inRenderer) const +{ + for (const Skinned &s : mSettings->mSkinnedConstraints) + { + const SkinState &skin_state = mSkinState[s.mVertex]; + DebugRenderer::sInstance->DrawArrow(mSkinStateTransform * skin_state.mPosition, mSkinStateTransform * (skin_state.mPosition + 0.1f * skin_state.mNormal), Color::sOrange, 0.01f); + } +} + void SoftBodyMotionProperties::DrawPredictedBounds(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const { inRenderer->DrawWireBox(inCenterOfMassTransform, mLocalPredictedBounds, Color::sRed); diff --git a/Jolt/Physics/SoftBody/SoftBodyMotionProperties.h b/Jolt/Physics/SoftBody/SoftBodyMotionProperties.h index ee17b7bae..0d0911af7 100644 --- a/Jolt/Physics/SoftBody/SoftBodyMotionProperties.h +++ b/Jolt/Physics/SoftBody/SoftBodyMotionProperties.h @@ -20,6 +20,7 @@ struct PhysicsSettings; class Body; class Shape; class SoftBodyCreationSettings; +class TempAllocator; #ifdef JPH_DEBUG_RENDERER class DebugRenderer; #endif // JPH_DEBUG_RENDERER @@ -35,6 +36,9 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties using Edge = SoftBodySharedSettings::Edge; using Face = SoftBodySharedSettings::Face; using Volume = SoftBodySharedSettings::Volume; + using InvBind = SoftBodySharedSettings::InvBind; + using SkinWeight = SoftBodySharedSettings::SkinWeight; + using Skinned = SoftBodySharedSettings::Skinned; /// Initialize the soft body motion properties void Initialize(const SoftBodyCreationSettings &inSettings); @@ -85,6 +89,7 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties void DrawVertices(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const; void DrawEdgeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const; void DrawVolumeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const; + void DrawSkinConstraints(DebugRenderer *inRenderer) const; void DrawPredictedBounds(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const; #endif // JPH_DEBUG_RENDERER @@ -94,6 +99,14 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties /// Restoring state for replay void RestoreState(StateRecorder &inStream); + /// Skin vertices to supplied joints, information is used by the skinned constraints. + /// @param inRootTransform Value of Body::GetCenterOfMassTransform(). + /// @param inJointMatrices The joint matrices must be expressed relative to inRootTransform. + /// @param inNumJoints Indicates how large the inJointMatrices array is (used only for validating out of bounds). + /// @param inHardSkinAll Can be used to position all vertices on the skinned vertices and can be used to hard reset the soft body. + /// @param ioTempAllocator Allocator. + void SkinVertices(RMat44Arg inRootTransform, const Mat44 *inJointMatrices, uint inNumJoints, bool inHardSkinAll, TempAllocator &ioTempAllocator); + /// This function allows you to update the soft body immediately without going through the PhysicsSystem. /// This is useful if the soft body is teleported and needs to 'settle' or it can be used if a the soft body /// is not added to the PhysicsSystem and needs to be updated manually. One reason for not adding it to the @@ -161,6 +174,13 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties Vec3 mOriginalAngularVelocity; ///< Angular velocity of the body in local space to the soft body at start }; + // Information about the state of all skinned vertices + struct SkinState + { + Vec3 mPosition = Vec3::sNaN(); + Vec3 mNormal = Vec3::sNaN(); + }; + /// Do a narrow phase check and determine the closest feature that we can collide with void DetermineCollisionPlanes(const SoftBodyUpdateContext &inContext, uint inVertexStart, uint inNumVertices); @@ -173,6 +193,9 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties /// Enforce all volume constraints void ApplyVolumeConstraints(const SoftBodyUpdateContext &inContext); + /// Enforce all skin constraints + void ApplySkinConstraints(const SoftBodyUpdateContext &inContext); + /// Enforce all edge constraints void ApplyEdgeConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex); @@ -194,9 +217,11 @@ class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties /// Returns 6 times the volume of the soft body float GetVolumeTimesSix() const; + RMat44 mSkinStateTransform = RMat44::sIdentity(); ///< The matrix that transforms mSkinState to world space RefConst mSettings; ///< Configuration of the particles and constraints Array mVertices; ///< Current state of all vertices in the simulation Array mCollidingShapes; ///< List of colliding shapes retrieved during the last update + Array mSkinState; ///< List of skinned positions (1-on-1 with mVertices but only those that are used by the skinning constraints are filled in) AABox mLocalBounds; ///< Bounding box of all vertices AABox mLocalPredictedBounds; ///< Predicted bounding box for all vertices using extrapolation of velocity by last step delta time uint32 mNumIterations; ///< Number of solver iterations diff --git a/Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp b/Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp index 82e0f6e65..62f6cf4fd 100644 --- a/Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp +++ b/Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp @@ -10,6 +10,8 @@ #include #include #include +#include +#include JPH_NAMESPACE_BEGIN @@ -40,6 +42,26 @@ JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Volume) JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Volume, mCompliance) } +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::InvBind) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::InvBind, mJointIndex) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::InvBind, mInvBind) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::SkinWeight) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::SkinWeight, mInvBindIndex) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::SkinWeight, mWeight) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Skinned) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Skinned, mVertex) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Skinned, mWeights) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Skinned, mMaxDistance) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Skinned, mBackStop) +} + JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings) { JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mVertices) @@ -47,6 +69,8 @@ JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings) JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mEdgeConstraints) JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mEdgeGroupEndIndices) JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mVolumeConstraints) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mSkinnedConstraints) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mInvBindMatrices) JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mMaterials) JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mVertexRadius) } @@ -77,6 +101,54 @@ void SoftBodySharedSettings::CalculateVolumeConstraintVolumes() } } +void SoftBodySharedSettings::CalculateSkinnedConstraintNormals() +{ + // Clear any previous results + mSkinnedConstraintNormals.clear(); + + // If there are no skinned constraints, we're done + if (mSkinnedConstraints.empty()) + return; + + // First collect all vertices that are skinned + UnorderedSet skinned_vertices; + skinned_vertices.reserve(mSkinnedConstraints.size()); + for (const Skinned &s : mSkinnedConstraints) + skinned_vertices.insert(s.mVertex); + + // Now collect all faces that connect only to skinned vertices + UnorderedMap> connected_faces; + connected_faces.reserve(mVertices.size()); + for (const Face &f : mFaces) + { + // Must connect to only skinned vertices + bool valid = true; + for (uint32 v : f.mVertex) + valid &= skinned_vertices.find(v) != skinned_vertices.end(); + if (!valid) + continue; + + // Store faces that connect to vertices + for (uint32 v : f.mVertex) + connected_faces[v].insert(uint32(&f - mFaces.data())); + } + + // Populate the list of connecting faces per skinned vertex + mSkinnedConstraintNormals.reserve(mFaces.size()); + for (Skinned &s : mSkinnedConstraints) + { + uint32 start = uint32(mSkinnedConstraintNormals.size()); + JPH_ASSERT((start >> 24) == 0); + const UnorderedSet &faces = connected_faces[s.mVertex]; + uint32 num = uint32(faces.size()); + JPH_ASSERT(num < 256); + mSkinnedConstraintNormals.insert(mSkinnedConstraintNormals.end(), faces.begin(), faces.end()); + QuickSort(mSkinnedConstraintNormals.begin() + start, mSkinnedConstraintNormals.begin() + start + num); + s.mNormalInfo = start + (num << 24); + } + mSkinnedConstraintNormals.shrink_to_fit(); +} + void SoftBodySharedSettings::Optimize(OptimizationResults &outResults) { const uint cMaxNumGroups = 32; @@ -143,7 +215,17 @@ void SoftBodySharedSettings::SaveBinaryState(StreamOut &inStream) const inStream.Write(mEdgeConstraints); inStream.Write(mEdgeGroupEndIndices); inStream.Write(mVolumeConstraints); + inStream.Write(mSkinnedConstraints); + inStream.Write(mSkinnedConstraintNormals); inStream.Write(mVertexRadius); + + // Can't write mInvBindMatrices directly because the class contains padding + inStream.Write(uint32(mInvBindMatrices.size())); + for (const InvBind &ib : mInvBindMatrices) + { + inStream.Write(ib.mJointIndex); + inStream.Write(ib.mInvBind); + } } void SoftBodySharedSettings::RestoreBinaryState(StreamIn &inStream) @@ -153,7 +235,18 @@ void SoftBodySharedSettings::RestoreBinaryState(StreamIn &inStream) inStream.Read(mEdgeConstraints); inStream.Read(mEdgeGroupEndIndices); inStream.Read(mVolumeConstraints); + inStream.Read(mSkinnedConstraints); + inStream.Read(mSkinnedConstraintNormals); inStream.Read(mVertexRadius); + + uint32 num_inv_bind_matrices = 0; + inStream.Read(num_inv_bind_matrices); + mInvBindMatrices.resize(num_inv_bind_matrices); + for (InvBind &ib : mInvBindMatrices) + { + inStream.Read(ib.mJointIndex); + inStream.Read(ib.mInvBind); + } } void SoftBodySharedSettings::SaveWithMaterials(StreamOut &inStream, SharedSettingsToIDMap &ioSettingsMap, MaterialToIDMap &ioMaterialMap) const diff --git a/Jolt/Physics/SoftBody/SoftBodySharedSettings.h b/Jolt/Physics/SoftBody/SoftBodySharedSettings.h index 2db8d16b0..a31114752 100644 --- a/Jolt/Physics/SoftBody/SoftBodySharedSettings.h +++ b/Jolt/Physics/SoftBody/SoftBodySharedSettings.h @@ -23,6 +23,9 @@ class JPH_EXPORT SoftBodySharedSettings : public RefTarget mEdgeConstraints; ///< The list of edges or springs of the body Array mEdgeGroupEndIndices; ///< The start index of each group of edges that can be solved in parallel Array mVolumeConstraints; ///< The list of volume constraints of the body that keep the volume of tetrahedra in the soft body constant + Array mSkinnedConstraints; ///< The list of vertices that are constrained to a skinned vertex + Array mSkinnedConstraintNormals; ///< A list of indices in the mFaces array used by mSkinnedConstraints and calculated by CalculateSkinnedConstraintNormals() + Array mInvBindMatrices; ///< The list of inverse bind matrices for skinning vertices 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 };